refactor(lane_change): make return previous output a common function (#6784)

* refactor(lane_change): make return previous output a common function

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

* Replace all prev module's variable

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

* fix build error in ablc

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

* slight refactoring

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

---------

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

Author: zulfaqar-azmi-t4

planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp

@@ -125,7 +125,7 @@ AvoidancePlanningData AvoidanceByLaneChange::calcAvoidancePlanningData(
   // reference pose
   data.reference_pose = getEgoPose();
 
-  data.reference_path_rough = prev_module_path_;
+  data.reference_path_rough = prev_module_output_.path;
 
   const auto resample_interval = avoidance_parameters_->resample_interval_for_planning;
   data.reference_path = utils::resamplePathWithSpline(data.reference_path_rough, resample_interval);

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

@@ -111,25 +111,9 @@ class LaneChangeBase
 
   virtual bool specialExpiredCheck() const { return false; }
 
-  virtual void setPreviousModulePaths(
-    const PathWithLaneId & prev_module_reference_path, const PathWithLaneId & prev_module_path)
+  void setPreviousModuleOutput(const BehaviorModuleOutput & prev_module_output)
   {
-    if (!prev_module_reference_path.points.empty()) {
-      prev_module_reference_path_ = prev_module_reference_path;
-    }
-    if (!prev_module_path.points.empty()) {
-      prev_module_path_ = prev_module_path;
-    }
-  };
-
-  virtual void setPreviousDrivableAreaInfo(const DrivableAreaInfo & prev_drivable_area_info)
-  {
-    prev_drivable_area_info_ = prev_drivable_area_info;
-  }
-
-  virtual void setPreviousTurnSignalInfo(const TurnSignalInfo & prev_turn_signal_info)
-  {
-    prev_turn_signal_info_ = prev_turn_signal_info;
+    prev_module_output_ = prev_module_output;
   }
 
   virtual void updateSpecialData() {}
@@ -233,10 +217,7 @@ class LaneChangeBase
   std::shared_ptr<LaneChangeParameters> lane_change_parameters_{};
   std::shared_ptr<LaneChangePath> abort_path_{};
   std::shared_ptr<const PlannerData> planner_data_{};
-  PathWithLaneId prev_module_reference_path_{};
-  PathWithLaneId prev_module_path_{};
-  DrivableAreaInfo prev_drivable_area_info_{};
-  TurnSignalInfo prev_turn_signal_info_{};
+  BehaviorModuleOutput prev_module_output_{};
   std::optional<Pose> lane_change_stop_pose_{std::nullopt};
 
   PathWithLaneId prev_approved_path_{};

planning/behavior_path_lane_change_module/src/interface.cpp

@@ -76,10 +76,7 @@ bool LaneChangeInterface::isExecutionReady() const
 
 void LaneChangeInterface::updateData()
 {
-  module_type_->setPreviousModulePaths(
-    getPreviousModuleOutput().reference_path, getPreviousModuleOutput().path);
-  module_type_->setPreviousDrivableAreaInfo(getPreviousModuleOutput().drivable_area_info);
-  module_type_->setPreviousTurnSignalInfo(getPreviousModuleOutput().turn_signal_info);
+  module_type_->setPreviousModuleOutput(getPreviousModuleOutput());
   module_type_->updateSpecialData();
 
   if (isWaitingApproval()) {

planning/behavior_path_lane_change_module/src/scene.cpp

@@ -140,73 +140,56 @@ LaneChangePath NormalLaneChange::getLaneChangePath() const
 
 BehaviorModuleOutput NormalLaneChange::getTerminalLaneChangePath() const
 {
-  BehaviorModuleOutput output;
+  auto output = prev_module_output_;
 
   if (isAbortState() && abort_path_) {
     output.path = abort_path_->path;
-    output.reference_path = prev_module_reference_path_;
-    output.turn_signal_info = prev_turn_signal_info_;
     extendOutputDrivableArea(output);
     const auto current_seg_idx = planner_data_->findEgoSegmentIndex(output.path.points);
     output.turn_signal_info = planner_data_->turn_signal_decider.use_prior_turn_signal(
-      output.path, getEgoPose(), current_seg_idx, prev_turn_signal_info_, output.turn_signal_info,
-      planner_data_->parameters.ego_nearest_dist_threshold,
+      output.path, getEgoPose(), current_seg_idx, prev_module_output_.turn_signal_info,
+      output.turn_signal_info, planner_data_->parameters.ego_nearest_dist_threshold,
       planner_data_->parameters.ego_nearest_yaw_threshold);
     return output;
   }
 
   const auto current_lanes = getCurrentLanes();
   if (current_lanes.empty()) {
-    RCLCPP_WARN(logger_, "Current lanes not found!!! Something wrong.");
-    output.path = prev_module_path_;
-    output.reference_path = prev_module_reference_path_;
-    output.drivable_area_info = prev_drivable_area_info_;
-    output.turn_signal_info = prev_turn_signal_info_;
-    return output;
+    RCLCPP_DEBUG(logger_, "Current lanes not found. Returning previous module's path as output.");
+    return prev_module_output_;
   }
 
   const auto terminal_path =
     calcTerminalLaneChangePath(current_lanes, getLaneChangeLanes(current_lanes, direction_));
   if (!terminal_path) {
-    RCLCPP_DEBUG(logger_, "Terminal path not found!!!");
-    output.path = prev_module_path_;
-    output.reference_path = prev_module_reference_path_;
-    output.drivable_area_info = prev_drivable_area_info_;
-    output.turn_signal_info = prev_turn_signal_info_;
-    return output;
+    RCLCPP_DEBUG(logger_, "Terminal path not found. Returning previous module's path as output.");
+    return prev_module_output_;
   }
 
   output.path = terminal_path->path;
-  output.reference_path = prev_module_reference_path_;
   output.turn_signal_info = updateOutputTurnSignal();
 
   extendOutputDrivableArea(output);
 
   const auto current_seg_idx = planner_data_->findEgoSegmentIndex(output.path.points);
   output.turn_signal_info = planner_data_->turn_signal_decider.use_prior_turn_signal(
-    output.path, getEgoPose(), current_seg_idx, prev_turn_signal_info_, output.turn_signal_info,
-    planner_data_->parameters.ego_nearest_dist_threshold,
+    output.path, getEgoPose(), current_seg_idx, prev_module_output_.turn_signal_info,
+    output.turn_signal_info, planner_data_->parameters.ego_nearest_dist_threshold,
     planner_data_->parameters.ego_nearest_yaw_threshold);
 
   return output;
 }
 
 BehaviorModuleOutput NormalLaneChange::generateOutput()
 {
-  BehaviorModuleOutput output;
-
   if (!status_.is_valid_path) {
-    output.path = prev_module_path_;
-    output.reference_path = prev_module_reference_path_;
-    output.drivable_area_info = prev_drivable_area_info_;
-    output.turn_signal_info = prev_turn_signal_info_;
-    return output;
+    RCLCPP_DEBUG(logger_, "No valid path found. Returning previous module's path as output.");
+    return prev_module_output_;
   }
 
+  auto output = prev_module_output_;
   if (isAbortState() && abort_path_) {
     output.path = abort_path_->path;
-    output.reference_path = prev_module_reference_path_;
-    output.turn_signal_info = prev_turn_signal_info_;
     insertStopPoint(status_.current_lanes, output.path);
   } else {
     output.path = getLaneChangePath().path;
@@ -235,8 +218,8 @@ BehaviorModuleOutput NormalLaneChange::generateOutput()
 
   const auto current_seg_idx = planner_data_->findEgoSegmentIndex(output.path.points);
   output.turn_signal_info = planner_data_->turn_signal_decider.use_prior_turn_signal(
-    output.path, getEgoPose(), current_seg_idx, prev_turn_signal_info_, output.turn_signal_info,
-    planner_data_->parameters.ego_nearest_dist_threshold,
+    output.path, getEgoPose(), current_seg_idx, prev_module_output_.turn_signal_info,
+    output.turn_signal_info, planner_data_->parameters.ego_nearest_dist_threshold,
     planner_data_->parameters.ego_nearest_yaw_threshold);
 
   return output;
@@ -256,8 +239,8 @@ void NormalLaneChange::extendOutputDrivableArea(BehaviorModuleOutput & output) c
   // for new architecture
   DrivableAreaInfo current_drivable_area_info;
   current_drivable_area_info.drivable_lanes = expanded_lanes;
-  output.drivable_area_info =
-    utils::combineDrivableAreaInfo(current_drivable_area_info, prev_drivable_area_info_);
+  output.drivable_area_info = utils::combineDrivableAreaInfo(
+    current_drivable_area_info, prev_module_output_.drivable_area_info);
 }
 
 void NormalLaneChange::insertStopPoint(
@@ -506,7 +489,7 @@ TurnSignalInfo NormalLaneChange::updateOutputTurnSignal() const
 
 lanelet::ConstLanelets NormalLaneChange::getCurrentLanes() const
 {
-  return utils::getCurrentLanesFromPath(prev_module_path_, planner_data_);
+  return utils::getCurrentLanesFromPath(prev_module_output_.path, planner_data_);
 }
 
 lanelet::ConstLanelets NormalLaneChange::getLaneChangeLanes(
@@ -738,10 +721,10 @@ std::pair<double, double> NormalLaneChange::calcCurrentMinMaxAcceleration() cons
   const auto vehicle_max_acc = std::min(p.max_acc, lane_change_parameters_->max_longitudinal_acc);
 
   const auto ego_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints(
-    prev_module_path_.points, getEgoPose(), p.ego_nearest_dist_threshold,
+    prev_module_output_.path.points, getEgoPose(), p.ego_nearest_dist_threshold,
     p.ego_nearest_yaw_threshold);
   const auto max_path_velocity =
-    prev_module_path_.points.at(ego_seg_idx).point.longitudinal_velocity_mps;
+    prev_module_output_.path.points.at(ego_seg_idx).point.longitudinal_velocity_mps;
 
   // calculate minimum and maximum acceleration
   const auto min_acc = utils::lane_change::calcMinimumAcceleration(
@@ -765,7 +748,7 @@ double NormalLaneChange::calcMaximumLaneChangeLength(
 std::vector<double> NormalLaneChange::sampleLongitudinalAccValues(
   const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes) const
 {
-  if (prev_module_path_.points.empty()) {
+  if (prev_module_output_.path.points.empty()) {
     return {};
   }
 
@@ -852,7 +835,7 @@ PathWithLaneId NormalLaneChange::getPrepareSegment(
     return PathWithLaneId();
   }
 
-  auto prepare_segment = prev_module_path_;
+  auto prepare_segment = prev_module_output_.path;
   const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints(
     prepare_segment.points, getEgoPose(), 3.0, 1.0);
   utils::clipPathLength(prepare_segment, current_seg_idx, prepare_length, backward_path_length);
@@ -1570,7 +1553,7 @@ std::optional<LaneChangePath> NormalLaneChange::calcTerminalLaneChangePath(
   }
 
   const auto lane_changing_start_pose = motion_utils::calcLongitudinalOffsetPose(
-    prev_module_path_.points, current_lane_terminal_point,
+    prev_module_output_.path.points, current_lane_terminal_point,
     -(lane_change_buffer + next_lane_change_buffer + distance_to_terminal_from_goal));
 
   if (!lane_changing_start_pose) {
@@ -1653,7 +1636,7 @@ std::optional<LaneChangePath> NormalLaneChange::calcTerminalLaneChangePath(
     lane_changing_start_pose.value(), target_segment.points.front().point.pose,
     target_lane_reference_path, shift_length);
 
-  auto reference_segment = prev_module_path_;
+  auto reference_segment = prev_module_output_.path;
   const double length_to_lane_changing_start = motion_utils::calcSignedArcLength(
     reference_segment.points, reference_segment.points.front().point.pose.position,
     lane_changing_start_pose->position);
@@ -1877,7 +1860,7 @@ bool NormalLaneChange::calcAbortPath()
     RCLCPP_ERROR(logger_, "failed to generate abort shifted path.");
   }
 
-  PathWithLaneId reference_lane_segment = prev_module_path_;
+  auto reference_lane_segment = prev_module_output_.path;
   {
     const auto terminal_path =
       calcTerminalLaneChangePath(reference_lanelets, selected_path.info.target_lanes);
@@ -2163,5 +2146,4 @@ bool NormalLaneChange::check_prepare_phase() const
 
   return check_in_intersection || check_in_turns || check_in_general_lanes;
 }
-
 }  // namespace behavior_path_planner