fix(bpp): overwrite turn signal by latter module (autowarefoundation#7045)

* fix(bpp): overwrite turn signal by latter module

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

* fix(avoidance): return previous module turn signal if it's in success state

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

* fix(avoidance): don't output turn signal if there is huge lateral deveation

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

* refactor: small update

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

* chore: use lower case

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

* refactor: remove redundant function call

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

---------

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

Author: satoshi-ota

planning/behavior_path_avoidance_module/src/scene.cpp

@@ -855,10 +855,12 @@ BehaviorModuleOutput AvoidanceModule::plan()
 
   if (data.state == AvoidanceState::SUCCEEDED) {
     removeRegisteredShiftLines(State::SUCCEEDED);
+    return getPreviousModuleOutput();
   }
 
   if (data.state == AvoidanceState::CANCEL) {
     removeRegisteredShiftLines(State::FAILED);
+    return getPreviousModuleOutput();
   }
 
   if (data.yield_required) {
@@ -901,11 +903,21 @@ BehaviorModuleOutput AvoidanceModule::plan()
     ignore_signal_ = is_ignore ? std::make_optional(uuid) : std::nullopt;
   };
 
+  const auto is_large_deviation = [this](const auto & path) {
+    constexpr double threshold = 1.0;
+    const auto current_seg_idx = planner_data_->findEgoSegmentIndex(path.points);
+    const auto lateral_deviation =
+      motion_utils::calcLateralOffset(path.points, getEgoPosition(), current_seg_idx);
+    return std::abs(lateral_deviation) > threshold;
+  };
+
   // turn signal info
   if (path_shifter_.getShiftLines().empty()) {
     output.turn_signal_info = getPreviousModuleOutput().turn_signal_info;
   } else if (is_ignore_signal(path_shifter_.getShiftLines().front().id)) {
     output.turn_signal_info = getPreviousModuleOutput().turn_signal_info;
+  } else if (is_large_deviation(spline_shift_path.path)) {
+    output.turn_signal_info = getPreviousModuleOutput().turn_signal_info;
   } else {
     const auto original_signal = getPreviousModuleOutput().turn_signal_info;
 
@@ -916,7 +928,7 @@ BehaviorModuleOutput AvoidanceModule::plan()
       helper_->getEgoShift(), is_driving_forward, egos_lane_is_shifted);
 
     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

@@ -1094,7 +1094,7 @@ void GoalPlannerModule::setTurnSignalInfo(BehaviorModuleOutput & output)
   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/scene.cpp

@@ -204,7 +204,7 @@ TurnSignalInfo NormalLaneChange::get_current_turn_signal_info()
     }
 
     // check the priority of turn signals
-    return getTurnSignalDecider().use_prior_turn_signal(
+    return 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);
   }
@@ -226,7 +226,7 @@ BehaviorModuleOutput NormalLaneChange::getTerminalLaneChangePath() const
     output.path = abort_path_->path;
     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_module_output_.turn_signal_info,
       output.turn_signal_info, planner_data_->parameters.ego_nearest_dist_threshold,
       planner_data_->parameters.ego_nearest_yaw_threshold);
@@ -252,7 +252,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_module_output_.turn_signal_info,
     output.turn_signal_info, planner_data_->parameters.ego_nearest_dist_threshold,
     planner_data_->parameters.ego_nearest_yaw_threshold);
@@ -297,7 +297,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_module_output_.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

@@ -64,6 +64,17 @@ struct TurnSignalInfo
     hazard_signal.command = HazardLightsCommand::NO_COMMAND;
   }
 
+  TurnSignalInfo(const Pose & start, const Pose & end)
+  {
+    turn_signal.command = TurnIndicatorsCommand::NO_COMMAND;
+    hazard_signal.command = HazardLightsCommand::NO_COMMAND;
+
+    desired_start_point = start;
+    desired_end_point = end;
+    required_start_point = start;
+    required_end_point = end;
+  }
+
   // desired turn signal
   TurnIndicatorsCommand turn_signal;
   HazardLightsCommand hazard_signal;
@@ -93,6 +104,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) const;
+
   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

