From f009ccea059f1c3b7ba141085c746c68603c4bf6 Mon Sep 17 00:00:00 2001 From: Mehul Goel Date: Wed, 23 Oct 2024 21:03:36 -0400 Subject: [PATCH 1/4] initial watchdog node commit --- .../src/buggy/buggy/watchdog/watchdog_node.py | 44 +++++++++++++++++++ rb_ws/src/buggy/launch/watchdog.xml | 4 ++ rb_ws/src/buggy/setup.py | 3 +- 3 files changed, 50 insertions(+), 1 deletion(-) create mode 100644 rb_ws/src/buggy/buggy/watchdog/watchdog_node.py create mode 100644 rb_ws/src/buggy/launch/watchdog.xml diff --git a/rb_ws/src/buggy/buggy/watchdog/watchdog_node.py b/rb_ws/src/buggy/buggy/watchdog/watchdog_node.py new file mode 100644 index 0000000..2e536cb --- /dev/null +++ b/rb_ws/src/buggy/buggy/watchdog/watchdog_node.py @@ -0,0 +1,44 @@ +import rclpy +from rclpy.node import Node + +from std_msgs.msg import Bool + +class Watchdog(Node): + + def __init__(self): + """ + Constructor for Watchdog class. + + Creates a ROS node with a publisher that periodically sends a message + indicating whether the node is still alive. + + """ + super().__init__('watchdog') + self.heartbeat_publisher = self.create_publisher(Bool, 'self/debug/heartbeat', 1) + timer_period = 0.01 # seconds (10 Hz) + self.timer = self.create_timer(timer_period, self.loop) + self.i = 0 # Loop Counter + + def loop(self): + # Loop for the code that operates at 0.1 Hz + msg = Bool() + msg.data = True + self.heartbeat_publisher.publish(msg) + + +def main(args=None): + rclpy.init(args=args) + + minimal_publisher = Watchdog() + + rclpy.spin(minimal_publisher) + + # Destroy the node explicitly + # (optional - otherwise it will be done automatically + # when the garbage collector destroys the node object) + minimal_publisher.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/rb_ws/src/buggy/launch/watchdog.xml b/rb_ws/src/buggy/launch/watchdog.xml new file mode 100644 index 0000000..28c784d --- /dev/null +++ b/rb_ws/src/buggy/launch/watchdog.xml @@ -0,0 +1,4 @@ + + + + \ No newline at end of file diff --git a/rb_ws/src/buggy/setup.py b/rb_ws/src/buggy/setup.py index 4373639..5ecef4e 100644 --- a/rb_ws/src/buggy/setup.py +++ b/rb_ws/src/buggy/setup.py @@ -24,7 +24,8 @@ tests_require=['pytest'], entry_points={ 'console_scripts': [ - 'hello_world = buggy.hello_world:main' + 'hello_world = buggy.hello_world:main', + 'watchdog = buggy.watchdog.watchdog_node:main' ], }, ) From 98e5912c5b8131982142b2a6a2da756406950f89 Mon Sep 17 00:00:00 2001 From: Mehul Goel Date: Wed, 23 Oct 2024 21:14:27 -0400 Subject: [PATCH 2/4] added subscriber and fixed --- rb_ws/src/buggy/buggy/watchdog/watchdog_node.py | 15 ++++++++++++--- rb_ws/src/buggy/setup.py | 4 ++-- 2 files changed, 14 insertions(+), 5 deletions(-) diff --git a/rb_ws/src/buggy/buggy/watchdog/watchdog_node.py b/rb_ws/src/buggy/buggy/watchdog/watchdog_node.py index 2e536cb..6f238da 100644 --- a/rb_ws/src/buggy/buggy/watchdog/watchdog_node.py +++ b/rb_ws/src/buggy/buggy/watchdog/watchdog_node.py @@ -25,18 +25,27 @@ def loop(self): msg.data = True self.heartbeat_publisher.publish(msg) + def heartbeat_listener(self, msg : Node): + """ + Subscriber Function that checks if Heartbeat is ever alse + It never actually is false, this is just a demonstration of a subscriber + If it ever actually is false, something cursed has happened + """ + if msg.data == False: + self.get_logger().error("Hearbeat Failed!") + def main(args=None): rclpy.init(args=args) - minimal_publisher = Watchdog() + watchdog = Watchdog() - rclpy.spin(minimal_publisher) + rclpy.spin(watchdog) # Destroy the node explicitly # (optional - otherwise it will be done automatically # when the garbage collector destroys the node object) - minimal_publisher.destroy_node() + watchdog.destroy_node() rclpy.shutdown() diff --git a/rb_ws/src/buggy/setup.py b/rb_ws/src/buggy/setup.py index 5ecef4e..0a9e40b 100644 --- a/rb_ws/src/buggy/setup.py +++ b/rb_ws/src/buggy/setup.py @@ -13,7 +13,7 @@ ('share/ament_index/resource_index/packages', ['resource/' + package_name]), ('share/' + package_name, ['package.xml']), - (os.path.join("share", package_name), glob("launch/*.xml")), + (os.path.join("share", package_name), glob("launch/*.xml")) ], install_requires=['setuptools'], zip_safe=True, @@ -28,4 +28,4 @@ 'watchdog = buggy.watchdog.watchdog_node:main' ], }, -) +) \ No newline at end of file From ead0cbe7e39d210c72714c90d56ecab7e6f2cb92 Mon Sep 17 00:00:00 2001 From: Mehul Goel Date: Wed, 23 Oct 2024 21:24:59 -0400 Subject: [PATCH 3/4] Finished subscriber --- rb_ws/src/buggy/buggy/watchdog/watchdog_node.py | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/rb_ws/src/buggy/buggy/watchdog/watchdog_node.py b/rb_ws/src/buggy/buggy/watchdog/watchdog_node.py index 6f238da..3b1609e 100644 --- a/rb_ws/src/buggy/buggy/watchdog/watchdog_node.py +++ b/rb_ws/src/buggy/buggy/watchdog/watchdog_node.py @@ -14,13 +14,19 @@ def __init__(self): """ super().__init__('watchdog') + + # Publishers self.heartbeat_publisher = self.create_publisher(Bool, 'self/debug/heartbeat', 1) - timer_period = 0.01 # seconds (10 Hz) + + # Subscribers + self.heartbeat_subscriber = self.create_subscription(Bool, 'self/debug/heartbeat', self.heartbeat_listener, 1) + + timer_period = 0.01 # seconds (100 Hz) self.timer = self.create_timer(timer_period, self.loop) self.i = 0 # Loop Counter def loop(self): - # Loop for the code that operates at 0.1 Hz + # Loop for the code that operates every 10ms msg = Bool() msg.data = True self.heartbeat_publisher.publish(msg) From 3bb264f014e0d0292fb9e8be47b1d711e63c7470 Mon Sep 17 00:00:00 2001 From: Mehul Goel Date: Wed, 23 Oct 2024 21:25:51 -0400 Subject: [PATCH 4/4] pylint --- rb_ws/src/buggy/buggy/watchdog/watchdog_node.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/rb_ws/src/buggy/buggy/watchdog/watchdog_node.py b/rb_ws/src/buggy/buggy/watchdog/watchdog_node.py index 3b1609e..b0338da 100644 --- a/rb_ws/src/buggy/buggy/watchdog/watchdog_node.py +++ b/rb_ws/src/buggy/buggy/watchdog/watchdog_node.py @@ -14,7 +14,7 @@ def __init__(self): """ super().__init__('watchdog') - + # Publishers self.heartbeat_publisher = self.create_publisher(Bool, 'self/debug/heartbeat', 1) @@ -29,7 +29,7 @@ def loop(self): # Loop for the code that operates every 10ms msg = Bool() msg.data = True - self.heartbeat_publisher.publish(msg) + self.heartbeat_publisher.publish(msg) def heartbeat_listener(self, msg : Node): """