Add general turnSignal method to start planner

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

Author: danielsanchezaran

planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp

@@ -33,6 +33,7 @@
 
 #include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
 
+#include <lanelet2_core/Forward.h>
 #include <tf2/utils.h>
 
 #include <atomic>
@@ -239,6 +240,7 @@ class StartPlannerModule : public SceneModuleInterface
   std::vector<std::shared_ptr<PullOutPlannerBase>> start_planners_;
   PullOutStatus status_;
   mutable StartPlannerDebugData debug_data_;
+  std::optional<lanelet::Id> ignore_signal_{std::nullopt};
 
   std::deque<nav_msgs::msg::Odometry::ConstSharedPtr> odometry_buffer_;
 
@@ -264,7 +266,7 @@ class StartPlannerModule : public SceneModuleInterface
   std::shared_ptr<LaneDepartureChecker> lane_departure_checker_;
 
   // turn signal
-  TurnSignalInfo calcTurnSignalInfo() const;
+  TurnSignalInfo calcTurnSignalInfo();
 
   void incrementPathIndex();
   PathWithLaneId getCurrentPath() const;

planning/behavior_path_start_planner_module/src/start_planner_module.cpp

@@ -1215,103 +1215,70 @@ bool StartPlannerModule::hasFinishedCurrentPath()
   return is_near_target && isStopped();
 }
 
