From bbe52258e80746fca1b0c6d765b028dbcf9a2941 Mon Sep 17 00:00:00 2001 From: kosuke55 Date: Mon, 6 Nov 2023 19:49:12 +0900 Subject: [PATCH 1/2] refactor(goal_planner): add prev_data instead of status Signed-off-by: kosuke55 --- .../goal_planner/goal_planner_module.hpp | 79 ++++--------- .../goal_planner/goal_planner_module.cpp | 107 ++++++++---------- 2 files changed, 69 insertions(+), 117 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp index d647caeabbc44..a31442def538f 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp @@ -68,64 +68,6 @@ using behavior_path_planner::utils::path_safety_checker::SafetyCheckParams; using behavior_path_planner::utils::path_safety_checker::TargetObjectsOnLane; using tier4_autoware_utils::Polygon2d; -#define DEFINE_SETTER(TYPE, NAME) \ -public: \ - void set_##NAME(const TYPE & value) \ - { \ - NAME##_ = value; \ - } - -#define DEFINE_GETTER(TYPE, NAME) \ -public: \ - TYPE get_##NAME() const \ - { \ - return NAME##_; \ - } - -#define DEFINE_SETTER_GETTER(TYPE, NAME) \ - DEFINE_SETTER(TYPE, NAME) \ - DEFINE_GETTER(TYPE, NAME) - -class PullOverStatus -{ -public: - // Reset all data members to their initial states - void reset() - { - lane_parking_pull_over_path_ = nullptr; - prev_stop_path_ = nullptr; - prev_stop_path_after_approval_ = nullptr; - - is_safe_ = false; - prev_found_path_ = false; - prev_is_safe_ = false; - } - - DEFINE_SETTER_GETTER(std::shared_ptr, lane_parking_pull_over_path) - DEFINE_SETTER_GETTER(std::shared_ptr, prev_stop_path) - DEFINE_SETTER_GETTER(std::shared_ptr, prev_stop_path_after_approval) - DEFINE_SETTER_GETTER(bool, is_safe) - DEFINE_SETTER_GETTER(bool, prev_found_path) - DEFINE_SETTER_GETTER(bool, prev_is_safe) - DEFINE_SETTER_GETTER(Pose, refined_goal_pose) - DEFINE_SETTER_GETTER(Pose, closest_goal_candidate_pose) - -private: - std::shared_ptr lane_parking_pull_over_path_{nullptr}; - std::shared_ptr prev_stop_path_{nullptr}; - std::shared_ptr prev_stop_path_after_approval_{nullptr}; - bool is_safe_{false}; - bool prev_found_path_{false}; - bool prev_is_safe_{false}; - - Pose refined_goal_pose_{}; - Pose closest_goal_candidate_pose_{}; -}; - -#undef DEFINE_SETTER -#undef DEFINE_GETTER -#undef DEFINE_SETTER_GETTER - #define DEFINE_SETTER_WITH_MUTEX(TYPE, NAME) \ public: \ void set_##NAME(const TYPE & value) \ @@ -288,6 +230,22 @@ struct LastApprovalData Pose pose{}; }; +struct PreviousPullOverData +{ + void reset() + { + stop_path = nullptr; + stop_path_after_approval = nullptr; + found_path = false; + is_safe = false; + } + + std::shared_ptr stop_path{nullptr}; + std::shared_ptr stop_path_after_approval{nullptr}; + bool found_path{false}; + bool is_safe{false}; +}; + class GoalPlannerModule : public SceneModuleInterface { public: @@ -380,10 +338,10 @@ class GoalPlannerModule : public SceneModuleInterface tier4_autoware_utils::LinearRing2d vehicle_footprint_; std::recursive_mutex mutex_; - PullOverStatus status_; ThreadSafeData thread_safe_data_; std::unique_ptr last_approval_data_{nullptr}; + PreviousPullOverData prev_data_{nullptr}; // approximate distance from the start point to the end point of pull_over. // this is used as an assumed value to decelerate, etc., before generating the actual path. @@ -412,7 +370,7 @@ class GoalPlannerModule : public SceneModuleInterface // goal seach Pose calcRefinedGoal(const Pose & goal_pose) const; - void generateGoalCandidates(); + GoalCandidates generateGoalCandidates() const; // stop or decelerate void deceleratePath(PullOverPath & pull_over_path) const; @@ -470,6 +428,7 @@ class GoalPlannerModule : public SceneModuleInterface // output setter void setOutput(BehaviorModuleOutput & output) const; void setStopPath(BehaviorModuleOutput & output) const; + void updatePreviousData(const BehaviorModuleOutput & output); /** * @brief Sets a stop path in the current path based on safety conditions and previous paths. 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 ba846efbb0a50..b41eec234814c 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 @@ -121,7 +121,7 @@ GoalPlannerModule::GoalPlannerModule( goal_planner_data_.collision_check); } - status_.reset(); + prev_data_.reset(); } // This function is needed for waiting for planner_data_ @@ -240,7 +240,7 @@ void GoalPlannerModule::onFreespaceParkingTimer() void GoalPlannerModule::updateData() { - if (getCurrentStatus() == ModuleStatus::IDLE) { + if (getCurrentStatus() == ModuleStatus::IDLE && !isExecutionRequested()) { return; } @@ -259,11 +259,14 @@ void GoalPlannerModule::updateData() updateOccupancyGrid(); - generateGoalCandidates(); + // update goal searcher and generate goal candidates + if (thread_safe_data_.get_goal_candidates().empty()) { + goal_searcher_->setPlannerData(planner_data_); + goal_searcher_->setReferenceGoal( + calcRefinedGoal(planner_data_->route_handler->getOriginalGoalPose())); + thread_safe_data_.set_goal_candidates(generateGoalCandidates()); + } - // Only after the path is decided, approval is allowed and the module is Activated. - // The path index is not incremented until after deciding the path. - // So return here, if (!isActivated()) { return; } @@ -308,7 +311,7 @@ void GoalPlannerModule::processOnExit() resetPathCandidate(); resetPathReference(); debug_marker_.markers.clear(); - status_.reset(); + prev_data_.reset(); last_approval_data_.reset(); } @@ -545,33 +548,24 @@ bool GoalPlannerModule::canReturnToLaneParking() return true; } -void GoalPlannerModule::generateGoalCandidates() +GoalCandidates GoalPlannerModule::generateGoalCandidates() const { - if (!thread_safe_data_.get_goal_candidates().empty()) { - return; - } - // calculate goal candidates const auto & route_handler = planner_data_->route_handler; - const Pose goal_pose = route_handler->getOriginalGoalPose(); - status_.set_refined_goal_pose(calcRefinedGoal(goal_pose)); if (goal_planner_utils::isAllowedGoalModification(route_handler)) { - const std::lock_guard lock(mutex_); - goal_searcher_->setPlannerData(planner_data_); - goal_searcher_->setReferenceGoal(status_.get_refined_goal_pose()); - thread_safe_data_.set_goal_candidates(goal_searcher_->search()); - status_.set_closest_goal_candidate_pose( - goal_searcher_->getClosetGoalCandidateAlongLanes(thread_safe_data_.get_goal_candidates()) - .goal_pose); - } else { - GoalCandidate goal_candidate{}; - goal_candidate.goal_pose = goal_pose; - goal_candidate.distance_from_original_goal = 0.0; - GoalCandidates goal_candidates{}; - goal_candidates.push_back(goal_candidate); - thread_safe_data_.set_goal_candidates(goal_candidates); - status_.set_closest_goal_candidate_pose(goal_pose); + return goal_searcher_->search(); } + + // NOTE: + // currently since pull over is performed only when isAllowedGoalModification is true, + // never be in the following process. + GoalCandidate goal_candidate{}; + goal_candidate.goal_pose = route_handler->getOriginalGoalPose(); + goal_candidate.distance_from_original_goal = 0.0; + GoalCandidates goal_candidates{}; + goal_candidates.push_back(goal_candidate); + + return goal_candidates; } BehaviorModuleOutput GoalPlannerModule::plan() @@ -695,14 +689,14 @@ void GoalPlannerModule::setOutput(BehaviorModuleOutput & output) const void GoalPlannerModule::setStopPath(BehaviorModuleOutput & output) const { - if (status_.get_prev_found_path() || !status_.get_prev_stop_path()) { + if (prev_data_.found_path || !prev_data_.stop_path) { // safe -> not_safe or no prev_stop_path: generate new stop_path output.path = std::make_shared(generateStopPath()); 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_.get_prev_stop_path(); + output.path = prev_data_.stop_path; // stop_pose_ is removed in manager every loop, so need to set every loop. stop_pose_ = utils::getFirstStopPoseFromPath(*output.path); RCLCPP_WARN_THROTTLE( @@ -713,7 +707,7 @@ void GoalPlannerModule::setStopPath(BehaviorModuleOutput & output) const void GoalPlannerModule::setStopPathFromCurrentPath(BehaviorModuleOutput & output) const { // safe or not safe(no feasible stop_path found) -> not_safe: try to find new feasible stop_path - if (status_.get_prev_is_safe() || !status_.get_prev_stop_path_after_approval()) { + if (prev_data_.is_safe || !prev_data_.stop_path_after_approval) { auto current_path = thread_safe_data_.get_pull_over_path()->getCurrentPath(); const auto stop_path = behavior_path_planner::utils::start_goal_planner_common::generateFeasibleStopPath( @@ -721,7 +715,6 @@ void GoalPlannerModule::setStopPathFromCurrentPath(BehaviorModuleOutput & output parameters_->maximum_jerk_for_stop); if (stop_path) { output.path = std::make_shared(*stop_path); - // status_.set_prev_stop_path_after_approval(output.path); RCLCPP_WARN_THROTTLE(getLogger(), *clock_, 5000, "Collision detected, generate stop path"); } else { output.path = @@ -730,10 +723,9 @@ void GoalPlannerModule::setStopPathFromCurrentPath(BehaviorModuleOutput & output getLogger(), *clock_, 5000, "Collision detected, no feasible stop path found, cannot stop."); } - // status_.set_last_path_update_time(std::make_shared(clock_->now())); } else { // not_safe safe(no feasible stop path found) -> not_safe: use previous stop path - output.path = status_.get_prev_stop_path_after_approval(); + output.path = prev_data_.stop_path_after_approval; // stop_pose_ is removed in manager every loop, so need to set every loop. stop_pose_ = utils::getFirstStopPoseFromPath(*output.path); RCLCPP_WARN_THROTTLE(getLogger(), *clock_, 5000, "Collision detected, use previous stop path"); @@ -934,7 +926,7 @@ BehaviorModuleOutput GoalPlannerModule::planPullOverAsOutput() path_candidate_ = std::make_shared(thread_safe_data_.get_pull_over_path()->getFullPath()); - updateStatus(output); + updatePreviousData(output); return output; } @@ -961,17 +953,16 @@ void GoalPlannerModule::postProcess() setStopReason(StopReason::GOAL_PLANNER, thread_safe_data_.get_pull_over_path()->getFullPath()); } -void GoalPlannerModule::updateStatus(const BehaviorModuleOutput & output) +void GoalPlannerModule::updatePreviousData(const BehaviorModuleOutput & output) { - if (status_.get_prev_found_path() || !status_.get_prev_stop_path()) { - status_.set_prev_stop_path(output.path); + if (prev_data_.found_path || !prev_data_.stop_path) { + prev_data_.stop_path = output.path; } // for the next loop setOutput(). // this is used to determine whether to generate a new stop path or keep the current stop path. - status_.set_prev_found_path(thread_safe_data_.foundPullOverPath()); - status_.set_prev_is_safe( - parameters_->safety_check_params.enable_safety_check ? isSafePath() : true); + prev_data_.found_path = thread_safe_data_.foundPullOverPath(); + prev_data_.is_safe = parameters_->safety_check_params.enable_safety_check ? isSafePath() : true; if (!isActivated()) { return; @@ -979,15 +970,15 @@ void GoalPlannerModule::updateStatus(const BehaviorModuleOutput & output) if ( !parameters_->safety_check_params.enable_safety_check || isSafePath() || - (!status_.get_prev_is_safe() && status_.get_prev_stop_path_after_approval())) { + (!prev_data_.is_safe && prev_data_.stop_path_after_approval)) { return; } - status_.set_prev_is_safe(false); + prev_data_.is_safe = false; const bool is_stop_path = std::any_of( output.path->points.begin(), output.path->points.end(), [](const auto & point) { return point.point.longitudinal_velocity_mps == 0.0; }); if (is_stop_path) { - status_.set_prev_stop_path_after_approval(output.path); + prev_data_.stop_path_after_approval = output.path; } } @@ -1043,10 +1034,9 @@ PathWithLaneId GoalPlannerModule::generateStopPath() const const double current_vel = planner_data_->self_odometry->twist.twist.linear.x; const double pull_over_velocity = parameters_->pull_over_velocity; - const lanelet::ConstLanelets current_lanes = utils::getExtendedCurrentLanes( - planner_data_, parameters_->backward_goal_search_length, - parameters_->forward_goal_search_length, - /*forward_only_in_route*/ false); + const auto current_lanes = utils::getExtendedCurrentLanes( + planner_data_, common_parameters.backward_path_length, std::numeric_limits::max(), + /*forward_only_in_route*/ true); if (current_lanes.empty()) { return PathWithLaneId{}; @@ -1061,8 +1051,10 @@ PathWithLaneId GoalPlannerModule::generateStopPath() const // calculate search start offset pose from the closest goal candidate pose with // approximate_pull_over_distance_ ego vehicle decelerates to this position. or if no feasible // stop point is found, stop at this position. + const auto closest_goal_candidate = + goal_searcher_->getClosetGoalCandidateAlongLanes(thread_safe_data_.get_goal_candidates()); const auto decel_pose = calcLongitudinalOffsetPose( - reference_path.points, status_.get_closest_goal_candidate_pose().position, + reference_path.points, closest_goal_candidate.goal_pose.position, -approximate_pull_over_distance_); // if not approved stop road lane. @@ -1132,10 +1124,9 @@ PathWithLaneId GoalPlannerModule::generateFeasibleStopPath() const const auto & common_parameters = planner_data_->parameters; // generate stop reference path - const lanelet::ConstLanelets current_lanes = utils::getExtendedCurrentLanes( - planner_data_, parameters_->backward_goal_search_length, - parameters_->forward_goal_search_length, - /*forward_only_in_route*/ false); + const auto current_lanes = utils::getExtendedCurrentLanes( + planner_data_, common_parameters.backward_path_length, std::numeric_limits::max(), + /*forward_only_in_route*/ true); const auto s_current = lanelet::utils::getArcCoordinates(current_lanes, current_pose).length; const double s_start = std::max(0.0, s_current - common_parameters.backward_path_length); const double s_end = s_current + common_parameters.forward_path_length; @@ -1432,8 +1423,10 @@ double GoalPlannerModule::calcSignedArcLengthFromEgo( void GoalPlannerModule::deceleratePath(PullOverPath & pull_over_path) const { // decelerate before the search area start + const auto closest_goal_candidate = + goal_searcher_->getClosetGoalCandidateAlongLanes(thread_safe_data_.get_goal_candidates()); const auto decel_pose = calcLongitudinalOffsetPose( - pull_over_path.getFullPath().points, status_.get_closest_goal_candidate_pose().position, + pull_over_path.getFullPath().points, closest_goal_candidate.goal_pose.position, -approximate_pull_over_distance_); auto & first_path = pull_over_path.partial_paths.front(); if (decel_pose) { @@ -1663,7 +1656,7 @@ bool GoalPlannerModule::isSafePath() const pull_over_lanes, route_handler, filtered_objects, objects_filtering_params_); const double hysteresis_factor = - status_.get_prev_is_safe() ? 1.0 : parameters_->hysteresis_factor_expand_rate; + prev_data_.is_safe ? 1.0 : parameters_->hysteresis_factor_expand_rate; utils::start_goal_planner_common::updateSafetyCheckTargetObjectsData( goal_planner_data_, filtered_objects, target_objects_on_lane, ego_predicted_path); @@ -1727,7 +1720,7 @@ void GoalPlannerModule::setDebugData() // Visualize pull over areas const auto color = hasDecidedPath() ? createMarkerColor(1.0, 1.0, 0.0, 0.999) // yellow : createMarkerColor(0.0, 1.0, 0.0, 0.999); // green - const double z = status_.get_refined_goal_pose().position.z; + const double z = planner_data_->route_handler->getGoalPose().position.z; add(goal_planner_utils::createPullOverAreaMarkerArray( goal_searcher_->getAreaPolygons(), header, color, z)); From 5c5173be440f1f537e8c8b3b1401cd9974dbbb39 Mon Sep 17 00:00:00 2001 From: kosuke55 Date: Mon, 6 Nov 2023 19:49:12 +0900 Subject: [PATCH 2/2] feat(goal_planner): keep margin against objects as possible Signed-off-by: kosuke55 --- .../goal_planner/goal_planner_module.hpp | 7 +- .../goal_planner/pull_over_planner_base.hpp | 1 + .../goal_planner/goal_planner_module.cpp | 161 ++++++++++++++---- 3 files changed, 138 insertions(+), 31 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp index a31442def538f..87340cc665edd 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp @@ -366,7 +366,10 @@ class GoalPlannerModule : public SceneModuleInterface // collision check void initializeOccupancyGridMap(); void updateOccupancyGrid(); - bool checkCollision(const PathWithLaneId & path) const; + bool checkOccupancyGridCollision(const PathWithLaneId & path) const; + bool checkObjectsCollision( + const PathWithLaneId & path, const double collision_check_margin, + const bool update_debug_data = false) const; // goal seach Pose calcRefinedGoal(const Pose & goal_pose) const; @@ -416,7 +419,7 @@ class GoalPlannerModule : public SceneModuleInterface BehaviorModuleOutput planPullOverAsCandidate(); std::optional> selectPullOverPath( const std::vector & pull_over_path_candidates, - const GoalCandidates & goal_candidates) const; + const GoalCandidates & goal_candidates, const double collision_check_margin) const; std::vector sortPullOverPathCandidatesByGoalPriority( const std::vector & pull_over_path_candidates, const GoalCandidates & goal_candidates) const; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/pull_over_planner_base.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/pull_over_planner_base.hpp index acb034a29d9e5..5023401263b8d 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/pull_over_planner_base.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/pull_over_planner_base.hpp @@ -53,6 +53,7 @@ struct PullOverPath Pose end_pose{}; std::vector debug_poses{}; size_t goal_id{}; + size_t id{}; bool decided_velocity{false}; PathWithLaneId getFullPath() const 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 b41eec234814c..4c0cb9e988e90 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 @@ -174,6 +174,7 @@ void GoalPlannerModule::onTimer() auto pull_over_path = planner->plan(goal_candidate.goal_pose); if (pull_over_path && isCrossingPossible(*pull_over_path)) { pull_over_path->goal_id = goal_candidate.id; + pull_over_path->id = path_candidates.size(); path_candidates.push_back(*pull_over_path); // calculate closest pull over start pose for stop path const double start_arc_length = @@ -533,7 +534,15 @@ bool GoalPlannerModule::canReturnToLaneParking() } const PathWithLaneId path = thread_safe_data_.get_lane_parking_pull_over_path()->getFullPath(); - if (checkCollision(path)) { + + if ( + parameters_->use_object_recognition && + checkObjectsCollision(path, parameters_->object_recognition_collision_check_margin)) { + return false; + } + + if ( + parameters_->use_occupancy_grid_for_path_collision_check && checkOccupancyGridCollision(path)) { return false; } @@ -597,6 +606,48 @@ std::vector GoalPlannerModule::sortPullOverPathCandidatesByGoalPri return goal_id_to_index[a.goal_id] < goal_id_to_index[b.goal_id]; }); + if (parameters_->use_object_recognition) { + // Create a map of PullOverPath pointer to largest collision check margin + auto calcLargestMargin = [&](const PullOverPath & pull_over_path) { + // check has safe goal + 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) { + return 0.0; + } + // calc largest margin + std::vector scale_factors{3.0, 2.0, 1.0}; + const double margin = parameters_->object_recognition_collision_check_margin; + const auto resampled_path = + utils::resamplePathWithSpline(pull_over_path.getParkingPath(), 0.5); + for (const auto & scale_factor : scale_factors) { + if (!checkObjectsCollision(resampled_path, margin * scale_factor)) { + return margin * scale_factor; + } + } + return 0.0; + }; + + // Create a map of PullOverPath pointer to largest collision check margin + std::map path_id_to_margin_map; + for (const auto & path : sorted_pull_over_path_candidates) { + path_id_to_margin_map[path.id] = calcLargestMargin(path); + } + + // sorts in descending order so the item with larger margin comes first + std::stable_sort( + sorted_pull_over_path_candidates.begin(), sorted_pull_over_path_candidates.end(), + [&path_id_to_margin_map](const PullOverPath & a, const PullOverPath & b) { + if (std::abs(path_id_to_margin_map[a.id] - path_id_to_margin_map[b.id]) < 0.01) { + return false; + } + return path_id_to_margin_map[a.id] > path_id_to_margin_map[b.id]; + }); + } + // Sort pull_over_path_candidates based on the order in efficient_path_order if (parameters_->path_priority == "efficient_path") { std::stable_sort( @@ -614,7 +665,7 @@ std::vector GoalPlannerModule::sortPullOverPathCandidatesByGoalPri std::optional> GoalPlannerModule::selectPullOverPath( const std::vector & pull_over_path_candidates, - const GoalCandidates & goal_candidates) const + const GoalCandidates & goal_candidates, const double collision_check_margin) const { for (const auto & pull_over_path : pull_over_path_candidates) { // check if goal is safe @@ -627,10 +678,20 @@ std::optional> GoalPlannerModule::selectP continue; } - // check if path is valid and safe + if (!hasEnoughDistance(pull_over_path)) { + continue; + } + + const auto resampled_path = utils::resamplePathWithSpline(pull_over_path.getParkingPath(), 0.5); + if ( + parameters_->use_object_recognition && + checkObjectsCollision(resampled_path, collision_check_margin, true /*update_debug_data*/)) { + continue; + } + if ( - !hasEnoughDistance(pull_over_path) || - checkCollision(utils::resamplePathWithSpline(pull_over_path.getParkingPath(), 0.5))) { + parameters_->use_occupancy_grid_for_path_collision_check && + checkOccupancyGridCollision(resampled_path)) { continue; } @@ -888,7 +949,9 @@ BehaviorModuleOutput GoalPlannerModule::planPullOverAsOutput() thread_safe_data_.get_pull_over_path_candidates(), goal_candidates); // select pull over path which is safe against static objects and get it's goal - const auto path_and_goal_opt = selectPullOverPath(pull_over_path_candidates, goal_candidates); + const auto path_and_goal_opt = selectPullOverPath( + pull_over_path_candidates, goal_candidates, + parameters_->object_recognition_collision_check_margin); // update thread_safe_data_ if (path_and_goal_opt) { @@ -1034,9 +1097,10 @@ PathWithLaneId GoalPlannerModule::generateStopPath() const const double current_vel = planner_data_->self_odometry->twist.twist.linear.x; const double pull_over_velocity = parameters_->pull_over_velocity; - const auto current_lanes = utils::getExtendedCurrentLanes( - planner_data_, common_parameters.backward_path_length, std::numeric_limits::max(), - /*forward_only_in_route*/ true); + const lanelet::ConstLanelets current_lanes = utils::getExtendedCurrentLanes( + planner_data_, parameters_->backward_goal_search_length, + parameters_->forward_goal_search_length, + /*forward_only_in_route*/ false); if (current_lanes.empty()) { return PathWithLaneId{}; @@ -1124,9 +1188,10 @@ PathWithLaneId GoalPlannerModule::generateFeasibleStopPath() const const auto & common_parameters = planner_data_->parameters; // generate stop reference path - const auto current_lanes = utils::getExtendedCurrentLanes( - planner_data_, common_parameters.backward_path_length, std::numeric_limits::max(), - /*forward_only_in_route*/ true); + const lanelet::ConstLanelets current_lanes = utils::getExtendedCurrentLanes( + planner_data_, parameters_->backward_goal_search_length, + parameters_->forward_goal_search_length, + /*forward_only_in_route*/ false); const auto s_current = lanelet::utils::getArcCoordinates(current_lanes, current_pose).length; const double s_start = std::max(0.0, s_current - common_parameters.backward_path_length); const double s_end = s_current + common_parameters.forward_path_length; @@ -1202,7 +1267,21 @@ bool GoalPlannerModule::isStuck() return false; } - return checkCollision(thread_safe_data_.get_pull_over_path()->getCurrentPath()); + if ( + parameters_->use_object_recognition && + checkObjectsCollision( + thread_safe_data_.get_pull_over_path()->getCurrentPath(), + parameters_->object_recognition_collision_check_margin)) { + return true; + } + + if ( + parameters_->use_occupancy_grid_for_path_collision_check && + checkOccupancyGridCollision(thread_safe_data_.get_pull_over_path()->getCurrentPath())) { + return true; + } + + return false; } bool GoalPlannerModule::hasFinishedCurrentPath() @@ -1294,18 +1373,19 @@ TurnSignalInfo GoalPlannerModule::calcTurnSignalInfo() const return turn_signal; } -bool GoalPlannerModule::checkCollision(const PathWithLaneId & path) const +bool GoalPlannerModule::checkOccupancyGridCollision(const PathWithLaneId & path) const { - if (parameters_->use_occupancy_grid_for_path_collision_check && 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 (!occupancy_grid_map_) { return false; } + const bool check_out_of_range = false; + return occupancy_grid_map_->hasObstacleOnPath(path, check_out_of_range); +} +bool GoalPlannerModule::checkObjectsCollision( + const PathWithLaneId & path, const double collision_check_margin, + const bool update_debug_data) const +{ const auto pull_over_lanes = goal_planner_utils::getPullOverLanes( *(planner_data_->route_handler), left_side_parking_, parameters_->backward_goal_search_length, parameters_->forward_goal_search_length); @@ -1320,7 +1400,30 @@ bool GoalPlannerModule::checkCollision(const PathWithLaneId & path) const obj_polygons.push_back(tier4_autoware_utils::toPolygon2d(object)); } - std::vector ego_polygons_expanded; + /* Expand ego collision check polygon + * - `collision_check_margin` in all directions + * - `extra_stopping_margin` in the moving direction + * - `extra_lateral_margin` in external direction of path curve + * + * + * ^ moving direction + * x + * x + * x + * +----------------------+------x--+ + * | | x | + * | +---------------+ | xx | + * | | | | xx | + * | | ego footprint |xxxxx | + * | | | | | + * | +---------------+ | extra_stopping_margin + * | margin | | + * +----------------------+ | + * | extra_lateral_margin | + * +--------------------------------+ + * + */ + std::vector ego_polygons_expanded{}; const auto curvatures = motion_utils::calcCurvature(path.points); for (size_t i = 0; i < path.points.size(); ++i) { const auto p = path.points.at(i); @@ -1338,16 +1441,16 @@ bool GoalPlannerModule::checkCollision(const PathWithLaneId & path) const tier4_autoware_utils::calcOffsetPose(p.point.pose, 0.0, extra_lateral_margin / 2.0, 0.0); const auto ego_polygon = tier4_autoware_utils::toFootprint( lateral_offset_pose, - planner_data_->parameters.base_link2front + - parameters_->object_recognition_collision_check_margin + extra_stopping_margin, - planner_data_->parameters.base_link2rear + - parameters_->object_recognition_collision_check_margin, - planner_data_->parameters.vehicle_width + - parameters_->object_recognition_collision_check_margin * 2.0 + + planner_data_->parameters.base_link2front + collision_check_margin + extra_stopping_margin, + planner_data_->parameters.base_link2rear + collision_check_margin, + planner_data_->parameters.vehicle_width + collision_check_margin * 2.0 + std::abs(extra_lateral_margin)); ego_polygons_expanded.push_back(ego_polygon); } - debug_data_.ego_polygons_expanded = ego_polygons_expanded; + + if (update_debug_data) { + debug_data_.ego_polygons_expanded = ego_polygons_expanded; + } return utils::path_safety_checker::checkPolygonsIntersects(ego_polygons_expanded, obj_polygons); }