add new status for ego departed

Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>

Author: danielsanchezaran

planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp

@@ -74,6 +74,8 @@ struct PullOutStatus
   //! 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};
+  // record if the ego has departed from the start point
+  bool has_departed{false};
 
   PullOutStatus() {}
 };

planning/behavior_path_start_planner_module/src/start_planner_module.cpp

@@ -241,15 +241,19 @@ void StartPlannerModule::updateData()
     DEBUG_PRINT("StartPlannerModule::updateData() received new route, reset status");
   }
 
-  constexpr double moving_velocity_threshold = 0.1;
-  const double & ego_velocity = planner_data_->self_odometry->twist.twist.linear.x;
   if (
     planner_data_->operation_mode->mode == OperationModeState::AUTONOMOUS &&
-    status_.driving_forward && !status_.first_engaged_and_driving_forward_time &&
-    ego_velocity > moving_velocity_threshold) {
+    status_.driving_forward && !status_.first_engaged_and_driving_forward_time) {
     status_.first_engaged_and_driving_forward_time = clock_->now();
   }
 
+  constexpr double moving_velocity_threshold = 0.1;
+  const double & ego_velocity = planner_data_->self_odometry->twist.twist.linear.x;
+  if (status_.first_engaged_and_driving_forward_time && ego_velocity > moving_velocity_threshold) {
+    // Ego is engaged, and has moved
+    status_.has_departed = true;
+  }
+
   status_.backward_driving_complete = hasFinishedBackwardDriving();
   if (status_.backward_driving_complete) {
     updateStatusAfterBackwardDriving();
@@ -1270,10 +1274,8 @@ TurnSignalInfo StartPlannerModule::calcTurnSignalInfo()
   // close to complete its shift, so it wrongly turns off the blinkers, this override helps avoid
   // this issue. Also, if the ego is not engaged (so it is stopped), the blinkers should still be
   // activated.
-  const bool override_ego_stopped_check = std::invoke([&]() {
-    if (!status_.first_engaged_and_driving_forward_time) {
-      return true;
-    }
+
+  const bool geometric_planner_has_not_finished_first_path = std::invoke([&]() {
     if (status_.planner_type != PlannerType::GEOMETRIC) {
       return false;
     }
@@ -1284,6 +1286,9 @@ TurnSignalInfo StartPlannerModule::calcTurnSignalInfo()
     return distance_from_ego_to_stop_point < distance_threshold;
   });
 
+  const bool override_ego_stopped_check =
+    !status_.has_departed || geometric_planner_has_not_finished_first_path;
+
   const auto [new_signal, is_ignore] = planner_data_->getBehaviorTurnSignalInfo(
     path, shift_start_idx, shift_end_idx, current_lanes, current_shift_length,
     status_.driving_forward, egos_lane_is_shifted, override_ego_stopped_check, is_pull_out);