From d02b99d2aa79ff5ce71767cc7b738813efd81ada Mon Sep 17 00:00:00 2001 From: meiqi <2510105031@mails.szu.edu.cn> Date: Mon, 10 Aug 2026 16:40:00 +0800 Subject: [PATCH] feat: capture head and waist RGB-D data --- README.md | 7 +- docs/天工3.0本地同构臂遥操迁移部署指南.md | 27 ++++- tg3_local_teleop/README.md | 29 ++++- tg3_local_teleop/config.toml | 37 +++++- tg3_local_teleop/data_collection.py | 112 +++++++++++++++++- tg3_local_teleop/data_recorder_node.py | 25 ++++ tg3_local_teleop/test_data_collection.py | 136 +++++++++++++++++++++- verify.sh | 31 ++++- 8 files changed, 388 insertions(+), 16 deletions(-) diff --git a/README.md b/README.md index db997bd..20e9a3f 100644 --- a/README.md +++ b/README.md @@ -53,8 +53,11 @@ TOML 解析检查。若还要核对外置 OmniSocketGo 版本，可设置任务 ## 数采保存 Nvidia 独立服务录制明确白名单内的机器人实测状态、双臂/双手命令与反馈、行走、 -IMU、电源状态和 xTELE 原始应用帧。它不接管或停止厂家 `/record_bag_node`，默认也不录 -相机和点云。正常结束后先校验 MCAP、生成 SHA-256 `manifest.json` 和 `READY`，再由 +IMU、电源状态、xTELE 应用帧以及头/腰 Orbbec 的 RGB-D。RGB 使用标准 JPEG 压缩流， +16 位深度使用无损 compressedDepth PNG，并同时保存相机内参与静态 TF；腰部相机是 +必采，头部相机未启动时不阻塞，之后启动即可被同一录制自动发现。它不接管或停止厂家 +`/record_bag_node`，也不录点云。正常结束后先校验 MCAP、生成 SHA-256 +`manifest.json` 和 `READY`，再由 本机服务拉取到： ```text diff --git a/docs/天工3.0本地同构臂遥操迁移部署指南.md b/docs/天工3.0本地同构臂遥操迁移部署指南.md index fd9e2f3..a262409 100644 --- a/docs/天工3.0本地同构臂遥操迁移部署指南.md +++ b/docs/天工3.0本地同构臂遥操迁移部署指南.md @@ -1058,9 +1058,30 @@ Python 进程专用的最小 `ArmStatus` 绑定，会遮蔽厂家完整 `ros2_br rosbag 加载 `RobotState/ArmCtrl` 时出现 undefined symbol。`run.sh` 仍需项目 overlay； `run_data_recorder.sh` 只用厂家完整消息 overlay。 还需 Python 3 的 PyYAML（Ubuntu 包 `python3-yaml`）解析 rosbag metadata。 -迁移时在 `config.toml [data_collection]` 核对 `base_directory`、20 GiB 余量、30 分钟上限、 -明确 topic 白名单和 required topics。不要改成 `ros2 bag record -a`，相机/点云需另行估算 -带宽和磁盘后再加入。 +迁移时在 `config.toml [data_collection]` 核对 `base_directory`、100 GiB 余量、30 分钟 +上限、明确 topic 白名单和 required topics。不要改成 `ros2 bag record -a`，点云需另行 +估算带宽和磁盘后再加入。当前白名单已经包含头/腰 Orbbec 的 JPEG RGB、无损 PNG 深度、 +CameraInfo、Metadata 和 `/tf_static`；腰部 RGB-D/Metadata/静态 TF 为 required，两路 +图像收尾时还要求各自平均频率至少 `20 Hz`。头部未启动时是 optional，以后启动 +`orbbec_head.service` 即可由正在录制的 rosbag 自动发现。 + +新机器人先确认相机服务和发布者。相机必须在按 L3 开始 episode 之前启动： + +```bash +systemctl is-active orbbec_waist.service orbbec_head.service +sudo systemctl enable --now orbbec_waist.service +# 同时需要头部 RGB-D 时： +sudo systemctl enable --now orbbec_head.service + +ros2 topic info /ob_camera_waist/color/image_raw/compressed +ros2 topic info /ob_camera_waist/depth/image_raw/compressedDepth +``` + +两条腰部话题都必须显示 `Publisher count: 1`。原始 1280x720 RGB+深度约 +`8.3 GB/min/相机`，所以默认不重复录 raw，而是录标准 image_transport 压缩流；现场 +单相机约 `1.3–1.5 GB/min`，实际随画面变化。头部和腰部都运行时应按约两倍预留空间与 +同步时间。`minimum_free_gib=100` 是停止下限，不是整条 episode 的容量预算；双相机 +长录制前至少预留“100 GiB + 预计数据量”，并在短录包中先确认实际带宽。 ### 11.2 PS 本机 diff --git a/tg3_local_teleop/README.md b/tg3_local_teleop/README.md index 3fee2de..e1f93af 100644 --- a/tg3_local_teleop/README.md +++ b/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++ 类型支持出现动态库符号冲突。 diff --git a/tg3_local_teleop/config.toml b/tg3_local_teleop/config.toml index f2adde7..82b7dea 100644 --- a/tg3_local_teleop/config.toml +++ b/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", diff --git a/tg3_local_teleop/data_collection.py b/tg3_local_teleop/data_collection.py index 43bcc67..279bed7 100755 --- a/tg3_local_teleop/data_collection.py +++ b/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, diff --git a/tg3_local_teleop/data_recorder_node.py b/tg3_local_teleop/data_recorder_node.py index 541a0f5..43282bd 100755 --- a/tg3_local_teleop/data_recorder_node.py +++ b/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, diff --git a/tg3_local_teleop/test_data_collection.py b/tg3_local_teleop/test_data_collection.py index 330e090..48caf57 100755 --- a/tg3_local_teleop/test_data_collection.py +++ b/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__": diff --git a/verify.sh b/verify.sh index ed4eefa..b01e05f 100755 --- a/verify.sh +++ b/verify.sh @@ -40,7 +40,36 @@ import sys import tomllib with open(sys.argv[1], "rb") as stream: - tomllib.load(stream) + document = tomllib.load(stream) +section = document["data_collection"] +topics = set(section["topics"]) +required = set(section["required_topics"]) +head_rgbd = { + "/ob_camera_head/color/image_raw/compressed", + "/ob_camera_head/color/camera_info", + "/ob_camera_head/depth/image_raw/compressedDepth", + "/ob_camera_head/depth/camera_info", +} +waist_rgbd = { + "/ob_camera_waist/color/image_raw/compressed", + "/ob_camera_waist/color/camera_info", + "/ob_camera_waist/depth/image_raw/compressedDepth", + "/ob_camera_waist/depth/camera_info", +} +assert head_rgbd <= topics, "head RGB-D topics must stay in the recording whitelist" +assert waist_rgbd <= topics, "waist RGB-D topics must stay in the recording whitelist" +assert waist_rgbd <= required, "live waist RGB-D topics must stay required" +assert head_rgbd.isdisjoint(required), "inactive head RGB-D must not block recording" +assert { + "/ob_camera_waist/color/metadata", + "/ob_camera_waist/depth/metadata", + "/tf_static", +} <= required +assert section["minimum_topic_rates_hz"] == { + "/ob_camera_waist/color/image_raw/compressed": 20.0, + "/ob_camera_waist/depth/image_raw/compressedDepth": 20.0, +} +assert section["minimum_free_gib"] >= 100.0 print("config.toml parse passed") PY