-
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: first_frame_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先不要把 get_entry_frame() 和 sample_running_frame() 当成两个必须背下来的回调。它们不是随意起的名字，而是为了解决两类具体的模型过渡问题。先从机器人为什么需要 Transition 开始。
普通行走模型正在输出一组关节目标和增益；切入深度行走或其他模型时，新模型给出的 qpos、kp 和 kd 可能与上一个控制周期差异很大。如果直接替换电机命令，目标关节角或控制力度会在一帧内跳变，机器人就可能猛地动一下。
框架内置了两种适合此类模型切换的思路。
first_frame_switch 先短暂保持切换前的电机输出，然后把目标状态的第一帧关节角作为固定目标。在当前预设中，kp 从 0 逐步增加到目标状态的 kp，kd 使用目标状态的值。随着位置控制逐步加强，机器人被平稳地带向目标状态的进入姿态。
这种过渡首先必须知道“目标状态的第一帧是什么”。因此，first_frame_switch 内部的 entry_gain_ramp 要求目标状态实现 EntryFrameProvider，而该协议规定的取帧方法就是 get_entry_frame(ctx)。
first_frame_switch
→ 需要目标状态的进入帧
→ 目标状态实现 EntryFrameProvider
→ 通过 get_entry_frame() 返回该 MotorFrame
dual_running_blend 不只是把机器人拉向一个固定姿态。它在过渡期间分别取得来源状态和目标状态的运行帧，再按过渡进度混合两边的 qpos、kp 和 kd。这更适合普通行走与深度行走这类“两边都在持续运行”的模型。
要完成混合，Transition 必须能在不直接发布电机命令的前提下，单独取到两边的 MotorFrame。因此，当 sample_from: true 和 sample_to: true 时，来源状态和目标状态都要实现 RunningFrameProvider，并通过 sample_running_frame(ctx, dt, advance=...) 提供运行帧。running_blend 还会取得目标状态的 entry frame 作为初始帧和回退帧，所以目标状态也要实现 EntryFrameProvider。
dual_running_blend
→ 需要两端模型的运行帧
→ 两端按采样配置实现 RunningFrameProvider
→ 通过 sample_running_frame() 分别返回 MotorFrame
→ Transition 负责混合并发布，状态不在采样方法中发布
状态正常运行时，状态机调用当前状态的 on_update()，由它计算并下发电机目标。Transition 激活后，切换过程暂时接管电机输出；此时不能直接调用两个状态的 on_update()：
-
on_update()会立即下发电机命令，Transition 无法先取得两端结果再混合。 - 模型的 history、timestep 或 action 可能被意外推进多次。
- Transition 不知道某个状态的目标位置应该从哪个模型字段读取。
因此，Provider 是在原有 RobotControlState 生命周期之上增加的“可选过渡能力”。RobotControlState 仍然是主要抽象；不同 Transition 只按它需要的数据，要求状态额外实现相应接口，而不是强迫所有状态实现一套万能接口。
两种帧能力的完整对应关系如下：
| Transition 需要什么 | 状态声明的能力 | 能力规定的方法 |
|---|---|---|
目标状态刚进入时的稳定 qpos/kp/kd
|
EntryFrameProvider |
get_entry_frame(ctx) |
| 状态处于运行阶段时的一帧输出 | RunningFrameProvider |
sample_running_frame(ctx, dt, advance=...) |
所以类声明中的两个 Provider 与下面两个方法是一一对应的：
class NormalDepthState(
RobotControlState,
EntryFrameProvider,
RunningFrameProvider,
):
...具体到内置 Transition：
-
instant和hold不向状态索取这两种能力。 -
entry_gain_ramp要求目标状态实现EntryFrameProvider，因为它需要固定目标姿态，并把 kp/kd 从起始值渐变到目标值。 -
running_blend总是要求目标状态实现EntryFrameProvider，把 entry frame 作为目标侧基准和回退帧。 -
running_blend在sample_from: true时要求来源状态实现RunningFrameProvider。 -
running_blend在sample_to: true时要求目标状态实现RunningFrameProvider。
本实例的 normal → normal_depth 和 zero_torque → normal_depth 都使用 first_frame_switch，因此深度状态作为目标侧必须实现 EntryFrameProvider。normal_depth → normal 使用 dual_running_blend：深度状态作为来源侧提供 running frame，普通行走作为目标侧提供 entry frame 和 running frame。这样，类声明中的两个 Provider 都由实际路由需求推导出来，而不是为了将来预留的样板代码。
get_entry_frame() 可以逐词理解：
-
get：Transition 主动读取结果，函数本身不发布电机命令。 -
entry：返回的是状态进入边界上的稳定目标，不是模型不断变化的普通运行帧。 -
frame：必须同时返回qpos、kp和kd组成的MotorFrame，不只是关节位置。
sample_running_frame() 的命名强调另一组语义：
-
sample：Transition 对状态进行采样，取得结果后还要与另一侧混合；状态不能在这里直接调用ctx.set_motor_target()。 -
running：采样的是状态正常运行阶段的输出，而不是固定的进入目标。 -
frame：同样返回完整MotorFrame。 -
advance：明确告诉状态这次采样是否允许推进模型内部状态。
这也是它没有叫 update() 或 get_frame() 的原因。update() 暗示推进并下发输出，get_frame() 又无法区分稳定进入目标和动态运行输出。
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,
)这里使用 policy.target_dof_pos，是因为 on_prepare() 已经先重置了策略，它代表深度模型可稳定进入的初始关节目标。kp/kd 也来自同一个策略，三者一起组成 Transition 所需的 MotorFrame。
方法只返回数据，不调用 _apply_frame() 或 ctx.set_motor_target()。真正如何插值和何时发布由 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)当 dual_running_blend 同时处理普通行走模型和深度行走模型时，它先分别取得两边的 MotorFrame，再按照过渡进度混合 qpos、kp 和 kd。状态不直接读取摇杆，而是通过 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)普通运行仍由 on_update() 驱动。它用 advance=True 取得一帧，再由状态发布：
frame = self.sample_running_frame(ctx, dt, advance=True)
if frame is not None:
self._apply_frame(ctx, frame)因此职责分得很清楚：sample_running_frame() 只计算，on_update() 在正常运行阶段发布，Transition 在切换阶段采样并发布混合结果。
Provider 是按需能力，不是每个状态都必须继承。选择取决于清单中真正使用的 Transition：
| 目标 | 最小继承与方法 |
|---|---|
只用 instant 或 hold 进入和退出 |
只继承 RobotControlState，实现 on_update()
|
作为 entry_gain_ramp 的目标 |
增加 EntryFrameProvider 和 get_entry_frame()
|
作为 running_blend 的来源，且 sample_from: true
|
增加 RunningFrameProvider 和 sample_running_frame()
|
作为 running_blend 的目标 |
至少需要 EntryFrameProvider；sample_to: true 时再增加 RunningFrameProvider
|
| 希望任一方向都能在两个动态模型之间混合 | 同时实现两个 Provider，也就是当前完整版本 |
完全不适配动态模型混合时，状态可以缩成：
class SimpleDepthState(RobotControlState):
def on_update(self, ctx, dt):
qpos = self._infer(ctx)
ctx.set_motor_target(qpos, self.policy.kps, self.policy.kds)对应路由只使用不要求能力的 Transition：
- from: com.bxi.basic_actions/normal
event: activate
to: normal_depth
transition: soft_switch # hold，不读取 Provider
- from: normal_depth
event: com.bxi.basic_actions/normal
to: com.bxi.basic_actions/normal
transition: instant如果仍想使用 running_blend，但不希望来源侧模型在切换期间被采样，可以设置 sample_from: false。此时深度状态作为来源侧不需要 RunningFrameProvider，Transition 会使用切换开始时保存的最后电机帧：
transition:
profile: dual_running_blend
sample_from: false这种简写代码更少，但切换期间来源模型不再随机器人观测更新。对两个在线行走模型之间的切换，完整 Provider 版本的语义更明确，也为以后启用 advance_from/advance_to 留出了空间。
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、关节顺序、增益和安全边界。