feat(behavior_path_planner): safety check against dynamic objects after approval in manual mode

planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp

@@ -81,13 +81,15 @@ struct PullOverStatus
   size_t current_path_idx{0};
   bool require_increment_{true};  // if false, keep current path idx.
   std::shared_ptr<PathWithLaneId> prev_stop_path{nullptr};
+  std::shared_ptr<PathWithLaneId> prev_stop_path_after_approval{nullptr};
   lanelet::ConstLanelets current_lanes{};
   lanelet::ConstLanelets pull_over_lanes{};
   std::vector<DrivableLanes> lanes{};  // current + pull_over
   bool has_decided_path{false};
   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 +232,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 PathWithLaneId The modified path with the stop point inserted. If no stop point is
+   * inserted, returns the original path.
+   */
+  PathWithLaneId generateStopPointInCurrentPath();
   void keepStoppedWithCurrentPath(PathWithLaneId & path);
   double calcSignedArcLengthFromEgo(const PathWithLaneId & path, const Pose & pose) const;
 
@@ -277,6 +291,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 setStopPathInCurrentPath(BehaviorModuleOutput & output);
   void setModifiedGoal(BehaviorModuleOutput & output) const;
   void setTurnSignalInfo(BehaviorModuleOutput & output) const;
 
@@ -299,6 +324,7 @@ class GoalPlannerModule : public SceneModuleInterface
   std::pair<double, double> calcDistanceToPathChange() const;
 
   // safety check
+  void initializeSafetyCheckParameters();
   SafetyCheckParams createSafetyCheckParams() const;
   void updateSafetyCheckTargetObjectsData(
     const PredictedObjects & filtered_objects, const TargetObjectsOnLane & target_objects_on_lane,

planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp

@@ -253,12 +253,25 @@ void GoalPlannerModule::initializeOccupancyGridMap()
   occupancy_grid_map_->setParam(occupancy_grid_map_param);
 }
 
+void GoalPlannerModule::initializeSafetyCheckParameters()
+{
+  utils::start_goal_planner_common::updateEgoPredictedPathParams(
+    ego_predicted_path_params_, parameters_);
+  utils::start_goal_planner_common::updateSafetyCheckParams(safety_check_params_, parameters_);
+  utils::start_goal_planner_common::updateObjectsFilteringParams(
+    objects_filtering_params_, parameters_);
+}
+
 void GoalPlannerModule::processOnEntry()
 {
   // Initialize occupancy grid map
   if (parameters_->use_occupancy_grid) {
     initializeOccupancyGridMap();
   }
+  // Initialize safety checker
+  if (parameters_->safety_check_params.enable_safety_check) {
+    initializeSafetyCheckParameters();
+  }
 }
 
 void GoalPlannerModule::processOnExit()
@@ -355,11 +368,6 @@ bool GoalPlannerModule::isExecutionReady() const
   }
 
   if (status_.is_safe_static_objects && parameters_->safety_check_params.enable_safety_check) {
-    utils::start_goal_planner_common::updateEgoPredictedPathParams(
-      ego_predicted_path_params_, parameters_);
-    utils::start_goal_planner_common::updateSafetyCheckParams(safety_check_params_, parameters_);
-    utils::start_goal_planner_common::updateObjectsFilteringParams(
-      objects_filtering_params_, parameters_);
     if (!isSafePath()) {
       RCLCPP_ERROR_THROTTLE(getLogger(), *clock_, 5000, "Path is not safe against dynamic objects");
       return false;
@@ -655,18 +663,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);
 
-    // 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<PathWithLaneId>(current_path);
+      output.reference_path = getPreviousModuleOutput().reference_path;
+    } else {
+      // 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
+      setStopPathInCurrentPath(output);
+    }
 
-    output.path = std::make_shared<PathWithLaneId>(current_path);
-    output.reference_path = getPreviousModuleOutput().reference_path;
   } else {
     // not safe: use stop_path
     setStopPath(output);
@@ -684,6 +699,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)
@@ -712,6 +728,28 @@ void GoalPlannerModule::setStopPath(BehaviorModuleOutput & output)
   }
 }
 
+void GoalPlannerModule::setStopPathInCurrentPath(BehaviorModuleOutput & output)
+{
+  if (status_.prev_is_safe_dynamic_objects || status_.prev_stop_path_after_approval == nullptr) {
+    // safe -> not_safe or no prev_stop_path: generate new stop_path
+    output.path = std::make_shared<PathWithLaneId>(generateStopPointInCurrentPath());
+    output.reference_path = getPreviousModuleOutput().reference_path;
+    status_.prev_stop_path_after_approval = output.path;
+    // set stop path as pull over path
+    mutex_.lock();
+    last_path_update_time_ = std::make_unique<rclcpp::Time>(clock_->now());
+    mutex_.unlock();
+    RCLCPP_WARN_THROTTLE(
+      getLogger(), *clock_, 5000, "Found approved pull_over path is not safe, generate stop path");
+  } else {
+    // not_safe -> not_safe: use previous stop path
+    output.path = status_.prev_stop_path_after_approval;
+    output.reference_path = getPreviousModuleOutput().reference_path;
+    RCLCPP_WARN_THROTTLE(
+      getLogger(), *clock_, 5000, "Found approved pull_over path is not safe, generate stop path");
+  }
+}
+
 void GoalPlannerModule::setDrivableAreaInfo(BehaviorModuleOutput & output) const
 {
   if (status_.pull_over_path->type == PullOverPlannerType::FREESPACE) {
@@ -1071,6 +1109,36 @@ PathWithLaneId GoalPlannerModule::generateFeasibleStopPath()
   return stop_path;
 }
 
+PathWithLaneId GoalPlannerModule::generateStopPointInCurrentPath()
+{
+  const auto & current_pose = planner_data_->self_odometry->pose.pose;
+
+  if (status_.current_lanes.empty()) {
+    return PathWithLaneId{};
+  }
+
+  // get current path
+  auto current_path = getCurrentPath();
+
+  // 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 current_path;
+  }
+
+  // set stop point
+  const auto stop_idx =
+    motion_utils::insertStopPoint(current_pose, *min_stop_distance, current_path.points);
+
+  if (stop_idx) {
+    stop_pose_ = current_path.points.at(*stop_idx).point.pose;
+  }
+
+  return current_path;
+}
+
 void GoalPlannerModule::transitionToNextPathIfFinishingCurrentPath()
 {
   if (isActivated() && last_approved_time_ != nullptr) {
@@ -1098,6 +1166,10 @@ bool GoalPlannerModule::incrementPathIndex()
 
 PathWithLaneId GoalPlannerModule::getCurrentPath() const
 {
+  if (status_.pull_over_path == nullptr) {
+    return PathWithLaneId{};
+  }
+
   if (status_.pull_over_path->partial_paths.size() <= status_.current_path_idx) {
     return PathWithLaneId{};
   }