机器人开发  /  ROS 2 HUMBLE  /  一线调试手册

启元Q1机器人SDK全链路实战——TouchState触摸消息、ROS 2图形调试与RViz模型联动

  ROS2  ·  启元Q1  ·  机器人SDK  ·  TouchState  ·  rclpy  ·  消息订阅  ·  rqt_graph  ·  QoS  ·  URDF  ·  RViz2

触摸功能看似只需“点一下，收一条消息”，真正接入时却可能卡在接口构建、终端环境、节点发现、QoS 与图形界面配置上。本文基于启元 Q1 SDK v1.0.0.0、Ubuntu 22.04 与 ROS 2 Humble，从 TouchState 字段解读出发，搭建隔离的本机触摸事件验证链路，提供完整 Python 订阅、有限条数发布和序号验收代码，使用 rqt_graph、Topic Monitor 与命令行交叉定位问题。随后检查 Q1 URDF 的连杆、关节与 STL 资源，适配 ROS 2 启动方式，并通过 RViz 验证肩肘姿态联动。文中结合流程图、兼容矩阵、排障案例及回归清单，明确模拟消息与真机反馈的边界，形成可重复检查的调试方法。

阅读定位  面向已接触 Linux 基础命令、希望独立验证 Q1 触摸接口和模型资源的开发者；代码中的终端输出均为“应出现的观察点”，不冒充现场采集记录。

 

01  /  结果预览

先看最终交付：两条链路、六个可验证的结果

不依赖整套运动控制，也能先把消息接口、可视化监测和模型显示分层跑通。

图 1｜触摸消息与 URDF 姿态显示两条链路独立验证；后续再按需整合。

完成以后，应该能回答这些问题

检查点	成功证据	无法通过时先看
SDK 是否可用	Python 导入 TouchState；接口字段可查看	环境加载、编译目录
回调是否接收	应用打印有效传感器与事件，测试序号连续	话题、域、QoS
图形窗口是否同步	节点连接可见，字段持续更新	rqt 订阅端点
机器人模型是否联动	RViz 中关节姿态随滑条改变	URDF、TF、固定坐标系

为什么是两条链路，而不是一步到位？

接口接入与模型显示之间没有天然的函数依赖。TouchState 回调可以先只记录事件，URDF 也能在没有传感器输入时独立渲染。先把这两条链路做成小型、可重复的检查单元，出现异常时就能明确定位到消息生成、发现通信、回调过滤、几何资源或关节变换的哪一层。

必须分清  本机模拟触摸事件能够验证软件链路，不等于已验证实体设备的传感器响应；RViz 的关节滑条也不代表向真机电机发出控制指令。

 

02  /  设计边界

一张图理解系统：触摸消息和姿态显示为何要分开

消息侧使用自定义接口，模型侧使用标准 JointState、TF 和 URDF；两者目标不同。

图 2｜同一 TouchState 话题可由多个订阅端同时观察；示意图非实时软件截图。

三种数据源，操作边界不同

模式	输入来自哪里	用途与注意事项
离线接口验证	本机 Python 合成 TouchState	单机隔离域中检查类型与订阅逻辑；不宣称覆盖硬件故障
真机事件观测	机器人官方触摸驱动发布	须匹配设备网络、ROS_DOMAIN_ID、RMW 与实际 QoS
模型姿态展示	GUI 或脚本提供 /joint_states	只做软件可视化；运行时保持唯一关节状态发布源

测试约束  本机示例使用 ROS_DOMAIN_ID=71 与 ROS_LOCALHOST_ONLY=1，确保测试流不意外传播到其他设备；真机对接时移除本机限制，并依据设备文档配置发现机制。

内容版本  本文以下载页可见的 SDK v1.0.0.0、Q1 URDF v1.0.0 和 ROS 2 Humble 为参考。包内目录或消息常量以手头实际下载版本的“ros2 interface show”为准。

把“可复现”拆成四种证据

环境证据说明当前终端实际加载哪个工作空间；接口证据说明触摸字段与常量怎样定义；通信证据说明 QoS 与 Domain 是否兼容；结果证据则用接收端序号和模型姿态证明链路正在工作。这四种证据相互补充，不用其中一个代替另一个。

 

03  /  环境核验

从空白 Ubuntu 到可用开发终端，先排除版本错位

图形工具与消息节点都运行在 Ubuntu 主机；若使用远程桌面，浏览器只是窗口的入口。

