From e4678d96d68efd2770f75aebefc4e6ef6b5bc572 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Tue, 17 Jan 2023 22:07:09 +0900 Subject: [PATCH] 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; }