fix(bpp): overwrite turn signal by latter module

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

Author: satoshi-ota

planning/behavior_path_avoidance_module/src/scene.cpp

@@ -942,7 +942,7 @@ BehaviorModuleOutput AvoidanceModule::plan()
       linear_shift_path, path_shifter_.getShiftLines().front(), helper_->getEgoShift(), avoid_data_,
       planner_data_);
     const auto current_seg_idx = planner_data_->findEgoSegmentIndex(spline_shift_path.path.points);
-    output.turn_signal_info = planner_data_->turn_signal_decider.use_prior_turn_signal(
+    output.turn_signal_info = planner_data_->turn_signal_decider.overwrite_turn_signal(
       spline_shift_path.path, getEgoPose(), current_seg_idx, original_signal, new_signal,
       planner_data_->parameters.ego_nearest_dist_threshold,
       planner_data_->parameters.ego_nearest_yaw_threshold);

planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp

@@ -971,7 +971,7 @@ void GoalPlannerModule::setTurnSignalInfo(BehaviorModuleOutput & output) const
   const auto original_signal = getPreviousModuleOutput().turn_signal_info;
   const auto new_signal = calcTurnSignalInfo();
   const auto current_seg_idx = planner_data_->findEgoSegmentIndex(output.path.points);
-  output.turn_signal_info = planner_data_->turn_signal_decider.use_prior_turn_signal(
+  output.turn_signal_info = planner_data_->turn_signal_decider.overwrite_turn_signal(
     output.path, getEgoPose(), current_seg_idx, original_signal, new_signal,
     planner_data_->parameters.ego_nearest_dist_threshold,
     planner_data_->parameters.ego_nearest_yaw_threshold);

planning/behavior_path_lane_change_module/src/interface.cpp

@@ -446,7 +446,7 @@ TurnSignalInfo LaneChangeInterface::getCurrentTurnSignalInfo(
     }
 
     // check the priority of turn signals
-    return module_type_->getTurnSignalDecider().use_prior_turn_signal(
+    return module_type_->getTurnSignalDecider().overwrite_turn_signal(
       path, current_pose, current_nearest_seg_idx, original_turn_signal_info,
       current_turn_signal_info, nearest_dist_threshold, nearest_yaw_threshold);
   }

planning/behavior_path_lane_change_module/src/scene.cpp

@@ -148,7 +148,7 @@ BehaviorModuleOutput NormalLaneChange::getTerminalLaneChangePath() const
     output.turn_signal_info = prev_turn_signal_info_;
     extendOutputDrivableArea(output);
     const auto current_seg_idx = planner_data_->findEgoSegmentIndex(output.path.points);
-    output.turn_signal_info = planner_data_->turn_signal_decider.use_prior_turn_signal(
+    output.turn_signal_info = planner_data_->turn_signal_decider.overwrite_turn_signal(
       output.path, getEgoPose(), current_seg_idx, prev_turn_signal_info_, output.turn_signal_info,
       planner_data_->parameters.ego_nearest_dist_threshold,
       planner_data_->parameters.ego_nearest_yaw_threshold);
@@ -183,7 +183,7 @@ BehaviorModuleOutput NormalLaneChange::getTerminalLaneChangePath() const
   extendOutputDrivableArea(output);
 
   const auto current_seg_idx = planner_data_->findEgoSegmentIndex(output.path.points);
-  output.turn_signal_info = planner_data_->turn_signal_decider.use_prior_turn_signal(
+  output.turn_signal_info = planner_data_->turn_signal_decider.overwrite_turn_signal(
     output.path, getEgoPose(), current_seg_idx, prev_turn_signal_info_, output.turn_signal_info,
     planner_data_->parameters.ego_nearest_dist_threshold,
     planner_data_->parameters.ego_nearest_yaw_threshold);
@@ -226,7 +226,7 @@ BehaviorModuleOutput NormalLaneChange::generateOutput()
   extendOutputDrivableArea(output);
 
   const auto current_seg_idx = planner_data_->findEgoSegmentIndex(output.path.points);
-  output.turn_signal_info = planner_data_->turn_signal_decider.use_prior_turn_signal(
+  output.turn_signal_info = planner_data_->turn_signal_decider.overwrite_turn_signal(
     output.path, getEgoPose(), current_seg_idx, prev_turn_signal_info_, output.turn_signal_info,
     planner_data_->parameters.ego_nearest_dist_threshold,
     planner_data_->parameters.ego_nearest_yaw_threshold);

planning/behavior_path_planner_common/include/behavior_path_planner_common/turn_signal_decider.hpp

@@ -82,6 +82,11 @@ class TurnSignalDecider
     const TurnSignalInfo & intersection_signal_info, const TurnSignalInfo & behavior_signal_info,
     const double nearest_dist_threshold, const double nearest_yaw_threshold);
 
+  TurnSignalInfo overwrite_turn_signal(
+    const PathWithLaneId & path, const Pose & current_pose, const size_t current_seg_idx,
+    const TurnSignalInfo & original_signal, const TurnSignalInfo & new_signal,
+    const double nearest_dist_threshold, const double nearest_yaw_threshold);
+
   TurnSignalInfo use_prior_turn_signal(
     const PathWithLaneId & path, const Pose & current_pose, const size_t current_seg_idx,
     const TurnSignalInfo & original_signal, const TurnSignalInfo & new_signal,

planning/behavior_path_planner_common/src/turn_signal_decider.cpp

@@ -392,6 +392,32 @@ TurnIndicatorsCommand TurnSignalDecider::resolve_turn_signal(
   return intersection_signal_info.turn_signal;
 }
 
+TurnSignalInfo TurnSignalDecider::overwrite_turn_signal(
+  const PathWithLaneId & path, const Pose & current_pose, const size_t current_seg_idx,
+  const TurnSignalInfo & original_signal, const TurnSignalInfo & new_signal,
+  const double nearest_dist_threshold, const double nearest_yaw_threshold)
+{
+  const auto get_distance = [&](const Pose & input_point) {
+    const size_t nearest_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints(
+      path.points, input_point, nearest_dist_threshold, nearest_yaw_threshold);
+    return motion_utils::calcSignedArcLength(
+      path.points, current_pose.position, current_seg_idx, input_point.position, nearest_seg_idx);
+  };
+
+  const auto & new_desired_start_point = new_signal.desired_start_point;
+  const auto & new_required_start_point = new_signal.required_start_point;
+
+  const double dist_to_new_desired_start = get_distance(new_desired_start_point) - base_link2front_;
+  const double dist_to_new_required_start =
+    get_distance(new_required_start_point) - base_link2front_;
+
+  if (dist_to_new_desired_start > 0.0 && dist_to_new_required_start > 0.0) {
+    return original_signal;
+  }
+
+  return new_signal;
+}
+
 TurnSignalInfo TurnSignalDecider::use_prior_turn_signal(
   const PathWithLaneId & path, const Pose & current_pose, const size_t current_seg_idx,
   const TurnSignalInfo & original_signal, const TurnSignalInfo & new_signal,