机器人 / 系统课时

用看门狗与急停停止危险运动

用看门狗与急停停止危险运动

本课为 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 主机宕机、总线断线、需要独立切断驱动力

ROS 2 速度安全闸门、急停硬件路径与锁定状态

实现带锁定的速度闸门

保存以下完整程序为 safety_gate.py。它读取 /cmd_vel_raw 和每秒至少 10 次更新的 /estop_state(std_msgs/msg/Bool,true 表示按下),向 /diff_drive_controller/cmd_vel 发布带新时间戳的 TwistStamped。启动时保持锁定;只有急停已释放且状态新鲜、上游正在发送新鲜零速、操作者显式调用复位服务,才重新允许指令通过。

python
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 控制器参数文件中设置以下参数。片段示例:依赖工程中已有的轮关节、轮距、轮半径等完整配置,不能把这三行当成完整控制器文件。

yaml
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 启动程序:

bash
source /opt/ros/jazzy/setup.bash
python3 safety_gate.py

预期结果:进程持续运行,尚未人工复位时输出零速。仿真使用 /clock 时改用 python3 safety_gate.py --ros-args -p use_sim_time:=true,并让控制器使用同一仿真时间,避免时间戳不在同一时基。

在已加载 Jazzy 环境的终端 B,持续发布仿真用的急停释放状态:

bash
ros2 topic pub -r 10 /estop_state std_msgs/msg/Bool "{data: false}"

预期结果:每秒约 10 次发送 false;该话题在实机应由真实安全设备只读提供,不能用 ROS 发布 false 代替物理解锁。

在终端 C 持续发布零速:

bash
ros2 topic pub -r 10 /cmd_vel_raw geometry_msgs/msg/TwistStamped "{twist: {linear: {x: 0.0}, angular: {z: 0.0}}}"

预期结果:闸门收到新鲜零速,但仍保持启动锁定。随后在终端 D 调用人工复位服务:

bash
ros2 service call /reset_motion_latch std_srvs/srv/Trigger "{}"

预期结果:响应中 success 为 True,消息为 Reset accepted; waiting for fresh command。若先停止零速发布,或停止急停状态心跳,响应中的 success 应为 False;即使收到 false 也不会自动解锁。

注入故障并核对停止结果

先在仿真或轮子完全离地、驱动受控的条件下测试。保持终端 B 的急停状态心跳,复位成功后,从新的终端启动非零速度发布,再关闭终端 C 的零速发布,确保只有一个上游速度源:

bash
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,闸门会先锁定;此时按上一节条件重新复位,而不是绕开保护。

另开终端观察最终速度话题:

bash
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 秒超时应让轮速命令归零

单次急停状态注入命令:

bash
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 检查没有绕过闸门的其他发布者。若控制器已设置减速度限幅,零速目标到实际停转仍可能需要额外时间。

按自己的节奏,确认真正理解。

目录与阅读设置

阅读设置

17

记录一个想法

用看门狗与急停停止危险运动

0 / 10000 · 笔记保存到账号。未提交的修改会在关闭、离开或刷新后丢失。

登录哪都通