小丸子博客

本站只有一些笔记，请不要有任何恶意

没有真机，我给启元 Q1 造了一台“假机器人“：官方 SDK 示例跑通实录

· 小丸子 · 阅读 0 · 分类： 技术分享

整理一篇学习笔记，把看到的一些要点和自己的理解都记下来。

拿到启元 Q1 的 SDK，examples/python/ 里整整齐齐躺着 18 个示例程序：读电池、读触摸、播语音、控关节、放视频……看着都能跑。

但一执行，大部分会在十秒后报错退出：

Timed out after 10.000 s waiting for /aima/hal/joint/state

原因很简单：这些示例都是"客户端"，它们在等机器人发数据。 没有机器人，就没有人对面应答。

那能不能不给它真机器人，给它一个"假机器人"？

这不是作弊，这是 ROS2 架构本来允许的事：只要话题名、消息类型、QoS 三者对得上，订阅端根本不在乎发布端是铁皮做的还是 Python 写的。这篇这篇就把这台假机器人从零造出来，让官方示例真正跑一遍。

实测环境：Windows 11 + WSL2（Ubuntu 22.04.5）+ ROS2 Humble + 启元 Q1 SDK v1.0.0.0 + Python 3.10。全部命令都可以照做。
完整代码：https://atomgit.com/2401_86326742/q1-mock-robot

一、先看清：示例到底在等什么

官方 Q1 SDK 仓库中的 Python 示例目录和 API 列表（

翻一遍 18 个示例，依赖关系其实只有三类：

依赖类型例子没有真机时的表现订阅状态话题get_bms_state.py、get_touch_state.py一直等，没有任何输出读取状态后闭环控制joint_control.py10 秒超时退出调用服务play_emotion.py、shutdown_task.py请求发出去，等不到响应

咱们要做的，只是把第一类填上，第二类就能跟着跑通。第三类（服务）这次先不碰，因为服务端要真正实现业务逻辑，留给以后。

先确认三件事：话题名、消息类型、QoS。这三样在 SDK 的接口定义文件里全都写着。

source /opt/ros/humble/setup.bash
source ~/workspace/sdk_q1/install/setup.bash

ros2 interface show aimdk_msgs/msg/JointStateArray
ros2 interface show aimdk_msgs/msg/Bms
ros2 interface show aimdk_msgs/msg/TouchState

二、假机器人发哪三条消息

话题消息类型官方说明里的频率与 QoS咱们实际发/aima/hal/joint/stateJointStateArray500 Hz，BestEffort，Depth 1020 Hz/aima/hal/bms/stateBms1 Hz，BestEffort，Depth 101 Hz/aima/hal/touch/stateTouchState触发式，Reliable，Depth 1每 5 秒一次

官方接口说明中 /aima/hal/joint/state 的 QoS 与频率定义（

关节状态为什么可以降到 20 Hz？因为示例要的是"当前姿态"这个快照，不是连续波形。joint_control.py 收到第一条状态就开始规划，之后靠自己以 500 Hz 往下发命令，不再依赖状态流。20 Hz 足够。

电池和触摸照原频率来，因为这两个示例的打印节奏依赖数据到达节奏，降频会让输出看起来"卡顿"。

三、第一个坑：QoS 配反了，消息就收不到

ROS2 的 QoS 兼容规则是单向的：发布者提供的质量务必不低于订阅者要求的质量。

  • 发布 RELIABLE → 订阅者要 BEST_EFFORT：兼容 ✅
  • 发布 BEST_EFFORT → 订阅者要 RELIABLE：不兼容 ❌
官方接口说明里，关节和电池状态是 BestEffort。但咱们的假机器人选择用 RELIABLE 发布，原因有两个：

1. 官方示例用 BestEffort 订阅，RELIABLE 发布能被它收到；
2. ros2 topic echo 默认按 RELIABLE 订阅，如果我们也发 BestEffort，命令行工具反而看不到数据，排查时很折磨。

一个参数的选择，同时满足了两拨订阅者。

