From 216bc9639608676f4664a3a2d50043ab4c6f9e17 Mon Sep 17 00:00:00 2001 From: Christopher Herr Date: Sat, 25 Mar 2023 16:23:01 +0100 Subject: [PATCH] Implemented the Message Overfolw Watchdog --- src/pubsub/pubsub/pubsub_library_v4.py | 18 ++++++++++++++++-- 1 file changed, 16 insertions(+), 2 deletions(-) diff --git a/src/pubsub/pubsub/pubsub_library_v4.py b/src/pubsub/pubsub/pubsub_library_v4.py index 55c3380..d1c3e9e 100644 --- a/src/pubsub/pubsub/pubsub_library_v4.py +++ b/src/pubsub/pubsub/pubsub_library_v4.py @@ -21,6 +21,7 @@ import rclpy from rclpy.node import Node from rclpy.action import ActionClient from rclpy.action import ActionServer, CancelResponse +import time # How to use, please refer to: # Topic: @@ -95,6 +96,8 @@ class MinimalSubscriber(Node): self.CUSTOM_MSG = MSG_TYPE self.NUM_MSGS = NUM_MSGS self.topic_received = False + self.overflow = False + self.watchdog = 0 # Init above laying class "Node" super().__init__(self.NODE_NAME) self.subscription = self.create_subscription( @@ -108,14 +111,25 @@ class MinimalSubscriber(Node): def listener_callback(self, msg): #self.get_logger().info('I heard: "%s"' % msg.data) + self.last_msg_time = time.time() self.topic_received = True self.msg = msg return def return_msg(self): return self.msg - - + + def timer_overflow(self, time_limit=0): + """Watchdog for Topic Publisher Messages\n + ture = timer_overflow >> error + false = no timer_overflow >> no error""" + if time_limit > 0: + if time.time() > self.last_msg_time + time_limit: + self.watchdog += 1 + self.get_logger().warn(f"Message-Watchdog triggered {self.watchdog} times") + if self.watchdog > 10: + self.overflow = True + return self.overflow #******************************************************************************#