fix(start_planner): start prepare blinker when autonomous mode is ava…

…ilable (autowarefoundation#6470) Signed-off-by: Mamoru Sobue <[email protected]>
tier4 · Mar 11, 2024 · df33eb0 · df33eb0
1 parent e6fbf9e
commit df33eb0
Show file tree

Hide file tree

Showing 2 changed files with 8 additions and 6 deletions.
diff --git a/..._start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp b/..._start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp
@@ -71,7 +71,7 @@ struct PullOutStatus
   bool has_stop_point{false};
   std::optional<Pose> stop_pose{std::nullopt};
   //! record the first time when the state changed from !isActivated() to isActivated()
-  std::optional<rclcpp::Time> first_approved_time{std::nullopt};
+  std::optional<rclcpp::Time> first_engaged_time{std::nullopt};
 
   PullOutStatus() {}
 };

diff --git a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp
@@ -161,8 +161,10 @@ void StartPlannerModule::updateData()
     DEBUG_PRINT("StartPlannerModule::updateData() received new route, reset status");
   }
 
-  if (!status_.first_approved_time && isActivated()) {
-    status_.first_approved_time = clock_->now();
+  if (
+    planner_data_->operation_mode->mode == OperationModeState::AUTONOMOUS &&
+    !status_.first_engaged_time) {
+    status_.first_engaged_time = clock_->now();
   }
 
   if (hasFinishedBackwardDriving()) {
@@ -968,11 +970,11 @@ bool StartPlannerModule::hasFinishedPullOut() const
 
 bool StartPlannerModule::needToPrepareBlinkerBeforeStart() const
 {
-  if (!status_.first_approved_time) {
+  if (!status_.first_engaged_time) {
     return true;
   }
-  const auto first_approved_time = status_.first_approved_time.value();
-  const double elapsed = rclcpp::Duration(clock_->now() - first_approved_time).seconds();
+  const auto first_engaged_time = status_.first_engaged_time.value();
+  const double elapsed = rclcpp::Duration(clock_->now() - first_engaged_time).seconds();
   return elapsed < parameters_->prepare_time_before_start;
 }