Hands On Depth Policy State

深度感知行走 Mod 开发实录

本文按当前实现说明 com.bxi.normal_depth 怎样消费仿真或真机深度相机。相机采集与 机器人控制已经解耦：bxi_depth_camera 管理真机，MuJoCo 发布仿真图像，深度行走 Mod 只订阅统一的 ROS 2 图像和 CameraInfo。

完成后的结构

真机：bxi_depth_camera
仿真：MuJoCo camera sensor
             │
             │ Image + CameraInfo
             ▼
NormalDepthState
  ├─ 解码完整深度图
  ├─ 根据真实内参裁剪策略 FOV
  ├─ 缩放、距离限幅和旋转
  ├─ 按策略频率选择深度帧
  └─ RobotControlState 输出 MotorFrame

当前边界如下：

• 相机包负责发现设备、序列号映射、SDK 滤波和标准 ROS 流发布。
• 深度行走 Mod 不导入 pyrealsense2，也不打开相机设备。
• 仿真与真机使用相同逻辑相机名和流级目录。
• simulation 与 hardware 只由 /topic_prefix 区分。
• 策略裁剪依赖每一帧配置对应的真实 CameraInfo，不硬编码输入相机内参。

1. 启动相机

真机可单独启动相机管理器：

ros2 launch bxi_depth_camera cameras.launch.py

硬件示例启动文件已经默认包含它：

ros2 launch bxi_example_py_elf3 example_demo_hw.launch.py

MuJoCo 不需要启动真机相机包。仿真包会发布 XML 中声明的 camera-backed user sensor。

2. 统一话题契约

逻辑相机名默认为 body_depth_camera。状态实际使用两条深度流：

/simulation/body_depth_camera/depth/image_rect_raw
/simulation/body_depth_camera/depth/camera_info

/hardware/body_depth_camera/depth/image_rect_raw
/hardware/body_depth_camera/depth/camera_info

图像要求：

• 类型：sensor_msgs/msg/Image；
• 编码：16UC1、mono16 或 32FC1；
• 16UC1/mono16 默认按毫米解释；
• 32FC1 按米解释；
• 允许 step 包含行尾 padding；
• 无效深度保持为 0。

相机信息要求：

• 类型：sensor_msgs/msg/CameraInfo；
• width、height 为正数；
• k 含 9 个有限值；
• fx=k[0]、fy=k[4] 为正数；
• 必须与当前深度流的分辨率和内参对应。

状态不会订阅旧的 /camera/depth/image_36x48 或 /camera/depth/image_64x36。策略尺寸由状态内部生成。

3. 真机逻辑名称与序列号

策略配置只写安装位置的逻辑名称：

camera_name: body_depth_camera

每台真机在 bxi_depth_camera 配置中把该名称映射到实际设备序列号：

/depth_camera_manager:
  ros__parameters:
    single_camera_name: body_depth_camera
    cameras:
      body_depth_camera:
        serial_no: "实际序列号"

显式序列号映射始终优先。如果只连接一台未映射的受支持相机，它会使用 single_camera_name（默认 body_depth_camera）；连接多台未映射相机时，各设备使用 SN_<serial>，因此多相机部署应显式配置逻辑名称，不能依赖 USB 枚举顺序。

4. 状态参数

当前 mod.yaml 的核心配置是：

states:
  normal_depth:
    params:
      mode: origin_camera
      camera_name: body_depth_camera
      topic: ""
      camera_info_topic: ""
      depth_uint16_scale: 0.001
      depth_timeout_sec: 0.5
      horizontal_fov: 45.2
      vertical_fov: 58.0616969
      min_dist: 0.2
      max_dist: 3.0

当 topic 和 camera_info_topic 都为空时，状态使用 /topic_prefix 和 camera_name 自动解析话题。两者必须同时为空或同时填写，不能只覆盖一条：

camera_name: ""
topic: /recorded_camera/depth/image_rect_raw
camera_info_topic: /recorded_camera/depth/camera_info

支持的策略模式：

mode	投影输出（旋转前）	状态旋转后 shape	默认策略 FOV	默认距离
origin_camera	36 × 48	(36, 48)	45.2° × 58.0616969°	0.2–3.0 m
depth_walk	64 × 36	(64, 36)	89.24° × 58.06°	0.2–2.5 m

