diff --git a/nebula_common/include/nebula_common/hesai/hesai_common.hpp b/nebula_common/include/nebula_common/hesai/hesai_common.hpp index 5c7d51c25..78ebace47 100644 --- a/nebula_common/include/nebula_common/hesai/hesai_common.hpp +++ b/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 downsample_mask_path; bool hires_mode; std::optional blockage_mask_horizontal_bin_size_mdeg; + bool retro_multi_reflection_filtering; std::optional sync_diagnostics_topic; std::optional 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 diff --git a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp index 13667b967..3fca0bce4 100644 --- a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp +++ b/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 diff --git a/nebula_hw_interfaces/src/nebula_hesai_hw_interfaces/hesai_hw_interface.cpp b/nebula_hw_interfaces/src/nebula_hesai_hw_interfaces/hesai_hw_interface.cpp index 79736f979..ecbf3e04e 100644 --- a/nebula_hw_interfaces/src/nebula_hesai_hw_interfaces/hesai_hw_interface.cpp +++ b/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 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 diff --git a/nebula_ros/config/lidar/hesai/Pandar128E4X.param.yaml b/nebula_ros/config/lidar/hesai/Pandar128E4X.param.yaml index 4d5878cf5..c5cd798aa 100644 --- a/nebula_ros/config/lidar/hesai/Pandar128E4X.param.yaml +++ b/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: diff --git a/nebula_ros/config/lidar/hesai/PandarQT128.param.yaml b/nebula_ros/config/lidar/hesai/PandarQT128.param.yaml index 6e0ee152c..36203416d 100644 --- a/nebula_ros/config/lidar/hesai/PandarQT128.param.yaml +++ b/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: diff --git a/nebula_ros/schema/Pandar128E4X.schema.json b/nebula_ros/schema/Pandar128E4X.schema.json index 59927847d..5997ee055 100644 --- a/nebula_ros/schema/Pandar128E4X.schema.json +++ b/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" }, diff --git a/nebula_ros/schema/PandarQT128.schema.json b/nebula_ros/schema/PandarQT128.schema.json index 38f3f9f4d..add10e43d 100644 --- a/nebula_ros/schema/PandarQT128.schema.json +++ b/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" }, diff --git a/nebula_ros/schema/sub/lidar_hesai.json b/nebula_ros/schema/sub/lidar_hesai.json index 22480a194..0162b130d 100644 --- a/nebula_ros/schema/sub/lidar_hesai.json +++ b/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", diff --git a/nebula_ros/src/hesai/hesai_ros_wrapper.cpp b/nebula_ros/src/hesai/hesai_ros_wrapper.cpp index c00d44580..5adaba966 100644 --- a/nebula_ros/src/hesai/hesai_ros_wrapper.cpp +++ b/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("hires_mode", param_read_write()); } + if (drivers::supports_retro_multi_reflection_filtering(config.sensor_model)) { + config.retro_multi_reflection_filtering = + this->declare_parameter("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) {