diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp index a30d7c379f80c..2d661cf41855f 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp @@ -297,7 +297,6 @@ class StartPlannerModule : public SceneModuleInterface const std::vector & ego_predicted_path) const; bool isSafePath() const; void setDrivableAreaInfo(BehaviorModuleOutput & output) const; - lanelet::ConstLanelets createDepartureCheckLanes() const; // check if the goal is located behind the ego in the same route segment. bool isGoalBehindOfEgoInSameRouteSegment() const; diff --git a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp index 3482618d9319c..0e6e5fae84fb8 100644 --- a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp +++ b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp @@ -508,47 +508,45 @@ BehaviorModuleOutput StartPlannerModule::plan() return output; } - PathWithLaneId path; + const auto path = std::invoke([&]() { + if (!status_.driving_forward && !status_.backward_driving_complete) { + return status_.backward_path; + } - // Check if backward motion is finished - if (status_.driving_forward || status_.backward_driving_complete) { // Increment path index if the current path is finished if (hasFinishedCurrentPath()) { RCLCPP_INFO(getLogger(), "Increment path index"); incrementPathIndex(); } - if (!status_.is_safe_dynamic_objects && !isWaitingApproval() && !status_.stop_pose) { + if (isWaitingApproval()) return getCurrentPath(); + + if (status_.stop_pose) { + // Delete stop point if conditions are met + if (status_.is_safe_dynamic_objects && isStopped()) { + status_.stop_pose = std::nullopt; + } + stop_pose_ = status_.stop_pose; + return *status_.prev_stop_path_after_approval; + } + + if (!status_.is_safe_dynamic_objects) { auto current_path = getCurrentPath(); const auto stop_path = behavior_path_planner::utils::parking_departure::generateFeasibleStopPath( current_path, planner_data_, stop_pose_, parameters_->maximum_deceleration_for_stop, parameters_->maximum_jerk_for_stop); + if (!stop_path.has_value()) return current_path; // Insert stop point in the path if needed - if (stop_path) { - RCLCPP_ERROR_THROTTLE( - getLogger(), *clock_, 5000, "Insert stop point in the path because of dynamic objects"); - path = *stop_path; - status_.prev_stop_path_after_approval = std::make_shared(path); - status_.stop_pose = stop_pose_; - } else { - path = current_path; - } - } else if (!isWaitingApproval() && status_.stop_pose) { - // Delete stop point if conditions are met - if (status_.is_safe_dynamic_objects && isStopped()) { - status_.stop_pose = std::nullopt; - path = getCurrentPath(); - } - path = *status_.prev_stop_path_after_approval; - stop_pose_ = status_.stop_pose; - } else { - path = getCurrentPath(); + RCLCPP_ERROR_THROTTLE( + getLogger(), *clock_, 5000, "Insert stop point in the path because of dynamic objects"); + status_.prev_stop_path_after_approval = std::make_shared(stop_path.value()); + status_.stop_pose = stop_pose_; + return stop_path.value(); } - } else { - path = status_.backward_path; - } + return getCurrentPath(); + }); output.path = path; output.reference_path = getPreviousModuleOutput().reference_path; @@ -768,8 +766,9 @@ bool StartPlannerModule::findPullOutPath( const Pose & refined_start_pose, const Pose & goal_pose, const double collision_check_margin) { // if start_pose_candidate is far from refined_start_pose, backward driving is necessary + constexpr double epsilon = 0.01; const bool backward_is_unnecessary = - tier4_autoware_utils::calcDistance2d(start_pose_candidate, refined_start_pose) < 0.01; + tier4_autoware_utils::calcDistance2d(start_pose_candidate, refined_start_pose) < epsilon; planner->setCollisionCheckMargin(collision_check_margin); planner->setPlannerData(planner_data_); @@ -856,26 +855,24 @@ lanelet::ConstLanelets StartPlannerModule::getPathRoadLanes(const PathWithLaneId const auto & lanelet_layer = route_handler->getLaneletMapPtr()->laneletLayer; std::vector lane_ids; + lanelet::ConstLanelets path_lanes; + for (const auto & p : path.points) { for (const auto & id : p.lane_ids) { if (id == lanelet::InvalId) { continue; } - if (route_handler->isShoulderLanelet(lanelet_layer.get(id))) { + const auto lanelet = lanelet_layer.get(id); + if (route_handler->isShoulderLanelet(lanelet)) { continue; } if (std::find(lane_ids.begin(), lane_ids.end(), id) == lane_ids.end()) { lane_ids.push_back(id); + path_lanes.push_back(lanelet); } } } - lanelet::ConstLanelets path_lanes; - path_lanes.reserve(lane_ids.size()); - for (const auto & id : lane_ids) { - path_lanes.push_back(lanelet_layer.get(id)); - } - return path_lanes; } @@ -954,11 +951,12 @@ void StartPlannerModule::updatePullOutStatus() if (hasFinishedBackwardDriving()) { updateStatusAfterBackwardDriving(); - } else { - status_.backward_path = start_planner_utils::getBackwardPath( - *route_handler, pull_out_lanes, current_pose, status_.pull_out_start_pose, - parameters_->backward_velocity); + return; } + status_.backward_path = start_planner_utils::getBackwardPath( + *route_handler, pull_out_lanes, current_pose, status_.pull_out_start_pose, + parameters_->backward_velocity); + return; } void StartPlannerModule::updateStatusAfterBackwardDriving() @@ -1047,7 +1045,7 @@ std::vector StartPlannerModule::searchPullOutStartPoseCandidates( if (distance_from_lane_end < parameters_->ignore_distance_from_lane_end) { RCLCPP_WARN_THROTTLE( getLogger(), *clock_, 5000, - "the ego is too close to the lane end, so needs backward driving"); + "the ego vehicle is too close to the lane end, so backwards driving is necessary"); continue; } @@ -1408,30 +1406,6 @@ void StartPlannerModule::setDrivableAreaInfo(BehaviorModuleOutput & output) cons } } -lanelet::ConstLanelets StartPlannerModule::createDepartureCheckLanes() const -{ - const double backward_path_length = - planner_data_->parameters.backward_path_length + parameters_->max_back_distance; - const auto pull_out_lanes = - start_planner_utils::getPullOutLanes(planner_data_, backward_path_length); - if (pull_out_lanes.empty()) { - return lanelet::ConstLanelets{}; - } - const auto road_lanes = utils::getExtendedCurrentLanes( - planner_data_, backward_path_length, std::numeric_limits::max(), - /*forward_only_in_route*/ true); - // extract shoulder lanes from pull out lanes - lanelet::ConstLanelets shoulder_lanes; - std::copy_if( - pull_out_lanes.begin(), pull_out_lanes.end(), std::back_inserter(shoulder_lanes), - [this](const auto & pull_out_lane) { - return planner_data_->route_handler->isShoulderLanelet(pull_out_lane); - }); - const auto departure_check_lanes = utils::transformToLanelets( - utils::generateDrivableLanesWithShoulderLanes(road_lanes, shoulder_lanes)); - return departure_check_lanes; -} - void StartPlannerModule::setDebugData() { using lanelet::visualization::laneletsAsTriangleMarkerArray;