feat(aeva): correct handling of the currently active return mode

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 <nlohmann/json.hpp>
 
 #include <cstdint>
+#include <optional>
 #include <ostream>
 #include <string>
 
@@ -31,6 +33,18 @@ struct Aeries2Config : public SensorConfigurationBase
 {
   std::string sensor_ip;
   json tree;
+
+  [[nodiscard]] std::optional<ReturnMode> getReturnMode() const
+  {
+    auto mode_name = util::get_if_exists<std::string>(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)

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;

nebula_decoders/include/nebula_decoders/nebula_decoders_aeva/aeva_aeries2_decoder.hpp

@@ -21,6 +21,7 @@
 #include <pcl/point_cloud.h>
 #include <sys/types.h>
 
+#include <atomic>
 #include <cmath>
 #include <cstddef>
 #include <cstdint>
@@ -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<ReturnMode> return_mode_;
   PointcloudState cloud_state_{};
-
-  std::mutex mtx_callback_;
 };
 }  // namespace nebula::drivers

nebula_decoders/src/nebula_decoders_aeva/aeva_aeries2_decoder.cpp

@@ -22,8 +22,6 @@ void AevaAeries2Decoder::processPointcloudMessage(const aeva::PointCloudMessage
     }
 
     if (static_cast<ssize_t>(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<uint8_t>(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<void(std::unique_ptr<AevaPointCloud>, uint64_t)> callback)
 {
-  std::lock_guard lock(mtx_callback_);
   callback_ = std::move(callback);
 }
+
 }  // namespace nebula::drivers

nebula_ros/src/aeva/aeva_ros_wrapper.cpp

@@ -223,6 +223,17 @@ Status AevaRosWrapper::validateAndSetConfig(std::shared_ptr<const Aeries2Config>
     }
   }
 
+  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;
 }