From 06adbc4e06c9c58eb184175f256a1ac7a0a1c739 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Mon, 21 Oct 2024 21:22:52 +0900 Subject: [PATCH] feat(goal_planner): use vehicle side edge to check isCrossingPossible for pull over execution (#9102) Signed-off-by: kosuke55 --- .../src/goal_planner_module.cpp | 14 +++++++++++--- 1 file changed, 11 insertions(+), 3 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp index b4707d50c20e1..b17a5588835a5 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp @@ -704,11 +704,19 @@ bool GoalPlannerModule::isExecutionRequested() const lanelet::ConstLanelet target_lane{}; lanelet::utils::query::getClosestLanelet(pull_over_lanes, goal_pose, &target_lane); - lanelet::ConstLanelet previous_module_terminal_lane{}; + // Get the lanelet of the ego vehicle's side edge at the terminal pose of the previous module's + // path. Check if the lane is the target lane or the neighboring lane. NOTE: This is because in + // the case of avoidance, there is a possibility of base_link entering the neighboring lane, and + // we want to activate the pull over at this time as well. + const Pose previous_terminal_pose = getPreviousModuleOutput().path.points.back().point.pose; + const double vehicle_half_width = planner_data_->parameters.vehicle_width / 2.0; + const Pose previous_terminal_vehicle_edge_pose = calcOffsetPose( + previous_terminal_pose, 0, left_side_parking_ ? vehicle_half_width : -vehicle_half_width, 0); + lanelet::ConstLanelet previous_module_terminal_vehicle_edge_lane{}; route_handler->getClosestLaneletWithinRoute( - getPreviousModuleOutput().path.points.back().point.pose, &previous_module_terminal_lane); + previous_terminal_vehicle_edge_pose, &previous_module_terminal_vehicle_edge_lane); - if (!isCrossingPossible(previous_module_terminal_lane, target_lane)) { + if (!isCrossingPossible(previous_module_terminal_vehicle_edge_lane, target_lane)) { return false; }