refactor(lane_change): move getCurrentTurnSignalInfo to scene file (#6943)

* refactor(lane_change): move getCurrentTurnSignalInfo to scene file

Signed-off-by: Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

* style(pre-commit): autofix

* removed reference from trivial variable

Signed-off-by: Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

---------

Signed-off-by: Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

Author: zulfaqar-azmi-t4

planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/interface.hpp

@@ -93,9 +93,6 @@ class LaneChangeInterface : public SceneModuleInterface
 
   MarkerArray getModuleVirtualWall() override;
 
-  TurnSignalInfo getCurrentTurnSignalInfo(
-    const PathWithLaneId & path, const TurnSignalInfo & original_turn_signal_info);
-
   // TODO(someone): remove this, and use base class function
   [[deprecated]] BehaviorModuleOutput run() override
   {

planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp

@@ -101,6 +101,8 @@ class NormalLaneChange : public LaneChangeBase
 
   bool isStoppedAtRedTrafficLight() const override;
 
+  TurnSignalInfo get_current_turn_signal_info() override;
+
 protected:
   lanelet::ConstLanelets getCurrentLanes() const override;
 

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

@@ -187,6 +187,8 @@ class LaneChangeBase
 
   void resetStopPose() { lane_change_stop_pose_ = std::nullopt; }
 
+  virtual TurnSignalInfo get_current_turn_signal_info() = 0;
+
 protected:
   virtual lanelet::ConstLanelets getCurrentLanes() const = 0;
 

planning/behavior_path_lane_change_module/src/interface.cpp

@@ -135,8 +135,7 @@ BehaviorModuleOutput LaneChangeInterface::planWaitingApproval()
 
   BehaviorModuleOutput out = getPreviousModuleOutput();
   module_type_->insertStopPoint(module_type_->getLaneChangeStatus().current_lanes, out.path);
-  out.turn_signal_info =
-    getCurrentTurnSignalInfo(out.path, getPreviousModuleOutput().turn_signal_info);
+  out.turn_signal_info = module_type_->get_current_turn_signal_info();
 
   const auto & lane_change_debug = module_type_->getDebugData();
   for (const auto & [uuid, data] : lane_change_debug.collision_check_objects) {
@@ -367,81 +366,4 @@ void LaneChangeInterface::updateSteeringFactorPtr(
     {output.start_distance_to_path_change, output.finish_distance_to_path_change},
     PlanningBehavior::LANE_CHANGE, steering_factor_direction, SteeringFactor::APPROACHING, "");
 }
-
-TurnSignalInfo LaneChangeInterface::getCurrentTurnSignalInfo(
-  const PathWithLaneId & path, const TurnSignalInfo & original_turn_signal_info)
-{
-  const auto & current_lanes = module_type_->getLaneChangeStatus().current_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 || current_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.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 & 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(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 path_length = motion_utils::calcArcLength(path.points);
-  const size_t & current_nearest_seg_idx =
-    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 =
-    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 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;
-}
 }  // namespace behavior_path_planner

planning/behavior_path_lane_change_module/src/scene.cpp

@@ -136,6 +136,83 @@ bool NormalLaneChange::isStoppedAtRedTrafficLight() const
     status_.lane_change_path.info.length.sum());
 }
 
+TurnSignalInfo NormalLaneChange::get_current_turn_signal_info()
+{
+  const auto original_turn_signal_info = prev_module_output_.turn_signal_info;
+
+  const auto & current_lanes = getLaneChangeStatus().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) {
+    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;
+  }
+
+  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 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();
+  const auto shift_intervals =
+    route_handler->getLateralIntervalsToPreferredLane(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 path_length = motion_utils::calcArcLength(path.points);
+  const size_t current_nearest_seg_idx =
+    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 =
+    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().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;
+}
+
 LaneChangePath NormalLaneChange::getLaneChangePath() const
 {
   return status_.lane_change_path;