diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_decoder.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_decoder.hpp index cc589e929..7bdfd069e 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_decoder.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_decoder.hpp @@ -102,7 +102,10 @@ class HesaiDecoder : public HesaiScanDecoder auto distance = getDistance(unit); - if (distance < SensorT::MIN_RANGE || distance > SensorT::MAX_RANGE) { + if ( + distance < SensorT::MIN_RANGE || SensorT::MAX_RANGE < distance || + distance < sensor_configuration_->min_range || + sensor_configuration_->max_range < distance) { continue; } diff --git a/nebula_ros/src/hesai/hesai_decoder_ros_wrapper.cpp b/nebula_ros/src/hesai/hesai_decoder_ros_wrapper.cpp index d8e206995..573de5696 100644 --- a/nebula_ros/src/hesai/hesai_decoder_ros_wrapper.cpp +++ b/nebula_ros/src/hesai/hesai_decoder_ros_wrapper.cpp @@ -220,6 +220,24 @@ Status HesaiDriverRosWrapper::GetParameters( calibration_configuration.calibration_file = this->get_parameter("calibration_file").as_string(); } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; + descriptor.read_only = false; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("min_range", 0.3, descriptor); + sensor_configuration.min_range = this->get_parameter("min_range").as_double(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; + descriptor.read_only = false; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("max_range", 300., descriptor); + sensor_configuration.max_range = this->get_parameter("max_range").as_double(); + } if (sensor_configuration.sensor_model == drivers::SensorModel::HESAI_PANDARAT128) { rcl_interfaces::msg::ParameterDescriptor descriptor; descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; diff --git a/nebula_tests/hesai/hesai_ros_decoder_test.cpp b/nebula_tests/hesai/hesai_ros_decoder_test.cpp index bc6852d2d..a07a4dabd 100644 --- a/nebula_tests/hesai/hesai_ros_decoder_test.cpp +++ b/nebula_tests/hesai/hesai_ros_decoder_test.cpp @@ -148,6 +148,24 @@ Status HesaiRosDecoderTest::GetParameters( calibration_configuration.calibration_file = this->get_parameter("calibration_file").as_string(); } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("min_range", params_.min_range, descriptor); + sensor_configuration.min_range = this->get_parameter("min_range").as_double(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("max_range", params_.max_range, descriptor); + sensor_configuration.max_range = this->get_parameter("max_range").as_double(); + } if (sensor_configuration.sensor_model == drivers::SensorModel::HESAI_PANDARAT128) { rcl_interfaces::msg::ParameterDescriptor descriptor; descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; @@ -217,7 +235,8 @@ Status HesaiRosDecoderTest::GetParameters( if (sensor_configuration.return_mode == nebula::drivers::ReturnMode::UNKNOWN) { return Status::INVALID_ECHO_MODE; } - if (sensor_configuration.frame_id.empty() || sensor_configuration.scan_phase > 360) { + if ( + sensor_configuration.frame_id.empty() || sensor_configuration.scan_phase > 360) { return Status::SENSOR_CONFIG_ERROR; } if (calibration_configuration.calibration_file.empty()) { diff --git a/nebula_tests/hesai/hesai_ros_decoder_test.hpp b/nebula_tests/hesai/hesai_ros_decoder_test.hpp index b66c996f8..a0477a366 100644 --- a/nebula_tests/hesai/hesai_ros_decoder_test.hpp +++ b/nebula_tests/hesai/hesai_ros_decoder_test.hpp @@ -40,6 +40,8 @@ struct HesaiRosDecoderTestParams std::string correction_file = ""; std::string frame_id = "hesai"; double scan_phase = 0.; + double min_range = 0.3; + double max_range = 300.; std::string storage_id = "sqlite3"; std::string format = "cdr"; std::string target_topic = "/pandar_packets"; @@ -108,7 +110,8 @@ class HesaiRosDecoderTest final : public rclcpp::Node, NebulaDriverRosWrapperBas /// @brief Read the specified bag file and compare the constructed point clouds with the /// corresponding PCD files - void ReadBag(std::function scan_callback); + void ReadBag( + std::function scan_callback); HesaiRosDecoderTestParams params_; }; diff --git a/nebula_tests/hesai/hesai_ros_decoder_test.yaml b/nebula_tests/hesai/hesai_ros_decoder_test.yaml index 0878791a3..ea4b66a98 100644 --- a/nebula_tests/hesai/hesai_ros_decoder_test.yaml +++ b/nebula_tests/hesai/hesai_ros_decoder_test.yaml @@ -12,6 +12,8 @@ rotation_speed: 200 # Motor RPM, the sensor's internal spin rate. cloud_min_angle: 0 # Field of View, start degrees. cloud_max_angle: 360 # Field of View, end degrees. + min_range: 0.3 # Minimum range. + max_range: 300. # Maximum range. diag_span: 1000 # milliseconds calibration_file: "./install/nebula_decoders/share/nebula_decoders/calibration/hesai/PandarAT128.csv" correction_file: "./install/nebula_decoders/share/nebula_decoders/calibration/hesai/PandarAT128.dat" diff --git a/nebula_tests/hesai/hesai_ros_decoder_test_main.cpp b/nebula_tests/hesai/hesai_ros_decoder_test_main.cpp index feda648f0..96ff00c3c 100644 --- a/nebula_tests/hesai/hesai_ros_decoder_test_main.cpp +++ b/nebula_tests/hesai/hesai_ros_decoder_test_main.cpp @@ -24,12 +24,22 @@ const nebula::ros::HesaiRosDecoderTestParams TEST_CONFIGS[6] = { "Dual", "Pandar40P.csv", "40p/1673400149412331409", + "", + "hesai", + 0, + 0.3f, + 200.f }, { "Pandar64", "Dual", "Pandar64.csv", "64/1673403880599376836", + "", + "hesai", + 0, + 0.3f, + 200.f }, { "PandarAT128", @@ -37,24 +47,43 @@ const nebula::ros::HesaiRosDecoderTestParams TEST_CONFIGS[6] = { "PandarAT128.csv", "at128/1679653308406038376", "PandarAT128.dat", + "hesai", + 0, + 1.f, + 180.f }, { "PandarQT64", "Dual", "PandarQT64.csv", "qt64/1673401195788312575", + "", + "hesai", + 0, + 0.1f, + 60.f }, { "PandarXT32", "Dual", "PandarXT32.csv", "xt32/1673400677802009732", + "", + "hesai", + 0, + 0.05f, + 120.f }, { "PandarXT32M", "LastStrongest", "PandarXT32M.csv", "xt32m/1660893203042895158", + "", + "hesai", + 0, + 0.5f, + 300.f }}; // Compares geometrical output of decoder against pre-recorded reference pointcloud.