From e5b43bfcdf9ce87e4f4d1499cd1cd9d4d200b5b3 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Tue, 10 Sep 2024 03:19:09 +0900 Subject: [PATCH] feat(goal_planner): execute goal planner if previous module path terminal is pull over neighboring lane (#8715) Signed-off-by: kosuke55 --- .../README.md | 2 +- .../src/goal_planner_module.cpp | 45 ++++++++++++------- 2 files changed, 29 insertions(+), 18 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/README.md b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/README.md index 5e93c86c9be1c..485ec4cb2a359 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/README.md +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/README.md @@ -107,7 +107,7 @@ If the target path contains a goal, modify the points of the path so that the pa - Route is set with `allow_goal_modification=true` . - We can set this option with [SetRoute](https://github.com/autowarefoundation/autoware_adapi_msgs/blob/main/autoware_adapi_v1_msgs/routing/srv/SetRoute.srv#L2) api service. - We support `2D Rough Goal Pose` with the key bind `r` in RViz, but in the future there will be a panel of tools to manipulate various Route API from RViz. -- ego-vehicle is in the same lane as the goal. +- The terminal point of the current path is in the same lane sequence as the goal. If goal is on the road shoulder, then it is in the adjacent road lane sequence. 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 2c8385fdcc293..b321abe1490a1 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 @@ -596,15 +596,25 @@ bool GoalPlannerModule::isExecutionRequested() const const Pose & current_pose = planner_data_->self_odometry->pose.pose; const Pose goal_pose = route_handler->getOriginalGoalPose(); - // check if goal_pose is in current_lanes. - lanelet::ConstLanelet current_lane{}; - // const lanelet::ConstLanelets current_lanes = utils::getCurrentLanes(planner_data_); + // check if goal_pose is in current_lanes or neighboring road lanes const lanelet::ConstLanelets current_lanes = utils::getCurrentLanesFromPath(getPreviousModuleOutput().reference_path, planner_data_); - lanelet::utils::query::getClosestLanelet(current_lanes, current_pose, ¤t_lane); - const bool goal_is_in_current_lanes = std::any_of( - current_lanes.begin(), current_lanes.end(), [&](const lanelet::ConstLanelet & current_lane) { - return lanelet::utils::isInLanelet(goal_pose, current_lane); + const auto getNeighboringLane = + [&](const lanelet::ConstLanelet & lane) -> std::optional { + return left_side_parking_ ? route_handler->getLeftLanelet(lane, false, true) + : route_handler->getRightLanelet(lane, false, true); + }; + lanelet::ConstLanelets goal_check_lanes = current_lanes; + for (const auto & lane : current_lanes) { + auto neighboring_lane = getNeighboringLane(lane); + while (neighboring_lane) { + goal_check_lanes.push_back(neighboring_lane.value()); + neighboring_lane = getNeighboringLane(neighboring_lane.value()); + } + } + const bool goal_is_in_current_segment_lanes = std::any_of( + goal_check_lanes.begin(), goal_check_lanes.end(), [&](const lanelet::ConstLanelet & lane) { + return lanelet::utils::isInLanelet(goal_pose, lane); }); // check that goal is in current neighbor shoulder lane @@ -621,7 +631,7 @@ bool GoalPlannerModule::isExecutionRequested() const // if goal is not in current_lanes and current_shoulder_lanes, do not execute goal_planner, // because goal arc coordinates cannot be calculated. - if (!goal_is_in_current_lanes && !goal_is_in_current_shoulder_lanes) { + if (!goal_is_in_current_segment_lanes && !goal_is_in_current_shoulder_lanes) { return false; } @@ -629,7 +639,7 @@ bool GoalPlannerModule::isExecutionRequested() const // 1) goal_pose is in current_lanes, plan path to the original fixed goal // 2) goal_pose is NOT in current_lanes, do not execute goal_planner if (!utils::isAllowedGoalModification(route_handler)) { - return goal_is_in_current_lanes; + return goal_is_in_current_segment_lanes; } // if goal arc coordinates can be calculated, check if goal is in request_length @@ -651,7 +661,12 @@ bool GoalPlannerModule::isExecutionRequested() const parameters_->forward_goal_search_length); lanelet::ConstLanelet target_lane{}; lanelet::utils::query::getClosestLanelet(pull_over_lanes, goal_pose, &target_lane); - if (!isCrossingPossible(current_lane, target_lane)) { + + lanelet::ConstLanelet previous_module_terminal_lane{}; + route_handler->getClosestLaneletWithinRoute( + getPreviousModuleOutput().path.points.back().point.pose, &previous_module_terminal_lane); + + if (!isCrossingPossible(previous_module_terminal_lane, target_lane)) { return false; } @@ -2274,14 +2289,10 @@ bool GoalPlannerModule::isCrossingPossible( end_lane_sequence = route_handler->getLaneletSequence(end_lane, dist, dist, false); } - // Lambda function to get the neighboring lanelet based on left_side_parking_ - auto getNeighboringLane = + const auto getNeighboringLane = [&](const lanelet::ConstLanelet & lane) -> std::optional { - const auto neighboring_lane = left_side_parking_ ? route_handler->getLeftShoulderLanelet(lane) - : route_handler->getRightShoulderLanelet(lane); - if (neighboring_lane) return neighboring_lane; - return left_side_parking_ ? route_handler->getLeftLanelet(lane) - : route_handler->getRightLanelet(lane); + return left_side_parking_ ? route_handler->getLeftLanelet(lane, false, true) + : route_handler->getRightLanelet(lane, false, true); }; // Iterate through start_lane_sequence to find a path to end_lane_sequence