From ce2329152e66164f862d4c580dad17e671104511 Mon Sep 17 00:00:00 2001 From: scepter914 Date: Thu, 14 Sep 2023 17:10:58 +0900 Subject: [PATCH 01/13] fix(tier4_perception_launch): Add object_merger of far_objects to fusion for Camera-LiDAR-Radar fusion Signed-off-by: scepter914 --- ...ar_radar_fusion_based_detection.launch.xml | 20 +++++++-- .../detection/detection.launch.xml | 25 ++++++----- .../lidar_based_detection.launch.xml | 4 +- .../lidar_radar_based_detection.launch.xml | 15 ++++--- .../radar_based_detection.launch.xml | 12 ++--- .../launch/perception.launch.xml | 44 ++++++++----------- 6 files changed, 65 insertions(+), 55 deletions(-) diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml index 463340efdecfe..87e5e3fc94f8e 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml @@ -9,7 +9,7 @@ - + @@ -48,6 +48,7 @@ + + + + + + + + + + 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 ca5d1d0f7e8bb..863e23971e00a 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 @@ -1,8 +1,9 @@ + - + @@ -10,7 +11,7 @@ - + @@ -29,14 +30,15 @@ - + + + + + - - - - + @@ -65,10 +67,11 @@ + - + @@ -97,7 +100,7 @@ - + @@ -107,6 +110,7 @@ + @@ -122,11 +126,12 @@ - + + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml index 5b6134dc45584..01943a49d3d35 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml @@ -10,7 +10,7 @@ - + @@ -74,7 +74,7 @@ - + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_radar_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_radar_based_detection.launch.xml index a47e6a86f1c7b..c3295be830308 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_radar_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_radar_based_detection.launch.xml @@ -1,18 +1,19 @@ - + - + - + + - + @@ -22,11 +23,11 @@ - + - + @@ -42,7 +43,7 @@ - + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/radar_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/radar_based_detection.launch.xml index 8e2e23b510a07..2d160d442b198 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/radar_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/radar_based_detection.launch.xml @@ -2,13 +2,13 @@ - - - - - + + + + + - + diff --git a/launch/tier4_perception_launch/launch/perception.launch.xml b/launch/tier4_perception_launch/launch/perception.launch.xml index c41177e51116b..ae287e4446d82 100644 --- a/launch/tier4_perception_launch/launch/perception.launch.xml +++ b/launch/tier4_perception_launch/launch/perception.launch.xml @@ -13,6 +13,7 @@ + @@ -24,11 +25,11 @@ - + - + @@ -61,42 +62,31 @@ - + + - + - + - + - + - + @@ -109,7 +99,7 @@ - + @@ -127,10 +117,10 @@ - + - + @@ -161,6 +151,7 @@ + @@ -171,14 +162,15 @@ - + - + + @@ -194,7 +186,7 @@ - + From 243823e1b756da8f6179c127a6876944db23aff9 Mon Sep 17 00:00:00 2001 From: scepter914 Date: Thu, 14 Sep 2023 17:12:24 +0900 Subject: [PATCH 02/13] apply pre-commit Signed-off-by: scepter914 --- ...dar_radar_fusion_based_detection.launch.xml | 1 - .../launch/perception.launch.xml | 18 +++++++++++++++--- 2 files changed, 15 insertions(+), 4 deletions(-) diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml index 87e5e3fc94f8e..1c5f0112f58be 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml @@ -406,5 +406,4 @@ - diff --git a/launch/tier4_perception_launch/launch/perception.launch.xml b/launch/tier4_perception_launch/launch/perception.launch.xml index ae287e4446d82..6c7b7afe61234 100644 --- a/launch/tier4_perception_launch/launch/perception.launch.xml +++ b/launch/tier4_perception_launch/launch/perception.launch.xml @@ -62,7 +62,11 @@ - + @@ -73,10 +77,18 @@ - + - + From e1405edad9f7a55f83d9927370644d517d4a5e8b Mon Sep 17 00:00:00 2001 From: scepter914 Date: Thu, 14 Sep 2023 18:29:42 +0900 Subject: [PATCH 03/13] fix comments Signed-off-by: scepter914 --- ...ra_lidar_fusion_based_detection.launch.xml | 6 ++--- ...ar_radar_fusion_based_detection.launch.xml | 14 +++++------ .../detection/detection.launch.xml | 20 ++++++++-------- .../lidar_radar_based_detection.launch.xml | 4 ++-- .../launch/perception.launch.xml | 24 +++++-------------- 5 files changed, 28 insertions(+), 40 deletions(-) diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml index eab2a56607dee..04fd5664a8304 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml @@ -1,9 +1,9 @@ - + - + @@ -19,7 +19,7 @@ - + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml index 1c5f0112f58be..a69479c1bedd4 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml @@ -1,9 +1,9 @@ - + - + @@ -19,7 +19,7 @@ - + @@ -46,7 +46,7 @@ - + @@ -63,7 +63,7 @@ --> - + @@ -293,7 +293,7 @@ - + @@ -302,7 +302,7 @@ - + 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 863e23971e00a..3ee93e6e49d26 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 @@ -1,9 +1,9 @@ - + - + @@ -11,7 +11,7 @@ - + @@ -30,15 +30,15 @@ - + - + - + @@ -71,7 +71,7 @@ - + @@ -100,7 +100,7 @@ - + @@ -114,7 +114,7 @@ - + @@ -126,7 +126,7 @@ - + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_radar_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_radar_based_detection.launch.xml index c3295be830308..9f577ad200668 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_radar_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_radar_based_detection.launch.xml @@ -1,6 +1,6 @@ - + @@ -9,7 +9,7 @@ - + diff --git a/launch/tier4_perception_launch/launch/perception.launch.xml b/launch/tier4_perception_launch/launch/perception.launch.xml index 6c7b7afe61234..39fbddd8e0237 100644 --- a/launch/tier4_perception_launch/launch/perception.launch.xml +++ b/launch/tier4_perception_launch/launch/perception.launch.xml @@ -25,7 +25,7 @@ - + @@ -62,35 +62,23 @@ - + - + - + - + - + From 2cbbf696e4291e1bcd3d3b5f15b0c6139e70cf95 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Thu, 14 Sep 2023 09:31:56 +0000 Subject: [PATCH 04/13] style(pre-commit): autofix --- .../launch/perception.launch.xml | 18 +++++++++++++++--- 1 file changed, 15 insertions(+), 3 deletions(-) diff --git a/launch/tier4_perception_launch/launch/perception.launch.xml b/launch/tier4_perception_launch/launch/perception.launch.xml index 39fbddd8e0237..ec3fce594540a 100644 --- a/launch/tier4_perception_launch/launch/perception.launch.xml +++ b/launch/tier4_perception_launch/launch/perception.launch.xml @@ -62,7 +62,11 @@ - + @@ -73,10 +77,18 @@ - + - + From 23cb2f1119f70e5ac0dc172749e9b1ee7907944e Mon Sep 17 00:00:00 2001 From: scepter914 Date: Thu, 14 Sep 2023 18:39:57 +0900 Subject: [PATCH 05/13] fix typo Signed-off-by: scepter914 --- .../launch/object_recognition/detection/detection.launch.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 3ee93e6e49d26..28c80c33e22a8 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 @@ -34,7 +34,7 @@ - + From f745ba9c03b78ffa1236ef8df360a08e7c388510 Mon Sep 17 00:00:00 2001 From: scepter914 Date: Tue, 19 Sep 2023 03:08:02 +0900 Subject: [PATCH 06/13] fix typo Signed-off-by: scepter914 --- .../object_recognition/detection/detection.launch.xml | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) 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 28c80c33e22a8..11c64c669ee37 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 @@ -3,7 +3,7 @@ - + @@ -11,7 +11,7 @@ - + @@ -30,7 +30,7 @@ - + From b6f47df00d544af6c4d168363eb3a6f293bba8f4 Mon Sep 17 00:00:00 2001 From: Satoshi Tanaka <16330533+scepter914@users.noreply.github.com> Date: Tue, 19 Sep 2023 11:54:44 +0900 Subject: [PATCH 07/13] Update launch/tier4_perception_launch/launch/object_recognition/detection/lidar_radar_based_detection.launch.xml Co-authored-by: Shunsuke Miura <37187849+miursh@users.noreply.github.com> Signed-off-by: scepter914 --- .../detection/lidar_radar_based_detection.launch.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_radar_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_radar_based_detection.launch.xml index 9f577ad200668..4eee611c33992 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_radar_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_radar_based_detection.launch.xml @@ -11,7 +11,7 @@ - + From d88deb15d47cc7ebda7c9d720a42b3bb698626b4 Mon Sep 17 00:00:00 2001 From: scepter914 Date: Tue, 19 Sep 2023 11:56:35 +0900 Subject: [PATCH 08/13] change default parameters Signed-off-by: scepter914 --- .../camera_lidar_radar_fusion_based_detection.launch.xml | 2 +- .../launch/object_recognition/detection/detection.launch.xml | 2 +- .../detection/radar_based_detection.launch.xml | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml index a69479c1bedd4..4308bab8294fe 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml @@ -48,7 +48,7 @@ - + - + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/radar_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/radar_based_detection.launch.xml index 2d160d442b198..52c8aedff8ef9 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/radar_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/radar_based_detection.launch.xml @@ -9,7 +9,7 @@ - + From c5a6d81e78f72603e7acb6050c2cdb3c99ee860d Mon Sep 17 00:00:00 2001 From: Kyoichi Sugahara Date: Tue, 19 Sep 2023 08:18:21 +0900 Subject: [PATCH 09/13] feat(behavior_path_planner): use common function of createPredictedPath from avoidance module (#5015) * update function of create predicted path Signed-off-by: kyoichi-sugahara * util common function of create predicted path from avoidance module Signed-off-by: kyoichi-sugahara --------- Signed-off-by: kyoichi-sugahara --- .../utils/avoidance/utils.hpp | 3 +- .../avoidance/avoidance_module.cpp | 9 ++-- .../src/utils/avoidance/utils.cpp | 42 +++++++++---------- 3 files changed, 26 insertions(+), 28 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp index 0dc07f0950716..1009540062a99 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp @@ -89,7 +89,8 @@ std::vector generateObstaclePolygonsForDrivableArea( std::vector convertToPredictedPath( const PathWithLaneId & path, const std::shared_ptr & planner_data, - const bool is_object_front, const std::shared_ptr & parameters); + const bool is_object_front, const bool limit_to_max_velocity, + const std::shared_ptr & parameters); double getLongitudinalVelocity(const Pose & p_ref, const Pose & p_target, const double v); diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp index 0f2605735eda1..ce5d13d17aedf 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp @@ -1842,10 +1842,11 @@ bool AvoidanceModule::isSafePath( return true; // if safety check is disabled, it always return safe. } - const auto ego_predicted_path_for_front_object = - utils::avoidance::convertToPredictedPath(shifted_path.path, planner_data_, true, parameters_); - const auto ego_predicted_path_for_rear_object = - utils::avoidance::convertToPredictedPath(shifted_path.path, planner_data_, false, parameters_); + const bool limit_to_max_velocity = false; + const auto ego_predicted_path_for_front_object = utils::avoidance::convertToPredictedPath( + shifted_path.path, planner_data_, true, limit_to_max_velocity, parameters_); + const auto ego_predicted_path_for_rear_object = utils::avoidance::convertToPredictedPath( + shifted_path.path, planner_data_, false, limit_to_max_velocity, parameters_); const auto ego_idx = planner_data_->findEgoIndex(shifted_path.path.points); const auto is_right_shift = [&]() -> std::optional { diff --git a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp index 058059037c6d6..1c1c155a7b661 100644 --- a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp +++ b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp @@ -16,6 +16,7 @@ #include "behavior_path_planner/utils/avoidance/avoidance_module_data.hpp" #include "behavior_path_planner/utils/avoidance/utils.hpp" +#include "behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp" #include "behavior_path_planner/utils/path_utils.hpp" #include "motion_utils/trajectory/path_with_lane_id.hpp" #include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" @@ -1445,34 +1446,29 @@ AvoidLineArray combineRawShiftLinesWithUniqueCheck( std::vector convertToPredictedPath( const PathWithLaneId & path, const std::shared_ptr & planner_data, - const bool is_object_front, const std::shared_ptr & parameters) + const bool is_object_front, const bool limit_to_max_velocity, + const std::shared_ptr & parameters) { - if (path.points.empty()) { - return {}; - } - - const auto & acceleration = parameters->max_acceleration; const auto & vehicle_pose = planner_data->self_odometry->pose.pose; const auto & initial_velocity = std::abs(planner_data->self_odometry->twist.twist.linear.x); - const auto & time_horizon = is_object_front ? parameters->time_horizon_for_front_object - : parameters->time_horizon_for_rear_object; - const auto & time_resolution = parameters->safety_check_time_resolution; - const size_t ego_seg_idx = planner_data->findEgoSegmentIndex(path.points); - std::vector predicted_path; - const auto vehicle_pose_frenet = - convertToFrenetPoint(path.points, vehicle_pose.position, ego_seg_idx); - - for (double t = 0.0; t < time_horizon + 1e-3; t += time_resolution) { - const double velocity = - std::max(initial_velocity + acceleration * t, parameters->min_slow_down_speed); - const double length = initial_velocity * t + 0.5 * acceleration * t * t; - const auto pose = - motion_utils::calcInterpolatedPose(path.points, vehicle_pose_frenet.length + length); - predicted_path.emplace_back(t, pose, velocity); - } - return predicted_path; + auto ego_predicted_path_params = + std::make_shared(); + + ego_predicted_path_params->min_velocity = parameters->min_slow_down_speed; + ego_predicted_path_params->acceleration = parameters->max_acceleration; + ego_predicted_path_params->max_velocity = std::numeric_limits::infinity(); + ego_predicted_path_params->time_horizon_for_front_object = + parameters->time_horizon_for_front_object; + ego_predicted_path_params->time_horizon_for_rear_object = + parameters->time_horizon_for_rear_object; + ego_predicted_path_params->time_resolution = parameters->safety_check_time_resolution; + ego_predicted_path_params->delay_until_departure = 0.0; + + return behavior_path_planner::utils::path_safety_checker::createPredictedPath( + ego_predicted_path_params, path.points, vehicle_pose, initial_velocity, ego_seg_idx, + is_object_front, limit_to_max_velocity); } ExtendedPredictedObject transform( From 189f48d01448dc7f47f8a4592743e8e92f0ade68 Mon Sep 17 00:00:00 2001 From: scepter914 Date: Tue, 19 Sep 2023 12:10:43 +0900 Subject: [PATCH 10/13] fix conflict Signed-off-by: scepter914 --- ...ra_lidar_fusion_based_detection.launch.xml | 23 +- ...ar_radar_fusion_based_detection.launch.xml | 22 +- .../detection/detection.launch.xml | 10 +- .../lidar_based_detection.launch.xml | 14 +- .../lidar_radar_based_detection.launch.xml | 2 +- .../detection/pointcloud_map_filter.launch.py | 39 ++- .../launch/perception.launch.xml | 10 +- .../config/compare_map.param.yaml | 20 -- .../config/outlier.param.yaml | 8 - .../config/voxel_grid.param.yaml | 7 - ...el_grid_based_euclidean_cluster.param.yaml | 14 +- .../launch/euclidean_cluster.launch.py | 73 ++---- .../launch/euclidean_cluster.launch.xml | 6 +- ...xel_grid_based_euclidean_cluster.launch.py | 239 ++---------------- ...el_grid_based_euclidean_cluster.launch.xml | 12 +- 15 files changed, 106 insertions(+), 393 deletions(-) delete mode 100644 perception/euclidean_cluster/config/compare_map.param.yaml delete mode 100644 perception/euclidean_cluster/config/outlier.param.yaml delete mode 100644 perception/euclidean_cluster/config/voxel_grid.param.yaml diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml index 04fd5664a8304..178e193dc2d99 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml @@ -11,13 +11,12 @@ - - + @@ -59,15 +58,16 @@ --> - + - + + @@ -75,19 +75,18 @@ - - + + - - - - + + + - + @@ -120,7 +119,7 @@ - + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml index 4308bab8294fe..df833296e3f66 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml @@ -11,8 +11,6 @@ - - @@ -79,15 +77,16 @@ - + - + + @@ -95,13 +94,12 @@ - - + + - - - - + + + @@ -152,7 +150,7 @@ - + @@ -185,7 +183,7 @@ - + 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 89370f8007a16..0c037763431a7 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 @@ -6,7 +6,6 @@ - @@ -66,6 +65,8 @@ + + @@ -97,6 +98,8 @@ + + @@ -110,7 +113,11 @@ +<<<<<<< HEAD +======= + +>>>>>>> 8eeb11dff (refactor(perception): rearrange clustering pipeline (#4999)) @@ -123,6 +130,7 @@ + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml index 01943a49d3d35..23e0297dc5e44 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml @@ -4,8 +4,6 @@ - - @@ -19,29 +17,27 @@ - + - + + - - - + - - + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_radar_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_radar_based_detection.launch.xml index 4eee611c33992..cee6829ea3b4a 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_radar_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_radar_based_detection.launch.xml @@ -3,7 +3,6 @@ - @@ -20,6 +19,7 @@ + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/pointcloud_map_filter.launch.py b/launch/tier4_perception_launch/launch/object_recognition/detection/pointcloud_map_filter.launch.py index a5d8d2113d2db..93d395ca3e466 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/pointcloud_map_filter.launch.py +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/pointcloud_map_filter.launch.py @@ -35,7 +35,6 @@ def __init__(self, context): ) with open(pointcloud_map_filter_param_path, "r") as f: self.pointcloud_map_filter_param = yaml.safe_load(f)["/**"]["ros__parameters"] - self.use_down_sample_filter = self.pointcloud_map_filter_param["use_down_sample_filter"] self.voxel_size = self.pointcloud_map_filter_param["down_sample_voxel_size"] self.distance_threshold = self.pointcloud_map_filter_param["distance_threshold"] self.downsize_ratio_z_axis = self.pointcloud_map_filter_param["downsize_ratio_z_axis"] @@ -46,47 +45,40 @@ def __init__(self, context): ] self.map_loader_radius = self.pointcloud_map_filter_param["map_loader_radius"] self.publish_debug_pcd = self.pointcloud_map_filter_param["publish_debug_pcd"] + self.use_pointcloud_map = LaunchConfiguration("use_pointcloud_map").perform(context) def create_pipeline(self): - if self.use_down_sample_filter: - return self.create_down_sample_pipeline() + if self.use_pointcloud_map == "true": + return self.create_compare_map_pipeline() else: - return self.create_normal_pipeline() + return self.create_no_compare_map_pipeline() - def create_normal_pipeline(self): + def create_no_compare_map_pipeline(self): components = [] components.append( ComposableNode( - package="compare_map_segmentation", - plugin="compare_map_segmentation::VoxelBasedCompareMapFilterComponent", - name="voxel_based_compare_map_filter", + package="pointcloud_preprocessor", + plugin="pointcloud_preprocessor::ApproximateDownsampleFilterComponent", + name="voxel_grid_downsample_filter", remappings=[ ("input", LaunchConfiguration("input_topic")), - ("map", "/map/pointcloud_map"), ("output", LaunchConfiguration("output_topic")), - ("map_loader_service", "/map/get_differential_pointcloud_map"), - ("kinematic_state", "/localization/kinematic_state"), ], parameters=[ { - "distance_threshold": self.distance_threshold, - "downsize_ratio_z_axis": self.downsize_ratio_z_axis, - "timer_interval_ms": self.timer_interval_ms, - "use_dynamic_map_loading": self.use_dynamic_map_loading, - "map_update_distance_threshold": self.map_update_distance_threshold, - "map_loader_radius": self.map_loader_radius, - "publish_debug_pcd": self.publish_debug_pcd, - "input_frame": "map", + "voxel_size_x": self.voxel_size, + "voxel_size_y": self.voxel_size, + "voxel_size_z": self.voxel_size, } ], extra_arguments=[ - {"use_intra_process_comms": False}, + {"use_intra_process_comms": LaunchConfiguration("use_intra_process")} ], - ) + ), ) return components - def create_down_sample_pipeline(self): + def create_compare_map_pipeline(self): components = [] down_sample_topic = ( "/perception/obstacle_segmentation/pointcloud_map_filtered/downsampled/pointcloud" @@ -94,7 +86,7 @@ def create_down_sample_pipeline(self): components.append( ComposableNode( package="pointcloud_preprocessor", - plugin="pointcloud_preprocessor::VoxelGridDownsampleFilterComponent", + plugin="pointcloud_preprocessor::ApproximateDownsampleFilterComponent", name="voxel_grid_downsample_filter", remappings=[ ("input", LaunchConfiguration("input_topic")), @@ -177,6 +169,7 @@ def add_launch_arg(name: str, default_value=None): add_launch_arg("use_intra_process", "True") add_launch_arg("use_pointcloud_container", "False") add_launch_arg("container_name", "pointcloud_map_filter_pipeline_container") + add_launch_arg("use_pointcloud_map", "true") set_container_executable = SetLaunchConfiguration( "container_executable", "component_container", diff --git a/launch/tier4_perception_launch/launch/perception.launch.xml b/launch/tier4_perception_launch/launch/perception.launch.xml index ec3fce594540a..36d43bab74894 100644 --- a/launch/tier4_perception_launch/launch/perception.launch.xml +++ b/launch/tier4_perception_launch/launch/perception.launch.xml @@ -1,10 +1,8 @@ - - @@ -60,8 +58,9 @@ - + + - - - + + diff --git a/perception/euclidean_cluster/config/compare_map.param.yaml b/perception/euclidean_cluster/config/compare_map.param.yaml deleted file mode 100644 index 3dd303464a2c1..0000000000000 --- a/perception/euclidean_cluster/config/compare_map.param.yaml +++ /dev/null @@ -1,20 +0,0 @@ -/**: - ros__parameters: - - # distance threshold for compare compare - distance_threshold: 0.5 - - # publish voxelized map pointcloud for debug - publish_debug_pcd: False - - # use dynamic map loading - use_dynamic_map_loading: True - - # time interval to check dynamic map loading - timer_interval_ms: 100 - - # distance threshold for dynamic map update - map_update_distance_threshold: 10.0 - - # radius map for dynamic map loading - map_loader_radius: 150.0 diff --git a/perception/euclidean_cluster/config/outlier.param.yaml b/perception/euclidean_cluster/config/outlier.param.yaml deleted file mode 100644 index 1962fba1f332a..0000000000000 --- a/perception/euclidean_cluster/config/outlier.param.yaml +++ /dev/null @@ -1,8 +0,0 @@ -/**: - ros__parameters: - input_frame: base_link - output_frame: base_link - voxel_size_x: 0.3 - voxel_size_y: 0.3 - voxel_size_z: 100.0 - voxel_points_threshold: 3 diff --git a/perception/euclidean_cluster/config/voxel_grid.param.yaml b/perception/euclidean_cluster/config/voxel_grid.param.yaml deleted file mode 100644 index 3ff32bfbb7146..0000000000000 --- a/perception/euclidean_cluster/config/voxel_grid.param.yaml +++ /dev/null @@ -1,7 +0,0 @@ -/**: - ros__parameters: - input_frame: base_link - output_frame: base_link - voxel_size_x: 0.15 - voxel_size_y: 0.15 - voxel_size_z: 0.15 diff --git a/perception/euclidean_cluster/config/voxel_grid_based_euclidean_cluster.param.yaml b/perception/euclidean_cluster/config/voxel_grid_based_euclidean_cluster.param.yaml index 2f3de2b789eb3..26b027f0077aa 100644 --- a/perception/euclidean_cluster/config/voxel_grid_based_euclidean_cluster.param.yaml +++ b/perception/euclidean_cluster/config/voxel_grid_based_euclidean_cluster.param.yaml @@ -7,9 +7,11 @@ max_cluster_size: 3000 use_height: false input_frame: "base_link" - max_x: 70.0 - min_x: -70.0 - max_y: 70.0 - min_y: -70.0 - max_z: 4.5 - min_z: -4.5 + + # low height crop box filter param + max_x: 200.0 + min_x: -200.0 + max_y: 200.0 + min_y: -200.0 + max_z: 2.0 + min_z: -10.0 diff --git a/perception/euclidean_cluster/launch/euclidean_cluster.launch.py b/perception/euclidean_cluster/launch/euclidean_cluster.launch.py index db86bbf80d250..66c25396a656e 100644 --- a/perception/euclidean_cluster/launch/euclidean_cluster.launch.py +++ b/perception/euclidean_cluster/launch/euclidean_cluster.launch.py @@ -35,61 +35,35 @@ def load_composable_node_param(param_path): ns = "" pkg = "euclidean_cluster" - # set voxel grid filter as a component - voxel_grid_filter_component = ComposableNode( + low_height_cropbox_filter_component = ComposableNode( package="pointcloud_preprocessor", - plugin="pointcloud_preprocessor::VoxelGridDownsampleFilterComponent", - name=AnonName("voxel_grid_filter"), + namespace=ns, + plugin="pointcloud_preprocessor::CropBoxFilterComponent", + name="low_height_crop_box_filter", remappings=[ ("input", LaunchConfiguration("input_pointcloud")), - ("output", "voxel_grid_filtered/pointcloud"), - ], - parameters=[load_composable_node_param("voxel_grid_param_path")], - ) - - # set compare map filter as a component - compare_map_filter_component = ComposableNode( - package="compare_map_segmentation", - plugin="compare_map_segmentation::VoxelBasedCompareMapFilterComponent", - name=AnonName("compare_map_filter"), - remappings=[ - ("input", "voxel_grid_filtered/pointcloud"), - ("map", LaunchConfiguration("input_map")), - ("output", "compare_map_filtered/pointcloud"), - ("map_loader_service", "/map/get_differential_pointcloud_map"), - ("kinematic_state", "/localization/kinematic_state"), - ], - parameters=[ - { - "distance_threshold": 0.5, - "timer_interval_ms": 100, - "use_dynamic_map_loading": True, - "downsize_ratio_z_axis": 0.5, - "map_update_distance_threshold": 10.0, - "map_loader_radius": 150.0, - "publish_debug_pcd": True, - "input_frame": "map", - } + ("output", "low_height/pointcloud"), ], + parameters=[load_composable_node_param("voxel_grid_based_euclidean_param_path")], ) - use_map_euclidean_cluster_component = ComposableNode( + use_low_height_euclidean_component = ComposableNode( package=pkg, plugin="euclidean_cluster::EuclideanClusterNode", name=AnonName("euclidean_cluster"), remappings=[ - ("input", "compare_map_filtered/pointcloud"), + ("input", "low_height/pointcloud"), ("output", LaunchConfiguration("output_clusters")), ], parameters=[load_composable_node_param("euclidean_param_path")], ) - disuse_map_euclidean_cluster_component = ComposableNode( + disuse_low_height_euclidean_component = ComposableNode( package=pkg, plugin="euclidean_cluster::EuclideanClusterNode", name=AnonName("euclidean_cluster"), remappings=[ - ("input", "voxel_grid_filtered/pointcloud"), + ("input", LaunchConfiguration("input_pointcloud")), ("output", LaunchConfiguration("output_clusters")), ], parameters=[load_composable_node_param("euclidean_param_path")], @@ -100,26 +74,26 @@ def load_composable_node_param(param_path): namespace=ns, package="rclcpp_components", executable="component_container", - composable_node_descriptions=[voxel_grid_filter_component], + composable_node_descriptions=[], output="screen", ) - use_map_loader = LoadComposableNodes( + use_low_height_pointcloud_loader = LoadComposableNodes( composable_node_descriptions=[ - compare_map_filter_component, - use_map_euclidean_cluster_component, + low_height_cropbox_filter_component, + use_low_height_euclidean_component, ], target_container=container, - condition=IfCondition(LaunchConfiguration("use_pointcloud_map")), + condition=IfCondition(LaunchConfiguration("use_low_height_cropbox")), ) - disuse_map_loader = LoadComposableNodes( - composable_node_descriptions=[disuse_map_euclidean_cluster_component], + disuse_low_height_pointcloud_loader = LoadComposableNodes( + composable_node_descriptions=[disuse_low_height_euclidean_component], target_container=container, - condition=UnlessCondition(LaunchConfiguration("use_pointcloud_map")), + condition=UnlessCondition(LaunchConfiguration("use_low_height_cropbox")), ) - return [container, use_map_loader, disuse_map_loader] + return [container, use_low_height_pointcloud_loader, disuse_low_height_pointcloud_loader] def generate_launch_description(): @@ -131,14 +105,7 @@ def add_launch_arg(name: str, default_value=None): add_launch_arg("input_pointcloud", "/perception/obstacle_segmentation/pointcloud"), add_launch_arg("input_map", "/map/pointcloud_map"), add_launch_arg("output_clusters", "clusters"), - add_launch_arg("use_pointcloud_map", "false"), - add_launch_arg( - "voxel_grid_param_path", - [ - FindPackageShare("autoware_launch"), - "/config/perception/object_recognition/detection/clustering/voxel_grid.param.yaml", - ], - ), + add_launch_arg("use_low_height_cropbox", "false"), add_launch_arg( "euclidean_param_path", [ diff --git a/perception/euclidean_cluster/launch/euclidean_cluster.launch.xml b/perception/euclidean_cluster/launch/euclidean_cluster.launch.xml index 7159fb4c42793..fd1ea82befae0 100644 --- a/perception/euclidean_cluster/launch/euclidean_cluster.launch.xml +++ b/perception/euclidean_cluster/launch/euclidean_cluster.launch.xml @@ -3,16 +3,14 @@ - - + - - + diff --git a/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.py b/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.py index 82ce5605b67cd..00bcd4422bd3c 100644 --- a/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.py +++ b/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.py @@ -16,10 +16,8 @@ from launch.actions import DeclareLaunchArgument from launch.actions import OpaqueFunction from launch.conditions import IfCondition -from launch.substitutions import AndSubstitution -from launch.substitutions import AnonName +from launch.conditions import UnlessCondition from launch.substitutions import LaunchConfiguration -from launch.substitutions import NotSubstitution from launch_ros.actions import ComposableNodeContainer from launch_ros.actions import LoadComposableNodes from launch_ros.descriptions import ComposableNode @@ -36,159 +34,31 @@ def load_composable_node_param(param_path): ns = "" pkg = "euclidean_cluster" - # set compare map filter as a component - compare_map_filter_component = ComposableNode( - package="compare_map_segmentation", - namespace=ns, - plugin="compare_map_segmentation::VoxelBasedCompareMapFilterComponent", - name=AnonName("compare_map_filter"), - remappings=[ - ("input", LaunchConfiguration("input_pointcloud")), - ("map", LaunchConfiguration("input_map")), - ("output", "map_filter/pointcloud"), - ("map_loader_service", "/map/get_differential_pointcloud_map"), - ("kinematic_state", "/localization/kinematic_state"), - ], - parameters=[load_composable_node_param("compare_map_param_path")], - ) - - # separate range of pointcloud when map_filter used - use_map_short_range_crop_box_filter_component = ComposableNode( - package="pointcloud_preprocessor", - namespace=ns, - plugin="pointcloud_preprocessor::CropBoxFilterComponent", - name="short_distance_crop_box_range", - remappings=[ - ("input", "map_filter/pointcloud"), - ("output", "short_range/pointcloud"), - ], - parameters=[ - load_composable_node_param("voxel_grid_based_euclidean_param_path"), - { - "negative": False, - }, - ], - ) - - use_map_long_range_crop_box_filter_component = ComposableNode( - package="pointcloud_preprocessor", - namespace=ns, - plugin="pointcloud_preprocessor::CropBoxFilterComponent", - name="long_distance_crop_box_range", - remappings=[ - ("input", LaunchConfiguration("input_pointcloud")), - ("output", "long_range/pointcloud"), - ], - parameters=[ - load_composable_node_param("voxel_grid_based_euclidean_param_path"), - { - "negative": True, - }, - ], - ) - - # disuse_map_voxel_grid_filter - disuse_map_short_range_crop_box_filter_component = ComposableNode( + low_height_cropbox_filter_component = ComposableNode( package="pointcloud_preprocessor", namespace=ns, plugin="pointcloud_preprocessor::CropBoxFilterComponent", - name="short_distance_crop_box_range", + name="low_height_crop_box_filter", remappings=[ ("input", LaunchConfiguration("input_pointcloud")), - ("output", "short_range/pointcloud"), - ], - parameters=[ - load_composable_node_param("voxel_grid_based_euclidean_param_path"), - { - "negative": False, - }, - ], - ) - - disuse_map_long_range_crop_box_filter_component = ComposableNode( - package="pointcloud_preprocessor", - namespace=ns, - plugin="pointcloud_preprocessor::CropBoxFilterComponent", - name="long_distance_crop_box_range", - remappings=[ - ("input", LaunchConfiguration("input_pointcloud")), - ("output", "long_range/pointcloud"), - ], - parameters=[ - load_composable_node_param("voxel_grid_based_euclidean_param_path"), - { - "negative": True, - }, - ], - ) - - # set voxel grid filter as a component - voxel_grid_filter_component = ComposableNode( - package="pointcloud_preprocessor", - namespace=ns, - plugin="pointcloud_preprocessor::ApproximateDownsampleFilterComponent", - name=AnonName("voxel_grid_filter"), - remappings=[ - ("input", "short_range/pointcloud"), - ("output", "downsampled/short_range/pointcloud"), - ], - parameters=[load_composable_node_param("voxel_grid_param_path")], - ) - - # set outlier filter as a component - outlier_filter_component = ComposableNode( - package="pointcloud_preprocessor", - namespace=ns, - plugin="pointcloud_preprocessor::VoxelGridOutlierFilterComponent", - name="outlier_filter", - remappings=[ - ("input", "downsampled/short_range/pointcloud"), - ("output", "outlier_filter/pointcloud"), - ], - parameters=[load_composable_node_param("outlier_param_path")], - ) - - # concat with-outlier pointcloud and without-outlier pcl - downsample_concat_component = ComposableNode( - package="pointcloud_preprocessor", - namespace=ns, - plugin="pointcloud_preprocessor::PointCloudConcatenateDataSynchronizerComponent", - name="concat_downsampled_pcl", - remappings=[("output", "downsampled/concatenated/pointcloud")], - parameters=[ - { - "input_topics": ["long_range/pointcloud", "outlier_filter/pointcloud"], - "output_frame": "base_link", - } - ], - extra_arguments=[{"use_intra_process_comms": True}], - ) - - # set euclidean cluster as a component - use_downsample_euclidean_cluster_component = ComposableNode( - package=pkg, - namespace=ns, - plugin="euclidean_cluster::VoxelGridBasedEuclideanClusterNode", - name="euclidean_cluster", - remappings=[ - ("input", "downsampled/concatenated/pointcloud"), - ("output", LaunchConfiguration("output_clusters")), + ("output", "low_height/pointcloud"), ], parameters=[load_composable_node_param("voxel_grid_based_euclidean_param_path")], ) - use_map_disuse_downsample_euclidean_component = ComposableNode( + use_low_height_euclidean_component = ComposableNode( package=pkg, namespace=ns, plugin="euclidean_cluster::VoxelGridBasedEuclideanClusterNode", name="euclidean_cluster", remappings=[ - ("input", "map_filter/pointcloud"), + ("input", "low_height/pointcloud"), ("output", LaunchConfiguration("output_clusters")), ], parameters=[load_composable_node_param("voxel_grid_based_euclidean_param_path")], ) - disuse_map_disuse_downsample_euclidean_component = ComposableNode( + + disuse_low_height_euclidean_component = ComposableNode( package=pkg, namespace=ns, plugin="euclidean_cluster::VoxelGridBasedEuclideanClusterNode", @@ -209,75 +79,24 @@ def load_composable_node_param(param_path): output="screen", ) - use_map_use_downsample_loader = LoadComposableNodes( + use_low_height_pointcloud_loader = LoadComposableNodes( composable_node_descriptions=[ - compare_map_filter_component, - use_map_short_range_crop_box_filter_component, - use_map_long_range_crop_box_filter_component, - voxel_grid_filter_component, - outlier_filter_component, - downsample_concat_component, - use_downsample_euclidean_cluster_component, + low_height_cropbox_filter_component, + use_low_height_euclidean_component, ], target_container=container, - condition=IfCondition( - AndSubstitution( - LaunchConfiguration("use_pointcloud_map"), - LaunchConfiguration("use_downsample_pointcloud"), - ) - ), + condition=IfCondition(LaunchConfiguration("use_low_height_cropbox")), ) - disuse_map_use_downsample_loader = LoadComposableNodes( - composable_node_descriptions=[ - disuse_map_short_range_crop_box_filter_component, - disuse_map_long_range_crop_box_filter_component, - voxel_grid_filter_component, - outlier_filter_component, - downsample_concat_component, - use_downsample_euclidean_cluster_component, - ], + disuse_low_height_pointcloud_loader = LoadComposableNodes( + composable_node_descriptions=[disuse_low_height_euclidean_component], target_container=container, - condition=IfCondition( - AndSubstitution( - NotSubstitution(LaunchConfiguration("use_pointcloud_map")), - LaunchConfiguration("use_downsample_pointcloud"), - ) - ), - ) - - use_map_disuse_downsample_loader = LoadComposableNodes( - composable_node_descriptions=[ - compare_map_filter_component, - use_map_disuse_downsample_euclidean_component, - ], - target_container=container, - condition=IfCondition( - AndSubstitution( - (LaunchConfiguration("use_pointcloud_map")), - NotSubstitution(LaunchConfiguration("use_downsample_pointcloud")), - ) - ), - ) - - disuse_map_disuse_downsample_loader = LoadComposableNodes( - composable_node_descriptions=[ - disuse_map_disuse_downsample_euclidean_component, - ], - target_container=container, - condition=IfCondition( - AndSubstitution( - NotSubstitution(LaunchConfiguration("use_pointcloud_map")), - NotSubstitution(LaunchConfiguration("use_downsample_pointcloud")), - ) - ), + condition=UnlessCondition(LaunchConfiguration("use_low_height_cropbox")), ) return [ container, - use_map_use_downsample_loader, - disuse_map_use_downsample_loader, - use_map_disuse_downsample_loader, - disuse_map_disuse_downsample_loader, + use_low_height_pointcloud_loader, + disuse_low_height_pointcloud_loader, ] @@ -290,29 +109,7 @@ def add_launch_arg(name: str, default_value=None): add_launch_arg("input_pointcloud", "/perception/obstacle_segmentation/pointcloud"), add_launch_arg("input_map", "/map/pointcloud_map"), add_launch_arg("output_clusters", "clusters"), - add_launch_arg("use_pointcloud_map", "false"), - add_launch_arg("use_downsample_pointcloud", "false"), - add_launch_arg( - "voxel_grid_param_path", - [ - FindPackageShare("autoware_launch"), - "/config/perception/object_recognition/detection/clustering/voxel_grid.param.yaml", - ], - ), - add_launch_arg( - "outlier_param_path", - [ - FindPackageShare("autoware_launch"), - "/config/perception/object_recognition/detection/clustering/outlier.param.yaml", - ], - ), - add_launch_arg( - "compare_map_param_path", - [ - FindPackageShare("autoware_launch"), - "/config/perception/object_recognition/detection/clustering/compare_map.param.yaml", - ], - ), + add_launch_arg("use_low_height_cropbox", "false"), add_launch_arg( "voxel_grid_based_euclidean_param_path", [ diff --git a/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.xml b/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.xml index 2833fed81c28e..b6a426dabfd12 100644 --- a/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.xml +++ b/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.xml @@ -3,22 +3,14 @@ - - - - - + - - - - - + From e229305597a30a814d7232dafeec1ef3c7138ed9 Mon Sep 17 00:00:00 2001 From: Kyoichi Sugahara Date: Tue, 19 Sep 2023 09:38:46 +0900 Subject: [PATCH 11/13] refactor(behavior_path_planner): use util function convertToPredictedPath from lane_change (#5013) --- .../utils/lane_change/utils.hpp | 3 --- .../src/scene_module/lane_change/normal.cpp | 2 +- .../src/utils/lane_change/utils.cpp | 13 ------------- 3 files changed, 1 insertion(+), 17 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp index f180afc2d1ffd..0c1fff52c74e2 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp @@ -160,9 +160,6 @@ std::vector convertToPredictedPath( const LaneChangePath & lane_change_path, const Twist & vehicle_twist, const Pose & pose, const BehaviorPathPlannerParameters & common_parameters, const double resolution); -PredictedPath convertToPredictedPath( - const std::vector & path, const double time_resolution); - bool isParkedObject( const PathWithLaneId & path, const RouteHandler & route_handler, const ExtendedPredictedObject & object, const double object_check_min_road_shoulder_width, diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp index 2f030689a29f5..333231e0ed0d4 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp @@ -1346,7 +1346,7 @@ PathSafetyStatus NormalLaneChange::isLaneChangePathSafe( const auto ego_predicted_path = utils::lane_change::convertToPredictedPath( lane_change_path, current_twist, current_pose, common_parameters, time_resolution); const auto debug_predicted_path = - utils::lane_change::convertToPredictedPath(ego_predicted_path, time_resolution); + utils::path_safety_checker::convertToPredictedPath(ego_predicted_path, time_resolution); auto collision_check_objects = target_objects.target_lane; diff --git a/planning/behavior_path_planner/src/utils/lane_change/utils.cpp b/planning/behavior_path_planner/src/utils/lane_change/utils.cpp index 3e098c651feef..29c139ab5a654 100644 --- a/planning/behavior_path_planner/src/utils/lane_change/utils.cpp +++ b/planning/behavior_path_planner/src/utils/lane_change/utils.cpp @@ -829,19 +829,6 @@ std::vector convertToPredictedPath( return predicted_path; } -PredictedPath convertToPredictedPath( - const std::vector & path, const double time_resolution) -{ - PredictedPath predicted_path; - predicted_path.time_step = rclcpp::Duration::from_seconds(time_resolution); - predicted_path.path.resize(path.size()); - - for (size_t i = 0; i < path.size(); ++i) { - predicted_path.path.at(i) = path.at(i).pose; - } - return predicted_path; -} - bool isParkedObject( const PathWithLaneId & path, const RouteHandler & route_handler, const ExtendedPredictedObject & object, const double object_check_min_road_shoulder_width, From b606c6fecb4281a1762b5ac2f337b30ff414ec87 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Tue, 19 Sep 2023 10:19:32 +0900 Subject: [PATCH 12/13] fix(lanelet2_projector): fix werror return-type (#5019) Signed-off-by: kosuke55 Co-authored-by: kminoda <44218668+kminoda@users.noreply.github.com> --- common/geography_utils/src/lanelet2_projector.cpp | 10 ++++------ 1 file changed, 4 insertions(+), 6 deletions(-) diff --git a/common/geography_utils/src/lanelet2_projector.cpp b/common/geography_utils/src/lanelet2_projector.cpp index 4958d661b0ce3..3e982ac2ccf4d 100644 --- a/common/geography_utils/src/lanelet2_projector.cpp +++ b/common/geography_utils/src/lanelet2_projector.cpp @@ -43,13 +43,11 @@ std::unique_ptr get_lanelet2_projector(const MapProjectorInf lanelet::Origin origin{position}; lanelet::projection::TransverseMercatorProjector projector{origin}; return std::make_unique(projector); - - } else { - const std::string error_msg = - "Invalid map projector type: " + projector_info.projector_type + - ". Currently supported types: MGRS, LocalCartesianUTM, and TransverseMercator"; - throw std::invalid_argument(error_msg); } + const std::string error_msg = + "Invalid map projector type: " + projector_info.projector_type + + ". Currently supported types: MGRS, LocalCartesianUTM, and TransverseMercator"; + throw std::invalid_argument(error_msg); } } // namespace geography_utils From 74865f01aea24c661dc0c1729afdb7979fef8ff8 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Tue, 19 Sep 2023 11:41:33 +0900 Subject: [PATCH 13/13] refactor(avoidance): generate drivable lanes in utils (#5018) * refactor(avoidance): generate drivable lanes in utils Signed-off-by: satoshi-ota * refactor(avoidance): use std::any_of Signed-off-by: satoshi-ota --------- Signed-off-by: satoshi-ota --- .../avoidance/avoidance_module.hpp | 2 +- .../utils/avoidance/utils.hpp | 4 + .../avoidance/avoidance_module.cpp | 162 +----------------- .../src/utils/avoidance/utils.cpp | 155 +++++++++++++++++ 4 files changed, 165 insertions(+), 158 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp index a719ab341b88b..c12f310fda088 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp @@ -460,7 +460,7 @@ class AvoidanceModule : public SceneModuleInterface // TODO(murooka) freespace during turning in intersection where there is no neighbor lanes // NOTE: Assume that there is no situation where there is an object in the middle lane of more // than two lanes since which way to avoid is not obvious - void generateExtendedDrivableArea(BehaviorModuleOutput & output) const; + void generateExpandDrivableLanes(BehaviorModuleOutput & output) const; /** * @brief fill debug markers. diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp index 1009540062a99..10ce16b00e2ae 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp @@ -168,6 +168,10 @@ std::pair separateObjectsByPath( const PathWithLaneId & path, const std::shared_ptr & planner_data, const AvoidancePlanningData & data, const std::shared_ptr & parameters, DebugData & debug); + +DrivableLanes generateExpandDrivableLanes( + const lanelet::ConstLanelet & lanelet, const std::shared_ptr & planner_data, + const std::shared_ptr & parameters); } // namespace behavior_path_planner::utils::avoidance #endif // BEHAVIOR_PATH_PLANNER__UTILS__AVOIDANCE__UTILS_HPP_ diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp index ce5d13d17aedf..eb6a1e125f412 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp @@ -69,22 +69,6 @@ using tier4_planning_msgs::msg::AvoidanceDebugFactor; namespace { -bool isEndPointsConnected( - const lanelet::ConstLanelet & left_lane, const lanelet::ConstLanelet & right_lane) -{ - const auto & left_back_point_2d = right_lane.leftBound2d().back().basicPoint(); - const auto & right_back_point_2d = left_lane.rightBound2d().back().basicPoint(); - - constexpr double epsilon = 1e-5; - return (right_back_point_2d - left_back_point_2d).norm() < epsilon; -} - -template -void pushUniqueVector(T & base_vector, const T & additional_vector) -{ - base_vector.insert(base_vector.end(), additional_vector.begin(), additional_vector.end()); -} - bool isDrivingSameLane( const lanelet::ConstLanelets & previous_lanes, const lanelet::ConstLanelets & current_lanes) { @@ -1912,148 +1896,12 @@ bool AvoidanceModule::isSafePath( return safe_ || safe_count_ > parameters_->hysteresis_factor_safe_count; } -void AvoidanceModule::generateExtendedDrivableArea(BehaviorModuleOutput & output) const +void AvoidanceModule::generateExpandDrivableLanes(BehaviorModuleOutput & output) const { - const auto has_same_lane = - [](const lanelet::ConstLanelets lanes, const lanelet::ConstLanelet & lane) { - if (lanes.empty()) return false; - const auto has_same = [&](const auto & ll) { return ll.id() == lane.id(); }; - return std::find_if(lanes.begin(), lanes.end(), has_same) != lanes.end(); - }; - - const auto & route_handler = planner_data_->route_handler; - const auto & current_lanes = avoid_data_.current_lanelets; - const auto & enable_opposite = parameters_->use_opposite_lane; std::vector drivable_lanes; - - for (const auto & current_lane : current_lanes) { - DrivableLanes current_drivable_lanes; - current_drivable_lanes.left_lane = current_lane; - current_drivable_lanes.right_lane = current_lane; - - if (!parameters_->use_adjacent_lane) { - drivable_lanes.push_back(current_drivable_lanes); - continue; - } - - // 1. get left/right side lanes - const auto update_left_lanelets = [&](const lanelet::ConstLanelet & target_lane) { - const auto all_left_lanelets = - route_handler->getAllLeftSharedLinestringLanelets(target_lane, enable_opposite, true); - if (!all_left_lanelets.empty()) { - current_drivable_lanes.left_lane = all_left_lanelets.back(); // leftmost lanelet - pushUniqueVector( - current_drivable_lanes.middle_lanes, - lanelet::ConstLanelets(all_left_lanelets.begin(), all_left_lanelets.end() - 1)); - } - }; - const auto update_right_lanelets = [&](const lanelet::ConstLanelet & target_lane) { - const auto all_right_lanelets = - route_handler->getAllRightSharedLinestringLanelets(target_lane, enable_opposite, true); - if (!all_right_lanelets.empty()) { - current_drivable_lanes.right_lane = all_right_lanelets.back(); // rightmost lanelet - pushUniqueVector( - current_drivable_lanes.middle_lanes, - lanelet::ConstLanelets(all_right_lanelets.begin(), all_right_lanelets.end() - 1)); - } - }; - - update_left_lanelets(current_lane); - update_right_lanelets(current_lane); - - // 2.1 when there are multiple lanes whose previous lanelet is the same - const auto get_next_lanes_from_same_previous_lane = - [&route_handler](const lanelet::ConstLanelet & lane) { - // get previous lane, and return false if previous lane does not exist - lanelet::ConstLanelets prev_lanes; - if (!route_handler->getPreviousLaneletsWithinRoute(lane, &prev_lanes)) { - return lanelet::ConstLanelets{}; - } - - lanelet::ConstLanelets next_lanes; - for (const auto & prev_lane : prev_lanes) { - const auto next_lanes_from_prev = route_handler->getNextLanelets(prev_lane); - pushUniqueVector(next_lanes, next_lanes_from_prev); - } - return next_lanes; - }; - - const auto next_lanes_for_right = - get_next_lanes_from_same_previous_lane(current_drivable_lanes.right_lane); - const auto next_lanes_for_left = - get_next_lanes_from_same_previous_lane(current_drivable_lanes.left_lane); - - // 2.2 look for neighbor lane recursively, where end line of the lane is connected to end line - // of the original lane - const auto update_drivable_lanes = - [&](const lanelet::ConstLanelets & next_lanes, const bool is_left) { - for (const auto & next_lane : next_lanes) { - const auto & edge_lane = - is_left ? current_drivable_lanes.left_lane : current_drivable_lanes.right_lane; - if (next_lane.id() == edge_lane.id()) { - continue; - } - - const auto & left_lane = is_left ? next_lane : edge_lane; - const auto & right_lane = is_left ? edge_lane : next_lane; - if (!isEndPointsConnected(left_lane, right_lane)) { - continue; - } - - if (is_left) { - current_drivable_lanes.left_lane = next_lane; - } else { - current_drivable_lanes.right_lane = next_lane; - } - - if (!has_same_lane(current_drivable_lanes.middle_lanes, edge_lane)) { - if (is_left) { - if (current_drivable_lanes.right_lane.id() != edge_lane.id()) { - current_drivable_lanes.middle_lanes.push_back(edge_lane); - } - } else { - if (current_drivable_lanes.left_lane.id() != edge_lane.id()) { - current_drivable_lanes.middle_lanes.push_back(edge_lane); - } - } - } - - return true; - } - return false; - }; - - const auto expand_drivable_area_recursively = - [&](const lanelet::ConstLanelets & next_lanes, const bool is_left) { - // NOTE: set max search num to avoid infinity loop for drivable area expansion - constexpr size_t max_recursive_search_num = 3; - for (size_t i = 0; i < max_recursive_search_num; ++i) { - const bool is_update_kept = update_drivable_lanes(next_lanes, is_left); - if (!is_update_kept) { - break; - } - if (i == max_recursive_search_num - 1) { - RCLCPP_ERROR( - rclcpp::get_logger("behavior_path_planner").get_child("avoidance"), - "Drivable area expansion reaches max iteration."); - } - } - }; - expand_drivable_area_recursively(next_lanes_for_right, false); - expand_drivable_area_recursively(next_lanes_for_left, true); - - // 3. update again for new left/right lanes - update_left_lanelets(current_drivable_lanes.left_lane); - update_right_lanelets(current_drivable_lanes.right_lane); - - // 4. compensate that current_lane is in either of left_lane, right_lane or middle_lanes. - if ( - current_drivable_lanes.left_lane.id() != current_lane.id() && - current_drivable_lanes.right_lane.id() != current_lane.id()) { - current_drivable_lanes.middle_lanes.push_back(current_lane); - } - - drivable_lanes.push_back(current_drivable_lanes); + for (const auto & lanelet : avoid_data_.current_lanelets) { + drivable_lanes.push_back( + utils::avoidance::generateExpandDrivableLanes(lanelet, planner_data_, parameters_)); } { // for new architecture @@ -2206,7 +2054,7 @@ BehaviorModuleOutput AvoidanceModule::plan() utils::clipPathLength(*output.path, ego_idx, planner_data_->parameters); // Drivable area generation. - generateExtendedDrivableArea(output); + generateExpandDrivableLanes(output); setDrivableLanes(output.drivable_area_info.drivable_lanes); return output; diff --git a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp index 1c1c155a7b661..331c8912fddf6 100644 --- a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp +++ b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp @@ -202,6 +202,22 @@ Polygon2d createOneStepPolygon( return hull_polygon; } + +bool isEndPointsConnected( + const lanelet::ConstLanelet & left_lane, const lanelet::ConstLanelet & right_lane) +{ + const auto & left_back_point_2d = right_lane.leftBound2d().back().basicPoint(); + const auto & right_back_point_2d = left_lane.rightBound2d().back().basicPoint(); + + constexpr double epsilon = 1e-5; + return (right_back_point_2d - left_back_point_2d).norm() < epsilon; +} + +template +void pushUniqueVector(T & base_vector, const T & additional_vector) +{ + base_vector.insert(base_vector.end(), additional_vector.begin(), additional_vector.end()); +} } // namespace bool isOnRight(const ObjectData & obj) @@ -1673,4 +1689,143 @@ std::pair separateObjectsByPath( return std::make_pair(target_objects, other_objects); } + +DrivableLanes generateExpandDrivableLanes( + const lanelet::ConstLanelet & lanelet, const std::shared_ptr & planner_data, + const std::shared_ptr & parameters) +{ + const auto & route_handler = planner_data->route_handler; + + DrivableLanes current_drivable_lanes; + current_drivable_lanes.left_lane = lanelet; + current_drivable_lanes.right_lane = lanelet; + + if (!parameters->use_adjacent_lane) { + return current_drivable_lanes; + } + + // 1. get left/right side lanes + const auto update_left_lanelets = [&](const lanelet::ConstLanelet & target_lane) { + const auto all_left_lanelets = route_handler->getAllLeftSharedLinestringLanelets( + target_lane, parameters->use_opposite_lane, true); + if (!all_left_lanelets.empty()) { + current_drivable_lanes.left_lane = all_left_lanelets.back(); // leftmost lanelet + pushUniqueVector( + current_drivable_lanes.middle_lanes, + lanelet::ConstLanelets(all_left_lanelets.begin(), all_left_lanelets.end() - 1)); + } + }; + const auto update_right_lanelets = [&](const lanelet::ConstLanelet & target_lane) { + const auto all_right_lanelets = route_handler->getAllRightSharedLinestringLanelets( + target_lane, parameters->use_opposite_lane, true); + if (!all_right_lanelets.empty()) { + current_drivable_lanes.right_lane = all_right_lanelets.back(); // rightmost lanelet + pushUniqueVector( + current_drivable_lanes.middle_lanes, + lanelet::ConstLanelets(all_right_lanelets.begin(), all_right_lanelets.end() - 1)); + } + }; + + update_left_lanelets(lanelet); + update_right_lanelets(lanelet); + + // 2.1 when there are multiple lanes whose previous lanelet is the same + const auto get_next_lanes_from_same_previous_lane = + [&route_handler](const lanelet::ConstLanelet & lane) { + // get previous lane, and return false if previous lane does not exist + lanelet::ConstLanelets prev_lanes; + if (!route_handler->getPreviousLaneletsWithinRoute(lane, &prev_lanes)) { + return lanelet::ConstLanelets{}; + } + + lanelet::ConstLanelets next_lanes; + for (const auto & prev_lane : prev_lanes) { + const auto next_lanes_from_prev = route_handler->getNextLanelets(prev_lane); + pushUniqueVector(next_lanes, next_lanes_from_prev); + } + return next_lanes; + }; + + const auto next_lanes_for_right = + get_next_lanes_from_same_previous_lane(current_drivable_lanes.right_lane); + const auto next_lanes_for_left = + get_next_lanes_from_same_previous_lane(current_drivable_lanes.left_lane); + + // 2.2 look for neighbor lane recursively, where end line of the lane is connected to end line + // of the original lane + const auto update_drivable_lanes = + [&](const lanelet::ConstLanelets & next_lanes, const bool is_left) { + for (const auto & next_lane : next_lanes) { + const auto & edge_lane = + is_left ? current_drivable_lanes.left_lane : current_drivable_lanes.right_lane; + if (next_lane.id() == edge_lane.id()) { + continue; + } + + const auto & left_lane = is_left ? next_lane : edge_lane; + const auto & right_lane = is_left ? edge_lane : next_lane; + if (!isEndPointsConnected(left_lane, right_lane)) { + continue; + } + + if (is_left) { + current_drivable_lanes.left_lane = next_lane; + } else { + current_drivable_lanes.right_lane = next_lane; + } + + const auto & middle_lanes = current_drivable_lanes.middle_lanes; + const auto has_same_lane = std::any_of( + middle_lanes.begin(), middle_lanes.end(), + [&edge_lane](const auto & lane) { return lane.id() == edge_lane.id(); }); + + if (!has_same_lane) { + if (is_left) { + if (current_drivable_lanes.right_lane.id() != edge_lane.id()) { + current_drivable_lanes.middle_lanes.push_back(edge_lane); + } + } else { + if (current_drivable_lanes.left_lane.id() != edge_lane.id()) { + current_drivable_lanes.middle_lanes.push_back(edge_lane); + } + } + } + + return true; + } + return false; + }; + + const auto expand_drivable_area_recursively = + [&](const lanelet::ConstLanelets & next_lanes, const bool is_left) { + // NOTE: set max search num to avoid infinity loop for drivable area expansion + constexpr size_t max_recursive_search_num = 3; + for (size_t i = 0; i < max_recursive_search_num; ++i) { + const bool is_update_kept = update_drivable_lanes(next_lanes, is_left); + if (!is_update_kept) { + break; + } + if (i == max_recursive_search_num - 1) { + RCLCPP_ERROR( + rclcpp::get_logger("behavior_path_planner").get_child("avoidance"), + "Drivable area expansion reaches max iteration."); + } + } + }; + expand_drivable_area_recursively(next_lanes_for_right, false); + expand_drivable_area_recursively(next_lanes_for_left, true); + + // 3. update again for new left/right lanes + update_left_lanelets(current_drivable_lanes.left_lane); + update_right_lanelets(current_drivable_lanes.right_lane); + + // 4. compensate that current_lane is in either of left_lane, right_lane or middle_lanes. + if ( + current_drivable_lanes.left_lane.id() != lanelet.id() && + current_drivable_lanes.right_lane.id() != lanelet.id()) { + current_drivable_lanes.middle_lanes.push_back(lanelet); + } + + return current_drivable_lanes; +} } // namespace behavior_path_planner::utils::avoidance