feat(lane_change_module): add general method to get turn signal for LC module (autowarefoundation#6627)

add general method to get turn signal for LC module

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

Author: danielsanchezaran

planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp

@@ -141,7 +141,9 @@ class NormalLaneChange : public LaneChangeBase
     const utils::path_safety_checker::RSSparams rss_params, const bool is_stuck,
     const bool check_safety = true) const override;
 
-  TurnSignalInfo calcTurnSignalInfo() override;
+  std::optional<LaneChangePath> calcTerminalLaneChangePath(
+    const lanelet::ConstLanelets & current_lanes,
+    const lanelet::ConstLanelets & target_lanes) const;
 
   bool isValidPath(const PathWithLaneId & path) const override;
 

planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/base_class.hpp

@@ -225,8 +225,6 @@ class LaneChangeBase
     LaneChangePaths * candidate_paths, const utils::path_safety_checker::RSSparams rss_params,
     const bool is_stuck, const bool check_safety) const = 0;
 
-  virtual TurnSignalInfo calcTurnSignalInfo() = 0;
-
   virtual bool isValidPath(const PathWithLaneId & path) const = 0;
 
   virtual bool isAbleToStopSafely() const = 0;

planning/behavior_path_lane_change_module/src/scene.cpp

@@ -426,14 +426,20 @@ void NormalLaneChange::resetParameters()
 
 TurnSignalInfo NormalLaneChange::updateOutputTurnSignal()
 {
-  TurnSignalInfo turn_signal_info = calcTurnSignalInfo();
-  const auto [turn_signal_command, distance_to_vehicle_front] = utils::getPathTurnSignal(
-    status_.current_lanes, status_.lane_change_path.shifted_path,
-    status_.lane_change_path.info.shift_line, getEgoPose(), getEgoTwist().linear.x,
-    planner_data_->parameters);
-  turn_signal_info.turn_signal.command = turn_signal_command.command;
-
-  return turn_signal_info;
+  const auto & pose = getEgoPose();
+  const auto & current_lanes = status_.current_lanes;
+  const auto & shift_line = status_.lane_change_path.info.shift_line;
+  const auto & shift_path = status_.lane_change_path.shifted_path;
+  const auto current_shift_length = lanelet::utils::getArcCoordinates(current_lanes, pose).distance;
+  constexpr bool is_driving_forward = true;
+  // The getBehaviorTurnSignalInfo method expects the shifted line to be generated off of the ego's
+  // current lane, lane change is different, so we set this flag to false.
+  constexpr bool egos_lane_is_shifted = false;
+
+  const auto [new_signal, is_ignore] = planner_data_->getBehaviorTurnSignalInfo(
+    shift_path, shift_line, current_lanes, current_shift_length, is_driving_forward,
+    egos_lane_is_shifted);
+  return new_signal;
 }
 
 lanelet::ConstLanelets NormalLaneChange::getCurrentLanes() const
@@ -1445,53 +1451,26 @@ PathSafetyStatus NormalLaneChange::isApprovedPathSafe() const
   return safety_status;
 }
 
-TurnSignalInfo NormalLaneChange::calcTurnSignalInfo()
+PathSafetyStatus NormalLaneChange::evaluateApprovedPathWithUnsafeHysteresis(
+  PathSafetyStatus approved_path_safety_status)
 {
-  const auto get_blinker_pose = [](const PathWithLaneId & path, const double length) {
-    double accumulated_length = 0.0;
-    for (size_t i = 0; i < path.points.size() - 1; ++i) {
-      accumulated_length +=
-        tier4_autoware_utils::calcDistance2d(path.points.at(i), path.points.at(i + 1));
-      if (accumulated_length > length) {
-        return path.points.at(i).point.pose;
-      }
-    }
-
-    return path.points.front().point.pose;
-  };
-
-  const auto & path = status_.lane_change_path;
-  const auto & shifted_path = path.shifted_path.path;
-
-  TurnSignalInfo turn_signal_info{};
-
-  // desired start pose = prepare start pose
-  turn_signal_info.desired_start_point = std::invoke([&]() {
-    const auto blinker_start_duration = planner_data_->parameters.turn_signal_search_time;
-    const auto diff_time = path.info.duration.prepare - blinker_start_duration;
-    if (diff_time < 1e-5) {
-      return path.path.points.front().point.pose;
+  if (!approved_path_safety_status.is_safe) {
+    ++unsafe_hysteresis_count_;
+    RCLCPP_DEBUG(
+      logger_, "%s: Increasing hysteresis count to %d.", __func__, unsafe_hysteresis_count_);
+  } else {
+    if (unsafe_hysteresis_count_ > 0) {
+      RCLCPP_DEBUG(logger_, "%s: Lane change is now SAFE. Resetting hysteresis count.", __func__);
     }
-
-    const auto current_twist = getEgoTwist();
-    const auto diff_length = std::abs(current_twist.linear.x) * diff_time;
-    return get_blinker_pose(path.path, diff_length);
-  });
-
-  // desired end pose
-  const auto length_ratio =
-    std::clamp(lane_change_parameters_->length_ratio_for_turn_signal_deactivation, 0.0, 1.0);
-  const auto desired_end_length = path.info.length.lane_changing * length_ratio;
-  turn_signal_info.desired_end_point = get_blinker_pose(shifted_path, desired_end_length);
-
-  // required start pose = lane changing start pose
-  turn_signal_info.required_start_point = path.info.shift_line.start;
-
-  // required end pose = in the middle of the lane change
-  const auto mid_lane_change_length = path.info.length.lane_changing / 2;
-  turn_signal_info.required_end_point = get_blinker_pose(shifted_path, mid_lane_change_length);
-
-  return turn_signal_info;
+    unsafe_hysteresis_count_ = 0;
+  }
+  if (unsafe_hysteresis_count_ > lane_change_parameters_->cancel.unsafe_hysteresis_threshold) {
+    RCLCPP_DEBUG(
+      logger_, "%s: hysteresis count exceed threshold. lane change is now %s", __func__,
+      (approved_path_safety_status.is_safe ? "safe" : "UNSAFE"));
+    return approved_path_safety_status;
+  }
+  return {};
 }
 
 bool NormalLaneChange::isValidPath(const PathWithLaneId & path) const