fix(start_planner): issue when ego does not straddle lane bounds and starts from 0 speed (autowarefoundation#7004)

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

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

* add new status for ego departed

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

---------

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/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

@@ -247,6 +247,13 @@ void StartPlannerModule::updateData()
     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();
@@ -1265,8 +1272,10 @@ 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.
-  const bool override_ego_stopped_check = std::invoke([&]() {
+  // this issue. Also, if the ego is not engaged (so it is stopped), the blinkers should still be
+  // activated.
+
+  const bool geometric_planner_has_not_finished_first_path = std::invoke([&]() {
     if (status_.planner_type != PlannerType::GEOMETRIC) {
       return false;
     }
@@ -1277,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);