但这条规则是双向都会咬人的。第九节会亲眼看到它反过来发生一次——那时候是官方示例发、我们的假机器人收，方向一换，参数就得跟着换。

官方接口说明中 /aima/hal/joint/command 的 QoS 定义（

四、22 个关节名不能瞎编

joint_control.py 是按名字匹配关节的（源码里 state_index_by_name 那张表），名字对不上就直接报错退出。所以假机器人上报的关节名，务必和官方模型一致。

这份名单在 SDK 的 URDF 配置里：q1-urdf 包的 config/joint_names_Q1-V2.0-URDF-0601.yaml。22 个：

JOINT_NAMES = [
    "left_hip_pitch_joint", "left_hip_roll_joint", "left_hip_yaw_joint",
    "left_knee_joint", "left_ankle_pitch_joint", "left_ankle_roll_joint",
    "right_hip_pitch_joint", "right_hip_roll_joint", "right_hip_yaw_joint",
    "right_knee_joint", "right_ankle_pitch_joint", "right_ankle_roll_joint",
    "waist_roll_joint", "waist_yaw_joint",
    "left_shoulder_pitch_joint", "left_shoulder_roll_joint",
    "left_shoulder_yaw_joint", "left_elbow_joint",
    "right_shoulder_pitch_joint", "right_shoulder_roll_joint",
    "right_shoulder_yaw_joint", "right_elbow_joint",
]

两腿 12 个、腰 2 个、手臂 8 个。顺序无所谓，名字务必一字不差。

官方 q1-urdf 仓库中的 22 个关节名（config/joint_names_Q1-V2.0-URDF-0601.yaml，

五、开跑前：把 Ruckig 编译出来

joint_control.py 的轨迹规划用的是 Ruckig，SDK 里带的是 ruckig_for_primebot 源码包，需要本地编译，而且它会同时构建一个 Python 模块。

先装构建依赖：

python3 -m pip install --user nanobind

直连 PyPI 失败：[Errno 101] Network is unreachable（实测截图） 坑：如果报 Network is unreachable 或连接 pypi.org 超时，是 WSL 的 NAT 网络问题——DNS 先返回 IPv6 地址，而 WSL 没有 IPv6 路由。换国内镜像即可：

python3 -m pip install --user nanobind -i https://pypi.tuna.tsinghua.edu.cn/simple

换清华镜像后安装成功：Successfully installed nanobind-3.1.0

再编译（只需要这一个包，不用全量）：

cd ~/workspace/sdk_q1
source /opt/ros/humble/setup.bash
colcon build --packages-select ruckig_for_primebot --cmake-args -DCMAKE_BUILD_TYPE=Release

Ruckig 编译完成：Summary: 1 package finished [9.59s]（实测）

编译完成后确认 Python 模块已经生成：

