feat(ot128): support for retro multi-reflection filtering setting

nebula_common/include/nebula_common/hesai/hesai_common.hpp

@@ -38,6 +38,7 @@ namespace drivers
 {
 
 bool supports_functional_safety(const SensorModel & sensor_model);
+bool supports_retro_multi_reflection_filtering(const SensorModel & sensor_model);
 
 struct AdvancedFunctionalSafetyConfiguration
 {
@@ -83,6 +84,7 @@ struct HesaiSensorConfiguration : public LidarConfigurationBase
   std::optional<std::string> downsample_mask_path;
   bool hires_mode;
   std::optional<uint32_t> blockage_mask_horizontal_bin_size_mdeg;
+  bool retro_multi_reflection_filtering;
   std::optional<std::string> sync_diagnostics_topic;
   std::optional<AdvancedFunctionalSafetyConfiguration> functional_safety;
 };
@@ -125,6 +127,10 @@ inline std::ostream & operator<<(std::ostream & os, HesaiSensorConfiguration con
                std::to_string(arg.blockage_mask_horizontal_bin_size_mdeg.value()) + " mdeg"
            : "disabled")
      << '\n';
+  if (supports_retro_multi_reflection_filtering(arg.sensor_model)) {
+    os << "Retro Multi-Reflection Filtering: "
+       << (arg.retro_multi_reflection_filtering ? "enabled" : "disabled") << '\n';
+  }
   os << "Synchronization Diagnostics: "
      << (arg.sync_diagnostics_topic ? ("enabled, topic: " + arg.sync_diagnostics_topic.value())
                                     : "disabled");
@@ -690,6 +696,21 @@ inline bool supports_blockage_mask(const SensorModel & sensor_model)
   }
 }
 
+/// @brief Whether the given sensor model supports retro multi-reflection filtering
+///
+/// @param sensor_model Sensor model
+/// @return True if the sensor model supports retro multi-reflection filtering, false otherwise
+inline bool supports_retro_multi_reflection_filtering(const SensorModel & sensor_model)
+{
+  switch (sensor_model) {
+    case SensorModel::HESAI_PANDARQT128:
+    case SensorModel::HESAI_PANDAR128_E4X:
+      return true;
+    default:
+      return false;
+  }
+}
+
 }  // namespace drivers
 }  // namespace nebula
 

nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp

@@ -85,6 +85,8 @@ const uint8_t g_ptc_command_set_rotate_direction = 0x2a;
 const uint8_t g_ptc_command_lidar_monitor = 0x27;
 const uint8_t g_ptc_command_set_up_close_blockage_detection = 0x58;
 const uint8_t g_ptc_command_get_up_close_blockage_detection = 0x59;
+const uint8_t g_ptc_command_set_retro_multi_reflection_filtering = 0x47;
+const uint8_t g_ptc_command_get_retro_multi_reflection_filtering = 0x48;
 
 const uint8_t g_ptc_error_code_no_error = 0x00;
 const uint8_t g_ptc_error_code_invalid_input_param = 0x01;
@@ -323,6 +325,13 @@ class HesaiHwInterface
   /// @brief Getting values with PTC_COMMAND_GET_UP_CLOSE_BLOCKAGE_DETECTION
   /// @return Resulting status
   bool get_up_close_blockage_detection();
