feat: capture head and waist RGB-D data

tg3_local_teleop/README.md

@@ -160,9 +160,20 @@ OmniSocket 进程；在此之前桥会保持最后一帧目标。未 START 或
 新会话和服务重启后都先锁定为“必须松开”，畸形或陈旧按键帧不能被当作有效松开。
 
 项目自有 `tg3-data-recorder.service` 在 Nvidia 上运行独立 `ros2 bag record`，不停止、
-重配或接管 Ubuntu 厂家 `/record_bag_node`。默认仅录 `config.toml` 中的明确白名单，
-包含 `/robot_state`、双臂/BrainCo 双手命令与反馈、HBWALK、IMU、电源状态以及完整
-xTELE 应用帧；相机和点云默认不录，也禁止改成 `-a`。
+重配或接管 Ubuntu 厂家 `/record_bag_node`。仅录 `config.toml` 中的明确白名单，包含
+`/robot_state`、双臂/BrainCo 双手命令与反馈、HBWALK、IMU、电源状态、完整 xTELE
+应用帧，以及头/腰 Orbbec 的 RGB、深度、内参和元数据；仍禁止改成 `-a` 或录点云。
+
+为避免 1280x720 原始 RGB+深度每台相机约 `8.3 GB/min` 的持续写盘负载，默认录制
+`color/image_raw/compressed`（JPEG）和 `depth/image_raw/compressedDepth`（16UC1
+无损 PNG）。现场单台腰部相机实测合计约 `22–25 MB/s`（约 `1.3–1.5 GB/min`，随
+场景变化）。腰部图像、内参、Metadata 和 `/tf_static` 是 required topics；两路图像还
+必须在整条 bag 内分别达到至少 `20 Hz` 平均频率。相机服务未发布或图像严重掉流时，
+L3 启动或最终校验会失败，不会生成缺图像却标记 READY 的 episode。头部话题在白名单
+中但不是 required：当前
+`orbbec_head.service` 未启动不影响腰部数采；将来在 episode 开始前启动后，rosbag 会
+自动发现并采集，无需再改配置。静态 TF 会被保存用于相机外参，但当前收尾只验证该
+Topic 非空，不解析每条 TF 的 frame 语义。
 
 机器人暂存目录：
 
