diff --git a/common/autoware_auto_perception_rviz_plugin/package.xml b/common/autoware_auto_perception_rviz_plugin/package.xml index c1ad3e3df140e..2033239824d95 100644 --- a/common/autoware_auto_perception_rviz_plugin/package.xml +++ b/common/autoware_auto_perception_rviz_plugin/package.xml @@ -8,7 +8,8 @@ Satoshi Tanaka Taiki Tanaka Takeshi Miura - + Shunsuke Miura + Yoshi Ri Apache 2.0 ament_cmake diff --git a/common/object_recognition_utils/package.xml b/common/object_recognition_utils/package.xml index 2f2472515ebad..b77d6a082e52b 100644 --- a/common/object_recognition_utils/package.xml +++ b/common/object_recognition_utils/package.xml @@ -8,6 +8,7 @@ Satoshi Tanaka Yusuke Muramatsu Shunsuke Miura + Yoshi Ri Apache License 2.0 ament_cmake_auto diff --git a/common/perception_utils/package.xml b/common/perception_utils/package.xml index 5304d481737e5..c3d849df273f9 100644 --- a/common/perception_utils/package.xml +++ b/common/perception_utils/package.xml @@ -7,6 +7,7 @@ Satoshi Tanaka Yusuke Muramatsu Shunsuke Miura + Yoshi Ri Apache License 2.0 ament_cmake_auto diff --git a/control/control_validator/package.xml b/control/control_validator/package.xml index c46c2d237b605..faf254708ddbb 100644 --- a/control/control_validator/package.xml +++ b/control/control_validator/package.xml @@ -7,6 +7,9 @@ Kyoichi Sugahara Takamasa Horibe Makoto Kurihara + Mamoru Sobue + Takayuki Murooka + Apache License 2.0 Kyoichi Sugahara diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml index 78c6c585d90f7..bd75d8af12fc9 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml @@ -77,7 +77,7 @@ - + @@ -177,6 +177,7 @@ + @@ -237,6 +238,7 @@ + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/detector/radar_detector.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/detector/radar_detector.launch.xml index 0ef0201d973e1..184caf4e6224b 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/detector/radar_detector.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/detector/radar_detector.launch.xml @@ -4,7 +4,7 @@ - + @@ -22,7 +22,7 @@ - + diff --git a/launch/tier4_perception_launch/launch/perception.launch.xml b/launch/tier4_perception_launch/launch/perception.launch.xml index e8c19382864ba..59475afbafd92 100644 --- a/launch/tier4_perception_launch/launch/perception.launch.xml +++ b/launch/tier4_perception_launch/launch/perception.launch.xml @@ -16,6 +16,8 @@ + + diff --git a/mkdocs_macros.py b/mkdocs_macros.py index 56292a815d80d..97f76442be491 100644 --- a/mkdocs_macros.py +++ b/mkdocs_macros.py @@ -42,6 +42,8 @@ def format_param_range(param): def extract_parameter_info(parameters, namespace=""): params = [] for k, v in parameters.items(): + if "$ref" in v.keys(): + continue if v["type"] != "object": param = {} param["Name"] = namespace + k 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); diff --git a/perception/object_velocity_splitter/CMakeLists.txt b/perception/object_velocity_splitter/CMakeLists.txt index bcc56f522f0a8..0bf6de39f8f3c 100644 --- a/perception/object_velocity_splitter/CMakeLists.txt +++ b/perception/object_velocity_splitter/CMakeLists.txt @@ -37,4 +37,5 @@ endif() ament_auto_package( INSTALL_TO_SHARE launch + config ) diff --git a/perception/object_velocity_splitter/config/object_velocity_splitter.param.yaml b/perception/object_velocity_splitter/config/object_velocity_splitter.param.yaml new file mode 100644 index 0000000000000..adac831aa402c --- /dev/null +++ b/perception/object_velocity_splitter/config/object_velocity_splitter.param.yaml @@ -0,0 +1,3 @@ +/**: + ros__parameters: + velocity_threshold: 3.0 diff --git a/perception/object_velocity_splitter/launch/object_velocity_splitter.launch.xml b/perception/object_velocity_splitter/launch/object_velocity_splitter.launch.xml index 42d7f3d61f386..8ab654d4907a4 100644 --- a/perception/object_velocity_splitter/launch/object_velocity_splitter.launch.xml +++ b/perception/object_velocity_splitter/launch/object_velocity_splitter.launch.xml @@ -5,13 +5,13 @@ - + - + diff --git a/planning/behavior_path_avoidance_module/src/scene.cpp b/planning/behavior_path_avoidance_module/src/scene.cpp index 7cea678fb8b03..5172bcbeb626c 100644 --- a/planning/behavior_path_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_avoidance_module/src/scene.cpp @@ -303,8 +303,7 @@ void AvoidanceModule::fillAvoidanceTargetObjects( data, parameters_, forward_detection_range + MARGIN, debug); for (const auto & object : object_outside_target_lane.objects) { - ObjectData other_object; - other_object.object = object; + ObjectData other_object = createObjectData(data, object); other_object.reason = "OutOfTargetArea"; data.other_objects.push_back(other_object); } diff --git a/planning/behavior_path_avoidance_module/src/utils.cpp b/planning/behavior_path_avoidance_module/src/utils.cpp index b5d351cbc48a8..7309882a94e16 100644 --- a/planning/behavior_path_avoidance_module/src/utils.cpp +++ b/planning/behavior_path_avoidance_module/src/utils.cpp @@ -389,6 +389,14 @@ bool isVehicleTypeObject(const ObjectData & object) return true; } +bool isMovingObject( + const ObjectData & object, const std::shared_ptr & parameters) +{ + const auto object_type = utils::getHighestProbLabel(object.object.classification); + const auto object_parameter = parameters->object_parameters.at(object_type); + return object.move_time > object_parameter.moving_time_threshold; +} + bool isWithinCrosswalk( const ObjectData & object, const std::shared_ptr & overall_graphs) @@ -665,9 +673,7 @@ bool isSatisfiedWithCommonCondition( } // Step2. filtered stopped objects. - const auto object_type = utils::getHighestProbLabel(object.object.classification); - const auto object_parameter = parameters->object_parameters.at(object_type); - if (object.move_time > object_parameter.moving_time_threshold) { + if (filtering_utils::isMovingObject(object, parameters)) { object.reason = AvoidanceDebugFactor::MOVING_OBJECT; return false; } diff --git a/planning/behavior_path_goal_planner_module/package.xml b/planning/behavior_path_goal_planner_module/package.xml index 213c0093b08d9..a3023389cdd32 100644 --- a/planning/behavior_path_goal_planner_module/package.xml +++ b/planning/behavior_path_goal_planner_module/package.xml @@ -11,6 +11,8 @@ Tomoya Kimura Shumpei Wakabayashi Tomohito Ando + Mamoru Sobue + Daniel Sanchez Apache License 2.0 diff --git a/planning/behavior_path_lane_change_module/src/interface.cpp b/planning/behavior_path_lane_change_module/src/interface.cpp index 9c04ff4c67c58..c8d53d8756cf3 100644 --- a/planning/behavior_path_lane_change_module/src/interface.cpp +++ b/planning/behavior_path_lane_change_module/src/interface.cpp @@ -70,7 +70,7 @@ bool LaneChangeInterface::isExecutionRequested() const bool LaneChangeInterface::isExecutionReady() const { - return module_type_->isSafe(); + return module_type_->isSafe() && !module_type_->isAbortState(); } void LaneChangeInterface::updateData() @@ -116,7 +116,16 @@ BehaviorModuleOutput LaneChangeInterface::plan() } updateSteeringFactorPtr(output); - clearWaitingApproval(); + if (module_type_->isAbortState()) { + waitApproval(); + removeRTCStatus(); + const auto candidate = planCandidate(); + path_candidate_ = std::make_shared(candidate.path_candidate); + updateRTCStatus( + candidate.start_distance_to_path_change, candidate.finish_distance_to_path_change); + } else { + clearWaitingApproval(); + } return output; } diff --git a/planning/behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_lane_change_module/src/scene.cpp index c5a0f21d0f909..a5c49c32e0eb6 100644 --- a/planning/behavior_path_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_lane_change_module/src/scene.cpp @@ -144,6 +144,19 @@ BehaviorModuleOutput NormalLaneChange::getTerminalLaneChangePath() const { BehaviorModuleOutput output; + if (isAbortState() && abort_path_) { + output.path = abort_path_->path; + output.reference_path = prev_module_reference_path_; + output.turn_signal_info = prev_turn_signal_info_; + extendOutputDrivableArea(output); + const auto current_seg_idx = planner_data_->findEgoSegmentIndex(output.path.points); + output.turn_signal_info = planner_data_->turn_signal_decider.use_prior_turn_signal( + output.path, getEgoPose(), current_seg_idx, prev_turn_signal_info_, output.turn_signal_info, + planner_data_->parameters.ego_nearest_dist_threshold, + planner_data_->parameters.ego_nearest_yaw_threshold); + return output; + } + const auto current_lanes = getCurrentLanes(); if (current_lanes.empty()) { RCLCPP_WARN(logger_, "Current lanes not found!!! Something wrong."); @@ -157,7 +170,7 @@ BehaviorModuleOutput NormalLaneChange::getTerminalLaneChangePath() const const auto terminal_path = calcTerminalLaneChangePath(current_lanes, getLaneChangeLanes(current_lanes, direction_)); if (!terminal_path) { - RCLCPP_WARN(logger_, "Terminal path not found!!!"); + RCLCPP_DEBUG(logger_, "Terminal path not found!!!"); output.path = prev_module_path_; output.reference_path = prev_module_reference_path_; output.drivable_area_info = prev_drivable_area_info_; @@ -1525,7 +1538,7 @@ std::optional NormalLaneChange::calcTerminalLaneChangePath( -(lane_change_buffer + next_lane_change_buffer + distance_to_terminal_from_goal)); if (!lane_changing_start_pose) { - RCLCPP_WARN(logger_, "Reject: lane changing start pose not found!!!"); + RCLCPP_DEBUG(logger_, "Reject: lane changing start pose not found!!!"); return {}; } @@ -1534,7 +1547,7 @@ std::optional NormalLaneChange::calcTerminalLaneChangePath( // Check if the lane changing start point is not on the lanes next to target lanes, if (target_length_from_lane_change_start_pose > 0.0) { - RCLCPP_WARN(logger_, "lane change start getEgoPose() is behind target lanelet!"); + RCLCPP_DEBUG(logger_, "lane change start getEgoPose() is behind target lanelet!"); return {}; } @@ -1552,7 +1565,7 @@ std::optional NormalLaneChange::calcTerminalLaneChangePath( minimum_lane_changing_velocity, next_lane_change_buffer); if (target_segment.points.empty()) { - RCLCPP_WARN(logger_, "Reject: target segment is empty!! something wrong..."); + RCLCPP_DEBUG(logger_, "Reject: target segment is empty!! something wrong..."); return {}; } @@ -1581,7 +1594,7 @@ std::optional NormalLaneChange::calcTerminalLaneChangePath( lane_change_info.terminal_lane_changing_velocity = minimum_lane_changing_velocity; if (!is_valid_start_point) { - RCLCPP_WARN( + RCLCPP_DEBUG( logger_, "Reject: lane changing points are not inside of the target preferred lanes or its " "neighbors"); @@ -1596,7 +1609,7 @@ std::optional NormalLaneChange::calcTerminalLaneChangePath( next_lane_change_buffer); if (target_lane_reference_path.points.empty()) { - RCLCPP_WARN(logger_, "Reject: target_lane_reference_path is empty!!"); + RCLCPP_DEBUG(logger_, "Reject: target_lane_reference_path is empty!!"); return {}; } @@ -1855,31 +1868,25 @@ bool NormalLaneChange::calcAbortPath() ShiftedPath shifted_path; // offset front side - // bool offset_back = false; if (!path_shifter.generate(&shifted_path)) { RCLCPP_ERROR(logger_, "failed to generate abort shifted path."); } - const auto arc_position = lanelet::utils::getArcCoordinates( - reference_lanelets, shifted_path.path.points.at(abort_return_idx).point.pose); - const PathWithLaneId reference_lane_segment = std::invoke([&]() { - const double s_start = arc_position.length; - double s_end = std::max(lanelet::utils::getLaneletLength2d(reference_lanelets), s_start); - - if ( - !reference_lanelets.empty() && - route_handler->isInGoalRouteSection(reference_lanelets.back())) { - const auto goal_arc_coordinates = - lanelet::utils::getArcCoordinates(reference_lanelets, route_handler->getGoalPose()); - const double forward_length = std::max(goal_arc_coordinates.length, s_start); - s_end = std::min(s_end, forward_length); + PathWithLaneId reference_lane_segment = prev_module_path_; + { + const auto terminal_path = + calcTerminalLaneChangePath(reference_lanelets, selected_path.info.target_lanes); + if (terminal_path) { + reference_lane_segment = terminal_path->path; } - PathWithLaneId ref = route_handler->getCenterLinePath(reference_lanelets, s_start, s_end, true); - ref.points.back().point.longitudinal_velocity_mps = std::min( - ref.points.back().point.longitudinal_velocity_mps, - static_cast(lane_change_parameters_->minimum_lane_changing_velocity)); - return ref; - }); + const auto return_pose = shifted_path.path.points.at(abort_return_idx).point.pose; + const auto seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + reference_lane_segment.points, return_pose, common_param.ego_nearest_dist_threshold, + common_param.ego_nearest_yaw_threshold); + reference_lane_segment.points = motion_utils::cropPoints( + reference_lane_segment.points, return_pose.position, seg_idx, + common_param.forward_path_length, 0.0); + } auto abort_path = selected_path; abort_path.shifted_path = shifted_path; diff --git a/planning/behavior_path_start_planner_module/config/start_planner.param.yaml b/planning/behavior_path_start_planner_module/config/start_planner.param.yaml index d174b54b9ccbe..9e5a05a2d60cd 100644 --- a/planning/behavior_path_start_planner_module/config/start_planner.param.yaml +++ b/planning/behavior_path_start_planner_module/config/start_planner.param.yaml @@ -5,15 +5,14 @@ th_arrived_distance: 1.0 th_stopped_velocity: 0.01 th_stopped_time: 1.0 - collision_check_margins: [2.0, 1.5, 1.0] - collision_check_distance_from_end: 1.0 + collision_check_margins: [2.0, 1.0, 0.5, 0.1] collision_check_margin_from_front_object: 5.0 th_moving_object_velocity: 1.0 th_distance_to_middle_of_the_road: 0.5 center_line_path_interval: 1.0 # shift pull out enable_shift_pull_out: true - check_shift_path_lane_departure: false + shift_collision_check_distance_from_end: -10.0 minimum_shift_pull_out_distance: 0.0 deceleration_interval: 15.0 lateral_jerk: 0.5 @@ -23,6 +22,7 @@ maximum_curvature: 0.07 # geometric pull out enable_geometric_pull_out: true + geometric_collision_check_distance_from_end: 0.0 divide_pull_out_path: true geometric_pull_out_velocity: 1.0 arc_path_interval: 1.0 @@ -32,7 +32,7 @@ # search start pose backward enable_back: true search_priority: "efficient_path" # "efficient_path" or "short_back_distance" - max_back_distance: 30.0 + max_back_distance: 20.0 backward_search_resolution: 2.0 backward_path_update_duration: 3.0 ignore_distance_from_lane_end: 15.0 diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/data_structs.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/data_structs.hpp index 8f8d2baf9c85e..f42aef4075777 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/data_structs.hpp +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/data_structs.hpp @@ -63,7 +63,6 @@ struct StartPlannerParameters double intersection_search_length{0.0}; double length_ratio_for_turn_signal_deactivation_near_intersection{0.0}; std::vector collision_check_margins{}; - double collision_check_distance_from_end{0.0}; double collision_check_margin_from_front_object{0.0}; double th_moving_object_velocity{0.0}; double center_line_path_interval{0.0}; @@ -71,6 +70,7 @@ struct StartPlannerParameters // shift pull out bool enable_shift_pull_out{false}; bool check_shift_path_lane_departure{false}; + double shift_collision_check_distance_from_end{0.0}; double minimum_shift_pull_out_distance{0.0}; int lateral_acceleration_sampling_num{0}; double lateral_jerk{0.0}; @@ -80,6 +80,7 @@ struct StartPlannerParameters double deceleration_interval{0.0}; // geometric pull out bool enable_geometric_pull_out{false}; + double geometric_collision_check_distance_from_end; bool divide_pull_out_path{false}; ParallelParkingParameters parallel_parking_parameters{}; // search start pose backward diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp index cffce8218c554..0dccceb1e3919 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp @@ -174,7 +174,8 @@ class StartPlannerModule : public SceneModuleInterface const Pose & start_pose_candidate, const std::shared_ptr & planner, const Pose & refined_start_pose, const Pose & goal_pose, const double collision_check_margin); - PathWithLaneId extractCollisionCheckSection(const PullOutPath & path); + PathWithLaneId extractCollisionCheckSection( + const PullOutPath & path, const behavior_path_planner::PlannerType & planner_type); void updateStatusWithCurrentPath( const behavior_path_planner::PullOutPath & path, const Pose & start_pose, const behavior_path_planner::PlannerType & planner_type); diff --git a/planning/behavior_path_start_planner_module/package.xml b/planning/behavior_path_start_planner_module/package.xml index cbfdba0d07a57..f897188d60444 100644 --- a/planning/behavior_path_start_planner_module/package.xml +++ b/planning/behavior_path_start_planner_module/package.xml @@ -11,6 +11,8 @@ Tomoya Kimura Shumpei Wakabayashi Tomohito Ando + Mamoru Sobue + Daniel Sanchez Apache License 2.0 diff --git a/planning/behavior_path_start_planner_module/src/manager.cpp b/planning/behavior_path_start_planner_module/src/manager.cpp index 8cc0b34082e44..7fb52d59c418b 100644 --- a/planning/behavior_path_start_planner_module/src/manager.cpp +++ b/planning/behavior_path_start_planner_module/src/manager.cpp @@ -45,8 +45,6 @@ void StartPlannerModuleManager::init(rclcpp::Node * node) ns + "length_ratio_for_turn_signal_deactivation_near_intersection"); p.collision_check_margins = node->declare_parameter>(ns + "collision_check_margins"); - p.collision_check_distance_from_end = - node->declare_parameter(ns + "collision_check_distance_from_end"); p.collision_check_margin_from_front_object = node->declare_parameter(ns + "collision_check_margin_from_front_object"); p.th_moving_object_velocity = node->declare_parameter(ns + "th_moving_object_velocity"); @@ -55,6 +53,8 @@ void StartPlannerModuleManager::init(rclcpp::Node * node) p.enable_shift_pull_out = node->declare_parameter(ns + "enable_shift_pull_out"); p.check_shift_path_lane_departure = node->declare_parameter(ns + "check_shift_path_lane_departure"); + p.shift_collision_check_distance_from_end = + node->declare_parameter(ns + "shift_collision_check_distance_from_end"); p.minimum_shift_pull_out_distance = node->declare_parameter(ns + "minimum_shift_pull_out_distance"); p.lateral_acceleration_sampling_num = @@ -66,6 +66,8 @@ void StartPlannerModuleManager::init(rclcpp::Node * node) p.deceleration_interval = node->declare_parameter(ns + "deceleration_interval"); // geometric pull out p.enable_geometric_pull_out = node->declare_parameter(ns + "enable_geometric_pull_out"); + p.geometric_collision_check_distance_from_end = + node->declare_parameter(ns + "geometric_collision_check_distance_from_end"); p.divide_pull_out_path = node->declare_parameter(ns + "divide_pull_out_path"); p.parallel_parking_parameters.pull_out_velocity = node->declare_parameter(ns + "geometric_pull_out_velocity"); diff --git a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp index 8937c8a837694..135a0b7fbf86c 100644 --- a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp +++ b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp @@ -657,8 +657,8 @@ bool StartPlannerModule::findPullOutPath( // check collision if (utils::checkCollisionBetweenPathFootprintsAndObjects( - vehicle_footprint, extractCollisionCheckSection(*pull_out_path), pull_out_lane_stop_objects, - collision_check_margin)) { + vehicle_footprint, extractCollisionCheckSection(*pull_out_path, planner->getPlannerType()), + pull_out_lane_stop_objects, collision_check_margin)) { return false; } @@ -672,8 +672,17 @@ bool StartPlannerModule::findPullOutPath( return true; } -PathWithLaneId StartPlannerModule::extractCollisionCheckSection(const PullOutPath & path) +PathWithLaneId StartPlannerModule::extractCollisionCheckSection( + const PullOutPath & path, const behavior_path_planner::PlannerType & planner_type) { + const std::map collision_check_distances = { + {behavior_path_planner::PlannerType::SHIFT, + parameters_->shift_collision_check_distance_from_end}, + {behavior_path_planner::PlannerType::GEOMETRIC, + parameters_->geometric_collision_check_distance_from_end}}; + + const double collision_check_distance_from_end = collision_check_distances.at(planner_type); + PathWithLaneId combined_path; for (const auto & partial_path : path.partial_paths) { combined_path.points.insert( @@ -682,7 +691,7 @@ PathWithLaneId StartPlannerModule::extractCollisionCheckSection(const PullOutPat // calculate collision check end idx const size_t collision_check_end_idx = std::invoke([&]() { const auto collision_check_end_pose = motion_utils::calcLongitudinalOffsetPose( - combined_path.points, path.end_pose.position, parameters_->collision_check_distance_from_end); + combined_path.points, path.end_pose.position, collision_check_distance_from_end); if (collision_check_end_pose) { return motion_utils::findNearestIndex( @@ -1403,9 +1412,14 @@ void StartPlannerModule::setDebugData() // visualize collision_check_end_pose and footprint { const auto local_footprint = vehicle_info_.createFootprint(); + std::map collision_check_distances = { + {PlannerType::SHIFT, parameters_->shift_collision_check_distance_from_end}, + {PlannerType::GEOMETRIC, parameters_->geometric_collision_check_distance_from_end}}; + + double collision_check_distance_from_end = collision_check_distances[status_.planner_type]; const auto collision_check_end_pose = motion_utils::calcLongitudinalOffsetPose( getFullPath().points, status_.pull_out_path.end_pose.position, - parameters_->collision_check_distance_from_end); + collision_check_distance_from_end); if (collision_check_end_pose) { add(createPoseMarkerArray( *collision_check_end_pose, "static_collision_check_end_pose", 0, 1.0, 0.0, 0.0)); diff --git a/sensing/radar_scan_to_pointcloud2/package.xml b/sensing/radar_scan_to_pointcloud2/package.xml index 2360e8b33901d..a9d4fe7ebda8f 100644 --- a/sensing/radar_scan_to_pointcloud2/package.xml +++ b/sensing/radar_scan_to_pointcloud2/package.xml @@ -5,6 +5,9 @@ radar_scan_to_pointcloud2 Satoshi Tanaka Shunsuke Miura + Yoshi Ri + Taekjin Lee + Satoshi Tanaka Apache License 2.0 diff --git a/sensing/radar_static_pointcloud_filter/package.xml b/sensing/radar_static_pointcloud_filter/package.xml index a15aa43d71cf6..05453ee9cb9e0 100644 --- a/sensing/radar_static_pointcloud_filter/package.xml +++ b/sensing/radar_static_pointcloud_filter/package.xml @@ -5,6 +5,9 @@ radar_static_pointcloud_filter Satoshi Tanaka Shunsuke Miura + Yoshi Ri + Taekjin Lee + Satoshi Tanaka Apache License 2.0 diff --git a/sensing/radar_threshold_filter/package.xml b/sensing/radar_threshold_filter/package.xml index 00bb530567dc4..6b81f225db971 100644 --- a/sensing/radar_threshold_filter/package.xml +++ b/sensing/radar_threshold_filter/package.xml @@ -5,6 +5,9 @@ radar_threshold_filter Satoshi Tanaka Shunsuke Miura + Yoshi Ri + Taekjin Lee + Satoshi Tanaka Apache License 2.0 diff --git a/sensing/radar_tracks_noise_filter/package.xml b/sensing/radar_tracks_noise_filter/package.xml index 3af9658535687..8420b7a174edf 100644 --- a/sensing/radar_tracks_noise_filter/package.xml +++ b/sensing/radar_tracks_noise_filter/package.xml @@ -6,6 +6,9 @@ radar_tracks_noise_filter Sathshi Tanaka Shunsuke Miura + Yoshi Ri + Taekjin Lee + Apache License 2.0 Sathshi Tanaka