fix issue when ego does not straddle lane bounds and starts from 0 speed

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

Author: danielsanchezaran

planning/behavior_path_planner_common/src/turn_signal_decider.cpp

@@ -731,10 +731,13 @@ std::pair<TurnSignalInfo, bool> TurnSignalDecider::getBehaviorTurnSignalInfo(
     return std::make_pair(TurnSignalInfo{}, true);
   }
 
-  if (!straddleRoadBound(path, shift_line, current_lanelets, p.vehicle_info)) {
+  // Check if the ego will cross lane bounds.
+  // Note that pull out requires blinkers, even if the ego does not cross lane bounds
+  if (!is_pull_out && !straddleRoadBound(path, shift_line, current_lanelets, p.vehicle_info)) {
     return std::make_pair(TurnSignalInfo{}, true);
   }
 
+  // If the ego has stopped and its close to completing its shift, turn off the blinkers
   constexpr double STOPPED_THRESHOLD = 0.1;  // [m/s]
   if (ego_speed < STOPPED_THRESHOLD && !override_ego_stopped_check) {
     if (isNearEndOfShift(

planning/behavior_path_start_planner_module/src/start_planner_module.cpp

@@ -241,9 +241,12 @@ 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) {
+    status_.driving_forward && !status_.first_engaged_and_driving_forward_time &&
+    ego_velocity > moving_velocity_threshold) {
     status_.first_engaged_and_driving_forward_time = clock_->now();
   }
 
@@ -1265,8 +1268,12 @@ TurnSignalInfo StartPlannerModule::calcTurnSignalInfo()
   // In Geometric pull out, the ego stops once and then steers the wheels to the opposite direction.
   // This sometimes causes the getBehaviorTurnSignalInfo method to detect the ego as stopped and
   // close to complete its shift, so it wrongly turns off the blinkers, this override helps avoid
-  // this issue.
+  // 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;
+    }
     if (status_.planner_type != PlannerType::GEOMETRIC) {
       return false;
     }