feat: range param for hesai (#121)

* add range parameter Signed-off-by: Yukihiro Saito <[email protected]> * add test Signed-off-by: Yukihiro Saito <[email protected]> * fix test param Signed-off-by: Yukihiro Saito <[email protected]> * hesai tests. set default min max range depending on model Signed-off-by: amc-nu <[email protected]> --------- Signed-off-by: Yukihiro Saito <[email protected]> Signed-off-by: amc-nu <[email protected]> Co-authored-by: amc-nu <[email protected]>
tier4 · Feb 28, 2024 · 4651feb · 4651feb
1 parent 76526ca
commit 4651feb
Show file tree

Hide file tree

Showing 6 changed files with 77 additions and 3 deletions.
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
@@ -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
@@ -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<double>("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<double>("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
@@ -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<double>("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<double>("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
@@ -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<void(uint64_t, uint64_t, nebula::drivers::NebulaPointCloudPtr)> scan_callback);
+  void ReadBag(
+    std::function<void(uint64_t, uint64_t, nebula::drivers::NebulaPointCloudPtr)> scan_callback);
 
   HesaiRosDecoderTestParams params_;
 };

diff --git 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
@@ -24,37 +24,66 @@ 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",
     "LastStrongest",
     "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.