add outdated policy

ais2msg/aismsg_publisher.py

@@ -7,6 +7,8 @@ from aismsg.msg import AISObjectStamped, AISObjectArray
 from ais2msg.object_helper import get_AISobj_from_decode_msg, decode_message
 
 QUEUE_SIZE = 1000
+OUTDATED_THRESHOLD_SEC = 20
+OUTDATED_THRESHOLD_NANOSEC = OUTDATED_THRESHOLD_SEC * 1000000000
 AIS_OBJECT_PUBLISHER_NAME = "AIS_Publisher"
 AIS_OBJECT_PUBLISHER_PUB_TOPIC = "/AISObjectPub"
 AIS_OBJECT_ARRAY_PUBLISHER_NAME = "AIS_Array_Publisher"
@@ -23,8 +25,12 @@ class AISObjectPublisher(Node):
 
         self._ros_setup()
 
+        self.outdated_threshold_nanosec = OUTDATED_THRESHOLD_NANOSEC
+
         self.counter = 0
         self.last_mmsi = None
+        self.last_timestamp = None
+
         self.mmsi_obj_dict = {}
 
     def publish_data(self, data):
@@ -43,7 +49,6 @@ class AISObjectPublisher(Node):
                 aisobj = get_AISobj_from_decode_msg(
                     decode_msg, self.mmsi_obj_dict[self.last_mmsi])
                 obj = aisobj.object
-                obj.timestamp_last_update = self.get_clock().now().nanoseconds
                 self.get_logger().info(f"Update Exist mmsi: {self.last_mmsi}")
             else:
                 # if mmsi not exist, create a new one
@@ -51,13 +56,15 @@ class AISObjectPublisher(Node):
                 aisobj = get_AISobj_from_decode_msg(decode_msg)
                 obj = aisobj.object
                 obj.timestamp_create = self.get_clock().now().nanoseconds
-                obj.timestamp_last_update = self.get_clock().now().nanoseconds
                 self.mmsi_obj_dict[self.last_mmsi] = aisobj
                 self.get_logger().info(f"Got New mmsi: {self.last_mmsi}")
 
+            aisobj.object.timestamp_last_update = self.get_clock().now(
+            ).nanoseconds
             aisobj.header.stamp = self.get_clock().now().to_msg()
             aisobj.header.frame_id = str(self.counter)
             self.counter += 1
+            self.last_timestamp = aisobj.object.timestamp_last_update
 
         except RuntimeError:
             self.get_logger().warning(
@@ -65,15 +72,27 @@ class AISObjectPublisher(Node):
             return
 
         self._publish()
-
-    def _publish(self):
-        self.publisher.publish(self.mmsi_obj_dict[self.last_mmsi])
+        self._remove_outdated()
 
     def _ros_setup(self):
         self.publisher = self.create_publisher(AISObjectStamped,
                                                AIS_OBJECT_PUBLISHER_PUB_TOPIC,
                                                QUEUE_SIZE)
 
+    def _publish(self):
+        self.publisher.publish(self.mmsi_obj_dict[self.last_mmsi])
+
+    def _remove_outdated(self):
+
+        out_mmsi = [
+            k for k, v in self.mmsi_obj_dict.items() if self.last_timestamp -
+            v.object.timestamp_last_update > self.outdated_threshold_nanosec
+        ]
+
+        for mmsi in out_mmsi:
+            self.mmsi_obj_dict.pop(mmsi)
+            self.get_logger().info(f"Remove outdated mmsi: {mmsi}")
+
 
 class AISObjectArrayPublisher(AISObjectPublisher):