From f9dcafba05b94aaca7675e379f34dbd50a52efd2 Mon Sep 17 00:00:00 2001 From: Kyoichi Sugahara Date: Thu, 11 Jan 2024 11:39:43 +0900 Subject: [PATCH] feat(start_planner): keep distance against front objects (#5983) * refactor extractCollisionCheckPath Signed-off-by: kyoichi-sugahara --------- Signed-off-by: kyoichi-sugahara Co-authored-by: Kosuke Takeuchi --- .../behavior_path_start_planner_module/README.md | 3 ++- .../config/start_planner.param.yaml | 1 + .../start_planner_module.hpp | 2 +- .../start_planner_parameters.hpp | 1 + .../src/manager.cpp | 2 ++ .../src/start_planner_module.cpp | 15 +++++++++++---- 6 files changed, 18 insertions(+), 6 deletions(-) diff --git a/planning/behavior_path_start_planner_module/README.md b/planning/behavior_path_start_planner_module/README.md index 2f90cc2ee3cf2..80ad8d6675e9a 100644 --- a/planning/behavior_path_start_planner_module/README.md +++ b/planning/behavior_path_start_planner_module/README.md @@ -75,7 +75,8 @@ PullOutPath --o PullOutPlannerBase | 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 point. currently only for pull out | 15.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 514d61e225ecd..d04728861a52e 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 @@ -7,6 +7,7 @@ th_stopped_time: 1.0 collision_check_margin: 1.0 collision_check_distance_from_end: 1.0 + 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 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 a451a92c16ff1..eb1827ec0045d 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,7 @@ class StartPlannerModule : public SceneModuleInterface const Pose & start_pose_candidate, const std::shared_ptr & planner, const Pose & refined_start_pose, const Pose & goal_pose); - PathWithLaneId extractCollisionCheckPath(const PullOutPath & path); + PathWithLaneId extractCollisionCheckSection(const PullOutPath & path); 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/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 783aace0983ca..2df75107d872c 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 @@ -44,6 +44,7 @@ struct StartPlannerParameters double length_ratio_for_turn_signal_deactivation_near_intersection{0.0}; double collision_check_margin{0.0}; 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}; diff --git a/planning/behavior_path_start_planner_module/src/manager.cpp b/planning/behavior_path_start_planner_module/src/manager.cpp index 58d0fbee7921b..f719718a3389e 100644 --- a/planning/behavior_path_start_planner_module/src/manager.cpp +++ b/planning/behavior_path_start_planner_module/src/manager.cpp @@ -46,6 +46,8 @@ void StartPlannerModuleManager::init(rclcpp::Node * node) p.collision_check_margin = node->declare_parameter(ns + "collision_check_margin"); 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"); p.center_line_path_interval = node->declare_parameter(ns + "center_line_path_interval"); // shift pull out 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 66e04d7e9bb7a..fd23adf654502 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 @@ -645,7 +645,7 @@ bool StartPlannerModule::findPullOutPath( // check collision if (utils::checkCollisionBetweenPathFootprintsAndObjects( - vehicle_footprint, extractCollisionCheckPath(*pull_out_path), pull_out_lane_stop_objects, + vehicle_footprint, extractCollisionCheckSection(*pull_out_path), pull_out_lane_stop_objects, parameters_->collision_check_margin)) { return false; } @@ -660,7 +660,7 @@ bool StartPlannerModule::findPullOutPath( return true; } -PathWithLaneId StartPlannerModule::extractCollisionCheckPath(const PullOutPath & path) +PathWithLaneId StartPlannerModule::extractCollisionCheckSection(const PullOutPath & path) { PathWithLaneId combined_path; for (const auto & partial_path : path.partial_paths) { @@ -912,6 +912,10 @@ std::vector StartPlannerModule::searchPullOutStartPoseCandidates( pull_out_lanes, start_pose.position, parameters_->th_moving_object_velocity, backward_path_length, std::numeric_limits::max()); + const auto front_stop_objects_in_pull_out_lanes = filterStopObjectsInPullOutLanes( + pull_out_lanes, start_pose.position, parameters_->th_moving_object_velocity, 0, + std::numeric_limits::max()); + // Set the maximum backward distance less than the distance from the vehicle's base_link to the // lane's rearmost point to prevent lane departure. const double current_arc_length = @@ -924,9 +928,12 @@ std::vector StartPlannerModule::searchPullOutStartPoseCandidates( back_distance += parameters_->backward_search_resolution) { const auto backed_pose = calcLongitudinalOffsetPose( back_path_from_start_pose.points, start_pose.position, -back_distance); - if (!backed_pose) { + if (!backed_pose) continue; + + if (utils::checkCollisionBetweenFootprintAndObjects( + local_vehicle_footprint, *backed_pose, front_stop_objects_in_pull_out_lanes, + parameters_->collision_check_margin_from_front_object)) continue; - } const double backed_pose_arc_length = lanelet::utils::getArcCoordinates(pull_out_lanes, *backed_pose).length;