feat(behavior_path_planner): enable lane change to activate turn sign…

…al when stopping (#3930)

planning/behavior_path_planner/config/lane_change/lane_change.param.yaml

@@ -6,6 +6,7 @@
 
       backward_length_buffer_for_end_of_lane: 3.0 # [m]
       lane_change_finish_judge_buffer: 2.0      # [m]
+      min_length_for_turn_signal_activation: 10.0 #[m]
 
       lane_changing_lateral_jerk: 0.5              # [m/s3]
       lateral_acc_switching_velocity: 4.0          #[m/s]

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

@@ -137,6 +137,8 @@ class LaneChangeBase
 
   const BehaviorPathPlannerParameters & getCommonParam() const { return planner_data_->parameters; }
 
+  LaneChangeParameters getLaneChangeParam() const { return *lane_change_parameters_; }
+
   bool isCancelEnabled() const { return lane_change_parameters_->enable_cancel_lane_change; }
 
   bool isAbortEnabled() const { return lane_change_parameters_->enable_abort_lane_change; }
@@ -165,6 +167,10 @@ class LaneChangeBase
 
   std::string getModuleTypeStr() const { return std::string{magic_enum::enum_name(type_)}; }
 
+  LaneChangeModuleType getModuleType() const { return type_; }
+
+  TurnSignalDecider getTurnSignalDecider() { return planner_data_->turn_signal_decider; }
+
   Direction getDirection() const
   {
     if (direction_ == Direction::NONE && !status_.lane_change_path.path.points.empty()) {

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

@@ -93,6 +93,9 @@ class LaneChangeInterface : public SceneModuleInterface
 
   MarkerArray getModuleVirtualWall() override;
 
+  TurnSignalInfo getCurrentTurnSignalInfo(
+    const PathWithLaneId & path, const TurnSignalInfo & original_turn_signal_info);
+
 protected:
   std::shared_ptr<LaneChangeParameters> parameters_;
 

planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp

@@ -35,6 +35,9 @@ struct LaneChangeParameters
   int longitudinal_acc_sampling_num{10};
   int lateral_acc_sampling_num{10};
 
+  // turn signal
+  double min_length_for_turn_signal_activation{10.0};
+
   // acceleration data
   double min_longitudinal_acc{-1.0};
   double max_longitudinal_acc{1.0};

planning/behavior_path_planner/src/behavior_path_planner_node.cpp

@@ -778,6 +778,10 @@ LaneChangeParameters BehaviorPathPlannerNode::getLaneChangeParam()
   p.lateral_acc_sampling_num =
     declare_parameter<int>(parameter("lateral_acceleration_sampling_num"));
 
+  // min length
+  p.min_length_for_turn_signal_activation =
+    declare_parameter<double>(parameter("min_length_for_turn_signal_activation"));
+
   // acceleration
   p.min_longitudinal_acc = declare_parameter<double>(parameter("min_longitudinal_acc"));
   p.max_longitudinal_acc = declare_parameter<double>(parameter("max_longitudinal_acc"));

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

@@ -249,6 +249,9 @@ BehaviorModuleOutput LaneChangeInterface::planWaitingApproval()
     getPreviousModuleOutput().reference_path, getPreviousModuleOutput().path);
   module_type_->updateLaneChangeStatus();
 
+  // change turn signal when the vehicle reaches at the end of the path for waiting lane change
+  out.turn_signal_info = getCurrentTurnSignalInfo(*out.path, out.turn_signal_info);
+
   if (!module_type_->isValidPath()) {
     path_candidate_ = std::make_shared<PathWithLaneId>();
     return out;
@@ -407,6 +410,84 @@ void LaneChangeInterface::acceptVisitor(const std::shared_ptr<SceneModuleVisitor
   }
 }
 
+TurnSignalInfo LaneChangeInterface::getCurrentTurnSignalInfo(
+  const PathWithLaneId & path, const TurnSignalInfo & original_turn_signal_info)
+{
+  const auto & target_lanes = module_type_->getLaneChangeStatus().lane_change_lanes;
+  const auto & is_valid = module_type_->getLaneChangeStatus().is_valid_path;
+  const auto & lane_change_path = module_type_->getLaneChangeStatus().lane_change_path;
+  const auto & lane_change_param = module_type_->getLaneChangeParam();
+
+  if (
+    module_type_->getModuleType() != LaneChangeModuleType::NORMAL || target_lanes.empty() ||
+    !is_valid) {
+    return original_turn_signal_info;
+  }
+
+  // check direction
+  TurnSignalInfo current_turn_signal_info;
+  const auto & current_pose = module_type_->getEgoPose();
+  const auto direction = module_type_->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 (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.lane_changing_end;
+    current_turn_signal_info.required_end_point = lane_change_path.lane_changing_end;
+    return current_turn_signal_info;
+  }
+
+  const auto & min_length_for_turn_signal_activation =
+    lane_change_param.min_length_for_turn_signal_activation;
+  const auto & route_handler = module_type_->getRouteHandler();
+  const auto & common_parameter = module_type_->getCommonParam();
+  const auto shift_intervals =
+    route_handler->getLateralIntervalsToPreferredLane(target_lanes.back());
+  const double next_lane_change_buffer =
+    utils::calcMinimumLaneChangeLength(common_parameter, 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 buffer = next_lane_change_buffer + min_length_for_turn_signal_activation;
+  const double path_length = motion_utils::calcArcLength(path.points);
+  const auto & front_point = path.points.front().point.pose.position;
+  const size_t & current_nearest_seg_idx =
+    motion_utils::findFirstNearestSegmentIndexWithSoftConstraints(
+      path.points, current_pose, nearest_dist_threshold, nearest_yaw_threshold);
+  const double length_front_to_ego = motion_utils::calcSignedArcLength(
+    path.points, front_point, static_cast<size_t>(0), current_pose.position,
+    current_nearest_seg_idx);
+  const auto start_pose =
+    motion_utils::calcLongitudinalOffsetPose(path.points, 0, std::max(path_length - buffer, 0.0));
+  if (path_length - length_front_to_ego < 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.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 module_type_->getTurnSignalDecider().use_prior_turn_signal(
+      path, current_pose, current_nearest_seg_idx, original_turn_signal_info,
+      current_turn_signal_info, nearest_dist_threshold, nearest_yaw_threshold);
+  }
+
+  // not in the vicinity of the end of the path. return original
+  return original_turn_signal_info;
+}
+
 void SceneModuleVisitor::visitLaneChangeInterface(const LaneChangeInterface * interface) const
 {
   lane_change_visitor_ = interface->get_debug_msg_array();