diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp index 690db082f601..e74a407b5a7e 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp @@ -83,6 +83,7 @@ class AvoidanceModule : public SceneModuleInterface RegisteredShiftPointArray left_shift_array_; RegisteredShiftPointArray right_shift_array_; + UUID candidate_uuid_; UUID uuid_left_; UUID uuid_right_; @@ -91,11 +92,13 @@ class AvoidanceModule : public SceneModuleInterface if (candidate.lateral_shift > 0.0) { rtc_interface_left_.updateCooperateStatus( uuid_left_, isExecutionReady(), candidate.distance_to_path_change, clock_->now()); + candidate_uuid_ = uuid_left_; return; } if (candidate.lateral_shift < 0.0) { rtc_interface_right_.updateCooperateStatus( uuid_right_, isExecutionReady(), candidate.distance_to_path_change, clock_->now()); + candidate_uuid_ = uuid_right_; return; } @@ -126,6 +129,15 @@ class AvoidanceModule : public SceneModuleInterface rtc_interface_right_.clearCooperateStatus(); } + void removeCandidateRTCStatus() + { + if (rtc_interface_left_.isRegistered(candidate_uuid_)) { + rtc_interface_left_.removeCooperateStatus(candidate_uuid_); + } else if (rtc_interface_right_.isRegistered(candidate_uuid_)) { + rtc_interface_right_.removeCooperateStatus(candidate_uuid_); + } + } + void removePreviousRTCStatusLeft() { if (rtc_interface_left_.isRegistered(uuid_left_)) { diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp index d39c0c7173d7..3117e1ff8da7 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp @@ -2129,6 +2129,7 @@ BehaviorModuleOutput AvoidanceModule::plan() addShiftPointIfApproved(*new_shift_points); } else if (isWaitingApproval()) { clearWaitingApproval(); + removeCandidateRTCStatus(); } // generate path with shift points that have been inserted. @@ -2219,6 +2220,9 @@ BehaviorModuleOutput AvoidanceModule::planWaitingApproval() if (candidate.distance_to_path_change > threshold_to_update_status) { updateCandidateRTCStatus(candidate); waitApproval(); + } else { + clearWaitingApproval(); + removeCandidateRTCStatus(); } out.path_candidate = std::make_shared(candidate.path_candidate); return out;