From 4d8d4f868fd5fdb087f8de49268afbbb8663c522 Mon Sep 17 00:00:00 2001 From: Kyoichi Sugahara Date: Thu, 11 Jan 2024 14:53:37 +0900 Subject: [PATCH] feat(start_planner): define collision check margin as list (#5994) * define collision check margins list in start planner module Signed-off-by: kyoichi-sugahara --------- Signed-off-by: kyoichi-sugahara --- .../parking_departure/common_module_data.hpp | 2 + .../README.md | 26 +++++------ .../config/start_planner.param.yaml | 2 +- .../start_planner_module.hpp | 2 +- .../start_planner_parameters.hpp | 2 +- .../src/manager.cpp | 3 +- .../src/start_planner_module.cpp | 45 ++++++++++--------- 7 files changed, 45 insertions(+), 37 deletions(-) diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/parking_departure/common_module_data.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/parking_departure/common_module_data.hpp index 91683f4547659..3dc230f0e92ed 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/parking_departure/common_module_data.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/parking_departure/common_module_data.hpp @@ -41,6 +41,8 @@ struct StartGoalPlannerData Pose refined_start_pose; std::vector start_pose_candidates; + size_t selected_start_pose_candidate_index; + double margin_for_start_pose_candidate; }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_start_planner_module/README.md b/planning/behavior_path_start_planner_module/README.md index 80ad8d6675e9a..aeefd18357a5c 100644 --- a/planning/behavior_path_start_planner_module/README.md +++ b/planning/behavior_path_start_planner_module/README.md @@ -65,19 +65,19 @@ PullOutPath --o PullOutPlannerBase ## General parameters for start_planner -| Name | Unit | Type | Description | Default value | -| :---------------------------------------------------------- | :---- | :----- | :-------------------------------------------------------------------------- | :------------ | -| th_arrived_distance_m | [m] | double | distance threshold for arrival of path termination | 1.0 | -| th_distance_to_middle_of_the_road | [m] | double | distance threshold to determine if the vehicle is on the middle of the road | 0.1 | -| th_stopped_velocity_mps | [m/s] | double | velocity threshold for arrival of path termination | 0.01 | -| th_stopped_time_sec | [s] | double | time threshold for arrival of path termination | 1.0 | -| th_turn_signal_on_lateral_offset | [m] | double | lateral distance threshold for turning on blinker | 1.0 | -| intersection_search_length | [m] | double | check if intersections exist within this length | 30.0 | -| length_ratio_for_turn_signal_deactivation_near_intersection | [m] | double | deactivate turn signal of this module near intersection | 0.5 | -| collision_check_margin | [m] | double | Obstacle collision check margin | 1.0 | -| collision_check_distance_from_end | [m] | double | collision check distance from end shift end pose | 1.0 | -| collision_check_margin_from_front_object | [m] | double | collision check margin from front object | 5.0 | -| center_line_path_interval | [m] | double | reference center line path point interval | 1.0 | +| Name | Unit | Type | Description | Default value | +| :---------------------------------------------------------- | :---- | :------- | :-------------------------------------------------------------------------- | :-------------- | +| th_arrived_distance_m | [m] | double | distance threshold for arrival of path termination | 1.0 | +| th_distance_to_middle_of_the_road | [m] | double | distance threshold to determine if the vehicle is on the middle of the road | 0.1 | +| th_stopped_velocity_mps | [m/s] | double | velocity threshold for arrival of path termination | 0.01 | +| th_stopped_time_sec | [s] | double | time threshold for arrival of path termination | 1.0 | +| th_turn_signal_on_lateral_offset | [m] | double | lateral distance threshold for turning on blinker | 1.0 | +| intersection_search_length | [m] | double | check if intersections exist within this length | 30.0 | +| length_ratio_for_turn_signal_deactivation_near_intersection | [m] | double | deactivate turn signal of this module near intersection | 0.5 | +| collision_check_margins | [m] | [double] | Obstacle collision check margins list | [2.0, 1.5, 1.0] | +| collision_check_distance_from_end | [m] | double | collision check distance from end shift end pose | 1.0 | +| collision_check_margin_from_front_object | [m] | double | collision check margin from front object | 5.0 | +| center_line_path_interval | [m] | double | reference center line path point interval | 1.0 | ## Safety check with static obstacles 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 d04728861a52e..d174b54b9ccbe 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,7 +5,7 @@ th_arrived_distance: 1.0 th_stopped_velocity: 0.01 th_stopped_time: 1.0 - collision_check_margin: 1.0 + collision_check_margins: [2.0, 1.5, 1.0] collision_check_distance_from_end: 1.0 collision_check_margin_from_front_object: 5.0 th_moving_object_velocity: 1.0 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 eb1827ec0045d..41a80e59d56bf 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 @@ -172,7 +172,7 @@ class StartPlannerModule : public SceneModuleInterface const std::string & search_priority, const size_t start_pose_candidates_num); bool findPullOutPath( const Pose & start_pose_candidate, const std::shared_ptr & planner, - const Pose & refined_start_pose, const Pose & goal_pose); + const Pose & refined_start_pose, const Pose & goal_pose, const double collision_check_margin); PathWithLaneId extractCollisionCheckSection(const PullOutPath & path); void updateStatusWithCurrentPath( diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_parameters.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_parameters.hpp index 2df75107d872c..66dbddebf99bb 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_parameters.hpp +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_parameters.hpp @@ -42,7 +42,7 @@ struct StartPlannerParameters double th_distance_to_middle_of_the_road{0.0}; double intersection_search_length{0.0}; double length_ratio_for_turn_signal_deactivation_near_intersection{0.0}; - double collision_check_margin{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}; diff --git a/planning/behavior_path_start_planner_module/src/manager.cpp b/planning/behavior_path_start_planner_module/src/manager.cpp index f719718a3389e..8cc0b34082e44 100644 --- a/planning/behavior_path_start_planner_module/src/manager.cpp +++ b/planning/behavior_path_start_planner_module/src/manager.cpp @@ -43,7 +43,8 @@ void StartPlannerModuleManager::init(rclcpp::Node * node) p.intersection_search_length = node->declare_parameter(ns + "intersection_search_length"); p.length_ratio_for_turn_signal_deactivation_near_intersection = node->declare_parameter( ns + "length_ratio_for_turn_signal_deactivation_near_intersection"); - p.collision_check_margin = node->declare_parameter(ns + "collision_check_margin"); + 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 = 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 6702e37f19077..d523f57125679 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 @@ -584,9 +584,16 @@ void StartPlannerModule::planWithPriority( const PriorityOrder order_priority = determinePriorityOrder(search_priority, start_pose_candidates.size()); - for (const auto & [index, planner] : order_priority) { - if (findPullOutPath(start_pose_candidates[index], planner, refined_start_pose, goal_pose)) - return; + for (const auto & collision_check_margin : parameters_->collision_check_margins) { + for (const auto & [index, planner] : order_priority) { + if (findPullOutPath( + start_pose_candidates[index], planner, refined_start_pose, goal_pose, + collision_check_margin)) { + start_planner_data_.selected_start_pose_candidate_index = index; + start_planner_data_.margin_for_start_pose_candidate = collision_check_margin; + return; + } + } } updateStatusIfNoSafePathFound(); @@ -617,7 +624,7 @@ PriorityOrder StartPlannerModule::determinePriorityOrder( bool StartPlannerModule::findPullOutPath( const Pose & start_pose_candidate, const std::shared_ptr & planner, - const Pose & refined_start_pose, const Pose & goal_pose) + const Pose & refined_start_pose, const Pose & goal_pose, const double collision_check_margin) { const auto & dynamic_objects = planner_data_->dynamic_object; const auto pull_out_lanes = start_planner_utils::getPullOutLanes( @@ -645,7 +652,7 @@ bool StartPlannerModule::findPullOutPath( // check collision if (utils::checkCollisionBetweenPathFootprintsAndObjects( vehicle_footprint, extractCollisionCheckSection(*pull_out_path), pull_out_lane_stop_objects, - parameters_->collision_check_margin)) { + collision_check_margin)) { return false; } @@ -666,23 +673,21 @@ PathWithLaneId StartPlannerModule::extractCollisionCheckSection(const PullOutPat combined_path.points.insert( combined_path.points.end(), partial_path.points.begin(), partial_path.points.end()); } - // calculate collision check end idx - size_t collision_check_end_idx = 0; - const auto collision_check_end_pose = motion_utils::calcLongitudinalOffsetPose( - combined_path.points, path.end_pose.position, parameters_->collision_check_distance_from_end); - - if (collision_check_end_pose) { - collision_check_end_idx = - motion_utils::findNearestIndex(combined_path.points, collision_check_end_pose->position); - } + 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); + if (collision_check_end_pose) { + return motion_utils::findNearestIndex( + combined_path.points, collision_check_end_pose->position); + } else { + return combined_path.points.size() - 1; + } + }); // remove the point behind of collision check end pose - if (collision_check_end_idx + 1 < combined_path.points.size()) { - combined_path.points.erase( - combined_path.points.begin() + collision_check_end_idx + 1, combined_path.points.end()); - } - + combined_path.points.erase( + combined_path.points.begin() + collision_check_end_idx + 1, combined_path.points.end()); return combined_path; } @@ -949,7 +954,7 @@ std::vector StartPlannerModule::searchPullOutStartPoseCandidates( if (utils::checkCollisionBetweenFootprintAndObjects( local_vehicle_footprint, *backed_pose, stop_objects_in_pull_out_lanes, - parameters_->collision_check_margin)) { + parameters_->collision_check_margins.back())) { break; // poses behind this has a collision, so break. }