-TurnSignalInfo StartPlannerModule::calcTurnSignalInfo() const
+TurnSignalInfo StartPlannerModule::calcTurnSignalInfo()
 {
-  TurnSignalInfo turn_signal{};  // output
+  const auto path = getFullPath();
+  if (path.points.empty()) return getPreviousModuleOutput().turn_signal_info;
 
   const Pose & current_pose = planner_data_->self_odometry->pose.pose;
-  const Pose & start_pose = status_.pull_out_path.start_pose;
-  const Pose & end_pose = status_.pull_out_path.end_pose;
-
-  // turn on hazard light when backward driving
-  if (!status_.driving_forward) {
-    turn_signal.hazard_signal.command = HazardLightsCommand::ENABLE;
-    const auto back_start_pose = planner_data_->route_handler->getOriginalStartPose();
-    turn_signal.desired_start_point = back_start_pose;
-    turn_signal.required_start_point = back_start_pose;
-    // pull_out start_pose is same to backward driving end_pose
-    turn_signal.required_end_point = start_pose;
-    turn_signal.desired_end_point = start_pose;
-    return turn_signal;
-  }
-
-  // turn on right signal until passing pull_out end point
-  const auto path = getFullPath();
-  // pull out path does not overlap
-  const double distance_from_end =
-    motion_utils::calcSignedArcLength(path.points, end_pose.position, current_pose.position);
+  const auto shift_start_idx =
+    motion_utils::findNearestIndex(path.points, status_.pull_out_path.start_pose.position);
+  const auto shift_end_idx =
+    motion_utils::findNearestIndex(path.points, status_.pull_out_path.end_pose.position);
+  const lanelet::ConstLanelets current_lanes = utils::getCurrentLanes(planner_data_);
+
+  const auto is_ignore_signal = [this](const lanelet::Id & id) {
+    if (!ignore_signal_.has_value()) {
+      return false;
+    }
+    return ignore_signal_.value() == id;
+  };
 
-  if (path.points.empty()) {
-    return {};
+  const auto update_ignore_signal = [this](const lanelet::Id & id, const bool is_ignore) {
+    return is_ignore ? std::make_optional(id) : std::nullopt;
+  };
+
+  lanelet::Lanelet closest_lanelet;
+  lanelet::utils::query::getClosestLanelet(current_lanes, current_pose, &closest_lanelet);
+
+  if (is_ignore_signal(closest_lanelet.id())) {
+    return getPreviousModuleOutput().turn_signal_info;
   }
 
-  // calculate lateral offset from pull out target lane center line
-  lanelet::ConstLanelet closest_road_lane;
-  const double backward_path_length =
-    planner_data_->parameters.backward_path_length + parameters_->max_back_distance;
-  const auto road_lanes = utils::getExtendedCurrentLanes(
-    planner_data_, backward_path_length, std::numeric_limits<double>::max(),
-    /*forward_only_in_route*/ true);
-  lanelet::utils::query::getClosestLanelet(road_lanes, start_pose, &closest_road_lane);
-  const double lateral_offset =
-    lanelet::utils::getLateralDistanceToCenterline(closest_road_lane, start_pose);
-
-  turn_signal.turn_signal.command = std::invoke([&]() {
-    if (distance_from_end >= 0.0) return TurnIndicatorsCommand::DISABLE;
-    if (lateral_offset > parameters_->th_turn_signal_on_lateral_offset)
-      return TurnIndicatorsCommand::ENABLE_RIGHT;
-    if (lateral_offset < -parameters_->th_turn_signal_on_lateral_offset)
-      return TurnIndicatorsCommand::ENABLE_LEFT;
-    return TurnIndicatorsCommand::DISABLE;
-  });
+  const double current_shift_length =
+    lanelet::utils::getArcCoordinates(current_lanes, current_pose).distance;
 
-  turn_signal.desired_start_point = start_pose;
-  turn_signal.required_start_point = start_pose;
-  turn_signal.desired_end_point = end_pose;
-
-  // check if intersection exists within search length
-  const bool is_near_intersection = std::invoke([&]() {
-    const double check_length = parameters_->intersection_search_length;
-    double accumulated_length = 0.0;
-    const size_t current_idx = motion_utils::findNearestIndex(path.points, current_pose.position);
-    for (size_t i = current_idx; i < path.points.size() - 1; ++i) {
-      const auto & p = path.points.at(i);
-      for (const auto & lane : planner_data_->route_handler->getLaneletsFromIds(p.lane_ids)) {
-        const std::string turn_direction = lane.attributeOr("turn_direction", "else");
-        if (turn_direction == "right" || turn_direction == "left" || turn_direction == "straight") {
-          return true;
-        }
-      }
-      accumulated_length += tier4_autoware_utils::calcDistance2d(p, path.points.at(i + 1));
-      if (accumulated_length > check_length) {
-        return false;
-      }
-    }
-    return false;
-  });
+  constexpr bool egos_lane_is_shifted = true;
+  constexpr bool is_pull_out = true;
 
-  turn_signal.required_end_point = std::invoke([&]() {
-    if (is_near_intersection) return end_pose;
-    const double length_start_to_end =
-      motion_utils::calcSignedArcLength(path.points, start_pose.position, end_pose.position);
-    const auto ratio = std::clamp(
-      parameters_->length_ratio_for_turn_signal_deactivation_near_intersection, 0.0, 1.0);
-
-    const double required_end_length = length_start_to_end * ratio;
-    double accumulated_length = 0.0;
-    const size_t start_idx = motion_utils::findNearestIndex(path.points, start_pose.position);
-    for (size_t i = start_idx; i < path.points.size() - 1; ++i) {
-      accumulated_length +=
-        tier4_autoware_utils::calcDistance2d(path.points.at(i), path.points.at(i + 1));
-      if (accumulated_length > required_end_length) {
-        return path.points.at(i).point.pose;
-      }
+  // 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([&]() {
+    if (status_.planner_type != PlannerType::GEOMETRIC) {
+      return false;
     }
-    // not found required end point
-    return end_pose;
+    constexpr double distance_threshold = 1.0;
+    const auto stop_point = status_.pull_out_path.partial_paths.front().points.back();
+    const double distance_from_ego_to_stop_point = std::abs(motion_utils::calcSignedArcLength(
+      path.points, stop_point.point.pose.position, current_pose.position));
+    return distance_from_ego_to_stop_point < distance_threshold;
   });
 
-  return turn_signal;
+  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);
+  ignore_signal_ = update_ignore_signal(closest_lanelet.id(), is_ignore);
+
+  const auto original_signal = getPreviousModuleOutput().turn_signal_info;
+  const auto current_seg_idx = planner_data_->findEgoSegmentIndex(path.points);
+  const auto output_turn_signal_info = planner_data_->turn_signal_decider.use_prior_turn_signal(
+    path, current_pose, current_seg_idx, original_signal, new_signal,
+    planner_data_->parameters.ego_nearest_dist_threshold,
+    planner_data_->parameters.ego_nearest_yaw_threshold);
+
+  return output_turn_signal_info;
 }
 
 bool StartPlannerModule::isSafePath() const