-
Notifications
You must be signed in to change notification settings - Fork 12
Hands On Depth Policy State
本文完整记录一个多模态控制状态从需求拆解到可运行 Mod 的开发过程。最终状态同时读取机器人本体观测和外部节点发布的深度图，运行 ONNX 策略，并在深度数据中断时主动退回普通行走。
本文重点： Mod 不负责连接或驱动某一种相机。任何 ROS 2 节点只要满足本文定义的
sensor_msgs/Image契约，都可以成为深度数据源。
| 能力 | 最终实现 |
|---|---|
| 核心状态 | RobotControlState + EntryFrameProvider + RunningFrameProvider |
| 外部输入 | ROS 2 sensor_msgs/msg/Image 订阅 |
| 推理输入 | 机器人本体观测 + 深度图历史 |
| 模型管理 | 两个惰性 ResourceHandle，按所选模式只加载一个 |
| 模式 |
origin_camera（默认）和 depth_walk
|
| 安全措施 | 姿态保护、输入校验、深度超时回退 |
| 工程措施 | 回调线程同步、输入节流、热重载资源释放 |
最终目录如下：
mods/com.bxi.normal_depth/
mod.yaml
plugin.py
state.py
amp_depth.py
assets/
dagger2.onnx
normal_depth.onnx
这次开发会依次经过四层，每一层都解决一个独立问题：
外部深度节点
↓ sensor_msgs/Image
NormalDepthState：订阅、校验、超时和生命周期
↓ float32 深度矩阵 + 机器人状态
amp_depth.py：观测构造、深度预处理和 ONNX 推理
↓ 29 维关节目标
RobotControlState：MotorFrame、Transition 和安全退出
最初的需求只是“让行走策略看到深度图”。第一反应可能是在状态里直接打开 RealSense，但这样会把设备 SDK、相机型号、滤波算法和机器人控制周期绑在一起。状态热重载时还必须处理设备重复打开，仿真也难以替换输入。
因此我们先确定边界：
- 外部节点负责采集、仿真或回放深度图。
- 外部节点负责把图像裁剪、缩放到策略所需视场。
- Mod 只订阅标准 ROS 消息，不知道发布端使用什么相机或语言。
- Mod 负责消息校验、单位转换、策略节流和失联回退。
这意味着发布端可以是 RealSense 节点、Gazebo/MuJoCo 桥接节点、录包回放节点，甚至测试程序。只要话题契约一致，状态代码不需要修改。
在写订阅代码前，先把两个模型真正需要的输入列出来：
mode |
默认话题 | 发布图像 width × height
|
状态顺时针旋转后 | 策略深度周期 |
|---|---|---|---|---|
origin_camera |
/camera/depth/image_36x48 |
36 × 48 |
(36, 48) |
0.05 s |
depth_walk |
/camera/depth/image_64x36 |
64 × 36 |
(64, 36) |
0.02 s |
消息还必须满足以下约定：
- 类型为
sensor_msgs/msg/Image。 - 编码可以是
16UC1、mono16或32FC1。 -
16UC1/mono16默认单位是毫米，乘depth_uint16_scale: 0.001转成米。 -
32FC1直接按米读取。 -
step可以包含行尾 padding；状态按step解码，不假设像素紧密排列。 - 发布应使用传感器数据 QoS。默认 Mod 同样使用
qos_profile_sensor_data的可靠性和持久性，队列深度为 1。 - 输入停止超过
depth_timeout_sec后，状态会请求返回普通行走。
话题名只是默认值。客户节点发布到其他名字时，只改 mod.yaml：
states:
normal_depth:
params:
mode: origin_camera
topic: /customer/perception/policy_depth
depth_uint16_scale: 0.001
depth_timeout_sec: 1.0这里不需要给发布节点增加任何 BXI 依赖。这就是标准 ROS 订阅作为 Mod 扩展面的价值。
先不接模型，只把状态放进独立命名空间。Mod ID 选择 com.bxi.normal_depth，状态本地名为 normal_depth，完整状态名因此是：
com.bxi.normal_depth/normal_depth
这个动作需要从基础 normal 进入，并在故障时返回 normal 或 zero_torque，所以清单显式依赖基础动作包：
requires:
- id: com.bxi.basic_actions
version: ">=1,<2"第一版路由只表达状态图，不掺杂推理代码：
events:
activate: {slot: btn_10, value: 8}
routes:
- from: com.bxi.basic_actions/normal
event: activate
to: normal_depth
transition: soft_switch
- from: normal_depth
event: com.bxi.basic_actions/normal
to: com.bxi.basic_actions/normal
transition: {profile: dual_running_blend, duration: 1.0}
- from: normal_depth
event: com.bxi.basic_actions/zero_torque
to: com.bxi.basic_actions/zero_torque跨 Mod 的状态、事件和速度 profile 都写完整名称。最终状态使用基础动作包的速度约束：
speed_profile: com.bxi.basic_actions/normal当前示例输入配置把 btn_10=8 绑定到“右扳机 + Y”和键盘 0。这是输入层到事件槽位的映射；换用其他输入 Driver 时，只要产生相同槽位和值，Mod 路由不需要改变。
这个状态需要自己管理 ROS 资源，因此我们直接使用核心基类 RobotControlState。PoseState 和普通 PolicyState 很适合单输入模型，但这里还要处理异步传感器回调、消息超时和策略深度频率，完整生命周期更清晰。
订阅必须放在 on_bind()，而不是构造函数：
def on_bind(self, ctx):
qos = QoSProfile(
depth=1,
durability=qos_profile_sensor_data.durability,
reliability=qos_profile_sensor_data.reliability,
)
self._depth_subscription = ctx.create_subscription(
Image,
self.depth_image_topic,
self.depth_image_callback,
qos,
)原因有两个：
-
__init__()阶段只有状态参数和 Resource handle，还没有 ROS node 上下文。 - 框架会在节点启动和热重载后统一调用
on_bind()。
创建了订阅，就必须实现对称清理：
def on_unbind(self, ctx):
subscription = self._depth_subscription
self._depth_subscription = None
if subscription is not None:
ctx.destroy_subscription(subscription)如果漏掉这一步，每次热重载都会留下旧回调。表面症状通常是同一帧被处理多次，严重时旧状态对象和模型也无法释放。
简单的 reshape(height, width) 不够可靠，因为 ROS Image 允许行跨度 step 大于有效像素宽度，还要考虑大小端和不同编码。
我们的解码过程是：
- 根据 encoding 选择
uint16或float32。 - 根据
is_bigendian建立正确字节序的 dtype。 - 用
step / itemsize计算每行实际元素数。 - 验证消息数据长度，拒绝不完整帧。
- 去掉行尾 padding。
- 把结果统一转换成以米为单位的连续
float32数组。
核心代码如下：
itemsize = dtype.itemsize
row_values = int(msg.step) // itemsize
expected_values = row_values * int(msg.height)
data = np.frombuffer(msg.data, dtype=dtype, count=expected_values)
depth = data.reshape(int(msg.height), row_values)[:, : int(msg.width)]
depth_meters = (depth.astype(np.float32) * scale).copy()随后按照相机在机器人上的安装方向顺时针旋转：
depth_rotated = np.ascontiguousarray(
np.rot90(depth_meters, k=-1).astype(np.float32)
)每种模式都有固定的旋转后尺寸。不匹配的帧只告警一次并丢弃，绝不把错误形状送进 ONNX。
策略不是通用的单输入 ONNX。amp_depth.py 同时承担以下工作：
- 把 ELF3 的 29 关节顺序转换为策略训练顺序。
- 构造角速度、重力方向、速度命令、关节位置、关节速度和上一次 action。
- 维护本体观测历史。
- 对深度图执行旋转、裁剪、高斯滤波、距离裁剪和归一化。
- 维护单帧或多帧深度历史。
- 检查 ONNX 是单输入拼接模型还是本体/深度双输入模型。
- 把 action 缩放并映射回机器人关节顺序。
这段逻辑只服务深度行走，因此放在：
mods/com.bxi.normal_depth/amp_depth.py
而不是放回框架公共 inference/。这样删除或交付这个 Mod 时，Python 实现和模型资源始终一起移动。
两个模型共享基础推理类，但采用不同 profile：
class HumanoidGaitOriginCameraPolicyIsaaclab(
HumanoidGaitDepthPolicyIsaaclab
):
def __init__(self, model_onnx_path, cmd_is_joystick_ratio=False):
super().__init__(
model_onnx_path,
cmd_is_joystick_ratio=cmd_is_joystick_ratio,
depth_profile="origin_camera",
)模型文件属于 Mod：
assets/dagger2.onnx
assets/normal_depth.onnx
我们为两套策略声明不同的全局 Resource key：
LEGACY_POLICY = ResourceKey[HumanoidGaitDepthPolicyIsaaclab](
"com.bxi.normal_depth/legacy_policy"
)
ORIGIN_POLICY = ResourceKey[HumanoidGaitOriginCameraPolicyIsaaclab](
"com.bxi.normal_depth/origin_policy"
)加载函数只允许通过 context.asset() 解析当前 Mod 的 assets/：
def _load_origin_policy(context):
return HumanoidGaitOriginCameraPolicyIsaaclab(
str(context.asset("assets/dagger2.onnx"))
)create_mod() 注册 Resource，然后把 handle 注入状态工厂。这里得到的是 ResourceHandle，不是已经创建的 ONNX Session：
context.register_resource(ORIGIN_POLICY, _load_origin_policy)
origin_policy = context.resource(ORIGIN_POLICY)最终只有清单所选模式的 handle 被状态访问。另一个模型仍在包中，但不会占用推理内存。
原型阶段很容易写出 use_origin_camera = True。这能验证算法，却要求切模式时改 Python。Mod 版本把它升级为清单参数：
params:
mode: origin_camera
topic: /camera/depth/image_36x48
depth_uint16_scale: 0.001
depth_timeout_sec: 1.0插件工厂读取 mode，选择对应 handle 和默认话题：
mode = state.string_param("mode", "origin_camera")
if mode == "origin_camera":
policy = origin_policy
default_topic = "/camera/depth/image_36x48"
elif mode == "depth_walk":
policy = legacy_policy
default_topic = "/camera/depth/image_64x36"
else:
raise ValueError("mode must be origin_camera or depth_walk")StateBuildContext 会拒绝错误类型和未消费字段。把 depth_timeout_secs 拼错不会被静默忽略，而是在状态机构建时立即失败。
切换到旧 8 帧模型时，同时修改 mode 和话题：
params:
mode: depth_walk
topic: /camera/depth/image_64x36
depth_uint16_scale: 0.001
depth_timeout_sec: 1.0进入动态策略状态需要三个不同阶段。
def on_prepare(self, ctx, from_state):
self.policy.reset()
self._depth_enter_time = time.monotonic()
self._policy_depth_rotated = None
self._policy_depth_frame_id = None
self._last_policy_depth_time = None模型在这里首次通过 handle 解析。插件加载本身不会创建 ONNX Session。
def get_entry_frame(self, ctx):
return self._motor_frame(
self.policy.target_dof_pos,
self.policy.kps,
self.policy.kds,
)实现 EntryFrameProvider 后，进入状态的 Transition 可以读取目标姿态和增益。
qpos = self.policy.inference_step(
ctx.current_q,
ctx.current_dq,
ctx.current_quat_wxyz,
ctx.current_omega,
self.get_cmd_vel(ctx),
depth_image,
depth_frame_id=depth_frame_id,
)
return self._motor_frame(qpos, self.policy.kps, self.policy.kds)实现 RunningFrameProvider 后，退出时的 dual_running_blend 可以继续采样该动态状态。状态不直接读取摇杆，而是通过 get_cmd_vel() 使用清单指定的速度 profile。
Transition 以 advance=False 采样时不能推进模型 history。为此状态保存最近一次正常推理得到的 MotorFrame；只读采样返回缓存，没有缓存时返回 entry frame。只有 advance=True 才真正调用策略：
if not advance:
return self._last_running_frame or self.get_entry_frame(ctx)ROS 回调和状态更新可能运行在不同线程。我们不让回调直接修改推理器，只让它原子地替换“最新有效图像”：
with self._depth_lock:
self._depth_rotated = depth_rotated
self._latest_depth_frame_id += 1
self._last_depth_time = now控制线程读取一致的图像引用和 frame id。随后按策略自己的 depth_update_period 决定是否采纳新帧：
-
origin_camera每 0.05 秒最多更新一次策略深度输入。 -
depth_walk每 0.02 秒最多更新一次。 - 两次更新之间可以继续运行本体控制周期，但复用上一次策略深度帧。
- 相同
depth_frame_id不会重复推进推理类内部的深度历史。
这避免了“相机发布越快，深度历史推进越快”的隐式耦合。
深度行走不能在感知输入失效后无限继续。on_update() 的检查顺序是：
- 机器人姿态不安全：立即请求
zero_torque。 - 深度数据超过超时：请求返回普通
normal。 - 有有效深度：运行策略并应用 MotorFrame。
- 还没收到首帧但尚未超时：保持过渡前的最后控制输出，不发送伪造深度推理结果。
核心逻辑：
if ctx.is_orientation_unsafe(ctx.current_quat_xyzw):
ctx.request_state(ZERO_TORQUE_STATE, trigger="safety")
return
if self._is_depth_timed_out():
ctx.request_state(NORMAL_STATE, trigger="no_depth")
return
frame = self.sample_running_frame(ctx, dt, advance=True)
if frame is not None:
self._apply_frame(ctx, frame)超时回到 normal，姿态异常进入 zero_torque，两者不能互换。前者表示外部感知服务异常但机器人仍可控；后者表示机器人本体已经进入危险姿态。
构建并加载工作区：
colcon build --merge-install --packages-select bxi_example_py_elf3
source install/setup.bash启动客户自己的深度发布节点后，先检查契约，不要直接进入真机动作：
ros2 topic info /camera/depth/image_36x48 --verbose
ros2 topic hz /camera/depth/image_36x48
ros2 topic echo /camera/depth/image_36x48 --once --field width
ros2 topic echo /camera/depth/image_36x48 --once --field height
ros2 topic echo /camera/depth/image_36x48 --once --field encoding
ros2 topic echo /camera/depth/image_36x48 --once --field step默认 origin-camera 模式应确认：
width: 36
height: 48
encoding: 16UC1 或 32FC1
然后检查控制节点日志：
depth state mode=origin_camera, topic=/camera/depth/image_36x48, post-rotation shape=(36, 48)
最后按以下顺序验证：
- 仿真或吊架环境进入深度行走，速度命令保持为零。
- 确认模型输出为 29 维且没有 NaN/Inf。
- 给小幅前进和转向命令，核对方向与训练定义一致。
- 主动停止外部深度发布节点，确认 1 秒后返回普通行走。
- 重新启动发布节点并再次进入，确认 history 已在
on_prepare()重置。 - 开启热重载，修改非结构参数，确认旧订阅被销毁且话题只有一个订阅者实例。
| 现象 | 原因 | 处理 |
|---|---|---|
| 一直提示 waiting for depth image | 话题名不一致或 QoS 不兼容 | 检查 topic info --verbose 和 params.topic
|
| unexpected post-rotation depth shape | 发布端输出宽高与 mode 不匹配 | origin 发布 36×48；legacy 发布 64×36
|
| unsupported depth image encoding | 发布端使用了 RGB、MONO8 或自定义 encoding | 转成 16UC1/mono16/32FC1
|
| 深度值全部贴近裁剪上限 |
16UC1 单位不是毫米或 scale 错误 |
修正 depth_uint16_scale
|
| 模型加载时才报资产错误 | Resource 是惰性的 | 检查所选 mode 对应的 ONNX 文件 |
| 修改后回调次数翻倍 | 自定义状态漏掉 on_unbind()
|
销毁订阅、timer 和 client |
| 外部节点停止后仍向前走 | 超时检查未放在 on_update() 前部 |
保留 depth_timeout_sec 回退逻辑 |
| 切回 normal 时突跳 | 退出没有动态采样能力 | 实现 RunningFrameProvider 并使用 running blend |
这个 Mod 已经越过了“单文件模型动作”的范围，但没有修改框架核心：
-
mod.yaml负责依赖、参数、界面和状态图。 -
plugin.py负责有类型的 Resource 和显式状态工厂。 -
amp_depth.py负责动作领域专属的观测与推理。 -
state.py负责 ROS 生命周期、实时数据协调和机器人安全。 - 外部节点只遵守 ROS 消息契约，不需要知道 Mod 的实现。
这正是完整 RobotControlState 的使用场景：便捷状态类帮助我们快速完成简单状态，而核心 API 允许一个 Mod 接入异步传感器、多输入模型、资源生命周期和自定义安全策略，同时仍然复用现有状态机、Transition、输入映射和热重载机制。
下一步可以继续阅读 把模型从仿真带到真机，系统检查 observation、关节顺序、增益和安全边界。