机器人 / 系统课时

接入开关服务完成远程控制

接入开关服务完成远程控制

把模拟设备的启停命令接入 ROS 2 服务,并用独立的话题报告模拟设备的当前开关状态。完成后,你能在终端发出开启或关闭请求,先检查服务响应,再等待新的状态消息,判断两条通信链路是否一致。

学习目标

  • 使用 std_srvs/srv/SetBool 实现启停服务,并用 std_msgs/msg/Bool 发布设备状态。
  • 编写带服务发现、响应超时和状态验证的 Python 控制客户端。
  • 验证开启、关闭两次操作,区分“命令已处理”和“状态已观测”。

约定服务与状态接口

以下示例适用于 Ubuntu 24.04、ROS 2 Jazzy 和系统 Python 3。设备启动时为关闭态;服务负责接收指令,话题负责持续报告状态,两者不可互相替代。

接口 类型 契约
/device/set_enabled std_srvs/srv/SetBool 请求 data=true/false;响应 success 与 message
/device/enabled std_msgs/msg/Bool data=true 表示开启,false 表示关闭
状态发布周期 0.5 秒 服务回调立即发布一次,定时器随后持续发布
客户端超时 每阶段 3 秒 分别限制服务发现、响应和话题状态等待

先在一个终端加载 ROS 环境并检查依赖:

bash
source /opt/ros/jazzy/setup.bash
python3 -c "import rclpy; from std_srvs.srv import SetBool; from std_msgs.msg import Bool; print('ROS 2 interfaces ready')"

预期输出:

text
ROS 2 interfaces ready

开关服务响应与状态话题的异步验证时序

实现设备服务端

新建 device_server.py。SetBool 回调先修改内存里的 enabled,立即报告新值,然后返回执行结果;0.5 秒定时器使晚加入的订阅客户端也能读到后续状态。

python
import rclpy
from rclpy.executors import ExternalShutdownException
from rclpy.node import Node
from std_msgs.msg import Bool
from std_srvs.srv import SetBool

class Device(Node):
    def __init__(self):
        super().__init__('device_server')
        self.enabled = False
        self.pub = self.create_publisher(Bool, '/device/enabled', 10)
        self.srv = self.create_service(
            SetBool, '/device/set_enabled', self.handle
        )
        self.timer = self.create_timer(0.5, self.report)
        self.report()
        print('device service ready', flush=True)

    def handle(self, request, response):
        # 先更新设备模拟态,随后报告;success 表示回调已完成
        self.enabled = request.data
        self.report()
        response.success = True
        response.message = 'enabled' if self.enabled else 'disabled'
        return response

    def report(self):
        # 周期发布补偿订阅建立前可能错过的瞬时消息
        self.pub.publish(Bool(data=self.enabled))

def main():
    rclpy.init()
    node = Device()
    try:
        rclpy.spin(node)
    except (KeyboardInterrupt, ExternalShutdownException):
        pass
    finally:
        node.destroy_node()
        if rclpy.ok():
            rclpy.shutdown()

if __name__ == '__main__':
    main()

预期结果:执行该文件后打印 device service ready,进程保持运行;收到启停请求时,/device/enabled 的布尔值随之改变。这里的 success=true 只代表模拟回调处理成功,不代表外部硬件已完成动作。

编写带状态核对的客户端

新建 device_client.py。客户端先创建状态订阅,再用 call_async() 发出请求。收到服务成功响应后记录当前消息计数,只接受其后新到达且与目标值相符的状态回调,避免直接拿订阅缓存中的旧值判断成功。

python
import sys
import time

import rclpy
from rclpy.node import Node
from std_msgs.msg import Bool
from std_srvs.srv import SetBool

