From 5f723c6390f5aa91301547942fcf21814d5c2594 Mon Sep 17 00:00:00 2001 From: "tomoya.kimura" Date: Fri, 19 Aug 2022 18:44:15 +0900 Subject: [PATCH 1/5] fix: remove rtc staus when no avoidance candidate is output Signed-off-by: tomoya.kimura --- .../src/scene_module/avoidance/avoidance_module.cpp | 4 ++++ 1 file changed, 4 insertions(+) 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 d39c0c7173d7c..938cc3466835a 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(); + removeRTCStatus(); } // 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(); + removeRTCStatus(); } out.path_candidate = std::make_shared(candidate.path_candidate); return out; From 5bbf257d880f48a4f4a35cffa480c9b91d1411dd Mon Sep 17 00:00:00 2001 From: "tomoya.kimura" Date: Fri, 19 Aug 2022 19:15:23 +0900 Subject: [PATCH 2/5] remove only candidate rtc status Signed-off-by: tomoya.kimura --- .../scene_module/avoidance/avoidance_module.hpp | 12 ++++++++++++ .../src/scene_module/avoidance/avoidance_module.cpp | 4 ++-- 2 files changed, 14 insertions(+), 2 deletions(-) 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 690db082f601d..bc1d29cd7bf2c 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 removeCandaiteRTCStatus() + { + 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 938cc3466835a..aea7a2aba52cb 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,7 +2129,7 @@ BehaviorModuleOutput AvoidanceModule::plan() addShiftPointIfApproved(*new_shift_points); } else if (isWaitingApproval()) { clearWaitingApproval(); - removeRTCStatus(); + removeCandaiteRTCStatus(); } // generate path with shift points that have been inserted. @@ -2222,7 +2222,7 @@ BehaviorModuleOutput AvoidanceModule::planWaitingApproval() waitApproval(); } else { clearWaitingApproval(); - removeRTCStatus(); + removeCandaiteRTCStatus(); } out.path_candidate = std::make_shared(candidate.path_candidate); return out; From ed42cc6f06f98acf8c35b8bd91e38012190a9e3c Mon Sep 17 00:00:00 2001 From: Tomoya Kimura Date: Mon, 22 Aug 2022 12:30:27 +0900 Subject: [PATCH 3/5] Update planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp Co-authored-by: Fumiya Watanabe --- .../scene_module/avoidance/avoidance_module.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 bc1d29cd7bf2c..e74a407b5a7e4 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 @@ -129,7 +129,7 @@ class AvoidanceModule : public SceneModuleInterface rtc_interface_right_.clearCooperateStatus(); } - void removeCandaiteRTCStatus() + void removeCandidateRTCStatus() { if (rtc_interface_left_.isRegistered(candidate_uuid_)) { rtc_interface_left_.removeCooperateStatus(candidate_uuid_); From fbe4cbad0a4a020de06da03e66d82584a443984e Mon Sep 17 00:00:00 2001 From: Tomoya Kimura Date: Mon, 22 Aug 2022 12:30:32 +0900 Subject: [PATCH 4/5] Update planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp Co-authored-by: Fumiya Watanabe --- .../src/scene_module/avoidance/avoidance_module.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 aea7a2aba52cb..ecff878c45c34 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 @@ -2222,7 +2222,7 @@ BehaviorModuleOutput AvoidanceModule::planWaitingApproval() waitApproval(); } else { clearWaitingApproval(); - removeCandaiteRTCStatus(); + removeCandidateRTCStatus(); } out.path_candidate = std::make_shared(candidate.path_candidate); return out; From d88adfad45ba3ddcd8a962b702a8784507074b54 Mon Sep 17 00:00:00 2001 From: Tomoya Kimura Date: Mon, 22 Aug 2022 12:30:42 +0900 Subject: [PATCH 5/5] Update planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp Co-authored-by: Fumiya Watanabe --- .../src/scene_module/avoidance/avoidance_module.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 ecff878c45c34..3117e1ff8da7d 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,7 +2129,7 @@ BehaviorModuleOutput AvoidanceModule::plan() addShiftPointIfApproved(*new_shift_points); } else if (isWaitingApproval()) { clearWaitingApproval(); - removeCandaiteRTCStatus(); + removeCandidateRTCStatus(); } // generate path with shift points that have been inserted.