From 3cd453b06aba6a14e146dc1817647132f34bf0c4 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Wed, 28 Dec 2022 01:15:48 +0900 Subject: [PATCH] feat(behavior_path_planner): generate pull over path candidates in advance (#2578) * feat(behavior_path_planner): generate pull over path candidates in advance Signed-off-by: kosuke55 * add update Signed-off-by: kosuke55 * Update planning/behavior_path_planner/src/scene_module/pull_over/goal_searcher.cpp Co-authored-by: Fumiya Watanabe * Update planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp Co-authored-by: Fumiya Watanabe * copy for multithread Signed-off-by: kosuke55 Signed-off-by: kosuke55 Co-authored-by: Fumiya Watanabe --- .../scene_module/pull_over/goal_searcher.hpp | 1 + .../pull_over/goal_searcher_base.hpp | 8 + .../pull_over/pull_over_module.hpp | 16 +- .../pull_over/pull_over_planner_base.hpp | 35 ++- .../pull_over/shift_pull_over.hpp | 4 - .../utils/geometric_parallel_parking.hpp | 1 - .../pull_over/geometric_pull_over.cpp | 16 +- .../scene_module/pull_over/goal_searcher.cpp | 72 +++-- .../pull_over/pull_over_module.cpp | 293 ++++++++++++------ .../pull_over/shift_pull_over.cpp | 88 +----- .../src/scene_module/pull_over/util.cpp | 4 +- .../utils/geometric_parallel_parking.cpp | 28 -- 12 files changed, 288 insertions(+), 278 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/goal_searcher.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/goal_searcher.hpp index de79f725e01c7..b0a909a36ac13 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/goal_searcher.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/goal_searcher.hpp @@ -37,6 +37,7 @@ class GoalSearcher : public GoalSearcherBase const std::shared_ptr & occupancy_grid_map); GoalCandidates search(const Pose & original_goal_pose) override; + void update(GoalCandidates & goal_candidates) const override; private: void createAreaPolygons(std::vector original_search_poses); diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/goal_searcher_base.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/goal_searcher_base.hpp index 2672d3c659500..3bea5528c7123 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/goal_searcher_base.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/goal_searcher_base.hpp @@ -36,9 +36,16 @@ struct GoalCandidate double distance_from_original_goal{0.0}; double lateral_offset{0.0}; size_t id{0}; + bool is_safe{true}; bool operator<(const GoalCandidate & other) const noexcept { + const double diff = distance_from_original_goal - other.distance_from_original_goal; + constexpr double eps = 0.01; + if (std::abs(diff) < eps) { + return lateral_offset < other.lateral_offset; + } + return distance_from_original_goal < other.distance_from_original_goal; } }; @@ -57,6 +64,7 @@ class GoalSearcherBase MultiPolygon2d getAreaPolygons() { return area_polygons_; } virtual GoalCandidates search(const Pose & original_goal_pose) = 0; + virtual void update([[maybe_unused]] GoalCandidates & goal_candidates) const { return; } protected: PullOverParameters parameters_; 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 633d8af9bf1cd..f86f592f0560f 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 @@ -53,7 +53,6 @@ enum PathType { struct PUllOverStatus { - std::shared_ptr planner{}; PullOverPath pull_over_path{}; size_t current_path_idx{0}; std::shared_ptr prev_stop_path{nullptr}; @@ -83,9 +82,6 @@ class PullOverModule : public SceneModuleInterface { return BT::NodeStatus::SUCCESS; } - void onTimer(); - bool planWithEfficientPath(); - bool planWithCloseGoal(); BehaviorModuleOutput plan() override; BehaviorModuleOutput planWaitingApproval() override; CandidateOutput planCandidate() const override; @@ -116,6 +112,8 @@ class PullOverModule : public SceneModuleInterface Pose modified_goal_pose_; Pose refined_goal_pose_; GoalCandidates goal_candidates_; + std::vector pull_over_path_candidates_; + std::optional closest_start_pose_; GeometricParallelParking parallel_parking_planner_; ParallelParkingParameters parallel_parking_parameters_; std::deque odometry_buffer_; @@ -126,7 +124,6 @@ class PullOverModule : public SceneModuleInterface void incrementPathIndex(); PathWithLaneId getCurrentPath() const; - PathWithLaneId getFullPath() const; Pose calcRefinedGoal(const Pose & goal_pose) const; ParallelParkingParameters getGeometricPullOverParameters() const; std::pair calcDistanceToPathChange() const; @@ -139,8 +136,17 @@ class PullOverModule : public SceneModuleInterface void updateOccupancyGrid(); void resetStatus(); + bool checkCollision(const PathWithLaneId & path) const; + bool hasEnoughDistance(const PullOverPath & pull_over_path) const; + TurnSignalInfo calcTurnSignalInfo() const; + // timer for generating pull over path candidates + void onTimer(); + rclcpp::TimerBase::SharedPtr timer_; + rclcpp::CallbackGroup::SharedPtr timer_cb_group_; + std::mutex mutex_; + // debug void setDebugData(); void printParkingPositionError() const; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_planner_base.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_planner_base.hpp index f6614595097ad..92d4d872a4010 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_planner_base.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_planner_base.hpp @@ -43,11 +43,44 @@ enum class PullOverPlannerType { struct PullOverPath { - PathWithLaneId path{}; + PullOverPlannerType type{PullOverPlannerType::NONE}; std::vector partial_paths{}; Pose start_pose{}; Pose end_pose{}; std::vector debug_poses{}; + size_t goal_id{}; + + PathWithLaneId getFullPath() const + { + PathWithLaneId path{}; + for (size_t i = 0; i < partial_paths.size(); ++i) { + if (i == 0) { + path.points.insert( + path.points.end(), partial_paths.at(i).points.begin(), partial_paths.at(i).points.end()); + } else { + // skip overlapping point + path.points.insert( + path.points.end(), next(partial_paths.at(i).points.begin()), + partial_paths.at(i).points.end()); + } + } + path.points = motion_utils::removeOverlapPoints(path.points); + + return path; + } + + PathWithLaneId getParkingPath() const + { + const PathWithLaneId full_path = getFullPath(); + const size_t start_idx = motion_utils::findNearestIndex(full_path.points, start_pose.position); + + PathWithLaneId parking_path{}; + std::copy( + full_path.points.begin() + start_idx, full_path.points.end(), + std::back_inserter(parking_path.points)); + + return parking_path; + } }; class PullOverPlannerBase diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/shift_pull_over.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/shift_pull_over.hpp index 3aaed89e2cca9..148e58d3ba904 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/shift_pull_over.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/shift_pull_over.hpp @@ -46,10 +46,6 @@ class ShiftPullOver : public PullOverPlannerBase boost::optional generatePullOverPath( const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & shoulder_lanes, const Pose & goal_pose, const double lateral_jerk) const; - bool hasEnoughDistance( - const PathWithLaneId & path, const lanelet::ConstLanelets & road_lanes, const Pose & start_pose, - const Pose & goal_pose, const double pull_over_distance) const; - bool isSafePath(const PathWithLaneId & path) const; static double calcBeforeShiftedArcLength( const PathWithLaneId & path, const double after_shifted_arc_length, const double dr); static std::vector splineTwoPoints( diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/utils/geometric_parallel_parking.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/utils/geometric_parallel_parking.hpp index 09786ee180382..3f8e20fa59659 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/utils/geometric_parallel_parking.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/utils/geometric_parallel_parking.hpp @@ -101,7 +101,6 @@ class GeometricParallelParking size_t current_path_idx_ = 0; void clearPaths(); - bool isEnoughDistanceToStart(const Pose & start_pose) const; std::vector planOneTrial( const Pose & start_pose, const Pose & goal_pose, const double R_E_r, const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & shoulder_lanes, diff --git a/planning/behavior_path_planner/src/scene_module/pull_over/geometric_pull_over.cpp b/planning/behavior_path_planner/src/scene_module/pull_over/geometric_pull_over.cpp index 4a23860e24e84..da42cd7b3acff 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_over/geometric_pull_over.cpp +++ b/planning/behavior_path_planner/src/scene_module/pull_over/geometric_pull_over.cpp @@ -65,22 +65,8 @@ boost::optional GeometricPullOver::plan(const Pose & goal_pose) // check lane departure with road and shoulder lanes if (lane_departure_checker_.checkPathWillLeaveLane(lanes, arc_path)) return {}; - // collision check - if (parameters_.use_occupancy_grid || !occupancy_grid_map_) { - const bool check_out_of_range = false; - if (occupancy_grid_map_->hasObstacleOnPath(arc_path, check_out_of_range)) { - return {}; - } - } - if (parameters_.use_object_recognition) { - if (util::checkCollisionBetweenPathFootprintsAndObjects( - vehicle_footprint_, arc_path, *(planner_data_->dynamic_object), - parameters_.object_recognition_collision_check_margin)) { - return {}; - } - } - PullOverPath pull_over_path{}; + pull_over_path.type = getPlannerType(); pull_over_path.partial_paths = planner_.getPaths(); pull_over_path.start_pose = planner_.getStartPose(); pull_over_path.end_pose = planner_.getArcEndPose(); diff --git a/planning/behavior_path_planner/src/scene_module/pull_over/goal_searcher.cpp b/planning/behavior_path_planner/src/scene_module/pull_over/goal_searcher.cpp index 1e17ba935a6ed..f8ef93edc55a5 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_over/goal_searcher.cpp +++ b/planning/behavior_path_planner/src/scene_module/pull_over/goal_searcher.cpp @@ -44,7 +44,6 @@ GoalCandidates GoalSearcher::search(const Pose & original_goal_pose) GoalCandidates goal_candidates{}; const auto & route_handler = planner_data_->route_handler; - const double vehicle_width = planner_data_->parameters.vehicle_width; const double forward_length = parameters_.forward_goal_search_length; const double backward_length = parameters_.backward_goal_search_length; const double margin_from_boundary = parameters_.margin_from_boundary; @@ -66,9 +65,10 @@ GoalCandidates GoalSearcher::search(const Pose & original_goal_pose) const auto [shoulder_lane_objects, others] = util::separateObjectsByLanelets(*(planner_data_->dynamic_object), pull_over_lanes); - std::vector original_search_poses{}; - for (size_t goal_id = 0; goal_id < center_line_path.points.size(); ++goal_id) { - const auto & center_pose = center_line_path.points.at(goal_id).point.pose; + std::vector original_search_poses{}; // for search area visualizing + size_t goal_id = 0; + for (const auto & p : center_line_path.points) { + const Pose & center_pose = p.point.pose; const auto distance_from_left_bound = util::getSignedDistanceFromShoulderLeftBoundary( pull_over_lanes, vehicle_footprint_, center_pose); @@ -79,7 +79,6 @@ GoalCandidates GoalSearcher::search(const Pose & original_goal_pose) original_search_poses.push_back(original_search_pose); // for createAreaPolygon Pose search_pose{}; // search goal_pose in lateral direction - bool found_lateral_no_collision_pose = false; double lateral_offset = 0.0; for (double dy = 0; dy <= max_lateral_offset; dy += lateral_offset_interval) { lateral_offset = dy; @@ -96,47 +95,52 @@ GoalCandidates GoalSearcher::search(const Pose & original_goal_pose) continue; } - if (checkCollision(search_pose)) { - continue; - } + GoalCandidate goal_candidate{}; + goal_candidate.goal_pose = search_pose; + goal_candidate.lateral_offset = lateral_offset; + goal_candidate.id = goal_id; + goal_id++; + // use longitudinal_distance as distance_from_original_goal + goal_candidate.distance_from_original_goal = std::abs(motion_utils::calcSignedArcLength( + center_line_path.points, original_goal_pose.position, search_pose.position)); + goal_candidates.push_back(goal_candidate); + } + } + createAreaPolygons(original_search_poses); - // if finding objects or detecting lane departure - // shift search_pose in lateral direction one more - // for avoiding them on other path points. - if (dy > 0 && dy < max_lateral_offset) { - search_pose = calcOffsetPose(search_pose, 0, -lateral_offset_interval, 0); - } + // Sort with distance from original goal + std::sort(goal_candidates.begin(), goal_candidates.end()); - found_lateral_no_collision_pose = true; - break; - } - if (!found_lateral_no_collision_pose) { + return goal_candidates; +} + +void GoalSearcher::update(GoalCandidates & goal_candidates) const +{ + // update is_safe + for (auto & goal_candidate : goal_candidates) { + const Pose goal_pose = goal_candidate.goal_pose; + + // check collision with footprint + if (checkCollision(goal_pose)) { + goal_candidate.is_safe = false; continue; } + // check margin with pull over lane objects + const auto pull_over_lanes = pull_over_utils::getPullOverLanes(*(planner_data_->route_handler)); + const auto [shoulder_lane_objects, others] = + util::separateObjectsByLanelets(*(planner_data_->dynamic_object), pull_over_lanes); constexpr bool filter_inside = true; const auto target_objects = pull_over_utils::filterObjectsByLateralDistance( - search_pose, vehicle_width, shoulder_lane_objects, + goal_pose, planner_data_->parameters.vehicle_width, shoulder_lane_objects, parameters_.object_recognition_collision_check_margin, filter_inside); - if (checkCollisionWithLongitudinalDistance(search_pose, target_objects)) { + if (checkCollisionWithLongitudinalDistance(goal_pose, target_objects)) { + goal_candidate.is_safe = false; continue; } - GoalCandidate goal_candidate{}; - goal_candidate.goal_pose = search_pose; - goal_candidate.lateral_offset = lateral_offset; - goal_candidate.id = goal_id; - // use longitudinal_distance as distance_from_original_goal - goal_candidate.distance_from_original_goal = std::abs(motion_utils::calcSignedArcLength( - center_line_path.points, original_goal_pose.position, search_pose.position)); - goal_candidates.push_back(goal_candidate); + goal_candidate.is_safe = true; } - createAreaPolygons(original_search_poses); - - // Sort with distance from original goal - std::sort(goal_candidates.begin(), goal_candidates.end()); - - return goal_candidates; } bool GoalSearcher::checkCollision(const Pose & pose) const 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 c1f9a145216ce..79dc8fd6b1fff 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 @@ -88,6 +88,12 @@ PullOverModule::PullOverModule( // for collision check with objects vehicle_footprint_ = createVehicleFootprint(vehicle_info_); + // timer callback for generating candidate paths + const auto period_ns = rclcpp::Rate(1.0).period(); + timer_cb_group_ = node.create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + timer_ = rclcpp::create_timer( + &node, clock_, period_ns, std::bind(&PullOverModule::onTimer, this), timer_cb_group_); + resetStatus(); } @@ -95,6 +101,9 @@ void PullOverModule::resetStatus() { PUllOverStatus initial_status{}; status_ = initial_status; + pull_over_path_candidates_.clear(); + closest_start_pose_.reset(); + goal_candidates_.clear(); } // This function is needed for waiting for planner_data_ @@ -107,6 +116,70 @@ void PullOverModule::updateOccupancyGrid() occupancy_grid_map_->setMap(*(planner_data_->occupancy_grid)); } +// generate pull over candidate paths +void PullOverModule::onTimer() +{ + // already generated pull over candidate paths + if (!pull_over_path_candidates_.empty()) { + return; + } + + // goals are not yet available. + if (goal_candidates_.empty()) { + return; + } + + // generate valid pull over path candidates and calculate closest start pose + const auto current_lanes = util::getExtendedCurrentLanes(planner_data_); + std::vector path_candidates{}; + std::optional closest_start_pose{}; + double min_start_arc_length = std::numeric_limits::max(); + const auto planCandidatePaths = [&]( + const std::shared_ptr & planner, + const GoalCandidate & goal_candidate) { + planner->setPlannerData(planner_data_); + auto pull_over_path = planner->plan(goal_candidate.goal_pose); + pull_over_path->goal_id = goal_candidate.id; + if (pull_over_path) { + path_candidates.push_back(*pull_over_path); + // calculate closest pull over start pose for stop path + const double start_arc_length = + lanelet::utils::getArcCoordinates(current_lanes, pull_over_path->start_pose).length; + if (start_arc_length < min_start_arc_length) { + min_start_arc_length = start_arc_length; + // closest start pose is stop point when not finding safe path + closest_start_pose = pull_over_path->start_pose; + } + } + }; + + // plan candidate paths and set them to the member variable + if (parameters_.search_priority == "efficient_path") { + for (const auto & planner : pull_over_planners_) { + for (const auto & goal_candidate : goal_candidates_) { + planCandidatePaths(planner, goal_candidate); + } + } + } else if (parameters_.search_priority == "close_goal") { + for (const auto & goal_candidate : goal_candidates_) { + for (const auto & planner : pull_over_planners_) { + planCandidatePaths(planner, goal_candidate); + } + } + } else { + RCLCPP_ERROR( + getLogger(), "search_priority should be efficient_path or close_goal, but %s is given.", + parameters_.search_priority.c_str()); + throw std::domain_error("[pull_over] invalid search_priority"); + } + + // set member variables + mutex_.lock(); + pull_over_path_candidates_ = path_candidates; + closest_start_pose_ = closest_start_pose; + mutex_.unlock(); +} + BehaviorModuleOutput PullOverModule::run() { current_state_ = BT::NodeStatus::RUNNING; @@ -163,19 +236,22 @@ void PullOverModule::onEntry() // Initialize parallel parking planner status parallel_parking_parameters_ = getGeometricPullOverParameters(); resetStatus(); + + refined_goal_pose_ = calcRefinedGoal(planner_data_->route_handler->getGoalPose()); + if (parameters_.enable_goal_research) { + goal_searcher_->setPlannerData(planner_data_); + goal_candidates_ = goal_searcher_->search(refined_goal_pose_); + } else { + GoalCandidate goal_candidate{}; + goal_candidate.goal_pose = refined_goal_pose_; + goal_candidate.distance_from_original_goal = 0.0; + goal_candidates_.push_back(goal_candidate); + } } last_received_time_ = std::make_unique(planner_data_->route_handler->getRouteHeader().stamp); // Use refined goal as modified goal when disabling goal research - refined_goal_pose_ = calcRefinedGoal(planner_data_->route_handler->getGoalPose()); - if (!parameters_.enable_goal_research) { - goal_candidates_.clear(); - GoalCandidate goal_candidate{}; - goal_candidate.goal_pose = refined_goal_pose_; - goal_candidate.distance_from_original_goal = 0.0; - goal_candidates_.push_back(goal_candidate); - } } void PullOverModule::onExit() @@ -185,8 +261,7 @@ void PullOverModule::onExit() removeRTCStatus(); resetPathCandidate(); steering_factor_interface_ptr_->clearSteeringFactors(); - - // A child node must never return IDLE + debug_marker_.markers.clear(); current_state_ = BT::NodeStatus::SUCCESS; } @@ -297,52 +372,9 @@ BT::NodeStatus PullOverModule::updateState() { // pull_out module will be run when setting new goal, so not need finishing pull_over module. // Finishing it causes wrong lane_following path generation. - // if (hasFinishedPullOver()) { - // current_state_ = BT::NodeStatus::SUCCESS; - // return current_state_; - // } - return current_state_; } -bool PullOverModule::planWithEfficientPath() -{ - for (const auto & planner : pull_over_planners_) { - for (const auto & goal_candidate : goal_candidates_) { - planner->setPlannerData(planner_data_); - const auto pull_over_path = planner->plan(goal_candidate.goal_pose); - if (!pull_over_path) { - continue; - } - modified_goal_pose_ = goal_candidate.goal_pose; - status_.pull_over_path = *pull_over_path; - status_.planner = planner; - return true; // found safe path - } - } - - return false; // not found safe path -} - -bool PullOverModule::planWithCloseGoal() -{ - for (const auto & goal_candidate : goal_candidates_) { - for (const auto & planner : pull_over_planners_) { - planner->setPlannerData(planner_data_); - const auto pull_over_path = planner->plan(goal_candidate.goal_pose); - if (!pull_over_path) { - continue; - } - modified_goal_pose_ = goal_candidate.goal_pose; - status_.pull_over_path = *pull_over_path; - status_.planner = planner; - return true; // found safe path - } - } - - return false; // not found safe path -} - BehaviorModuleOutput PullOverModule::plan() { const auto & current_pose = planner_data_->self_pose->pose; @@ -365,8 +397,8 @@ BehaviorModuleOutput PullOverModule::plan() } } - // Use decided path if (status_.has_decided_path) { + // Use decided path if (!status_.has_requested_approval) { // request approval again one the final path is decided waitApproval(); @@ -404,24 +436,35 @@ BehaviorModuleOutput PullOverModule::plan() incrementPathIndex(); } } + } else { + // select safe path from pull over path candidates + goal_searcher_->setPlannerData(planner_data_); + mutex_.lock(); + goal_searcher_->update(goal_candidates_); + const auto pull_over_path_candidates = pull_over_path_candidates_; + const auto goal_candidates = goal_candidates_; + mutex_.unlock(); + status_.is_safe = false; + for (const auto & pull_over_path : pull_over_path_candidates) { + // check if goal is safe + const auto goal_candidate_it = std::find_if( + goal_candidates.begin(), goal_candidates.end(), + [pull_over_path](const auto & goal_candidate) { + return goal_candidate.id == pull_over_path.goal_id; + }); + if (goal_candidate_it != goal_candidates.end() && !goal_candidate_it->is_safe) { + continue; + } - } else { // Replan shift -> arc forward -> arc backward path with each goal candidate. - // Research goal when enabling research and final path has not been decided - if (parameters_.enable_goal_research) { - goal_searcher_->setPlannerData(planner_data_); - goal_candidates_ = goal_searcher_->search(refined_goal_pose_); - } + // check if path is valid and safe + if (!hasEnoughDistance(pull_over_path) || checkCollision(pull_over_path.getParkingPath())) { + continue; + } - // plan paths with several goals and planner - if (parameters_.search_priority == "efficient_path") { - status_.is_safe = planWithEfficientPath(); - } else if (parameters_.search_priority == "close_goal") { - status_.is_safe = planWithCloseGoal(); - } else { - RCLCPP_ERROR( - getLogger(), "search_priority should be efficient_path or close_goal, but %s is given.", - parameters_.search_priority.c_str()); - throw std::domain_error("[pull_over] invalid search_priority"); + status_.is_safe = true; + status_.pull_over_path = pull_over_path; + modified_goal_pose_ = pull_over_path.getFullPath().points.back().point.pose; + break; } // Decelerate before the minimum shift distance from the goal search area. @@ -440,6 +483,8 @@ BehaviorModuleOutput PullOverModule::plan() // generate drivable area for each partial path for (auto & path : status_.pull_over_path.partial_paths) { + const size_t ego_idx = findEgoIndex(path.points); + util::clipPathLength(path, ego_idx, planner_data_->parameters); const auto shorten_lanes = util::cutOverlappedLanes(path, status_.lanes); const auto expanded_lanes = util::expandLanelets( shorten_lanes, parameters_.drivable_area_left_bound_offset, @@ -450,25 +495,28 @@ BehaviorModuleOutput PullOverModule::plan() } BehaviorModuleOutput output; - // safe: use pull over path if (status_.is_safe) { + // safe: use pull over path status_.stop_pose.reset(); output.path = std::make_shared(getCurrentPath()); - path_candidate_ = std::make_shared(getFullPath()); + path_candidate_ = std::make_shared(status_.pull_over_path.getFullPath()); } else { - RCLCPP_WARN_THROTTLE( - getLogger(), *clock_, 5000, "Not found safe pull_over path. Stop in road lane."); - // safe -> not_safe or no prev_stop_path: generate new stop_path + // not safe: use stop_path if (status_.prev_is_safe || status_.prev_stop_path == nullptr) { + // safe -> not_safe or no prev_stop_path: generate new stop_path output.path = std::make_shared(generateStopPath()); status_.prev_stop_path = output.path; // set stop path as pull over path PullOverPath pull_over_path{}; status_.pull_over_path = pull_over_path; - status_.pull_over_path.path = *output.path; status_.pull_over_path.partial_paths.push_back(*output.path); - } else { // not_safe -> not_safe: use previous stop path + RCLCPP_WARN_THROTTLE( + getLogger(), *clock_, 5000, "Not found safe pull_over path, generate stop path"); + } else { + // not_safe -> not_safe: use previous stop path output.path = status_.prev_stop_path; + RCLCPP_WARN_THROTTLE( + getLogger(), *clock_, 5000, "Not found safe pull_over path, use previous stop path"); } } status_.prev_is_safe = status_.is_safe; @@ -526,8 +574,9 @@ BehaviorModuleOutput PullOverModule::planWaitingApproval() BehaviorModuleOutput out; plan(); // update status_ out.path = std::make_shared(generateStopPath()); - path_candidate_ = status_.is_safe ? std::make_shared(getFullPath()) : out.path; - + path_candidate_ = status_.is_safe + ? std::make_shared(status_.pull_over_path.getFullPath()) + : out.path; const auto distance_to_path_change = calcDistanceToPathChange(); updateRTCStatus(distance_to_path_change.first, distance_to_path_change.second); @@ -552,7 +601,7 @@ BehaviorModuleOutput PullOverModule::planWaitingApproval() std::pair PullOverModule::calcDistanceToPathChange() const { - const auto & full_path = getFullPath(); + const auto & full_path = status_.pull_over_path.getFullPath(); const auto dist_to_parking_start_pose = calcSignedArcLength( full_path.points, planner_data_->self_pose->pose, status_.pull_over_path.start_pose.position, std::numeric_limits::max(), M_PI_2); @@ -586,14 +635,23 @@ PathWithLaneId PullOverModule::generateStopPath() auto reference_path = route_handler->getCenterLinePath(status_.current_lanes, s_start, s_end, true); - // if not approved, stop parking start position or goal search start position. + // if not approved stop road lane. + // stop point priority is + // 1. actual start pose + // 2. closest candidate start pose + // 3. search start pose + // (In the case of the curve lane, the position is not aligned due to the + // difference between the outer and inner sides) + // 4. emergency stop const auto search_start_pose = calcLongitudinalOffsetPose( reference_path.points, refined_goal_pose_.position, -parameters_.backward_goal_search_length - planner_data_->parameters.base_link2front); - if (!status_.is_safe && !search_start_pose) { + if (!status_.is_safe && !closest_start_pose_ && !search_start_pose) { return generateEmergencyStopPath(); } - const Pose stop_pose = status_.is_safe ? status_.pull_over_path.start_pose : *search_start_pose; + const Pose stop_pose = + status_.is_safe ? status_.pull_over_path.start_pose + : (closest_start_pose_ ? closest_start_pose_.value() : *search_start_pose); // if stop pose is behind current pose, stop as soon as possible const size_t ego_idx = findEgoIndex(reference_path.points); @@ -697,24 +755,6 @@ PathWithLaneId PullOverModule::getCurrentPath() const return status_.pull_over_path.partial_paths.at(status_.current_path_idx); } -PathWithLaneId PullOverModule::getFullPath() const -{ - PathWithLaneId path{}; - const auto & paths = status_.pull_over_path.partial_paths; - for (size_t i = 0; i < paths.size(); ++i) { - if (i == 0) { - path.points.insert(path.points.end(), paths.at(i).points.begin(), paths.at(i).points.end()); - } else { - // skip overlapping point - path.points.insert( - path.points.end(), next(paths.at(i).points.begin()), paths.at(i).points.end()); - } - } - path.points = motion_utils::removeOverlapPoints(path.points); - - return path; -} - bool PullOverModule::isStopped() { odometry_buffer_.push_back(planner_data_->self_odometry); @@ -765,7 +805,7 @@ TurnSignalInfo PullOverModule::calcTurnSignalInfo() const const auto & current_pose = planner_data_->self_pose->pose; const auto & start_pose = status_.pull_over_path.start_pose; const auto & end_pose = status_.pull_over_path.end_pose; - const auto & full_path = getFullPath(); + const auto full_path = status_.pull_over_path.getFullPath(); // calc TurnIndicatorsCommand { @@ -822,7 +862,7 @@ void PullOverModule::setDebugData() status_.pull_over_path.start_pose, "pull_over_start_pose", 0, 0.3, 0.3, 0.9)); add(createPoseMarkerArray( status_.pull_over_path.end_pose, "pull_over_end_pose", 0, 0.3, 0.3, 0.9)); - add(createPathMarkerArray(getFullPath(), "full_path", 0, 0.0, 0.5, 0.9)); + add(createPathMarkerArray(status_.pull_over_path.getFullPath(), "full_path", 0, 0.0, 0.5, 0.9)); } // Visualize debug poses @@ -840,6 +880,53 @@ void PullOverModule::setDebugData() } } +bool PullOverModule::checkCollision(const PathWithLaneId & path) const +{ + if (parameters_.use_occupancy_grid || !occupancy_grid_map_) { + const bool check_out_of_range = false; + if (occupancy_grid_map_->hasObstacleOnPath(path, check_out_of_range)) { + return true; + } + } + + if (parameters_.use_object_recognition) { + if (util::checkCollisionBetweenPathFootprintsAndObjects( + vehicle_footprint_, path, *(planner_data_->dynamic_object), + parameters_.object_recognition_collision_check_margin)) { + return true; + } + } + + return false; +} + +bool PullOverModule::hasEnoughDistance(const PullOverPath & pull_over_path) const +{ + const Pose & current_pose = planner_data_->self_pose->pose; + const double current_vel = planner_data_->self_odometry->twist.twist.linear.x; + + // once stopped, the vehicle cannot start again if start_pose is close. + // so need enough distance to restart. + // distance to restart should be less than decide_path_distance. + // otherwise, the goal would change immediately after departure. + constexpr double eps_vel = 0.01; + const double distance_to_start = calcSignedArcLength( + pull_over_path.getFullPath().points, current_pose.position, pull_over_path.start_pose.position); + const double distance_to_restart = parameters_.decide_path_distance / 2; + if (std::abs(current_vel) < eps_vel && distance_to_start < distance_to_restart) { + return false; + } + + // prevent emergency stop + const double current_to_stop_distance = + std::pow(current_vel, 2) / parameters_.maximum_deceleration / 2; + if (distance_to_start < current_to_stop_distance) { + return false; + } + + return true; +} + void PullOverModule::printParkingPositionError() const { const auto current_pose = planner_data_->self_pose->pose; diff --git a/planning/behavior_path_planner/src/scene_module/pull_over/shift_pull_over.cpp b/planning/behavior_path_planner/src/scene_module/pull_over/shift_pull_over.cpp index 69ab6a56b0cf6..d322a9613c6cf 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_over/shift_pull_over.cpp +++ b/planning/behavior_path_planner/src/scene_module/pull_over/shift_pull_over.cpp @@ -178,15 +178,6 @@ boost::optional ShiftPullOver::generatePullOverPath( return {}; } - // check collision - PathWithLaneId collision_check_path = shifted_path.path; - collision_check_path.points.clear(); - std::copy( - shifted_path.path.points.begin() + path_shifter.getShiftLines().front().start_idx, - shifted_path.path.points.end(), std::back_inserter(collision_check_path.points)); - - if (!isSafePath(collision_check_path)) return {}; - // set lane_id and velocity to shifted_path for (size_t i = path_shifter.getShiftLines().front().start_idx; i < shifted_path.path.points.size() - 1; ++i) { @@ -214,91 +205,16 @@ boost::optional ShiftPullOver::generatePullOverPath( // set pull over path PullOverPath pull_over_path{}; - pull_over_path.path = shifted_path.path; - pull_over_path.partial_paths.push_back(pull_over_path.path); + pull_over_path.type = getPlannerType(); + pull_over_path.partial_paths.push_back(shifted_path.path); pull_over_path.start_pose = path_shifter.getShiftLines().front().start; pull_over_path.end_pose = path_shifter.getShiftLines().front().end; pull_over_path.debug_poses.push_back(shift_end_pose_road_lane); pull_over_path.debug_poses.push_back(actual_shift_end_pose); - // check enough distance - if (!hasEnoughDistance( - pull_over_path.path, road_lanes, pull_over_path.start_pose, goal_pose, - pull_over_distance)) { - return {}; - } - return pull_over_path; } -bool ShiftPullOver::hasEnoughDistance( - const PathWithLaneId & path, const lanelet::ConstLanelets & road_lanes, const Pose & start_pose, - const Pose & goal_pose, const double pull_over_distance) const -{ - const auto & current_pose = planner_data_->self_pose->pose; - const auto & common_params = planner_data_->parameters; - const double current_vel = planner_data_->self_odometry->twist.twist.linear.x; - - const size_t ego_segment_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - path.points, current_pose, common_params.ego_nearest_dist_threshold, - common_params.ego_nearest_yaw_threshold); - const size_t start_segment_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - path.points, start_pose, common_params.ego_nearest_dist_threshold, - common_params.ego_nearest_yaw_threshold); - const double dist_to_start_pose = motion_utils::calcSignedArcLength( - path.points, current_pose.position, ego_segment_idx, start_pose.position, start_segment_idx); - - // once stopped, it cannot start again if start_pose is close. - // so need enough distance to restart - constexpr double eps_vel = 0.01; - // dist to restart should be less than decide_path_distance. - // otherwise, the goal would change immediately after departure. - const double dist_to_restart = parameters_.decide_path_distance / 2; - if (std::abs(current_vel) < eps_vel && dist_to_start_pose < dist_to_restart) { - return false; - } - const double current_to_stop_distance = - std::pow(current_vel, 2) / parameters_.maximum_deceleration / 2; - if (dist_to_start_pose < current_to_stop_distance) { - return false; - } - - const double road_lane_dist_to_goal = dist_to_start_pose + pull_over_distance; - if (road_lane_dist_to_goal > util::getDistanceToEndOfLane(current_pose, road_lanes)) { - return false; - } - - const bool is_in_goal_route_section = - planner_data_->route_handler->isInGoalRouteSection(road_lanes.back()); - if ( - is_in_goal_route_section && - road_lane_dist_to_goal > util::getSignedDistance(current_pose, goal_pose, road_lanes)) { - return false; - } - - return true; -} - -bool ShiftPullOver::isSafePath(const PathWithLaneId & path) const -{ - if (parameters_.use_occupancy_grid || !occupancy_grid_map_) { - const bool check_out_of_range = false; - if (occupancy_grid_map_->hasObstacleOnPath(path, check_out_of_range)) { - return false; - } - } - - if (parameters_.use_object_recognition) { - if (util::checkCollisionBetweenPathFootprintsAndObjects( - vehicle_footprint_, path, *(planner_data_->dynamic_object), - parameters_.object_recognition_collision_check_margin)) { - return false; - } - } - - return true; -} - double ShiftPullOver::calcBeforeShiftedArcLength( const PathWithLaneId & path, const double target_after_arc_length, const double dr) { 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 b2a7af91ef3ad..c691208c9415c 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 @@ -148,7 +148,9 @@ MarkerArray createGoalCandidatesMarkerArray( // convert to pose vector std::vector pose_vector{}; for (const auto & goal_candidate : goal_candidates) { - pose_vector.push_back(goal_candidate.goal_pose); + if (goal_candidate.is_safe) { + pose_vector.push_back(goal_candidate.goal_pose); + } } auto marker_array = createPosesMarkerArray(pose_vector, "goal_candidates", color); diff --git a/planning/behavior_path_planner/src/scene_module/utils/geometric_parallel_parking.cpp b/planning/behavior_path_planner/src/scene_module/utils/geometric_parallel_parking.cpp index 9b4ba77a3c7f6..38cecf8ee699d 100644 --- a/planning/behavior_path_planner/src/scene_module/utils/geometric_parallel_parking.cpp +++ b/planning/behavior_path_planner/src/scene_module/utils/geometric_parallel_parking.cpp @@ -98,31 +98,6 @@ PathWithLaneId GeometricParallelParking::getArcPath() const bool GeometricParallelParking::isParking() const { return current_path_idx_ > 0; } -bool GeometricParallelParking::isEnoughDistanceToStart(const Pose & start_pose) const -{ - const Pose current_pose = planner_data_->self_pose->pose; - const Pose current_to_start = - inverseTransformPose(start_pose, current_pose); // todo: arc length is better - - // not enough to stop with max deceleration - const double current_vel = util::l2Norm(planner_data_->self_odometry->twist.twist.linear); - const double stop_distance = std::pow(current_vel, 2) / parameters_.maximum_deceleration / 2; - if (current_to_start.position.x < stop_distance) { - return false; - } - - // not enough to restart from stopped - constexpr double min_restart_distance = 3.0; - if ( - current_vel < parameters_.th_stopped_velocity && - current_to_start.position.x > parameters_.th_arrived_distance && - current_to_start.position.x < min_restart_distance) { - return false; - } - - return true; -} - void GeometricParallelParking::setVelocityToArcPaths( std::vector & arc_paths, const double velocity) { @@ -143,9 +118,6 @@ std::vector GeometricParallelParking::generatePullOverPaths( const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & shoulder_lanes, const bool is_forward, const double end_pose_offset, const double velocity) { - if (!isEnoughDistanceToStart(start_pose)) { - return std::vector{}; - } const double lane_departure_margin = is_forward ? parameters_.forward_parking_lane_departure_margin : parameters_.backward_parking_lane_departure_margin;