这些是策略输入要求，不是物理相机的完整 FOV。状态根据物理相机的 CameraInfo 计算中心 ROI，再缩放到策略尺寸。

5. API 4 Resource

两套策略使用不同的全局 Resource key：

LEGACY_POLICY = ResourceKey[HumanoidGaitDepthPolicyIsaaclab](
    "com.bxi.normal_depth/legacy_policy"
)
ORIGIN_POLICY = ResourceKey[HumanoidGaitOriginCameraPolicyIsaaclab](
    "com.bxi.normal_depth/origin_policy"
)

当前内置 Mod 在启动控制循环前准备两个模型：

context.register_resource(
    LEGACY_POLICY,
    _load_legacy_policy,
    policy="startup",
)
context.register_resource(
    ORIGIN_POLICY,
    _load_origin_policy,
    policy="startup",
)

如果客户动作希望首次进入时才加载，可改为 policy="on_demand"。此时状态必须声明 资源依赖；NormalDepthState 已按 API 4 这样实现：

class NormalDepthState(
    RobotControlState,
    EntryFrameProvider,
    RunningFrameProvider,
):
    def __init__(self, name, state_id, policy, **config):
        super().__init__(name, state_id, resources=(policy,))
        self._policy = policy

ResourceHandle.get() 只读取已经就绪的模型。它不会在 ROS 回调或控制周期中同步加载。

6. 订阅生命周期

状态在 on_bind() 中创建长期订阅，因为进入状态前就必须收到新鲜图像：

def on_bind(self, ctx):
    if not self.depth_image_topic:
        prefix = str(
            ctx.ros_node.get_parameter("/topic_prefix").value
        ).strip("/")
        self.depth_image_topic, self.camera_info_topic = resolve_camera_topics(
            prefix,
            self.camera_name,
        )

    self._depth_subscription = ctx.ros_node.create_subscription(
        Image,
        self.depth_image_topic,
        self.depth_image_callback,
        qos_profile_sensor_data,
    )
    self._camera_info_subscription = ctx.ros_node.create_subscription(
        CameraInfo,
        self.camera_info_topic,
        self.camera_info_callback,
        qos_profile_sensor_data,
    )

实际实现使用深度为 1 的 sensor-data QoS。on_unbind() 对称销毁两个 subscription。 不要在 on_enter() 反复创建订阅，否则刚请求状态时还没有图像，健康检查也无法通过。

7. 用 CameraInfo 生成策略输入

回调流程是：

1. 按 encoding、is_bigendian 和 step 解码深度图；
2. 将深度统一转换为米；
3. 读取对应 CameraInfo 的 fx、fy；
4. 按策略水平/垂直 FOV 和输出宽高比选择中心 ROI；
5. 使用最近邻缩放，避免为深度制造不存在的插值表面；
6. 只对有效像素做距离限幅，无效像素继续保持 0；
7. 按原策略方向顺时针旋转 90°；
8. 在锁内替换最新完整数组和递增的 frame id。

核心调用如下：

projected, crop = project_depth(
    depth_meters,
    camera_info,
    self.projection,
)
depth_rotated = np.ascontiguousarray(
    np.rot90(projected, k=-1).astype(np.float32)
)

相机分辨率变化时，只要 Image 与 CameraInfo 同步更新，裁剪会按新内参重新计算；不需要 为每一种分辨率硬编码一套 K/P 数组。

8. 进入健康检查

is_available() 必须非阻塞，只读取回调维护的快照：

def is_available(self, ctx):
    with self._depth_lock:
        depth = self._depth_rotated
        stamp = self._last_depth_time
        camera_info = self._camera_info
    return (
        self.depth_configured
        and camera_info is not None
        and depth is not None
        and stamp is not None
        and time.monotonic() - stamp <= self.depth_timeout_sec
    )

没有 CameraInfo、没有有效图像或图像已超时，状态请求会返回 False，机器人保持当前 状态。force=True 只绕过健康检查，不会补出缺失的深度数据，普通业务不要使用。

9. Transition 能力

进入 normal_depth 使用 first_frame_switch，所以目标状态必须实现 EntryFrameProvider：

