From 418ad8a46405824815e5538839fbc36b9cd86407 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Mon, 12 Sep 2022 22:08:50 +0900 Subject: [PATCH] refactor(behavior_path_planner): refactor pull_over (#1842) Signed-off-by: kosuke55 Signed-off-by: kosuke55 --- .../pull_over/pull_over_module.hpp | 6 +- .../scene_module/pull_over/util.hpp | 26 +- .../behavior_path_planner/utilities.hpp | 2 - .../src/behavior_path_planner_node.cpp | 12 +- .../pull_over/pull_over_module.cpp | 171 +++++-------- .../src/scene_module/pull_over/util.cpp | 233 ++---------------- .../include/route_handler/route_handler.hpp | 9 +- planning/route_handler/src/route_handler.cpp | 75 +----- 8 files changed, 93 insertions(+), 441 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_module.hpp index 845cb91f0358d..9616777a4d015 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_module.hpp @@ -189,9 +189,7 @@ class PullOverModule : public SceneModuleInterface PathWithLaneId getReferencePath() const; PathWithLaneId generateStopPath() const; - lanelet::ConstLanelets getPullOverLanes() const; - std::pair getSafePath(ShiftParkingPath & safe_path) const; - Pose getRefinedGoal() const; + Pose calcRefinedGoal() const; Pose getParkingStartPose() const; ParallelParkingParameters getGeometricPullOverParameters() const; bool isLongEnoughToParkingStart( @@ -202,7 +200,7 @@ class PullOverModule : public SceneModuleInterface double calcMinimumShiftPathDistance() const; std::pair calcDistanceToPathChange() const; - bool planShiftPath(); + bool planShiftPath(const Pose goal_pose); bool isStopped(); bool hasFinishedCurrentPath(); bool hasFinishedPullOver(); diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/util.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/util.hpp index 0cc6eb6ebdb9e..43ac1f9dc9536 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/util.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/util.hpp @@ -43,37 +43,25 @@ using geometry_msgs::msg::Twist; // TODO(sugahara) move to util PathWithLaneId combineReferencePath(const PathWithLaneId path1, const PathWithLaneId path2); -bool isPathInLanelets( - const PathWithLaneId & path, const lanelet::ConstLanelets & original_lanelets, - const lanelet::ConstLanelets & target_lanelets); -std::vector getShiftParkingPaths( + +std::vector generateShiftParkingPaths( const RouteHandler & route_handler, const lanelet::ConstLanelets & original_lanelets, const lanelet::ConstLanelets & target_lanelets, const Pose & pose, const Pose & goal_pose, - const Twist & twist, const BehaviorPathPlannerParameters & common_parameter, + const BehaviorPathPlannerParameters & common_parameter, const behavior_path_planner::PullOverParameters & parameter); std::vector selectValidPaths( const std::vector & paths, const lanelet::ConstLanelets & current_lanes, - const lanelet::ConstLanelets & target_lanes, - const lanelet::routing::RoutingGraphContainer & overall_graphs, const Pose & current_pose, - const bool isInGoalRouteSection, const Pose & goal_pose, + const lanelet::ConstLanelets & target_lanes, const Pose & current_pose, + const bool is_in_goal_route_section, const Pose & goal_pose, const lane_departure_checker::LaneDepartureChecker & lane_departure_checker); bool selectSafePath( const std::vector & paths, const OccupancyGridBasedCollisionDetector & occupancy_grid_map, ShiftParkingPath & selected_path); -bool isPullOverPathSafe( - const PathWithLaneId & path, const lanelet::ConstLanelets & current_lanes, - const lanelet::ConstLanelets & target_lanes, - const PredictedObjects::ConstSharedPtr dynamic_objects, const Pose & current_pose, - const size_t current_seg_idx, const Twist & current_twist, const double vehicle_width, - const behavior_path_planner::PullOverParameters & ros_parameters, const bool use_buffer = true, - const double acceleration = 0.0); bool hasEnoughDistance( const ShiftParkingPath & path, const lanelet::ConstLanelets & current_lanes, - const lanelet::ConstLanelets & target_lanes, const Pose & current_pose, - const bool isInGoalRouteSection, const Pose & goal_pose, - const lanelet::routing::RoutingGraphContainer & overall_graphs); -bool isObjectFront(const Pose & ego_pose, const Pose & obj_pose); + const Pose & current_pose, const bool is_in_goal_route_section, const Pose & goal_pose); +lanelet::ConstLanelets getPullOverLanes(const RouteHandler & route_handler); } // namespace pull_over_utils } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp index 5bb865c898d59..0f02e75f57cb4 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp @@ -97,8 +97,6 @@ void getProjectedDistancePointFromPolygons( Pose & point_on_object); // data conversions -Path convertToPathFromPathWithLaneId(const PathWithLaneId & path_with_lane_id); - std::vector convertToPoseArray(const PathWithLaneId & path); std::vector convertToGeometryPointArray(const PathWithLaneId & path); diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index 24ff07a18096c..3be04884b6243 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -779,20 +779,12 @@ PathWithLaneId BehaviorPathPlannerNode::modifyPathForSmoothGoalConnection( const PathWithLaneId & path) const { const auto goal = planner_data_->route_handler->getGoalPose(); - const auto is_approved = planner_data_->approval.is_approved.data; - auto goal_lane_id = planner_data_->route_handler->getGoalLaneId(); + const auto goal_lane_id = planner_data_->route_handler->getGoalLaneId(); Pose refined_goal{}; { lanelet::ConstLanelet goal_lanelet; - lanelet::ConstLanelet pull_over_lane; - geometry_msgs::msg::Pose pull_over_goal; - if ( - is_approved && planner_data_->route_handler->getPullOverTarget( - planner_data_->route_handler->getShoulderLanelets(), &pull_over_lane)) { - refined_goal = planner_data_->route_handler->getPullOverGoalPose(); - goal_lane_id = pull_over_lane.id(); - } else if (planner_data_->route_handler->getGoalLanelet(&goal_lanelet)) { + if (planner_data_->route_handler->getGoalLanelet(&goal_lanelet)) { refined_goal = util::refineGoal(goal, goal_lanelet); } else { refined_goal = goal; diff --git a/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp b/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp index b43dc54de37a2..a2181c595761a 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp @@ -155,7 +155,7 @@ void PullOverModule::onEntry() if (!parameters_.enable_goal_research) { goal_candidates_.clear(); GoalCandidate goal_candidate; - goal_candidate.goal_pose = getRefinedGoal(); + goal_candidate.goal_pose = calcRefinedGoal(); goal_candidate.distance_from_original_goal = 0.0; goal_candidates_.push_back(goal_candidate); } @@ -230,7 +230,7 @@ bool PullOverModule::isExecutionRequested() const bool PullOverModule::isExecutionReady() const { return true; } -Pose PullOverModule::getRefinedGoal() const +Pose PullOverModule::calcRefinedGoal() const { lanelet::ConstLanelet goal_lane; Pose goal_pose = planner_data_->route_handler->getGoalPose(); @@ -256,7 +256,7 @@ void PullOverModule::researchGoal() { // Find goals in pull over areas. goal_candidates_.clear(); - const Pose refined_goal_pose = getRefinedGoal(); + const Pose refined_goal_pose = calcRefinedGoal(); for (double dx = -parameters_.backward_goal_search_length; dx <= parameters_.forward_goal_search_length; dx += parameters_.goal_search_interval) { const Pose search_pose = calcOffsetPose(refined_goal_pose, dx, 0, 0); @@ -406,11 +406,12 @@ bool PullOverModule::planWithEfficientPath() // shift parking path if (parameters_.enable_shift_parking) { for (const auto goal_candidate : goal_candidates_) { - modified_goal_pose_ = goal_candidate.goal_pose; if ( - planShiftPath() && isLongEnoughToParkingStart( - shift_parking_path_.path, shift_parking_path_.shift_point.start)) { + planShiftPath(goal_candidate.goal_pose) && + isLongEnoughToParkingStart( + shift_parking_path_.path, shift_parking_path_.shift_point.start)) { // shift parking plan already confirms safety and no lane departure in it's own function. + modified_goal_pose_ = goal_candidate.goal_pose; status_.path = shift_parking_path_.path; status_.path_type = PathType::SHIFT; status_.is_safe = true; @@ -422,17 +423,17 @@ bool PullOverModule::planWithEfficientPath() // forward arc path if (parameters_.enable_arc_forward_parking) { for (const auto goal_candidate : goal_candidates_) { - modified_goal_pose_ = goal_candidate.goal_pose; parallel_parking_planner_.setData(planner_data_, parallel_parking_parameters_); if ( parallel_parking_planner_.planPullOver( - modified_goal_pose_, status_.current_lanes, status_.pull_over_lanes, true) && + goal_candidate.goal_pose, status_.current_lanes, status_.pull_over_lanes, true) && isLongEnoughToParkingStart( parallel_parking_planner_.getCurrentPath(), parallel_parking_planner_.getStartPose().pose) && !checkCollisionWithPath(parallel_parking_planner_.getArcPath()) && !lane_departure_checker_->checkPathWillLeaveLane( status_.lanes, parallel_parking_planner_.getArcPath())) { + modified_goal_pose_ = goal_candidate.goal_pose; status_.path = parallel_parking_planner_.getCurrentPath(); status_.path_type = PathType::ARC_FORWARD; status_.is_safe = true; @@ -444,17 +445,17 @@ bool PullOverModule::planWithEfficientPath() // backward arc path if (parameters_.enable_arc_backward_parking) { for (const auto goal_candidate : goal_candidates_) { - modified_goal_pose_ = goal_candidate.goal_pose; parallel_parking_planner_.setData(planner_data_, parallel_parking_parameters_); if ( parallel_parking_planner_.planPullOver( - modified_goal_pose_, status_.current_lanes, status_.pull_over_lanes, false) && + goal_candidate.goal_pose, status_.current_lanes, status_.pull_over_lanes, false) && isLongEnoughToParkingStart( parallel_parking_planner_.getCurrentPath(), parallel_parking_planner_.getStartPose().pose) && !checkCollisionWithPath(parallel_parking_planner_.getArcPath()) && !lane_departure_checker_->checkPathWillLeaveLane( status_.lanes, parallel_parking_planner_.getArcPath())) { + modified_goal_pose_ = goal_candidate.goal_pose; status_.path = parallel_parking_planner_.getCurrentPath(); status_.path_type = PathType::ARC_BACKWARD; status_.is_safe = true; @@ -474,9 +475,10 @@ bool PullOverModule::planWithCloseGoal() // Generate arc shift path. if ( - parameters_.enable_shift_parking && planShiftPath() && + parameters_.enable_shift_parking && planShiftPath(goal_candidate.goal_pose) && isLongEnoughToParkingStart(shift_parking_path_.path, shift_parking_path_.shift_point.start)) { // shift parking plan already confirms safety and no lane departure in it's own function. + modified_goal_pose_ = goal_candidate.goal_pose; status_.path = shift_parking_path_.path; status_.path_type = PathType::SHIFT; status_.is_safe = true; @@ -488,13 +490,14 @@ bool PullOverModule::planWithCloseGoal() if ( parameters_.enable_arc_forward_parking && parallel_parking_planner_.planPullOver( - modified_goal_pose_, status_.current_lanes, status_.pull_over_lanes, true) && + goal_candidate.goal_pose, status_.current_lanes, status_.pull_over_lanes, true) && isLongEnoughToParkingStart( parallel_parking_planner_.getCurrentPath(), parallel_parking_planner_.getStartPose().pose) && !checkCollisionWithPath(parallel_parking_planner_.getArcPath()) && !lane_departure_checker_->checkPathWillLeaveLane( status_.lanes, parallel_parking_planner_.getArcPath())) { + modified_goal_pose_ = goal_candidate.goal_pose; status_.path = parallel_parking_planner_.getCurrentPath(); status_.path_type = PathType::ARC_FORWARD; status_.is_safe = true; @@ -505,13 +508,14 @@ bool PullOverModule::planWithCloseGoal() if ( parameters_.enable_arc_backward_parking && parallel_parking_planner_.planPullOver( - modified_goal_pose_, status_.current_lanes, status_.pull_over_lanes, false) && + goal_candidate.goal_pose, status_.current_lanes, status_.pull_over_lanes, false) && isLongEnoughToParkingStart( parallel_parking_planner_.getCurrentPath(), parallel_parking_planner_.getStartPose().pose) && !checkCollisionWithPath(parallel_parking_planner_.getArcPath()) && !lane_departure_checker_->checkPathWillLeaveLane( status_.lanes, parallel_parking_planner_.getArcPath())) { + modified_goal_pose_ = goal_candidate.goal_pose; status_.path = parallel_parking_planner_.getCurrentPath(); status_.path_type = PathType::ARC_BACKWARD; status_.is_safe = true; @@ -542,7 +546,7 @@ Pose PullOverModule::getParkingStartPose() const BehaviorModuleOutput PullOverModule::plan() { status_.current_lanes = util::getExtendedCurrentLanes(planner_data_); - status_.pull_over_lanes = getPullOverLanes(); + status_.pull_over_lanes = pull_over_utils::getPullOverLanes(*(planner_data_->route_handler)); status_.lanes = lanelet::ConstLanelets{}; status_.lanes.insert( status_.lanes.end(), status_.current_lanes.begin(), status_.current_lanes.end()); @@ -721,39 +725,13 @@ void PullOverModule::setParameters(const PullOverParameters & parameters) parameters_ = parameters; } -bool PullOverModule::planShiftPath() -{ - const auto & route_handler = planner_data_->route_handler; - const auto common_parameters = planner_data_->parameters; - - lanelet::ConstLanelet target_shoulder_lane; - - if (route_handler->getPullOverTarget( - route_handler->getShoulderLanelets(), &target_shoulder_lane)) { - route_handler->setPullOverGoalPose( - target_shoulder_lane, common_parameters.vehicle_width, parameters_.margin_from_boundary); - } else { - RCLCPP_ERROR_THROTTLE(getLogger(), *clock_, 5000, "failed to get shoulder lane!!!"); - } - - // Find pull_over path - bool found_valid_path, found_safe_path; - std::tie(found_valid_path, found_safe_path) = getSafePath(shift_parking_path_); - if (!found_safe_path) { - return found_safe_path; - } - - shift_parking_path_.path.header = planner_data_->route_handler->getRouteHeader(); - return found_safe_path; -} - PathWithLaneId PullOverModule::getReferencePath() const { const auto & route_handler = planner_data_->route_handler; - const auto current_pose = planner_data_->self_pose->pose; - const auto common_parameters = planner_data_->parameters; + const auto & current_pose = planner_data_->self_pose->pose; + const auto & common_parameters = planner_data_->parameters; - const Pose refined_goal_pose = getRefinedGoal(); + const Pose refined_goal_pose = calcRefinedGoal(); if (status_.current_lanes.empty()) { return PathWithLaneId{}; } @@ -803,8 +781,8 @@ PathWithLaneId PullOverModule::getReferencePath() const PathWithLaneId PullOverModule::generateStopPath() const { const auto & route_handler = planner_data_->route_handler; - const auto current_pose = planner_data_->self_pose->pose; - const auto common_parameters = planner_data_->parameters; + const auto & current_pose = planner_data_->self_pose->pose; + const auto & common_parameters = planner_data_->parameters; const double current_vel = planner_data_->self_odometry->twist.twist.linear.x; auto stop_path = util::getCenterLinePath( @@ -839,86 +817,49 @@ PathWithLaneId PullOverModule::generateStopPath() const return stop_path; } -lanelet::ConstLanelets PullOverModule::getPullOverLanes() const +bool PullOverModule::planShiftPath(const Pose goal_pose) { - lanelet::ConstLanelets pull_over_lanes; + std::vector valid_paths; + const auto & route_handler = planner_data_->route_handler; - const auto current_pose = planner_data_->self_pose->pose; - lanelet::ConstLanelet target_shoulder_lane; + const auto & current_pose = planner_data_->self_pose->pose; + const auto & common_parameters = planner_data_->parameters; - if (status_.current_lanes.empty()) { - return pull_over_lanes; + if (!isLongEnough(status_.current_lanes, goal_pose) || status_.pull_over_lanes.empty()) { + return false; } - // Get shoulder lanes - lanelet::ConstLanelet current_lane; - lanelet::utils::query::getClosestLanelet( - status_.current_lanes, planner_data_->self_pose->pose, ¤t_lane); - - if (route_handler->getPullOverTarget( - route_handler->getShoulderLanelets(), &target_shoulder_lane)) { - pull_over_lanes = route_handler->getShoulderLaneletSequence( - target_shoulder_lane, current_pose, pull_over_lane_length_, pull_over_lane_length_); - - } else { - pull_over_lanes.clear(); + // find candidate paths + const auto pull_over_paths = pull_over_utils::generateShiftParkingPaths( + *route_handler, status_.current_lanes, status_.pull_over_lanes, current_pose, goal_pose, + common_parameters, parameters_); + if (pull_over_paths.empty()) { + return false; } - return pull_over_lanes; -} - -std::pair PullOverModule::getSafePath(ShiftParkingPath & safe_path) const -{ - std::vector valid_paths; - - const auto & route_handler = planner_data_->route_handler; - const auto current_pose = planner_data_->self_pose->pose; - const auto current_twist = planner_data_->self_odometry->twist.twist; - const auto common_parameters = planner_data_->parameters; - - if (!isLongEnough(status_.current_lanes, modified_goal_pose_)) { - return std::make_pair(false, false); + // select valid path + valid_paths = pull_over_utils::selectValidPaths( + pull_over_paths, status_.current_lanes, status_.pull_over_lanes, current_pose, + route_handler->isInGoalRouteSection(status_.current_lanes.back()), goal_pose, + *lane_departure_checker_); + if (valid_paths.empty()) { + return false; } - if (!status_.pull_over_lanes.empty()) { - // find candidate paths - const auto pull_over_paths = pull_over_utils::getShiftParkingPaths( - *route_handler, status_.current_lanes, status_.pull_over_lanes, current_pose, - modified_goal_pose_, current_twist, common_parameters, parameters_); - - // get lanes used for detection - lanelet::ConstLanelets check_lanes; - if (!pull_over_paths.empty()) { - const auto & longest_path = pull_over_paths.front(); - // we want to see check_distance [m] behind vehicle so add lane changing length - const double check_distance_with_path = - check_distance_ + longest_path.preparation_length + longest_path.pull_over_length; - check_lanes = route_handler->getCheckTargetLanesFromPath( - longest_path.path, status_.pull_over_lanes, check_distance_with_path); - } - - // select valid path - valid_paths = pull_over_utils::selectValidPaths( - pull_over_paths, status_.current_lanes, check_lanes, *route_handler->getOverallGraphPtr(), - current_pose, route_handler->isInGoalRouteSection(status_.current_lanes.back()), - modified_goal_pose_, *lane_departure_checker_); - if (valid_paths.empty()) { - return std::make_pair(false, false); - } - // select safe path - bool found_safe_path = false; - for (const auto & path : valid_paths) { - if (!checkCollisionWithPath(path.shifted_path.path)) { - safe_path = path; - found_safe_path = true; - break; - } + // select safe path + bool found_safe_path = false; + for (const auto & path : valid_paths) { + if (!checkCollisionWithPath(path.shifted_path.path)) { + shift_parking_path_ = path; + found_safe_path = true; + break; } - - safe_path.is_safe = found_safe_path; - return std::make_pair(true, found_safe_path); } - return std::make_pair(false, false); + + shift_parking_path_.is_safe = found_safe_path; + shift_parking_path_.path.header = route_handler->getRouteHeader(); + + return found_safe_path; } double PullOverModule::calcMinimumShiftPathDistance() const @@ -1114,7 +1055,7 @@ void PullOverModule::publishDebugData() // Visualize pull over areas if (parameters_.enable_goal_research) { - const Pose refined_goal_pose = getRefinedGoal(); + const Pose refined_goal_pose = calcRefinedGoal(); const Pose start_pose = calcOffsetPose(refined_goal_pose, -parameters_.backward_goal_search_length, 0, 0); const Pose end_pose = diff --git a/planning/behavior_path_planner/src/scene_module/pull_over/util.cpp b/planning/behavior_path_planner/src/scene_module/pull_over/util.cpp index 38cd746ee355e..067cc03f91242 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_over/util.cpp +++ b/planning/behavior_path_planner/src/scene_module/pull_over/util.cpp @@ -52,34 +52,10 @@ PathWithLaneId combineReferencePath(const PathWithLaneId path1, const PathWithLa return path; } -bool isPathInLanelets( - const PathWithLaneId & path, const lanelet::ConstLanelets & original_lanelets, - const lanelet::ConstLanelets & target_lanelets) -{ - for (const auto & pt : path.points) { - bool is_in_lanelet = false; - for (const auto & llt : original_lanelets) { - if (lanelet::utils::isInLanelet(pt.point.pose, llt, 0.1)) { - is_in_lanelet = true; - } - } - for (const auto & llt : target_lanelets) { - if (lanelet::utils::isInLanelet(pt.point.pose, llt, 0.1)) { - is_in_lanelet = true; - } - } - if (!is_in_lanelet) { - return false; - } - } - return true; -} - -std::vector getShiftParkingPaths( +std::vector generateShiftParkingPaths( const RouteHandler & route_handler, const lanelet::ConstLanelets & original_lanelets, const lanelet::ConstLanelets & target_lanelets, const Pose & current_pose, const Pose & goal_pose, - [[maybe_unused]] const Twist & twist, const BehaviorPathPlannerParameters & common_parameter, - const PullOverParameters & parameter) + const BehaviorPathPlannerParameters & common_parameter, const PullOverParameters & parameter) { std::vector candidate_paths; @@ -287,9 +263,8 @@ std::vector getShiftParkingPaths( std::vector selectValidPaths( const std::vector & paths, const lanelet::ConstLanelets & current_lanes, - const lanelet::ConstLanelets & target_lanes, - const lanelet::routing::RoutingGraphContainer & overall_graphs, const Pose & current_pose, - const bool isInGoalRouteSection, const Pose & goal_pose, + const lanelet::ConstLanelets & target_lanes, const Pose & current_pose, + const bool is_in_goal_route_section, const Pose & goal_pose, const lane_departure_checker::LaneDepartureChecker & lane_departure_checker) { // combine road and shoulder lanes @@ -299,8 +274,7 @@ std::vector selectValidPaths( std::vector available_paths; for (const auto & path : paths) { if (!hasEnoughDistance( - path, current_lanes, target_lanes, current_pose, isInGoalRouteSection, goal_pose, - overall_graphs)) { + path, current_lanes, current_pose, is_in_goal_route_section, goal_pose)) { continue; } @@ -336,9 +310,7 @@ bool selectSafePath( bool hasEnoughDistance( const ShiftParkingPath & path, const lanelet::ConstLanelets & current_lanes, - [[maybe_unused]] const lanelet::ConstLanelets & target_lanes, const Pose & current_pose, - const bool isInGoalRouteSection, const Pose & goal_pose, - [[maybe_unused]] const lanelet::routing::RoutingGraphContainer & overall_graphs) + const Pose & current_pose, const bool is_in_goal_route_section, const Pose & goal_pose) { const double pull_over_prepare_distance = path.preparation_length; const double pull_over_distance = path.pull_over_length; @@ -348,197 +320,28 @@ bool hasEnoughDistance( return false; } - // if (pull_over_total_distance > - // util::getDistanceToNextIntersection(current_pose, current_lanes)) { - // return false; - // } - if ( - isInGoalRouteSection && + is_in_goal_route_section && pull_over_total_distance > util::getSignedDistance(current_pose, goal_pose, current_lanes)) { return false; } - // if ( - // pullover_total_distance > - // util::getDistanceToCrosswalk(current_pose, current_lanes, overall_graphs)) { - // return false; - // } return true; } -// Use occupancy grid to check safety instead of this function. -bool isPullOverPathSafe( - const PathWithLaneId & path, const lanelet::ConstLanelets & current_lanes, - const lanelet::ConstLanelets & target_lanes, - const PredictedObjects::ConstSharedPtr dynamic_objects, const Pose & current_pose, - const size_t current_seg_idx, const Twist & current_twist, const double vehicle_width, - const PullOverParameters & ros_parameters, const bool use_buffer, const double acceleration) +lanelet::ConstLanelets getPullOverLanes(const RouteHandler & route_handler) { - if (path.points.empty()) { - return false; - } - if (target_lanes.empty() || current_lanes.empty()) { - return false; - } - if (dynamic_objects == nullptr) { - return true; - } - const auto arc = lanelet::utils::getArcCoordinates(current_lanes, current_pose); - constexpr double check_distance = 100.0; - - // parameters - const double time_resolution = ros_parameters.prediction_time_resolution; - const double min_thresh = ros_parameters.min_stop_distance; - const double stop_time = ros_parameters.stop_time; - - double buffer; - double lateral_buffer; - if (use_buffer) { - buffer = ros_parameters.hysteresis_buffer_distance; - lateral_buffer = 0.5; - } else { - buffer = 0.0; - lateral_buffer = 0.0; - } - double current_lane_check_start_time = 0.0; - const double current_lane_check_end_time = - ros_parameters.pull_over_prepare_duration + ros_parameters.pull_over_duration; - double target_lane_check_start_time = 0.0; - const double target_lane_check_end_time = - ros_parameters.pull_over_prepare_duration + ros_parameters.pull_over_duration; - if (!ros_parameters.enable_collision_check_at_prepare_phase) { - current_lane_check_start_time = ros_parameters.pull_over_prepare_duration; - target_lane_check_start_time = ros_parameters.pull_over_prepare_duration; - } - - // find obstacle in pull_over target lanes - // retrieve lanes that are merging target lanes as well - const auto target_lane_object_indices = - util::filterObjectIndicesByLanelets(*dynamic_objects, target_lanes); - - // find objects in current lane - const auto current_lane_object_indices_lanelet = util::filterObjectIndicesByLanelets( - *dynamic_objects, current_lanes, arc.length, arc.length + check_distance); - const auto current_lane_object_indices = util::filterObjectsIndicesByPath( - *dynamic_objects, current_lane_object_indices_lanelet, path, - vehicle_width / 2 + lateral_buffer); - - const auto & vehicle_predicted_path = util::convertToPredictedPath( - path, current_twist, current_pose, current_seg_idx, target_lane_check_end_time, time_resolution, - acceleration); - - // Collision check for objects in current lane - for (const auto & i : current_lane_object_indices) { - const auto & obj = dynamic_objects->objects.at(i); - std::vector predicted_paths; - if (ros_parameters.use_all_predicted_path) { - predicted_paths.resize(obj.kinematics.predicted_paths.size()); - std::copy( - obj.kinematics.predicted_paths.begin(), obj.kinematics.predicted_paths.end(), - predicted_paths.begin()); - } else { - auto & max_confidence_path = *(std::max_element( - obj.kinematics.predicted_paths.begin(), obj.kinematics.predicted_paths.end(), - [](const auto & path1, const auto & path2) { - return path1.confidence > path2.confidence; - })); - predicted_paths.push_back(max_confidence_path); - } - for (const auto & obj_path : predicted_paths) { - double distance = util::getDistanceBetweenPredictedPaths( - obj_path, vehicle_predicted_path, current_lane_check_start_time, - current_lane_check_end_time, time_resolution); - double thresh; - if (isObjectFront(current_pose, obj.kinematics.initial_pose_with_covariance.pose)) { - thresh = util::l2Norm(current_twist.linear) * stop_time; - } else { - thresh = - util::l2Norm(obj.kinematics.initial_twist_with_covariance.twist.linear) * stop_time; - } - thresh = std::max(thresh, min_thresh); - thresh += buffer; - if (distance < thresh) { - return false; - } - } + lanelet::ConstLanelets pull_over_lanes; + lanelet::ConstLanelet target_shoulder_lane; + const Pose goal_pose = route_handler.getGoalPose(); + + if (route_handler.getPullOverTarget( + route_handler.getShoulderLanelets(), goal_pose, &target_shoulder_lane)) { + constexpr double pull_over_lane_length = 200; + pull_over_lanes = route_handler.getShoulderLaneletSequence( + target_shoulder_lane, goal_pose, pull_over_lane_length, pull_over_lane_length); } - - // Collision check for objects in pull over target lane - for (const auto & i : target_lane_object_indices) { - const auto & obj = dynamic_objects->objects.at(i); - std::vector predicted_paths; - if (ros_parameters.use_all_predicted_path) { - predicted_paths.resize(obj.kinematics.predicted_paths.size()); - std::copy( - obj.kinematics.predicted_paths.begin(), obj.kinematics.predicted_paths.end(), - predicted_paths.begin()); - } else { - auto & max_confidence_path = *(std::max_element( - obj.kinematics.predicted_paths.begin(), obj.kinematics.predicted_paths.end(), - [](const auto & path1, const auto & path2) { - return path1.confidence > path2.confidence; - })); - predicted_paths.push_back(max_confidence_path); - } - - bool is_object_in_target = false; - if (ros_parameters.use_predicted_path_outside_lanelet) { - is_object_in_target = true; - } else { - for (const auto & llt : target_lanes) { - if (lanelet::utils::isInLanelet(obj.kinematics.initial_pose_with_covariance.pose, llt)) { - is_object_in_target = true; - } - } - } - - if (is_object_in_target) { - for (const auto & obj_path : predicted_paths) { - const double distance = util::getDistanceBetweenPredictedPaths( - obj_path, vehicle_predicted_path, target_lane_check_start_time, - target_lane_check_end_time, time_resolution); - double thresh; - if (isObjectFront(current_pose, obj.kinematics.initial_pose_with_covariance.pose)) { - thresh = util::l2Norm(current_twist.linear) * stop_time; - } else { - thresh = - util::l2Norm(obj.kinematics.initial_twist_with_covariance.twist.linear) * stop_time; - } - thresh = std::max(thresh, min_thresh); - thresh += buffer; - if (distance < thresh) { - return false; - } - } - } else { - const double distance = util::getDistanceBetweenPredictedPathAndObject( - obj, vehicle_predicted_path, target_lane_check_start_time, target_lane_check_end_time, - time_resolution); - double thresh = min_thresh; - if (isObjectFront(current_pose, obj.kinematics.initial_pose_with_covariance.pose)) { - thresh = std::max(thresh, util::l2Norm(current_twist.linear) * stop_time); - } - thresh += buffer; - if (distance < thresh) { - return false; - } - } - } - - return true; -} - -bool isObjectFront(const Pose & ego_pose, const Pose & obj_pose) -{ - tf2::Transform tf_map2ego, tf_map2obj; - Pose obj_from_ego; - tf2::fromMsg(ego_pose, tf_map2ego); - tf2::fromMsg(obj_pose, tf_map2obj); - tf2::toMsg(tf_map2ego.inverse() * tf_map2obj, obj_from_ego); - - return obj_from_ego.position.x > 0; + return pull_over_lanes; } - } // namespace pull_over_utils } // namespace behavior_path_planner diff --git a/planning/route_handler/include/route_handler/route_handler.hpp b/planning/route_handler/include/route_handler/route_handler.hpp index 92737ca586fc1..b0615d8da94d5 100644 --- a/planning/route_handler/include/route_handler/route_handler.hpp +++ b/planning/route_handler/include/route_handler/route_handler.hpp @@ -66,8 +66,6 @@ class RouteHandler void setMap(const HADMapBin & map_msg); void setRoute(const HADMapRoute & route_msg); void setRouteLanelets(const lanelet::ConstLanelets & path_lanelets); - void setPullOverGoalPose( - const lanelet::ConstLanelet target_lane, const double vehicle_width, const double margin); // const methods @@ -93,7 +91,6 @@ class RouteHandler // for goal bool isInGoalRouteSection(const lanelet::ConstLanelet & lanelet) const; Pose getGoalPose() const; - Pose getPullOverGoalPose() const; lanelet::Id getGoalLaneId() const; bool getGoalLanelet(lanelet::ConstLanelet * goal_lanelet) const; std::vector getLanesBeforePose( @@ -234,7 +231,7 @@ class RouteHandler const double backward_distance = std::numeric_limits::max(), const double forward_distance = std::numeric_limits::max()) const; lanelet::ConstLanelets getShoulderLaneletSequence( - const lanelet::ConstLanelet & lanelet, const Pose & current_pose, + const lanelet::ConstLanelet & lanelet, const Pose & pose, const double backward_distance = std::numeric_limits::max(), const double forward_distance = std::numeric_limits::max()) const; lanelet::ConstLanelets getCheckTargetLanesFromPath( @@ -251,7 +248,8 @@ class RouteHandler bool getLaneChangeTarget( const lanelet::ConstLanelets & lanelets, lanelet::ConstLanelet * target_lanelet) const; bool getPullOverTarget( - const lanelet::ConstLanelets & lanelets, lanelet::ConstLanelet * target_lanelet) const; + const lanelet::ConstLanelets & lanelets, const Pose & goal_pose, + lanelet::ConstLanelet * target_lanelet) const; bool getPullOutStartLane( const lanelet::ConstLanelets & lanelets, const Pose & pose, const double vehicle_width, lanelet::ConstLanelet * target_lanelet) const; @@ -271,7 +269,6 @@ class RouteHandler lanelet::ConstLanelets start_lanelets_; lanelet::ConstLanelets goal_lanelets_; lanelet::ConstLanelets shoulder_lanelets_; - Pose pull_over_goal_pose_; HADMapRoute route_msg_; rclcpp::Logger logger_{rclcpp::get_logger("route_handler")}; diff --git a/planning/route_handler/src/route_handler.cpp b/planning/route_handler/src/route_handler.cpp index 1ed082216d3bf..621365eda0e7c 100644 --- a/planning/route_handler/src/route_handler.cpp +++ b/planning/route_handler/src/route_handler.cpp @@ -86,17 +86,6 @@ lanelet::ConstPoint3d get3DPointFrom2DArcLength( return lanelet::ConstPoint3d(); } -Path convertToPathFromPathWithLaneId(const PathWithLaneId & path_with_lane_id) -{ - Path path; - path.header = path_with_lane_id.header; - path.drivable_area = path_with_lane_id.drivable_area; - for (const auto & pt_with_lane_id : path_with_lane_id.points) { - path.points.push_back(pt_with_lane_id.point); - } - return path; -} - PathWithLaneId removeOverlappingPoints(const PathWithLaneId & input_path) { PathWithLaneId filtered_path; @@ -122,27 +111,6 @@ PathWithLaneId removeOverlappingPoints(const PathWithLaneId & input_path) return filtered_path; } -double getDistanceToShoulderBoundary( - const lanelet::ConstLanelets & shoulder_lanelets, const Pose & pose) -{ - lanelet::ConstLanelet closest_shoulder_lanelet; - lanelet::ArcCoordinates arc_coordinates; - if (lanelet::utils::query::getClosestLanelet( - shoulder_lanelets, pose, &closest_shoulder_lanelet)) { - const auto lanelet_point = lanelet::utils::conversion::toLaneletPoint(pose.position); - const auto & left_line_2d = lanelet::utils::to2D(closest_shoulder_lanelet.leftBound3d()); - arc_coordinates = lanelet::geometry::toArcCoordinates( - left_line_2d, lanelet::utils::to2D(lanelet_point).basicPoint()); - - } else { - RCLCPP_ERROR_STREAM( - rclcpp::get_logger("behavior_path_planner").get_child("utilities"), - "closest shoulder lanelet not found."); - } - - return arc_coordinates.distance; -} - std::string toString(const geometry_msgs::msg::Pose & pose) { std::stringstream ss; @@ -398,40 +366,6 @@ lanelet::ConstLanelets RouteHandler::getRouteLanelets() const { return route_lan Pose RouteHandler::getGoalPose() const { return route_msg_.goal_pose; } -void RouteHandler::setPullOverGoalPose( - const lanelet::ConstLanelet target_lane, const double vehicle_width, const double margin) -{ - const auto arc_position_goal = - lanelet::utils::getArcCoordinates({target_lane}, route_msg_.goal_pose); - Path centerline_path = convertToPathFromPathWithLaneId( - getCenterLinePath({target_lane}, 0.0, arc_position_goal.length + 10)); - const auto seg_idx = - motion_utils::findNearestSegmentIndex(centerline_path.points, route_msg_.goal_pose.position); - const double d_lat = motion_utils::calcLongitudinalOffsetToSegment( - centerline_path.points, seg_idx, route_msg_.goal_pose.position); - const auto shoulder_point = - tier4_autoware_utils::calcOffsetPose(centerline_path.points.at(seg_idx).pose, d_lat, 0.0, 0.0); - pull_over_goal_pose_.orientation = shoulder_point.orientation; - pull_over_goal_pose_.position = shoulder_point.position; - - // distance between shoulder lane's left boundary and shoulder lane center - double distance_shoulder_to_left_boundary = - getDistanceToShoulderBoundary({target_lane}, shoulder_point); - - // distance between shoulder lane center and target line - double distance_shoulder_to_target = - distance_shoulder_to_left_boundary + vehicle_width / 2 + margin; - - // Apply shifting shoulder lane to adjust to target line - double offset = -distance_shoulder_to_target; - - double yaw = tf2::getYaw(shoulder_point.orientation); - pull_over_goal_pose_.position.x = shoulder_point.position.x - std::sin(yaw) * offset; - pull_over_goal_pose_.position.y = shoulder_point.position.y + std::cos(yaw) * offset; -} - -Pose RouteHandler::getPullOverGoalPose() const { return pull_over_goal_pose_; } - lanelet::Id RouteHandler::getGoalLaneId() const { if (route_msg_.segments.empty()) { @@ -731,7 +665,7 @@ lanelet::ConstLanelets RouteHandler::getShoulderLaneletSequenceUpTo( } lanelet::ConstLanelets RouteHandler::getShoulderLaneletSequence( - const lanelet::ConstLanelet & lanelet, const Pose & current_pose, const double backward_distance, + const lanelet::ConstLanelet & lanelet, const Pose & pose, const double backward_distance, const double forward_distance) const { lanelet::ConstLanelets lanelet_sequence; @@ -743,7 +677,7 @@ lanelet::ConstLanelets RouteHandler::getShoulderLaneletSequence( lanelet_sequence_forward = getShoulderLaneletSequenceAfter(lanelet, forward_distance); - const auto arc_coordinate = lanelet::utils::getArcCoordinates({lanelet}, current_pose); + const auto arc_coordinate = lanelet::utils::getArcCoordinates({lanelet}, pose); if (arc_coordinate.length < backward_distance) { lanelet_sequence_backward = getShoulderLaneletSequenceUpTo(lanelet, backward_distance); } @@ -1130,10 +1064,11 @@ bool RouteHandler::getLaneChangeTarget( } bool RouteHandler::getPullOverTarget( - const lanelet::ConstLanelets & lanelets, lanelet::ConstLanelet * target_lanelet) const + const lanelet::ConstLanelets & lanelets, const Pose & goal_pose, + lanelet::ConstLanelet * target_lanelet) const { for (const auto & shoulder_lanelet : lanelets) { - if (lanelet::utils::isInLanelet(getGoalPose(), shoulder_lanelet, 0.1)) { + if (lanelet::utils::isInLanelet(goal_pose, shoulder_lanelet, 0.1)) { *target_lanelet = shoulder_lanelet; return true; }