fix(bpp, bvp): keep rtc cooperate status even after manager stops mod…

…ule execution (autowarefoundation#7032) * feat(rtc_interface): add new function to remove old status Signed-off-by: satoshi-ota <[email protected]> * fix(bpp): don't remove rtc cooperate status Signed-off-by: satoshi-ota <[email protected]> * fix(bvp): don't remove rtc cooperate status Signed-off-by: satoshi-ota <[email protected]> --------- Signed-off-by: satoshi-ota <[email protected]>
Interplai · Jun 3, 2024 · 1115452 · 1115452
1 parent 22a0b40
commit 1115452
Show file tree

Hide file tree

Showing 6 changed files with 23 additions and 3 deletions.
diff --git a/..._planner_common/include/behavior_path_planner_common/interface/scene_module_interface.hpp b/..._planner_common/include/behavior_path_planner_common/interface/scene_module_interface.hpp
@@ -187,7 +187,6 @@ class SceneModuleInterface
     RCLCPP_DEBUG(getLogger(), "%s %s", name_.c_str(), __func__);
 
     clearWaitingApproval();
-    removeRTCStatus();
     unlockNewModuleLaunch();
     unlockOutputPath();
     steering_factor_interface_ptr_->clearSteeringFactors();

diff --git a/..._common/include/behavior_path_planner_common/interface/scene_module_manager_interface.hpp b/..._common/include/behavior_path_planner_common/interface/scene_module_manager_interface.hpp
@@ -103,6 +103,7 @@ class SceneModuleManagerInterface
   {
     for (const auto & [module_name, ptr] : rtc_interface_ptr_map_) {
       if (ptr) {
+        ptr->removeExpiredCooperateStatus();
         ptr->publishCooperateStatus(rclcpp::Clock(RCL_ROS_TIME).now());
       }
     }

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
@@ -247,7 +247,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);
     }

diff --git a/planning/rtc_interface/include/rtc_interface/rtc_interface.hpp b/planning/rtc_interface/include/rtc_interface/rtc_interface.hpp
@@ -56,6 +56,7 @@ class RTCInterface
     const UUID & uuid, const bool safe, const uint8_t state, const double start_distance,
     const double finish_distance, const rclcpp::Time & stamp);
   void removeCooperateStatus(const UUID & uuid);
+  void removeExpiredCooperateStatus();
   void clearCooperateStatus();
   bool isActivated(const UUID & uuid) const;
   bool isRegistered(const UUID & uuid) const;

diff --git a/planning/rtc_interface/src/rtc_interface.cpp b/planning/rtc_interface/src/rtc_interface.cpp
@@ -265,6 +265,18 @@ void RTCInterface::removeStoredCommand(const UUID & uuid)
   }
 }
 
+void RTCInterface::removeExpiredCooperateStatus()
+{
+  std::lock_guard<std::mutex> lock(mutex_);
+  const auto itr = std::remove_if(
+    registered_status_.statuses.begin(), registered_status_.statuses.end(),
+    [](const auto & status) {
+      return (rclcpp::Clock{RCL_ROS_TIME}.now() - status.stamp).seconds() > 10.0;
+    });
+
+  registered_status_.statuses.erase(itr, registered_status_.statuses.end());
+}
+
 void RTCInterface::clearCooperateStatus()
 {
   std::lock_guard<std::mutex> lock(mutex_);