启元 T1 二次开发实录：关节怎么动，状态怎么读
看启元 T1 的产品展示，很容易先被“变形”吸引：人形轮足和四足，看起来像两台机器人。但打开开发资料以后，我想先弄清楚一个更小的问题：程序给了一个目标，模型实际动到了哪里；程序收到了一个状态，页面有没有把它读对。这两件事做扎实，再往上叠动作和交互，排查问题才有抓手。
在T1 产品页里，两种形态放在一起很好辨认。到了工程模型中，这些外形会被拆成连杆、转轴、关节限位和执行器。轮足在哪里、前肢怎样折叠，都能顺着关节树继续查下去，和只看一段产品视频的感受很不一样。
这次就从模型与接口各选一个切口：用官方模型做一段固定基座的关节往返实验，再用官方 SDK 的 Bms 类型搭一个状态观察页。前者看目标角与实际角的差别，后者检查单位、状态位和消息是否仍在更新。过程中还特意保留了一个“明明看得到话题，却一条都收不到”的 QoS 反例。
先交代数据来源：关节角由 MuJoCo 物理步进产生；电池消息由 LOCAL_TEST 测试节点构造，经 ROS 2 实际发布、订阅后显示。两组实验独立运行，电池曲线没有接到仿真模型上。文中的代码、动图与数值都按这个分工阅读。
先把 T1 的模型认清，别急着改角度
从启元开发资料下载页取到 T1 URDF 包后，我先看的不是 launch 文件，而是模型清单。下载页标注 T1 V3.0，包内这份 MJCF 文件仍命名为 T1-V2.xml；下文的关节名和限位都对应这份具体文件，不能单凭下载页名称把它当成所有硬件版本的统一参数。
| 资料中的对象 | 这次实际读到什么 | 在实验里负责什么 |
| T1-V2.urdf | 17 个 link；16 个 joint，其中 14 个 revolute、2 个 fixed | 查连杆关系、关节类型和命名 |
| xml/T1-V2.xml | 14 个电机执行器 | 作为 MuJoCo 物理步进的模型入口 |
| meshes/ | 17 份 STL 网格资源 | 呈现各部件外形，保留与 XML 的相对路径 |
| 包内 launch 文件 | ROS 1 启动方式 | 不能直接交给 ros2 launch；本次直接加载 MJCF |
模型里保留了局部透明材质，转动时能看见结构之间的相对位置。我把基座固定，并调整到便于观察前侧肢体的朝向。这样可以集中看关节跟踪，图中竖直的姿态由固定约束维持。后面的曲线回答的是关节响应问题，轮式自平衡和完整步态需要另设实验。
给一个缓慢目标，再看关节怎样跟上
被观察的关节是 FL_HIP_PITCH_Joint。前一秒保留初始姿态，一秒后在 π/2 附近加上幅度 0.5 rad、频率 0.4 Hz 的正弦目标；同侧的 roll 和 knee 也做小幅配合。目标一个周期是 2.5 秒，动图中很容易看清摆动方向和转折，而不是只看到一团快速晃动的模型。
真正推动关节的关键代码只有下面三步，但三步的顺序很重要：先根据当前位置与速度算输入，再按模型的执行器控制范围裁剪，最后执行 mj_step。除初始化姿态外，循环没有逐帧把目标角直接写进 qpos；画面中显示的角度是步进后的状态。
torque = 55 * (target - data.qpos[qpos]) - 4 * data.qvel[qvel]
data.ctrl[:] = np.clip(torque, model.actuator_ctrlrange[:, 0], model.actuator_ctrlrange[:, 1])
mujoco.mj_step(model, data)这里的位置误差项把关节拉向目标，速度反馈项抑制运动中的晃动。这版控制器用的是实际速度阻尼，没有单独加入目标速度前馈。55、4 以及额外设置的关节阻尼 0.25，都是这次固定基座实验的参数，并非官方控制器默认值。保留了原模型的网格、惯量和电机范围，不等于整份模型所有参数都没变。
动图左上角同时保留了仿真时间和实际关节角，便于把某一帧与记录对应起来。这次共推进 4500 步，步长 0.002 秒，得到约 9 秒仿真时间；从第 500 步后每 50 步取一帧，所以留下 80 帧。GIF 每帧显示 100 毫秒，与 0.1 秒的采样间隔相对应。这里的仿真步频、截图频率和电脑实际渲染速度是三个不同的量。
动图之外，把目标角和实际角画在同一张图上
蓝线使用原始运行记录里的 80 个实际角度，橙色虚线按同一控制公式还原对应时刻的目标。日志在 mj_step 之后记录，因此还原目标时要把记录时间减去一个步长，即 0.002 秒。下半图画“实际角减目标角”，看起来比只凭动图判断是否跟上更直接。
实际角落在约 1.094～2.050 rad，位于模型给出的 -1.1868～4.3284 rad 范围内。这 80 个采样点上的最大绝对角差约为 0.118 rad：上升和下降阶段能看到滞后，转折附近的振幅也略有差别。它说明“目标角”与“实际角”不能混用；0.118 只统计已保存的采样点，不是全部 4500 个步进的误差上界，更不是硬件精度指标。
程序末尾检查了两类条件：整个步进过程中 qpos 都是有限数值；保存下来的目标关节采样点没有越过限位，且确实产生了足够幅度的运动。控制输入的 clip 限制的是执行器输入，角度范围检查则是另一项事。把两者分开，才能看出代码究竟限制了什么、又验证了什么。
完整程序与运行方法
环境采用 MuJoCo 3.16.0，通过 OSMesa 离屏渲染。先按原目录解压模型，使 T1_URDF/xml/T1-V2.xml 和 T1_URDF/meshes 都存在；把下面完整程序保存为 t1_joint_demo.py，再执行这组命令。输出目录中会有逐帧 PNG、GIF 和 simulation-verification.json，后者包含每帧时间、角度及校验信息。
sudo apt install -y libosmesa6 python3-venv
python3 -m venv "$HOME/primebot_sdk_lab/sim-venv" --system-site-packages
"$HOME/primebot_sdk_lab/sim-venv/bin/python" -m pip install mujoco==3.16.0 pillow
"$HOME/primebot_sdk_lab/sim-venv/bin/python" t1_joint_demo.py --model "$HOME/primebot_sdk_lab/models/T1_URDF/xml/T1-V2.xml" --output sim_results"""Run and record a fixed-base joint experiment from the official T1 MJCF."""
import argparse
import hashlib
import json
import os
from pathlib import Path
import xml.etree.ElementTree as ET
os.environ.setdefault('MUJOCO_GL', 'osmesa')
import mujoco
import numpy as np
from PIL import Image, ImageDraw, ImageFont
def main():
parser = argparse.ArgumentParser()
parser.add_argument('--model', type=Path, required=True)
parser.add_argument('--output', type=Path, default=Path('sim_results'))
args = parser.parse_args()
args.output.mkdir(parents=True, exist_ok=True)
tree = ET.parse(args.model)
root = tree.getroot()
base = root.find('worldbody/body')
base.remove(base.find('freejoint'))
base.set('pos', '0 0 0.66')
base.set('quat', '0.70710678 0 -0.70710678 0')
ET.SubElement(root, 'option', timestep='0.002', integrator='implicitfast')
visual = ET.SubElement(root, 'visual')
ET.SubElement(visual, 'global', offwidth='1280', offheight='720')
ET.SubElement(visual, 'headlight', ambient='0.45 0.45 0.45',
diffuse='0.65 0.65 0.65')
asset = root.find('asset')
ET.SubElement(asset, 'texture', name='sky', type='skybox', builtin='gradient',
rgb1='0.23 0.30 0.38', rgb2='0.06 0.09 0.14', width='256', height='1536')
ET.SubElement(asset, 'texture', name='grid', type='2d', builtin='checker',
rgb1='0.25 0.28 0.31', rgb2='0.18 0.21 0.24', width='128', height='128')
ET.SubElement(asset, 'material', name='ground', texture='grid', texrepeat='8 8', texuniform='true')
world = root.find('worldbody')
ET.SubElement(world, 'light', pos='-1 -2 3', dir='0 1 -1', diffuse='0.8 0.8 0.8')
ET.SubElement(world, 'geom', type='plane', size='3 3 .1', material='ground')
# Save beside the original XML, preserving its relative mesh directory.
fixture = args.model.with_name('t1_joint_fixture.xml')
tree.write(fixture)
model = mujoco.MjModel.from_xml_path(str(fixture))
data = mujoco.MjData(model)
model.dof_damping[:] = 0.25
joint_ids = model.actuator_trnid[:, 0]
qpos = model.jnt_qposadr[joint_ids]
qvel = model.jnt_dofadr[joint_ids]
names = [mujoco.mj_id2name(model, mujoco.mjtObj.mjOBJ_JOINT, int(j)) for j in joint_ids]
rest = np.zeros(model.nu)
for i, name in enumerate(names):
if 'HIP_PITCH' in name:
rest[i] = np.pi / 2
if name.startswith(('RL_', 'RR_')) and 'KNEE' in name:
rest[i] = -0.6
data.qpos[qpos] = rest
camera = mujoco.MjvCamera()
camera.lookat[:] = [0, 0, .57]
camera.distance, camera.azimuth, camera.elevation = 1.75, 130, -12
selected = names.index('FL_HIP_PITCH_Joint')
frames, samples = [], []
font = ImageFont.truetype('DejaVuSansMono.ttf', 18)
valid = True
with mujoco.Renderer(model, height=720, width=1280) as renderer:
for step in range(4500):
target = rest.copy()
phase = 2 * np.pi * .4 * max(0, data.time - 1)
if data.time >= 1:
target[selected] += .5 * np.sin(phase)
target[names.index('FL_HIP_ROLL_Joint')] = .18 * np.sin(phase)
target[names.index('FL_KNEE_Joint')] = .4 * (1 - np.cos(phase))
torque = 55 * (target - data.qpos[qpos]) - 4 * data.qvel[qvel]
data.ctrl[:] = np.clip(torque, model.actuator_ctrlrange[:, 0], model.actuator_ctrlrange[:, 1])
mujoco.mj_step(model, data)
valid = valid and bool(np.isfinite(data.qpos).all())
if step >= 500 and step % 50 == 0:
renderer.update_scene(data, camera=camera)
image = Image.fromarray(renderer.render())
draw = ImageDraw.Draw(image)
draw.rectangle((18, 18, 900, 88), fill=(18, 27, 36))
draw.text((30, 28), f'MuJoCo {mujoco.__version__} | T1 fixed-base joint simulation', fill='white', font=font)
draw.text((30, 56), f't = {data.time:.3f} s | FL_HIP_PITCH = {data.qpos[qpos[selected]]:.3f} rad', fill='#88d9e5', font=font)
index = len(frames)
path = args.output / f'frame-{index:03}.png'
image.save(path)
frames.append(image)
samples.append({'time':float(data.time),'position':float(data.qpos[qpos[selected]]),
'frame':path.name,'sha256':hashlib.sha256(path.read_bytes()).hexdigest()})
for label, azimuth in [('front', 90), ('side', 170)]:
camera.azimuth = azimuth
renderer.update_scene(data, camera=camera)
Image.fromarray(renderer.render()).save(args.output / f't1-{label}.png')
frames[0].save(args.output / 't1-joints.gif', save_all=True,
append_images=frames[1:], duration=100, loop=0, optimize=False)
positions = [s['position'] for s in samples]
lower, upper = model.jnt_range[joint_ids[selected]]
assert valid and min(positions) >= lower and max(positions) <= upper
assert max(positions) - min(positions) > .5
report = {'engine':mujoco.__version__, 'fixedBase':True, 'actuators':int(model.nu),
'simulationSeconds':float(data.time), 'frames':len(frames),
'finite':valid, 'observedJoint':names[selected],
'positionRange':[min(positions),max(positions)],
'jointLimit':[float(lower),float(upper)], 'withinJointLimit':True,
'samples':samples}
(args.output / 'simulation-verification.json').write_text(json.dumps(report, indent=2))
print(json.dumps({k:v for k,v in report.items() if k != 'samples'}, indent=2))
if __name__ == '__main__':
main()
侧面的这张图用的是同一次步进结束后的状态，只改变观察相机。它更方便检查前后肢、轮足与躯干之间的位置关系。源程序会在原 XML 旁生成 t1_joint_fixture.xml，因此模型目录需要可写；这份派生文件用于固定基座并补充地面、灯光和相机设置，原来的 T1-V2.xml 保留。加载模型和离屏渲染的接口可继续对照MuJoCo Python 文档。
模型之外，再搭一个能核对数值的状态页
关节实验中，“动到了哪里”要看实际状态；到了应用层，“收到了什么”也需要独立记录。这里选 Bms 电池消息，因为同一条消息同时包含有符号电流、毫单位数值、浮点温度和按位编码的状态。一个页面能显示出数字，并不意味着这几种数据都解释对了。
这段动图中的计数来自实际 ROS 2 回调。发布器每 0.2 秒构造一条 Bms，监视器换算并写日志，浏览器每 250 毫秒读取监视器快照。页面刷新与消息发布各有自己的节奏，刷新四次一秒不能被写成设备的采样率。
| 环节 | 输入与输出 | 读者可以核对什么 |
| 测试发布器 | 构造官方 Bms 类型，写入 LOCAL_TEST 标记 | 序号、原始单位和预设数据规律 |
| ROS 2 订阅回调 | 收到 Bms，换算后写入 JSONL | 序号是否连续，负号与状态位是否保留 |
| 官方 get_bms_state.py | 独立订阅同一条测试话题 | 对照官方示例输出，检查自己的换算 |
| 观察页 | 读取本地 HTTP 快照并绘图 | 展示值、接收计数、最后更新时间是否一致 |
电量从 80% 降到 21% 是发布器写好的输入序列，适合重复跑同一组核对题。LOCAL_TEST 标记贯穿消息与页面，避免把这种可重复的协议测试数据读成续航测量。它没有由 MuJoCo 的动作推算电耗，也不用于比较机器人电池性能。
| 项目 | 本次实际环境 |
| 开发系统 | Windows 上的 WSL 2 / Ubuntu 22.04.5 LTS |
| CPU 架构 | x86_64 |
| ROS 2 | Humble / ROS Base |
| Python | 3.10.12 |
| 启元 SDK | T1 SDK v1.0.0.0 |
| 编译范围 | 官方 aimdk_msgs 消息包 |
| 通信实现 | rmw_fastrtps_cpp |
| 数据来源 | ROS 2 本地测试发布器 |
SDK 使用官方下载页的 T1 v1.0.0.0，配套指南采用 Ubuntu 22.04 与 ROS 2 Humble；本次在 Windows 的 WSL 2 中运行这一 Linux 环境。源码入口是T1 SDK。下面先编译消息类型，再放入完整的发布器、监视器和页面文件，最后把三路输出一起核对。
先让 Python 认识 Bms，再讨论页面怎么画
ROS 2 尚未安装时，先按Humble 官方安装说明配置软件源。下面这组命令以软件源已就绪为前提；这里选择 ROS Base，Bms 收发不需要先装桌面仿真工具。
sudo apt update
sudo apt install -y ros-humble-ros-base python3-colcon-common-extensions build-essential cmake
source /opt/ros/humble/setup.bash
python3 -c "import rclpy; print('rclpy import OK')"SDK 解压后，SDK_DIR 指向同时包含 aimdk_msgs 与 examples 的那层目录。只编译 aimdk_msgs，是因为这次实际需要的是消息类型支持；视频示例和轨迹规划的额外依赖并不在这条实验路径上。源代码放在 ws/src，构建输出留在工作区里，自己写的演示文件放在 demo，互不混杂。
SDK_DIR="$HOME/Downloads/sdk_t1-v1.0.0.0"
LAB_DIR="$HOME/primebot_sdk_lab"
mkdir -p "$LAB_DIR/ws/src" "$LAB_DIR/demo" "$LAB_DIR/results"
cp -a "$SDK_DIR/aimdk_msgs" "$LAB_DIR/ws/src/"
cp "$SDK_DIR/examples/python/get_bms_state.py" "$LAB_DIR/demo/"
cd "$LAB_DIR/ws"
colcon build --packages-select aimdk_msgs
source install/setup.bash
python3 -c "from aimdk_msgs.msg import Bms; print('Bms import OK')"
ros2 interface show aimdk_msgs/msg/Bms这次构建汇总是 25.8 秒，一个包完成。这个时间只覆盖本次消息包构建，不能当成整套 SDK 的安装耗时。构建之后再执行 import Bms 和 ros2 interface show：前者检查 Python 环境能否加载类型，后者直接展开生成的接口。
Finished <<< aimdk_msgs [25.6s]
Summary: 1 package finished [25.8s]
Official aimdk_msgs.msg.Bms import OK原始 Bms.msg 是协议定义，编译后才有 Python 类、类型支持库和接口元数据。把一个 .msg 文件复制进 demo 目录不会自动完成这一步。新开终端后还要重新 source 工作区环境；“编译过一次”和“这个终端现在能用”要分别检查。
接收端的第一道题是单位。下面保留的是官方接口中的相关字段，展示代码继续使用原字段名。电流为 int32，注释明确充电为正、放电为负；温度本来就是摄氏度，不能看到电压除了一千，就把温度也一起除掉。
uint32 voltage # mV
int32 current # mA，充电为正、放电为负
int32 power # mW
float32 temperature # ℃
uint8 remaining_capacity_percentage # %
uint32 status # 状态位，bit 2 是放电标志| 字段 | 测试输入 | 展示与判断 |
| voltage / uint32 | 24000 mV | 24.000 V |
| current / int32 | -1200 mA | -1.200 A，保留放电方向 |
| power / int32 | -28800 mW | -28.800 W |
| temperature / float32 | 30.25 ℃ | 30.25 ℃，不做千倍换算 |
| status / uint32 | 1 << 2 | bool(status & (1 << 2)) 为 true |
status 还有其他含义不同的位，因此这里用按位与提取 bit 2，而不是判断 status == 4。后者只在恰好只有这一位置位时成立；如果同时出现另一个标志，它就会把仍然存在的放电标志漏掉。这类错误从页面外观很难发现，用确定输入逐项对照更省事。
发布器：先准备六十条有标准答案的消息
所有终端先执行同一组环境加载命令。ROS_DOMAIN_ID=73 是为这次实验选择的隔离编号，ROS_LOCALHOST_ONLY=1 将发现与通信限制在本机；它们是实验设置，不是启元 SDK 固定要求的连接参数。
source /opt/ros/humble/setup.bash
source "$HOME/primebot_sdk_lab/ws/install/setup.bash"
export ROS_DOMAIN_ID=73
export ROS_LOCALHOST_ONLY=1
cd "$HOME/primebot_sdk_lab/demo"话题放在 /primebot_lab/bms/state，便于和默认设备话题 /aima/hal/bms/state 区分。发布器启动后等两秒再开始发送，随后每 0.2 秒一条，共六十条。两秒只是这次演示的启动安排，节点发现是否完成仍取决于运行环境。
数据规律写得很简单：sequence 从 1 递增到 60；电流保持 -1200 mA；电压每十条在 24000～24900 mV 间循环；电量从 80% 逐条减一。这样首尾值都有答案，接收端即便画出一条很顺的曲线，也还得接受序号与原始字段的核对。
下面保存为 demo/bms_fixture.py。manufacturer 写为 LOCAL_TEST，status 只设置放电位；未赋值字段仍是消息默认值。本例的核对范围就是已明确设置的字段，不借这些默认值推断硬件版本或设备状态。
"""Publish explicit local test data using the official PrimeBot Bms type."""
import argparse
import time
import rclpy
from rclpy.node import Node
from rclpy.qos import qos_profile_sensor_data
from aimdk_msgs.msg import Bms
TOPIC = '/primebot_lab/bms/state'
class BmsFixture(Node):
def __init__(self, count, period):
super().__init__('primebot_bms_fixture')
self.publisher = self.create_publisher(Bms, TOPIC, qos_profile_sensor_data)
self.sequence = 0
self.count = count
self.ready_at = time.monotonic() + 2.0
self.done = False
self.timer = self.create_timer(period, self.tick)
def tick(self):
if time.monotonic() < self.ready_at:
return
if self.sequence >= self.count:
self.done = True
return
self.sequence += 1
msg = Bms()
msg.header.stamp = self.get_clock().now().to_msg()
msg.header.sequence = self.sequence
msg.header.frame_id = 'local_test_fixture'
msg.manufacturer = 'LOCAL_TEST'
msg.serial_number = 'LAB_ONLY'
msg.protocol_version = 'fixture-1'
msg.voltage = 24000 + ((self.sequence - 1) % 10) * 100
msg.current = -1200
msg.power = round(msg.voltage * msg.current / 1000)
msg.temperature = 30.25 + ((self.sequence - 1) % 10) * 0.25
msg.remaining_capacity_percentage = 80 - ((self.sequence - 1) % 60)
msg.remaining_capacity = msg.remaining_capacity_percentage * 50
msg.status = 1 << 2
self.publisher.publish(msg)
self.get_logger().info(
f'LOCAL_TEST seq={msg.header.sequence} '
f'voltage={msg.voltage}mV current={msg.current}mA '
f'soc={msg.remaining_capacity_percentage}%')
def main():
parser = argparse.ArgumentParser()
parser.add_argument('--count', type=int, default=60)
parser.add_argument('--period', type=float, default=0.2)
args, ros_args = parser.parse_known_args()
if args.count < 1 or args.period <= 0:
parser.error('count and period must be positive')
rclpy.init(args=ros_args)
node = BmsFixture(args.count, args.period)
try:
while rclpy.ok() and not node.done:
rclpy.spin_once(node, timeout_sec=0.1)
finally:
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()功率这行也值得算一遍：24000 mV × -1200 mA = -28800000 μW，除以 1000 后是 -28800 mW。接收端再除以 1000 才得到 -28.8 W。保留负数，不取绝对值，电流与功率的方向才不会在中间处理时丢失。
接收器：把数值、历史与更新时间放在同一处
发布与接收都使用 qos_profile_sensor_data。它采用 best effort 可靠性，适合这种持续更新的状态流，但不保证每条消息送达。先把正常路径跑通，后面再单独改变订阅可靠性做对照。
on_bms 回调只处理真正收到的消息：换算 V、A、W，提取放电位，保存 sequence 与来源，然后追加 JSONL。观察页不生成新的电池数据。这样曲线上的每一个点都能在接收日志里找到对应记录，页面渲染出了问题，也不会连带丢掉原始观察依据。
更新时间采用 time.monotonic()。每次回调记录一次，取快照时计算距离最后一次回调过去多久；达到 1.5 秒就标为过期。这衡量的是本机接收新鲜度，不是跨设备传输延迟。消息头时间戳可以保留用于追踪，但跨机时间差要先解决时钟同步，不能直接拿来当网络耗时。
完整文件保存为 demo/bms_monitor.py。ROS 回调在线程中运行，主线程提供只读 HTTP 服务。历史队列、累计计数和更新时间共用一把锁，snapshot 在锁内复制列表，避免网页读取时恰好撞上追加。历史窗口只保留最近六十条，JSONL 则持续记录已经收到的消息；这两种保留策略各有用途。
"""Receive DDS messages and expose their actual values to a local web page."""
import argparse
from collections import deque
from http.server import BaseHTTPRequestHandler, ThreadingHTTPServer
import json
from pathlib import Path
import threading
import time
import rclpy
from rclpy.node import Node
from rclpy.qos import qos_profile_sensor_data
from aimdk_msgs.msg import Bms
TOPIC = '/primebot_lab/bms/state'
class BmsMonitor(Node):
def __init__(self, output):
super().__init__('primebot_bms_monitor')
self.lock = threading.Lock()
self.history = deque(maxlen=60)
self.received = 0
self.last_at = None
self.output = Path(output).open('w', encoding='utf-8', buffering=1)
self.subscription = self.create_subscription(
Bms, TOPIC, self.on_bms, qos_profile_sensor_data)
def on_bms(self, msg):
row = {
'sequence': msg.header.sequence,
'source': msg.manufacturer,
'voltage_v': msg.voltage / 1000.0,
'current_a': msg.current / 1000.0,
'power_w': msg.power / 1000.0,
'temperature_c': msg.temperature,
'soc': msg.remaining_capacity_percentage,
'discharging': bool(msg.status & (1 << 2)),
}
with self.lock:
self.received += 1
self.last_at = time.monotonic()
self.history.append(row)
self.output.write(json.dumps(row) + '\n')
self.get_logger().info(json.dumps(row))
def snapshot(self):
with self.lock:
age = None if self.last_at is None else time.monotonic() - self.last_at
return {
'topic': TOPIC, 'received': self.received,
'age_s': age, 'fresh': age is not None and age < 1.5,
'history': list(self.history),
}
def main():
parser = argparse.ArgumentParser()
parser.add_argument('--output', default='received.jsonl')
parser.add_argument('--port', type=int, default=8765)
args, ros_args = parser.parse_known_args()
rclpy.init(args=ros_args)
node = BmsMonitor(args.output)
page = Path(__file__).with_name('index.html').read_bytes()
class Handler(BaseHTTPRequestHandler):
def do_GET(self):
if self.path == '/api/state':
payload = json.dumps(node.snapshot()).encode()
content_type = 'application/json'
elif self.path == '/':
payload = page
content_type = 'text/html; charset=utf-8'
else:
self.send_error(404)
return
self.send_response(200)
self.send_header('Content-Type', content_type)
self.send_header('Cache-Control', 'no-store')
self.send_header('Content-Length', str(len(payload)))
self.end_headers()
self.wfile.write(payload)
def log_message(self, *_):
pass
server = ThreadingHTTPServer(('127.0.0.1', args.port), Handler)
worker = threading.Thread(target=rclpy.spin, args=(node,), daemon=True)
worker.start()
node.get_logger().info(f'Local monitor: http://127.0.0.1:{args.port}')
try:
server.serve_forever()
except KeyboardInterrupt:
pass
finally:
server.server_close()
if rclpy.ok():
rclpy.shutdown()
worker.join(timeout=2)
node.output.close()
node.destroy_node()
if __name__ == '__main__':
main()HTTP 只绑定 127.0.0.1，根路径返回页面，/api/state 返回快照。这个接口属于本例的观察工具，官方 SDK 的通信仍是 ROS 2；浏览器并没有绕过订阅回调去直接读取机器人。多了一层 HTTP，是为了把数据解释与页面绘制分开，排障时可以独立查看快照内容。
页面只画已经收到的数据
把下面保存为与监视器同目录的 index.html。刚打开时显示“等待第一条消息”，收到后才填入数值；每 250 毫秒读取一次快照。曲线使用回调留下的历史点，计数使用 received，最近更新提示使用 age_s。它们来自同一份快照，避免出现“曲线还在动，数字却停在另一条消息上”的展示不一致。
<!doctype html>
<html lang="zh-CN">
<meta charset="utf-8">
<meta name="viewport" content="width=device-width,initial-scale=1">
<title>启元 T1 SDK · 本地消息实验</title>
<style>
*{box-sizing:border-box}body{margin:0;background:#f4f1e9;color:#202c2b;font:16px/1.6 system-ui,"Microsoft YaHei",sans-serif}main{max-width:1180px;margin:auto;padding:38px 40px}header{display:flex;justify-content:space-between;align-items:center}.eyebrow{font-size:13px;color:#55706d;letter-spacing:2px}h1{font-size:34px;line-height:1.3;margin:10px 0 12px}.note{color:#5d6b68;margin:0}.badge{background:#ffedbf;color:#684a00;padding:8px 16px;border-radius:30px;font-weight:650}.status{display:flex;justify-content:space-between;margin:25px 0 20px;border-top:1px solid #d3d7cc;padding-top:18px}#connection{color:#947232;font-weight:700}.cards{display:grid;grid-template-columns:repeat(4,1fr);gap:16px}.card,section{background:#fffdfa;border:1px solid #deded4;border-radius:14px;padding:22px}.card label{color:#687571;font-size:14px}.value{font-size:38px;line-height:1.5;font-weight:700;font-variant-numeric:tabular-nums}.unit{font-size:16px;color:#71817c;margin-left:6px}.lower{display:grid;grid-template-columns:1.5fr 1fr;gap:18px;margin-top:20px}h2{font-size:17px;margin:0 0 15px}canvas{width:100%;height:180px}pre{font:14px/1.8 Consolas,monospace;margin:0;color:#3f5951;white-space:pre-wrap}footer{font-size:13px;color:#647670;margin-top:18px}#age{font-variant-numeric:tabular-nums}.online{color:#167261!important}.stale{color:#b15336!important}
</style>
<main>
<header><div><div class="eyebrow">PRIMEBOT / ROS 2 HUMBLE</div><h1>让电池消息先在本机跑起来</h1><p class="note">官方 Bms 消息类型 · DDS 实际收发 · 本地测试数据</p></div><div class="badge">LOCAL_TEST</div></header>
<div class="status"><span id="connection">等待第一条消息</span><span>已接收 <b id="received">0</b> 条 · <span id="age">尚无数据</span></span></div>
<div class="cards">
<div class="card"><label>电压 / mV → V</label><div class="value"><span id="voltage">—</span><span class="unit">V</span></div></div>
<div class="card"><label>电流 / mA → A</label><div class="value"><span id="current">—</span><span class="unit">A</span></div></div>
<div class="card"><label>测试电量</label><div class="value"><span id="soc">—</span><span class="unit">%</span></div></div>
<div class="card"><label>温度</label><div class="value"><span id="temperature">—</span><span class="unit">℃</span></div></div>
</div>
<div class="lower"><section><h2>最近 60 条消息的电量变化</h2><canvas id="chart" width="620" height="180"></canvas></section><section><h2>刚刚收到的消息</h2><pre id="message">等待 ROS 2 订阅回调…</pre></section></div>
<footer>/primebot_lab/bms/state ｜ 停止发布后，1.5 秒转为“数据已过期” ｜ LOCAL_TEST 测试数据</footer>
</main>
<script>
const el=id=>document.getElementById(id);
function plot(rows){
const c=el('chart'),g=c.getContext('2d');g.clearRect(0,0,c.width,c.height);
g.font='12px system-ui';g.strokeStyle='#e3e7de';g.fillStyle='#80918a';
[20,50,80].forEach(v=>{const y=160-v*1.6;g.beginPath();g.moveTo(28,y);g.lineTo(610,y);g.stroke();g.fillText(v+'%',0,y+4)});
if(!rows.length)return;
g.strokeStyle='#247b67';g.lineWidth=3;g.beginPath();
rows.forEach((r,i)=>{const x=32+i*575/59,y=160-r.soc*1.6;i?g.lineTo(x,y):g.moveTo(x,y)});g.stroke();
}
async function update(){
try{
const s=await(await fetch('/api/state',{cache:'no-store'})).json();
el('received').textContent=s.received;el('age').textContent=s.age_s===null?'尚无数据':s.age_s.toFixed(1)+' 秒前更新';
el('connection').textContent=s.fresh?'● 正在接收本地测试消息':s.received?'● 数据已过期，等待新消息':'等待第一条消息';
el('connection').className=s.fresh?'online':s.received?'stale':'';
const r=s.history.at(-1);
if(r){el('voltage').textContent=r.voltage_v.toFixed(3);el('current').textContent=r.current_a.toFixed(3);el('soc').textContent=r.soc;el('temperature').textContent=r.temperature_c.toFixed(2);el('message').textContent='sequence: '+r.sequence+'\nsource: '+r.source+'\nvoltage: '+r.voltage_v.toFixed(3)+' V\ncurrent: '+r.current_a.toFixed(3)+' A\ndischarging: '+r.discharging;}
plot(s.history);
}catch(e){el('connection').textContent='监视器连接中断';el('connection').className='stale'}
}
update();setInterval(update,250);
</script>
</html>fresh、received 和最近一个数值要分开使用：收到过数据但已经过期，与从未收到第一条消息是不同状态；HTTP 请求失败又是另一种情况。代码里分别给出等待、过期和监视器连接中断提示，排查时才知道先去看发布器、ROS 回调，还是本地 HTTP 进程。
三路结果对一遍，再主动把消息停掉
三个终端都先执行前面的环境命令。终端 A 运行监视器，在浏览器打开 http://127.0.0.1:8765；还没有发布消息时，页面应保持等待。如果端口被占用，可以用 --port 换一个端口，浏览器地址也一起调整。
python3 bms_monitor.py --output ../results/received.jsonl终端 B 启动官方 get_bms_state.py。这里不修改脚本，用 ROS 2 remap 参数把原订阅话题映射到实验话题。多保留这一条独立接收路径，是为了防止自己的发布器与自己的页面碰巧用同一种错误换算，互相“对上了”却一起读错。
python3 get_bms_state.py --ros-args -r /aima/hal/bms/state:=/primebot_lab/bms/state官方脚本启动时可能仍打印默认话题名，因为那句话写在日志字符串里；实际订阅由 remap 决定，不能凭提示文字判断映射是否成功。终端 C 再启动发布器，便能同时观察终端输出、网页计数与曲线。
python3 bms_fixture.py --count 60 --period 0.2这一轮独立进程收发中，监视器收到六十条消息，sequence 从 1 到 60 连续。官方示例首条输出和监视器首条记录一致，最后一条的电压、电量、温度也与发布器公式对应。把核对结果摊开看，比只留一张“成功运行”的截图更容易发现单位和顺序上的错位。
| 检查项 | 本次结果 |
| 本地消息接收数 | 60 条 |
| 序号 | 1～60 连续 |
| 电压换算 | 24000 mV → 24.000 V |
| 电流符号与换算 | -1200 mA → -1.200 A |
| 放电状态位 | bit 2 置位，读取结果为 true |
| 官方示例交叉核对 | 收到本地测试数据 |
| 停止发布后的页面 | 超过 1.5 秒显示数据已过期 |
日志中的“本轮六十条都收到”只描述这次本机实验。best effort 的协议语义没有改变；更换网络、提高发送频率或增加处理负载后，仍要观察丢包与积压。需要检查序号时，应读 JSONL，而不是数网页上看见了多少次刷新。
数值留下来，为什么还要标成过期
发布器发完六十条后退出，监视器继续运行。超过 1.5 秒，页面保留最后的 24.900 V、21% 电量等数值，但把状态改为“数据已过期”。这是有意保留的一步：如果直接把所有数值清零，读者无法区分“设备真的报了零”和“暂时没有新消息”。
1.5 秒是为本例 0.2 秒发送周期选的展示阈值，相当于留出数个消息周期的余量；换成别的状态话题，应该按其更新规律重新设定。这个提示描述数据新鲜度，本身不作设备故障诊断。重复实验时可只重启发布器，监视器计数会累加；要重新从零核对，就连监视器也一起重启。
同一个发布器：一端六十条，另一端零条
正常路径跑通之后，我保留话题名、消息类型和发布器不变，只把另一个订阅端改成 reliable。这个反例很容易复现，却能纠正一个常见判断：“节点能发现、类型也对，收不到就一定是网络断了。”本机通信同样会因为 QoS 请求与提供不匹配而无法传递消息。
| 发布端可靠性 | 订阅端可靠性 | 仅就可靠性这一项是否兼容 |
| Best effort | Best effort | 兼容 |
| Best effort | Reliable | 不兼容，本次故意设置的组合 |
| Reliable | Best effort | 兼容 |
| Reliable | Reliable | 兼容 |
注意这张表有方向：发布端提供 best effort，满足不了订阅端要求的 reliable；反过来却可以。其他策略也要兼容，不能只看这一列。对应关系来自ROS 2 Humble 官方 QoS 文档，这里再用同一个发布器验证其中的不兼容情形。
将下面完整文件保存为 demo/qos_probe.py。先开探针，再在另一终端启动发布器；正常监视器可以同时运行。探针观察十八秒后打印收到的数量，从而把不同接收配置的结果放在一起比较。
"""A reliable subscriber intentionally mismatched with the best-effort fixture."""
import json
import time
import rclpy
from rclpy.node import Node
from rclpy.qos import QoSProfile, ReliabilityPolicy
from aimdk_msgs.msg import Bms
rclpy.init()
node = Node('primebot_reliable_probe')
received = []
subscription = node.create_subscription(
Bms, '/primebot_lab/bms/state',
lambda msg: received.append(msg.header.sequence),
QoSProfile(depth=5, reliability=ReliabilityPolicy.RELIABLE))
deadline = time.monotonic() + 18
try:
while time.monotonic() < deadline:
rclpy.spin_once(node, timeout_sec=0.1)
print(json.dumps({'reliable_probe_received': len(received)}), flush=True)
finally:
node.destroy_node()
rclpy.shutdown()python3 qos_probe.py
# 在另一个终端运行：
python3 bms_fixture.py --count 60本次探针最终收到 0 条，警告明确写出不兼容策略为 RELIABILITY；正常 sensor_data 订阅端则收到测试消息。这里有用的证据是“只改了一项配置后的差异”：发布器能够工作，topic 和 Bms 也能用，问题就可以收窄到订阅声明上。
offering incompatible QoS. No messages will be received from it.
Last incompatible policy: RELIABILITY
{"reliable_probe_received": 0}几种相似的空白，处理位置并不相同
| 现象 | 先核对什么 | 本例的处理位置 |
| import Bms 失败 | 消息包是否构建、当前 shell 是否 source 正确工作区 | 构建与终端环境 |
| 所有接收端都无消息 | 发布器是否启动、话题和 ROS_DOMAIN_ID 是否一致 | 发布端与发现设置 |
| 正常监视器有消息，reliable 探针为零 | QoS 警告中的 RELIABILITY | 订阅可靠性设置 |
| 页面保留最后值并显示过期 | 发布器是否已结束、接收时间是否停止更新 | 数据新鲜度判断 |
| 浏览器显示监视器连接中断 | 本地 HTTP 进程与端口 | 展示服务 |
环境准备时还遇到过一个更早的错误：脚本开启 set -u，source ROS 环境时出现 AMENT_TRACE_SETUP_FILES 未定义。它发生在 shell 加载阶段，与 Bms 单位或 DDS 发现无关；先关闭 nounset，再加载环境即可，不需要因此重新编译或安装 SDK。
set +u
source /opt/ros/humble/setup.bash
source "$HOME/primebot_sdk_lab/ws/install/setup.bash"
python3 -c "from aimdk_msgs.msg import Bms; print('Bms import OK')"跑完以后，留下的比一张成功截图多一点
这次最有收获的地方，是把两个容易被“看起来正常”掩盖的问题拆开了。关节在动，还要分清目标角和实际角；页面有数，还要确认单位、序号和最后更新时间。对照图里能看到跟踪滞后，停止发布后的页面能看到过期，而 QoS 探针又能在同一条话题上得到零接收——每一个结果都有对应的输入和代码位置。
T1 官方模型提供了连杆、关节及执行器的起点，SDK 消息定义提供了状态解释的依据。本文把固定基座响应、Bms 接收和网页观察串成一个入门实验，但各层仍保持独立：换一种关节目标，不必重做消息页面；换一种状态来源，也能保留日志、换算和新鲜度判断。后面要扩展触摸、关节反馈或其他状态时，就有了一套可逐层检查的起点。
复现时建议按本文顺序核对：先看模型能否加载、实际角是否变化，再确认 Bms 能否导入，最后对照官方示例与自己的接收日志。不要跳过最后的停发和 QoS 对照；它们往往比一段持续“正常”的演示更能暴露接收与展示中的问题。
相关资料：启元开发资料包、启元开源组织、T1 SDK、MuJoCo Python 文档。机器人接口以本文使用版本的源码定义及配套指南为准。
DAMO开发者矩阵，由阿里巴巴达摩院和中国互联网协会联合发起，致力于探讨最前沿的技术趋势与应用成果，搭建高质量的交流与分享平台，推动技术创新与产业应用链接，围绕“人工智能与新型计算”构建开放共享的开发者生态。
更多推荐
1
0- 0
已为社区贡献8条内容
所有评论(0)