Skip to content

Commit

Permalink
Merge pull request autowarefoundation#247 from tier4/feat/pullover_ho…
Browse files Browse the repository at this point in the history 
…tfix

* feat(behavior_path_planner): ignore pull over goal near lane start (autowarefoundation#2667)

feat(behavior_path_planner) ignore pull over goal near lane start

Signed-off-by: kosuke55 <[email protected]>

Signed-off-by: kosuke55 <[email protected]>

* feat(behavior_path_planner): add option for combining arc pull out paths (autowarefoundation#2669)

* feat(behavior_path_planner): add option for combining arc pull out paths

Signed-off-by: kosuke55 <[email protected]>

* divide_pull_out_path

Signed-off-by: kosuke55 <[email protected]>

Signed-off-by: kosuke55 <[email protected]>

Signed-off-by: kosuke55 <[email protected]>
Co-authored-by: Kosuke Takeuchi <[email protected]>
  • Loading branch information
@tkimura4 @kosuke55
tkimura4 and kosuke55 authored Jan 17, 2023
2 parents b9fce1f + 557270f commit f5936bd
Show file tree
Hide file tree
Showing 9 changed files with 47 additions and 22 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 @@ -240,16 +240,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_out/pull_out.param.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,6 +16,7 @@
deceleration_interval: 15.0
# geometric pull out
enable_geometric_pull_out: true
divide_pull_out_path: false
geometric_pull_out_velocity: 1.0
arc_path_interval: 1.0
lane_departure_margin: 0.2
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 ..._path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_parameters.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -37,6 +37,7 @@ struct PullOutParameters
double deceleration_interval;
// geometric pull out
bool enable_geometric_pull_out;
bool divide_pull_out_path;
double geometric_pull_out_velocity;
double arc_path_interval;
double lane_departure_margin;
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 @@ -38,6 +38,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
2 changes: 2 additions & 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 @@ -430,6 +430,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 Expand Up @@ -523,6 +524,7 @@ PullOutParameters BehaviorPathPlannerNode::getPullOutParam()
p.deceleration_interval = dp("deceleration_interval", 10.0);
// geometric pull out
p.enable_geometric_pull_out = dp("enable_geometric_pull_out", true);
p.divide_pull_out_path = dp("divide_pull_out_path", false);
p.geometric_pull_out_velocity = dp("geometric_pull_out_velocity", 1.0);
p.arc_path_interval = dp("arc_path_interval", 1.0);
p.lane_departure_margin = dp("lane_departure_margin", 0.2);
Expand Down
12 changes: 11 additions & 1 deletion planning/behavior_path_planner/src/scene_module/pull_out/geometric_pull_out.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -59,7 +59,17 @@ boost::optional<PullOutPath> GeometricPullOut::plan(Pose start_pose, Pose goal_p
return {};
}

output.partial_paths = planner_.getPaths();
if (parameters_.divide_pull_out_path) {
output.partial_paths = planner_.getPaths();
} else {
const auto partial_paths = planner_.getPaths();
auto combined_path = combineReferencePath(partial_paths.at(0), partial_paths.at(1));
// set same velocity to all points not to stop
for (auto & point : combined_path.points) {
point.point.longitudinal_velocity_mps = parameters_.geometric_pull_out_velocity;
}
output.partial_paths.push_back(combined_path);
}
output.start_pose = planner_.getArcPaths().at(0).points.back().point.pose;
output.end_pose = planner_.getArcPaths().at(1).points.back().point.pose;

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
22 changes: 11 additions & 11 deletions planning/behavior_path_planner/src/scene_module/pull_over/shift_pull_over.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -167,17 +167,6 @@ boost::optional<PullOverPath> ShiftPullOver::generatePullOverPath(
p.point.pose.position.z = goal_pose.position.z;
}

// check lane departure with road and shoulder lanes
const auto drivable_lanes =
util::generateDrivableLanesWithShoulderLanes(road_lanes, shoulder_lanes);
const auto expanded_lanes = util::expandLanelets(
drivable_lanes, parameters_.drivable_area_left_bound_offset,
parameters_.drivable_area_right_bound_offset);
if (lane_departure_checker_.checkPathWillLeaveLane(
util::transformToLanelets(expanded_lanes), shifted_path.path)) {
return {};
}

// set lane_id and velocity to shifted_path
for (size_t i = path_shifter.getShiftLines().front().start_idx;
i < shifted_path.path.points.size() - 1; ++i) {
Expand Down Expand Up @@ -212,6 +201,17 @@ boost::optional<PullOverPath> ShiftPullOver::generatePullOverPath(
pull_over_path.debug_poses.push_back(shift_end_pose_road_lane);
pull_over_path.debug_poses.push_back(actual_shift_end_pose);

// check if the parking path will leave lanes
const auto drivable_lanes =
util::generateDrivableLanesWithShoulderLanes(road_lanes, shoulder_lanes);
const auto expanded_lanes = util::expandLanelets(
drivable_lanes, parameters_.drivable_area_left_bound_offset,
parameters_.drivable_area_right_bound_offset);
if (lane_departure_checker_.checkPathWillLeaveLane(
util::transformToLanelets(expanded_lanes), pull_over_path.getParkingPath())) {
return {};
}

return pull_over_path;
}

Expand Down

0 comments on commit f5936bd

Please sign in to comment.