fix(bvp): don't remove rtc cooperate status

Signed-off-by: satoshi-ota <[email protected]>
autowarefoundation · May 16, 2024 · 8526341 · 8526341
1 parent 89f81c0
commit 8526341
Show file tree

Hide file tree

Showing 2 changed files with 9 additions and 2 deletions.
diff --git a/...locity_planner_common/include/behavior_velocity_planner_common/scene_module_interface.hpp b/...locity_planner_common/include/behavior_velocity_planner_common/scene_module_interface.hpp
@@ -245,7 +245,11 @@ class SceneModuleManagerInterfaceWithRTC : public SceneModuleManagerInterface
 
   void removeRTCStatus(const UUID & uuid) { rtc_interface_.removeCooperateStatus(uuid); }
 
-  void publishRTCStatus(const Time & stamp) { rtc_interface_.publishCooperateStatus(stamp); }
+  void publishRTCStatus(const Time & stamp)
+  {
+    rtc_interface_.removeExpiredCooperateStatus();
+    rtc_interface_.publishCooperateStatus(stamp);
+  }
 
   UUID getUUID(const int64_t & module_id) const;
 

diff --git a/planning/behavior_velocity_planner_common/src/scene_module_interface.cpp b/planning/behavior_velocity_planner_common/src/scene_module_interface.cpp
@@ -272,7 +272,10 @@ void SceneModuleManagerInterfaceWithRTC::deleteExpiredModules(
 
   for (const auto & scene_module : copied_scene_modules) {
     if (isModuleExpired(scene_module)) {
-      removeRTCStatus(getUUID(scene_module->getModuleId()));
+      const UUID uuid = getUUID(scene_module->getModuleId());
+      updateRTCStatus(
+        uuid, scene_module->isSafe(), State::SUCCEEDED, std::numeric_limits<double>::lowest(),
+        clock_->now());
       removeUUID(scene_module->getModuleId());
       unregisterModule(scene_module);
     }