From 9fb03ac1878d810e2b554476706cb091ee2c558f Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Wed, 24 Jan 2024 18:01:57 +0900 Subject: [PATCH] refactor(lane_change): update lc status in updateData (#6088) * refactor(lane_change): update lc status in updateData Signed-off-by: Muhammad Zulfaqar Azmi * update current lane and target lane during requested Signed-off-by: Muhammad Zulfaqar Azmi * set is_valid_path as false by default Signed-off-by: Muhammad Zulfaqar Azmi * reset flags after finished lane change Signed-off-by: Muhammad Zulfaqar Azmi --------- Signed-off-by: Muhammad Zulfaqar Azmi --- .../scene.hpp | 2 +- .../utils/base_class.hpp | 2 +- .../utils/path.hpp | 2 +- .../src/interface.cpp | 24 ++++++++----------- .../src/scene.cpp | 17 +++++++++---- 5 files changed, 25 insertions(+), 22 deletions(-) diff --git a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp index 9c3c371627a78..f991ca0d849f0 100644 --- a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp @@ -89,7 +89,7 @@ class NormalLaneChange : public LaneChangeBase bool isAbortState() const override; - bool isLaneChangeRequired() const override; + bool isLaneChangeRequired() override; bool isStoppedAtRedTrafficLight() const override; diff --git a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/base_class.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/base_class.hpp index 451899fbf27e6..400d5505dc49f 100644 --- a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/base_class.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/base_class.hpp @@ -80,7 +80,7 @@ class LaneChangeBase virtual bool hasFinishedAbort() const = 0; - virtual bool isLaneChangeRequired() const = 0; + virtual bool isLaneChangeRequired() = 0; virtual bool isAbortState() const = 0; diff --git a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/path.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/path.hpp index 9e42b49635b82..3af4f487a0fa0 100644 --- a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/path.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/path.hpp @@ -45,7 +45,7 @@ struct LaneChangeStatus std::vector lane_follow_lane_ids{}; std::vector lane_change_lane_ids{}; bool is_safe{false}; - bool is_valid_path{true}; + bool is_valid_path{false}; double start_distance{0.0}; }; diff --git a/planning/behavior_path_lane_change_module/src/interface.cpp b/planning/behavior_path_lane_change_module/src/interface.cpp index 448f83ecaf15e..c8e1229fe5af5 100644 --- a/planning/behavior_path_lane_change_module/src/interface.cpp +++ b/planning/behavior_path_lane_change_module/src/interface.cpp @@ -50,9 +50,6 @@ LaneChangeInterface::LaneChangeInterface( void LaneChangeInterface::processOnEntry() { waitApproval(); - module_type_->setPreviousModulePaths( - getPreviousModuleOutput().reference_path, getPreviousModuleOutput().path); - module_type_->updateLaneChangeStatus(); } void LaneChangeInterface::processOnExit() @@ -80,6 +77,14 @@ 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); + + if (isWaitingApproval()) { + module_type_->updateLaneChangeStatus(); + } + setObjectDebugVisualization(); + module_type_->updateSpecialData(); module_type_->resetStopPose(); } @@ -98,8 +103,6 @@ BehaviorModuleOutput LaneChangeInterface::plan() return {}; } - module_type_->setPreviousDrivableAreaInfo(getPreviousModuleOutput().drivable_area_info); - module_type_->setPreviousTurnSignalInfo(getPreviousModuleOutput().turn_signal_info); auto output = module_type_->generateOutput(); path_reference_ = std::make_shared(output.reference_path); *prev_approved_path_ = getPreviousModuleOutput().path; @@ -128,22 +131,14 @@ BehaviorModuleOutput LaneChangeInterface::planWaitingApproval() out.reference_path = getPreviousModuleOutput().reference_path; out.turn_signal_info = getPreviousModuleOutput().turn_signal_info; out.drivable_area_info = getPreviousModuleOutput().drivable_area_info; - - module_type_->setPreviousModulePaths( - getPreviousModuleOutput().reference_path, getPreviousModuleOutput().path); - module_type_->updateLaneChangeStatus(); - setObjectDebugVisualization(); + out.turn_signal_info = getCurrentTurnSignalInfo(out.path, out.turn_signal_info); for (const auto & [uuid, data] : module_type_->getDebugData()) { const auto color = data.is_safe ? ColorName::GREEN : ColorName::RED; setObjectsOfInterestData(data.current_obj_pose, data.obj_shape, color); } - // change turn signal when the vehicle reaches at the end of the path for waiting lane change - out.turn_signal_info = getCurrentTurnSignalInfo(out.path, out.turn_signal_info); - path_reference_ = std::make_shared(getPreviousModuleOutput().reference_path); - stop_pose_ = module_type_->getStopPose(); if (!module_type_->isValidPath()) { @@ -211,6 +206,7 @@ bool LaneChangeInterface::canTransitSuccessState() } if (module_type_->hasFinishedLaneChange()) { + module_type_->resetParameters(); log_debug_throttled("Lane change process has completed."); return true; } diff --git a/planning/behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_lane_change_module/src/scene.cpp index c7ddd8fe63636..f5100f16129c2 100644 --- a/planning/behavior_path_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_lane_change_module/src/scene.cpp @@ -115,17 +115,17 @@ std::pair NormalLaneChange::getSafePath(LaneChangePath & safe_path) return {true, found_safe_path}; } -bool NormalLaneChange::isLaneChangeRequired() const +bool NormalLaneChange::isLaneChangeRequired() { - const auto current_lanes = getCurrentLanes(); + status_.current_lanes = getCurrentLanes(); - if (current_lanes.empty()) { + if (status_.current_lanes.empty()) { return false; } - const auto target_lanes = getLaneChangeLanes(current_lanes, direction_); + status_.target_lanes = getLaneChangeLanes(status_.current_lanes, direction_); - return !target_lanes.empty(); + return !status_.target_lanes.empty(); } bool NormalLaneChange::isStoppedAtRedTrafficLight() const @@ -420,8 +420,15 @@ void NormalLaneChange::resetParameters() is_abort_approval_requested_ = false; current_lane_change_state_ = LaneChangeStates::Normal; abort_path_ = nullptr; + status_ = {}; object_debug_.clear(); + object_debug_after_approval_.clear(); + debug_filtered_objects_.current_lane.clear(); + debug_filtered_objects_.target_lane.clear(); + debug_filtered_objects_.other_lane.clear(); + debug_valid_path_.clear(); + RCLCPP_DEBUG(logger_, "reset all flags and debug information."); } TurnSignalInfo NormalLaneChange::updateOutputTurnSignal()