diff --git a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp index 56ca29543fcec..51638e5485fe2 100644 --- a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp +++ b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp @@ -230,6 +230,13 @@ struct LastApprovalData Pose pose{}; }; +enum class DecidingPathStatus { + NOT_DECIDED, + DECIDING, + DECIDED, +}; +using DecidingPathStatusWithStamp = std::pair; + struct PreviousPullOverData { struct SafetyStatus @@ -242,12 +249,12 @@ struct PreviousPullOverData { found_path = false; safety_status = SafetyStatus{}; - has_decided_path = false; + deciding_path_status = DecidingPathStatusWithStamp{}; } bool found_path{false}; SafetyStatus safety_status{}; - bool has_decided_path{false}; + DecidingPathStatusWithStamp deciding_path_status{}; }; // store stop_pose_ pointer with reason string @@ -439,6 +446,8 @@ class GoalPlannerModule : public SceneModuleInterface bool needPathUpdate(const double path_update_duration) const; bool isStuck(); bool hasDecidedPath() const; + bool hasNotDecidedPath() const; + DecidingPathStatusWithStamp checkDecidingPathStatus() const; void decideVelocity(); bool foundPullOverPath() const; void updateStatus(const BehaviorModuleOutput & output); diff --git a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_searcher.hpp b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_searcher.hpp index 94cf7bc7ff5dd..e487d9d2ae0b2 100644 --- a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_searcher.hpp +++ b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_searcher.hpp @@ -35,8 +35,13 @@ class GoalSearcher : public GoalSearcherBase GoalCandidates search() override; void update(GoalCandidates & goal_candidates) const override; + + // todo(kosuke55):　Functions for this specific use should not be in the interface, + // so it is better to consider interface design when we implement other goal searchers. GoalCandidate getClosetGoalCandidateAlongLanes( const GoalCandidates & goal_candidates) const override; + bool isSafeGoalWithMarginScaleFactor( + const GoalCandidate & goal_candidate, const double margin_scale_factor) const override; private: void countObjectsToAvoid( diff --git a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_searcher_base.hpp b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_searcher_base.hpp index 93472ab6c0703..2ddacc0aac46d 100644 --- a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_searcher_base.hpp +++ b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_searcher_base.hpp @@ -60,6 +60,8 @@ class GoalSearcherBase virtual void update([[maybe_unused]] GoalCandidates & goal_candidates) const { return; } virtual GoalCandidate getClosetGoalCandidateAlongLanes( const GoalCandidates & goal_candidates) const = 0; + virtual bool isSafeGoalWithMarginScaleFactor( + const GoalCandidate & goal_candidate, const double margin_scale_factor) const = 0; protected: GoalPlannerParameters parameters_{}; diff --git a/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp b/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp index 34278bfa74939..faa9e28e28b3b 100644 --- a/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp +++ b/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp @@ -903,36 +903,108 @@ void GoalPlannerModule::updateSteeringFactor( bool GoalPlannerModule::hasDecidedPath() const { + return checkDecidingPathStatus().first == DecidingPathStatus::DECIDED; +} + +bool GoalPlannerModule::hasNotDecidedPath() const +{ + return checkDecidingPathStatus().first == DecidingPathStatus::NOT_DECIDED; +} + +DecidingPathStatusWithStamp GoalPlannerModule::checkDecidingPathStatus() const +{ + const auto & prev_status = prev_data_.deciding_path_status; + // Once this function returns true, it will continue to return true thereafter - if (prev_data_.has_decided_path) { - return true; + if (prev_status.first == DecidingPathStatus::DECIDED) { + return prev_status; } // if path is not safe, not decided if (!thread_safe_data_.foundPullOverPath()) { - return false; + return {DecidingPathStatus::NOT_DECIDED, clock_->now()}; } - // If it is dangerous before approval, do not determine the path. + // If it is dangerous against dynamic objects before approval, do not determine the path. // This eliminates a unsafe path to be approved if ( parameters_->safety_check_params.enable_safety_check && !isSafePath().first && !isActivated()) { - return false; + RCLCPP_DEBUG( + getLogger(), + "[DecidingPathStatus]: NOT_DECIDED. path is not safe against dynamic objects before " + "approval"); + return {DecidingPathStatus::NOT_DECIDED, clock_->now()}; + } + + // if object recognition for path collision check is enabled, transition to DECIDED after + // DECIDING for a certain period of time if there is no collision. + const auto pull_over_path = thread_safe_data_.get_pull_over_path(); + const auto current_path = pull_over_path->getCurrentPath(); + if (prev_status.first == DecidingPathStatus::DECIDING) { + const double hysteresis_factor = 0.9; + + // check goal pose collision + goal_searcher_->setPlannerData(planner_data_); + const auto modified_goal_opt = thread_safe_data_.get_modified_goal_pose(); + if ( + modified_goal_opt && !goal_searcher_->isSafeGoalWithMarginScaleFactor( + modified_goal_opt.value(), hysteresis_factor)) { + RCLCPP_DEBUG(getLogger(), "[DecidingPathStatus]: DECIDING->NOT_DECIDED. goal is not safe"); + return {DecidingPathStatus::NOT_DECIDED, clock_->now()}; + } + + // check current parking path collision + const auto parking_path = utils::resamplePathWithSpline(pull_over_path->getParkingPath(), 0.5); + const double margin = + parameters_->object_recognition_collision_check_margin * hysteresis_factor; + if (checkObjectsCollision(parking_path, margin)) { + RCLCPP_DEBUG( + getLogger(), + "[DecidingPathStatus]: DECIDING->NOT_DECIDED. path has collision with objects"); + return {DecidingPathStatus::NOT_DECIDED, clock_->now()}; + } + + // if enough time has passed since deciding status starts, transition to DECIDED + constexpr double check_collision_duration = 1.0; + const double elapsed_time_from_deciding = (clock_->now() - prev_status.second).seconds(); + if (elapsed_time_from_deciding > check_collision_duration) { + RCLCPP_DEBUG( + getLogger(), "[DecidingPathStatus]: DECIDING->DECIDED. has enough safe time passed"); + return {DecidingPathStatus::DECIDED, clock_->now()}; + } + + // if enough time has NOT passed since deciding status starts, keep DECIDING + RCLCPP_DEBUG( + getLogger(), "[DecidingPathStatus]: keep DECIDING. elapsed_time_from_deciding: %f", + elapsed_time_from_deciding); + return prev_status; } // if ego is sufficiently close to the start of the nearest candidate path, the path is decided const auto & current_pose = planner_data_->self_odometry->pose.pose; - const size_t ego_segment_idx = motion_utils::findNearestSegmentIndex( - thread_safe_data_.get_pull_over_path()->getCurrentPath().points, current_pose.position); + const size_t ego_segment_idx = + motion_utils::findNearestSegmentIndex(current_path.points, current_pose.position); - const size_t start_pose_segment_idx = motion_utils::findNearestSegmentIndex( - thread_safe_data_.get_pull_over_path()->getCurrentPath().points, - thread_safe_data_.get_pull_over_path()->start_pose.position); + const size_t start_pose_segment_idx = + motion_utils::findNearestSegmentIndex(current_path.points, pull_over_path->start_pose.position); const double dist_to_parking_start_pose = calcSignedArcLength( - thread_safe_data_.get_pull_over_path()->getCurrentPath().points, current_pose.position, - ego_segment_idx, thread_safe_data_.get_pull_over_path()->start_pose.position, - start_pose_segment_idx); - return dist_to_parking_start_pose < parameters_->decide_path_distance; + current_path.points, current_pose.position, ego_segment_idx, + pull_over_path->start_pose.position, start_pose_segment_idx); + if (dist_to_parking_start_pose > parameters_->decide_path_distance) { + return {DecidingPathStatus::NOT_DECIDED, clock_->now()}; + } + + // if object recognition for path collision check is enabled, transition to DECIDING to check + // collision for a certain period of time. Otherwise, transition to DECIDED directly. + if (parameters_->use_object_recognition) { + RCLCPP_DEBUG( + getLogger(), + "[DecidingPathStatus]: NOT_DECIDED->DECIDING. start checking collision for certain " + "period of time"); + return {DecidingPathStatus::DECIDING, clock_->now()}; + } + RCLCPP_DEBUG(getLogger(), "[DecidingPathStatus]: NOT_DECIDED->DECIDED"); + return {DecidingPathStatus::DECIDED, clock_->now()}; } void GoalPlannerModule::decideVelocity() @@ -993,10 +1065,11 @@ BehaviorModuleOutput GoalPlannerModule::planPullOverAsOutput() return getPreviousModuleOutput(); } - if (!hasDecidedPath() && needPathUpdate(1.0 /*path_update_duration*/)) { + if (hasNotDecidedPath() && needPathUpdate(1.0 /*path_update_duration*/)) { // if the final path is not decided and enough time has passed since last path update, // select safe path from lane parking pull over path candidates // and set it to thread_safe_data_ + RCLCPP_DEBUG(getLogger(), "Update pull over path candidates"); thread_safe_data_.clearPullOverPath(); @@ -1083,7 +1156,7 @@ void GoalPlannerModule::updatePreviousData() // this is used to determine whether to generate a new stop path or keep the current stop path. prev_data_.found_path = thread_safe_data_.foundPullOverPath(); - prev_data_.has_decided_path = hasDecidedPath(); + prev_data_.deciding_path_status = checkDecidingPathStatus(); // This is related to safety check, so if it is disabled, return here. if (!parameters_->safety_check_params.enable_safety_check) { diff --git a/planning/behavior_path_goal_planner_module/src/goal_searcher.cpp b/planning/behavior_path_goal_planner_module/src/goal_searcher.cpp index fd795177eb0b7..e2d12dd82cbe4 100644 --- a/planning/behavior_path_goal_planner_module/src/goal_searcher.cpp +++ b/planning/behavior_path_goal_planner_module/src/goal_searcher.cpp @@ -312,6 +312,43 @@ void GoalSearcher::update(GoalCandidates & goal_candidates) const } } +// Note: this function is not just return goal_candidate.is_safe but check collision with +// current planner_data_ and margin scale factor. +// And is_safe is not updated in this function. +bool GoalSearcher::isSafeGoalWithMarginScaleFactor( + const GoalCandidate & goal_candidate, const double margin_scale_factor) const +{ + if (!parameters_.use_object_recognition) { + return true; + } + + const Pose goal_pose = goal_candidate.goal_pose; + + const auto pull_over_lane_stop_objects = + goal_planner_utils::extractStaticObjectsInExpandedPullOverLanes( + *(planner_data_->route_handler), left_side_parking_, parameters_.backward_goal_search_length, + parameters_.forward_goal_search_length, parameters_.detection_bound_offset, + *(planner_data_->dynamic_object), parameters_.th_moving_object_velocity); + + const double margin = parameters_.object_recognition_collision_check_margin * margin_scale_factor; + + if (utils::checkCollisionBetweenFootprintAndObjects( + vehicle_footprint_, goal_pose, pull_over_lane_stop_objects, margin)) { + return false; + } + + // check longitudinal margin with pull over lane objects + constexpr bool filter_inside = true; + const auto target_objects = goal_planner_utils::filterObjectsByLateralDistance( + goal_pose, planner_data_->parameters.vehicle_width, pull_over_lane_stop_objects, margin, + filter_inside); + if (checkCollisionWithLongitudinalDistance(goal_pose, target_objects)) { + return false; + } + + return true; +} + bool GoalSearcher::checkCollision(const Pose & pose, const PredictedObjects & objects) const { if (parameters_.use_occupancy_grid_for_goal_search) {