fix(static_obstacle_avoidance): improve turn signal output timing when the ego returns original lane

planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/debug.hpp

@@ -53,7 +53,8 @@ MarkerArray createAmbiguousObjectsMarkerArray(
 MarkerArray createStopTargetObjectMarkerArray(const AvoidancePlanningData & data);
 
 MarkerArray createDebugMarkerArray(
-  const AvoidancePlanningData & data, const PathShifter & shifter, const DebugData & debug,
+  const BehaviorModuleOutput & output, const AvoidancePlanningData & data,
+  const PathShifter & shifter, const DebugData & debug,
   const std::shared_ptr<AvoidanceParameters> & parameters);
 }  // namespace autoware::behavior_path_planner::utils::static_obstacle_avoidance
 

planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/scene.hpp

@@ -346,7 +346,8 @@ class StaticObstacleAvoidanceModule : public SceneModuleInterface
    * @brief fill debug markers.
    */
   void updateDebugMarker(
-    const AvoidancePlanningData & data, const PathShifter & shifter, const DebugData & debug) const;
+    const BehaviorModuleOutput & output, const AvoidancePlanningData & data,
+    const PathShifter & shifter, const DebugData & debug) const;
 
   /**
    * @brief fill information markers that are shown in Rviz by default.
@@ -368,6 +369,9 @@ class StaticObstacleAvoidanceModule : public SceneModuleInterface
    */
   bool isSafePath(ShiftedPath & shifted_path, DebugData & debug) const;
 