def get_entry_frame(self, ctx):
    return self._motor_frame_from_target(ctx, self.policy.output.joints)

切回普通行走使用 dual_running_blend。深度状态作为来源侧提供 RunningFrameProvider：

def sample_running_frame(self, ctx, dt, *, advance):
    if not advance:
        return self._last_running_frame or self.get_entry_frame(ctx)

    depth, frame_id = self._get_depth_for_inference()
    self.get_cmd_vel(ctx)
    ctx.inference_frame.depth = depth
    ctx.inference_frame.depth_frame_id = frame_id
    output = self.policy.step(ctx.inference_frame, dt, advance=True)
    return self._motor_frame_from_target(ctx, output.joints)

advance=False 不能推进策略 history、时间或动作缓存。当前实现返回最近一次真实推理帧， 避免 Transition 预览重复消费深度数据。

10. 相机频率与控制频率

控制循环和相机不需要同频。回调随相机更新完整图像；控制周期按 policy.depth_update_period 选择是否接收最新 frame id。策略可在多个控制周期复用同一张 深度图，但不会把同一帧误认为新的传感器采样。

不要在控制周期里等待下一张图，也不要在持有 _depth_lock 时执行推理。锁只保护引用、 时间戳和 frame id 的短暂复制。

11. 运行期安全退出

状态每周期检查：

• 姿态超出安全范围：请求 com.bxi.basic_actions/zero_torque；
• 深度超过 depth_timeout_sec 未更新：请求 com.bxi.basic_actions/normal；
• 新深度暂不可用：本周期不发布新的策略帧；
• 输入编码、尺寸或内参非法：记录一次警告并丢弃该帧。

安全请求只发起状态切换，不在回调线程直接发布电机命令。

12. 验证顺序

先确认相机契约：

ros2 topic info /simulation/body_depth_camera/depth/image_rect_raw --verbose
ros2 topic echo /simulation/body_depth_camera/depth/camera_info --once
ros2 topic hz /simulation/body_depth_camera/depth/image_rect_raw

真机把 simulation 换成 hardware。然后检查：

ros2 topic echo /simulation/state_machine_info --once

推荐顺序：

1. 相机图和 CameraInfo 都持续发布；
2. 日志中的 image、camera_info 和 post-rotation shape 正确；
3. 静止仿真进入深度状态；
4. 遮挡或停止相机，确认自动退回 normal；
5. 验证 normal 与 normal_depth 的进入、退出 Transition；
6. 最后在吊架和低速条件下验证真机。

常见故障

现象	优先检查
状态不可进入	CameraInfo、图像新鲜度、逻辑相机名
一直等待 CameraInfo	相机是否发布 /depth/camera_info，话题前缀是否正确
shape 不符合策略	mode、FOV 参数、图像与 CameraInfo 是否属于同一 profile
深度全零	编码、单位、设备量程和 SDK 滤波
真机找不到相机	bxi_depth_camera 配置中的序列号映射
仿真正常、真机失败	比较两端 CameraInfo、安装姿态、深度单位和 frame rate
切换时突跳	检查 entry/running frame 和 Transition profile
首次进入等待模型	资源使用了 policy="on_demand"，查看 preparing 状态

最终检查表

1. 状态不直接依赖 RealSense 或 Orbbec SDK。
2. 仿真与真机只替换 simulation/hardware 前缀。
3. 图像与 CameraInfo 成对配置。
4. Resource 使用 API 4 的 startup/on_demand 策略。
5. 状态通过 resources=(policy,) 声明模型依赖。
6. advance=False 不推进策略。
7. 回调不执行模型推理，控制周期不等待相机。
8. 图像失联和姿态异常都有明确退出路径。

当前实现以以下文件为准：

src/bxi_example_py_elf3/mods/com.bxi.normal_depth/mod.yaml
src/bxi_example_py_elf3/mods/com.bxi.normal_depth/plugin.py
src/bxi_example_py_elf3/mods/com.bxi.normal_depth/state.py
src/bxi_example_py_elf3/mods/com.bxi.normal_depth/depth_projection.py
src/bxi_example_py_elf3/mods/com.bxi.normal_depth/README.md