Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

feat(pullover): hotfix to beta/v0.7.0 #247

Merged
merged 4 commits into from
Jan 17, 2023
Merged

feat(pullover): hotfix to beta/v0.7.0 #247

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
9f5ac01
feat(behavior_path_planner): ignore pull over goal near lane start (#…
kosuke55 Jan 17, 2023
59adfaf
feat(behavior_path_planner): add option for combining arc pull out pa…
kosuke55 Jan 17, 2023
e4678d9
fix(behavior_path_planner): pull_over checks lane departure only for …
kosuke55 Jan 17, 2023
557270f
Merge branch 'beta/v0.7.0' into feat/pullover_hotfix
tkimura4 Jan 17, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
21 changes: 11 additions & 10 deletions planning/behavior_path_planner/README.md
View file
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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