fix(behavior_path_planner): lane change turn signal during approval

planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module.hpp

@@ -189,6 +189,7 @@ class LaneChangeModule : public SceneModuleInterface
   void updateOutputTurnSignal(BehaviorModuleOutput & output);
   void updateSteeringFactorPtr(const BehaviorModuleOutput & output);
   bool isApprovedPathSafe(Pose & ego_pose_before_collision) const;
+  void calcTurnSignalInfo();
 
   void updateSteeringFactorPtr(
     const CandidateOutput & output, const LaneChangePath & selected_path) const;

planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/util.hpp

@@ -132,11 +132,6 @@ bool isEgoWithinOriginalLane(
   const lanelet::ConstLanelets & current_lanes, const Pose & current_pose,
   const BehaviorPathPlannerParameters & common_param);
 
-TurnSignalInfo calc_turn_signal_info(
-  const PathWithLaneId & prepare_path, const double prepare_velocity,
-  const double min_prepare_distance, const double prepare_duration, const ShiftLine & shift_line,
-  const ShiftedPath & lane_changing_path);
-
 void get_turn_signal_info(
   const LaneChangePath & lane_change_path, TurnSignalInfo * turn_signal_info);
 

planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp

@@ -697,6 +697,7 @@ bool LaneChangeModule::isApprovedPathSafe(Pose & ego_pose_before_collision) cons
 
 void LaneChangeModule::updateOutputTurnSignal(BehaviorModuleOutput & output)
 {
+  calcTurnSignalInfo();
   const auto turn_signal_info = util::getPathTurnSignal(
     status_.current_lanes, status_.lane_change_path.shifted_path,
     status_.lane_change_path.shift_line, getEgoPose(), getEgoTwist().linear.x,
@@ -706,6 +707,49 @@ void LaneChangeModule::updateOutputTurnSignal(BehaviorModuleOutput & output)
   lane_change_utils::get_turn_signal_info(status_.lane_change_path, &output.turn_signal_info);
 }
 
+void LaneChangeModule::calcTurnSignalInfo()
+{
+  const auto get_blinker_pose =
+    [this](const PathWithLaneId & path, const lanelet::ConstLanelets & lanes, const double length) {
+      const auto & points = path.points;
+      const auto arc_front = lanelet::utils::getArcCoordinates(lanes, points.front().point.pose);
+      for (const auto & point : points) {
+        const auto & pt = point.point.pose;
+        const auto arc_current = lanelet::utils::getArcCoordinates(lanes, pt);
+        const auto diff = arc_current.length - arc_front.length;
+        if (diff > length) {
+          return pt;
+        }
+      }
+
+      RCLCPP_WARN(getLogger(), "unable to determine blinker pose...");
+      return points.front().point.pose;
+    };
+
+  const auto & path = status_.lane_change_path;
+  TurnSignalInfo turn_signal_info{};
+
+  turn_signal_info.desired_start_point = std::invoke([&]() {
+    const auto blinker_start_duration = planner_data_->parameters.turn_signal_search_time;
+    const auto prepare_duration = parameters_->lane_change_prepare_duration;
+    const auto prepare_to_blinker_start_diff = prepare_duration - blinker_start_duration;
+    if (prepare_to_blinker_start_diff < 1e-5) {
+      return path.path.points.front().point.pose;
+    }
+
+    return get_blinker_pose(path.path, path.reference_lanelets, prepare_to_blinker_start_diff);
+  });
+  turn_signal_info.desired_end_point = path.shift_line.end;
+
+  turn_signal_info.required_start_point = path.shift_line.start;
+  const auto mid_lane_change_length = path.lane_change_length / 2;
+  const auto & shifted_path = path.shifted_path.path;
+  turn_signal_info.required_end_point =
+    get_blinker_pose(shifted_path, path.target_lanelets, mid_lane_change_length);
+
+  status_.lane_change_path.turn_signal_info = turn_signal_info;
+}
+
 void LaneChangeModule::resetParameters()
 {
   is_abort_path_approved_ = false;

planning/behavior_path_planner/src/scene_module/lane_change/util.cpp

@@ -99,8 +99,7 @@ std::optional<LaneChangePath> constructCandidatePath(
   const PathWithLaneId & prepare_segment, const PathWithLaneId & lane_changing_segment,
   const PathWithLaneId & target_lane_reference_path, const ShiftLine & shift_line,
   const lanelet::ConstLanelets & original_lanelets, const lanelet::ConstLanelets & target_lanelets,
-  const double acceleration, const double prepare_duration, const LaneChangePhaseInfo distance,
-  const LaneChangePhaseInfo speed, const BehaviorPathPlannerParameters & params,
+  const double acceleration, const LaneChangePhaseInfo distance, const LaneChangePhaseInfo speed,
   const LaneChangeParameters & lane_change_param)
 {
   PathShifter path_shifter;
@@ -175,9 +174,6 @@ std::optional<LaneChangePath> constructCandidatePath(
     return std::nullopt;
   }
 
-  candidate_path.turn_signal_info = lane_change_utils::calc_turn_signal_info(
-    prepare_segment, speed.prepare, params.minimum_lane_change_prepare_distance, prepare_duration,
-    shift_line, shifted_path);
   // check candidate path is in lanelet
   if (!isPathInLanelets(candidate_path.path, original_lanelets, target_lanelets)) {
     return std::nullopt;
@@ -294,8 +290,7 @@ LaneChangePaths getLaneChangePaths(
     const auto lc_speed = LaneChangePhaseInfo{prepare_speed, lane_changing_speed};
     const auto candidate_path = constructCandidatePath(
       prepare_segment_reference, lane_changing_segment_reference, target_lane_reference_path,
-      shift_line, original_lanelets, target_lanelets, acceleration, lane_change_prepare_duration,
-      lc_dist, lc_speed, common_parameter, parameter);
+      shift_line, original_lanelets, target_lanelets, acceleration, lc_dist, lc_speed, parameter);
 
     if (!candidate_path) {
       continue;
@@ -710,37 +705,6 @@ bool isEgoWithinOriginalLane(
     lanelet::utils::to2D(lane_poly).basicPolygon());
 }
 
-TurnSignalInfo calc_turn_signal_info(
-  const PathWithLaneId & prepare_path, const double prepare_velocity,
-  const double min_prepare_distance, const double prepare_duration, const ShiftLine & shift_line,
-  const ShiftedPath & lane_changing_path)
-{
-  TurnSignalInfo turn_signal_info{};
-  constexpr double turn_signal_start_duration{3.0};
-  turn_signal_info.desired_start_point =
-    std::invoke([&prepare_path, &prepare_velocity, &min_prepare_distance, &prepare_duration]() {
-      if (prepare_velocity * turn_signal_start_duration > min_prepare_distance) {
-        const auto duration = static_cast<double>(prepare_path.points.size()) / prepare_duration;
-        double time{-duration};
-        for (auto itr = prepare_path.points.crbegin(); itr != prepare_path.points.crend(); ++itr) {
-          time += duration;
-          if (time >= turn_signal_start_duration) {
-            return itr->point.pose;
-          }
-        }
-      }
-      return prepare_path.points.front().point.pose;
-    });
-
-  turn_signal_info.required_start_point = shift_line.start;
-  turn_signal_info.required_end_point = std::invoke([&lane_changing_path]() {
-    const auto mid_path_idx = lane_changing_path.path.points.size() / 2;
-    return lane_changing_path.path.points.at(mid_path_idx).point.pose;
-  });
-  turn_signal_info.desired_end_point = shift_line.end;
-  return turn_signal_info;
-}
-
 void get_turn_signal_info(
   const LaneChangePath & lane_change_path, TurnSignalInfo * turn_signal_info)
 {