From 80621b0821188736852021b5c81f2f6d85b2826f Mon Sep 17 00:00:00 2001 From: TetsuKawa Date: Fri, 6 Dec 2024 18:58:52 +0900 Subject: [PATCH] Revert "feat(goal_planner): execute goal planner if previous module path terminal is pull over neighboring lane (#8715)" This reverts commit 11cb352cc823ca702628f607851072b146de4c4f. --- .../README.md | 2 +- .../src/goal_planner_module.cpp | 45 +++++++------------ 2 files changed, 18 insertions(+), 29 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 485ec4cb2a359..5e93c86c9be1c 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. -- 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. +- ego-vehicle is in the same lane as the goal. 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 7e2f400580d91..1f0d185212831 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 @@ -581,25 +581,15 @@ 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 or neighboring road lanes + // check if goal_pose is in current_lanes. + lanelet::ConstLanelet current_lane{}; + // const lanelet::ConstLanelets current_lanes = utils::getCurrentLanes(planner_data_); const lanelet::ConstLanelets current_lanes = utils::getCurrentLanesFromPath(getPreviousModuleOutput().reference_path, planner_data_); - 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); + 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); }); // check that goal is in current neighbor shoulder lane @@ -616,7 +606,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_segment_lanes && !goal_is_in_current_shoulder_lanes) { + if (!goal_is_in_current_lanes && !goal_is_in_current_shoulder_lanes) { return false; } @@ -624,7 +614,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_segment_lanes; + return goal_is_in_current_lanes; } // if goal arc coordinates can be calculated, check if goal is in request_length @@ -646,12 +636,7 @@ bool GoalPlannerModule::isExecutionRequested() const parameters_->forward_goal_search_length); lanelet::ConstLanelet target_lane{}; lanelet::utils::query::getClosestLanelet(pull_over_lanes, goal_pose, &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)) { + if (!isCrossingPossible(current_lane, target_lane)) { return false; } @@ -2272,10 +2257,14 @@ bool GoalPlannerModule::isCrossingPossible( end_lane_sequence = route_handler->getLaneletSequence(end_lane, dist, dist, false); } - const auto getNeighboringLane = + // Lambda function to get the neighboring lanelet based on left_side_parking_ + auto getNeighboringLane = [&](const lanelet::ConstLanelet & lane) -> std::optional { - return left_side_parking_ ? route_handler->getLeftLanelet(lane, false, true) - : route_handler->getRightLanelet(lane, false, true); + 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); }; // Iterate through start_lane_sequence to find a path to end_lane_sequence