ROS2 ROS2 Node: Safe Motor Shutdown On Error

Problem Statement

Implement a safety watchdog node that monitors a /heartbeat topic and sends zero-velocity commands if the heartbeat stops.

Requirements

Your node must:

• Subscribe to /heartbeat (std_msgs/String) — the topic being monitored
• Subscribe to /cmd_vel_input (geometry_msgs/Twist) — incoming velocity commands to pass through
• Create a publisher for /cmd_vel (geometry_msgs/Twist) — the safe output
• Create a 200ms timer (create_timer(0.2, ...)) that runs check_heartbeat()
• In check_heartbeat():
  • Compute time since last heartbeat: time.time() - self.last_heartbeat_time
  • If elapsed > 0.5 seconds: set self.healthy = False, log a warning, publish zero Twist
  • If elapsed ≤ 0.5 seconds and previously unhealthy: log recovery, set self.healthy = True
• In the /cmd_vel_input callback: only forward the command if self.healthy == True, otherwise publish zero Twist
• In the /heartbeat callback: update self.last_heartbeat_time = time.time()
• Log "Safety Watchdog Ready" on startup

Input / Output

Input: std_msgs/String on /heartbeat | geometry_msgs/Twist on /cmd_vel_input Output: geometry_msgs/Twist on /cmd_vel (pass-through when healthy, zero when not)

🤖 Why This Matters in Real Robots

This is the architecture used in every safety-critical robotics deployment. The Nav2 controller server monitors a control frequency watchdog. The MoveIt2 servo package has a joint-angle watchdog. If any upstream node stops responding, the robot stops — it never keeps moving blindly based on stale commands.

⚠️ Common Pitfalls

• Using ROS time vs wall time: self.get_clock().now() can freeze if the executor isn't spinning. Use Python's time.time() for the watchdog timeout — it always runs.
• Not initialising last_heartbeat_time: If last_heartbeat_time starts as None, the first check_heartbeat() call crashes. Initialise it to time.time() in __init__.