refactor(lane_change): update filtered objects only once (#8489)

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

Author: zulfaqar-azmi-t4

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

@@ -53,6 +53,8 @@ class NormalLaneChange : public LaneChangeBase
 
   void update_lanes(const bool is_approved) final;
 
+  void update_filtered_objects() final;
+
   void updateLaneChangeStatus() override;
 
   std::pair<bool, bool> getSafePath(LaneChangePath & safe_path) const override;

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

@@ -67,6 +67,8 @@ class LaneChangeBase
 
   virtual void update_lanes(const bool is_approved) = 0;
 
+  virtual void update_filtered_objects() = 0;
+
   virtual void updateLaneChangeStatus() = 0;
 
   virtual std::pair<bool, bool> getSafePath(LaneChangePath & safe_path) const = 0;
@@ -252,6 +254,7 @@ class LaneChangeBase
   std::shared_ptr<LaneChangePath> abort_path_{};
   std::shared_ptr<const PlannerData> planner_data_{};
   lane_change::CommonDataPtr common_data_ptr_{};
+  FilteredByLanesExtendedObjects filtered_objects_{};
   BehaviorModuleOutput prev_module_output_{};
   std::optional<Pose> lane_change_stop_pose_{std::nullopt};
 

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp

@@ -76,6 +76,7 @@ void LaneChangeInterface::updateData()
   universe_utils::ScopedTimeTrack st(__func__, *getTimeKeeper());
   module_type_->setPreviousModuleOutput(getPreviousModuleOutput());
   module_type_->update_lanes(getCurrentStatus() == ModuleStatus::RUNNING);
+  module_type_->update_filtered_objects();
   module_type_->updateSpecialData();
 
   if (isWaitingApproval() || module_type_->isAbortState()) {

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp

@@ -99,6 +99,11 @@ void NormalLaneChange::update_lanes(const bool is_approved)
   *common_data_ptr_->lanes_polygon_ptr = create_lanes_polygon(common_data_ptr_);
 }
 
+void NormalLaneChange::update_filtered_objects()
+{
+  filtered_objects_ = filterObjects();
+}
+
 void NormalLaneChange::updateLaneChangeStatus()
 {
   universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
@@ -1412,8 +1417,7 @@ bool NormalLaneChange::getLaneChangePaths(
     return false;
   }
 
-  const auto filtered_objects = filterObjects();
-  const auto target_objects = getTargetObjects(filtered_objects, current_lanes);
+  const auto target_objects = getTargetObjects(filtered_objects_, current_lanes);
 
   const auto prepare_durations = calcPrepareDuration(current_lanes, target_lanes);
 
@@ -1661,7 +1665,7 @@ bool NormalLaneChange::getLaneChangePaths(
 
         if (
           !is_stuck && !utils::lane_change::passed_parked_objects(
-                         common_data_ptr_, *candidate_path, filtered_objects.target_lane_leading,
+                         common_data_ptr_, *candidate_path, filtered_objects_.target_lane_leading,
                          lane_change_buffer, lane_change_debug_.collision_check_objects)) {
           debug_print_lat(
             "Reject: parking vehicle exists in the target lane, and the ego is not in stuck. Skip "
@@ -1849,8 +1853,7 @@ PathSafetyStatus NormalLaneChange::isApprovedPathSafe() const
     return {true, true};
   }
 
-  const auto filtered_objects = filterObjects();
-  const auto target_objects = getTargetObjects(filtered_objects, current_lanes);
+  const auto target_objects = getTargetObjects(filtered_objects_, current_lanes);
 
   CollisionCheckDebugMap debug_data;
 
@@ -1859,7 +1862,7 @@ PathSafetyStatus NormalLaneChange::isApprovedPathSafe() const
     common_data_ptr_->route_handler_ptr->getLateralIntervalsToPreferredLane(current_lanes.back()));
 
   const auto has_passed_parked_objects = utils::lane_change::passed_parked_objects(
-    common_data_ptr_, path, filtered_objects.target_lane_leading, min_lc_length, debug_data);
+    common_data_ptr_, path, filtered_objects_.target_lane_leading, min_lc_length, debug_data);
 
   if (!has_passed_parked_objects) {
     RCLCPP_DEBUG(logger_, "Lane change has been delayed.");