@@ -399,6 +399,40 @@ 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
+{
+  if (original_signal.turn_signal.command == TurnIndicatorsCommand::NO_COMMAND) {
+    return new_signal;
+  }
+
+  if (original_signal.turn_signal.command == TurnIndicatorsCommand::DISABLE) {
+    return new_signal;
+  }
+
+  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) -
+           base_link2front_;
+  };
+
+  const auto & original_desired_end_point = original_signal.desired_end_point;
+  const auto & new_desired_start_point = new_signal.desired_start_point;
+
+  const double dist_to_original_desired_end = get_distance(original_desired_end_point);
+  const double dist_to_new_desired_start = get_distance(new_desired_start_point);
+  if (dist_to_new_desired_start > dist_to_original_desired_end) {
+    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,
@@ -622,6 +656,8 @@ std::pair<TurnSignalInfo, bool> TurnSignalDecider::getBehaviorTurnSignalInfo(
   const double current_shift_length, const bool is_driving_forward, const bool egos_lane_is_shifted,
   const bool override_ego_stopped_check, const bool is_pull_out) const
 {
+  using tier4_autoware_utils::getPose;
+
   const auto & p = parameters;
   const auto & rh = route_handler;
   const auto & ego_pose = self_odometry->pose.pose;
@@ -674,16 +710,19 @@ std::pair<TurnSignalInfo, bool> TurnSignalDecider::getBehaviorTurnSignalInfo(
 
   const auto relative_shift_length = end_shift_length - start_shift_length;
 
+  const auto p_path_start = getPose(path.path.points.front());
+  const auto p_path_end = getPose(path.path.points.back());
+
   // If shift length is shorter than the threshold, it does not need to turn on blinkers
   if (std::fabs(relative_shift_length) < p.turn_signal_shift_length_threshold) {
-    return std::make_pair(TurnSignalInfo{}, true);
+    return std::make_pair(TurnSignalInfo(p_path_start, p_path_end), true);
   }
 
   // If the vehicle does not shift anymore, we turn off the blinker
   if (
     std::fabs(end_shift_length - current_shift_length) <
     p.turn_signal_remaining_shift_length_threshold) {
-    return std::make_pair(TurnSignalInfo{}, true);
+    return std::make_pair(TurnSignalInfo(p_path_start, p_path_end), true);
   }
 
   const auto get_command = [](const auto & shift_length) {
@@ -698,7 +737,7 @@ std::pair<TurnSignalInfo, bool> TurnSignalDecider::getBehaviorTurnSignalInfo(
     p.vehicle_info.max_longitudinal_offset_m;
 
   if (signal_prepare_distance < ego_front_to_shift_start) {
-    return std::make_pair(TurnSignalInfo{}, false);
+    return std::make_pair(TurnSignalInfo(p_path_start, p_path_end), false);
   }
 
   const auto blinker_start_pose = path.path.points.at(shift_line.start_idx).point.pose;
@@ -715,13 +754,13 @@ std::pair<TurnSignalInfo, bool> TurnSignalDecider::getBehaviorTurnSignalInfo(
   turn_signal_info.turn_signal.command = get_command(relative_shift_length);
 
   if (!p.turn_signal_on_swerving) {
-    return std::make_pair(turn_signal_info, false);
+    return std::make_pair(TurnSignalInfo(p_path_start, p_path_end), false);
   }
 
   lanelet::ConstLanelet lanelet;
   const auto query_pose = (egos_lane_is_shifted) ? shift_line.end : shift_line.start;
   if (!rh->getClosestLaneletWithinRoute(query_pose, &lanelet)) {
-    return std::make_pair(TurnSignalInfo{}, true);
+    return std::make_pair(TurnSignalInfo(p_path_start, p_path_end), true);
   }
 
   const auto left_same_direction_lane = rh->getLeftLanelet(lanelet, true, true);
@@ -736,13 +775,13 @@ std::pair<TurnSignalInfo, bool> TurnSignalDecider::getBehaviorTurnSignalInfo(
     !is_pull_out && !existShiftSideLane(
                       start_shift_length, end_shift_length, !has_left_lane, !has_right_lane,
                       p.turn_signal_shift_length_threshold)) {
-    return std::make_pair(TurnSignalInfo{}, true);
+    return std::make_pair(TurnSignalInfo(p_path_start, p_path_end), true);
   }
 
   // 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);
+    return std::make_pair(TurnSignalInfo(p_path_start, p_path_end), true);
   }
 
   // If the ego has stopped and its close to completing its shift, turn off the blinkers
@@ -751,7 +790,7 @@ std::pair<TurnSignalInfo, bool> TurnSignalDecider::getBehaviorTurnSignalInfo(
     if (isNearEndOfShift(
           start_shift_length, end_shift_length, ego_pose.position, current_lanelets,
           p.turn_signal_shift_length_threshold)) {
-      return std::make_pair(TurnSignalInfo{}, true);
+      return std::make_pair(TurnSignalInfo(p_path_start, p_path_end), true);
     }
   }