组件	参考组合	验证方法
系统 / 架构	Ubuntu 22.04 · x86_64	lsb_release -ds ; uname -m
语言与运行时	Python 3.10 · ROS 2 Humble	python3 --version ; printenv ROS_DISTRO
消息与构建工具	colcon、rosidl、rclpy	ros2 pkg list ; colcon --help
图形与模型	rqt、RViz2、robot_state_publisher	command -v rviz2 ; ros2 pkg executables

先按官方教程配置 ROS 软件源，再执行

sudo apt-get update

sudo apt-get install -y python3-colcon-common-extensions   ros-humble-desktop ros-humble-rqt-graph ros-humble-rqt-topic   ros-humble-robot-state-publisher ros-humble-joint-state-publisher-gui

source /opt/ros/humble/setup.bash

printf 'ROS_DISTRO=%s

' "$ROS_DISTRO"

ros2 --help | head -n 5

检查原则  “软件装了”不等于“当前终端加载了”。先确保基础的 ros2 命令有效，再编译 SDK 自定义消息；不要把 apt 的安装步骤当作消息包生成的替代。

新终端为什么会“忽然失效”？

source 只修改当前 shell 的环境变量。即使刚刚成功运行过 Python 导入，只要打开新终端，overlay 就可能丢失。建议把文中环境准备命令集中放入一个 lab_env.sh 文件，通过 source lab_env.sh 加载，但不要在不确认路径的情况下修改全局 shell 配置。

远程图形注意  SSH 终端如果没有 DISPLAY 或可用的桌面会话，rqt/RViz 可能报显示服务错误。这不是 TouchState 本身有问题；先验证 echo "$DISPLAY" 与 GUI 会话。

 

04  /  SDK 构建

只编译本次需要的 aimdk_msgs：把接口问题单独隔离出来

先确认压缩包中的目录，再从源码生成 ROS 2 Python 消息绑定。

图 3｜源码、构建产物、当前 shell 的可见性是三个不同检查层。

创建独立实验目录并构建

mkdir -p "$HOME/q1_touch_lab"

cd "$HOME/q1_touch_lab"

# 将从开发者下载页取得的 Q1 SDK 解压到此处

# 确认最终存在：sdk_q1-v1.0.0.0/aimdk_msgs/package.xml

source /opt/ros/humble/setup.bash

colcon build --base-paths sdk_q1-v1.0.0.0/aimdk_msgs   --packages-select aimdk_msgs --executor sequential

source install/setup.bash

ros2 interface show aimdk_msgs/msg/TouchState

python3 -c 'from aimdk_msgs.msg import TouchState; print(TouchState.TOUCH_EVENT_SINGLE_CLICK)'

判断标准  能读出消息定义并成功导入 TouchState 才算消息接口层就绪。单次构建出现 warning 并不自动等于失败，应以退出码、最终构建摘要和 Python 导入结果共同判断。

两个额外的定位命令

ros2 pkg prefix aimdk_msgs

python3 -c 'import aimdk_msgs; print(aimdk_msgs.__file__)'

# 包可见性与 Python 模块位置都应指向当前实验工作空间

如果 ros2 interface show 成功但 Python 导入失败，还要检查是否在 Conda、venv 或非 ROS 的 Python 解释器中执行。构建时用到的解释器、当前 shell 的 PYTHONPATH，以及 ROS 2 安装方式不一致，都可能引入类似故障。

 

05  /  消息协议

TouchState 不是普通 std_msgs：字段含义要逐项对应

不要把值为 1 的字段直接理解为“头部单击”，必须连同字段名和版本常量解释。

图 4｜TouchState 字段结构与本机校验条件，具体事件能力以当前 SDK 文档为准。

字段	正确理解	本机测试取值
sensor_id	传感器位置，需与事件联合判断	1：头部传感器
event_type	事件分类，不代表位置	TOUCH_EVENT_SINGLE_CLICK
header.sequence	同一批测试消息的顺序检查	1 到 10
header.stamp / meas_stamp	时间戳字段，填写规则以接口定义为准	测试时使用节点时钟
header.frame_id	SDK 定义的关联坐标系	仅在隔离测试中用作来源标记

关键区别  触摸属于事件上报，不是必然以 1 Hz 产生的周期状态流。后文发布器按 1 Hz 发包只是为了让 GUI 和日志更容易观察，不能据此推断实体传感器的真实上报频率。

阅读自定义接口的推荐顺序

