机器人 / 系统课时
用看门狗与急停停止危险运动
用看门狗与急停停止危险运动
本课为 ROS 2 Jazzy 差速底盘添加速度安全闸门:指令超过 250 ms 未更新、急停按下或急停状态心跳消失时,持续输出零速度并锁定。你将配置控制器自身的超时保护,并验证故障消失后不会未经人工复位就重新运动。
学习目标
- 区分 ROS 速度看门狗、控制器超时和独立硬件急停的职责。
- 用
rclpy实现超时锁定、急停锁定以及有条件的人工复位。 - 通过断流、急停和错误复位测试,验证输出与轮速确实归零。
先确定停止链路
适用环境是 Ubuntu 24.04、ROS 2 Jazzy 和使用非链式模式的 diff_drive_controller。Jazzy 控制器接收 geometry_msgs/msg/TwistStamped,其 cmd_vel_timeout 默认 0.5 秒;实际话题名取决于控制器命名空间。下面假设控制器叫 diff_drive_controller。
| 保护层 | 本例设置 | 负责的故障 |
|---|---|---|
| 速度安全闸门 | 20 Hz 输出;250 ms 指令超时后锁定 | 上游导航或遥控断流、急停状态异常 |
diff_drive_controller |
cmd_vel_timeout: 0.5 |
安全闸门进程停止发布 |
| 电机驱动与物理急停 | 驱动器本地指令看门狗建议先按 200 ms 评估;实体急停直连安全回路或 STO | ROS 主机宕机、总线断线、需要独立切断驱动力 |

