forward subscribed messages with reduced rate

fkie · Oct 19, 2023 · b626d7d · b626d7d
1 parent 010210e
commit b626d7d
Show file tree

Hide file tree

Showing 2 changed files with 20 additions and 4 deletions.
diff --git a/fkie_node_manager_daemon/fkie_node_manager_daemon/subscriber_node.py b/fkie_node_manager_daemon/fkie_node_manager_daemon/subscriber_node.py
@@ -134,6 +134,7 @@ def __init__(self, test_env=False):
         self._crossbar_port = parsed_args.crossbar_port
         self._crossbar_realm = parsed_args.crossbar_realm
 
+        self._send_ts = 0
         self._latched_messages = []
         # stats parameter
         self._last_received_ts = 0
@@ -255,8 +256,15 @@ def _msg_handle(self, data):
         event.count = self._count_received
         self._calc_stats(data, event)
         print(f"publish_to: ", f"ros.subscriber.event.{self._topic.replace('/', '_')}")
-        self.publish_to(
-            f"ros.subscriber.event.{self._topic.replace('/', '_')}", event, resend_after_connect=self._latched)
+        timeouted = self._hz == 0
+        if self._hz != 0:
+            now = time.time()
+            if now - self._send_ts > 1.0 / self._hz:
+                self._send_ts = now
+                timeouted = True
+        if event.latched or timeouted:
+            self.publish_to(
+                f"ros.subscriber.event.{self._topic.replace('/', '_')}", event, resend_after_connect=self._latched)
 
     def _get_message_size(self, msg):
         # print("size:", msg.__sizeof__())

diff --git a/fkie_node_manager_daemon/src/fkie_node_manager_daemon/subscriber_node.py b/fkie_node_manager_daemon/src/fkie_node_manager_daemon/subscriber_node.py
@@ -108,6 +108,7 @@ def __init__(self, node_name: str, log_level: int = rospy.INFO, test_env: bool =
         self._crossbar_port = parsed_args.crossbar_port
         self._crossbar_realm = parsed_args.crossbar_realm
 
+        self._send_ts = 0
         self._latched_messages = []
         # stats parameter
         self._last_received_ts = 0
@@ -189,8 +190,15 @@ def _msg_handle(self, data):
                 data, cls=MsgEncoder, **{"no_arr": self._no_arr, "no_str": self._no_str}))
         event.count = self._count_received
         self._calc_stats(data, event)
-        self.publish_to(
-            f"ros.subscriber.event.{self._topic.replace('/', '_')}", event, resend_after_connect=self._latched)
+        timeouted = self._hz == 0
+        if self._hz != 0:
+            now = time.time()
+            if now - self._send_ts > 1.0 / self._hz:
+                self._send_ts = now
+                timeouted = True
+        if event.latched or timeouted:
+            self.publish_to(
+                f"ros.subscriber.event.{self._topic.replace('/', '_')}", event, resend_after_connect=self._latched)
 
     def _get_message_size(self, msg):
         buff = None