第一遍只看字段与常量名称，搞清传感器编号和事件类型的关系；第二遍查看嵌套 header 定义，明确 timestamp、sequence 的数据类型；第三遍打开 SDK 的事件能力说明，确认当前型号实际支持哪些事件。消息定义中出现某个常量，并不表示 Q1 实体硬件就支持相应的全部交互手势。

 

06  /  PYTHON 订阅

第一个可执行程序：先接收，后判断，再留下证据

这份订阅器只识别头部单击，额外过滤隔离测试标记，并给出接收顺序与超时结论。

保存为 touch_audit.py（完整示例）

#!/usr/bin/env python3

import time

import rclpy

from rclpy.node import Node

from rclpy.qos import qos_profile_sensor_data

from aimdk_msgs.msg import TouchState

 

TOPIC = '/aima/hal/touch/state'

EXPECTED = list(range(1, 11))

 

class TouchAudit(Node):

    def __init__(self):

        super().__init__('q1_touch_audit')

        self.seq = []

        self.create_subscription(TouchState, TOPIC,

                                 self.on_message, qos_profile_sensor_data)

 

    def on_message(self, msg):

        # 此标记仅用于隔离的本机自测，不可代替正式坐标系

        if msg.header.frame_id != 'sdk_local_test':

            return

        if msg.sensor_id != 1:

            return

        if msg.event_type != TouchState.TOUCH_EVENT_SINGLE_CLICK:

            return

        seq = int(msg.header.sequence)

        self.seq.append(seq)

        print(f'接收单击: 序号={seq}, 数量={len(self.seq)}', flush=True)

 

def main():

    rclpy.init()

    node = TouchAudit()

    result = 1

    try:

        end = time.monotonic() + 25.0

        while rclpy.ok() and len(node.seq) < 10 and time.monotonic() < end:

            rclpy.spin_once(node, timeout_sec=0.2)

        passed = node.seq == EXPECTED

        result = 0 if passed else 1

        print(f'验收={passed}; 实收={node.seq}', flush=True)

    finally:

        node.destroy_node()

        if rclpy.ok(): rclpy.shutdown()

    return result

 

if __name__ == '__main__':

    raise SystemExit(main())

设计说明  QoS 使用 ROS 2 的 sensor_data 预设，通常为 Best Effort；接收端要求较低，能够匹配 Reliable 发布端。此文件专为一轮 10 条合成事件统计，不是持续监听真机的最终业务程序。

 

07  /  PYTHON 发布 · 上

模拟输入不是截图：生成“可数、可解释、可隔离”的十条事件

真正重要的不是 publish 调用发生了几次，而是消息字段是否正确、订阅端是否完整收到。

保存为 touch_fixture.py（上半部分）

#!/usr/bin/env python3

import argparse

import time

import rclpy

from rclpy.node import Node

from rclpy.qos import QoSProfile, ReliabilityPolicy, DurabilityPolicy

from aimdk_msgs.msg import TouchState

 

TOPIC = '/aima/hal/touch/state'

 

def parse_args():

    p = argparse.ArgumentParser()

    p.add_argument('--count', type=int, default=10)

    p.add_argument('--hz', type=float, default=1.0)

    return p.parse_args()

 

def main():

    args = parse_args()

    if not 1 <= args.count <= 10000 or not 0 < args.hz <= 50:

        raise SystemExit('参数范围：count=1..10000，hz=(0,50]')

    rclpy.init()

    node = Node('q1_touch_local_fixture')

    qos = QoSProfile(depth=10, reliability=ReliabilityPolicy.RELIABLE,

                     durability=DurabilityPolicy.VOLATILE)

    pub = node.create_publisher(TouchState, TOPIC, qos)

    sent = 0

    try:

        deadline = time.monotonic() + 8.0

        while pub.get_subscription_count() == 0 and time.monotonic() < deadline:

            rclpy.spin_once(node, timeout_sec=0.1)

        if pub.get_subscription_count() == 0:

            raise RuntimeError('尚未发现订阅端，停止本次发布')

为什么先等待订阅端？

ROS 2 的端点发现并非同步函数调用。有限条数实验如果启动发布端过快，即使消息类型完全正确，首条消息也可能早于接收器建立连接。先发现订阅端，再留短暂稳定时间，可以减小这类时序干扰；但 Best Effort 仍不保证零丢失。

先别运行  这段是同一文件的上半部分，紧接着把下一页的剩余代码追加到文件末尾再执行。请只在隔离的单机实验域使用该合成发布器。

 

