diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp index 64882b3c6add..ff3af92442fb 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp +++ b/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 getSafePath(LaneChangePath & safe_path) const override; diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp index 1ebe1de4641a..6c7dc31e857e 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp +++ b/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 getSafePath(LaneChangePath & safe_path) const = 0; @@ -252,6 +254,7 @@ class LaneChangeBase std::shared_ptr abort_path_{}; std::shared_ptr planner_data_{}; lane_change::CommonDataPtr common_data_ptr_{}; + FilteredByLanesExtendedObjects filtered_objects_{}; BehaviorModuleOutput prev_module_output_{}; std::optional lane_change_stop_pose_{std::nullopt}; diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp index 6451d6abadfa..a7cf89193b37 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp +++ b/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()) { diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp index 850283157247..c71c971e7b51 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp +++ b/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.");