chore(ground_segmentation): rework params (#6209)

* chore: remove default params

Signed-off-by: badai-nguyen <[email protected]>

* fix: scan ground gtest

Signed-off-by: badai-nguyen <[email protected]>

* fix: ray ground filter gtest

Signed-off-by: badai-nguyen <[email protected]>

---------

Signed-off-by: badai-nguyen <[email protected]>

perception/ground_segmentation/CMakeLists.txt

@@ -89,6 +89,7 @@ if(BUILD_TESTING)
 
   target_link_libraries(test_ray_ground_filter
     ground_segmentation
+    ${YAML_CPP_LIBRARIES}
   )
 
   ament_add_ros_isolated_gtest(test_scan_ground_filter

perception/ground_segmentation/config/ground_segmentation.param.yaml

@@ -29,3 +29,7 @@
         gnd_grid_buffer_size: 4
         detection_range_z_max: 2.5
         elevation_grid_mode: true
+        low_priority_region_x: -20.0
+        center_pcl_shift: 0.0
+        radial_divider_angle_deg: 1.0
+        use_recheck_ground_cluster: true

perception/ground_segmentation/config/ransac_ground_filter.param.yaml

@@ -0,0 +1,14 @@
+/**:
+  ros__parameters:
+    base_frame: "base_link"
+    unit_axis: "z"
+    max_iterations: 1000
+    min_trial: 5000
+    min_points: 1000
+    outlier_threshold: 0.01
+    plane_slope_threshold: 10.0
+    voxel_size_x: 0.04
+    voxel_size_y: 0.04
+    voxel_size_z: 0.04
+    height_threshold: 0.01
+    debug: false

perception/ground_segmentation/config/ray_ground_filter.param.yaml

@@ -0,0 +1,14 @@
+/**:
+  ros__parameters:
+      min_x: -0.01
+      max_x: 0.01
+      min_y: -0.01
+      max_y: 0.01
+      use_vehicle_footprint: false
+      general_max_slope: 8.0
+      local_max_slope: 6.0
+      initial_max_slope: 3.0
+      radial_divider_angle: 1.0
+      min_height_threshold: 0.15
+      concentric_divider_distance: 0.0
+      reclass_distance_threshold: 0.1

perception/ground_segmentation/config/scan_ground_filter.param.yaml

@@ -0,0 +1,17 @@
+/**:
+  ros__parameters:
+    global_slope_max_angle_deg: 10.0
+    local_slope_max_angle_deg: 13.0 # recommended 30.0 for non elevation_grid_mode
+    split_points_distance_tolerance: 0.2
+    use_virtual_ground_point: True
+    split_height_distance: 0.2
+    non_ground_height_threshold: 0.20
+    grid_size_m: 0.1
+    grid_mode_switch_radius: 20.0
+    gnd_grid_buffer_size: 4
+    detection_range_z_max: 2.5
+    elevation_grid_mode: true
+    low_priority_region_x: -20.0
+    center_pcl_shift: 0.0
+    radial_divider_angle_deg: 1.0
+    use_recheck_ground_cluster: true

perception/ground_segmentation/launch/ransac_ground_filter.launch.xml

@@ -0,0 +1,11 @@
+<?xml version="1.0"?>
+<launch>
+  <arg name="ground_segmentation_param_file" default="$(find-pkg-share ground_segmentation)/config/ransac_ground_filter.param.yaml"/>
+  <arg name="input/pointcloud" default="/sensing/lidar/concatenated/pointcloud"/>
+  <arg name="output/pointcloud" default="/perception/obstacle_segmentation/pointcloud"/>
+  <node pkg="ground_segmentation" exec="ransac_ground_filter_node" name="ransac_ground_filter_node" output="screen">
+    <remap from="input" to="$(var input/pointcloud)"/>
+    <remap from="output" to="$(var output/pointcloud)"/>
+    <param from="$(var ground_segmentation_param_file)"/>
+  </node>
+</launch>

perception/ground_segmentation/launch/ray_ground_filter.launch.xml

@@ -0,0 +1,11 @@
+<?xml version="1.0"?>
+<launch>
+  <arg name="ground_segmentation_param_file" default="$(find-pkg-share ground_segmentation)/config/ray_ground_filter.param.yaml"/>
+  <arg name="input/pointcloud" default="/sensing/lidar/concatenated/pointcloud"/>
+  <arg name="output/pointcloud" default="/perception/obstacle_segmentation/pointcloud"/>
+  <node pkg="ground_segmentation" exec="ray_ground_filter_node" name="ray_ground_filter_node" output="screen">
+    <remap from="input" to="$(var input/pointcloud)"/>
+    <remap from="output" to="$(var output/pointcloud)"/>
+    <param from="$(var ground_segmentation_param_file)"/>
+  </node>
+</launch>

perception/ground_segmentation/launch/scan_ground_filter.launch.xml

@@ -0,0 +1,15 @@
+<?xml version="1.0"?>
+<launch>
+  <arg name="ground_segmentation_param_file" default="$(find-pkg-share ground_segmentation)/config/scan_ground_filter.param.yaml"/>
+
+  <!-- vehicle info -->
+  <arg name="vehicle_info_param_file" default="$(find-pkg-share vehicle_info_util)/config/vehicle_info.param.yaml"/>
+  <arg name="input/pointcloud" default="/sensing/lidar/concatenated/pointcloud"/>
+  <arg name="output/pointcloud" default="/perception/obstacle_segmentation/pointcloud"/>
+  <node pkg="ground_segmentation" exec="scan_ground_filter_node" name="scan_ground_filter_node" output="screen">
+    <remap from="input" to="$(var input/pointcloud)"/>
+    <remap from="output" to="$(var output/pointcloud)"/>
+    <param from="$(var ground_segmentation_param_file)"/>
+    <param from="$(var vehicle_info_param_file)"/>
+  </node>
+</launch>

perception/ground_segmentation/src/ransac_ground_filter_nodelet.cpp

@@ -85,18 +85,18 @@ using pointcloud_preprocessor::get_param;
 RANSACGroundFilterComponent::RANSACGroundFilterComponent(const rclcpp::NodeOptions & options)
 : Filter("RANSACGroundFilter", options)
 {
-  base_frame_ = declare_parameter("base_frame", "base_link");
-  unit_axis_ = declare_parameter("unit_axis", "z");
-  max_iterations_ = declare_parameter("max_iterations", 1000);
-  min_inliers_ = declare_parameter("min_trial", 5000);
-  min_points_ = declare_parameter("min_points", 1000);
-  outlier_threshold_ = declare_parameter("outlier_threshold", 0.01);
-  plane_slope_threshold_ = declare_parameter("plane_slope_threshold", 10.0);
-  voxel_size_x_ = declare_parameter("voxel_size_x", 0.04);
-  voxel_size_y_ = declare_parameter("voxel_size_y", 0.04);
-  voxel_size_z_ = declare_parameter("voxel_size_z", 0.04);
-  height_threshold_ = declare_parameter("height_threshold", 0.01);
-  debug_ = declare_parameter("debug", false);
+  base_frame_ = declare_parameter<std::string>("base_frame", "base_link");
+  unit_axis_ = declare_parameter<std::string>("unit_axis");
+  max_iterations_ = declare_parameter<int>("max_iterations");
+  min_inliers_ = declare_parameter<int>("min_trial");
+  min_points_ = declare_parameter<int>("min_points");
+  outlier_threshold_ = declare_parameter<double>("outlier_threshold");
+  plane_slope_threshold_ = declare_parameter<double>("plane_slope_threshold");
+  voxel_size_x_ = declare_parameter<double>("voxel_size_x");
+  voxel_size_y_ = declare_parameter<double>("voxel_size_y");
+  voxel_size_z_ = declare_parameter<double>("voxel_size_z");
+  height_threshold_ = declare_parameter<double>("height_threshold");
+  debug_ = declare_parameter<bool>("debug");
 
   if (unit_axis_ == "x") {
     unit_vec_ = Eigen::Vector3d::UnitX();

perception/ground_segmentation/src/ray_ground_filter_nodelet.cpp

@@ -48,22 +48,22 @@ RayGroundFilterComponent::RayGroundFilterComponent(const rclcpp::NodeOptions & o
     grid_precision_ = 0.2;
     ray_ground_filter::generateColors(colors_, color_num_);
 
-    min_x_ = declare_parameter("min_x", -0.01);
-    max_x_ = declare_parameter("max_x", 0.01);
-    min_y_ = declare_parameter("min_y", -0.01);
-    max_y_ = declare_parameter("max_y", 0.01);
+    min_x_ = declare_parameter<double>("min_x");
+    max_x_ = declare_parameter<double>("max_x");
+    min_y_ = declare_parameter<double>("min_y");
+    max_y_ = declare_parameter<double>("max_y");
 
     setVehicleFootprint(min_x_, max_x_, min_y_, max_y_);
 
-    use_vehicle_footprint_ = declare_parameter("use_vehicle_footprint", false);
+    use_vehicle_footprint_ = declare_parameter<bool>("use_vehicle_footprint", false);
 
-    general_max_slope_ = declare_parameter("general_max_slope", 8.0);
-    local_max_slope_ = declare_parameter("local_max_slope", 6.0);
-    initial_max_slope_ = declare_parameter("initial_max_slope", 3.0);
-    radial_divider_angle_ = declare_parameter("radial_divider_angle", 1.0);
-    min_height_threshold_ = declare_parameter("min_height_threshold", 0.15);
-    concentric_divider_distance_ = declare_parameter("concentric_divider_distance", 0.0);
-    reclass_distance_threshold_ = declare_parameter("reclass_distance_threshold", 0.1);
+    general_max_slope_ = declare_parameter<double>("general_max_slope");
+    local_max_slope_ = declare_parameter<double>("local_max_slope");
+    initial_max_slope_ = declare_parameter<double>("initial_max_slope");
+    radial_divider_angle_ = declare_parameter<double>("radial_divider_angle");
+    min_height_threshold_ = declare_parameter<double>("min_height_threshold");
+    concentric_divider_distance_ = declare_parameter<double>("concentric_divider_distance");
+    reclass_distance_threshold_ = declare_parameter<double>("reclass_distance_threshold");
   }
 
   using std::placeholders::_1;

perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp

@@ -37,24 +37,22 @@ ScanGroundFilterComponent::ScanGroundFilterComponent(const rclcpp::NodeOptions &
 {
   // set initial parameters
   {
-    low_priority_region_x_ = static_cast<float>(declare_parameter("low_priority_region_x", -20.0f));
-    detection_range_z_max_ = static_cast<float>(declare_parameter("detection_range_z_max", 2.5f));
-    center_pcl_shift_ = static_cast<float>(declare_parameter("center_pcl_shift", 0.0));
-    non_ground_height_threshold_ =
-      static_cast<float>(declare_parameter("non_ground_height_threshold", 0.20));
-    grid_mode_switch_radius_ =
-      static_cast<float>(declare_parameter("grid_mode_switch_radius", 20.0));
-
-    grid_size_m_ = static_cast<float>(declare_parameter("grid_size_m", 0.5));
-    gnd_grid_buffer_size_ = static_cast<int>(declare_parameter("gnd_grid_buffer_size", 4));
-    elevation_grid_mode_ = static_cast<bool>(declare_parameter("elevation_grid_mode", true));
-    global_slope_max_angle_rad_ = deg2rad(declare_parameter("global_slope_max_angle_deg", 8.0));
-    local_slope_max_angle_rad_ = deg2rad(declare_parameter("local_slope_max_angle_deg", 10.0));
-    radial_divider_angle_rad_ = deg2rad(declare_parameter("radial_divider_angle_deg", 1.0));
-    split_points_distance_tolerance_ = declare_parameter("split_points_distance_tolerance", 0.2);
-    split_height_distance_ = declare_parameter("split_height_distance", 0.2);
-    use_virtual_ground_point_ = declare_parameter("use_virtual_ground_point", true);
-    use_recheck_ground_cluster_ = declare_parameter("use_recheck_ground_cluster", true);
+    low_priority_region_x_ = declare_parameter<float>("low_priority_region_x");
+    detection_range_z_max_ = declare_parameter<float>("detection_range_z_max");
+    center_pcl_shift_ = declare_parameter<float>("center_pcl_shift");
+    non_ground_height_threshold_ = declare_parameter<float>("non_ground_height_threshold");
+    grid_mode_switch_radius_ = declare_parameter<float>("grid_mode_switch_radius");
+
+    grid_size_m_ = declare_parameter<float>("grid_size_m");
+    gnd_grid_buffer_size_ = declare_parameter<int>("gnd_grid_buffer_size");
+    elevation_grid_mode_ = declare_parameter<bool>("elevation_grid_mode");
+    global_slope_max_angle_rad_ = deg2rad(declare_parameter<float>("global_slope_max_angle_deg"));
+    local_slope_max_angle_rad_ = deg2rad(declare_parameter<float>("local_slope_max_angle_deg"));
+    radial_divider_angle_rad_ = deg2rad(declare_parameter<float>("radial_divider_angle_deg"));
+    split_points_distance_tolerance_ = declare_parameter<float>("split_points_distance_tolerance");
+    split_height_distance_ = declare_parameter<float>("split_height_distance");
+    use_virtual_ground_point_ = declare_parameter<bool>("use_virtual_ground_point");
+    use_recheck_ground_cluster_ = declare_parameter<bool>("use_recheck_ground_cluster");
     radial_dividers_num_ = std::ceil(2.0 * M_PI / radial_divider_angle_rad_);
     vehicle_info_ = VehicleInfoUtil(*this).getVehicleInfo();
 

perception/ground_segmentation/test/test_ray_ground_filter.cpp

@@ -29,7 +29,7 @@
 #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
 #include <tf2_sensor_msgs/tf2_sensor_msgs.hpp>
 #endif
-
+#include <yaml-cpp/yaml.h>
 class RayGroundFilterComponentTestSuite : public ::testing::Test
 {
 protected:
@@ -63,8 +63,25 @@ class RayGroundFilterComponentTest : public ground_segmentation::RayGroundFilter
 
 TEST_F(RayGroundFilterComponentTestSuite, TestCase1)
 {
-  // read pcd to pointcloud
   const auto share_dir = ament_index_cpp::get_package_share_directory("ground_segmentation");
+  const auto config_path = share_dir + "/config/ray_ground_filter.param.yaml";
+  // std::cout << "config_path:" << config_path << std::endl;
+  YAML::Node config = YAML::LoadFile(config_path);
+  auto params = config["/**"]["ros__parameters"];
+
+  double min_x_ = params["min_x"].as<float>();
+  double max_x_ = params["max_x"].as<float>();
+  double min_y_ = params["min_y"].as<float>();
+  double max_y_ = params["max_y"].as<float>();
+  bool use_vehicle_footprint_ = params["use_vehicle_footprint"].as<bool>();
+  double general_max_slope_ = params["general_max_slope"].as<float>();
+  double local_max_slope_ = params["local_max_slope"].as<float>();
+  double initial_max_slope_ = params["initial_max_slope"].as<float>();
+  double radial_divider_angle_ = params["radial_divider_angle"].as<float>();
+  double min_height_threshold_ = params["min_height_threshold"].as<float>();
+  double concentric_divider_distance_ = params["concentric_divider_distance"].as<float>();
+  double reclass_distance_threshold_ = params["reclass_distance_threshold"].as<float>();
+
   const auto pcd_path = share_dir + "/data/test.pcd";
   pcl::PointCloud<pcl::PointXYZ> cloud;
   pcl::io::loadPCDFile<pcl::PointXYZ>(pcd_path, cloud);
@@ -94,10 +111,23 @@ TEST_F(RayGroundFilterComponentTestSuite, TestCase1)
 
   rclcpp::NodeOptions node_options;
   std::vector<rclcpp::Parameter> parameters;
+
   parameters.emplace_back(rclcpp::Parameter("base_frame", "base_link"));
-  parameters.emplace_back(rclcpp::Parameter("general_max_slope", 2.0));
-  parameters.emplace_back(rclcpp::Parameter("local_max_slope", 3.0));
-  parameters.emplace_back(rclcpp::Parameter("initial_max_slope", 1.0));
+  parameters.emplace_back(rclcpp::Parameter("general_max_slope", general_max_slope_));
+  parameters.emplace_back(rclcpp::Parameter("local_max_slope", local_max_slope_));
+  parameters.emplace_back(rclcpp::Parameter("initial_max_slope", initial_max_slope_));
+  parameters.emplace_back(rclcpp::Parameter("radial_divider_angle", radial_divider_angle_));
+  parameters.emplace_back(rclcpp::Parameter("min_height_threshold", min_height_threshold_));
+  parameters.emplace_back(
+    rclcpp::Parameter("concentric_divider_distance", concentric_divider_distance_));
+  parameters.emplace_back(
+    rclcpp::Parameter("reclass_distance_threshold", reclass_distance_threshold_));
+  parameters.emplace_back(rclcpp::Parameter("min_x", min_x_));
+  parameters.emplace_back(rclcpp::Parameter("max_x", max_x_));
+  parameters.emplace_back(rclcpp::Parameter("min_y", min_y_));
+  parameters.emplace_back(rclcpp::Parameter("max_y", max_y_));
+  parameters.emplace_back(rclcpp::Parameter("use_vehicle_footprint", use_vehicle_footprint_));
+
   node_options.parameter_overrides(parameters);
   auto ray_ground_filter_test = std::make_shared<RayGroundFilterComponentTest>(node_options);
   ray_ground_filter_test->input_pointcloud_pub_->publish(*input_msg_ptr);

perception/ground_segmentation/test/test_scan_ground_filter.cpp

@@ -38,6 +38,8 @@ class ScanGroundFilterTest : public ::testing::Test
   {
     rclcpp::init(0, nullptr);
 
+    parse_yaml();
+
     dummy_node_ = std::make_shared<rclcpp::Node>("ScanGroundFilterTest");
     input_pointcloud_pub_ = rclcpp::create_publisher<sensor_msgs::msg::PointCloud2>(
       dummy_node_, "/test_scan_ground_filter/input_cloud", 1);
@@ -58,6 +60,30 @@ class ScanGroundFilterTest : public ::testing::Test
     parameters.emplace_back(rclcpp::Parameter("right_overhang", 0.1));
     parameters.emplace_back(rclcpp::Parameter("vehicle_height", 2.5));
     parameters.emplace_back(rclcpp::Parameter("max_steer_angle", 0.7));
+
+    parameters.emplace_back(
+      rclcpp::Parameter("global_slope_max_angle_deg", global_slope_max_angle_deg_));
+    parameters.emplace_back(
+      rclcpp::Parameter("local_slope_max_angle_deg", local_slope_max_angle_deg_));
+    parameters.emplace_back(
+      rclcpp::Parameter("split_points_distance_tolerance", split_points_distance_tolerance_));
+    parameters.emplace_back(
+      rclcpp::Parameter("use_virtual_ground_point", use_virtual_ground_point_));
+    parameters.emplace_back(rclcpp::Parameter("split_height_distance", split_height_distance_));
+    parameters.emplace_back(
+      rclcpp::Parameter("non_ground_height_threshold", non_ground_height_threshold_));
+    parameters.emplace_back(rclcpp::Parameter("grid_size_m", grid_size_m_));
+    parameters.emplace_back(rclcpp::Parameter("grid_mode_switch_radius", grid_mode_switch_radius_));
+    parameters.emplace_back(rclcpp::Parameter("gnd_grid_buffer_size", gnd_grid_buffer_size_));
+    parameters.emplace_back(rclcpp::Parameter("detection_range_z_max", detection_range_z_max_));
+    parameters.emplace_back(rclcpp::Parameter("elevation_grid_mode", elevation_grid_mode_));
+    parameters.emplace_back(rclcpp::Parameter("low_priority_region_x", low_priority_region_x_));
+    parameters.emplace_back(rclcpp::Parameter("center_pcl_shift", center_pcl_shift_));
+    parameters.emplace_back(
+      rclcpp::Parameter("radial_divider_angle_deg", radial_divider_angle_deg_));
+    parameters.emplace_back(
+      rclcpp::Parameter("use_recheck_ground_cluster", use_recheck_ground_cluster_));
+
     options.parameter_overrides(parameters);
 
     scan_ground_filter_ = std::make_shared<ground_segmentation::ScanGroundFilterComponent>(options);
@@ -88,8 +114,6 @@ class ScanGroundFilterTest : public ::testing::Test
     t.transform.rotation.w = q.w();
 
     tf2::doTransform(*origin_input_msg_ptr, *input_msg_ptr_, t);
-
-    parse_yaml();
   }
 
   ScanGroundFilterTest() {}
@@ -113,10 +137,10 @@ class ScanGroundFilterTest : public ::testing::Test
   void parse_yaml()
   {
     const auto share_dir = ament_index_cpp::get_package_share_directory("ground_segmentation");
-    const auto config_path = share_dir + "/config/ground_segmentation.param.yaml";
+    const auto config_path = share_dir + "/config/scan_ground_filter.param.yaml";
     // std::cout << "config_path:" << config_path << std::endl;
     YAML::Node config = YAML::LoadFile(config_path);
-    auto params = config["/**"]["ros__parameters"]["common_ground_filter"]["parameters"];
+    auto params = config["/**"]["ros__parameters"];
     global_slope_max_angle_deg_ = params["global_slope_max_angle_deg"].as<float>();
     local_slope_max_angle_deg_ = params["local_slope_max_angle_deg"].as<float>();
     split_points_distance_tolerance_ = params["split_points_distance_tolerance"].as<float>();
@@ -127,6 +151,11 @@ class ScanGroundFilterTest : public ::testing::Test
     gnd_grid_buffer_size_ = params["gnd_grid_buffer_size"].as<uint16_t>();
     detection_range_z_max_ = params["detection_range_z_max"].as<float>();
     elevation_grid_mode_ = params["elevation_grid_mode"].as<bool>();
+    low_priority_region_x_ = params["low_priority_region_x"].as<float>();
+    use_virtual_ground_point_ = params["use_virtual_ground_point"].as<bool>();
+    center_pcl_shift_ = params["center_pcl_shift"].as<float>();
+    radial_divider_angle_deg_ = params["radial_divider_angle_deg"].as<float>();
+    use_recheck_ground_cluster_ = params["use_recheck_ground_cluster"].as<bool>();
   }
 
   float global_slope_max_angle_deg_ = 0.0;
@@ -139,34 +168,18 @@ class ScanGroundFilterTest : public ::testing::Test
   uint16_t gnd_grid_buffer_size_ = 0;
   float detection_range_z_max_ = 0.0;
   bool elevation_grid_mode_ = false;
+  float low_priority_region_x_ = 0.0;
+  bool use_virtual_ground_point_;
+  float center_pcl_shift_;
+  float radial_divider_angle_deg_;
+  bool use_recheck_ground_cluster_;
 };
 
 TEST_F(ScanGroundFilterTest, TestCase1)
 {
   input_pointcloud_pub_->publish(*input_msg_ptr_);
   sensor_msgs::msg::PointCloud2 out_cloud;
 
-  // set filter parameter
-  scan_ground_filter_->set_parameter(
-    rclcpp::Parameter("global_slope_max_angle_deg", global_slope_max_angle_deg_));
-  scan_ground_filter_->set_parameter(
-    rclcpp::Parameter("local_slope_max_angle_deg", local_slope_max_angle_deg_));
-  scan_ground_filter_->set_parameter(
-    rclcpp::Parameter("split_points_distance_tolerance", split_points_distance_tolerance_));
-  scan_ground_filter_->set_parameter(
-    rclcpp::Parameter("split_height_distance", split_height_distance_));
-  scan_ground_filter_->set_parameter(
-    rclcpp::Parameter("non_ground_height_threshold", non_ground_height_threshold_));
-  scan_ground_filter_->set_parameter(rclcpp::Parameter("grid_size_m", grid_size_m_));
-  scan_ground_filter_->set_parameter(
-    rclcpp::Parameter("grid_mode_switch_radius", grid_mode_switch_radius_));
-  scan_ground_filter_->set_parameter(
-    rclcpp::Parameter("gnd_grid_buffer_size", gnd_grid_buffer_size_));
-  scan_ground_filter_->set_parameter(
-    rclcpp::Parameter("detection_range_z_max", detection_range_z_max_));
-  scan_ground_filter_->set_parameter(
-    rclcpp::Parameter("elevation_grid_mode", elevation_grid_mode_));
-
   filter(out_cloud);
   output_pointcloud_pub_->publish(out_cloud);