From b626d7db62ffd83e6caf630e4c7733ffb6be64e3 Mon Sep 17 00:00:00 2001 From: Alexander Tiderko Date: Thu, 19 Oct 2023 09:54:51 +0200 Subject: [PATCH] forward subscribed messages with reduced rate --- .../fkie_node_manager_daemon/subscriber_node.py | 12 ++++++++++-- .../src/fkie_node_manager_daemon/subscriber_node.py | 12 ++++++++++-- 2 files changed, 20 insertions(+), 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 index 0a2fde13..e2e4af14 100644 --- 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 index 7f61f965..dafd5528 100644 --- 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