fix: remove rtc staus when no avoidance candidate is output (#1636)

* fix: remove rtc staus when no avoidance candidate is output Signed-off-by: tomoya.kimura <tomoya.kimura@tier4.jp> * remove only candidate rtc status Signed-off-by: tomoya.kimura <tomoya.kimura@tier4.jp> * Update planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp Co-authored-by: Fumiya Watanabe <rej55.g@gmail.com> * Update planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp Co-authored-by: Fumiya Watanabe <rej55.g@gmail.com> * Update planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp Co-authored-by: Fumiya Watanabe <rej55.g@gmail.com> Signed-off-by: tomoya.kimura <tomoya.kimura@tier4.jp> Co-authored-by: Fumiya Watanabe <rej55.g@gmail.com>
autowarefoundation · Aug 22, 2022 · 1115898 · 1115898
1 parent ae56406
commit 1115898
Show file tree

Hide file tree

Showing 2 changed files with 16 additions and 0 deletions.
diff --git a/...or_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp b/...or_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
@@ -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<PathWithLaneId>(candidate.path_candidate);
   return out;