fix(lane_change): activate turn signal as soon as we have the intention to change lanes

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp

@@ -109,14 +109,16 @@ class NormalLaneChange : public LaneChangeBase
 
   bool isStoppedAtRedTrafficLight() const override;
 
-  TurnSignalInfo get_current_turn_signal_info() override;
+  TurnSignalInfo get_current_turn_signal_info() const override;
 
 protected:
   lanelet::ConstLanelets getLaneChangeLanes(
     const lanelet::ConstLanelets & current_lanes, Direction direction) const override;
 
   int getNumToPreferredLane(const lanelet::ConstLanelet & lane) const override;
 
+  TurnSignalInfo get_terminal_turn_signal_info() const override;
+
   std::vector<double> sampleLongitudinalAccValues(
     const lanelet::ConstLanelets & current_lanes,
     const lanelet::ConstLanelets & target_lanes) const;

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp

@@ -213,7 +213,7 @@
 
   LaneChangeModuleType getModuleType() const { return type_; }
 
-  TurnSignalDecider getTurnSignalDecider() { return planner_data_->turn_signal_decider; }
+  TurnSignalDecider getTurnSignalDecider() const { return planner_data_->turn_signal_decider; }
 
   Direction getDirection() const
   {
@@ -229,7 +229,7 @@
 
   void resetStopPose() { lane_change_stop_pose_ = std::nullopt; }
 
-  virtual TurnSignalInfo get_current_turn_signal_info() = 0;
+  virtual TurnSignalInfo get_current_turn_signal_info() const = 0;
 
 protected:
   virtual int getNumToPreferredLane(const lanelet::ConstLanelet & lane) const = 0;
@@ -245,6 +245,31 @@
   virtual lanelet::ConstLanelets getLaneChangeLanes(
     const lanelet::ConstLanelets & current_lanes, Direction direction) const = 0;
 
+  virtual TurnSignalInfo get_terminal_turn_signal_info() const = 0;
+
+  TurnSignalInfo get_turn_signal(const Pose & start, const Pose & end) const
+  {
+    TurnSignalInfo turn_signal;
+    switch (direction_) {
+      case Direction::LEFT:
+        turn_signal.turn_signal.command = TurnIndicatorsCommand::ENABLE_LEFT;
+        break;
+      case Direction::RIGHT:
+        turn_signal.turn_signal.command = TurnIndicatorsCommand::ENABLE_RIGHT;
+        break;
+      default:
+        turn_signal.turn_signal.command = TurnIndicatorsCommand::NO_COMMAND;
+        break;
+    }
+
+    turn_signal.desired_start_point = start;
+    turn_signal.desired_end_point = end;
+    turn_signal.required_start_point = turn_signal.desired_start_point;
+    turn_signal.required_end_point = turn_signal.desired_end_point;
+
+    return turn_signal;
+  }
+
   LaneChangeStatus status_{};
   PathShifter path_shifter_{};
 

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/calculation.hpp

@@ -73,7 +73,7 @@ double calc_stopping_distance(const LCParamPtr & lc_param_ptr);
  * @return distance to last fit width position along the lane
  */
 double calc_dist_to_last_fit_width(
-  const lanelet::ConstLanelets lanelets, const Pose & src_pose,
+  const lanelet::ConstLanelets & lanelets, const Pose & src_pose,
   const BehaviorPathPlannerParameters & bpp_param, const double margin = 0.1);
 
 /**

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp

@@ -1,4 +1,4 @@
 // Copyright 2023 TIER IV, Inc.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -181,81 +181,64 @@
     status_.lane_change_path.info.length.sum());
 }
 
-TurnSignalInfo NormalLaneChange::get_current_turn_signal_info()
+TurnSignalInfo NormalLaneChange::get_current_turn_signal_info() const
 {
   const auto original_turn_signal_info = prev_module_output_.turn_signal_info;
 
-  const auto & current_lanes = get_current_lanes();
-  const auto is_valid = getLaneChangeStatus().is_valid_path;
-  const auto & lane_change_path = getLaneChangeStatus().lane_change_path;
-  const auto & lane_change_param = getLaneChangeParam();
-
-  if (getModuleType() != LaneChangeModuleType::NORMAL || current_lanes.empty() || !is_valid) {
+  if (getModuleType() != LaneChangeModuleType::NORMAL || get_current_lanes().empty()) {
     return original_turn_signal_info;
   }
 
-  // check direction
-  TurnSignalInfo current_turn_signal_info;
-  const auto & current_pose = getEgoPose();
-  const auto direction = getDirection();
-  if (direction == Direction::LEFT) {
-    current_turn_signal_info.turn_signal.command = TurnIndicatorsCommand::ENABLE_LEFT;
-  } else if (direction == Direction::RIGHT) {
-    current_turn_signal_info.turn_signal.command = TurnIndicatorsCommand::ENABLE_RIGHT;
+  if (direction_ != Direction::LEFT && direction_ != Direction::RIGHT) {
+    return original_turn_signal_info;
   }
 
   const auto & path = prev_module_output_.path;
-  if (path.points.empty()) {
-    current_turn_signal_info.desired_start_point = current_pose;
-    current_turn_signal_info.required_start_point = current_pose;
-    current_turn_signal_info.desired_end_point = lane_change_path.info.lane_changing_end;
-    current_turn_signal_info.required_end_point = lane_change_path.info.lane_changing_end;
-    return current_turn_signal_info;
+  const auto & original_command = original_turn_signal_info.turn_signal.command;
+  if (
+    !path.points.empty() && original_command != TurnIndicatorsCommand::DISABLE &&
+    original_command != TurnIndicatorsCommand::NO_COMMAND) {
+    return get_terminal_turn_signal_info();
   }
 
-  const auto min_length_for_turn_signal_activation =
-    lane_change_param.min_length_for_turn_signal_activation;
-  const auto & route_handler = getRouteHandler();
-  const auto & common_parameter = getCommonParam();
+  return get_turn_signal(getEgoPose(), getLaneChangePath().info.lane_changing_end);
+}
+
+TurnSignalInfo NormalLaneChange::get_terminal_turn_signal_info() const
+{
+  const auto & lane_change_param = getLaneChangeParam();
+  const auto & common_param = getCommonParam();
+  const auto & current_pose = getEgoPose();
+  const auto & path = prev_module_output_.path;
+
+  const auto original_turn_signal_info = prev_module_output_.turn_signal_info;
+
   const auto shift_intervals =
-    route_handler->getLateralIntervalsToPreferredLane(current_lanes.back());
+    getRouteHandler()->getLateralIntervalsToPreferredLane(get_current_lanes().back());
   const double next_lane_change_buffer =
     utils::lane_change::calcMinimumLaneChangeLength(lane_change_param, shift_intervals);
-  const double nearest_dist_threshold = common_parameter.ego_nearest_dist_threshold;
-  const double nearest_yaw_threshold = common_parameter.ego_nearest_yaw_threshold;
-  const double base_to_front = common_parameter.base_link2front;
 
-  const double buffer =
-    next_lane_change_buffer + min_length_for_turn_signal_activation + base_to_front;
+  const double buffer = next_lane_change_buffer +
+                        lane_change_param.min_length_for_turn_signal_activation +
+                        common_param.base_link2front;
   const double path_length = autoware::motion_utils::calcArcLength(path.points);
-  const size_t current_nearest_seg_idx =
-    autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints(
-      path.points, current_pose, nearest_dist_threshold, nearest_yaw_threshold);
-  const double dist_to_terminal = utils::getDistanceToEndOfLane(current_pose, current_lanes);
   const auto start_pose = autoware::motion_utils::calcLongitudinalOffsetPose(
     path.points, 0, std::max(path_length - buffer, 0.0));
-  if (dist_to_terminal - base_to_front < buffer && start_pose) {
-    // modify turn signal
-    current_turn_signal_info.desired_start_point = *start_pose;
-    current_turn_signal_info.desired_end_point = lane_change_path.info.lane_changing_end;
-    current_turn_signal_info.required_start_point = current_turn_signal_info.desired_start_point;
-    current_turn_signal_info.required_end_point = current_turn_signal_info.desired_end_point;
-
-    const auto & original_command = original_turn_signal_info.turn_signal.command;
-    if (
-      original_command == TurnIndicatorsCommand::DISABLE ||
-      original_command == TurnIndicatorsCommand::NO_COMMAND) {
-      return current_turn_signal_info;
-    }
 
-    // check the priority of turn signals
-    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);
-  }
+  if (!start_pose) return original_turn_signal_info;
+
+  const auto terminal_turn_signal_info =
+    get_turn_signal(*start_pose, getLaneChangePath().info.lane_changing_end);
+
+  const double nearest_dist_threshold = common_param.ego_nearest_dist_threshold;
+  const double nearest_yaw_threshold = common_param.ego_nearest_yaw_threshold;
+  const size_t current_nearest_seg_idx =
+    autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints(
+      path.points, current_pose, nearest_dist_threshold, nearest_yaw_threshold);
 
-  // not in the vicinity of the end of the path. return original
-  return original_turn_signal_info;
+  return getTurnSignalDecider().overwrite_turn_signal(
+    path, current_pose, current_nearest_seg_idx, original_turn_signal_info,
+    terminal_turn_signal_info, nearest_dist_threshold, nearest_yaw_threshold);
 }
 
 LaneChangePath NormalLaneChange::getLaneChangePath() const
@@ -319,14 +302,15 @@
     output.path = abort_path_->path;
     insertStopPoint(get_current_lanes(), output.path);
   } else {
-    output.path = getLaneChangePath().path;
+    output.path = status_.lane_change_path.path;
 
     const auto found_extended_path = extendPath();
     if (found_extended_path) {
       output.path = utils::combinePath(output.path, *found_extended_path);
     }
     output.reference_path = getReferencePath();
-    output.turn_signal_info = updateOutputTurnSignal();
+    output.turn_signal_info =
+      get_turn_signal(getEgoPose(), status_.lane_change_path.info.lane_changing_end);
 
     if (isStopState()) {
       const auto current_velocity = getEgoVelocity();

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp

@@ -58,7 +58,7 @@ double calc_stopping_distance(const LCParamPtr & lc_param_ptr)
 }
 
 double calc_dist_to_last_fit_width(
-  const lanelet::ConstLanelets lanelets, const Pose & src_pose,
+  const lanelet::ConstLanelets & lanelets, const Pose & src_pose,
   const BehaviorPathPlannerParameters & bpp_param, const double margin)
 {
   if (lanelets.empty()) return 0.0;