From 04ef74c00b4a2629a1c3ea7135d15f4b9c49a370 Mon Sep 17 00:00:00 2001 From: kyoichi-sugahara Date: Tue, 23 Jan 2024 14:27:59 +0900 Subject: [PATCH 1/7] Update collision check margins in start planner module Signed-off-by: kyoichi-sugahara --- .../README.md | 27 ++++++++++--------- .../config/start_planner.param.yaml | 3 ++- .../data_structs.hpp | 3 ++- .../src/manager.cpp | 6 +++-- .../src/start_planner_module.cpp | 10 +++---- 5 files changed, 27 insertions(+), 22 deletions(-) diff --git a/planning/behavior_path_start_planner_module/README.md b/planning/behavior_path_start_planner_module/README.md index b26864eb450d5..04f73ca621375 100644 --- a/planning/behavior_path_start_planner_module/README.md +++ b/planning/behavior_path_start_planner_module/README.md @@ -65,19 +65,20 @@ 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_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 | +| 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 | +| front_objects_collision_check_margins | [m] | [double] | obstacle collision check margins against front static objects 　 from the footprint on the trajectory in pull_out_lanes | [2.0, 1.0, 0.5, 0.1] | +| collision_check_distance_from_end | [m] | double | collision check distance from end shift end pose | 1.0 | +| back_objects_collision_check_margin | [m] | double | obstacle collision check margins against back static objects from footprint on the start pose in pull_out_lanes | 3.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 d174b54b9ccbe..56cfee6e53fe2 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,8 +5,9 @@ th_arrived_distance: 1.0 th_stopped_velocity: 0.01 th_stopped_time: 1.0 - collision_check_margins: [2.0, 1.5, 1.0] + front_objects_collision_check_margins: [2.0, 1.0, 0.5, 0.1] collision_check_distance_from_end: 1.0 + back_objects_collision_check_margin: 3.0 collision_check_margin_from_front_object: 5.0 th_moving_object_velocity: 1.0 th_distance_to_middle_of_the_road: 0.5 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..c0f7456a41e4b 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 @@ -62,8 +62,9 @@ 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}; - std::vector collision_check_margins{}; + std::vector front_objects_collision_check_margins{}; double collision_check_distance_from_end{0.0}; + double back_objects_collision_check_margin{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 8cc0b34082e44..fdb980fa59c26 100644 --- a/planning/behavior_path_start_planner_module/src/manager.cpp +++ b/planning/behavior_path_start_planner_module/src/manager.cpp @@ -43,10 +43,12 @@ 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_margins = - node->declare_parameter>(ns + "collision_check_margins"); + p.front_objects_collision_check_margins = + node->declare_parameter>(ns + "front_objects_collision_check_margins"); p.collision_check_distance_from_end = node->declare_parameter(ns + "collision_check_distance_from_end"); + p.back_objects_collision_check_margin = + node->declare_parameter(ns + "back_objects_collision_check_margin"); 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"); 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 6c9000caac956..1c93e53627687 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 @@ -595,7 +595,7 @@ void StartPlannerModule::planWithPriority( const PriorityOrder order_priority = determinePriorityOrder(search_priority, start_pose_candidates.size()); - for (const auto & collision_check_margin : parameters_->collision_check_margins) { + for (const auto & collision_check_margin : parameters_->front_objects_collision_check_margins) { for (const auto & [index, planner] : order_priority) { if (findPullOutPath( start_pose_candidates[index], planner, refined_start_pose, goal_pose, @@ -923,9 +923,9 @@ std::vector StartPlannerModule::searchPullOutStartPoseCandidates( const double backward_path_length = planner_data_->parameters.backward_path_length + parameters_->max_back_distance; - const auto stop_objects_in_pull_out_lanes = filterStopObjectsInPullOutLanes( + const auto back_stop_objects_in_pull_out_lanes = filterStopObjectsInPullOutLanes( pull_out_lanes, start_pose.position, parameters_->th_moving_object_velocity, - backward_path_length, std::numeric_limits::max()); + backward_path_length, 0); const auto front_stop_objects_in_pull_out_lanes = filterStopObjectsInPullOutLanes( pull_out_lanes, start_pose.position, parameters_->th_moving_object_velocity, 0, @@ -964,8 +964,8 @@ std::vector StartPlannerModule::searchPullOutStartPoseCandidates( } if (utils::checkCollisionBetweenFootprintAndObjects( - local_vehicle_footprint, *backed_pose, stop_objects_in_pull_out_lanes, - parameters_->collision_check_margins.back())) { + local_vehicle_footprint, *backed_pose, back_stop_objects_in_pull_out_lanes, + parameters_->back_objects_collision_check_margin)) { break; // poses behind this has a collision, so break. } From 3e919f698428eefa81d060b2c049a4c53c944f9b Mon Sep 17 00:00:00 2001 From: kyoichi-sugahara Date: Tue, 23 Jan 2024 15:41:50 +0900 Subject: [PATCH 2/7] rename variable and update readme Signed-off-by: kyoichi-sugahara --- .../README.md | 28 +++++++++---------- .../config/start_planner.param.yaml | 2 +- .../data_structs.hpp | 2 +- .../src/manager.cpp | 4 +-- .../src/start_planner_module.cpp | 2 +- 5 files changed, 19 insertions(+), 19 deletions(-) diff --git a/planning/behavior_path_start_planner_module/README.md b/planning/behavior_path_start_planner_module/README.md index 04f73ca621375..6d1c3061def22 100644 --- a/planning/behavior_path_start_planner_module/README.md +++ b/planning/behavior_path_start_planner_module/README.md @@ -65,20 +65,20 @@ 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 | -| front_objects_collision_check_margins | [m] | [double] | obstacle collision check margins against front static objects 　 from the footprint on the trajectory in pull_out_lanes | [2.0, 1.0, 0.5, 0.1] | -| collision_check_distance_from_end | [m] | double | collision check distance from end shift end pose | 1.0 | -| back_objects_collision_check_margin | [m] | double | obstacle collision check margins against back static objects from footprint on the start pose in pull_out_lanes | 3.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 | +| objects_collision_check_margins | [m] | [double] | obstacle collision check margins against static objects from the footprint on the trajectory in pull_out_lanes | [2.0, 1.0, 0.5, 0.1] | +| collision_check_distance_from_end | [m] | double | collision check distance from end shift end pose | 1.0 | +| back_objects_collision_check_margin | [m] | double | obstacle collision check margin against back static objects from the footprint on the start pose in pull_out_lanes | 3.0 | +| collision_check_margin_from_front_object | [m] | double | obstacle collision check margin against front static objects from the footprint on the start pose in pull_out_lanes | 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 56cfee6e53fe2..68a15f17d6af2 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 - front_objects_collision_check_margins: [2.0, 1.0, 0.5, 0.1] + objects_collision_check_margins: [2.0, 1.0, 0.5, 0.1] collision_check_distance_from_end: 1.0 back_objects_collision_check_margin: 3.0 collision_check_margin_from_front_object: 5.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 c0f7456a41e4b..c8f305b4b5cee 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 @@ -62,7 +62,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}; - std::vector front_objects_collision_check_margins{}; + std::vector objects_collision_check_margins{}; double collision_check_distance_from_end{0.0}; double back_objects_collision_check_margin{0.0}; double collision_check_margin_from_front_object{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 fdb980fa59c26..cc67b1881aa08 100644 --- a/planning/behavior_path_start_planner_module/src/manager.cpp +++ b/planning/behavior_path_start_planner_module/src/manager.cpp @@ -43,8 +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.front_objects_collision_check_margins = - node->declare_parameter>(ns + "front_objects_collision_check_margins"); + p.objects_collision_check_margins = + node->declare_parameter>(ns + "objects_collision_check_margins"); p.collision_check_distance_from_end = node->declare_parameter(ns + "collision_check_distance_from_end"); p.back_objects_collision_check_margin = 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 1c93e53627687..983f518602316 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 @@ -595,7 +595,7 @@ void StartPlannerModule::planWithPriority( const PriorityOrder order_priority = determinePriorityOrder(search_priority, start_pose_candidates.size()); - for (const auto & collision_check_margin : parameters_->front_objects_collision_check_margins) { + for (const auto & collision_check_margin : parameters_->objects_collision_check_margins) { for (const auto & [index, planner] : order_priority) { if (findPullOutPath( start_pose_candidates[index], planner, refined_start_pose, goal_pose, From e27daf8df67a5805d76b5493432018fa5777a682 Mon Sep 17 00:00:00 2001 From: kyoichi-sugahara Date: Wed, 7 Feb 2024 17:52:45 +0900 Subject: [PATCH 3/7] Fix filterStopObjectsInPullOutLanes arguments order Signed-off-by: kyoichi-sugahara --- .../src/start_planner_module.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) 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 a2b0ea712aeee..3aab349eca010 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 @@ -965,12 +965,12 @@ std::vector StartPlannerModule::searchPullOutStartPoseCandidates( planner_data_->parameters.backward_path_length + parameters_->max_back_distance; const auto back_stop_objects_in_pull_out_lanes = filterStopObjectsInPullOutLanes( - pull_out_lanes, start_pose.position, parameters_->th_moving_object_velocity, - backward_path_length, 0); + pull_out_lanes, start_pose.position, parameters_->th_moving_object_velocity, 0, + backward_path_length); 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()); + pull_out_lanes, start_pose.position, parameters_->th_moving_object_velocity, + std::numeric_limits::max(), 0); // 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. From 098b6fd80e51e9e4f3df3b1438745d67809f478d Mon Sep 17 00:00:00 2001 From: kyoichi-sugahara Date: Fri, 9 Feb 2024 02:16:05 +0900 Subject: [PATCH 4/7] calculate longitudinal distance Signed-off-by: kyoichi-sugahara --- .../src/start_planner_module.cpp | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) 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 3aab349eca010..4ad59673b3d2c 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 @@ -1004,10 +1004,12 @@ std::vector StartPlannerModule::searchPullOutStartPoseCandidates( continue; } - if (utils::checkCollisionBetweenFootprintAndObjects( - local_vehicle_footprint, *backed_pose, back_stop_objects_in_pull_out_lanes, - parameters_->back_objects_collision_check_margin)) { - break; // poses behind this has a collision, so break. + if ( + utils::calcLongitudinalDistanceFromEgoToObjects( + *backed_pose, planner_data_->parameters.base_link2front, + planner_data_->parameters.base_link2rear, + back_stop_objects_in_pull_out_lanes) < parameters_->back_objects_collision_check_margin) { + break; // poses behind this is too close to back static object, so break. } pull_out_start_pose_candidates.push_back(*backed_pose); From 16e7104136a1703b70e6ab8c81b6c9290d31df22 Mon Sep 17 00:00:00 2001 From: kyoichi-sugahara Date: Sat, 10 Feb 2024 03:40:28 +0900 Subject: [PATCH 5/7] calculate minimum arc length Signed-off-by: kyoichi-sugahara --- .../util.hpp | 25 +++++++++++++++ .../src/start_planner_module.cpp | 13 ++++---- .../src/util.cpp | 32 +++++++++++++++++++ 3 files changed, 63 insertions(+), 7 deletions(-) diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/util.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/util.hpp index 147632329d926..4081b32bd69a5 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/util.hpp +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/util.hpp @@ -46,10 +46,35 @@ using route_handler::RouteHandler; PathWithLaneId getBackwardPath( const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes, const Pose & current_pose, const Pose & backed_pose, const double velocity); + +/** + * @brief Get a sequence of lanelets for pulling out from the current position. + * + * If the ego vehicle's current position is on a shoulder lane, the function retrieves a sequence of + * shoulder lanelets. If it is on a road lane, the function returns a sequence of road lanelets. + * + */ lanelet::ConstLanelets getPullOutLanes( const std::shared_ptr & planner_data, const double backward_length); Pose getBackedPose( const Pose & current_pose, const double & yaw_shoulder_lane, const double & back_distance); + +/** + * @brief Calculate the minimum arc length distance from the ego vehicle to static objects within + the same lanelets. + * + * Calculates the minimum arc length distance between the ego vehicle and static objects in given + * lanelets. It compares each corner of the vehicle's transformed footprint with every corner of + * object polygons to find the shortest distance within the lanelet system. + */ + +double calcMinArcLengthDistanceFromEgoToObjects( + const tier4_autoware_utils::LinearRing2d & local_vehicle_footprint, const Pose & ego_pose, + const lanelet::ConstLanelets & lanelets, const PredictedObjects & static_objects); + +double getArcLengthForPoint( + const lanelet::ConstLanelets & lanelets, const tier4_autoware_utils::Point2d & point); + } // namespace behavior_path_planner::start_planner_utils #endif // BEHAVIOR_PATH_START_PLANNER_MODULE__UTIL_HPP_ 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 4ad59673b3d2c..c69182584e2a7 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 @@ -1005,9 +1005,8 @@ std::vector StartPlannerModule::searchPullOutStartPoseCandidates( } if ( - utils::calcLongitudinalDistanceFromEgoToObjects( - *backed_pose, planner_data_->parameters.base_link2front, - planner_data_->parameters.base_link2rear, + start_planner_utils::calcMinArcLengthDistanceFromEgoToObjects( + local_vehicle_footprint, *backed_pose, pull_out_lanes, back_stop_objects_in_pull_out_lanes) < parameters_->back_objects_collision_check_margin) { break; // poses behind this is too close to back static object, so break. } @@ -1019,8 +1018,8 @@ std::vector StartPlannerModule::searchPullOutStartPoseCandidates( PredictedObjects StartPlannerModule::filterStopObjectsInPullOutLanes( const lanelet::ConstLanelets & pull_out_lanes, const geometry_msgs::msg::Point & current_point, - const double velocity_threshold, const double object_check_forward_distance, - const double object_check_backward_distance) const + const double velocity_threshold, const double object_check_backward_distance, + const double object_check_forward_distance) const { const auto stop_objects = utils::path_safety_checker::filterObjectsByVelocity( *planner_data_->dynamic_object, velocity_threshold); @@ -1034,8 +1033,8 @@ PredictedObjects StartPlannerModule::filterStopObjectsInPullOutLanes( pull_out_lanes, object_check_backward_distance, object_check_forward_distance); utils::path_safety_checker::filterObjectsByPosition( - stop_objects_in_pull_out_lanes, path.points, current_point, object_check_forward_distance, - object_check_backward_distance); + stop_objects_in_pull_out_lanes, path.points, current_point, object_check_backward_distance, + object_check_forward_distance); return stop_objects_in_pull_out_lanes; } diff --git a/planning/behavior_path_start_planner_module/src/util.cpp b/planning/behavior_path_start_planner_module/src/util.cpp index b34398083a95c..83838165b54d1 100644 --- a/planning/behavior_path_start_planner_module/src/util.cpp +++ b/planning/behavior_path_start_planner_module/src/util.cpp @@ -17,6 +17,7 @@ #include "behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" #include "behavior_path_planner_common/utils/path_utils.hpp" #include "behavior_path_planner_common/utils/utils.hpp" +#include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" #include #include @@ -104,4 +105,35 @@ lanelet::ConstLanelets getPullOutLanes( /*forward_only_in_route*/ true); } +double calcMinArcLengthDistanceFromEgoToObjects( + const tier4_autoware_utils::LinearRing2d & local_vehicle_footprint, const Pose & ego_pose, + const lanelet::ConstLanelets & lanelets, const PredictedObjects & static_objects) +{ + double min_distance = std::numeric_limits::max(); + const auto vehicle_footprint = + transformVector(local_vehicle_footprint, tier4_autoware_utils::pose2transform(ego_pose)); + for (const auto & obj : static_objects.objects) { + const auto obj_polygon = tier4_autoware_utils::toPolygon2d(obj); + for (const auto & obj_outer_point : obj_polygon.outer()) { + const auto obj_pose_arc_length = getArcLengthForPoint(lanelets, obj_outer_point); + for (const auto & vehicle_corner_point : vehicle_footprint) { + const auto vehicle_pose_arc_length = getArcLengthForPoint(lanelets, vehicle_corner_point); + const double distance = std::abs(obj_pose_arc_length - vehicle_pose_arc_length); + min_distance = std::min(min_distance, distance); + } + } + } + + return min_distance; +} + +double getArcLengthForPoint( + const lanelet::ConstLanelets & lanelets, const tier4_autoware_utils::Point2d & point) +{ + geometry_msgs::msg::Pose pose; + pose.position.x = point.x(); + pose.position.y = point.y(); + return lanelet::utils::getArcCoordinates(lanelets, pose).length; +} + } // namespace behavior_path_planner::start_planner_utils From b73b55c2dcb2ee02aa32cf9a50d3857ababd76c9 Mon Sep 17 00:00:00 2001 From: kyoichi-sugahara Date: Sat, 10 Feb 2024 04:50:11 +0900 Subject: [PATCH 6/7] add lateral condition Signed-off-by: kyoichi-sugahara --- .../src/start_planner_module.cpp | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) 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 c69182584e2a7..f136b503da69d 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 @@ -1005,9 +1005,12 @@ std::vector StartPlannerModule::searchPullOutStartPoseCandidates( } if ( - start_planner_utils::calcMinArcLengthDistanceFromEgoToObjects( - local_vehicle_footprint, *backed_pose, pull_out_lanes, - back_stop_objects_in_pull_out_lanes) < parameters_->back_objects_collision_check_margin) { + (start_planner_utils::calcMinArcLengthDistanceFromEgoToObjects( + local_vehicle_footprint, *backed_pose, pull_out_lanes, + back_stop_objects_in_pull_out_lanes) > parameters_->back_objects_collision_check_margin) && + (utils::checkCollisionBetweenFootprintAndObjects( + local_vehicle_footprint, *backed_pose, back_stop_objects_in_pull_out_lanes, + parameters_->objects_collision_check_margins.back()))) { break; // poses behind this is too close to back static object, so break. } From 2b7d88f1c3abd574710c39a878bd1ef5166d8bb7 Mon Sep 17 00:00:00 2001 From: kyoichi-sugahara Date: Sun, 11 Feb 2024 15:16:34 +0900 Subject: [PATCH 7/7] update Signed-off-by: kyoichi-sugahara --- .../config/start_planner.param.yaml | 2 +- .../include/behavior_path_start_planner_module/util.hpp | 1 - .../src/start_planner_module.cpp | 2 +- 3 files changed, 2 insertions(+), 3 deletions(-) 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 60b81fe5d462b..2ff013ec95906 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 @@ -6,7 +6,7 @@ th_stopped_velocity: 0.01 th_stopped_time: 1.0 objects_collision_check_margins: [2.0, 1.0, 0.5, 0.1] - back_objects_collision_check_margin: 3.0 + back_objects_collision_check_margin: 0.5 collision_check_margin_from_front_object: 5.0 th_moving_object_velocity: 1.0 th_distance_to_middle_of_the_road: 0.5 diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/util.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/util.hpp index 4081b32bd69a5..cd4dd23b6567b 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/util.hpp +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/util.hpp @@ -67,7 +67,6 @@ Pose getBackedPose( * lanelets. It compares each corner of the vehicle's transformed footprint with every corner of * object polygons to find the shortest distance within the lanelet system. */ - double calcMinArcLengthDistanceFromEgoToObjects( const tier4_autoware_utils::LinearRing2d & local_vehicle_footprint, const Pose & ego_pose, const lanelet::ConstLanelets & lanelets, const PredictedObjects & static_objects); 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 f136b503da69d..cd87892aebf1c 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 @@ -1007,7 +1007,7 @@ std::vector StartPlannerModule::searchPullOutStartPoseCandidates( if ( (start_planner_utils::calcMinArcLengthDistanceFromEgoToObjects( local_vehicle_footprint, *backed_pose, pull_out_lanes, - back_stop_objects_in_pull_out_lanes) > parameters_->back_objects_collision_check_margin) && + back_stop_objects_in_pull_out_lanes) < parameters_->back_objects_collision_check_margin) || (utils::checkCollisionBetweenFootprintAndObjects( local_vehicle_footprint, *backed_pose, back_stop_objects_in_pull_out_lanes, parameters_->objects_collision_check_margins.back()))) {