+  auto getTurnSignal(const ShiftedPath & spline_shift_path, const ShiftedPath & linear_shift_path)
+    -> TurnSignalInfo;
+
   // post process
 
   /**

planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/debug.cpp

@@ -229,6 +229,67 @@ MarkerArray unAvoidableObjectsMarkerArray(const ObjectDataArray & objects, std::
   return msg;
 }
 
+MarkerArray createTurnSignalMarkerArray(const TurnSignalInfo & turn_signal_info, std::string && ns)
+{
+  MarkerArray msg;
+
+  if (turn_signal_info.turn_signal.command == TurnIndicatorsCommand::NO_COMMAND) {
+    return msg;
+  }
+
+  const auto yaw_offset = [&turn_signal_info]() {
+    if (turn_signal_info.turn_signal.command == TurnIndicatorsCommand::ENABLE_RIGHT) {
+      return -1.0 * M_PI_2;
+    }
+
+    if (turn_signal_info.turn_signal.command == TurnIndicatorsCommand::ENABLE_LEFT) {
+      return M_PI_2;
+    }
+
+    return 0.0;
+  }();
+
+  size_t i = 0;
+  {
+    auto marker = createDefaultMarker(
+      "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, i++, Marker::ARROW,
+      createMarkerScale(0.6, 0.3, 0.3), createMarkerColor(0.0, 0.0, 1.0, 0.999));
+    marker.pose = turn_signal_info.desired_start_point;
+    marker.pose = calcOffsetPose(marker.pose, 0.0, 0.0, 0.0, yaw_offset);
+
+    msg.markers.push_back(marker);
+  }
+  {
+    auto marker = createDefaultMarker(
+      "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, i++, Marker::ARROW,
+      createMarkerScale(0.6, 0.3, 0.3), createMarkerColor(0.0, 0.0, 1.0, 0.999));
+    marker.pose = turn_signal_info.desired_end_point;
+    marker.pose = calcOffsetPose(marker.pose, 0.0, 0.0, 0.0, yaw_offset);
+
+    msg.markers.push_back(marker);
+  }
+  {
+    auto marker = createDefaultMarker(
+      "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, i++, Marker::ARROW,
+      createMarkerScale(0.8, 0.4, 0.4), createMarkerColor(1.0, 0.0, 0.0, 0.999));
+    marker.pose = turn_signal_info.required_start_point;
+    marker.pose = calcOffsetPose(marker.pose, 0.0, 0.0, 0.0, yaw_offset);
+
+    msg.markers.push_back(marker);
+  }
+  {
+    auto marker = createDefaultMarker(
+      "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, i++, Marker::ARROW,
+      createMarkerScale(0.8, 0.4, 0.4), createMarkerColor(1.0, 0.0, 0.0, 0.999));
+    marker.pose = turn_signal_info.required_end_point;
+    marker.pose = calcOffsetPose(marker.pose, 0.0, 0.0, 0.0, yaw_offset);
+
+    msg.markers.push_back(marker);
+  }
+
+  return msg;
+}
+
 }  // namespace
 
 MarkerArray createEgoStatusMarkerArray(
@@ -591,7 +652,8 @@ MarkerArray createDrivableBounds(
 }
 
 MarkerArray createDebugMarkerArray(
-  const AvoidancePlanningData & data, const PathShifter & shifter, const DebugData & debug,
+  const BehaviorModuleOutput & output, const AvoidancePlanningData & data,
+  const PathShifter & shifter, const DebugData & debug,
   const std::shared_ptr<AvoidanceParameters> & parameters)
 {
   using autoware::behavior_path_planner::utils::transformToLanelets;
@@ -737,6 +799,7 @@ MarkerArray createDebugMarkerArray(
   // misc
   if (parameters->enable_misc_marker) {
     add(createPathMarkerArray(path, "centerline_resampled", 0, 0.0, 0.9, 0.5));
+    add(createTurnSignalMarkerArray(output.turn_signal_info, "turn_signal_info"));
   }
 
   return msg;

planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp

@@ -92,7 +92,7 @@ bool StaticObstacleAvoidanceModule::isExecutionRequested() const
 
   // Check ego is in preferred lane
   updateInfoMarker(avoid_data_);
-  updateDebugMarker(avoid_data_, path_shifter_, debug_data_);
+  updateDebugMarker(BehaviorModuleOutput{}, avoid_data_, path_shifter_, debug_data_);
 
   // there is object that should be avoid. return true.
   if (!!avoid_data_.stop_target_object) {
@@ -947,6 +947,112 @@ PathWithLaneId StaticObstacleAvoidanceModule::extendBackwardLength(
   return extended_path;
 }
 
+auto StaticObstacleAvoidanceModule::getTurnSignal(
+  const ShiftedPath & spline_shift_path, const ShiftedPath & linear_shift_path) -> TurnSignalInfo
+{
+  using autoware::motion_utils::calcSignedArcLength;
+
+  const auto is_ignore_signal = [this](const UUID & uuid) {
+    if (!ignore_signal_.has_value()) {
+      return false;
+    }
+
+    return ignore_signal_.value() == uuid;
+  };
+
+  const auto update_ignore_signal = [this](const UUID & uuid, const bool is_ignore) {
+    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 =
+      autoware::motion_utils::calcLateralOffset(path.points, getEgoPosition(), current_seg_idx);
+    return std::abs(lateral_deviation) > threshold;
+  };
+
+  auto shift_lines = path_shifter_.getShiftLines();
+  if (shift_lines.empty()) {
+    return getPreviousModuleOutput().turn_signal_info;
+  }
+
+  if (is_large_deviation(spline_shift_path.path)) {
+    return getPreviousModuleOutput().turn_signal_info;
+  }
+
+  const auto itr =
+    std::remove_if(shift_lines.begin(), shift_lines.end(), [&, this](const auto & s) {
+      const auto threshold = planner_data_->parameters.turn_signal_shift_length_threshold;
+      return std::abs(s.start_shift_length - s.end_shift_length) < threshold ||
+             is_ignore_signal(s.id);
+    });
+  shift_lines.erase(itr, shift_lines.end());
+
+  if (shift_lines.empty()) {
+    return getPreviousModuleOutput().turn_signal_info;
+  }
+
+  const auto target_shift_line = [&]() {
+    const auto & s1 = shift_lines.front();
+
+    for (size_t i = 1; i < shift_lines.size(); i++) {
+      const auto & s2 = shift_lines.at(i);
+
+      const auto s1_relative_length = s1.start_shift_length - s1.end_shift_length;
+      const auto s2_relative_length = s2.start_shift_length - s2.end_shift_length;
+
+      // same side shift
+      if (s1_relative_length > 0.0 && s2_relative_length > 0.0) {
+        continue;
+      }
+
+      // same side shift
+      if (s1_relative_length < 0.0 && s2_relative_length < 0.0) {
+        continue;
+      }
+
+      // different side shift
+      const auto & points = path_shifter_.getReferencePath().points;
+      const size_t idx = planner_data_->findEgoIndex(points);
+
+      // output turn signal for near shift line.
+      if (calcSignedArcLength(points, idx, s1.start_idx) > 0.0) {
+        return s1;
+      }
+
+      // output turn signal for far shift line.
+      if (
+        calcSignedArcLength(points, idx, s2.start_idx) <
+        getEgoSpeed() * parameters_->max_prepare_time) {
+        return s2;
+      }
+
+      // output turn signal for near shift line.
+      return s1;
+    }
+
+    return s1;
+  }();
+
+  const auto original_signal = getPreviousModuleOutput().turn_signal_info;
+
+  constexpr bool is_driving_forward = true;
+  constexpr bool egos_lane_is_shifted = true;
+
+  const auto [new_signal, is_ignore] = planner_data_->getBehaviorTurnSignalInfo(
+    linear_shift_path, target_shift_line, avoid_data_.current_lanelets, helper_->getEgoShift(),
+    is_driving_forward, egos_lane_is_shifted);
+
+  update_ignore_signal(target_shift_line.id, is_ignore);
+
+  const auto current_seg_idx = planner_data_->findEgoSegmentIndex(spline_shift_path.path.points);
+  return 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);
+}
+
 BehaviorModuleOutput StaticObstacleAvoidanceModule::plan()
 {
   universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
@@ -992,48 +1098,9 @@ BehaviorModuleOutput StaticObstacleAvoidanceModule::plan()
 
   BehaviorModuleOutput output;
 
-  const auto is_ignore_signal = [this](const UUID & uuid) {
-    if (!ignore_signal_.has_value()) {
-      return false;
-    }
-
-    return ignore_signal_.value() == uuid;
-  };
-
-  const auto update_ignore_signal = [this](const UUID & uuid, const bool is_ignore) {
-    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 =
-      autoware::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;
-
-    constexpr bool is_driving_forward = true;
-    constexpr bool egos_lane_is_shifted = true;
-    const auto [new_signal, is_ignore] = planner_data_->getBehaviorTurnSignalInfo(
-      linear_shift_path, path_shifter_.getShiftLines().front(), avoid_data_.current_lanelets,
-      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.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);
-    update_ignore_signal(path_shifter_.getShiftLines().front().id, is_ignore);
+  // turn signal
+  {
+    output.turn_signal_info = getTurnSignal(spline_shift_path, linear_shift_path);
   }
 
   // sparse resampling for computational cost
@@ -1046,7 +1113,7 @@ BehaviorModuleOutput StaticObstacleAvoidanceModule::plan()
   {
     updateEgoBehavior(data, spline_shift_path);
     updateInfoMarker(avoid_data_);
-    updateDebugMarker(avoid_data_, path_shifter_, debug_data_);
+    updateDebugMarker(output, avoid_data_, path_shifter_, debug_data_);
   }
 
   if (isDrivingSameLane(helper_->getPreviousDrivingLanes(), data.current_lanelets)) {
@@ -1492,12 +1559,13 @@ void StaticObstacleAvoidanceModule::updateInfoMarker(const AvoidancePlanningData
 }
 
 void StaticObstacleAvoidanceModule::updateDebugMarker(
-  const AvoidancePlanningData & data, const PathShifter & shifter, const DebugData & debug) const
+  const BehaviorModuleOutput & output, const AvoidancePlanningData & data,
+  const PathShifter & shifter, const DebugData & debug) const
 {
   universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
   debug_marker_.markers.clear();
-  debug_marker_ =
-    utils::static_obstacle_avoidance::createDebugMarkerArray(data, shifter, debug, parameters_);
+  debug_marker_ = utils::static_obstacle_avoidance::createDebugMarkerArray(
+    output, data, shifter, debug, parameters_);
 }
 
 void StaticObstacleAvoidanceModule::updateAvoidanceDebugData(