08  /  PYTHON 发布 · 下

填充 SDK 字段、按节奏发布，并正确释放节点

完整文件执行后，应看到按序发送记录；是否“验收通过”仍以订阅端结果为准。

touch_fixture.py（接上页的下半部分）

        time.sleep(0.7)  # 发现稳定时间，不是送达保证

        for number in range(1, args.count + 1):

            msg = TouchState()

            msg.header.stamp = node.get_clock().now().to_msg()

            msg.header.meas_stamp = msg.header.stamp

            msg.header.sequence = number

            msg.header.frame_id = 'sdk_local_test'  # 仅本机模拟

            msg.sensor_id = 1

            msg.event_type = TouchState.TOUCH_EVENT_SINGLE_CLICK

            pub.publish(msg)

            sent += 1

            print(f'模拟发送: 序号={number}', flush=True)

            rclpy.spin_once(node, timeout_sec=0.05)

            if number < args.count:

                time.sleep(1.0 / args.hz)

        rclpy.spin_once(node, timeout_sec=0.4)

    finally:

        print(f'发布调用次数={sent}', flush=True)

        node.destroy_node()

        if rclpy.ok(): rclpy.shutdown()

 

if __name__ == '__main__':

    main()

运行之前，确认两个约束

标记只在实验中有效  这里借用 frame_id 放入 sdk_local_test，以便示例计数器识别测试数据。真实接入应恢复该字段在 SDK 中的坐标系语义，并以隔离域、专用话题或其他方式区分测试流。

谨慎连接实际设备  此脚本是在测试域向官方同名话题注入合成事件；不要在未隔离的真机运行环境中运行它，否则应用可能把模拟消息误判为真实触摸。

预期而非实测日志  本机发布器应依次打印“模拟发送: 序号=1”至“序号=10”，最后打印“发布调用次数=10”。这些文字是代码预期输出，不构成实际到达的证据。

 

09  /  复现实验

三个终端，完成十条触摸消息的完整验收

将接口层、通信层与接收应用分开观察；每次打开新终端都需要重载环境。

图 5｜有限次数消息测试的正确时序与最终验收条件。

统一终端环境（每个终端先执行）

export Q1_LAB="$HOME/q1_touch_lab"

source /opt/ros/humble/setup.bash

source "$Q1_LAB/install/setup.bash"

export ROS_DOMAIN_ID=71

export ROS_LOCALHOST_ONLY=1

export RMW_IMPLEMENTATION=rmw_fastrtps_cpp

终端 A  python3 touch_audit.py —— 启动后应进入接收等待。

终端 B  python3 touch_fixture.py --count 10 --hz 1 —— 先检查是否发现订阅者，再逐条发送。

终端 C  ros2 topic info -v /aima/hal/touch/state —— 观察类型、端点数量及 QoS。

三组独立的观察记录

第一组保留终端 A 的实收序号与最终返回码；第二组保留终端 B 的发送调用计数；第三组保存终端 C 的发布/订阅端点 QoS。只有三组记录对应同一次启动条件，才能得出有效结论。改变发布频率或 QoS 后，应重新启动整轮实验并保存新的结果，而不是把不同轮的截图拼作一次成功。

本轮通过标准  A 端出现“验收=True”，且序号完整为 1～10；B 端报告发送 10 次；C 端可看到相互兼容的发布和订阅端。若前两者不一致，以接收端为准追查。

 

10  /  RQT 可视化

为什么同时打开 rqt_graph 与 Topic Monitor？

一张节点图能看到连通结构，却不能证明应用拿到了正确字段；两个窗口需要配合使用。

图 6｜rqt 界面结构示意：左侧为节点关系，右侧为消息字段；非真实软件截图。

有图形会话的终端，分别启动

ros2 run rqt_graph rqt_graph

# 新终端，加载完全相同的 ROS 环境

ros2 run rqt_topic rqt_topic

看节点  在 rqt_graph 选择包含所有节点与话题的显示模式并刷新；注意发布端是否连接到了 /aima/hal/touch/state，以及订阅端是否仍在运行。

看字段  在 Topic Monitor 展开 TouchState 的 header、sensor_id、event_type，观察 sequence 是否持续变化。字段值变化才说明窗口正在收到当前输入。

看含义  1 Hz 是本机发布器的节奏；真实触摸属于事件驱动。界面展示的频率和带宽并不是“机器人硬件性能指标”。

图形窗口的三个错觉

