diff --git a/perception/ground_segmentation/CMakeLists.txt b/perception/ground_segmentation/CMakeLists.txt index bdd793896062a..656ffec486c3c 100644 --- a/perception/ground_segmentation/CMakeLists.txt +++ b/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 diff --git a/perception/ground_segmentation/config/ground_segmentation.param.yaml b/perception/ground_segmentation/config/ground_segmentation.param.yaml index 8d56e12244716..756882391183e 100644 --- a/perception/ground_segmentation/config/ground_segmentation.param.yaml +++ b/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 diff --git a/perception/ground_segmentation/config/ransac_ground_filter.param.yaml b/perception/ground_segmentation/config/ransac_ground_filter.param.yaml new file mode 100644 index 0000000000000..0d4a7d574499c --- /dev/null +++ b/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 diff --git a/perception/ground_segmentation/config/ray_ground_filter.param.yaml b/perception/ground_segmentation/config/ray_ground_filter.param.yaml new file mode 100644 index 0000000000000..5b9e8c06595f7 --- /dev/null +++ b/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 diff --git a/perception/ground_segmentation/config/scan_ground_filter.param.yaml b/perception/ground_segmentation/config/scan_ground_filter.param.yaml new file mode 100644 index 0000000000000..bc02a1ab7e6e7 --- /dev/null +++ b/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 diff --git a/perception/ground_segmentation/launch/ransac_ground_filter.launch.xml b/perception/ground_segmentation/launch/ransac_ground_filter.launch.xml new file mode 100644 index 0000000000000..f8280b774c6b0 --- /dev/null +++ b/perception/ground_segmentation/launch/ransac_ground_filter.launch.xml @@ -0,0 +1,11 @@ + + + + + + + + + + + diff --git a/perception/ground_segmentation/launch/ray_ground_filter.launch.xml b/perception/ground_segmentation/launch/ray_ground_filter.launch.xml new file mode 100644 index 0000000000000..86375c520a426 --- /dev/null +++ b/perception/ground_segmentation/launch/ray_ground_filter.launch.xml @@ -0,0 +1,11 @@ + + + + + + + + + + + diff --git a/perception/ground_segmentation/launch/scan_ground_filter.launch.xml b/perception/ground_segmentation/launch/scan_ground_filter.launch.xml new file mode 100644 index 0000000000000..2a9e4983ecb56 --- /dev/null +++ b/perception/ground_segmentation/launch/scan_ground_filter.launch.xml @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/perception/ground_segmentation/src/ransac_ground_filter_nodelet.cpp b/perception/ground_segmentation/src/ransac_ground_filter_nodelet.cpp index ebcb02df2c614..ed5fb6abe7fe7 100644 --- a/perception/ground_segmentation/src/ransac_ground_filter_nodelet.cpp +++ b/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("base_frame", "base_link"); + unit_axis_ = declare_parameter("unit_axis"); + max_iterations_ = declare_parameter("max_iterations"); + min_inliers_ = declare_parameter("min_trial"); + min_points_ = declare_parameter("min_points"); + outlier_threshold_ = declare_parameter("outlier_threshold"); + plane_slope_threshold_ = declare_parameter("plane_slope_threshold"); + voxel_size_x_ = declare_parameter("voxel_size_x"); + voxel_size_y_ = declare_parameter("voxel_size_y"); + voxel_size_z_ = declare_parameter("voxel_size_z"); + height_threshold_ = declare_parameter("height_threshold"); + debug_ = declare_parameter("debug"); if (unit_axis_ == "x") { unit_vec_ = Eigen::Vector3d::UnitX(); diff --git a/perception/ground_segmentation/src/ray_ground_filter_nodelet.cpp b/perception/ground_segmentation/src/ray_ground_filter_nodelet.cpp index 4b2e81d272e30..7bcae47fd9f1f 100644 --- a/perception/ground_segmentation/src/ray_ground_filter_nodelet.cpp +++ b/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("min_x"); + max_x_ = declare_parameter("max_x"); + min_y_ = declare_parameter("min_y"); + max_y_ = declare_parameter("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("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("general_max_slope"); + local_max_slope_ = declare_parameter("local_max_slope"); + initial_max_slope_ = declare_parameter("initial_max_slope"); + radial_divider_angle_ = declare_parameter("radial_divider_angle"); + min_height_threshold_ = declare_parameter("min_height_threshold"); + concentric_divider_distance_ = declare_parameter("concentric_divider_distance"); + reclass_distance_threshold_ = declare_parameter("reclass_distance_threshold"); } using std::placeholders::_1; diff --git a/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp b/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp index 953a9feb4d21e..f164e0102e0e9 100644 --- a/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp +++ b/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(declare_parameter("low_priority_region_x", -20.0f)); - detection_range_z_max_ = static_cast(declare_parameter("detection_range_z_max", 2.5f)); - center_pcl_shift_ = static_cast(declare_parameter("center_pcl_shift", 0.0)); - non_ground_height_threshold_ = - static_cast(declare_parameter("non_ground_height_threshold", 0.20)); - grid_mode_switch_radius_ = - static_cast(declare_parameter("grid_mode_switch_radius", 20.0)); - - grid_size_m_ = static_cast(declare_parameter("grid_size_m", 0.5)); - gnd_grid_buffer_size_ = static_cast(declare_parameter("gnd_grid_buffer_size", 4)); - elevation_grid_mode_ = static_cast(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("low_priority_region_x"); + detection_range_z_max_ = declare_parameter("detection_range_z_max"); + center_pcl_shift_ = declare_parameter("center_pcl_shift"); + non_ground_height_threshold_ = declare_parameter("non_ground_height_threshold"); + grid_mode_switch_radius_ = declare_parameter("grid_mode_switch_radius"); + + grid_size_m_ = declare_parameter("grid_size_m"); + gnd_grid_buffer_size_ = declare_parameter("gnd_grid_buffer_size"); + elevation_grid_mode_ = declare_parameter("elevation_grid_mode"); + global_slope_max_angle_rad_ = deg2rad(declare_parameter("global_slope_max_angle_deg")); + local_slope_max_angle_rad_ = deg2rad(declare_parameter("local_slope_max_angle_deg")); + radial_divider_angle_rad_ = deg2rad(declare_parameter("radial_divider_angle_deg")); + split_points_distance_tolerance_ = declare_parameter("split_points_distance_tolerance"); + split_height_distance_ = declare_parameter("split_height_distance"); + use_virtual_ground_point_ = declare_parameter("use_virtual_ground_point"); + use_recheck_ground_cluster_ = declare_parameter("use_recheck_ground_cluster"); radial_dividers_num_ = std::ceil(2.0 * M_PI / radial_divider_angle_rad_); vehicle_info_ = VehicleInfoUtil(*this).getVehicleInfo(); diff --git a/perception/ground_segmentation/test/test_ray_ground_filter.cpp b/perception/ground_segmentation/test/test_ray_ground_filter.cpp index af8bb29ab12b7..0c1db47a5ae09 100644 --- a/perception/ground_segmentation/test/test_ray_ground_filter.cpp +++ b/perception/ground_segmentation/test/test_ray_ground_filter.cpp @@ -29,7 +29,7 @@ #include #include #endif - +#include 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(); + double max_x_ = params["max_x"].as(); + double min_y_ = params["min_y"].as(); + double max_y_ = params["max_y"].as(); + bool use_vehicle_footprint_ = params["use_vehicle_footprint"].as(); + double general_max_slope_ = params["general_max_slope"].as(); + double local_max_slope_ = params["local_max_slope"].as(); + double initial_max_slope_ = params["initial_max_slope"].as(); + double radial_divider_angle_ = params["radial_divider_angle"].as(); + double min_height_threshold_ = params["min_height_threshold"].as(); + double concentric_divider_distance_ = params["concentric_divider_distance"].as(); + double reclass_distance_threshold_ = params["reclass_distance_threshold"].as(); + const auto pcd_path = share_dir + "/data/test.pcd"; pcl::PointCloud cloud; pcl::io::loadPCDFile(pcd_path, cloud); @@ -94,10 +111,23 @@ TEST_F(RayGroundFilterComponentTestSuite, TestCase1) rclcpp::NodeOptions node_options; std::vector 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(node_options); ray_ground_filter_test->input_pointcloud_pub_->publish(*input_msg_ptr); diff --git a/perception/ground_segmentation/test/test_scan_ground_filter.cpp b/perception/ground_segmentation/test/test_scan_ground_filter.cpp index acc81f8817cd8..9df9c7e9c7433 100644 --- a/perception/ground_segmentation/test/test_scan_ground_filter.cpp +++ b/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("ScanGroundFilterTest"); input_pointcloud_pub_ = rclcpp::create_publisher( 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(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(); local_slope_max_angle_deg_ = params["local_slope_max_angle_deg"].as(); split_points_distance_tolerance_ = params["split_points_distance_tolerance"].as(); @@ -127,6 +151,11 @@ class ScanGroundFilterTest : public ::testing::Test gnd_grid_buffer_size_ = params["gnd_grid_buffer_size"].as(); detection_range_z_max_ = params["detection_range_z_max"].as(); elevation_grid_mode_ = params["elevation_grid_mode"].as(); + low_priority_region_x_ = params["low_priority_region_x"].as(); + use_virtual_ground_point_ = params["use_virtual_ground_point"].as(); + center_pcl_shift_ = params["center_pcl_shift"].as(); + radial_divider_angle_deg_ = params["radial_divider_angle_deg"].as(); + use_recheck_ground_cluster_ = params["use_recheck_ground_cluster"].as(); } float global_slope_max_angle_deg_ = 0.0; @@ -139,6 +168,11 @@ 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) @@ -146,27 +180,6 @@ 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);