From 9f5ac014c8c560731605532e04801dfd980f83bf Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Tue, 17 Jan 2023 18:38:10 +0900 Subject: [PATCH 1/3] 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 b2a0f6085f532..e5b0160b4414f 100644 --- a/planning/behavior_path_planner/README.md +++ b/planning/behavior_path_planner/README.md @@ -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** 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 e0c7d5b86f9f1..74dc831f499a0 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 @@ -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; 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 80c2e16f9e9a2..1299d011abbdc 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -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 = 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; From 59adfafefa33ed77fd14442b9d5733667ff768ca Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Tue, 17 Jan 2023 21:01:49 +0900 Subject: [PATCH 2/3] feat(behavior_path_planner): add option for combining arc pull out paths (#2669) * feat(behavior_path_planner): add option for combining arc pull out paths Signed-off-by: kosuke55 * divide_pull_out_path Signed-off-by: kosuke55 Signed-off-by: kosuke55 --- .../config/pull_out/pull_out.param.yaml | 1 + .../scene_module/pull_out/pull_out_parameters.hpp | 1 + .../src/behavior_path_planner_node.cpp | 1 + .../src/scene_module/pull_out/geometric_pull_out.cpp | 12 +++++++++++- 4 files changed, 14 insertions(+), 1 deletion(-) diff --git a/planning/behavior_path_planner/config/pull_out/pull_out.param.yaml b/planning/behavior_path_planner/config/pull_out/pull_out.param.yaml index b092eb7fc8802..bde6daa42d231 100644 --- a/planning/behavior_path_planner/config/pull_out/pull_out.param.yaml +++ b/planning/behavior_path_planner/config/pull_out/pull_out.param.yaml @@ -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 diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_parameters.hpp index 21b66d1ad8457..b69e27da9f467 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_parameters.hpp @@ -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; 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 1299d011abbdc..b8d4e20470537 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -524,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); diff --git a/planning/behavior_path_planner/src/scene_module/pull_out/geometric_pull_out.cpp b/planning/behavior_path_planner/src/scene_module/pull_out/geometric_pull_out.cpp index a080eefdf64f7..2316ac35c2ef5 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_out/geometric_pull_out.cpp +++ b/planning/behavior_path_planner/src/scene_module/pull_out/geometric_pull_out.cpp @@ -59,7 +59,17 @@ boost::optional 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; From e4678d96d68efd2770f75aebefc4e6ef6b5bc572 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Tue, 17 Jan 2023 22:07:09 +0900 Subject: [PATCH 3/3] fix(behavior_path_planner): pull_over checks lane departure only for parking part (#2673) Signed-off-by: kosuke55 Signed-off-by: kosuke55 --- .../pull_over/shift_pull_over.cpp | 22 +++++++++---------- 1 file changed, 11 insertions(+), 11 deletions(-) diff --git a/planning/behavior_path_planner/src/scene_module/pull_over/shift_pull_over.cpp b/planning/behavior_path_planner/src/scene_module/pull_over/shift_pull_over.cpp index d322a9613c6cf..731b818666347 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_over/shift_pull_over.cpp +++ b/planning/behavior_path_planner/src/scene_module/pull_over/shift_pull_over.cpp @@ -167,17 +167,6 @@ boost::optional 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) { @@ -212,6 +201,17 @@ boost::optional 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; }