打开 rqt_graph 看到节点并不代表数据必然送达；Topic Monitor 的字段保持不变也可能只是最近一次缓存显示；刷新按钮更不等于新消息产生。正确做法是保留持续输入，核对 sequence 随时间递增，再用接收程序的统计结果验证消息是否真正进入回调。

 

11  /  QOS 原理

节点发现了，为什么接收器仍然“零消息”？

QoS 使用 Request/Offered 兼容模型；检查 Reliability 是最快的定位起点。

图 7｜仅对 Reliability 的兼容性作图，不等于已经验证全部 QoS 策略。

发布方提供	订阅方要求	是否兼容	工程含义
Best Effort	Best Effort	是	可能出现丢样；仍要做序号统计
Best Effort	Reliable	否	要求高于提供，通常无法匹配
Reliable	Best Effort	是	订阅端可接受较低传输保证
Reliable	Reliable	是	链路可匹配，但应用仍须验证结果

别漏掉其他策略  Durability、History、Depth 也可能影响观测行为；例如新订阅者是否能拿到历史数据，与只检查 Reliability 完全不同。对每个端点，用 ros2 topic info -v 查看实际配置。

一次最小的 QoS 对照试验

保持话题、消息类型和 Domain 不变，只将测试发布器的 Reliability 从 Reliable 改成 Best Effort，而订阅端仍使用 Reliable。此时预期出现不兼容现象；再把订阅端改成 Best Effort，重新启动并观察消息是否恢复。两轮之间关闭旧端点，能避免多发布源掩盖真正的配置关系。

 

12  /  通信巡检

当“看得到话题”不等于“收得到数据”时，按层排查

将端点发现、消息定义、QoS 与时间顺序分开，避免误把一个警告当成全部原因。

推荐的命令组合

ros2 topic list -t

ros2 topic type /aima/hal/touch/state

ros2 interface show aimdk_msgs/msg/TouchState

ros2 topic info -v /aima/hal/touch/state

ros2 topic echo /aima/hal/touch/state   --qos-reliability best_effort --qos-durability volatile

ros2 node list

printenv ROS_DOMAIN_ID ROS_LOCALHOST_ONLY RMW_IMPLEMENTATION

观察到的症状	优先原因	下一步验证
Python 模块无法导入	未 source SDK 工作空间	检查 install/setup.bash 与 sys.path
列表没有目标话题	发布节点未启动或域不一致	ros2 node list；比较通信环境
看到端点但 echo 为空	QoS 不兼容 / 并未发送数据	topic info -v 与发布日志
少收首条 / 收齐不稳定	发现与启动时序、Best Effort	预热、序号检查、独立复测
仅 rqt 为空而 Python 正常	GUI 订阅配置不同	对照 GUI 端点 QoS

特别提醒  ROS 2 daemon 的缓存与节点发现可能导致短时显示滞后。必要时可使用 ros2 daemon stop 再重新查询，但不要将其当作修复 QoS 不兼容的办法。

跨机器观察时，再多查三项

本机实验将通信限制在 localhost；跨设备连接时必须按实际网络解除该约束。同时检查防火墙、网卡选择与多播发现条件，并核对设备与开发机使用相同的通信域和 DDS 实现。不能把单机能运行直接理解为跨机器必然连通。

 

13  /  案例式排错

从 ModuleNotFoundError 到零消息：用故障树定位根因

排障先复现最小失败，再保留改变前后的配置。

图 8｜四层故障定位树；每层配有可以独立验证的观测点。

两个高频故障的正确处理顺序

故障 A · 能 import rclpy，却不能 import aimdk_msgs  先检查当前终端是否执行 source ~/q1_touch_lab/install/setup.bash；再用 python3 -c 检查消息模块；只有产物确实缺失时再重新构建。

故障 B · 端点可见，回调不输出  先执行 ros2 topic info -v 对比发布者与订阅者的 Reliability、Durability；随后核对 topic 字符串、ROS_DOMAIN_ID、RMW 与接收端过滤条件。

故障 C · 发了 10 条，收了 9 条  记录缺失的是哪一个序号；若只有首条缺失，重点检查订阅端启动与发现稳定时间。若中间丢失，结合 QoS、负载和网络进一步复测。

建议记录的故障三元组

保存“输入条件—可观察现象—最小修复”三项信息：例如仅加载系统 ROS 环境时模块找不到；补充 overlay 后导入恢复；再改动 QoS 后才出现事件回调。故障与修复都可重复，才有条件把解决方法迁移到另一台开发机。

