From 96f632d1b17f2702d987d14e1ff7be5b65be0842 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Fri, 13 Dec 2024 20:00:18 +0900 Subject: [PATCH] fix of PR9641 Signed-off-by: Mamoru Sobue --- .../pull_over_planner/bezier_pull_over.cpp | 40 +++++++++---------- 1 file changed, 19 insertions(+), 21 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/bezier_pull_over.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/bezier_pull_over.cpp index c652e2ceac7c1..0da5ab5dae38b 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/bezier_pull_over.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/bezier_pull_over.cpp @@ -119,7 +119,7 @@ std::vector BezierPullOver::generateBezierPath( const GoalCandidate & goal_candidate, const size_t id, const std::shared_ptr planner_data, const BehaviorModuleOutput & previous_module_output, const lanelet::ConstLanelets & road_lanes, - const lanelet::ConstLanelets & shoulder_lanes, const double lateral_jerk) const + const lanelet::ConstLanelets & pull_over_lanes, const double lateral_jerk) const { const double pull_over_velocity = parameters_.pull_over_velocity; @@ -244,14 +244,23 @@ std::vector BezierPullOver::generateBezierPath( autoware::motion_utils::insertOrientation(shifted_path.path.points, true); + // combine road lanes and shoulder lanes to find closest lanelet id + const auto lanes = std::invoke([&]() -> lanelet::ConstLanelets { + auto lanes = road_lanes; + lanes.insert(lanes.end(), pull_over_lanes.begin(), pull_over_lanes.end()); + return lanes; // not copy the value (Return Value Optimization) + }); + // set goal pose with velocity 0 { PathPointWithLaneId p{}; p.point.longitudinal_velocity_mps = 0.0; p.point.pose = goal_pose; - p.lane_ids = shifted_path.path.points.back().lane_ids; - for (const auto & lane : shoulder_lanes) { - p.lane_ids.push_back(lane.id()); + lanelet::Lanelet goal_lanelet{}; + if (lanelet::utils::query::getClosestLanelet(lanes, goal_pose, &goal_lanelet)) { + p.lane_ids = {goal_lanelet.id()}; + } else { + p.lane_ids = shifted_path.path.points.back().lane_ids; } shifted_path.path.points.push_back(p); } @@ -260,24 +269,13 @@ std::vector BezierPullOver::generateBezierPath( for (size_t i = path_shifter.getShiftLines().front().start_idx; i < shifted_path.path.points.size() - 1; ++i) { auto & point = shifted_path.path.points.at(i); - // set velocity point.point.longitudinal_velocity_mps = std::min(point.point.longitudinal_velocity_mps, static_cast(pull_over_velocity)); - - // add target lanes to points after shift start - // add road lane_ids if not found - for (const auto id : shifted_path.path.points.back().lane_ids) { - if (std::find(point.lane_ids.begin(), point.lane_ids.end(), id) == point.lane_ids.end()) { - point.lane_ids.push_back(id); - } - } - // add shoulder lane_id if not found - for (const auto & lane : shoulder_lanes) { - if ( - std::find(point.lane_ids.begin(), point.lane_ids.end(), lane.id()) == - point.lane_ids.end()) { - point.lane_ids.push_back(lane.id()); - } + lanelet::Lanelet lanelet{}; + if (lanelet::utils::query::getClosestLanelet(lanes, point.point.pose, &lanelet)) { + point.lane_ids = {lanelet.id()}; // overwrite lane_ids + } else { + point.lane_ids = shifted_path.path.points.at(i - 1).lane_ids; } } @@ -314,7 +312,7 @@ std::vector BezierPullOver::generateBezierPath( }); const bool is_in_lanes = std::invoke([&]() -> bool { const auto drivable_lanes = - utils::generateDrivableLanesWithShoulderLanes(road_lanes, shoulder_lanes); + utils::generateDrivableLanesWithShoulderLanes(road_lanes, pull_over_lanes); const auto & dp = planner_data->drivable_area_expansion_parameters; const auto expanded_lanes = utils::expandLanelets( drivable_lanes, dp.drivable_area_left_bound_offset, dp.drivable_area_right_bound_offset,