fix(start_planner): check safety only when waiting approval (autoware…

…foundation#5792)

1. The `updateData()` function now sets `status_.is_safe_dynamic_objects` to true when `requiresDynamicObjectsCollisionDetection()` returns false.

2. The `isExecutionReady()` function now checks for dynamic object collisions only if `requiresDynamicObjectsCollisionDetection()` returns true and `isWaitingApproval()` also returns true. This change ensures that dynamic object collision detection is performed only when necessary and approval is pending.

Signed-off-by: kyoichi-sugahara <[email protected]>

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

@@ -146,6 +146,8 @@ void StartPlannerModule::updateData()
 
   if (requiresDynamicObjectsCollisionDetection()) {
     status_.is_safe_dynamic_objects = !hasCollisionWithDynamicObjects();
+  } else {
+    status_.is_safe_dynamic_objects = true;
   }
 }
 
@@ -271,20 +273,21 @@ bool StartPlannerModule::isStopped()
 bool StartPlannerModule::isExecutionReady() const
 {
   bool is_safe = true;
-
   // Evaluate safety. The situation is not safe if any of the following conditions are met:
   // 1. pull out path has not been found
   // 2. waiting for approval and there is a collision with dynamic objects
   if (!status_.found_pull_out_path) {
     is_safe = false;
+  } else if (
+    isWaitingApproval() && requiresDynamicObjectsCollisionDetection() &&
+    hasCollisionWithDynamicObjects()) {
+    is_safe = false;
   }
 
-  if (requiresDynamicObjectsCollisionDetection()) {
-    is_safe = !hasCollisionWithDynamicObjects();
+  if (!is_safe) {
+    stop_pose_ = planner_data_->self_odometry->pose.pose;
   }
 
-  if (!is_safe) stop_pose_ = planner_data_->self_odometry->pose.pose;
-
   return is_safe;
 }