证据优先  失败样例要注明“预期复现实验现象”与“本次实际日志”的区别。本教程不展示未经现场运行取得的假截图或虚构的性能指标。

 

14  /  认识模型

走进 Q1 URDF：先搞清 link、joint、mesh 的职责

任何 RViz 外形显示问题，都应先问它是几何资源错误，还是坐标变换问题。

图 9｜Q1 关节层级概念图，分支仅用于教学，不替代实际 URDF 全树。

模型对象	具体作用	排查入口
link	定义刚体、惯量、可视化和碰撞几何	XML 的 <link>、visual、collision
joint	连接 parent/child，定义轴、原点与限位	<joint> 的 type、axis、limit
mesh	从 STL 等文件加载外形	<mesh filename="…"> 的资源路径
TF	描述 link 之间的实时坐标变换	/tf、/tf_static 与 Fixed Frame

参考模型信息  Q1 V3.0 的参考包使用 q1_v3/urdf/q1_v3.urdf。模型中的 link、joint 数量与名称建议通过脚本自行枚举；不要把某个旧版本的模型统计硬套到新下载包。

三种可见症状分别意味着什么

整机完全不出现，应先看 robot_description 和 Fixed Frame；外形有缺损，要从 mesh URI 和资源定位入手；外形可见但滑条无响应，要看 /joint_states 是否发布、名称是否与 URDF 完全匹配，以及 TF 是否实时更新。按症状分层，比一次性修改 URDF 更可控。

 

15  /  模型资源

STL 明明存在，RViz 为什么还是只显示一部分？

最常见的原因不是外形损坏，而是 mesh URI 未被当前 ROS 2 环境正确解析。

图 10｜从官方 URDF 到派生可视化 URDF 的资源解析过程。

先检查真实路径，不急着改 XML

export Q1_MODEL="$HOME/q1_touch_lab/model/q1_v3/urdf/q1_v3.urdf"

test -f "$Q1_MODEL" && echo 'URDF 文件存在'

python3 - "$Q1_MODEL" <<'PYCODE'

import sys, xml.etree.ElementTree as ET

root = ET.parse(sys.argv[1]).getroot()

links = root.findall('link'); joints = root.findall('joint')

meshes = root.findall('.//mesh')

active = [j for j in joints if j.get('type') != 'fixed']

print('link 数量:', len(links))

print('joint 数量:', len(joints), '活动关节:', len(active))

print('mesh 引用:', len(meshes))

print('根部示例:', links[0].get('name') if links else '无')

PYCODE

诊断方法  若可见 link 却没有网格，先检查相对路径、package:// 的包名、大小写和资源解压目录。所有路径都存在后，再排查姿态树、Fixed Frame 和渲染驱动问题。

为什么不直接覆盖官方 URDF？

覆盖原件后很难判断哪些改动来自资源适配、哪些改动触及几何和动力学参数。派生文件只修正路径，原始 URDF 保持可追溯；一旦模型版本更新，重新执行检查和适配即可生成新文件，也便于对比网格数量与关节名称变化。

 

16  /  URDF 适配

一个可复用的模型派生脚本：保留原文件、检查每处 mesh

脚本读取实际 URDF，遇到未知包名前缀或缺失 STL 会直接停止；不会默默跳过错误。

保存为 prepare_mesh.py

#!/usr/bin/env python3

from pathlib import Path

import sys

import xml.etree.ElementTree as ET

 

src = Path(sys.argv[1]).expanduser().resolve()

out = Path(sys.argv[2]).expanduser().resolve()

root = ET.parse(src)

base_pkg = src.parent.parent  # .../q1_v3

changed = 0

for mesh in root.findall('.//mesh'):

    uri = mesh.attrib['filename']

    if uri.startswith('file://'):

        candidate = Path(uri.removeprefix('file://'))

    elif uri.startswith('package://'):

        spec = uri.removeprefix('package://')

        pkg, sep, rest = spec.partition('/')

        if not sep or pkg != base_pkg.name:

            raise ValueError(f'未知包路径：{uri}')

        candidate = base_pkg / rest

    else:

        candidate = src.parent / uri

    candidate = candidate.resolve()

    if not candidate.is_file():

        raise FileNotFoundError(f'缺少网格文件：{candidate}')

    mesh.set('filename', candidate.as_uri())

    changed += 1

out.parent.mkdir(parents=True, exist_ok=True)

root.write(out, encoding='utf-8', xml_declaration=True)