实现带锁定的速度闸门
保存以下完整程序为 safety_gate.py。它读取 /cmd_vel_raw 和每秒至少 10 次更新的 /estop_state(std_msgs/msg/Bool,true 表示按下),向 /diff_drive_controller/cmd_vel 发布带新时间戳的 TwistStamped。启动时保持锁定;只有急停已释放且状态新鲜、上游正在发送新鲜零速、操作者显式调用复位服务,才重新允许指令通过。
import math
import time
import rclpy
from geometry_msgs.msg import TwistStamped
from rclpy.node import Node
from std_msgs.msg import Bool
from std_srvs.srv import Trigger
CMD_TIMEOUT = 0.25
ESTOP_TIMEOUT = 0.30
class SafetyGate(Node):
def __init__(self):
super().__init__('safety_gate')
self.latched = True # 启动不授权运动
self.estop_pressed = True # 状态未知按故障处理
self.estop_seen = None
self.cmd_seen = None
self.armed_at = time.monotonic()
self.command = None
self.create_subscription(TwistStamped, '/cmd_vel_raw', self.on_cmd, 10)
self.create_subscription(Bool, '/estop_state', self.on_estop, 10)
self.pub = self.create_publisher(TwistStamped, '/diff_drive_controller/cmd_vel', 10)
self.create_service(Trigger, '/reset_motion_latch', self.on_reset)
self.create_timer(0.05, self.on_tick)
def latch(self, reason):
if not self.latched:
self.get_logger().warn(f'Locked: {reason}')
self.latched = True
def on_cmd(self, msg):
x, z = msg.twist.linear.x, msg.twist.angular.z
if not (math.isfinite(x) and math.isfinite(z)):
self.command = None
self.cmd_seen = None
self.latch('invalid command')
return
self.command = msg
self.cmd_seen = time.monotonic()
def on_estop(self, msg):
self.estop_pressed = msg.data
self.estop_seen = time.monotonic()
if msg.data:
self.latch('E-stop pressed')
def on_reset(self, _request, response):
now = time.monotonic()
estop_ok = (
self.estop_seen is not None
and now - self.estop_seen < ESTOP_TIMEOUT
and not self.estop_pressed
)
zero_ok = (
self.cmd_seen is not None
and now - self.cmd_seen < CMD_TIMEOUT
and self.command is not None
and abs(self.command.twist.linear.x) < 0.001
and abs(self.command.twist.angular.z) < 0.001
)
response.success = bool(estop_ok and zero_ok)
if response.success:
self.latched = False
self.armed_at = now
# 不把复位前缓存的命令带入新一轮运行
self.command = None
self.cmd_seen = None
response.message = 'Reset accepted; waiting for fresh command'
else:
response.message = 'Need fresh released E-stop and fresh zero command'
return response
def on_tick(self):
now = time.monotonic()
if (
self.estop_seen is None
or now - self.estop_seen >= ESTOP_TIMEOUT
or self.estop_pressed
):
self.latch('E-stop missing or pressed')
since = self.cmd_seen if self.cmd_seen is not None else self.armed_at
if not self.latched and now - since >= CMD_TIMEOUT:
self.latch('command timeout')
out = TwistStamped()
out.header.stamp = self.get_clock().now().to_msg()
out.header.frame_id = 'base_link'
if not self.latched and self.command is not None:
out.twist.linear.x = self.command.twist.linear.x
out.twist.angular.z = self.command.twist.angular.z
self.pub.publish(out)
def main():
rclpy.init()
node = SafetyGate()
try:
rclpy.spin(node)
finally:
node.destroy_node()
if rclpy.ok():
rclpy.shutdown()
if __name__ == '__main__':
main()预期结果:节点以约 20 Hz 发布;未复位、任何输入过期或急停按下时,/diff_drive_controller/cmd_vel 的 twist.linear.x、twist.angular.z 始终为 0.0。这里使用 time.monotonic() 监测接收间隔,避免系统时钟调整影响超时判断;真实系统还应校验上游消息时间戳,防止排队或重放的旧指令被重新盖上新时间戳。
接入控制器并执行复位
在现有 ros2_control 控制器参数文件中设置以下参数。片段示例:依赖工程中已有的轮关节、轮距、轮半径等完整配置,不能把这三行当成完整控制器文件。
diff_drive_controller:
ros__parameters:
cmd_vel_timeout: 0.5预期生效行为:控制器超过 0.5 秒没有收到新的 TwistStamped 时停止执行过期速度指令;详情见 Jazzy 官方控制器文档。本示例把闸门输出直接接到 /diff_drive_controller/cmd_vel;用 ros2 topic info /diff_drive_controller/cmd_vel -v 核对该话题只有安全闸门一个发布者,并确保导航、手柄等只能发布到 /cmd_vel_raw,不能绕过闸门。如果控制器位于其他命名空间,须同步修改代码的输出话题。
在终端 A 启动程序:
source /opt/ros/jazzy/setup.bash
python3 safety_gate.py预期结果:进程持续运行,尚未人工复位时输出零速。仿真使用 /clock 时改用 python3 safety_gate.py --ros-args -p use_sim_time:=true,并让控制器使用同一仿真时间,避免时间戳不在同一时基。
在已加载 Jazzy 环境的终端 B,持续发布仿真用的急停释放状态:
ros2 topic pub -r 10 /estop_state std_msgs/msg/Bool "{data: false}"预期结果:每秒约 10 次发送 false;该话题在实机应由真实安全设备只读提供,不能用 ROS 发布 false 代替物理解锁。
在终端 C 持续发布零速:
ros2 topic pub -r 10 /cmd_vel_raw geometry_msgs/msg/TwistStamped "{twist: {linear: {x: 0.0}, angular: {z: 0.0}}}"预期结果:闸门收到新鲜零速,但仍保持启动锁定。随后在终端 D 调用人工复位服务:
ros2 service call /reset_motion_latch std_srvs/srv/Trigger "{}"预期结果:响应中 success 为 True,消息为 Reset accepted; waiting for fresh command。若先停止零速发布,或停止急停状态心跳,响应中的 success 应为 False;即使收到 false 也不会自动解锁。
注入故障并核对停止结果
先在仿真或轮子完全离地、驱动受控的条件下测试。保持终端 B 的急停状态心跳,复位成功后,从新的终端启动非零速度发布,再关闭终端 C 的零速发布,确保只有一个上游速度源:
ros2 topic pub -r 10 /cmd_vel_raw geometry_msgs/msg/TwistStamped "{twist: {linear: {x: 0.2}, angular: {z: 0.0}}}"预期结果:上游指令持续且已解除锁定时,/diff_drive_controller/cmd_vel 线速度为 0.2 m/s。若切换发布者耗时超过 250 ms,闸门会先锁定;此时按上一节条件重新复位,而不是绕开保护。
另开终端观察最终速度话题:
ros2 topic echo /diff_drive_controller/cmd_vel --field twist.linear.x预期结果:正常允许运动时连续显示 0.2,停止非零发布者约 250~300 ms 后连续显示 0.0。计时受 50 ms 定时器和 ROS 调度影响,不能把该数字当成实机停止距离或安全响应时间。
| 故障注入 | 应观察到的结果 |
|---|---|
停止 /cmd_vel_raw 发布,再恢复 0.2 m/s |
看门狗锁定并持续输出 0.0;恢复消息不会自动运行 |
保持运动命令,额外发布一次 /estop_state: true |
立即锁定并输出 0.0;随后恢复 false 也不解锁 |
| 急停未释放、状态心跳停止、输入非零或输入已过期时调用复位 | 服务返回 success: False |
停止 safety_gate.py 进程 |
/diff_drive_controller/cmd_vel 断流,控制器自身的 0.5 秒超时应让轮速命令归零 |
单次急停状态注入命令:
ros2 topic pub --once /estop_state std_msgs/msg/Bool "{data: true}"预期结果:即使随后持续收到 false,闸门仍输出零速,必须重新满足复位条件才能恢复。验收时同时查看 /diff_drive_controller/cmd_vel、轮关节反馈以及真实驱动使能状态;只看 /diff_drive_controller/cmd_vel 为零,不能证明电机已经停转。用 ros2 topic info /diff_drive_controller/cmd_vel -v 检查没有绕过闸门的其他发布者。若控制器已设置减速度限幅,零速目标到实际停转仍可能需要额外时间。