Skip to content

Commit

Permalink
feat(behavior_path_planner): ignore pull over goal near lane start (a…
Browse files Browse the repository at this point in the history 
…utowarefoundation#2667)

feat(behavior_path_planner) ignore pull over goal near lane start

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>
  • Loading branch information
@kosuke55 @maxime-clem
kosuke55 authored and maxime-clem committed Jan 30, 2023
1 parent fb01a20 commit 1797699
Show file tree
Hide file tree
Showing 5 changed files with 22 additions and 10 deletions.
21 changes: 11 additions & 10 deletions planning/behavior_path_planner/README.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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**

Expand Down
1 change: 1 addition & 0 deletions planning/behavior_path_planner/config/pull_over/pull_over.param.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
1 change: 1 addition & 0 deletions ...ath_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_parameters.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
1 change: 1 addition & 0 deletions planning/behavior_path_planner/src/behavior_path_planner_node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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 =
Expand Down
8 changes: 8 additions & 0 deletions planning/behavior_path_planner/src/scene_module/pull_over/goal_searcher.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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_);
Expand All @@ -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;
Expand Down

0 comments on commit 1797699

Please sign in to comment.