print(f'已写入派生 URDF：{out}，已校验 mesh：{changed}')

python3 prepare_mesh.py "$Q1_MODEL"   "$HOME/q1_touch_lab/derived/q1_rviz.urdf"

适配边界  仅修改 mesh 的 filename，不修改 joint 的原点、轴、限位、质量和惯量。绝对 file:// 路径用于本机可视化；拷贝到另一台机器需重新生成派生文件。

 

17  /  ROS 2 LAUNCH

把 robot_state_publisher、关节滑条与 RViz 放进同一次启动

这一步只控制模型显示链路，与触摸发布器是两套独立实验。

保存为 q1_view.launch.py

from pathlib import Path

import os

from launch import LaunchDescription

from launch_ros.actions import Node

 

def generate_launch_description():

    path = Path(os.environ['Q1_URDF']).expanduser()

    xml = path.read_text(encoding='utf-8')

    return LaunchDescription([

        Node(package='robot_state_publisher',

             executable='robot_state_publisher',

             parameters=[{'robot_description': xml}],

             output='screen'),

        Node(package='joint_state_publisher_gui',

             executable='joint_state_publisher_gui',

             output='screen'),

        Node(package='rviz2', executable='rviz2',

             output='screen'),

    ])

启动与核对

source /opt/ros/humble/setup.bash

export ROS_DOMAIN_ID=72

export ROS_LOCALHOST_ONLY=1

export Q1_URDF="$HOME/q1_touch_lab/derived/q1_rviz.urdf"

ros2 launch ./q1_view.launch.py

# 可在另一个终端验证：

ros2 topic list -t | grep -E 'joint_states|tf|robot_description'

定位关键  robot_state_publisher 读取 robot_description 和 /joint_states，负责计算并发布 /tf、/tf_static。GUI 发布的是可视化关节状态，不会自动触发机器人实体关节运动。

 

18  /  RVIZ 设置

五步让 Q1 模型出现，并随肩肘滑条连续更新

“看到模型”只是第一层；还要确认 Fixed Frame、TF 和关节名称完整对应。

图 11｜GUI 滑条、/joint_states、robot_state_publisher 与 RViz 的因果关系示意。

01 · 打开显示项  启动 rviz2 后添加 RobotModel；如果版本要求选择模型来源，请选与 robot_description 对应的主题或参数。

02 · 选择参考坐标  Global Options → Fixed Frame 优先填实际 URDF 根坐标系；参考 Q1 模型通常为 pelvis。

03 · 检查模型资源  若机器人只显示坐标轴或部分网格，返回派生 URDF 检查 file:// 路径与材质显示。

04 · 调节一个关节  在 GUI 中先改 left_shoulder_roll_joint，观察上臂是否变化；再尝试 left_elbow_joint。

05 · 复位并比对  将滑条返回零位或初始位置，并用 ros2 topic echo /joint_states 核对角度与关节名称。

如何判断调的是正确关节？

先只动左肩 roll，看上臂横向姿态；复位后只动左肘，观察前臂相对上臂旋转。一次仅改变一个变量，可把关节名称、旋转轴与可视化结果形成对应关系。最后一起调节时再观察组合运动，避免一次拉动多个滑条后无法解释是哪一个 joint 生效。

坐标与单位  关节面板值通常以弧度（rad）表示；模型初始外形还叠加 URDF 中的关节原点旋转。看到的角度不等同于相机中直接测出的视觉角度。

 

19  /  姿态自动演示

把滑条改成脚本：先读限位，再发布 JointState

不用向机器人控制接口发送任何命令，也能验证模型的运动学连通性。

保存为 pose_preview.py（在 GUI 已关闭时运行）

#!/usr/bin/env python3

import os, time, math

import xml.etree.ElementTree as ET

import rclpy

from rclpy.node import Node

from sensor_msgs.msg import JointState

 

xml = ET.parse(os.environ['Q1_URDF']).getroot()

limits = {}

for j in xml.findall('joint'):

    if j.get('type') not in ('revolute', 'continuous'): continue

    lim = j.find('limit')

    lo = float(lim.get('lower', '-3.14')) if lim is not None else -3.14

    hi = float(lim.get('upper', '3.14')) if lim is not None else 3.14

    limits[j.get('name')] = (lo, hi)

 

rclpy.init()

n = Node('q1_joint_preview')

pub = n.create_publisher(JointState, '/joint_states', 10)

names = list(limits)

start = time.monotonic()