class Controller(Node):
    def __init__(self):
        super().__init__('device_controller')
        self.count = 0
        self.value = None
        self.sub = self.create_subscription(
            Bool, '/device/enabled', self.on_state, 10
        )
        self.cli = self.create_client(SetBool, '/device/set_enabled')

    def on_state(self, msg):
        self.count += 1
        self.value = msg.data

    def run(self, target):
        if not self.cli.wait_for_service(timeout_sec=3.0):
            raise TimeoutError('service unavailable')

        request = SetBool.Request()
        request.data = target
        future = self.cli.call_async(request)
        deadline = time.monotonic() + 3.0

        # 主线程驱动执行器,等待异步服务响应但不无限阻塞
        while not future.done() and time.monotonic() < deadline:
            rclpy.spin_once(self, timeout_sec=0.1)
        if not future.done():
            raise TimeoutError('service response timeout')

        response = future.result()
        if response is None:
            raise RuntimeError('service returned no response')
        print(f'service: success={response.success} message={response.message}')
        if not response.success:
            raise RuntimeError('service rejected command')

        # 从收到服务响应之后再观察一次状态话题
        seen_at_response = self.count
        deadline = time.monotonic() + 3.0
        while time.monotonic() < deadline:
            rclpy.spin_once(self, timeout_sec=0.1)
            if self.count > seen_at_response and self.value == target:
                print(f'topic: /device/enabled data={str(self.value).lower()}')
                print('PASS: response matches observed topic')
                return
        raise TimeoutError('no matching state update after response')

def main():
    if len(sys.argv) != 2 or sys.argv[1] not in ('on', 'off'):
        print('usage: python3 device_client.py on|off')
        raise SystemExit(2)

    rclpy.init()
    node = Controller()
    try:
        node.run(sys.argv[1] == 'on')
    except (TimeoutError, RuntimeError) as exc:
        print(f'FAIL: {exc}')
        raise SystemExit(1)
    finally:
        node.destroy_node()
        if rclpy.ok():
            rclpy.shutdown()

if __name__ == '__main__':
    main()

预期结果:服务可用且话题持续报告正确状态时,程序输出 PASS 并以退出码 0 结束;发现超时、拒绝请求或没有匹配的新状态时输出 FAIL 并以非零状态结束。等待循环在 main() 中运行,不要把这种等待直接放进同一单线程执行器的 ROS 回调。

运行并核对启停结果

两个终端必须加载相同 ROS 环境并使用同一 ROS_DOMAIN_ID。在保存上述两个文件的目录操作:

  1. 终端 A 运行服务端,并保持进程运行。
  2. 终端 B 先发 on,再发 off,观察每次的服务响应和随后的新话题消息。
  3. 若要独立检查接口类型,在终端 B 查询服务和话题类型。

终端 A:

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

预期输出:

text
device service ready

终端 B:

bash
source /opt/ros/jazzy/setup.bash
python3 device_client.py on
python3 device_client.py off

预期输出:

text
service: success=True message=enabled
topic: /device/enabled data=true
PASS: response matches observed topic
service: success=True message=disabled
topic: /device/enabled data=false
PASS: response matches observed topic

再核对接口声明:

bash
ros2 service type /device/set_enabled
ros2 topic type /device/enabled

预期输出:

text
std_srvs/srv/SetBool
std_msgs/msg/Bool

这些输出仅保留本例命令的确定内容;终端的实际启动时间及其他节点日志可能不同。按 Ctrl+C 退出终端 A。

排除断连与误判

  • 如果显示 service unavailable,先确认终端 A 未退出,再用 ros2 service list 查找 /device/set_enabled;两端 ROS_DOMAIN_ID 不同会阻止发现。
  • 如果响应成功但状态验证超时,用 ros2 topic info /device/enabled --verbose 查看发布者、订阅者和 QoS。示例两端都用深度 10 的默认可靠、易失性 QoS,且发布周期为 0.5 秒。
  • 如果服务 success=True 但状态仍错误,不能把响应当成实际状态。模拟示例直接修改内存;接入真实电机或继电器后,应在硬件反馈确认后发布 /device/enabled。

std_msgs/msg/Bool 没有时间戳或命令 ID,本例核对的是“服务响应后观察到新的匹配值”,还不能严格证明该值来自本次请求。多控制端同时操作时,应改用带 command_id、状态序号或时间戳的自定义状态消息,并按请求关联结果。

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

目录与阅读设置

阅读设置

17

记录一个想法

接入开关服务完成远程控制

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

登录哪都通