@@ -174,11 +185,21 @@ xTELE 应用帧；相机和点云默认不录，也禁止改成 `-a`。
 ```
 
 MCAP 使用 `zstd_fast`、64 MiB cache、每 300 秒分片；每条 episode 最长 30 分钟，
-启动/运行最低保留 20 GiB。正常停止以 SIGINT 让 rosbag 写完 `metadata.yaml`，随后执行
+加入双 RGB-D 后启动/运行最低保留 100 GiB。正常停止以 SIGINT 让 rosbag 写完
+`metadata.yaml`，随后执行
 `ros2 bag info`，并确认所有 required topics 都有非零消息数；然后计算 SHA-256、写
 `manifest.json` 和 `READY`，最后才原子进入 `ready/`。Nvidia 需已安装 `python3-yaml`
 （现场已验证 PyYAML 6.0.1）。
 
+相机由厂家系统服务管理，本项目只订阅、不自动启停。录制前检查或按需启动：
+
+```bash
+systemctl is-active orbbec_waist.service orbbec_head.service
+sudo systemctl enable --now orbbec_waist.service
+# 需要头部图像时再执行；启动后下一条 episode 会自动包含头部 RGB-D：
+sudo systemctl enable --now orbbec_head.service
+```
+
 录制服务只加载厂家完整 `/opt/robot_tele_server/install` 消息 overlay，不加载本项目
 `ros2_py/install`；项目 overlay 仅供桥的 Python `ArmStatus` 绑定使用。两者混在同一个
 rosbag 环境会让部分 `ros2_bridge_msgs` C++ 类型支持出现动态库符号冲突。

tg3_local_teleop/config.toml

@@ -75,12 +75,21 @@ control_topic = "/tg3/data_collection/control"
 status_topic = "/tg3/data_collection/status"
 iarm_frame_topic = "/tg3/data_collection/iarm_frame"
 base_directory = "/home/nvidia/tg3_data_collection"
-minimum_free_gib = 20.0
+minimum_free_gib = 100.0
 max_duration_seconds = 1800.0
+# A camera endpoint merely existing is insufficient: completed episodes must
+# also sustain this conservative average rate across the rosbag data span.
+# The driver targets 30 Hz; 20 Hz tolerates startup/discovery jitter while
+# rejecting a stream that only delivered a few frames or stopped mid-episode.
+minimum_topic_rates_hz."/ob_camera_waist/color/image_raw/compressed" = 20.0
+minimum_topic_rates_hz."/ob_camera_waist/depth/image_raw/compressedDepth" = 20.0
 
-# Explicit control-data whitelist.  Camera images and point clouds are omitted
-# by default to bound disk and network load; never replace this with `-a` on the
-# live robot.  /robot_state is the authoritative measured robot state.
+# Explicit data whitelist; never replace this with `-a` on the live robot.
+# RGB is recorded through image_transport JPEG and 16UC1 depth through its
+# lossless compressedDepth PNG transport. Waist RGB-D is required because that
+# camera is enabled on this robot. Head RGB-D stays optional: rosbag discovers
+# and records it automatically whenever orbbec_head.service is running.
+# /robot_state is the authoritative measured robot state.
 topics = [
   "/robot_state",
   "/encoder_identical_joint",
@@ -100,6 +109,19 @@ topics = [
   "/power/board/key_status",
   "/power/board/status",
   "/power/battery/status",
+  "/ob_camera_head/color/image_raw/compressed",
+  "/ob_camera_head/color/camera_info",
+  "/ob_camera_head/color/metadata",
+  "/ob_camera_head/depth/image_raw/compressedDepth",
+  "/ob_camera_head/depth/camera_info",
+  "/ob_camera_head/depth/metadata",
+  "/ob_camera_waist/color/image_raw/compressed",
+  "/ob_camera_waist/color/camera_info",
+  "/ob_camera_waist/color/metadata",
+  "/ob_camera_waist/depth/image_raw/compressedDepth",
+  "/ob_camera_waist/depth/camera_info",
+  "/ob_camera_waist/depth/metadata",
+  "/tf_static",
   "/tg3/data_collection/control",
   "/tg3/data_collection/status",
   "/tg3/data_collection/iarm_frame",
@@ -115,6 +137,13 @@ required_topics = [
   "/left_hand/motor_status",
   "/right_hand/motor_status",
   "/hric/robot/rl_state",
+  "/ob_camera_waist/color/image_raw/compressed",
+  "/ob_camera_waist/color/camera_info",
+  "/ob_camera_waist/color/metadata",
+  "/ob_camera_waist/depth/image_raw/compressedDepth",
+  "/ob_camera_waist/depth/camera_info",
+  "/ob_camera_waist/depth/metadata",
+  "/tf_static",
   "/tg3/data_collection/control",
   "/tg3/data_collection/status",
   "/tg3/data_collection/iarm_frame",

tg3_local_teleop/data_collection.py

@@ -20,9 +20,10 @@ import threading
 import time
 import uuid
 from collections.abc import Callable, Mapping, Sequence
-from dataclasses import dataclass
+from dataclasses import dataclass, field
 from datetime import datetime, timezone
 from pathlib import Path
+from types import MappingProxyType
 from typing import Any, Literal, Protocol
 
 import yaml
@@ -201,6 +202,7 @@ class RecorderConfig:
     base_directory: Path | str = Path("/home/nvidia/tg3_data_collection")
     topics: Sequence[str] = ()
     required_topics: Sequence[str] = ()
+    minimum_topic_rates_hz: Mapping[str, float] = field(default_factory=dict)
     minimum_free_bytes: int = 5 * 1024**3
     max_duration_seconds: float = 30 * 60.0
     poll_interval_seconds: float = 0.1
@@ -214,6 +216,7 @@ class RecorderConfig:
         base = Path(self.base_directory).expanduser()
         topics = tuple(self.topics)
         required_topics = tuple(self.required_topics)
+        minimum_topic_rates: dict[str, float] = {}
         if not topics:
             raise ValueError("at least one recording topic is required")
         if any(
@@ -238,6 +241,23 @@ class RecorderConfig:
             raise ValueError("required topics must be absolute ROS topic names")
         if not set(required_topics).issubset(topics):
             raise ValueError("required topics must be a subset of recording topics")
+        if not isinstance(self.minimum_topic_rates_hz, Mapping):
+            raise ValueError("minimum topic rates must be a mapping")
+        for topic, rate in self.minimum_topic_rates_hz.items():
+            if topic not in required_topics:
+                raise ValueError(
+                    "minimum-rate topics must be a subset of required topics"
+                )
+            if (
+                isinstance(rate, bool)
+                or not isinstance(rate, (int, float))
+                or not math.isfinite(rate)
+                or rate <= 0.0
+            ):
+                raise ValueError(
+                    f"minimum topic rate for {topic!r} must be positive and finite"
+                )
+            minimum_topic_rates[topic] = float(rate)
         if (
             isinstance(self.minimum_free_bytes, bool)
             or not isinstance(self.minimum_free_bytes, int)
@@ -260,6 +280,11 @@ class RecorderConfig:
         object.__setattr__(self, "base_directory", base)
         object.__setattr__(self, "topics", topics)
         object.__setattr__(self, "required_topics", required_topics)
+        object.__setattr__(
+            self,
+            "minimum_topic_rates_hz",
+            MappingProxyType(minimum_topic_rates),
+        )
 
 
 class RecorderProcess(Protocol):
@@ -296,7 +321,11 @@ class _Episode:
     node_name: str = ""
     command: tuple[str, ...] = ()
     bag_info_validation: dict[str, Any] | None = None
+    topic_message_counts: dict[str, int] | None = None
     required_topic_message_counts: dict[str, int] | None = None
+    minimum_topic_message_counts: dict[str, int] | None = None
+    observed_topic_rates_hz: dict[str, float] | None = None
+    metadata_duration_nanoseconds: int | None = None
     process: RecorderProcess | None = None
     stdout_stream: Any = None
     stderr_stream: Any = None
@@ -728,8 +757,19 @@ class DataRecorderManager:
             "start_reason": episode.request.reason,
             "stop_reason": stop_reason,
             "topics": list(self.config.topics),
+            "topic_message_counts": episode.topic_message_counts,
             "required_topics": list(self.config.required_topics),
             "required_topic_message_counts": required_topic_counts,
+            "minimum_topic_rates_hz": dict(
+                self.config.minimum_topic_rates_hz
+            ),
+            "minimum_topic_message_counts": (
+                episode.minimum_topic_message_counts
+            ),
+            "observed_topic_rates_hz": episode.observed_topic_rates_hz,
+            "metadata_duration_nanoseconds": (
+                episode.metadata_duration_nanoseconds
+            ),
             "storage_id": "mcap",
             "custom_data": {
                 "capture_id": episode.request.episode_id,
@@ -778,6 +818,9 @@ class DataRecorderManager:
                 raise RecordingError(
                     "bag metadata.yaml lacks topics_with_message_count"
                 )
+            episode.topic_message_counts = {
+                topic: 0 for topic in self.config.topics
+            }
             episode.required_topic_message_counts = {}
             return {}
 
@@ -812,6 +855,9 @@ class DataRecorderManager:
             topic: all_counts.get(topic, 0)
             for topic in self.config.required_topics
         }
+        episode.topic_message_counts = {
+            topic: all_counts.get(topic, 0) for topic in self.config.topics
+        }
         episode.required_topic_message_counts = required_counts
         missing = [
             topic for topic in self.config.required_topics if topic not in all_counts
@@ -828,8 +874,61 @@ class DataRecorderManager:
             problems.append("zero-message required topics: " + ", ".join(empty))
         if problems:
             raise RecordingError("; ".join(problems))
+        self._validate_minimum_topic_rates(episode, information, all_counts)
         return required_counts
 
+    def _validate_minimum_topic_rates(
+        self,
+        episode: _Episode,
+        information: Mapping[str, Any],
+        all_counts: Mapping[str, int],
+    ) -> None:
+        configured = self.config.minimum_topic_rates_hz
+        if not configured:
+            episode.minimum_topic_message_counts = {}
+            episode.observed_topic_rates_hz = {}
+            return
+
+        duration = information.get("duration")
+        if not isinstance(duration, Mapping):
+            raise RecordingError(
+                "bag metadata.yaml lacks duration for minimum-rate validation"
+            )
+        nanoseconds = duration.get("nanoseconds")
+        if (
+            isinstance(nanoseconds, bool)
+            or not isinstance(nanoseconds, int)
+            or nanoseconds <= 0
+        ):
+            raise RecordingError(
+                "bag metadata.yaml duration.nanoseconds must be a positive integer"
+            )
+
+        episode.metadata_duration_nanoseconds = nanoseconds
+        minimum_counts: dict[str, int] = {}
+        observed_rates: dict[str, float] = {}
+        failures: list[str] = []
+        duration_seconds = nanoseconds / 1_000_000_000.0
+        for topic, minimum_rate in configured.items():
+            count = all_counts[topic]
+            minimum_count = math.floor(duration_seconds * minimum_rate)
+            observed_rate = count / duration_seconds
+            minimum_counts[topic] = minimum_count
+            observed_rates[topic] = observed_rate
+            if count < minimum_count:
+                failures.append(
+                    f"{topic}: {count} messages < {minimum_count} required "
+                    f"over {duration_seconds:.6f}s "
+                    f"({observed_rate:.3f} Hz < {minimum_rate:.3f} Hz)"
+                )
+        episode.minimum_topic_message_counts = minimum_counts
+        episode.observed_topic_rates_hz = observed_rates
+        if failures:
+            raise RecordingError(
+                "required topics below minimum average rate: "
+                + "; ".join(failures)
+            )
+
     def _validate_bag_info(self, episode: _Episode) -> dict[str, Any]:
         """Run rosbag2's reader-level validation before publishing READY."""
 
@@ -952,10 +1051,21 @@ class DataRecorderManager:
                 "start_reason": episode.request.reason,
                 "stop_reason": stop_reason,
                 "topics": list(self.config.topics),
+                "topic_message_counts": episode.topic_message_counts,
                 "required_topics": list(self.config.required_topics),
                 "required_topic_message_counts": (
                     episode.required_topic_message_counts
                 ),
+                "minimum_topic_rates_hz": dict(
+                    self.config.minimum_topic_rates_hz
+                ),
+                "minimum_topic_message_counts": (
+                    episode.minimum_topic_message_counts
+                ),
+                "observed_topic_rates_hz": episode.observed_topic_rates_hz,
+                "metadata_duration_nanoseconds": (
+                    episode.metadata_duration_nanoseconds
+                ),
                 "storage_id": "mcap",
                 "custom_data": {
                     "capture_id": episode.request.episode_id,

tg3_local_teleop/data_recorder_node.py

@@ -89,6 +89,19 @@ def _topic_list(value: Any, name: str, *, allow_empty: bool) -> tuple[str, ...]:
     return result
 
 
+def _minimum_topic_rates(value: Any) -> dict[str, float]:
+    name = "minimum_topic_rates_hz"
+    if not isinstance(value, dict):
+        raise ValueError(f"data_collection.{name} must be a TOML table")
+    result: dict[str, float] = {}
+    for topic, rate in value.items():
+        normalized_topic = _topic(topic, f"{name} key")
+        result[normalized_topic] = _positive_number(
+            rate, f"{name}.{normalized_topic}"
+        )
+    return result
+
+
 @dataclass(frozen=True)
 class SupervisorSettings:
     enabled: bool
@@ -134,6 +147,17 @@ def load_settings(path: Path | str) -> SupervisorSettings:
             "data_collection.required_topics must also appear in topics: "
             + ", ".join(unknown_required)
         )
+    minimum_topic_rates = _minimum_topic_rates(
+        section.get("minimum_topic_rates_hz", {})
+    )
+    unknown_rate_topics = sorted(
+        set(minimum_topic_rates) - set(required_topics)
+    )
+    if unknown_rate_topics:
+        raise ValueError(
+            "data_collection.minimum_topic_rates_hz keys must also appear in "
+            "required_topics: " + ", ".join(unknown_rate_topics)
+        )
 
     heartbeat_timeout = _positive_number(
         section.get("heartbeat_timeout_seconds", 3.0),
@@ -162,6 +186,7 @@ def load_settings(path: Path | str) -> SupervisorSettings:
         base_directory=Path(base_directory).expanduser(),
         topics=topics,
         required_topics=required_topics,
+        minimum_topic_rates_hz=minimum_topic_rates,
         minimum_free_bytes=int(minimum_free_gib * 1024**3),
         max_duration_seconds=max_duration,
         poll_interval_seconds=0.1,

tg3_local_teleop/test_data_collection.py

@@ -165,6 +165,7 @@ class FakeProcess:
         unexpected_returncode: int | None = None,
         ignore_sigint: bool = False,
         topic_counts: Mapping[str, int] | None = None,
+        duration_nanoseconds: int = 1_000_000_000,
     ) -> None:
         self.command = list(command)
         self.signals: list[int] = []
@@ -178,6 +179,7 @@ class FakeProcess:
             metadata = {
                 "rosbag2_bagfile_information": {
                     "storage_identifier": "mcap",
+                    "duration": {"nanoseconds": duration_nanoseconds},
                     "topics_with_message_count": [
                         {
                             "topic_metadata": {"name": topic},
@@ -286,7 +288,9 @@ class DataRecorderManagerTest(unittest.TestCase):
         process_options: dict[str, Any] | None = None,
         command_runner: Callable[..., Any] | None = None,
         validate_bag_info: bool = True,
+        topics: Sequence[str] = ("/joint_states", "/tf"),
         required_topics: Sequence[str] = ("/joint_states", "/tf"),
+        minimum_topic_rates_hz: Mapping[str, float] | None = None,
     ) -> tuple[DataRecorderManager, Any]:
         if factory is None:
             factory = FakeProcessFactory(**(process_options or {}))
@@ -298,8 +302,9 @@ class DataRecorderManagerTest(unittest.TestCase):
         manager = DataRecorderManager(
             RecorderConfig(
                 base_directory=self.base,
-                topics=("/joint_states", "/tf"),
+                topics=topics,
                 required_topics=required_topics,
+                minimum_topic_rates_hz=minimum_topic_rates_hz or {},
                 minimum_free_bytes=minimum_free,
                 max_duration_seconds=max_duration,
                 poll_interval_seconds=0.005,
@@ -346,6 +351,10 @@ class DataRecorderManagerTest(unittest.TestCase):
         manifest = json.loads((ready / "manifest.json").read_text())
         self.assertEqual(manifest["session_id"], "a" * 32)
         self.assertEqual(manifest["topics"], ["/joint_states", "/tf"])
+        self.assertEqual(
+            manifest["topic_message_counts"],
+            {"/joint_states": 25, "/tf": 50},
+        )
         self.assertEqual(
             manifest["required_topic_message_counts"],
             {"/joint_states": 25, "/tf": 50},
@@ -553,6 +562,108 @@ class DataRecorderManagerTest(unittest.TestCase):
         self.assertEqual(bag_info.calls, [])
         self.assertFalse((failed / "READY").exists())
 
+    def test_minimum_average_topic_rates_are_recorded_in_ready_manifest(
+        self,
+    ) -> None:
+        configured_rates = {"/joint_states": 20.0, "/tf": 40.0}
+        manager, _ = self.make_manager(
+            minimum_topic_rates_hz=configured_rates
+        )
+        # RecorderConfig owns an immutable copy, not the caller's dictionary.
+        configured_rates["/tf"] = 1.0
+        with self.assertRaises(TypeError):
+            manager.config.minimum_topic_rates_hz["/tf"] = 2.0  # type: ignore[index]
+
+        self.assertTrue(manager.request_start("b0" * 16))
+        self.wait_for(lambda: manager.status()["recording"])
+        self.assertTrue(manager.request_stop())
+        self.assertTrue(manager.wait_until_idle(2.0))
+        status = manager.status()
+        self.assertEqual(status["last_result"], "ready")
+        ready = Path(status["last_episode_directory"])
+        manifest = json.loads((ready / "manifest.json").read_text())
+        self.assertEqual(
+            manifest["minimum_topic_rates_hz"],
+            {"/joint_states": 20.0, "/tf": 40.0},
+        )
+        self.assertEqual(
+            manifest["minimum_topic_message_counts"],
+            {"/joint_states": 20, "/tf": 40},
+        )
+        self.assertEqual(
+            manifest["observed_topic_rates_hz"],
+            {"/joint_states": 25.0, "/tf": 50.0},
+        )
+        self.assertEqual(
+            manifest["metadata_duration_nanoseconds"], 1_000_000_000
+        )
+
+    def test_optional_topic_counts_show_absent_and_active_publishers(self) -> None:
+        head_topic = "/ob_camera_head/color/image_raw/compressed"
+        topics = ("/joint_states", "/tf", head_topic)
+        for head_count in (None, 17):
+            with self.subTest(head_count=head_count):
+                counts = {"/joint_states": 25, "/tf": 50}
+                if head_count is not None:
+                    counts[head_topic] = head_count
+                manager, _ = self.make_manager(
+                    topics=topics,
+                    process_options={"topic_counts": counts},
+                )
+                self.assertTrue(manager.request_start("c0" * 16))
+                self.wait_for(lambda: manager.status()["recording"])
+                self.assertTrue(manager.request_stop())
+                self.assertTrue(manager.wait_until_idle(2.0))
+                status = manager.status()
+                self.assertEqual(status["last_result"], "ready")
+                manifest = json.loads(
+                    (
+                        Path(status["last_episode_directory"])
+                        / "manifest.json"
+                    ).read_text()
+                )
+                self.assertEqual(
+                    manifest["topic_message_counts"][head_topic],
+                    0 if head_count is None else head_count,
+                )
+
+    def test_topic_that_stops_mid_episode_fails_average_rate(self) -> None:
+        bag_info = FakeBagInfoRunner()
+        manager, _ = self.make_manager(
+            command_runner=bag_info,
+            minimum_topic_rates_hz={"/joint_states": 20.0, "/tf": 20.0},
+            process_options={
+                "duration_nanoseconds": 10_000_000_000,
+                # /tf delivered briefly, then stopped for most of the bag.
+                "topic_counts": {"/joint_states": 250, "/tf": 50},
+            },
+        )
+        self.assertTrue(manager.request_start("b1" * 16))
+        self.wait_for(lambda: manager.status()["recording"])
+        self.assertTrue(manager.request_stop())
+        self.assertTrue(manager.wait_until_idle(2.0))
+        status = manager.status()
+        self.assertEqual(status["last_result"], "failed")
+        self.assertIn(
+            "/tf: 50 messages < 200 required", status["last_error"]
+        )
+        failed = Path(status["last_episode_directory"])
+        manifest = json.loads((failed / "manifest.json").read_text())
+        self.assertEqual(
+            manifest["topic_message_counts"],
+            {"/joint_states": 250, "/tf": 50},
+        )
+        self.assertEqual(
+            manifest["observed_topic_rates_hz"],
+            {"/joint_states": 25.0, "/tf": 5.0},
+        )
+        self.assertEqual(
+            manifest["minimum_topic_message_counts"],
+            {"/joint_states": 200, "/tf": 200},
+        )
+        self.assertEqual(bag_info.calls, [])
+        self.assertFalse((failed / "READY").exists())
+
     def test_bag_info_timeout_preserves_episode_as_failed(self) -> None:
         manager, _ = self.make_manager(
             command_runner=FakeBagInfoRunner(timeout=True)
@@ -693,6 +804,29 @@ class DataRecorderManagerTest(unittest.TestCase):
                 ("/recorded",),
                 required_topics=("/recorded", "/recorded"),
             )
+        with self.assertRaises(ValueError):
+            RecorderConfig(
+                self.base,
+                ("/recorded", "/other"),
+                required_topics=("/recorded",),
+                minimum_topic_rates_hz={"/other": 1.0},
+            )
+        for invalid_rate in (0, -1, float("nan"), float("inf"), True, "20"):
+            with self.subTest(invalid_rate=invalid_rate):
+                with self.assertRaises(ValueError):
+                    RecorderConfig(
+                        self.base,
+                        ("/recorded",),
+                        required_topics=("/recorded",),
+                        minimum_topic_rates_hz={"/recorded": invalid_rate},
+                    )
+        with self.assertRaises(ValueError):
+            RecorderConfig(
+                self.base,
+                ("/recorded",),
+                required_topics=("/recorded",),
+                minimum_topic_rates_hz=[],  # type: ignore[arg-type]
+            )
 
 
 if __name__ == "__main__":