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..b0338da --- /dev/null +++ b/rb_ws/src/buggy/buggy/watchdog/watchdog_node.py @@ -0,0 +1,59 @@ +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') + + # Publishers + self.heartbeat_publisher = self.create_publisher(Bool, 'self/debug/heartbeat', 1) + + # 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 every 10ms + msg = Bool() + 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) + + watchdog = Watchdog() + + rclpy.spin(watchdog) + + # Destroy the node explicitly + # (optional - otherwise it will be done automatically + # when the garbage collector destroys the node object) + watchdog.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..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, @@ -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' ], }, -) +) \ No newline at end of file