try:

    while rclpy.ok() and time.monotonic() - start < 8:

        t = time.monotonic() - start

        m = JointState(); m.header.stamp = n.get_clock().now().to_msg()

        m.name = names

        m.position = []

        for name in names:

            lo, hi = limits[name]

            zero = min(max(0.0, lo), hi)

            delta = min(0.12, max(0.0, (hi - lo) / 5))

            pos = zero + (delta * math.sin(t) if name in

                   ('left_shoulder_roll_joint', 'left_elbow_joint') else 0)

            m.position.append(min(max(pos, lo), hi))

        pub.publish(m)

        rclpy.spin_once(n, timeout_sec=0.01)

        time.sleep(0.05)

finally:

    n.destroy_node(); rclpy.shutdown()

操作边界  关闭 Joint State Publisher GUI 后再单独运行这段脚本；否则同时有两个 /joint_states 来源，姿态会互相覆盖。若 URDF 中对应的关节名称不同，代码会保持其它关节静态，应按实际名称更新。

 

20  /  验收与复用

从“试着跑一下”升级到可回归的六项验收

再换一次终端、另启一次模型窗口，仍能复现同样的判断，流程才算可复用。

图 12｜独立六项验收卡，适合作为下一次 SDK 更新后的回归检查。

检查项	通过条件	失败留存证据
消息绑定	ros2 interface show 与 Python import 同时正常	接口定义与环境快照
事件字段	sensor_id / event_type 符合 SDK 常量	接收端字段快照
通信配对	端点显示 QoS 兼容	topic info -v 输出
有限条数	接收数与 [1..10] 顺序一致	序号、数量与耗时
图形界面	rqt 节点图、字段更新合理	域、话题名与窗口设置
模型显示	关节更新，RViz 姿态相应变化	URDF、关节名称与 TF

需要特别保留的环境快照  SDK 压缩包版本、工作空间位置、ROS_DISTRO、ROS_DOMAIN_ID、RMW_IMPLEMENTATION、Python 版本、模型文件路径和脚本版本。它们比单张漂亮截图更有助于下次定位变化。

一次配置变化，只对应一次结论

切换消息库版本时先复查自定义消息定义；切换 RMW 或网络环境时重新验证端点发现与 QoS；更换 URDF 时重新核对 mesh 和 joint 名称。将配置变化与复测记录一一对应，才能把调试经验真正积累成可迁移的工程知识。

 

21  /  常见问答与资料

最后五问：把第一次调试最容易混淆的边界讲清

这些问题在“消息能收、模型也能显示”之后仍然很重要。

Q1 · 需要机器人实体才能完成全文吗？  不需要。消息类型构建、本机模拟收发、rqt 和离线模型都可独立完成；但实体头部触摸的真实性能必须在设备连接后另行验证。

Q2 · Reliable 一定保证十条消息全部到达吗？  不能将通信策略等同于端到端验收。节点发现、发布退出时机和应用处理仍影响有限次试验，因此仍要从接收端统计。

Q3 · rqt_graph 有连线，为什么消息字段不更新？  节点发现与有效载荷到达是两个观测层；继续核对 GUI 端点 QoS、消息来源、订阅过滤及发送端当前是否仍在发布。

Q4 · RViz 中模型变形，是 URDF 文件坏了吗？  不一定。先确认 mesh 路径、Fixed Frame、/joint_states 的 name/position 对应关系，再看关节轴和原点，而不是直接修改惯量参数。

Q5 · 触摸事件能直接触发动作吗？  可以在业务应用中将事件转换为经过权限与安全边界检查的决策输入，但触摸反馈本身不是运动控制指令；本文刻意不涉及实体设备动作下发。

资料与接口入口

① 启元机器人开发者资源与 Q1 SDK / URDF 下载：https://dev.primebot.com/download

② ROS 2 Humble 安装与教程：https://docs.ros.org/en/humble/

③ ROS 2 QoS 兼容规则：https://github.com/ros2/ros2_documentation/blob/rolling/source/ROS-Framework/interfaces/topics/About-Quality-of-Service-Settings.rst

④ robot_state_publisher 组件说明：https://index.ros.org/p/robot_state_publisher/

⑤ joint_state_publisher 组件说明：https://docs.ros.org/en/ros2_packages/rolling/api/joint_state_publisher/index.html

收束  真正值得复用的不是某条成功日志，而是“接口可导入、字段可解释、传输可观测、故障可定位、姿态可复现”的完整因果链。