From 4eef0870d82431f356edcb5d00f7262d35dc2ff9 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Sun, 4 Jun 2023 01:25:52 +0900 Subject: [PATCH] fix(goal_planner): fix goal planner execution (#3893) * fix(goal_planner): fix goal planner execution Signed-off-by: kosuke55 * fix Signed-off-by: kosuke55 --------- Signed-off-by: kosuke55 --- .../goal_planner/goal_planner_module.cpp | 50 +++++++++++++------ 1 file changed, 36 insertions(+), 14 deletions(-) diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp index b78d212af0469..0060a3cc1b808 100644 --- a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp @@ -276,25 +276,51 @@ bool GoalPlannerModule::isExecutionRequested() const if (current_state_ == ModuleStatus::RUNNING) { return true; } - const auto & route_handler = planner_data_->route_handler; - // if current position is far from goal, do not execute pull over + const auto & route_handler = planner_data_->route_handler; const Pose & current_pose = planner_data_->self_odometry->pose.pose; const Pose & goal_pose = route_handler->getGoalPose(); + + // if goal is shoulder lane, allow goal modification + allow_goal_modification_ = + route_handler->isAllowedGoalModification() || checkOriginalGoalIsInShoulder(); + + // check if goal_pose is in current_lanes. lanelet::ConstLanelet current_lane{}; const lanelet::ConstLanelets current_lanes = utils::getCurrentLanes(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); + }); + + // check that goal is in current neghibor shoulder lane + const bool goal_is_in_current_shoulder_lanes = std::invoke([&]() { + lanelet::ConstLanelet neighbor_shoulder_lane{}; + for (const auto & lane : current_lanes) { + const bool has_shoulder_lane = + left_side_parking_ ? route_handler->getLeftShoulderLanelet(lane, &neighbor_shoulder_lane) + : route_handler->getRightShoulderLanelet(lane, &neighbor_shoulder_lane); + if (has_shoulder_lane && lanelet::utils::isInLanelet(goal_pose, neighbor_shoulder_lane)) { + return true; + } + } + return false; + }); + + // 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) { + return false; + } + + // if goal arc coordinates can be calculated, check if goal is in request_length const double self_to_goal_arc_length = utils::getSignedDistance(current_pose, goal_pose, current_lanes); - allow_goal_modification_ = - route_handler->isAllowedGoalModification() || checkOriginalGoalIsInShoulder(); const double request_length = allow_goal_modification_ ? calcModuleRequestLength() : parameters_->minimum_request_length; - const double backward_goal_search_length = - allow_goal_modification_ ? parameters_->backward_goal_search_length : 0.0; - if ( - self_to_goal_arc_length < -backward_goal_search_length || - self_to_goal_arc_length > request_length) { + if (self_to_goal_arc_length < 0.0 || self_to_goal_arc_length > request_length) { + // if current position is far from goal or behind goal, do not execute goal_planner return false; } @@ -302,11 +328,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 (!allow_goal_modification_) { - // check if goal_pose is in current_lanes. - return std::any_of( - current_lanes.begin(), current_lanes.end(), [&](const lanelet::ConstLanelet & current_lane) { - return lanelet::utils::isInLanelet(goal_pose, current_lane); - }); + return goal_is_in_current_lanes; } // if (A) or (B) is met execute pull over