ls -l examples/python/*.so

编译产物已自动复制到 examples/python/：ruckig_for_primebot.cpython-310-x86_64-linux-gnu.so

应当能看到 ruckig_for_primebot.cpython-310-x86_64-linux-gnu.so 之类的文件。

六、假机器人的完整代码

只需要一个文件，约 190 行。它做四件事：按 20 Hz 发关节状态、按 1 Hz 发电池、每 5 秒发一次触摸事件、顺手统计收到了多少条关节命令。

#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
mock_q1_robot.py —— 一台"假机器人"

按启元 Q1 SDK 的接口定义，模拟机器人向外上报状态：
  /aima/hal/joint/state   JointStateArray   关节状态   20 Hz
  /aima/hal/bms/state     Bms               电池状态    1 Hz
  /aima/hal/touch/state   TouchState        触摸事件    每 5 秒

用法：
  source /opt/ros/humble/setup.bash
  source ~/workspace/sdk_q1/install/setup.bash
  python3 ~/workspace/mock_q1_robot.py
"""

import signal

import rclpy
from rclpy.executors import ExternalShutdownException
from rclpy.node import Node
from rclpy.qos import (
    QoSProfile, ReliabilityPolicy, HistoryPolicy, DurabilityPolicy,
)

from aimdk_msgs.msg import (
    Bms, JointCommandArray, JointState, JointStateArray,
    MessageHeader, TouchState,
)

JOINT_NAMES = [
    "left_hip_pitch_joint", "left_hip_roll_joint", "left_hip_yaw_joint",
    "left_knee_joint", "left_ankle_pitch_joint", "left_ankle_roll_joint",
    "right_hip_pitch_joint", "right_hip_roll_joint", "right_hip_yaw_joint",
    "right_knee_joint", "right_ankle_pitch_joint", "right_ankle_roll_joint",
    "waist_roll_joint", "waist_yaw_joint",
    "left_shoulder_pitch_joint", "left_shoulder_roll_joint",
    "left_shoulder_yaw_joint", "left_elbow_joint",
    "right_shoulder_pitch_joint", "right_shoulder_roll_joint",
    "right_shoulder_yaw_joint", "right_elbow_joint",
]

class MockQ1Robot(Node):
    def __init__(self):
        super().__init__("mock_q1_robot")

        qos = QoSProfile(
            history=HistoryPolicy.KEEP_LAST,
            depth=10,
            reliability=ReliabilityPolicy.RELIABLE,
            durability=DurabilityPolicy.VOLATILE,
        )

        self.joint_pub = self.create_publisher(
            JointStateArray, "/aima/hal/joint/state", qos
        )
        self.bms_pub = self.create_publisher(Bms, "/aima/hal/bms/state", qos)
        self.touch_pub = self.create_publisher(TouchState, "/aima/hal/touch/state", qos)

        # 订阅端单独用 BestEffort：joint_control.py 以 BestEffort 发命令，
        # 订阅方若请求 Reliable，两边不兼容，一条都收不到。
        cmd_qos = QoSProfile(
            history=HistoryPolicy.KEEP_LAST,
            depth=10,
            reliability=ReliabilityPolicy.BEST_EFFORT,
            durability=DurabilityPolicy.VOLATILE,
        )

        self.cmd_sub = self.create_subscription(
            JointCommandArray, "/aima/hal/joint/command", self.on_command, cmd_qos
        )
        self.cmd_count = 0
        self.seq = 0
        self.touch_seq = 0

        self.create_timer(0.05, self.publish_joint_state)   # 20 Hz
        self.create_timer(1.0, self.publish_bms)            # 1 Hz
        self.create_timer(5.0, self.publish_touch_event)    # 每 5 秒

        self.get_logger().info(
            "假机器人已上线：joint/state 20Hz, bms/state 1Hz, touch/state 每 5 秒一次"
        )

    def _fill_header(self, header: MessageHeader) -> None:
        now = self.get_clock().now().to_msg()
        header.stamp = now
        header.meas_stamp = now
        header.frame_id = "pelvis"
        header.sequence = self.seq
        self.seq += 1

    def publish_joint_state(self) -> None:
        msg = JointStateArray()
        self._fill_header(msg.header)
        for name in JOINT_NAMES:
            joint = JointState()
            joint.name = name
            joint.position = 0.0
            joint.velocity = 0.0
            joint.effort = 0.0
            joint.coil_temp = 35
            joint.motor_temp = 38
            joint.motor_vol = 48
            msg.joints.append(joint)
        self.joint_pub.publish(msg)

    def publish_bms(self) -> None:
        msg = Bms()
        self._fill_header(msg.header)
        msg.manufacturer = "MOCK"
        msg.serial_number = "MOCK-Q1-0001"
        msg.protocol_version = "1.0"
        msg.hardware_version = "v1.0"
        msg.software_version = "v1.0"
        msg.status = 1  48.0 V
        msg.current = 500             # mA，充电为正
        msg.power = 24_000            # mW
        msg.temperature = 36.5
        msg.remaining_capacity = 4_800
        msg.remaining_capacity_percentage = 96
        msg.cycle_count = 37
        msg.cycle_total_capacity = 210_000
        self.bms_pub.publish(msg)

    def publish_touch_event(self) -> None:
        msg = TouchState()
        self._fill_header(msg.header)
        msg.sensor_id = 1             # Q 机型：1 = 头部
        msg.event_type = 1            # 单击
        msg.header.sequence = self.touch_seq
        self.touch_seq += 1
        self.touch_pub.publish(msg)

    def on_command(self, msg: JointCommandArray) -> None:
        self.cmd_count += 1
        if self.cmd_count == 1 or self.cmd_count % 200 == 0:
            self.get_logger().info(
                f"收到关节命令 {self.cmd_count} 条，本帧含 {len(msg.joints)} 个关节"
            )

def main(args=None) -> int:
    rclpy.init(args=args)
    node = MockQ1Robot()

    def _on_sigterm(_signum, _frame):
        if rclpy.ok():
            rclpy.shutdown()

    signal.signal(signal.SIGTERM, _on_sigterm)

    try:
        rclpy.spin(node)
    except (KeyboardInterrupt, ExternalShutdownException):
        pass
    finally:
        node.destroy_node()
        if rclpy.ok():
            rclpy.shutdown()
    return 0

if __name__ == "__main__":
    raise SystemExit(main())

保存为 ~/workspace/mock_q1_robot.py，开一个终端挂着：

source /opt/ros/humble/setup.bash
source ~/workspace/sdk_q1/install/setup.bash
python3 ~/workspace/mock_q1_robot.py

看到这一行，假机器人就上班了：

[INFO] [...] [mock_q1_robot]: 假机器人已上线：joint/state 20Hz, bms/state 1Hz, touch/state 每 5 秒一次

这个终端保持开着，后面的示例都在另一个终端跑。

双终端分工：左侧编译并启动假机器人，右侧运行官方 BMS 示例

七、实测一：get_bms_state.py，收到一份完整电池报告

另开终端：

cd ~/workspace/sdk_q1
source /opt/ros/humble/setup.bash
source install/setup.bash
python3 examples/python/get_bms_state.py

输出（实测原样，此后每秒一条，这里只贴第一条）：

[INFO] [1792378007.884948651] [get_bms_state]: Subscribing bms topic: /aima/hal/bms/state
[INFO] [1792378008.075471793] [get_bms_state]: BmsState received
  manufacturer:                 MOCK
  serial_number:                MOCK-Q1-0001
  protocol_version:             1.0
  hardware_version:             v1.0
  software_version:             v1.0
  power_request:                0
  status:                       1048576
  balance_line_resistance:      12 mOhm
  voltage:                      48.000 V
  current:                      0.500 A
  power:                        24.000 W
  temperature:                  36.500 °C
  remaining_capacity:           4800 mAh
  remaining_capacity_percentage:96 %
  cycle_count:                  37
  cycle_total_capacity:         210.000 Ah

数值是我们编的，但格式一个字段都不差——这就是接口定义的作用：形状对了，数据来源是谁并不重要。

顺手读两个数：

  • status: 1048576 = 2²⁰，对应接口注释里的 bit20「电池充满」。
  • current: 0.500 A，正数表示充电方向。
示例每收到一条就打印一次，1 Hz。Ctrl+C 退出。
官方 get_bms_state.py 收到假机器人上报的电池数据

八、实测二：get_touch_state.py，收到第一个触摸事件

python3 examples/python/get_touch_state.py

前面会先打印订阅确认，之后每 5 秒一条事件：

[INFO] [1792378021.775844287] [get_touch_state]: Subscribing touch topic: /aima/hal/touch/state
[INFO] [1792378025.075240415] [get_touch_state]: TouchState received
  frame_id: pelvis
  sequence: 9
  stamp:    1792378025.074177 s
  meas:     1792378025.074177 s
  sensor:   1
  event:    1
[INFO] [1792378030.075102619] [get_touch_state]: TouchState received
  frame_id: pelvis
  sequence: 10
  stamp:    1792378030.073844 s
  meas:     1792378030.073844 s
  sensor:   1
  event:    1

两条事件间隔 5 秒，和假机器人设定的节奏对上了。sensor: 1 是头部（Q 机型），event: 1 是单击。

官方 get_touch_state.py 连续收到 4 次触摸事件，间隔 5 秒（实测）
坑：Ctrl+C 退出时，终端会连打两段 Traceback：先是 KeyboardInterrupt，接着是 rclpy._rclpy_pybind11.RCLError: failed to shutdown: rcl_shutdown already called。原因是示例的 finally 里又调了一次 rclpy.shutdown()，而键盘中断已经把上下文关掉了。两个官方示例（BMS 和触摸）都会这样，属于退出处理不完善，不影响数据接收，可以直接忽略。

九、实测三：joint_control.py，从"能收"走到"能控"

这是这次的重头戏。它不再是只读，而是真的往 /aima/hal/joint/command 发指令：

python3 examples/python/joint_control.py --ros-args \
  -p joint_names:="['left_elbow_joint']" \
  -p target_positions:="[0.6]" \
  -p max_velocity:=0.5 \
  -p max_acceleration:=2.0

（只动左肘一个关节，目标 0.6 弧度。真机上跑之前必须把机器人平躺或悬挂——示例源码第一行就写了这句警告。）

启动后的完整实测输出：

[INFO] [1792378291.863166067] [joint_control]: Waiting for /aima/hal/joint/state, joints=[left_elbow_joint], targets=[0.600000], control_period_s=0.002, state_timeout_s=10.000, publish_full_command=true
[INFO] [1792378291.873032055] [joint_control]: Trajectory initialized from current=[0.000000] to target=[0.600000], stiffness=[20.000000], damping=[2.000000]
[INFO] [1792378293.403385983] [joint_control]: Trajectory completed. planned_duration=1.530000 s, runtime=1.530420 s, cycles=765, actual_hz=499.86, avg_cycle_ms=0.200, max_cycle_ms=0.410, overruns=0, avg_overrun_ms=0.000, max_overrun_ms=0.000

官方 joint_control.py 完成 0→0.6 rad 轨迹规划（500 Hz，零超时）

这段统计怎么读：

字段含义planned_durationRuckig 算出来的理论用时runtime实际跑了多久cycles控制周期数（500 Hz × 时长）actual_hz实际循环频率，越接近 500 越好overruns单个周期超过 2 ms 的次数

同时在假机器人那个终端，会看到（实测原样）：

[INFO] [1792378284.733522645] [mock_q1_robot]: 假机器人已上线：joint/state 20Hz, bms/state 1Hz, touch/state 每 5 秒一次
[INFO] [1792378291.875940836] [mock_q1_robot]: 收到关节命令 1 条，本帧含 22 个关节
[INFO] [1792378292.273350214] [mock_q1_robot]: 收到关节命令 200 条，本帧含 22 个关节
[INFO] [1792378292.673649714] [mock_q1_robot]: 收到关节命令 400 条，本帧含 22 个关节
[INFO] [1792378293.073614579] [mock_q1_robot]: 收到关节命令 600 条，本帧含 22 个关节

最终一条停在 600 而不是 765，是因为脚本每 200 条才打印一次——765 还没到 800 的打印点。这条日志是整篇这篇的闭环证明：官方示例真的把命令发出去了，假机器人真的收到了。

假机器人端收到 1 / 200 / 400 / 600 条命令，每帧 22 个关节（实测）

一个细节：publish_full_command 默认是 true，意思是每帧把 22 个关节全发一遍，只改动目标关节的位置，其余保持当前值。这是为了兼容那些"整帧覆盖"的控制器。如果你想只发被控关节，加 -p publish_full_command:=false 即可。

中间那条 WARN：QoS 反过来咬了一次

上面输出是修好之后的干净版本。第一次跑时其实多冒出来一条警告：

New subscription discovered on topic '/aima/hal/joint/command', requesting incompatible QoS.
No messages will be sent to it. Last incompatible policy: RELIABILITY

第一次运行时的现场：假机器人订阅 QoS 与 joint_control.py 的发布 QoS 不兼容，一条命令都收不到

joint_control.py 用 BEST_EFFORT 发命令，而假机器人最初订阅命令时，偷懒复用了发布状态那套 RELIABLE 的 QoS。方向反过来，规则就翻了脸：发布端提供 BEST_EFFORT，订阅端要求 RELIABLE，不兼容，ROS2 直接把这条链路掐掉——一条命令都收不到。

改法很简单：给订阅单独建一个 QoS，用 BEST_EFFORT。第六节的代码里已经改好了，就是那段 cmd_qos。

这件事说明：QoS 不是全局设置，而是每个发布者、每个订阅者各自声明的一份契约。 你自己发的时候可以挑最稳的，去订阅别人的时候必须按对方的规格来。

十、三个典型报错，对号入座

报错原因处理Missing joints in /aima/hal/joint/state: ['xxx']关节名和假机器人发的对不上对照 JOINT_NAMES 逐字检查Timed out after 10.000 s waiting for /aima/hal/joint/state假机器人没开，或两边 ROS_DOMAIN_ID 不同先确认终端 A 在跑；再 echo $ROS_DOMAIN_ID 两边比对ModuleNotFoundError: No module named 'ruckig_for_primebot'Ruckig 没编译，或没 source 环境回第五节编译；确认已在 SDK 根目录执行 source install/setup.bash

十一、这台假机器人能验什么，不能验什么

说清楚边界，免得被当成"仿真"：

能验：

  • 接口链路通不通（话题名、消息类型、QoS 三者是否匹配）
  • 官方示例的代码逻辑和参数含义
  • Ruckig 轨迹规划的输出、控制循环的实际频率、有没有超时
  • 你自己写的上层程序，能不能正确收发这些消息
不能验：
  • 动力学、平衡、地面接触、力矩响应
  • 真实执行器的延迟和误差
  • 动作好不好看
它替代的是"通信对端"，不是"物理世界"。想要后者，得上真实仿真器或者真机。

十二、小结

整个过程其实只做了一件事：把接口定义当成合同，照着合同造了一个能签字的对方。

三条消息、190 行代码，换来的是 18 个官方示例从"跑不起来"变成"能跑、能看输出、能改参数做实验"。对于暂时没有真机的人来说，这是成本最低的一条路——不用装 Gazebo，不用配物理引擎，一个 Python 文件就够。

完整代码、运行步骤和六个坑的排查记录，都在文首那个仓库里（mock_q1_robot.py / README.md / troubleshooting.md）。

版本记录：

组件版本操作系统Ubuntu 22.04.5 LTS（WSL2）ROS2Humble启元 Q1 SDKv1.0.0.0Python3.10ruckig_for_primebotv0.15.5nanobind3.1.0

如果你也在没有真机的情况下折腾这套 SDK，欢迎把踩到的坑贴在评论区。真机到手之前，假机器人先顶着。

本文用到的 SDK、URDF 资料包和接口文档，都来自启元机器人开发者社区：https://dev.primebot.com/ 。下载页里还有 PrimeConsole 桌面端、各机型 SDK 和工具链，感兴趣可以去瞧瞧。
码，换来的是 18 个官方示例从"跑不起来"变成"能跑、能看输出、能改参数做实验"。对于暂时没有真机的人来说，这是成本最低的一条路——不用装 Gazebo，不用配物理引擎，一个 Python 文件就够。

完整代码、运行步骤和六个坑的排查记录，都在文首那个仓库里（mock_q1_robot.py / README.md / troubleshooting.md）。

版本记录：

组件版本操作系统Ubuntu 22.04.5 LTS（WSL2）ROS2Humble启元 Q1 SDKv1.0.0.0Python3.10ruckig_for_primebotv0.15.5nanobind3.1.0

本文用到的 SDK、URDF 资料包和接口文档，都来自启元机器人开发者社区：https://dev.primebot.com/ 。下载页里还有 PrimeConsole 桌面端、各机型 SDK 和工具链，感兴趣可以去瞧瞧。
以上就是这次整理的全部内容，希望对你有所启发。如果有不同见解，欢迎在评论区交流讨论。
标签： #人工智能 #机器人

评论 (0)

暂无评论