feat(behavior_path_planner): safety check against dynamic objects after approval for start_planner (#5056)

* add stop judgement after approval

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

* update param

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

* add status of has_stop_point

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

---------

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

Author: kyoichi-sugahara

planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml

@@ -132,7 +132,7 @@
           # detection range
           object_check_forward_distance: 10.0
           object_check_backward_distance: 100.0
-          ignore_object_velocity_threshold: 0.0
+          ignore_object_velocity_threshold: 1.0
           # ObjectTypesToCheck
           object_types_to_check:
             check_car: true
@@ -164,11 +164,11 @@
           check_all_predicted_path: true
           publish_debug_marker: false
           rss_params:
-            rear_vehicle_reaction_time: 1.0
+            rear_vehicle_reaction_time: 2.0
             rear_vehicle_safety_time_margin: 1.0
-            lateral_distance_max_threshold: 1.0
-            longitudinal_distance_min_threshold: 1.0
-            longitudinal_velocity_delta_time: 1.0
+            lateral_distance_max_threshold: 2.0
+            longitudinal_distance_min_threshold: 3.0
+            longitudinal_velocity_delta_time: 0.8
           # hysteresis factor to expand/shrink polygon with the value
           hysteresis_factor_expand_rate: 1.0
           # temporary

planning/behavior_path_planner/config/start_planner/start_planner.param.yaml

@@ -95,7 +95,7 @@
           # detection range
           object_check_forward_distance: 10.0
           object_check_backward_distance: 100.0
-          ignore_object_velocity_threshold: 0.0
+          ignore_object_velocity_threshold: 1.0
           # ObjectTypesToCheck
           object_types_to_check:
             check_car: true
@@ -127,11 +127,11 @@
           check_all_predicted_path: true
           publish_debug_marker: false
           rss_params:
-            rear_vehicle_reaction_time: 1.0
+            rear_vehicle_reaction_time: 2.0
             rear_vehicle_safety_time_margin: 1.0
-            lateral_distance_max_threshold: 1.0
-            longitudinal_distance_min_threshold: 1.0
-            longitudinal_velocity_delta_time: 1.0
+            lateral_distance_max_threshold: 2.0
+            longitudinal_distance_min_threshold: 3.0
+            longitudinal_velocity_delta_time: 0.8
           # hysteresis factor to expand/shrink polygon
           hysteresis_factor_expand_rate: 1.0
           # temporary

planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp

@@ -67,6 +67,9 @@ struct PullOutStatus
     false};  // after backward driving is complete, this is set to true (warning: this is set to
              // false at next cycle after backward driving is complete)
   Pose pull_out_start_pose{};
+  bool prev_is_safe_dynamic_objects{false};
+  std::shared_ptr<PathWithLaneId> prev_stop_path_after_approval{nullptr};
+  bool has_stop_point{false};
 
   PullOutStatus() {}
 };

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

@@ -365,12 +365,7 @@ bool GoalPlannerModule::isExecutionRequested() const
 
 bool GoalPlannerModule::isExecutionReady() const
 {
-  // TODO(Sugahara): should check safe or not but in the current flow, it is not possible.
-  if (status_.pull_over_path == nullptr) {
-    return true;
-  }
-
-  if (status_.is_safe_static_objects && parameters_->safety_check_params.enable_safety_check) {
+  if (parameters_->safety_check_params.enable_safety_check && isWaitingApproval()) {
     if (!isSafePath()) {
       RCLCPP_ERROR_THROTTLE(getLogger(), *clock_, 5000, "Path is not safe against dynamic objects");
       return false;

planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp

@@ -136,6 +136,12 @@ void StartPlannerModule::updateData()
   if (has_received_new_route) {
     status_ = PullOutStatus();
   }
+  // check safety status after back finished
+  if (parameters_->safety_check_params.enable_safety_check && status_.back_finished) {
+    status_.is_safe_dynamic_objects = isSafePath();
+  } else {
+    status_.is_safe_dynamic_objects = true;
+  }
 }
 
 bool StartPlannerModule::isExecutionRequested() const
@@ -179,17 +185,19 @@ bool StartPlannerModule::isExecutionRequested() const
 
 bool StartPlannerModule::isExecutionReady() const
 {
+  // when is_safe_static_objects is false,the path is not generated and approval shouldn't be
+  // allowed
   if (!status_.is_safe_static_objects) {
+    RCLCPP_ERROR_THROTTLE(getLogger(), *clock_, 5000, "Path is not safe against static objects");
     return false;
   }
 
-  if (status_.pull_out_path.partial_paths.empty()) {
-    return true;
-  }
-
-  if (status_.is_safe_static_objects && parameters_->safety_check_params.enable_safety_check) {
+  if (
+    parameters_->safety_check_params.enable_safety_check && status_.back_finished &&
+    isWaitingApproval()) {
     if (!isSafePath()) {
       RCLCPP_ERROR_THROTTLE(getLogger(), *clock_, 5000, "Path is not safe against dynamic objects");
+      stop_pose_ = planner_data_->self_odometry->pose.pose;
       return false;
     }
   }
@@ -221,6 +229,7 @@ void StartPlannerModule::updateCurrentState()
   if (status_.backward_driving_complete) {
     current_state_ = ModuleStatus::SUCCESS;  // for breaking loop
   }
+
   print(magic_enum::enum_name(from), magic_enum::enum_name(current_state_));
 }
 
@@ -252,12 +261,42 @@ BehaviorModuleOutput StartPlannerModule::plan()
   }
 
   PathWithLaneId path;
+
+  // Check if backward motion is finished
   if (status_.back_finished) {
+    // Increment path index if the current path is finished
     if (hasFinishedCurrentPath()) {
       RCLCPP_INFO(getLogger(), "Increment path index");
       incrementPathIndex();
     }
-    path = getCurrentPath();
+
+    if (!status_.is_safe_dynamic_objects && !isWaitingApproval() && !status_.has_stop_point) {
+      auto current_path = getCurrentPath();
+      const auto stop_path =
+        behavior_path_planner::utils::start_goal_planner_common::generateFeasibleStopPath(
+          current_path, planner_data_, *stop_pose_, parameters_->maximum_deceleration_for_stop,
+          parameters_->maximum_jerk_for_stop);
+
+      // Insert stop point in the path if needed
+      if (stop_path) {
+        RCLCPP_ERROR_THROTTLE(
+          getLogger(), *clock_, 5000, "Insert stop point in the path because of dynamic objects");
+        path = *stop_path;
+        status_.prev_stop_path_after_approval = std::make_shared<PathWithLaneId>(path);
+        status_.has_stop_point = true;
+      } else {
+        path = current_path;
+      }
+    } else if (!isWaitingApproval() && status_.has_stop_point) {
+      // Delete stop point if conditions are met
+      if (status_.is_safe_dynamic_objects && isStopped()) {
+        status_.has_stop_point = false;
+        path = getCurrentPath();
+      }
+      path = *status_.prev_stop_path_after_approval;
+    } else {
+      path = getCurrentPath();
+    }
   } else {
     path = status_.backward_path;
   }