+  /// @brief Setting values with PTC_COMMAND_SET_RETRO_MULTI_REFLECTION_FILTERING
+  /// @param enable Enable retro multi-reflection filtering
+  /// @return Resulting status
+  Status set_retro_multi_reflection_filtering(bool enable);
+  /// @brief Getting values with PTC_COMMAND_GET_RETRO_MULTI_REFLECTION_FILTERING
+  /// @return Resulting status
+  bool get_retro_multi_reflection_filtering();
 
   /**
    * @brief Given the HW interface's sensor configuration and a given calibration, set the sensor

nebula_hw_interfaces/src/nebula_hesai_hw_interfaces/hesai_hw_interface.cpp

@@ -652,6 +652,30 @@ bool HesaiHwInterface::get_up_close_blockage_detection()
   return response[0] > 0x00;
 }
 
+Status HesaiHwInterface::set_retro_multi_reflection_filtering(bool enable)
+{
+  std::vector<unsigned char> request_payload;
+  request_payload.emplace_back(enable ? 0x01 : 0x00);
+
+  auto response_or_err =
+    send_receive(g_ptc_command_set_retro_multi_reflection_filtering, request_payload);
+  response_or_err.value_or_throw(pretty_print_ptc_error(response_or_err.error_or({})));
+  return Status::OK;
+}
+
+bool HesaiHwInterface::get_retro_multi_reflection_filtering()
+{
+  auto response_or_err = send_receive(g_ptc_command_get_retro_multi_reflection_filtering);
+  auto response =
+    response_or_err.value_or_throw(pretty_print_ptc_error(response_or_err.error_or({})));
+
+  if (response.size() != 1) {
+    throw std::runtime_error("Unexpected payload size");
+  }
+
+  return response[0] > 0x00;
+}
+
 Status HesaiHwInterface::check_and_set_lidar_range(
   const HesaiCalibrationConfigurationBase & calibration)
 {
@@ -1197,6 +1221,21 @@ HesaiStatus HesaiHwInterface::check_and_set_config(
     }
   }
 
+  if (supports_retro_multi_reflection_filtering(sensor_configuration->sensor_model)) {
+    auto retro_filtering_currently_enabled = get_retro_multi_reflection_filtering();
+    if (
+      retro_filtering_currently_enabled != sensor_configuration->retro_multi_reflection_filtering) {
+      logger_->info(
+        "current lidar retro_multi_reflection_filtering: " +
+        std::to_string(retro_filtering_currently_enabled));
+      logger_->info(
+        "current configuration retro_multi_reflection_filtering: " +
+        std::to_string(sensor_configuration->retro_multi_reflection_filtering));
+      logger_->info("Setting retro_multi_reflection_filtering via TCP.");
+      set_retro_multi_reflection_filtering(sensor_configuration->retro_multi_reflection_filtering);
+    }
+  }
+
 #ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE
   logger_->debug("End CheckAndSetConfig(HesaiConfig)!");
 #endif

nebula_ros/config/lidar/hesai/Pandar128E4X.param.yaml

@@ -30,6 +30,7 @@
     ptp_lock_threshold: 100
     retry_hw: true
     dual_return_distance_threshold: 0.1
+    retro_multi_reflection_filtering: false
     hires_mode: true
     diagnostics:
       pointcloud_publish_rate:

nebula_ros/config/lidar/hesai/PandarQT128.param.yaml

@@ -30,6 +30,7 @@
     ptp_lock_threshold: 100
     retry_hw: true
     dual_return_distance_threshold: 0.1
+    retro_multi_reflection_filtering: false
     diagnostics:
       pointcloud_publish_rate:
         frequency_ok:

nebula_ros/schema/Pandar128E4X.schema.json

@@ -114,6 +114,9 @@
         "dual_return_distance_threshold": {
           "$ref": "sub/misc.json#/definitions/dual_return_distance_threshold"
         },
+        "retro_multi_reflection_filtering": {
+          "$ref": "sub/lidar_hesai.json#/definitions/retro_multi_reflection_filtering"
+        },
         "point_filters": {
           "$ref": "sub/misc.json#/definitions/point_filters"
         },

nebula_ros/schema/PandarQT128.schema.json

@@ -108,6 +108,9 @@
         "dual_return_distance_threshold": {
           "$ref": "sub/misc.json#/definitions/dual_return_distance_threshold"
         },
+        "retro_multi_reflection_filtering": {
+          "$ref": "sub/lidar_hesai.json#/definitions/retro_multi_reflection_filtering"
+        },
         "point_filters": {
           "$ref": "sub/misc.json#/definitions/point_filters"
         },

nebula_ros/schema/sub/lidar_hesai.json

@@ -109,6 +109,11 @@
       "description": "Whether to turn on or off the sensor's high resolution mode",
       "default": false
     },
+    "retro_multi_reflection_filtering": {
+      "type": "boolean",
+      "description": "Whether to turn on or off retro multi-reflection filtering",
+      "default": false
+    },
     "blockage_mask_output": {
       "type": "object",
       "description": "Enables blockage detection and outputs a blockage mask per scan",

nebula_ros/src/hesai/hesai_ros_wrapper.cpp

@@ -189,6 +189,11 @@ nebula::Status HesaiRosWrapper::declare_and_get_sensor_config_params()
     config.hires_mode = this->declare_parameter<bool>("hires_mode", param_read_write());
   }
 
+  if (drivers::supports_retro_multi_reflection_filtering(config.sensor_model)) {
+    config.retro_multi_reflection_filtering =
+      this->declare_parameter<bool>("retro_multi_reflection_filtering", param_read_write());
+  }
+
   {
     rcl_interfaces::msg::ParameterDescriptor descriptor = param_read_write();
     RCLCPP_DEBUG_STREAM(get_logger(), config.sensor_model);
@@ -480,6 +485,12 @@ rcl_interfaces::msg::SetParametersResult HesaiRosWrapper::on_parameter_change(
     get_param(
       p, "blockage_mask_output.horizontal_bin_size_mdeg", blockage_mask_horizontal_bin_size_mdeg);
 
+  if (drivers::supports_retro_multi_reflection_filtering(sensor_cfg_ptr_->sensor_model)) {
+    got_any =
+      get_param(p, "retro_multi_reflection_filtering", new_cfg.retro_multi_reflection_filtering) |
+      got_any;
+  }
+
   // Currently, all of the sub-wrappers read-only parameters, so they do not be queried for updates
 
   if (!got_any) {