From 1797699dfb1110324125ef1b3e171ad927a2ef1e Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Tue, 17 Jan 2023 18:38:10 +0900 Subject: [PATCH] feat(behavior_path_planner): ignore pull over goal near lane start (#2667) feat(behavior_path_planner) ignore pull over goal near lane start Signed-off-by: kosuke55 Signed-off-by: kosuke55 --- planning/behavior_path_planner/README.md | 21 ++++++++++--------- .../config/pull_over/pull_over.param.yaml | 1 + .../pull_over/pull_over_parameters.hpp | 1 + .../src/behavior_path_planner_node.cpp | 1 + .../scene_module/pull_over/goal_searcher.cpp | 8 +++++++ 5 files changed, 22 insertions(+), 10 deletions(-) diff --git a/planning/behavior_path_planner/README.md b/planning/behavior_path_planner/README.md index a9d04d307fdd2..520d61f864f64 100644 --- a/planning/behavior_path_planner/README.md +++ b/planning/behavior_path_planner/README.md @@ -253,16 +253,17 @@ searched for in certain range of the shoulder lane. ##### Parameters for goal search -| Name | Unit | Type | Description | Default value | -| :-------------------------- | :--- | :----- | :----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------- | -| search_priority | - | string | In case `efficient_path` use a goal that can generate an efficient path( priority is `shift_parking` -> `arc_forward_parking` -> `arc_backward_parking`). In case `close_goal` use the closest goal to the original one. | efficient_path | -| enable_goal_research | - | double | flag whether to search goal | true | -| forward_goal_search_length | [m] | double | length of forward range to be explored from the original goal | 20.0 | -| backward_goal_search_length | [m] | double | length of backward range to be explored from the original goal | 20.0 | -| goal_search_interval | [m] | double | distance interval for goal search | 2.0 | -| longitudinal_margin | [m] | double | margin between ego-vehicle at the goal position and obstacles | 3.0 | -| max_lateral_offset | [m] | double | maximum offset of goal search in the lateral direction | 3.0 | -| lateral_offset_interval | [m] | double | distance interval of goal search in the lateral direction | 3.0 | +| Name | Unit | Type | Description | Default value | +| :------------------------------ | :--- | :----- | :----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------- | +| search_priority | - | string | In case `efficient_path` use a goal that can generate an efficient path( priority is `shift_parking` -> `arc_forward_parking` -> `arc_backward_parking`). In case `close_goal` use the closest goal to the original one. | efficient_path | +| enable_goal_research | - | double | flag whether to search goal | true | +| forward_goal_search_length | [m] | double | length of forward range to be explored from the original goal | 20.0 | +| backward_goal_search_length | [m] | double | length of backward range to be explored from the original goal | 20.0 | +| goal_search_interval | [m] | double | distance interval for goal search | 2.0 | +| longitudinal_margin | [m] | double | margin between ego-vehicle at the goal position and obstacles | 3.0 | +| max_lateral_offset | [m] | double | maximum offset of goal search in the lateral direction | 3.0 | +| lateral_offset_interval | [m] | double | distance interval of goal search in the lateral direction | 3.0 | +| ignore_distance_from_lane_start | [m] | double | distance from start of pull over lanes for ignoring goal candidates | 15.0 | #### **Path Generation** diff --git a/planning/behavior_path_planner/config/pull_over/pull_over.param.yaml b/planning/behavior_path_planner/config/pull_over/pull_over.param.yaml index 75b5facaaaafe..5a3d4fb771381 100644 --- a/planning/behavior_path_planner/config/pull_over/pull_over.param.yaml +++ b/planning/behavior_path_planner/config/pull_over/pull_over.param.yaml @@ -19,6 +19,7 @@ longitudinal_margin: 3.0 max_lateral_offset: 1.0 lateral_offset_interval: 0.25 + ignore_distance_from_lane_start: 15.0 # occupancy grid map use_occupancy_grid: true use_occupancy_grid_for_longitudinal_margin: false diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_parameters.hpp index 5f43cd152832b..c47e0eefc5409 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_parameters.hpp @@ -39,6 +39,7 @@ struct PullOverParameters double longitudinal_margin; double max_lateral_offset; double lateral_offset_interval; + double ignore_distance_from_lane_start; // occupancy grid map bool use_occupancy_grid; bool use_occupancy_grid_for_longitudinal_margin; diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index 48f2099f03717..5d7867a60defd 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -550,6 +550,7 @@ PullOverParameters BehaviorPathPlannerNode::getPullOverParam() p.longitudinal_margin = dp("longitudinal_margin", 3.0); p.max_lateral_offset = dp("max_lateral_offset", 1.0); p.lateral_offset_interval = dp("lateral_offset_interval", 0.25); + p.ignore_distance_from_lane_start = dp("ignore_distance_from_lane_start", 15.0); // occupancy grid map p.use_occupancy_grid = dp("use_occupancy_grid", true); p.use_occupancy_grid_for_longitudinal_margin = diff --git a/planning/behavior_path_planner/src/scene_module/pull_over/goal_searcher.cpp b/planning/behavior_path_planner/src/scene_module/pull_over/goal_searcher.cpp index f8ef93edc55a5..fa1beb8ddee9e 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_over/goal_searcher.cpp +++ b/planning/behavior_path_planner/src/scene_module/pull_over/goal_searcher.cpp @@ -49,6 +49,7 @@ GoalCandidates GoalSearcher::search(const Pose & original_goal_pose) const double margin_from_boundary = parameters_.margin_from_boundary; const double lateral_offset_interval = parameters_.lateral_offset_interval; const double max_lateral_offset = parameters_.max_lateral_offset; + const double ignore_distance_from_lane_start = parameters_.ignore_distance_from_lane_start; const auto pull_over_lanes = pull_over_utils::getPullOverLanes(*route_handler); auto lanes = util::getExtendedCurrentLanes(planner_data_); @@ -70,6 +71,13 @@ GoalCandidates GoalSearcher::search(const Pose & original_goal_pose) for (const auto & p : center_line_path.points) { const Pose & center_pose = p.point.pose; + // ignore goal_pose near lane start + const double distance_from_lane_start = + lanelet::utils::getArcCoordinates(pull_over_lanes, center_pose).length; + if (distance_from_lane_start < ignore_distance_from_lane_start) { + continue; + } + const auto distance_from_left_bound = util::getSignedDistanceFromShoulderLeftBoundary( pull_over_lanes, vehicle_footprint_, center_pose); if (!distance_from_left_bound) continue;