From 38f1f88d072418e423b9edc66a88333b00f6dd81 Mon Sep 17 00:00:00 2001 From: kosuke55 Date: Fri, 19 Jan 2024 16:43:49 +0900 Subject: [PATCH] fix(goal_planner): fix sudden stop and simpify process Signed-off-by: kosuke55 --- .../goal_planner_module.hpp | 21 +---- .../src/goal_planner_module.cpp | 91 +++---------------- 2 files changed, 18 insertions(+), 94 deletions(-) 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 a6ebaf00aed78..56ca29543fcec 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 @@ -240,15 +240,11 @@ struct PreviousPullOverData void reset() { - stop_path = nullptr; - stop_path_after_approval = nullptr; found_path = false; safety_status = SafetyStatus{}; has_decided_path = false; } - 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}; @@ -387,7 +383,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. @@ -428,7 +424,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; @@ -477,19 +473,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/src/goal_planner_module.cpp b/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp index fa6a44b97a623..83ac671b40f5d 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 @@ -806,7 +806,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; } @@ -816,7 +818,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 @@ -836,58 +842,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; - RCLCPP_WARN_THROTTLE( - getLogger(), *clock_, 5000, "Not found safe pull_over path, use previous stop path"); - // stop_pose_ is removed in manager every loop, so need to set every loop. - const auto stop_pose_opt = utils::getFirstStopPoseFromPath(output.path); - if (stop_pose_opt) { - debug_stop_pose_with_info_.set(stop_pose_opt.value(), std::string("keep previous stop pose")); - } - } -} - -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; - debug_stop_pose_with_info_.set(std::string("feasible stop after approval")); - 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. - const auto stop_pose_opt = utils::getFirstStopPoseFromPath(output.path); - if (stop_pose_opt) { - debug_stop_pose_with_info_.set( - stop_pose_opt.value(), std::string("keep feasible stop after approval")); - } - 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) { @@ -1096,7 +1050,7 @@ BehaviorModuleOutput GoalPlannerModule::planPullOverAsOutput() path_candidate_ = std::make_shared(thread_safe_data_.get_pull_over_path()->getFullPath()); - updatePreviousData(output); + updatePreviousData(); return output; } @@ -1123,12 +1077,8 @@ 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(); @@ -1152,17 +1102,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() @@ -1269,7 +1208,7 @@ PathWithLaneId GoalPlannerModule::generateStopPath() const return std::make_pair(decel_pose.value(), "stop at search start pose"); }); if (!stop_pose_with_info) { - const auto feasible_stop_path = generateFeasibleStopPath(); + 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; @@ -1284,7 +1223,7 @@ PathWithLaneId GoalPlannerModule::generateStopPath() const 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) { - const auto feasible_stop_path = 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; @@ -1316,17 +1255,17 @@ 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);