From 948c4bb1c7fc08d924154b8ac82d08628896786b Mon Sep 17 00:00:00 2001
From: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>
Date: Thu, 21 Sep 2023 23:29:41 +0900
Subject: [PATCH] update

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>
---
 .../start_planner/start_planner_module.cpp    | 28 ++++++++++++++++---
 .../utils/start_goal_planner_common/utils.cpp |  3 ++
 2 files changed, 27 insertions(+), 4 deletions(-)

diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp
index 48c8d296203ce..71222c74c9407 100644
--- a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp
+++ b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp
@@ -193,8 +193,12 @@ ModuleStatus StartPlannerModule::updateState()
   // check safety status after back finished
   if (parameters_->safety_check_params.enable_safety_check && status_.back_finished) {
     status_.is_safe_dynamic_objects = isSafePath();
+    std::cerr << "status_.is_safe_dynamic_objects in updateState after checking safety: "
+              << status_.is_safe_dynamic_objects << std::endl;
   } else {
     status_.is_safe_dynamic_objects = true;
+    std::cerr << "status_.is_safe_dynamic_objects in updateState: "
+              << status_.is_safe_dynamic_objects << std::endl;
   }
 
   checkBackFinished();
@@ -238,12 +242,22 @@ BehaviorModuleOutput StartPlannerModule::plan()
       incrementPathIndex();
     }
 
-    std::cerr << "\n\n\n\n\n status_.is_safe_dynamic_objects: " << status_.is_safe_dynamic_objects
-              << "\n\n\n\n\n\n\n"
+    std::cerr << "status_.is_safe_dynamic_objects: " << status_.is_safe_dynamic_objects
               << std::endl;
 
-    std::cerr << "\n\n\n\n\n current_state_ is running: "
-              << (current_state_ == ModuleStatus::RUNNING) << "\n\n\n\n\n\n\n"
+    std::cerr << "current_state_ is running: " << (current_state_ == ModuleStatus::RUNNING)
+              << std::endl;
+
+    std::cerr << "status_.prev_is_safe_dynamic_objects: " << status_.prev_is_safe_dynamic_objects
+              << std::endl;
+    std::cerr << "status_.prev_stop_path_after_approval == nullptr: "
+              << (status_.prev_stop_path_after_approval == nullptr) << std::endl;
+    std::cerr << " !status_.is_safe_dynamic_objects && (current_state_ == ModuleStatus::RUNNING) "
+                 "&& (status_.prev_is_safe_dynamic_objects || "
+                 "status_.prev_stop_path_after_approval == nullptr)"
+              << (!status_.is_safe_dynamic_objects && (current_state_ == ModuleStatus::RUNNING) &&
+                  (status_.prev_is_safe_dynamic_objects ||
+                   status_.prev_stop_path_after_approval == nullptr))
               << std::endl;
     if (
       !status_.is_safe_dynamic_objects && (current_state_ == ModuleStatus::RUNNING) &&
@@ -253,12 +267,18 @@ BehaviorModuleOutput StartPlannerModule::plan()
         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);
+      if (stop_path) {
+        RCLCPP_ERROR_THROTTLE(
+          getLogger(), *clock_, 5000, "Insert stop point in the path because of dynamic objects");
+      }
+
       status_.prev_stop_path_after_approval =
         stop_path ? std::make_shared<PathWithLaneId>(*stop_path) : nullptr;
       path = stop_path ? *status_.prev_stop_path_after_approval : current_path;
     } else {
       path = getCurrentPath();
     }
+    status_.prev_is_safe_dynamic_objects = status_.is_safe_dynamic_objects;
   } else {
     path = status_.backward_path;
   }
diff --git a/planning/behavior_path_planner/src/utils/start_goal_planner_common/utils.cpp b/planning/behavior_path_planner/src/utils/start_goal_planner_common/utils.cpp
index d52cebc6ce5eb..77f53d7abf8cd 100644
--- a/planning/behavior_path_planner/src/utils/start_goal_planner_common/utils.cpp
+++ b/planning/behavior_path_planner/src/utils/start_goal_planner_common/utils.cpp
@@ -188,6 +188,7 @@ std::optional<PathWithLaneId> generateFeasibleStopPath(
   const double maximum_jerk)
 {
   if (current_path.points.empty()) {
+    std::cerr << "current_path.points is empty" << std::endl;
     return {};
   }
 
@@ -198,6 +199,7 @@ std::optional<PathWithLaneId> generateFeasibleStopPath(
       planner_data, maximum_deceleration, maximum_jerk, 0.0);
 
   if (!min_stop_distance) {
+    std::cerr << "min_stop_distance is invalid" << std::endl;
     return {};
   }
 
@@ -206,6 +208,7 @@ std::optional<PathWithLaneId> generateFeasibleStopPath(
     planner_data->self_odometry->pose.pose, *min_stop_distance, current_path.points);
 
   if (!stop_idx) {
+    std::cerr << "stop_idx is invalid" << std::endl;
     return {};
   }