From 7ea9137a5654732c0c997776735860d62fe9460c Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Mon, 8 Jul 2024 23:53:52 +0900 Subject: [PATCH] feat(aeva): correct handling of the currently active return mode --- .../nebula_common/aeva/config_types.hpp | 14 +++++++ .../include/nebula_common/nebula_common.hpp | 6 +++ .../aeva_aeries2_decoder.hpp | 8 +++- .../aeva_aeries2_decoder.cpp | 40 +++++++++++-------- nebula_ros/src/aeva/aeva_ros_wrapper.cpp | 11 +++++ 5 files changed, 61 insertions(+), 18 deletions(-) diff --git a/nebula_common/include/nebula_common/aeva/config_types.hpp b/nebula_common/include/nebula_common/aeva/config_types.hpp index 8acef5cef..7155dbb27 100644 --- a/nebula_common/include/nebula_common/aeva/config_types.hpp +++ b/nebula_common/include/nebula_common/aeva/config_types.hpp @@ -15,10 +15,12 @@ #pragma once #include "nebula_common/nebula_common.hpp" +#include "nebula_common/util/parsing.hpp" #include #include +#include #include #include @@ -31,6 +33,18 @@ struct Aeries2Config : public SensorConfigurationBase { std::string sensor_ip; json tree; + + [[nodiscard]] std::optional getReturnMode() const + { + auto mode_name = util::get_if_exists(tree, {"dsp_control", "second_peak_type"}); + + if (!mode_name) return {}; + if (mode_name == "strongest") return ReturnMode::DUAL_STRONGEST_SECONDSTRONGEST; + if (mode_name == "farthest") return ReturnMode::DUAL_STRONGEST_LAST; + if (mode_name == "closest") return ReturnMode::DUAL_STRONGEST_FIRST; + + return ReturnMode::UNKNOWN; + } }; inline std::ostream & operator<<(std::ostream & os, const Aeries2Config & arg) diff --git a/nebula_common/include/nebula_common/nebula_common.hpp b/nebula_common/include/nebula_common/nebula_common.hpp index e11f6cc30..d969082ae 100644 --- a/nebula_common/include/nebula_common/nebula_common.hpp +++ b/nebula_common/include/nebula_common/nebula_common.hpp @@ -66,6 +66,7 @@ enum class ReturnMode : uint8_t { FIRST, DUAL_LAST_FIRST, DUAL_FIRST_STRONGEST, + DUAL_STRONGEST_SECONDSTRONGEST, DUAL }; @@ -198,6 +199,8 @@ inline uint8_t ReturnModeToInt(const ReturnMode & mode) case ReturnMode::DUAL: return 18; break; + case ReturnMode::DUAL_STRONGEST_SECONDSTRONGEST: + return 19; default: case ReturnMode::UNKNOWN: return 0; @@ -308,6 +311,9 @@ inline std::ostream & operator<<(std::ostream & os, nebula::drivers::ReturnMode case ReturnMode::DUAL_FIRST_STRONGEST: os << "FirstStrongest"; break; + case ReturnMode::DUAL_STRONGEST_SECONDSTRONGEST: + os << "StrongestSecondstrongest"; + break; case ReturnMode::DUAL: os << "Dual"; break; diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_aeva/aeva_aeries2_decoder.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_aeva/aeva_aeries2_decoder.hpp index cdd90cde8..f2900c1ef 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_aeva/aeva_aeries2_decoder.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_aeva/aeva_aeries2_decoder.hpp @@ -21,6 +21,7 @@ #include #include +#include #include #include #include @@ -46,6 +47,8 @@ class AevaAeries2Decoder void registerPointCloudCallback(callback_t callback); + void onParameterChange(ReturnMode return_mode); + private: struct DecoderState { @@ -62,9 +65,10 @@ class AevaAeries2Decoder uint64_t timestamp; }; + ReturnType getReturnType(uint32_t peak_id); + callback_t callback_; + std::atomic return_mode_; PointcloudState cloud_state_{}; - - std::mutex mtx_callback_; }; } // namespace nebula::drivers diff --git a/nebula_decoders/src/nebula_decoders_aeva/aeva_aeries2_decoder.cpp b/nebula_decoders/src/nebula_decoders_aeva/aeva_aeries2_decoder.cpp index e324886c1..af34c69ec 100644 --- a/nebula_decoders/src/nebula_decoders_aeva/aeva_aeries2_decoder.cpp +++ b/nebula_decoders/src/nebula_decoders_aeva/aeva_aeries2_decoder.cpp @@ -22,8 +22,6 @@ void AevaAeries2Decoder::processPointcloudMessage(const aeva::PointCloudMessage } if (static_cast(i) == state.new_frame_index) { - std::scoped_lock lock(mtx_callback_); - if (callback_) { callback_(std::move(cloud_state_.cloud), cloud_state_.timestamp); } @@ -51,19 +49,7 @@ void AevaAeries2Decoder::processPointcloudMessage(const aeva::PointCloudMessage point.azimuth = -raw_point.azimuth.value() * M_PI_2f; point.elevation = raw_point.elevation.value() * M_PI_4f; - ReturnType return_type{ReturnType::UNKNOWN}; - // TODO(mojomex): Currently, there is no info published by the sensor on which return mode is - // active. Here, the default one is hardcoded for now. - switch (raw_point.peak_id) { - case 0: - return_type = ReturnType::STRONGEST; - break; - case 1: - return_type = ReturnType::SECONDSTRONGEST; - break; - default: - return_type = ReturnType::UNKNOWN; - } + ReturnType return_type = getReturnType(raw_point.peak_id); point.return_type = static_cast(return_type); @@ -82,10 +68,32 @@ void AevaAeries2Decoder::processPointcloudMessage(const aeva::PointCloudMessage } } +ReturnType AevaAeries2Decoder::getReturnType(uint32_t peak_id) +{ + if (peak_id == 0) return ReturnType::STRONGEST; + if (peak_id > 1) return ReturnType::UNKNOWN; + + switch (return_mode_.load()) { + case ReturnMode::DUAL_STRONGEST_FIRST: + return ReturnType::FIRST; + case ReturnMode::DUAL_STRONGEST_LAST: + return ReturnType::LAST; + case ReturnMode::DUAL_STRONGEST_SECONDSTRONGEST: + return ReturnType::SECONDSTRONGEST; + default: + return ReturnType::UNKNOWN; + } +} + +void AevaAeries2Decoder::onParameterChange(ReturnMode return_mode) +{ + return_mode_.store(return_mode); +} + void AevaAeries2Decoder::registerPointCloudCallback( std::function, uint64_t)> callback) { - std::lock_guard lock(mtx_callback_); callback_ = std::move(callback); } + } // namespace nebula::drivers diff --git a/nebula_ros/src/aeva/aeva_ros_wrapper.cpp b/nebula_ros/src/aeva/aeva_ros_wrapper.cpp index ed6e5defd..22ac7bdcf 100644 --- a/nebula_ros/src/aeva/aeva_ros_wrapper.cpp +++ b/nebula_ros/src/aeva/aeva_ros_wrapper.cpp @@ -223,6 +223,17 @@ Status AevaRosWrapper::validateAndSetConfig(std::shared_ptr } } + auto return_mode_opt = new_config->getReturnMode(); + + if (return_mode_opt && *return_mode_opt == drivers::ReturnMode::UNKNOWN) { + RCLCPP_ERROR_STREAM(get_logger(), "Invalid return mode"); + return Status::SENSOR_CONFIG_ERROR; + } + + if (return_mode_opt) { + decoder_.onParameterChange(*return_mode_opt); + } + sensor_cfg_ptr_ = new_config; return Status::OK; }