diff --git a/planning/behavior_path_dynamic_avoidance_module/src/scene.cpp b/planning/behavior_path_dynamic_avoidance_module/src/scene.cpp index b43102ecbd16d..73cabd00592a8 100644 --- a/planning/behavior_path_dynamic_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_dynamic_avoidance_module/src/scene.cpp @@ -397,6 +397,7 @@ BehaviorModuleOutput DynamicAvoidanceModule::plan() current_drivable_area_info, getPreviousModuleOutput().drivable_area_info); output.reference_path = getPreviousModuleOutput().reference_path; output.turn_signal_info = getPreviousModuleOutput().turn_signal_info; + output.modified_goal = getPreviousModuleOutput().modified_goal; return output; } diff --git a/planning/behavior_path_goal_planner_module/README.md b/planning/behavior_path_goal_planner_module/README.md index f777e49e5fd78..6bd2f8b9c79e4 100644 --- a/planning/behavior_path_goal_planner_module/README.md +++ b/planning/behavior_path_goal_planner_module/README.md @@ -155,6 +155,7 @@ Generate footprints from ego-vehicle path points and determine obstacle collisio | use_object_recognition | [-] | bool | flag whether to use object recognition for collision check | true | | object_recognition_collision_check_margin | [m] | double | margin to calculate ego-vehicle cells from footprint. | 0.6 | | object_recognition_collision_check_max_extra_stopping_margin | [m] | double | maximum value when adding longitudinal distance margin for collision check considering stopping distance | 1.0 | +| detection_bound_offset | [m] | double | expand pull over lane with this offset to make detection area for collision check of path generation | 15.0 | ## **Goal Search** diff --git a/planning/behavior_path_goal_planner_module/config/goal_planner.param.yaml b/planning/behavior_path_goal_planner_module/config/goal_planner.param.yaml index 0ab617a1a7ab9..6f51b906c4c8f 100644 --- a/planning/behavior_path_goal_planner_module/config/goal_planner.param.yaml +++ b/planning/behavior_path_goal_planner_module/config/goal_planner.param.yaml @@ -38,6 +38,7 @@ object_recognition_collision_check_margin: 0.6 # this should be larger than `surround_check_distance` of surround_obstacle_checker object_recognition_collision_check_max_extra_stopping_margin: 1.0 th_moving_object_velocity: 1.0 + detection_bound_offset: 15.0 # pull over pull_over: 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 9fb9c17f1ca70..2c839f582be12 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 @@ -240,18 +247,37 @@ struct PreviousPullOverData void reset() { - stop_path = nullptr; - stop_path_after_approval = nullptr; found_path = false; safety_status = SafetyStatus{}; - has_decided_path = false; + deciding_path_status = DecidingPathStatusWithStamp{}; } - std::shared_ptr stop_path{nullptr}; - std::shared_ptr stop_path_after_approval{nullptr}; bool found_path{false}; SafetyStatus safety_status{}; - bool has_decided_path{false}; + DecidingPathStatusWithStamp deciding_path_status{}; +}; + +// store stop_pose_ pointer with reason string +struct PoseWithString +{ + std::optional * pose; + std::string string; + + explicit PoseWithString(std::optional * shared_pose) : pose(shared_pose), string("") {} + + void set(const Pose & new_pose, const std::string & new_string) + { + *pose = new_pose; + string = new_string; + } + + void set(const std::string & new_string) { string = new_string; } + + void clear() + { + pose->reset(); + string = ""; + } }; class GoalPlannerModule : public SceneModuleInterface @@ -364,7 +390,7 @@ class GoalPlannerModule : public SceneModuleInterface ThreadSafeData thread_safe_data_; std::unique_ptr last_approval_data_{nullptr}; - PreviousPullOverData prev_data_{nullptr}; + PreviousPullOverData prev_data_{}; // 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. @@ -385,6 +411,7 @@ class GoalPlannerModule : public SceneModuleInterface // debug mutable GoalPlannerDebugData debug_data_; + mutable PoseWithString debug_stop_pose_with_info_; // collision check void initializeOccupancyGridMap(); @@ -392,7 +419,7 @@ class GoalPlannerModule : public SceneModuleInterface bool checkOccupancyGridCollision(const PathWithLaneId & path) const; bool checkObjectsCollision( const PathWithLaneId & path, const double collision_check_margin, - const bool update_debug_data = false) const; + const bool extract_static_objects, const bool update_debug_data = false) const; // goal seach Pose calcRefinedGoal(const Pose & goal_pose) const; @@ -404,7 +431,7 @@ class GoalPlannerModule : public SceneModuleInterface void decelerateBeforeSearchStart( const Pose & search_start_offset_pose, PathWithLaneId & path) const; PathWithLaneId generateStopPath() const; - PathWithLaneId generateFeasibleStopPath() const; + PathWithLaneId generateFeasibleStopPath(const PathWithLaneId & path) const; void keepStoppedWithCurrentPath(PathWithLaneId & path) const; double calcSignedArcLengthFromEgo(const PathWithLaneId & path, const Pose & pose) const; @@ -419,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); @@ -453,19 +482,8 @@ class GoalPlannerModule : public SceneModuleInterface // output setter void setOutput(BehaviorModuleOutput & output) const; - void setStopPath(BehaviorModuleOutput & output) const; - void updatePreviousData(const BehaviorModuleOutput & output); + void updatePreviousData(); - /** - * @brief Sets a stop path in the current path based on safety conditions and previous paths. - * - * This function sets a stop path in the current path. Depending on whether the previous safety - * judgement against dynamic objects were safe or if a previous stop path existed, it either - * generates a new stop path or uses the previous stop path. - * - * @param output BehaviorModuleOutput - */ - void setStopPathFromCurrentPath(BehaviorModuleOutput & output) const; void setModifiedGoal(BehaviorModuleOutput & output) const; void setTurnSignalInfo(BehaviorModuleOutput & output) const; diff --git a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_parameters.hpp b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_parameters.hpp index 21eb22046bff6..2310c0c4d422c 100644 --- a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_parameters.hpp +++ b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_parameters.hpp @@ -74,6 +74,7 @@ struct GoalPlannerParameters double object_recognition_collision_check_margin{0.0}; double object_recognition_collision_check_max_extra_stopping_margin{0.0}; double th_moving_object_velocity{0.0}; + double detection_bound_offset{0.0}; // pull over general params double pull_over_minimum_request_length{0.0}; 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/include/behavior_path_goal_planner_module/util.hpp b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/util.hpp index 294a5125a0d13..be56d9dc30196 100644 --- a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/util.hpp +++ b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/util.hpp @@ -45,9 +45,19 @@ using visualization_msgs::msg::MarkerArray; lanelet::ConstLanelets getPullOverLanes( const RouteHandler & route_handler, const bool left_side, const double backward_distance, const double forward_distance); +lanelet::ConstLanelets generateExpandedPullOverLanes( + const RouteHandler & route_handler, const bool left_side, const double backward_distance, + const double forward_distance, double bound_offset); +PredictedObjects extractObjectsInExpandedPullOverLanes( + const RouteHandler & route_handler, const bool left_side, const double backward_distance, + const double forward_distance, double bound_offset, const PredictedObjects & objects); PredictedObjects filterObjectsByLateralDistance( const Pose & ego_pose, const double vehicle_width, const PredictedObjects & objects, const double distance_thresh, const bool filter_inside); +PredictedObjects extractStaticObjectsInExpandedPullOverLanes( + const RouteHandler & route_handler, const bool left_side, const double backward_distance, + const double forward_distance, double bound_offset, const PredictedObjects & objects, + const double velocity_thresh); double calcLateralDeviationBetweenPaths( const PathWithLaneId & reference_path, const PathWithLaneId & target_path); 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 a13344ee87e7c..00810432a6f82 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 @@ -61,7 +61,8 @@ GoalPlannerModule::GoalPlannerModule( : SceneModuleInterface{name, node, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map}, // NOLINT parameters_{parameters}, vehicle_info_{vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo()}, - thread_safe_data_{mutex_, clock_} + thread_safe_data_{mutex_, clock_}, + debug_stop_pose_with_info_{&stop_pose_} { LaneDepartureChecker lane_departure_checker{}; lane_departure_checker.setVehicleInfo(vehicle_info_); @@ -275,6 +276,10 @@ void GoalPlannerModule::onTimer() void GoalPlannerModule::onFreespaceParkingTimer() { + if (getCurrentStatus() == ModuleStatus::IDLE) { + return; + } + if (!planner_data_) { return; } @@ -619,7 +624,9 @@ bool GoalPlannerModule::canReturnToLaneParking() if ( parameters_->use_object_recognition && - checkObjectsCollision(path, parameters_->object_recognition_collision_check_margin)) { + checkObjectsCollision( + path, parameters_->object_recognition_collision_check_margin, + /*extract_static_objects=*/false)) { return false; } @@ -706,7 +713,9 @@ std::vector GoalPlannerModule::sortPullOverPathCandidatesByGoalPri 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)) { + if (!checkObjectsCollision( + resampled_path, margin * scale_factor, + /*extract_static_objects=*/true)) { return margin * scale_factor; } } @@ -766,8 +775,10 @@ std::optional> GoalPlannerModule::selectP 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*/)) { + parameters_->use_object_recognition && checkObjectsCollision( + resampled_path, collision_check_margin, + /*extract_static_objects=*/true, + /*update_debug_data=*/true)) { continue; } @@ -801,7 +812,9 @@ void GoalPlannerModule::setOutput(BehaviorModuleOutput & output) const if (!thread_safe_data_.foundPullOverPath()) { // situation : not safe against static objects use stop_path - setStopPath(output); + output.path = generateStopPath(); + RCLCPP_WARN_THROTTLE( + getLogger(), *clock_, 5000, "Not found safe pull_over path, generate stop path"); setDrivableAreaInfo(output); return; } @@ -811,7 +824,11 @@ void GoalPlannerModule::setOutput(BehaviorModuleOutput & output) const // situation : not safe against dynamic objects after approval // insert stop point in current path if ego is able to stop with acceleration and jerk // constraints - setStopPathFromCurrentPath(output); + output.path = + generateFeasibleStopPath(thread_safe_data_.get_pull_over_path()->getCurrentPath()); + RCLCPP_WARN_THROTTLE( + getLogger(), *clock_, 5000, "Not safe against dynamic objects, generate stop path"); + debug_stop_pose_with_info_.set(std::string("feasible stop after approval")); } else { // situation : (safe against static and dynamic objects) or (safe against static objects and // before approval) don't stop @@ -831,50 +848,6 @@ void GoalPlannerModule::setOutput(BehaviorModuleOutput & output) const } } -void GoalPlannerModule::setStopPath(BehaviorModuleOutput & output) const -{ - if (prev_data_.found_path || !prev_data_.stop_path) { - // safe -> not_safe or no prev_stop_path: generate new stop_path - output.path = 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 = *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( - getLogger(), *clock_, 5000, "Not found safe pull_over path, use previous stop path"); - } -} - -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 (prev_data_.safety_status.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::parking_departure::generateFeasibleStopPath( - current_path, planner_data_, stop_pose_, parameters_->maximum_deceleration_for_stop, - parameters_->maximum_jerk_for_stop); - if (stop_path) { - output.path = *stop_path; - RCLCPP_WARN_THROTTLE(getLogger(), *clock_, 5000, "Collision detected, generate stop path"); - } else { - output.path = thread_safe_data_.get_pull_over_path()->getCurrentPath(); - RCLCPP_WARN_THROTTLE( - getLogger(), *clock_, 5000, - "Collision detected, no feasible stop path found, cannot stop."); - } - } else { - // not_safe safe(no feasible stop path found) -> not_safe: use previous stop path - 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"); - } -} - void GoalPlannerModule::setDrivableAreaInfo(BehaviorModuleOutput & output) const { if (thread_safe_data_.getPullOverPlannerType() == PullOverPlannerType::FREESPACE) { @@ -936,36 +909,115 @@ 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; + const bool enable_safety_check = parameters_->safety_check_params.enable_safety_check; + // 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; + if (enable_safety_check && !isSafePath().first && !isActivated()) { + 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, /*extract_static_objects=*/false)) { + RCLCPP_DEBUG( + getLogger(), + "[DecidingPathStatus]: DECIDING->NOT_DECIDED. path has collision with objects"); + return {DecidingPathStatus::NOT_DECIDED, clock_->now()}; + } + + if (enable_safety_check && !isSafePath().first) { + RCLCPP_DEBUG( + getLogger(), + "[DecidingPathStatus]: DECIDING->NOT_DECIDED. path is not safe against dynamic 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() @@ -1014,6 +1066,8 @@ BehaviorModuleOutput GoalPlannerModule::planPullOverAsCandidate() return output; } + setDebugData(); + return output; } @@ -1024,10 +1078,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(); @@ -1081,7 +1136,7 @@ BehaviorModuleOutput GoalPlannerModule::planPullOverAsOutput() path_candidate_ = std::make_shared(thread_safe_data_.get_pull_over_path()->getFullPath()); - updatePreviousData(output); + updatePreviousData(); return output; } @@ -1108,17 +1163,13 @@ void GoalPlannerModule::postProcess() setStopReason(StopReason::GOAL_PLANNER, thread_safe_data_.get_pull_over_path()->getFullPath()); } -void GoalPlannerModule::updatePreviousData(const BehaviorModuleOutput & output) +void GoalPlannerModule::updatePreviousData() { - if (prev_data_.found_path || !prev_data_.stop_path) { - prev_data_.stop_path = std::make_shared(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. 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) { @@ -1137,17 +1188,6 @@ void GoalPlannerModule::updatePreviousData(const BehaviorModuleOutput & output) prev_data_.safety_status.safe_start_time = std::nullopt; } prev_data_.safety_status.is_safe = is_safe; - - // If safety check is enabled, save current path as stop_path_after_approval - // This path is set only once after approval. - if (!isActivated() || (!is_safe && prev_data_.stop_path_after_approval)) { - return; - } - auto stop_path = std::make_shared(output.path); - for (auto & point : stop_path->points) { - point.point.longitudinal_velocity_mps = 0.0; - } - prev_data_.stop_path_after_approval = stop_path; } BehaviorModuleOutput GoalPlannerModule::planWaitingApproval() @@ -1238,37 +1278,47 @@ PathWithLaneId GoalPlannerModule::generateStopPath() const // (In the case of the curve lane, the position is not aligned due to the // difference between the outer and inner sides) // 4. feasible stop - const auto stop_pose = std::invoke([&]() -> std::optional { - if (thread_safe_data_.foundPullOverPath()) { - return thread_safe_data_.get_pull_over_path()->start_pose; - } - if (thread_safe_data_.get_closest_start_pose()) { - return thread_safe_data_.get_closest_start_pose().value(); - } - if (!decel_pose) { - return std::nullopt; - } - return decel_pose.value(); - }); - if (!stop_pose) { - return generateFeasibleStopPath(); + const auto stop_pose_with_info = + std::invoke([&]() -> std::optional> { + if (thread_safe_data_.foundPullOverPath()) { + return std::make_pair( + thread_safe_data_.get_pull_over_path()->start_pose, "stop at selected start pose"); + } + if (thread_safe_data_.get_closest_start_pose()) { + return std::make_pair( + thread_safe_data_.get_closest_start_pose().value(), "stop at closest start pose"); + } + if (!decel_pose) { + return std::nullopt; + } + return std::make_pair(decel_pose.value(), "stop at search start pose"); + }); + if (!stop_pose_with_info) { + const auto feasible_stop_path = generateFeasibleStopPath(getPreviousModuleOutput().path); + // override stop pose info debug string + debug_stop_pose_with_info_.set(std::string("feasible stop: not calculate stop pose")); + return feasible_stop_path; } + const Pose stop_pose = stop_pose_with_info->first; // if stop pose is closer than min_stop_distance, stop as soon as possible - const double ego_to_stop_distance = calcSignedArcLengthFromEgo(extended_prev_path, *stop_pose); + const double ego_to_stop_distance = calcSignedArcLengthFromEgo(extended_prev_path, stop_pose); const auto min_stop_distance = calcFeasibleDecelDistance( planner_data_, parameters_->maximum_deceleration, parameters_->maximum_jerk, 0.0); const double eps_vel = 0.01; const bool is_stopped = std::abs(current_vel) < eps_vel; const double buffer = is_stopped ? stop_distance_buffer_ : 0.0; if (min_stop_distance && ego_to_stop_distance + buffer < *min_stop_distance) { - return generateFeasibleStopPath(); + const auto feasible_stop_path = generateFeasibleStopPath(getPreviousModuleOutput().path); + debug_stop_pose_with_info_.set( + std::string("feasible stop: stop pose is closer than min_stop_distance")); + return feasible_stop_path; } // slow down for turn signal, insert stop point to stop_pose auto stop_path = extended_prev_path; - decelerateForTurnSignal(*stop_pose, stop_path); - stop_pose_ = *stop_pose; // for debug wall marker + decelerateForTurnSignal(stop_pose, stop_path); + debug_stop_pose_with_info_.set(stop_pose, stop_pose_with_info->second); // slow down before the search area. if (decel_pose) { @@ -1291,22 +1341,22 @@ PathWithLaneId GoalPlannerModule::generateStopPath() const return stop_path; } -PathWithLaneId GoalPlannerModule::generateFeasibleStopPath() const +PathWithLaneId GoalPlannerModule::generateFeasibleStopPath(const PathWithLaneId & path) const { // calc minimum stop distance under maximum deceleration const auto min_stop_distance = calcFeasibleDecelDistance( planner_data_, parameters_->maximum_deceleration, parameters_->maximum_jerk, 0.0); if (!min_stop_distance) { - return getPreviousModuleOutput().path; + return path; } // set stop point - auto stop_path = getPreviousModuleOutput().path; + auto stop_path = path; const auto & current_pose = planner_data_->self_odometry->pose.pose; const auto stop_idx = motion_utils::insertStopPoint(current_pose, *min_stop_distance, stop_path.points); if (stop_idx) { - stop_pose_ = stop_path.points.at(*stop_idx).point.pose; + debug_stop_pose_with_info_.set(stop_path.points.at(*stop_idx).point.pose, "feasible stop"); } return stop_path; @@ -1369,7 +1419,7 @@ bool GoalPlannerModule::isStuck() parameters_->use_object_recognition && checkObjectsCollision( thread_safe_data_.get_pull_over_path()->getCurrentPath(), - parameters_->object_recognition_collision_check_margin)) { + /*extract_static_objects=*/false, parameters_->object_recognition_collision_check_margin)) { return true; } @@ -1482,19 +1532,24 @@ bool GoalPlannerModule::checkOccupancyGridCollision(const PathWithLaneId & path) 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); - const auto [pull_over_lane_objects, others] = - utils::path_safety_checker::separateObjectsByLanelets( - *(planner_data_->dynamic_object), pull_over_lanes, - utils::path_safety_checker::isPolygonOverlapLanelet); - const auto pull_over_lane_stop_objects = utils::path_safety_checker::filterObjectsByVelocity( - pull_over_lane_objects, parameters_->th_moving_object_velocity); + const bool extract_static_objects, const bool update_debug_data) const +{ + const auto target_objects = std::invoke([&]() { + const auto & p = parameters_; + const auto & rh = *(planner_data_->route_handler); + const auto objects = *(planner_data_->dynamic_object); + if (extract_static_objects) { + return goal_planner_utils::extractStaticObjectsInExpandedPullOverLanes( + rh, left_side_parking_, p->backward_goal_search_length, p->forward_goal_search_length, + p->detection_bound_offset, objects, p->th_moving_object_velocity); + } + return goal_planner_utils::extractObjectsInExpandedPullOverLanes( + rh, left_side_parking_, p->backward_goal_search_length, p->forward_goal_search_length, + p->detection_bound_offset, objects); + }); + std::vector obj_polygons; - for (const auto & object : pull_over_lane_stop_objects.objects) { + for (const auto & object : target_objects.objects) { obj_polygons.push_back(tier4_autoware_utils::toPolygon2d(object)); } @@ -2080,6 +2135,20 @@ void GoalPlannerModule::setDebugData() planner_type_marker_array.markers.push_back(marker); add(planner_type_marker_array); } + + // Visualize stop pose info + if (debug_stop_pose_with_info_.pose->has_value()) { + visualization_msgs::msg::MarkerArray stop_pose_marker_array{}; + const auto color = createMarkerColor(1.0, 1.0, 1.0, 0.99); + auto marker = createDefaultMarker( + header.frame_id, header.stamp, "stop_pose_info", 0, + visualization_msgs::msg::Marker::TEXT_VIEW_FACING, createMarkerScale(0.0, 0.0, 0.5), color); + marker.pose = debug_stop_pose_with_info_.pose->value(); + marker.text = debug_stop_pose_with_info_.string; + stop_pose_marker_array.markers.push_back(marker); + add(stop_pose_marker_array); + add(createPoseMarkerArray(marker.pose, "stop_pose", 1.0, 1.0, 1.0, 0.9)); + } } void GoalPlannerModule::printParkingPositionError() const 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 31f99e8ea5aa1..e2d12dd82cbe4 100644 --- a/planning/behavior_path_goal_planner_module/src/goal_searcher.cpp +++ b/planning/behavior_path_goal_planner_module/src/goal_searcher.cpp @@ -266,14 +266,11 @@ void GoalSearcher::countObjectsToAvoid( void GoalSearcher::update(GoalCandidates & goal_candidates) const { - const auto stop_objects = utils::path_safety_checker::filterObjectsByVelocity( - *(planner_data_->dynamic_object), parameters_.th_moving_object_velocity); - 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); - const auto [pull_over_lane_stop_objects, others] = - utils::path_safety_checker::separateObjectsByLanelets( - stop_objects, pull_over_lanes, utils::path_safety_checker::isPolygonOverlapLanelet); + 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); if (parameters_.prioritize_goals_before_objects) { countObjectsToAvoid(goal_candidates, pull_over_lane_stop_objects); @@ -315,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) { diff --git a/planning/behavior_path_goal_planner_module/src/manager.cpp b/planning/behavior_path_goal_planner_module/src/manager.cpp index 8608d6cce2c81..c6fe97874f012 100644 --- a/planning/behavior_path_goal_planner_module/src/manager.cpp +++ b/planning/behavior_path_goal_planner_module/src/manager.cpp @@ -101,6 +101,7 @@ void GoalPlannerModuleManager::init(rclcpp::Node * node) node->declare_parameter( ns + "object_recognition_collision_check_max_extra_stopping_margin"); p.th_moving_object_velocity = node->declare_parameter(ns + "th_moving_object_velocity"); + p.detection_bound_offset = node->declare_parameter(ns + "detection_bound_offset"); } // pull over general params diff --git a/planning/behavior_path_goal_planner_module/src/util.cpp b/planning/behavior_path_goal_planner_module/src/util.cpp index dfdaa2a3a18c1..8ad90d26f58f2 100644 --- a/planning/behavior_path_goal_planner_module/src/util.cpp +++ b/planning/behavior_path_goal_planner_module/src/util.cpp @@ -14,6 +14,7 @@ #include "behavior_path_goal_planner_module/util.hpp" +#include "behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" #include "behavior_path_planner_common/utils/utils.hpp" #include @@ -41,6 +42,7 @@ using tier4_autoware_utils::createDefaultMarker; using tier4_autoware_utils::createMarkerColor; using tier4_autoware_utils::createMarkerScale; using tier4_autoware_utils::createPoint; + lanelet::ConstLanelets getPullOverLanes( const RouteHandler & route_handler, const bool left_side, const double backward_distance, const double forward_distance) @@ -74,6 +76,41 @@ lanelet::ConstLanelets getPullOverLanes( outermost_lane, backward_distance_with_buffer, forward_distance, only_route_lanes); } +lanelet::ConstLanelets generateExpandedPullOverLanes( + const RouteHandler & route_handler, const bool left_side, const double backward_distance, + const double forward_distance, double bound_offset) +{ + const auto pull_over_lanes = + getPullOverLanes(route_handler, left_side, backward_distance, forward_distance); + + return left_side ? lanelet::utils::getExpandedLanelets(pull_over_lanes, bound_offset, 0.0) + : lanelet::utils::getExpandedLanelets(pull_over_lanes, 0.0, bound_offset); +} + +PredictedObjects extractObjectsInExpandedPullOverLanes( + const RouteHandler & route_handler, const bool left_side, const double backward_distance, + const double forward_distance, double bound_offset, const PredictedObjects & objects) +{ + const auto lanes = generateExpandedPullOverLanes( + route_handler, left_side, backward_distance, forward_distance, bound_offset); + + const auto [objects_in_lanes, others] = utils::path_safety_checker::separateObjectsByLanelets( + objects, lanes, utils::path_safety_checker::isPolygonOverlapLanelet); + + return objects_in_lanes; +} + +PredictedObjects extractStaticObjectsInExpandedPullOverLanes( + const RouteHandler & route_handler, const bool left_side, const double backward_distance, + const double forward_distance, double bound_offset, const PredictedObjects & objects, + const double velocity_thresh) +{ + const auto objects_in_lanes = extractObjectsInExpandedPullOverLanes( + route_handler, left_side, backward_distance, forward_distance, bound_offset, objects); + + return utils::path_safety_checker::filterObjectsByVelocity(objects_in_lanes, velocity_thresh); +} + PredictedObjects filterObjectsByLateralDistance( const Pose & ego_pose, const double vehicle_width, const PredictedObjects & objects, const double distance_thresh, const bool filter_inside) diff --git a/planning/behavior_path_planner/src/planner_manager.cpp b/planning/behavior_path_planner/src/planner_manager.cpp index cbcec2e3095d3..4caf5683f3982 100644 --- a/planning/behavior_path_planner/src/planner_manager.cpp +++ b/planning/behavior_path_planner/src/planner_manager.cpp @@ -630,32 +630,38 @@ BehaviorModuleOutput PlannerManager::runApprovedModules(const std::shared_ptrisKeepLast(); + const auto get_sorted_keep_last_modules = [this](const auto & modules) { + std::vector keep_last_modules; + + std::copy_if( + modules.begin(), modules.end(), std::back_inserter(keep_last_modules), + [this](const auto & m) { return getManager(m)->isKeepLast(); }); + + // sort by priority (low -> high) + std::sort( + keep_last_modules.begin(), keep_last_modules.end(), [this](const auto & a, const auto & b) { + return getManager(a)->getPriority() < getManager(b)->getPriority(); + }); + + return keep_last_modules; }; - move_to_end(approved_module_ptrs_, keep_last_module_cond); + + for (const auto & module : get_sorted_keep_last_modules(approved_module_ptrs_)) { + move_to_end(approved_module_ptrs_, module); + } } // lock approved modules besides last one @@ -768,6 +774,25 @@ BehaviorModuleOutput PlannerManager::runApprovedModules(const std::shared_ptrgetCurrentStatus() == ModuleStatus::SUCCESS; }; diff --git a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp index 9ad7f72d6af4e..cbcb8c2e03f8f 100644 --- a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp +++ b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp @@ -85,6 +85,10 @@ StartPlannerModule::StartPlannerModule( void StartPlannerModule::onFreespacePlannerTimer() { + if (getCurrentStatus() == ModuleStatus::IDLE) { + return; + } + if (!planner_data_) { return; } @@ -95,7 +99,11 @@ void StartPlannerModule::onFreespacePlannerTimer() const bool is_new_costmap = (clock_->now() - planner_data_->costmap->header.stamp).seconds() < 1.0; - if (isStuck() && is_new_costmap) { + if (!is_new_costmap) { + return; + } + + if (isStuck()) { planFreespacePath(); } }