From 175ecbc55bb2e406a797296268e8f2cc80408f45 Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Fri, 16 Aug 2024 14:58:48 +0900 Subject: [PATCH] refactor(lane_change): update filtered objects only once (#8489) Signed-off-by: Muhammad Zulfaqar Azmi --- .../behavior_path_lane_change_module/scene.hpp | 2 ++ .../utils/base_class.hpp | 3 +++ .../src/interface.cpp | 1 + .../src/scene.cpp | 15 +++++++++------ 4 files changed, 15 insertions(+), 6 deletions(-) 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 3a11b0ed903dc..ac854f6e957ab 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 65f57fa583817..3203745367c36 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; @@ -258,6 +260,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 1bbc2d25fc0bc..005ccd595fb46 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 28efae2549d1e..fe1783b103a54 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_); @@ -1430,8 +1435,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); @@ -1679,7 +1683,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 " @@ -1860,8 +1864,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; @@ -1870,7 +1873,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.");