feat(rtc_interface): add new function to remove old status

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

Author: satoshi-ota

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;

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_);