feat(behavior_path_planner): safety check against dynamic objects aft…

…er approval in manual mode (#4927)

* add feature of stop after approval for goal_planner

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

* insert stop point after approval

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

---------

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>

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<PathWithLaneId> prev_stop_path{nullptr};
+  std::shared_ptr<PathWithLaneId> 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<DrivableLanes> 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<PathWithLaneId> 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;
 

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<PathWithLaneId>(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<PathWithLaneId>(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<PathWithLaneId>(*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<PathWithLaneId>(getCurrentPath());
+      RCLCPP_WARN_THROTTLE(
+        getLogger(), *clock_, 5000,
+        "Collision detected, no feasible stop path found, cannot stop.");
+    }
+    std::lock_guard<std::mutex> lock(mutex_);
+    last_path_update_time_ = std::make_unique<rclcpp::Time>(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<PathWithLaneId> 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) {