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 6ba90f54e48fe..a6f9565c08aae 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 @@ -81,13 +81,16 @@ struct PullOverStatus size_t current_path_idx{0}; bool require_increment_{true}; // if false, keep current path idx. std::shared_ptr prev_stop_path{nullptr}; + std::shared_ptr prev_stop_path_after_approval{nullptr}; + // stop path after approval, stop path is not updated until safety is confirmed lanelet::ConstLanelets current_lanes{}; // TODO(someone): explain lanelet::ConstLanelets pull_over_lanes{}; // TODO(someone): explain std::vector lanes{}; // current + pull_over - bool has_decided_path{false}; // if true, the path is decided and safe against static objects + bool has_decided_path{false}; // if true, the path has is decided and safe against static objects bool is_safe_static_objects{false}; // current path is safe against *static* objects bool is_safe_dynamic_objects{false}; // current path is safe against *dynamic* objects bool prev_is_safe{false}; + bool prev_is_safe_dynamic_objects{false}; bool has_decided_velocity{false}; bool has_requested_approval{false}; bool is_ready{false}; @@ -230,6 +233,18 @@ class GoalPlannerModule : public SceneModuleInterface const Pose & search_start_offset_pose, PathWithLaneId & path) const; PathWithLaneId generateStopPath(); PathWithLaneId generateFeasibleStopPath(); + + /** + * @brief Generate a stop point in the current path based on the vehicle's pose and constraints. + * + * This function generates a stop point in the current path. If no lanes are available or if + * stopping is not feasible due to constraints (maximum deceleration, maximum jerk), no stop point + * is inserted into the path. + * + * @return the modified path with the stop point inserted. If no feasible stop point can be + * determined, returns an empty optional. + */ + std::optional generateStopInsertedCurrentPath(); void keepStoppedWithCurrentPath(PathWithLaneId & path); double calcSignedArcLengthFromEgo(const PathWithLaneId & path, const Pose & pose) const; @@ -277,6 +292,17 @@ class GoalPlannerModule : public SceneModuleInterface // output setter void setOutput(BehaviorModuleOutput & output); void setStopPath(BehaviorModuleOutput & output); + + /** + * @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); void setModifiedGoal(BehaviorModuleOutput & output) const; void setTurnSignalInfo(BehaviorModuleOutput & output) 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 54a052c91f180..12f6f243e9a9c 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 @@ -664,18 +664,25 @@ void GoalPlannerModule::setLanes() void GoalPlannerModule::setOutput(BehaviorModuleOutput & output) { if (status_.is_safe_static_objects) { - // clear stop pose when the path is safe and activated - if (isActivated()) { - resetWallPoses(); + if (isSafePath()) { + // clear stop pose when the path is safe against static/dynamic objects and activated + if (isActivated()) { + resetWallPoses(); + } + // keep stop if not enough time passed, + // because it takes time for the trajectory to be reflected + auto current_path = getCurrentPath(); + keepStoppedWithCurrentPath(current_path); + + output.path = std::make_shared(current_path); + output.reference_path = getPreviousModuleOutput().reference_path; + } else if (status_.has_decided_path && isActivated()) { + // 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); } - // keep stop if not enough time passed, - // because it takes time for the trajectory to be reflected - auto current_path = getCurrentPath(); - keepStoppedWithCurrentPath(current_path); - - output.path = std::make_shared(current_path); - output.reference_path = getPreviousModuleOutput().reference_path; } else { // not safe: use stop_path setStopPath(output); @@ -693,6 +700,7 @@ void GoalPlannerModule::setOutput(BehaviorModuleOutput & output) // for the next loop setOutput(). // this is used to determine whether to generate a new stop path or keep the current stop path. status_.prev_is_safe = status_.is_safe_static_objects; + status_.prev_is_safe_dynamic_objects = status_.is_safe_dynamic_objects; } void GoalPlannerModule::setStopPath(BehaviorModuleOutput & output) @@ -720,6 +728,30 @@ void GoalPlannerModule::setStopPath(BehaviorModuleOutput & output) output.reference_path = getPreviousModuleOutput().reference_path; } +void GoalPlannerModule::setStopPathFromCurrentPath(BehaviorModuleOutput & output) +{ + // safe or not safe(no feasible stop_path found) -> not_safe: try to find new feasible stop_path + if (status_.prev_is_safe_dynamic_objects || status_.prev_stop_path_after_approval == nullptr) { + if (const auto stop_inserted_path = generateStopInsertedCurrentPath()) { + output.path = std::make_shared(*stop_inserted_path); + status_.prev_stop_path_after_approval = output.path; + RCLCPP_WARN_THROTTLE(getLogger(), *clock_, 5000, "Collision detected, generate stop path"); + } else { + output.path = std::make_shared(getCurrentPath()); + RCLCPP_WARN_THROTTLE( + getLogger(), *clock_, 5000, + "Collision detected, no feasible stop path found, cannot stop."); + } + std::lock_guard lock(mutex_); + last_path_update_time_ = std::make_unique(clock_->now()); + } else { + // not_safe safe(no feasible stop path found) -> not_safe: use previous stop path + output.path = status_.prev_stop_path_after_approval; + RCLCPP_WARN_THROTTLE(getLogger(), *clock_, 5000, "Collision detected, use previous stop path"); + } + output.reference_path = getPreviousModuleOutput().reference_path; +} + void GoalPlannerModule::setDrivableAreaInfo(BehaviorModuleOutput & output) const { if (status_.pull_over_path->type == PullOverPlannerType::FREESPACE) { @@ -1079,6 +1111,34 @@ PathWithLaneId GoalPlannerModule::generateFeasibleStopPath() return stop_path; } +std::optional GoalPlannerModule::generateStopInsertedCurrentPath() +{ + auto current_path = getCurrentPath(); + if (current_path.points.empty()) { + return {}; + } + + // try to insert stop point in current_path after approval + // but if can't stop with constraints(maximum deceleration, maximum jerk), don't insert stop point + const auto min_stop_distance = calcFeasibleDecelDistance( + planner_data_, parameters_->maximum_deceleration, parameters_->maximum_jerk, 0.0); + if (!min_stop_distance) { + return {}; + } + + // set stop point + const auto stop_idx = motion_utils::insertStopPoint( + planner_data_->self_odometry->pose.pose, *min_stop_distance, current_path.points); + + if (!stop_idx) { + return {}; + } else { + stop_pose_ = current_path.points.at(*stop_idx).point.pose; + } + + return current_path; +} + void GoalPlannerModule::transitionToNextPathIfFinishingCurrentPath() { if (isActivated() && last_approved_time_ != nullptr) {