fix(start_planner): consider backward completion before preparing bli…

…nker (autowarefoundation#6558) consider backward completion Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>
tier4 · Mar 7, 2024 · 45356fe · 45356fe
1 parent 11f1692
commit 45356fe
Show file tree

Hide file tree

Showing 2 changed files with 14 additions and 11 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
@@ -70,8 +70,9 @@ struct PullOutStatus
   std::shared_ptr<PathWithLaneId> prev_stop_path_after_approval{nullptr};
   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_engaged_time{std::nullopt};
+  //! record the first time when ego started forward-driving (maybe after backward driving
+  //! completion) in AUTONOMOUS operation mode
+  std::optional<rclcpp::Time> first_engaged_and_driving_forward_time{std::nullopt};
 
   PullOutStatus() {}
 };
@@ -231,7 +232,7 @@ class StartPlannerModule : public SceneModuleInterface
   void updateStatusAfterBackwardDriving();
   PredictedObjects filterStopObjectsInPullOutLanes(
     const lanelet::ConstLanelets & pull_out_lanes, const double velocity_threshold) const;
-  bool needToPrepareBlinkerBeforeStart() const;
+  bool needToPrepareBlinkerBeforeStartDrivingForward() const;
   bool hasFinishedPullOut() const;
   bool hasFinishedBackwardDriving() const;
   bool hasCollisionWithDynamicObjects() const;

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
@@ -104,7 +104,7 @@ void StartPlannerModule::onFreespacePlannerTimer()
 BehaviorModuleOutput StartPlannerModule::run()
 {
   updateData();
-  if (!isActivated() || needToPrepareBlinkerBeforeStart()) {
+  if (!isActivated() || needToPrepareBlinkerBeforeStartDrivingForward()) {
     return planWaitingApproval();
   }
 
@@ -163,8 +163,8 @@ void StartPlannerModule::updateData()
 
   if (
     planner_data_->operation_mode->mode == OperationModeState::AUTONOMOUS &&
-    !status_.first_engaged_time) {
-    status_.first_engaged_time = clock_->now();
+    status_.driving_forward && !status_.first_engaged_and_driving_forward_time) {
+    status_.first_engaged_and_driving_forward_time = clock_->now();
   }
 
   if (hasFinishedBackwardDriving()) {
@@ -968,13 +968,15 @@ bool StartPlannerModule::hasFinishedPullOut() const
   return has_finished;
 }
 
-bool StartPlannerModule::needToPrepareBlinkerBeforeStart() const
+bool StartPlannerModule::needToPrepareBlinkerBeforeStartDrivingForward() const
 {
-  if (!status_.first_engaged_time) {
-    return true;
+  if (!status_.first_engaged_and_driving_forward_time) {
+    return false;
   }
-  const auto first_engaged_time = status_.first_engaged_time.value();
-  const double elapsed = rclcpp::Duration(clock_->now() - first_engaged_time).seconds();
+  const auto first_engaged_and_driving_forward_time =
+    status_.first_engaged_and_driving_forward_time.value();
+  const double elapsed =
+    rclcpp::Duration(clock_->now() - first_engaged_and_driving_forward_time).seconds();
   return elapsed < parameters_->prepare_time_before_start;
 }