update

Signed-off-by: kyoichi-sugahara <[email protected]>
kyoichi-sugahara · Sep 21, 2023 · 948c4bb · 948c4bb
1 parent a25bc2a
commit 948c4bb
Show file tree

Hide file tree

Showing 2 changed files with 27 additions and 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
@@ -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
@@ -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 {};
   }