diff --git a/lightweight-operators/ubt-mcap-pipeline/README.md b/lightweight-operators/ubt-mcap-pipeline/README.md new file mode 100644 index 00000000..a045cb80 --- /dev/null +++ b/lightweight-operators/ubt-mcap-pipeline/README.md @@ -0,0 +1,49 @@ +# UBT MCAP pipeline + +This lightweight operator package implements three stages over one shared +Python 3.10 CPU image: + +1. `python main.py prepare` +2. `python main.py check` +3. `python main.py report` + +The code is mounted from +`lightweight-operators/ubt-mcap-pipeline`. Prepared data is persisted under +`/data/ubt-prepared//` on the shared workspace PVC. +Only manifests and reports flow through the artifact bus; raw episodes are not +copied between workflow nodes. + +The checker source is the minimal UBT-specific subset of RoboOrchard commit +`c742b39e0821590605385ee4ddabcafb258432d8`. Data files, URDF files, credentials, +and generated reports are intentionally excluded from this repository. + +## Runtime parameters + +Prepare: + +- `DATASET_ID` +- `USER_NAME` +- `TASK_NAME` +- `ARCHIVE_GLOB` +- `URDF_SOURCE_PATH` +- `REUSE_PREPARED=true` +- `MIN_FREE_GIB=50` +- `ON_DUPLICATE_EPISODE=error` + +Check: + +- `DATE_PREFIXES` +- `USER_NAMES` +- `TASK_NAMES` +- `NUM_WORKERS=1` +- `MAX_EPISODES=0` +- `INSPECT_CONFIG_JSON` +- `VIDEO_EXPORT=false` +- `VIS_INTERVAL=3` +- `VIS_RESIZE_SCALE=1.0` +- `VIS_DEPTH=true` +- `FAIL_ON_QUALITY_ERROR=false` + +Report output paths are rooted at +`results////`; the existing result uploader adds +the configured BOS prefix. diff --git a/lightweight-operators/ubt-mcap-pipeline/THIRD_PARTY_NOTICES.md b/lightweight-operators/ubt-mcap-pipeline/THIRD_PARTY_NOTICES.md new file mode 100644 index 00000000..a14f4f5c --- /dev/null +++ b/lightweight-operators/ubt-mcap-pipeline/THIRD_PARTY_NOTICES.md @@ -0,0 +1,21 @@ +# Third-party notices + +The files below are copied from the internal RoboOrchard project at commit +`c742b39e0821590605385ee4ddabcafb258432d8`: + +- `robo_orchard_lab/dataset/ubt_manipulation/tools/mcap_checker_ubt.py` +- `robo_orchard_lab/dataset/ubt_manipulation/tools/utils.py` +- `robo_orchard_lab/dataset/ubt_manipulation/tools/check_models.py` +- `robo_orchard_lab/dataset/ubt_manipulation/tools/check_rules.py` +- `robo_orchard_lab/dataset/ubt_manipulation/tools/check_config.py` +- `robo_orchard_lab/dataset/ubt_manipulation/tools/ubt_common.py` +- `robo_orchard_lab/dataset/ubt_manipulation/tools/templates/*` + +Copyright (c) 2024-2025 Horizon Robotics. All Rights Reserved. + +Licensed under the Apache License, Version 2.0. A copy of the license is +available at . + +The copied Python files retain their original copyright and license headers. +The vendored checker has one local compatibility patch: rendered resize +dimensions are explicitly converted to integers before calling OpenCV. diff --git a/lightweight-operators/ubt-mcap-pipeline/image-manifest.json b/lightweight-operators/ubt-mcap-pipeline/image-manifest.json new file mode 100644 index 00000000..f793ab01 --- /dev/null +++ b/lightweight-operators/ubt-mcap-pipeline/image-manifest.json @@ -0,0 +1,9 @@ +{ + "items": [ + { + "operatorId": "operator-custom-ubt-mcap-runtime", + "operatorSlug": "ubt_mcap_runtime", + "dockerfile": "operator-images/lightweight/ubt-mcap-runtime/Dockerfile" + } + ] +} diff --git a/lightweight-operators/ubt-mcap-pipeline/main.py b/lightweight-operators/ubt-mcap-pipeline/main.py new file mode 100644 index 00000000..c8a68d64 --- /dev/null +++ b/lightweight-operators/ubt-mcap-pipeline/main.py @@ -0,0 +1,26 @@ +#!/usr/bin/env python3 +from __future__ import annotations + +import argparse +import logging + +from ubt_pipeline.check import run_check +from ubt_pipeline.prepare import run_prepare +from ubt_pipeline.report import run_report + + +def main() -> None: + parser = argparse.ArgumentParser(description="Drobodata UBT MCAP pipeline") + parser.add_argument("stage", choices=("prepare", "check", "report")) + args = parser.parse_args() + logging.basicConfig( + level=logging.INFO, + format="%(asctime)s %(levelname)s %(name)s: %(message)s", + ) + {"prepare": run_prepare, "check": run_check, "report": run_report}[ + args.stage + ]() + + +if __name__ == "__main__": + main() diff --git a/lightweight-operators/ubt-mcap-pipeline/operators.template.json b/lightweight-operators/ubt-mcap-pipeline/operators.template.json new file mode 100644 index 00000000..ab4d9d23 --- /dev/null +++ b/lightweight-operators/ubt-mcap-pipeline/operators.template.json @@ -0,0 +1,201 @@ +[ + { + "name": "UBT 数据准备", + "type": "custom", + "category": "ROBOTICS", + "description": "从 BOS 对象清单安全下载并解压 UBT 数据,生成可复用的 fingerprint 缓存与 prepared manifest。", + "version": "v1.0.0", + "image": "__IMAGE_DIGEST__", + "imageId": "__IMAGE_DIGEST__", + "containerCommand": [ + "python", + "/opt/droboticflow/lightweight/operator/main.py" + ], + "containerArgs": [ + "prepare" + ], + "inputs": [ + { + "id": "source-objects", + "name": "BOS 对象", + "type": "file", + "transmission": "bus" + } + ], + "outputs": [ + { + "id": "prepared-manifest", + "name": "准备清单", + "type": "json", + "transmission": "bus" + } + ], + "hyperparams": [ + {"key": "DATASET_ID", "value": "ubt-poc-2026-06"}, + {"key": "USER_NAME", "value": "ubt"}, + {"key": "TASK_NAME", "value": "test_task"}, + {"key": "ARCHIVE_GLOB", "value": "*.zip"}, + { + "key": "URDF_SOURCE_PATH", + "value": "bos://testxyz/projects/ubt-mcap-quality/raw/zbxzh060101_holobrain_train.urdf" + }, + {"key": "REUSE_PREPARED", "value": "true"}, + {"key": "MIN_FREE_GIB", "value": "50"}, + {"key": "ON_DUPLICATE_EPISODE", "value": "error"} + ], + "resources": { + "cpuRequest": "1", + "cpuLimit": "2", + "memoryRequest": "2Gi", + "memoryLimit": "4Gi" + }, + "runtimeContract": { + "lightweightOperator": { + "enabled": true, + "codeSubPath": "lightweight-operators/ubt-mcap-pipeline", + "mountPath": "/opt/droboticflow/lightweight/operator", + "codeReadOnly": true, + "mounts": [ + { + "name": "prepared-data", + "pvcName": "droboticflow-workspace", + "subPath": "datasets/ubt-prepared", + "mountPath": "/data/ubt-prepared", + "readOnly": false, + "ideEnabled": true + } + ] + } + }, + "skillDefinition": "安全下载并解压 UBT zip 和 URDF。开始前检查共享空间,拒绝目录穿越、重复 episode、缺失 MCAP、非法目录和无效 URDF。按对象 key/size/ETag 与 URDF SHA-256 生成 fingerprint;完整缓存复用,残缺缓存重建。" + }, + { + "name": "UBT MCAP 批量质检", + "type": "custom", + "category": "ROBOTICS", + "description": "使用固定版本的 UBT Checker 对全部匹配 episode 执行 MCAP、topic、时间、图像、关节、夹爪、FK 与末端姿态检查。", + "version": "v1.0.0", + "image": "__IMAGE_DIGEST__", + "imageId": "__IMAGE_DIGEST__", + "containerCommand": [ + "python", + "/opt/droboticflow/lightweight/operator/main.py" + ], + "containerArgs": [ + "check" + ], + "inputs": [ + { + "id": "prepared-manifest", + "name": "准备清单", + "type": "json", + "transmission": "bus" + } + ], + "outputs": [ + { + "id": "checker-results", + "name": "质检结果", + "type": "file", + "transmission": "bus" + } + ], + "hyperparams": [ + {"key": "DATE_PREFIXES", "value": "2026_06_16,2026_06_17"}, + {"key": "USER_NAMES", "value": "ubt"}, + {"key": "TASK_NAMES", "value": "test_task"}, + {"key": "NUM_WORKERS", "value": "1"}, + {"key": "MAX_EPISODES", "value": "1"}, + {"key": "INSPECT_CONFIG_JSON", "value": ""}, + {"key": "VIDEO_EXPORT", "value": "false"}, + {"key": "VIS_INTERVAL", "value": "3"}, + {"key": "VIS_RESIZE_SCALE", "value": "1.0"}, + {"key": "VIS_DEPTH", "value": "true"}, + {"key": "FAIL_ON_QUALITY_ERROR", "value": "false"} + ], + "resources": { + "cpuRequest": "2", + "cpuLimit": "4", + "memoryRequest": "8Gi", + "memoryLimit": "16Gi", + "sharedMemorySize": "2Gi" + }, + "runtimeContract": { + "lightweightOperator": { + "enabled": true, + "codeSubPath": "lightweight-operators/ubt-mcap-pipeline", + "mountPath": "/opt/droboticflow/lightweight/operator", + "codeReadOnly": true, + "mounts": [ + { + "name": "prepared-data", + "pvcName": "droboticflow-workspace", + "subPath": "datasets/ubt-prepared", + "mountPath": "/data/ubt-prepared", + "readOnly": true, + "ideEnabled": true + } + ] + } + }, + "skillDefinition": "读取 prepared manifest,日期支持 YYYY-MM-DD 或 YYYY_MM_DD 及逗号分隔。默认处理全部匹配 episode 且不导出视频。质量错误记录为 ERROR,不提前终止;运行异常记录为 RUNTIME_ERROR。" + }, + { + "name": "UBT 结果汇总", + "type": "custom", + "category": "ROBOTICS", + "description": "汇总 UBT episode 结果,输出 JSON、CSV、pass/fail manifest、规则统计、自包含 HTML 与 Checker 原始报告。", + "version": "v1.0.0", + "image": "__IMAGE_DIGEST__", + "imageId": "__IMAGE_DIGEST__", + "containerCommand": [ + "python", + "/opt/droboticflow/lightweight/operator/main.py" + ], + "containerArgs": [ + "report" + ], + "inputs": [ + { + "id": "checker-results", + "name": "质检结果", + "type": "file", + "transmission": "bus" + } + ], + "outputs": [ + { + "id": "quality-bundle", + "name": "质检结果包", + "type": "file", + "transmission": "bus" + } + ], + "hyperparams": [], + "resources": { + "cpuRequest": "500m", + "cpuLimit": "1", + "memoryRequest": "512Mi", + "memoryLimit": "2Gi" + }, + "runtimeContract": { + "lightweightOperator": { + "enabled": true, + "codeSubPath": "lightweight-operators/ubt-mcap-pipeline", + "mountPath": "/opt/droboticflow/lightweight/operator", + "codeReadOnly": true, + "mounts": [ + { + "name": "prepared-data", + "pvcName": "droboticflow-workspace", + "subPath": "datasets/ubt-prepared", + "mountPath": "/data/ubt-prepared", + "readOnly": true, + "ideEnabled": true + } + ] + } + }, + "skillDefinition": "生成稳定的 PASS、ERROR、RUNTIME_ERROR 汇总,按日期和规则统计。pass/fail manifest 仅保留原始 BOS 路径与 prepared 路径,不复制 episode 数据。" + } +] diff --git a/lightweight-operators/ubt-mcap-pipeline/requirements.txt b/lightweight-operators/ubt-mcap-pipeline/requirements.txt new file mode 100644 index 00000000..7f2026ab --- /dev/null +++ b/lightweight-operators/ubt-mcap-pipeline/requirements.txt @@ -0,0 +1,9 @@ +bce-python-sdk==0.9.46 +matplotlib==3.10.3 +mcap==1.4.0 +mcap-ros2-support==0.5.7 +numpy==1.26.4 +opencv-python-headless==4.10.0.84 +pytorch-kinematics==0.10.0 +scipy==1.15.3 +torch==2.4.1+cpu diff --git a/lightweight-operators/ubt-mcap-pipeline/robo_orchard_lab/__init__.py b/lightweight-operators/ubt-mcap-pipeline/robo_orchard_lab/__init__.py new file mode 100644 index 00000000..aae50cf5 --- /dev/null +++ b/lightweight-operators/ubt-mcap-pipeline/robo_orchard_lab/__init__.py @@ -0,0 +1 @@ +"""Minimal Apache-2.0 RoboOrchard namespace vendored for the UBT checker.""" diff --git a/lightweight-operators/ubt-mcap-pipeline/robo_orchard_lab/dataset/__init__.py b/lightweight-operators/ubt-mcap-pipeline/robo_orchard_lab/dataset/__init__.py new file mode 100644 index 00000000..1a0bab48 --- /dev/null +++ b/lightweight-operators/ubt-mcap-pipeline/robo_orchard_lab/dataset/__init__.py @@ -0,0 +1 @@ +"""Dataset tools namespace.""" diff --git a/lightweight-operators/ubt-mcap-pipeline/robo_orchard_lab/dataset/ubt_manipulation/__init__.py b/lightweight-operators/ubt-mcap-pipeline/robo_orchard_lab/dataset/ubt_manipulation/__init__.py new file mode 100644 index 00000000..98525bcf --- /dev/null +++ b/lightweight-operators/ubt-mcap-pipeline/robo_orchard_lab/dataset/ubt_manipulation/__init__.py @@ -0,0 +1 @@ +"""UBT manipulation dataset tools.""" diff --git a/lightweight-operators/ubt-mcap-pipeline/robo_orchard_lab/dataset/ubt_manipulation/tools/__init__.py b/lightweight-operators/ubt-mcap-pipeline/robo_orchard_lab/dataset/ubt_manipulation/tools/__init__.py new file mode 100644 index 00000000..c3a4e839 --- /dev/null +++ b/lightweight-operators/ubt-mcap-pipeline/robo_orchard_lab/dataset/ubt_manipulation/tools/__init__.py @@ -0,0 +1 @@ +"""UBT MCAP checker tools.""" diff --git a/lightweight-operators/ubt-mcap-pipeline/robo_orchard_lab/dataset/ubt_manipulation/tools/check_config.py b/lightweight-operators/ubt-mcap-pipeline/robo_orchard_lab/dataset/ubt_manipulation/tools/check_config.py new file mode 100644 index 00000000..4e1973dd --- /dev/null +++ b/lightweight-operators/ubt-mcap-pipeline/robo_orchard_lab/dataset/ubt_manipulation/tools/check_config.py @@ -0,0 +1,71 @@ +# Project RoboOrchard +# +# Copyright (c) 2024-2025 Horizon Robotics. All Rights Reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or +# implied. See the License for the specific language governing +# permissions and limitations under the License. + +from __future__ import annotations +import json +from dataclasses import asdict, dataclass +from pathlib import Path + + +@dataclass(frozen=True) +class InspectConfig: + """Thresholds used by rule-based MCAP inspection. + + This dataclass centralizes the default thresholds shared by JSON config + loading, rule evaluation, and validation reporting. + """ + + camera_topics_mean_fps_limit: float = 25.0 + camera_topics_min_fps_limit: float = 5.0 + robot_state_topics_mean_fps_limit: float = 180.0 + robot_state_topics_min_fps_limit: float = 50.0 + timestamp_limit: float = 0.5 + alignment_time_diff_limit: float = 0.5 + joint_limit_tolerance: float = 0.1 + joint_change_tolerance: float = 0.1 + master_slave_joint_tolerance: float = 0.1 + ee_pose_position_tolerance: float = 0.02 + ee_pose_orientation_tolerance: float = 0.05 + gripper_state_discrete_tolerance: float = 0.05 + gripper_position_range_tolerance: float = 0.0 + + +def build_default_inspect_config() -> InspectConfig: + """Build the default inspection thresholds. + + Returns: + InspectConfig: Default inspection thresholds. + """ + + return InspectConfig() + + +def load_inspect_config(config_path: str | None = None) -> InspectConfig: + """Load inspection thresholds from JSON and merge them with defaults. + + Args: + config_path: Optional JSON file path that overrides default thresholds. + + Returns: + InspectConfig: Effective inspection thresholds. + """ + + if not config_path: + return build_default_inspect_config() + payload = json.loads(Path(config_path).read_text(encoding="utf-8")) + default_payload = asdict(build_default_inspect_config()) + default_payload.update(payload) + return InspectConfig(**default_payload) diff --git a/lightweight-operators/ubt-mcap-pipeline/robo_orchard_lab/dataset/ubt_manipulation/tools/check_models.py b/lightweight-operators/ubt-mcap-pipeline/robo_orchard_lab/dataset/ubt_manipulation/tools/check_models.py new file mode 100644 index 00000000..335b611b --- /dev/null +++ b/lightweight-operators/ubt-mcap-pipeline/robo_orchard_lab/dataset/ubt_manipulation/tools/check_models.py @@ -0,0 +1,133 @@ +# Project RoboOrchard +# +# Copyright (c) 2024-2025 Horizon Robotics. All Rights Reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or +# implied. See the License for the specific language governing +# permissions and limitations under the License. + +from __future__ import annotations +from dataclasses import dataclass, field +from typing import Any + +import numpy as np + + +@dataclass +class EpisodeData: + """Normalized episode payload consumed by rule evaluation. + + This dataclass keeps the normalized arrays, topic statistics, camera + payloads, and parse warnings consumed by rule evaluation and reporting. + """ + + uuid: str + meta: dict[str, Any] + topic_counts: dict[str, int] + topic_frequencies: dict[str, float | Any] + base_time: np.ndarray + joint_positions: np.ndarray + master_joint_positions: np.ndarray + fk_ee_poses: np.ndarray + recorded_ee_poses: np.ndarray | None + images: dict[str, dict[str, Any]] + depths: dict[str, dict[str, Any]] + required_topics: list[str] + observed_topics: set[str] = field(default_factory=set) + joint_limit_names: list[str] = field(default_factory=list) + joint_lower_limits: np.ndarray | None = None + joint_upper_limits: np.ndarray | None = None + parse_warnings: list[str] = field(default_factory=list) + cam_names: list[str] = field(default_factory=list) + extrinsic: dict[str, np.ndarray] = field(default_factory=dict) + intrinsic: dict[str, np.ndarray] = field(default_factory=dict) + topic_summaries: dict[str, dict[str, Any]] = field(default_factory=dict) + alignment_time_diff_stats: dict[str, dict[str, float]] = field( + default_factory=dict + ) + static_filter_applied: bool = False + + +@dataclass(frozen=True) +class RuleResult: + """Structured result for one inspection rule. + + It captures the stable rule id, status values such as ``pass`` or + ``warning``, the rule severity, a human-readable message, and optional + metrics. + """ + + rule_id: str + status: str + severity: str + message: str + metrics: dict[str, Any] = field(default_factory=dict) + + def to_dict(self) -> dict[str, Any]: + """Convert the rule result to a plain dictionary. + + Returns: + dict[str, Any]: Serializable rule payload. + """ + + return { + "rule_id": self.rule_id, + "status": self.status, + "severity": self.severity, + "message": self.message, + "metrics": self.metrics, + } + + +@dataclass +class EpisodeInspection: + """Aggregated rule output for one episode before rendering. + + It groups the episode status, per-rule results, render hints, and + aggregate metrics before serialization. + """ + + episode_status: str + rule_results: list[RuleResult] + render_hints: list[str] + episode_metrics: dict[str, Any] + + +@dataclass +class EpisodeReport: + """Serializable episode-level report emitted by the checker. + + It stores the serialized inspection outcome together with optional video + export and runtime error metadata. + """ + + uuid: str + episode_status: str + rule_results: list[RuleResult] + episode_metrics: dict[str, Any] + video_file: str | None + runtime_error: str | None = None + + def to_dict(self) -> dict[str, Any]: + """Convert the episode report to a plain dictionary. + + Returns: + dict[str, Any]: Serializable episode report payload. + """ + + return { + "uuid": self.uuid, + "episode_status": self.episode_status, + "rule_results": [item.to_dict() for item in self.rule_results], + "episode_metrics": self.episode_metrics, + "video_file": self.video_file, + "runtime_error": self.runtime_error, + } diff --git a/lightweight-operators/ubt-mcap-pipeline/robo_orchard_lab/dataset/ubt_manipulation/tools/check_rules.py b/lightweight-operators/ubt-mcap-pipeline/robo_orchard_lab/dataset/ubt_manipulation/tools/check_rules.py new file mode 100644 index 00000000..8c54d7e3 --- /dev/null +++ b/lightweight-operators/ubt-mcap-pipeline/robo_orchard_lab/dataset/ubt_manipulation/tools/check_rules.py @@ -0,0 +1,750 @@ +# Project RoboOrchard +# +# Copyright (c) 2024-2025 Horizon Robotics. All Rights Reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or +# implied. See the License for the specific language governing +# permissions and limitations under the License. + +from __future__ import annotations +from collections import Counter + +import numpy as np +from scipy.spatial.transform import Rotation + +from robo_orchard_lab.dataset.ubt_manipulation.tools.check_config import ( + InspectConfig, +) +from robo_orchard_lab.dataset.ubt_manipulation.tools.check_models import ( + EpisodeData, + EpisodeInspection, + RuleResult, +) +def gripper_state_joint_indices(joint_limit_names: list[str]) -> list[int]: + """Return column indices of gripper-state joints in a recorded joint vector.""" + return [ + idx + for idx, name in enumerate(joint_limit_names) + if name.endswith("_gripper_state") + ] + + +def gripper_position_joint_indices(joint_limit_names: list[str]) -> list[int]: + """Return column indices of gripper-position joints in a recorded joint vector.""" + return [ + idx + for idx, name in enumerate(joint_limit_names) + if name.endswith("_gripper_position") + ] + + +def _camera_topic(topic: str) -> bool: + """Return whether the topic belongs to a camera stream. + + Args: + topic: Topic name to classify. + + Returns: + bool: True when the topic is a camera stream. + """ + + return "/observation/cameras/" in topic + + +def _robot_state_topic(topic: str) -> bool: + """Return whether the topic belongs to a robot-state stream. + + Args: + topic: Topic name to classify. + + Returns: + bool: True when the topic is a robot-state stream. + """ + + return "/observation/robot_state/" in topic + + +def _to_scalar_frequency(value) -> float: + """Collapse scalar or windowed frequency values into one comparable mean. + + Args: + value: Scalar frequency or per-step frequency array. + + Returns: + float: Comparable mean frequency value. + """ + + if isinstance(value, np.ndarray): + if value.size == 0: + return 0.0 + finite_value = value[np.isfinite(value) & (value > 0)] + if finite_value.size == 0: + return 0.0 + return float(np.mean(finite_value)) + return float(value) if value else 0.0 + + +def _fps_rule_metrics( + freq, mean_fps_limit: float, min_fps_limit: float +) -> dict[str, float | int]: + """Build FPS-related metrics for threshold and dropped-frame checks. + + Args: + freq: Scalar or per-step frequency payload. + mean_fps_limit: Configured mean FPS lower bound for the topic. + min_fps_limit: Configured minimum FPS lower bound for the topic. + + Returns: + dict[str, float | int]: Metrics used by FPS-related rules and logs. + """ + + if isinstance(freq, np.ndarray): + if freq.size == 0: + mean_fps = 0.0 + min_fps = 0.0 + max_interval = 0.0 + exceed_count = 0 + else: + finite = np.isfinite(freq) & (freq > 0) + mean_fps = float(np.mean(freq[finite])) if finite.any() else 0.0 + min_fps = float(np.min(freq[finite])) if finite.any() else 0.0 + intervals = np.where(finite, 1.0 / freq, np.inf) + if np.isfinite(intervals).any(): + max_interval = float(np.max(intervals)) + else: + max_interval = float("inf") + interval_limit = ( + 1.0 / min_fps_limit if min_fps_limit > 0 else float("inf") + ) + exceed_count = int(np.count_nonzero(intervals > interval_limit)) + else: + mean_fps = ( + float(freq) if isinstance(freq, (int, float, np.number)) else 0.0 + ) + min_fps = mean_fps + if mean_fps > 0: + max_interval = 1.0 / mean_fps + interval_limit = ( + 1.0 / min_fps_limit if min_fps_limit > 0 else float("inf") + ) + exceed_count = 1 if max_interval > interval_limit else 0 + else: + max_interval = 0.0 + exceed_count = 0 + interval_limit = ( + 1.0 / min_fps_limit if min_fps_limit > 0 else float("inf") + ) + return { + "mean_fps": round(mean_fps, 6), + "min_fps": round(min_fps, 6), + "mean_fps_limit": round(mean_fps_limit, 6), + "min_fps_limit": round(min_fps_limit, 6), + "max_interval": round(max_interval, 6), + "max_interval_limit": round(interval_limit, 6) + if np.isfinite(interval_limit) + else float("inf"), + "max_interval_exceed_count": exceed_count, + } + + +def _timestamp_metric(actual: float, expected: float) -> dict[str, float]: + """Build a timestamp comparison payload for logging and reporting. + + Args: + actual: Observed timestamp-derived value in seconds. + expected: Reference value in seconds. + + Returns: + dict[str, float]: Timestamp comparison payload. + """ + + return { + "actual": round(actual, 6), + "expected": round(expected, 6), + "delta": round(actual - expected, 6), + } + + +def _joint_limit_violation_metrics( + joint_positions: np.ndarray, + joint_names: list[str], + joint_lower_limits: np.ndarray, + joint_upper_limits: np.ndarray, + tolerance: float, +) -> dict[str, float | int | str] | None: + """Build URDF-based joint-limit violation metrics. + + Args: + joint_positions: Recorded joint positions with shape ``[T, J]``. + joint_names: Recorded joint names aligned with ``joint_positions``. + joint_lower_limits: Lower URDF joint limits. + joint_upper_limits: Upper URDF joint limits. + tolerance: Symmetric tolerance applied around the URDF limits. + + Returns: + dict[str, float | int | str] | None: Metrics for the largest + violation, or ``None`` when all joints are within limits. + """ + + if ( + joint_lower_limits.shape != joint_upper_limits.shape + or joint_lower_limits.ndim != 1 + or joint_lower_limits.shape[0] != joint_positions.shape[1] + ): + raise ValueError( + "joint limit arrays must be 1D and aligned with joint_positions" + ) + + lower_with_tolerance = joint_lower_limits - tolerance + upper_with_tolerance = joint_upper_limits + tolerance + above_upper = joint_positions - upper_with_tolerance[None, :] + below_lower = lower_with_tolerance[None, :] - joint_positions + violation = np.maximum(above_upper, below_lower) + max_violation = float(np.max(violation)) + if max_violation <= 0: + return None + + step_index, joint_index = np.unravel_index( + int(np.argmax(violation)), violation.shape + ) + return { + "joint_name": ( + joint_names[joint_index] + if joint_index < len(joint_names) + else f"joint_{joint_index}" + ), + "joint_index": int(joint_index), + "step_index": int(step_index), + "actual": round(float(joint_positions[step_index, joint_index]), 6), + "lower_limit": round(float(lower_with_tolerance[joint_index]), 6), + "upper_limit": round(float(upper_with_tolerance[joint_index]), 6), + "max_violation": round(max_violation, 6), + } + + +def _gripper_position_range_metrics( + joint_positions: np.ndarray, + joint_names: list[str], + lower: float, + upper: float, + tolerance: float, +) -> dict[str, float | int] | None: + """Build metrics when gripper position values are outside [lower, upper].""" + + position_indices = gripper_position_joint_indices(joint_names) + if not position_indices: + return None + + pos_values = joint_positions[:, position_indices] + above = pos_values - (upper + tolerance) + below = (lower - tolerance) - pos_values + violation = np.maximum(above, below) + max_violation = float(np.max(violation)) + if max_violation <= 0: + return None + + step_index, local_index = np.unravel_index( + int(np.argmax(violation)), violation.shape + ) + joint_index = position_indices[local_index] + return { + "joint_name": ( + joint_names[joint_index] + if joint_index < len(joint_names) + else f"joint_{joint_index}" + ), + "joint_index": int(joint_index), + "step_index": int(step_index), + "actual": round(float(pos_values[step_index, local_index]), 6), + "lower_limit": round(lower - tolerance, 6), + "upper_limit": round(upper + tolerance, 6), + "max_violation": round(max_violation, 6), + } + + +def _gripper_state_discrete_metrics( + joint_positions: np.ndarray, + joint_names: list[str], + tolerance: float, +) -> dict[str, float | int] | None: + """Build metrics when gripper state values are not close to 0 or 1.""" + + state_indices = gripper_state_joint_indices(joint_names) + if not state_indices: + return None + + state_values = joint_positions[:, state_indices] + # Valid discrete states: 0 (open), 1 (closed), -1 (VR arm uninitialized). + deviation = np.minimum( + np.minimum(np.abs(state_values), np.abs(state_values - 1.0)), + np.abs(state_values + 1.0), + ) + max_deviation = float(np.max(deviation)) + if max_deviation <= tolerance: + return None + + step_index, local_index = np.unravel_index( + int(np.argmax(deviation)), deviation.shape + ) + joint_index = state_indices[local_index] + return { + "joint_name": ( + joint_names[joint_index] + if joint_index < len(joint_names) + else f"joint_{joint_index}" + ), + "joint_index": int(joint_index), + "step_index": int(step_index), + "actual": round(float(state_values[step_index, local_index]), 6), + "max_deviation": round(max_deviation, 6), + "tolerance": round(tolerance, 6), + } + + +def _max_orientation_gap( + recorded_ee_poses: np.ndarray, fk_ee_poses: np.ndarray +) -> float: + """Return the largest rotation-angle gap between recorded and FK poses. + + Args: + recorded_ee_poses: + Recorded EE poses in ``[..., x, y, z, qx, qy, qz, qw]``. + fk_ee_poses: FK EE poses in the same layout. + + Returns: + float: Maximum angle difference in radians. + """ + + recorded_rot = Rotation.from_quat( + recorded_ee_poses[..., 3:].reshape(-1, 4), scalar_first=False + ) + fk_rot = Rotation.from_quat( + fk_ee_poses[..., 3:].reshape(-1, 4), scalar_first=False + ) + return float(np.max((fk_rot.inv() * recorded_rot).magnitude())) + + +def _aggregate_rule_results( + episode_data: EpisodeData, rule_results: list[RuleResult] +) -> EpisodeInspection: + """Collapse rule hits into an episode-level status and summary metrics. + + Args: + episode_data: Normalized episode payload. + rule_results: Rule results collected for the episode. + + Returns: + EpisodeInspection: Aggregate inspection output. + """ + + blocking_failed = any( + item.status == "fail" and item.severity == "blocking" + for item in rule_results + ) + has_signal = any( + item.status in {"warning", "fail"} for item in rule_results + ) + if blocking_failed: + episode_status = "fail" + elif has_signal: + episode_status = "warning" + else: + episode_status = "pass" + + render_hints = [] + for item in rule_results: + if item.status == "pass": + continue + metric_text = ", ".join( + f"{key}={value}" for key, value in item.metrics.items() + ) + hint = f"{item.status}: {item.rule_id}" + if metric_text: + hint = f"{hint} ({metric_text})" + render_hints.append(hint) + render_hints = render_hints[:3] + status_counts = Counter(item.status for item in rule_results) + episode_metrics = { + "rule_hit_count": sum(item.status != "pass" for item in rule_results), + "parse_warning_count": len(episode_data.parse_warnings), + "warning_rule_count": status_counts.get("warning", 0), + "failed_rule_count": status_counts.get("fail", 0), + } + return EpisodeInspection( + episode_status=episode_status, + rule_results=rule_results, + render_hints=render_hints, + episode_metrics=episode_metrics, + ) + + +def evaluate_episode_rules( + episode_data: EpisodeData, config: InspectConfig +) -> EpisodeInspection: + """Run the configured rule set on one normalized episode. + + Args: + episode_data: Normalized episode payload. + config: Inspection thresholds and tolerances. + + Returns: + EpisodeInspection: Aggregate inspection result for the episode. + """ + + rule_results: list[RuleResult] = [] + + observed_topics = episode_data.observed_topics or set( + episode_data.topic_counts + ) + missing_topics = [ + topic + for topic in episode_data.required_topics + if topic not in observed_topics + ] + if missing_topics: + rule_results.append( + RuleResult( + rule_id="missing_topic", + status="fail", + severity="blocking", + message="required topics are missing", + metrics={"missing_topics": ",".join(sorted(missing_topics))}, + ) + ) + empty_topics = [ + topic + for topic in episode_data.required_topics + if topic in observed_topics + and episode_data.topic_counts.get(topic, 0) <= 0 + ] + if empty_topics: + rule_results.append( + RuleResult( + rule_id="empty_stream", + status="fail", + severity="blocking", + message="required topics have no usable messages", + metrics={"empty_topics": ",".join(sorted(empty_topics))}, + ) + ) + + if episode_data.base_time.size > 1: + deltas = np.diff(episode_data.base_time) + if np.any(deltas < -config.timestamp_limit): + rule_results.append( + RuleResult( + rule_id="timestamp_non_monotonic", + status="fail", + severity="blocking", + message="base timestamps are not strictly increasing", + metrics={"min_delta": float(np.min(deltas))}, + ) + ) + + fps_failures: dict[str, dict[str, float | int]] = {} + interval_failures: dict[str, dict[str, float | int]] = {} + for topic, fps in episode_data.topic_frequencies.items(): + if _camera_topic(topic): + mean_fps_limit = config.camera_topics_mean_fps_limit + min_fps_limit = config.camera_topics_min_fps_limit + elif _robot_state_topic(topic): + mean_fps_limit = config.robot_state_topics_mean_fps_limit + min_fps_limit = config.robot_state_topics_min_fps_limit + else: + continue + metrics = _fps_rule_metrics(fps, mean_fps_limit, min_fps_limit) + if float(metrics.get("mean_fps", 0.0)) < mean_fps_limit: + fps_failures[topic] = metrics + elif float(metrics.get("min_fps", 0.0)) < min_fps_limit: + interval_failures[topic] = { + "mean_fps": float(metrics["mean_fps"]), + "mean_fps_limit": float(metrics["mean_fps_limit"]), + "min_fps": float(metrics["min_fps"]), + "min_fps_limit": float(metrics["min_fps_limit"]), + } + if fps_failures: + rule_results.append( + RuleResult( + rule_id="fps_out_of_range", + status="fail", + severity="blocking", + message="stream fps is below configured threshold", + metrics=fps_failures, + ) + ) + if interval_failures: + rule_results.append( + RuleResult( + rule_id="interval_spike_or_drop_frame", + status="fail", + severity="blocking", + message="stream min fps is below configured threshold", + metrics=interval_failures, + ) + ) + if ( + not episode_data.static_filter_applied + and episode_data.base_time.size + and episode_data.topic_summaries + ): + base_start = float(episode_data.base_time[0]) + base_end = float(episode_data.base_time[-1]) + base_duration = base_end - base_start + start_mismatches: dict[str, dict[str, float]] = {} + end_mismatches: dict[str, dict[str, float]] = {} + duration_mismatches: dict[str, dict[str, float]] = {} + + for topic, count in episode_data.topic_counts.items(): + if count <= 0: + continue + summary = episode_data.topic_summaries.get(topic) + if not summary: + continue + start_time_ns = summary.get("start_time_ns") + end_time_ns = summary.get("end_time_ns") + if start_time_ns is None or end_time_ns is None: + continue + actual_start = float(start_time_ns) / 1e9 + actual_end = float(end_time_ns) / 1e9 + actual_duration = actual_end - actual_start + + if abs(actual_start - base_start) > config.timestamp_limit: + start_mismatches[topic] = _timestamp_metric( + actual_start, base_start + ) + if abs(actual_end - base_end) > config.timestamp_limit: + end_mismatches[topic] = _timestamp_metric(actual_end, base_end) + if abs(actual_duration - base_duration) > config.timestamp_limit: + duration_mismatches[topic] = _timestamp_metric( + actual_duration, base_duration + ) + + if start_mismatches: + rule_results.append( + RuleResult( + rule_id="start_ts_mismatch", + status="warning", + severity="non_blocking", + message=( + "stream start timestamps differ from base timeline" + ), + metrics=start_mismatches, + ) + ) + if end_mismatches: + rule_results.append( + RuleResult( + rule_id="end_ts_mismatch", + status="warning", + severity="non_blocking", + message="stream end timestamps differ from base timeline", + metrics=end_mismatches, + ) + ) + if duration_mismatches: + rule_results.append( + RuleResult( + rule_id="duration_mismatch", + status="warning", + severity="non_blocking", + message="stream durations differ from base timeline", + metrics=duration_mismatches, + ) + ) + + if not episode_data.static_filter_applied: + alignment_mismatches: dict[str, dict[str, float]] = {} + for topic, stats in episode_data.alignment_time_diff_stats.items(): + max_time_diff = float(stats.get("max_time_diff", 0.0)) + if max_time_diff <= config.alignment_time_diff_limit: + continue + alignment_mismatches[topic] = { + "max_time_diff": round(max_time_diff, 6), + "mean_time_diff": round( + float(stats.get("mean_time_diff", 0.0)), 6 + ), + "limit": round(config.alignment_time_diff_limit, 6), + } + if alignment_mismatches: + rule_results.append( + RuleResult( + rule_id="alignment_time_diff_out_of_range", + status="warning", + severity="non_blocking", + message=( + "aligned timestamps deviate too much from base " + "timeline" + ), + metrics=alignment_mismatches, + ) + ) + + if episode_data.joint_positions.size: + if ( + episode_data.joint_lower_limits is not None + and episode_data.joint_upper_limits is not None + ): + joint_limit_metrics = _joint_limit_violation_metrics( + joint_positions=episode_data.joint_positions, + joint_names=episode_data.joint_limit_names, + joint_lower_limits=np.asarray( + episode_data.joint_lower_limits, dtype=np.float64 + ), + joint_upper_limits=np.asarray( + episode_data.joint_upper_limits, dtype=np.float64 + ), + tolerance=config.joint_limit_tolerance, + ) + else: + joint_limit_metrics = None + if joint_limit_metrics is not None: + rule_results.append( + RuleResult( + rule_id="joint_limit_violation", + status="fail", + severity="blocking", + message="joint position exceeds URDF joint limit", + metrics=joint_limit_metrics, + ) + ) + gripper_state_metrics = _gripper_state_discrete_metrics( + joint_positions=episode_data.joint_positions, + joint_names=episode_data.joint_limit_names, + tolerance=config.gripper_state_discrete_tolerance, + ) + if gripper_state_metrics is not None: + rule_results.append( + RuleResult( + rule_id="gripper_state_not_discrete", + status="warning", + severity="non_blocking", + message="gripper state is not close to 0 or 1", + metrics=gripper_state_metrics, + ) + ) + gripper_pos_metrics = _gripper_position_range_metrics( + joint_positions=episode_data.joint_positions, + joint_names=episode_data.joint_limit_names, + lower=0.0, + upper=1.0, + tolerance=config.gripper_position_range_tolerance, + ) + if gripper_pos_metrics is not None: + rule_results.append( + RuleResult( + rule_id="gripper_position_out_of_range", + status="fail", + severity="blocking", + message="gripper position is outside [0, 1] range", + metrics=gripper_pos_metrics, + ) + ) + if episode_data.joint_positions.shape[0] > 1: + jump_indices = [ + idx + for idx, name in enumerate(episode_data.joint_limit_names) + if not name.endswith("_gripper_state") + ] + if jump_indices: + joint_delta = np.abs( + np.diff( + episode_data.joint_positions[:, jump_indices], + axis=0, + ) + ) + max_joint_delta = float(np.max(joint_delta)) + else: + max_joint_delta = float( + np.max( + np.abs(np.diff(episode_data.joint_positions, axis=0)) + ) + ) + if max_joint_delta > config.joint_change_tolerance: + rule_results.append( + RuleResult( + rule_id="joint_jump_violation", + status="warning", + severity="non_blocking", + message="adjacent joint delta exceeds tolerance", + metrics={"max_joint_delta": round(max_joint_delta, 6)}, + ) + ) + + if ( + episode_data.master_joint_positions.size + and episode_data.joint_positions.size + and episode_data.master_joint_positions.shape + == episode_data.joint_positions.shape + ): + # UBT master arm joints are VR-empty; only gripper width is comparable. + compare_indices = gripper_position_joint_indices( + episode_data.joint_limit_names + ) + if compare_indices: + master_values = episode_data.master_joint_positions[ + :, compare_indices + ] + slave_values = episode_data.joint_positions[:, compare_indices] + else: + master_values = episode_data.master_joint_positions + slave_values = episode_data.joint_positions + max_gap = float(np.max(np.abs(master_values - slave_values))) + if max_gap > config.master_slave_joint_tolerance: + rule_results.append( + RuleResult( + rule_id="master_slave_joint_gap", + status="warning", + severity="non_blocking", + message="master and follower joints diverge", + metrics={ + "max_gap": round(max_gap, 6), + "compared_joint_indices": compare_indices or "all", + }, + ) + ) + + if ( + episode_data.recorded_ee_poses is not None + and episode_data.recorded_ee_poses.size + and episode_data.fk_ee_poses.size + and episode_data.recorded_ee_poses.shape + == episode_data.fk_ee_poses.shape + ): + position_gap = float( + np.max( + np.abs( + episode_data.recorded_ee_poses[..., :3] + - episode_data.fk_ee_poses[..., :3] + ) + ) + ) + orientation_gap = _max_orientation_gap( + episode_data.recorded_ee_poses, + episode_data.fk_ee_poses, + ) + if ( + position_gap > config.ee_pose_position_tolerance + or orientation_gap > config.ee_pose_orientation_tolerance + ): + rule_results.append( + RuleResult( + rule_id="fk_ee_pose_mismatch", + status="warning", + severity="non_blocking", + message="recorded EE pose differs from FK result", + metrics={ + "position_gap": round(position_gap, 6), + "orientation_gap": round(orientation_gap, 6), + }, + ) + ) + + return _aggregate_rule_results(episode_data, rule_results) diff --git a/lightweight-operators/ubt-mcap-pipeline/robo_orchard_lab/dataset/ubt_manipulation/tools/mcap_checker_ubt.py b/lightweight-operators/ubt-mcap-pipeline/robo_orchard_lab/dataset/ubt_manipulation/tools/mcap_checker_ubt.py new file mode 100644 index 00000000..e829f11a --- /dev/null +++ b/lightweight-operators/ubt-mcap-pipeline/robo_orchard_lab/dataset/ubt_manipulation/tools/mcap_checker_ubt.py @@ -0,0 +1,1619 @@ +# Project RoboOrchard +# +# Copyright (c) 2024-2025 Horizon Robotics. All Rights Reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or +# implied. See the License for the specific language governing +# permissions and limitations under the License. + +"""MCAP checker for UBT dual 7-axis arm robots. + +Standalone implementation – does NOT inherit from PiperMcapChecker. + +All episode-discovery, rule-evaluation, video-rendering, logging, and +multiprocessing orchestration logic is implemented directly in +:class:`UbtMcapChecker`. The Piper MCAP checker is no longer required. + +External dependencies that are still imported from ``horizon_manipulation``: +* ``tools.utils`` – format_time, sample, get_frequency, pose_to_mat, and + all reporting/video utilities used by the orchestration + layer. +* ``tools.check_models`` – EpisodeData, EpisodeReport dataclasses. +* ``tools.check_rules`` – evaluate_episode_rules. +* ``utils`` – decode_depth, decode_img, depth_visualize. + +``check_config`` is provided by the local copy in this package. + +UBT differs from Piper mainly inside ``parse``: + +* each arm is recorded as 9 values: 7 arm joints + gripper position + state; +* the URDF exposes 7 actuated joints per arm (no symmetric gripper pair); +* end-effector links, camera parent links and ``tf_static`` frame names use + the UBT naming convention; +* recorded EE poses are expressed in the ``prismatic_link`` frame and must be + converted into the FK base frame before projection/inspection. +""" + +import copy +import glob +import json +import logging +import multiprocessing +import os +import subprocess +from typing import Any + +import cv2 +import numpy as np +import pytorch_kinematics as pk +from mcap.reader import make_reader +from mcap_ros2.decoder import DecoderFactory +from scipy.spatial.transform import Rotation + +import robo_orchard_lab.dataset.ubt_manipulation.tools.utils as _utils +from robo_orchard_lab.dataset.ubt_manipulation.tools.check_models import ( + EpisodeData, + EpisodeReport, +) +from robo_orchard_lab.dataset.ubt_manipulation.tools.check_rules import ( + evaluate_episode_rules, +) +from robo_orchard_lab.dataset.ubt_manipulation.tools.utils import ( + FfmpegVideoWriter, + build_job_summary, + build_report_row, + concat_videos, + count_topics, + decode_depth, + decode_img, + depth_visualize, + format_numeric_detail_table, + format_rule_detail_lines, + format_text_detail_table, + format_time, + format_timestamp, + format_topic_summary_line, + get_frequency, + iter_logged_topics, + iter_rule_results, + pose_to_mat, + sample, + topic_sort_key, + write_error_logs, + write_full_log_episode, + write_manual_review_artifacts, + write_mcap_lists, + write_rule_block, +) +from robo_orchard_lab.dataset.ubt_manipulation.tools.check_config import ( + build_default_inspect_config, + load_inspect_config, +) +from robo_orchard_lab.dataset.ubt_manipulation.tools.ubt_common import ( + RECORDED_JOINT_DIM_PER_ARM, + UBT_FRAME_ID_EXTRINSIC_MAP, + UbtKinematicsMixin, + get_slave_joint_topics, + reshape_joint_component, + zero_gripper_velocity_effort, +) + +logger = logging.getLogger(__name__) + +# --------------------------------------------------------------------------- +# Subprocess / h264-encoder proxy (mirrors mcap_checker.py module-level code +# so that FfmpegVideoWriter uses the correct encoder and subprocess handle). +# --------------------------------------------------------------------------- + +_default_get_h264_encoder = _utils.get_h264_encoder + + +def get_h264_encoder() -> str: + return _default_get_h264_encoder() + + +def _popen_proxy(*args, **kwargs): + return subprocess.Popen(*args, **kwargs) + + +def _run_proxy(*args, **kwargs): + return subprocess.run(*args, **kwargs) + + +class _SubprocessProxy: + PIPE = subprocess.PIPE + DEVNULL = subprocess.DEVNULL + Popen = staticmethod(_popen_proxy) + run = staticmethod(_run_proxy) + + +_utils.get_h264_encoder = lambda: get_h264_encoder() +_utils.subprocess = _SubprocessProxy() + + +class UbtMcapChecker(UbtKinematicsMixin): + """Checker for UBT dual 7-axis arm MCAP episodes. + + Standalone implementation that does not inherit from PiperMcapChecker. + All orchestration (episode discovery, rule evaluation, multiprocessing, + video export, report generation) is implemented directly in this class. + + UBT records each arm as 9 values: 7 arm joints, gripper position (idx 7), + and gripper state bool (idx 8). FK only consumes the first 7 arm values. + """ + + recorded_joint_dim = RECORDED_JOINT_DIM_PER_ARM + + def __init__( + self, + input_path, + output_path, + urdf, + calibration_dict=None, + task_names=None, + user_names=None, + static_threshold: float = 0, + head_time_to_filter: float = 1e8, + tile_time_to_filter: float = None, + date_prefix: str = None, + num_workers: int = 4, + vis_interval: int = 3, + vis_fps: int = 150, + vis_resize_scale: float = 1, + vis_depth: bool = True, + inspect_config=None, + inspect_config_path: str | None = None, + enable_ffmpeg_log: bool = False, + skip_video_export: bool = False, + **kwargs, + ): + self.input_path = input_path + self.output_path = output_path + self.calibration_dict = self.calibration_process(calibration_dict) + self.task_names = task_names + self.user_names = user_names + self.chain = pk.build_chain_from_urdf(open(urdf, "rb").read()) + self.static_threshold = static_threshold + self.head_time_to_filter = head_time_to_filter + self.tile_time_to_filter = tile_time_to_filter + if date_prefix is not None: + self.date_prefix = date_prefix.strip().split(",") + else: + self.date_prefix = date_prefix + self.episodes = self.input_path_handler(input_path) + self.num_workers = num_workers + self.vis_interval = vis_interval + self.vis_fps = vis_fps + self.vis_resize_scale = vis_resize_scale + self.vis_depth = vis_depth + if inspect_config is None: + inspect_config = load_inspect_config(inspect_config_path) + self.inspect_config = inspect_config + self.inspect_config_path = inspect_config_path + self.enable_ffmpeg_log = enable_ffmpeg_log + self.skip_video_export = skip_video_export + + # ------------------------------------------------------------------ + # Episode discovery + # ------------------------------------------------------------------ + + def calibration_process(self, calibration_dict): + """Normalize camera extrinsics into camera-to-world matrices.""" + + if calibration_dict is None: + return None + for camera, calib in calibration_dict.items(): + calibration_dict[camera] = np.linalg.inv(pose_to_mat(calib)) + return calibration_dict + + def input_path_handler(self, input_paths): + """Expand configured roots into a sorted list of candidate episodes.""" + + episodes = [] + input_paths = input_paths.strip().split(",") + logger.info(f"input_paths: {input_paths}") + for input_path in input_paths: + for user in os.listdir(input_path): + if self.user_names is not None and user not in self.user_names: + continue + + user_path = os.path.join(input_path, user) + for task in os.listdir(user_path): + if ( + self.task_names is not None + and task not in self.task_names + ): + continue + task_path = os.path.join(user_path, task) + for ep in os.listdir(task_path): + if not ep.startswith("episode_"): + logger.warning(f"invalid episode dir name {ep}") + continue + path = os.path.join(task_path, ep) + time = ep.replace("episode_", "") + + if self.date_prefix is not None and not any( + [time.startswith(x) for x in self.date_prefix] + ): + continue + episodes.append([path, user, task, time]) + episodes.sort() + logger.info(f"number of valid episodes: {len(episodes)}") + return episodes + + # ------------------------------------------------------------------ + # Topic / frame layout + # ------------------------------------------------------------------ + + def get_joint_topics(self) -> dict[str, str]: + """Return logical joint stream names (UBT checker does not read master).""" + + return get_slave_joint_topics() + + def get_frame_id_extrinsic_map(self) -> dict[str, str]: + """Return UBT static TF frame pairs mapped to logical cameras.""" + + return dict(UBT_FRAME_ID_EXTRINSIC_MAP) + + def normalize_recorded_ee_poses( + self, + recorded_ee_poses: np.ndarray | None, + camera_parent_poses: dict[str, np.ndarray | None], + ) -> np.ndarray | None: + """Convert UBT recorded EE poses from prismatic frame to base frame.""" + + if recorded_ee_poses is None or recorded_ee_poses.size == 0: + return recorded_ee_poses + base_from_prismatic = camera_parent_poses.get("middle") + if base_from_prismatic is None: + return recorded_ee_poses + + output = np.empty_like(recorded_ee_poses) + for side_idx in range(recorded_ee_poses.shape[1]): + pose = recorded_ee_poses[:, side_idx] + ref_from_ee = np.repeat( + np.eye(4, dtype="float64")[None], pose.shape[0], axis=0 + ) + ref_from_ee[:, :3, 3] = pose[:, :3] + ref_from_ee[:, :3, :3] = Rotation.from_quat( + pose[:, 3:], scalar_first=False + ).as_matrix() + + base_from_ee = base_from_prismatic @ ref_from_ee + output[:, side_idx, :3] = base_from_ee[:, :3, 3] + output[:, side_idx, 3:] = Rotation.from_matrix( + base_from_ee[:, :3, :3] + ).as_quat(scalar_first=False) + return output + + def _default_joint_series(self, step_count: int) -> list[list[float]]: + """Build a zero-filled joint stream matching recorded joint width.""" + + return [[0.0] * self.recorded_joint_dim for _ in range(step_count)] + + def reshape_joint_component( + self, + values: Any, + topic: str, + field_name: str, + uuid: str, + parse_warnings: list[str], + ) -> np.ndarray: + """Normalize UBT JointState components to 7 arm + position + state.""" + + return reshape_joint_component( + values, + topic, + field_name, + uuid, + parse_warnings, + recorded_joint_dim=self.recorded_joint_dim, + ) + + # ------------------------------------------------------------------ + # Visualization + # ------------------------------------------------------------------ + + def draw_ee_pose(self, img, intrinsic, extrinsic, ee_poses): + """Project EE axes into the image for visualization output. + + Reads the quaternion in ROS ``[qx, qy, qz, qw]`` order + (``scalar_first=False``) and uses OpenCV BGR colors so the X/Y/Z + axes render as red/green/blue. + """ + + rot = Rotation.from_quat( + ee_poses[:, 3:], scalar_first=False + ).as_matrix() + trans = ee_poses[:, :3] + axis_length = 0.1 + points = np.float32( + [ + [axis_length, 0, 0], + [0, axis_length, 0], + [0, 0, axis_length], + [0, 0, 0], + ] + ) + # npose, 1, 3, 3 @ [4, 3, 3] + [npose, 1, 3] + points = (rot[:, None] @ points[..., None]).squeeze(-1) + points += trans[:, None] # npose, 4, 3 + projection_mat = intrinsic @ extrinsic + + pts_2d = points @ projection_mat[:3, :3].T + pts_2d = pts_2d + projection_mat[:3, 3] + depth = pts_2d[..., 2] + pts_2d = pts_2d[..., :2] / depth[..., None] + pts_2d = pts_2d.astype(np.int32) + + axis_colors_bgr = [ + (0, 0, 255), # +X, red in OpenCV BGR order. + (0, 255, 0), # +Y, green. + (255, 0, 0), # +Z, blue. + ] + for pose_idx in range(len(ee_poses)): + for i in range(3): + if depth[pose_idx, i] < 0.02: + continue + cv2.circle( + img, + (pts_2d[pose_idx, i, 0], pts_2d[pose_idx, i, 1]), + 6, + axis_colors_bgr[i], + -1, + ) + cv2.line( + img, + (pts_2d[pose_idx, i, 0], pts_2d[pose_idx, i, 1]), + (pts_2d[pose_idx, 3, 0], pts_2d[pose_idx, 3, 1]), + axis_colors_bgr[i], + 3, + ) + return img + + # ------------------------------------------------------------------ + # Episode parsing + # ------------------------------------------------------------------ + + def parse(self, ep_id, payload_mode="inspect"): + """Parse one UBT episode and build the raw inspection payload. + + Args: + ep_id: Episode index in ``self.episodes``. + payload_mode: ``inspect`` omits raw visual payloads, while + ``render`` keeps them for video export. + + Returns: + dict: Raw episode payload used by inspection and rendering. + """ + keep_visual_payload = payload_mode == "render" + + cameras = ["left", "middle", "right"] + image_topics = [ + f"/observation/cameras/{x}/color_image/image_raw" for x in cameras + ] + image_intrinsic_topics = [ + f"/observation/cameras/{x}/color_image/camera_info" + for x in cameras + ] + depth_topics = [ + f"/observation/cameras/{x}/depth_image/image_raw" for x in cameras + ] + depth_intrinsic_topics = [ + f"/observation/cameras/{x}/depth_image/camera_info" + for x in cameras + ] + image_extrinsic_topic = ["/tf_static"] + joint_topics = self.get_joint_topics() + left_joint_topic = joint_topics["left"] + right_joint_topic = joint_topics["right"] + left_ee_topic = "/observation/robot_state/left/end_pose" + right_ee_topic = "/observation/robot_state/right/end_pose" + + frame_id_extrinsic_map = self.get_frame_id_extrinsic_map() + + all_useful_topics = ( + image_topics + + image_intrinsic_topics + + depth_topics + + depth_intrinsic_topics + + image_extrinsic_topic + + [ + left_joint_topic, + right_joint_topic, + left_ee_topic, + right_ee_topic, + ] + ) + + ep = self.episodes[ep_id] + path, user, task_name, time = ep + + uuid = f"{task_name}/{user}/{time}" + mcap = glob.glob(f"{path}/episode_*_0.mcap") + assert len(mcap) == 1 + mcap = mcap[0] + meta = json.load(open(os.path.join(path, "episode_meta.json"), "r")) + assert user == meta["user_name"] + assert task_name == meta["task_name"] + + image_extrinsic = {} + images = {} + for t in image_topics: + images[t] = { + "time": [], + "intrinsic": None, + } + if keep_visual_payload: + images[t]["data"] = [] + image_intrinsics = {} + for t in image_intrinsic_topics: + image_intrinsics[t] = { + "time": [], + } + depths = {} + for t in depth_topics: + depths[t] = { + "time": [], + "intrinsic": None, + } + if keep_visual_payload: + depths[t]["data"] = [] + depth_intrinsics = {} + for t in depth_intrinsic_topics: + depth_intrinsics[t] = { + "time": [], + } + joints = {} + for t in [left_joint_topic, right_joint_topic]: + joints[t] = { + "position": [], + "velocity": [], + "effort": [], + "time": [], + } + ee_pose_streams = {} + for t in [left_ee_topic, right_ee_topic]: + ee_pose_streams[t] = { + "pose": [], + "time": [], + } + observed_topics: set[str] = set() + parse_warnings: list[str] = [] + topic_summaries = {} + alignment_time_diff_stats = {} + + reader = make_reader( + open(mcap, "rb"), decoder_factories=[DecoderFactory()] + ) + + for ( + _schema, + channel, + _message, + ros_msg, + ) in reader.iter_decoded_messages( + topics=all_useful_topics, log_time_order=True + ): + if hasattr(ros_msg, "header"): + time = ( + ros_msg.header.stamp.sec, + ros_msg.header.stamp.nanosec, + ) + topic = channel.topic + if topic != "/tf_static": + observed_topics.add(topic) + if topic != "/tf_static": + if hasattr(ros_msg, "header"): + summary_time_ns = ( + ros_msg.header.stamp.sec * int(1e9) + + ros_msg.header.stamp.nanosec + ) + else: + summary_time_ns = _message.log_time + summary = topic_summaries.setdefault( + topic, + { + "count": 0, + "size_bytes": 0, + "start_time_ns": None, + "end_time_ns": None, + "mean_frequency": 0.0, + "min_frequency": 0.0, + }, + ) + summary["count"] += 1 + summary["size_bytes"] += len(_message.data) + if summary["start_time_ns"] is None: + summary["start_time_ns"] = summary_time_ns + summary["end_time_ns"] = summary_time_ns + else: + summary["start_time_ns"] = min( + summary["start_time_ns"], summary_time_ns + ) + summary["end_time_ns"] = max( + summary["end_time_ns"], summary_time_ns + ) + if topic in images: + if keep_visual_payload: + images[topic]["data"].append(ros_msg.data) + images[topic]["time"].append(time) + elif topic in depths: + if keep_visual_payload: + depths[topic]["data"].append(ros_msg.data) + depths[topic]["time"].append(time) + elif topic in joints: + joints[topic]["position"].append(ros_msg.position) + joints[topic]["velocity"].append(ros_msg.velocity) + joints[topic]["effort"].append(ros_msg.effort) + joints[topic]["time"].append(time) + elif topic in ee_pose_streams: + pos = ros_msg.pose.position + rot = ros_msg.pose.orientation + x, y, z = pos.x, pos.y, pos.z + qx, qy, qz, qw = rot.x, rot.y, rot.z, rot.w + ee_pose_streams[topic]["pose"].append( + [x, y, z, qx, qy, qz, qw] + ) + ee_pose_streams[topic]["time"].append(time) + elif topic in image_intrinsic_topics: + image_intrinsics[topic]["time"].append(time) + img_topic = image_topics[image_intrinsic_topics.index(topic)] + images[img_topic]["intrinsic"] = np.array(ros_msg.p).reshape( + 3, 4 + ) + elif topic in depth_intrinsic_topics: + depth_intrinsics[topic]["time"].append(time) + dpt_topic = depth_topics[depth_intrinsic_topics.index(topic)] + depths[dpt_topic]["intrinsic"] = np.array(ros_msg.p).reshape( + 3, 4 + ) + elif topic in image_extrinsic_topic: + for tf in ros_msg.transforms: + frame_id = f"{tf.child_frame_id}/{tf.header.frame_id}" + if frame_id in frame_id_extrinsic_map: + camera_name = frame_id_extrinsic_map[frame_id] + extrinsic = np.linalg.inv(pose_to_mat(tf.transform)) + image_extrinsic[camera_name] = extrinsic + + for data in [images, depths, joints, ee_pose_streams]: + for t in data: + data[t]["time"] = format_time(data[t]["time"]) + for data in [image_intrinsics, depth_intrinsics]: + for t in data: + if data[t]["time"]: + data[t]["time"] = format_time(data[t]["time"]) + else: + data[t]["time"] = np.array([], dtype="float64") + + def _update_topic_frequency(topic: str, freq: np.ndarray) -> None: + summary = topic_summaries.setdefault( + topic, + { + "count": 0, + "size_bytes": 0, + "start_time_ns": None, + "end_time_ns": None, + "mean_frequency": 0.0, + "min_frequency": 0.0, + }, + ) + if freq.size > 0: + summary["mean_frequency"] = float(freq.mean()) + summary["min_frequency"] = float(freq.min()) + else: + summary["mean_frequency"] = 0.0 + summary["min_frequency"] = 0.0 + + raw_topic_counts = { + topic: len(payload["time"]) for topic, payload in images.items() + } + raw_topic_counts.update( + {topic: len(payload["time"]) for topic, payload in depths.items()} + ) + raw_topic_counts.update( + { + topic: len(payload["position"]) + for topic, payload in joints.items() + } + ) + raw_topic_counts.update( + { + topic: len(payload["pose"]) + for topic, payload in ee_pose_streams.items() + } + ) + + base_time = images[image_topics[0]]["time"] + all_steps = len(base_time) + + def _empty_aligned_time() -> np.ndarray: + return np.array([], dtype="float64") + + def _aligned_time_or_base( + src_time: np.ndarray, topic: str + ) -> np.ndarray: + if base_time.size == 0: + return _empty_aligned_time() + if src_time.size == 0: + return np.copy(base_time) + return sample(base_time, src_time, prefix=uuid + " | " + topic) + + def _default_pose_series(step_count: int) -> list[list[float]]: + return [ + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0] for _ in range(step_count) + ] + + for t in images: + images[t]["freq"] = get_frequency( + images[t]["time"], uuid + " | " + t + ) + _update_topic_frequency(t, images[t]["freq"]) + if t == image_topics[0]: + continue + if "data" in images[t]: + if base_time.size > 0 and images[t]["time"].size == 0: + raise RuntimeError( + f"{uuid} | required render stream is empty: {t}" + ) + images[t]["time"], (images[t]["data"],) = sample( + base_time, + images[t]["time"], + [images[t]["data"]], + prefix=uuid + " | " + t, + ) + else: + images[t]["time"] = _aligned_time_or_base(images[t]["time"], t) + time_diff = np.abs(base_time - images[t]["time"]) + alignment_time_diff_stats[t] = { + "max_time_diff": float(time_diff.max()) + if time_diff.size + else 0.0, + "mean_time_diff": float(time_diff.mean()) + if time_diff.size + else 0.0, + } + for t in depths: + depths[t]["freq"] = get_frequency( + depths[t]["time"], uuid + " | " + t + ) + _update_topic_frequency(t, depths[t]["freq"]) + if "data" in depths[t]: + if base_time.size > 0 and depths[t]["time"].size == 0: + raise RuntimeError( + f"{uuid} | required render stream is empty: {t}" + ) + depths[t]["time"], (depths[t]["data"],) = sample( + base_time, + depths[t]["time"], + [depths[t]["data"]], + prefix=uuid + " | " + t, + ) + else: + depths[t]["time"] = _aligned_time_or_base(depths[t]["time"], t) + time_diff = np.abs(base_time - depths[t]["time"]) + alignment_time_diff_stats[t] = { + "max_time_diff": float(time_diff.max()) + if time_diff.size + else 0.0, + "mean_time_diff": float(time_diff.mean()) + if time_diff.size + else 0.0, + } + for t in joints: + joints[t]["freq"] = get_frequency( + joints[t]["time"], uuid + " | " + t + ) + _update_topic_frequency(t, joints[t]["freq"]) + if base_time.size == 0: + joints[t]["time"] = _empty_aligned_time() + joints[t]["position"] = [] + joints[t]["velocity"] = [] + joints[t]["effort"] = [] + elif joints[t]["time"].size == 0: + joints[t]["time"] = np.copy(base_time) + joints[t]["position"] = self._default_joint_series( + len(base_time) + ) + joints[t]["velocity"] = self._default_joint_series( + len(base_time) + ) + joints[t]["effort"] = self._default_joint_series( + len(base_time) + ) + else: + ( + joints[t]["time"], + ( + joints[t]["position"], + joints[t]["velocity"], + joints[t]["effort"], + ), + ) = sample( + base_time, + joints[t]["time"], + [ + joints[t]["position"], + joints[t]["velocity"], + joints[t]["effort"], + ], + prefix=uuid + " | " + t, + ) + time_diff = np.abs(base_time - joints[t]["time"]) + alignment_time_diff_stats[t] = { + "max_time_diff": float(time_diff.max()) + if time_diff.size + else 0.0, + "mean_time_diff": float(time_diff.mean()) + if time_diff.size + else 0.0, + } + joints[t]["position"] = self.reshape_joint_component( + joints[t]["position"], t, "position", uuid, parse_warnings + ) + joints[t]["velocity"] = self.reshape_joint_component( + joints[t]["velocity"], t, "velocity", uuid, parse_warnings + ) + joints[t]["effort"] = self.reshape_joint_component( + joints[t]["effort"], t, "effort", uuid, parse_warnings + ) + joints[t]["velocity"] = zero_gripper_velocity_effort( + joints[t]["velocity"] + ) + joints[t]["effort"] = zero_gripper_velocity_effort( + joints[t]["effort"] + ) + + if joints[left_joint_topic]["position"].shape[0] == 0: + left_ee_pose = np.repeat(np.eye(4)[None], 0, axis=0) + right_ee_pose = np.repeat(np.eye(4)[None], 0, axis=0) + camera_parent_poses = { + "left": left_ee_pose, + "right": right_ee_pose, + "middle": None, + } + else: + poses = self.forward_kinematics( + joints[left_joint_topic]["position"], + joints[right_joint_topic]["position"], + ) + left_ee_pose = poses[self.left_ee_link_name].get_matrix().numpy() + right_ee_pose = poses[self.right_ee_link_name].get_matrix().numpy() + camera_parent_poses = self._camera_parent_poses( + poses, left_ee_pose, right_ee_pose + ) + + for t in ee_pose_streams: + ee_pose_streams[t]["freq"] = get_frequency( + ee_pose_streams[t]["time"], uuid + " | " + t + ) + _update_topic_frequency(t, ee_pose_streams[t]["freq"]) + if base_time.size == 0: + ee_pose_streams[t]["time"] = _empty_aligned_time() + ee_pose_streams[t]["pose"] = [] + elif ee_pose_streams[t]["time"].size == 0: + ee_pose_streams[t]["time"] = np.copy(base_time) + ee_pose_streams[t]["pose"] = _default_pose_series( + len(base_time) + ) + else: + ( + ee_pose_streams[t]["time"], + (ee_pose_streams[t]["pose"],), + ) = sample( + base_time, + ee_pose_streams[t]["time"], + [ee_pose_streams[t]["pose"]], + prefix=uuid + " | " + t, + ) + time_diff = np.abs(base_time - ee_pose_streams[t]["time"]) + alignment_time_diff_stats[t] = { + "max_time_diff": float(time_diff.max()) + if time_diff.size + else 0.0, + "mean_time_diff": float(time_diff.mean()) + if time_diff.size + else 0.0, + } + for t in image_intrinsics: + freq = get_frequency(image_intrinsics[t]["time"], uuid + " | " + t) + _update_topic_frequency(t, freq) + for t in depth_intrinsics: + freq = get_frequency(depth_intrinsics[t]["time"], uuid + " | " + t) + _update_topic_frequency(t, freq) + if base_time.size == 0: + recorded_ee_poses = np.zeros((0, 2, 7), dtype="float64") + elif ( + raw_topic_counts[left_ee_topic] == 0 + or raw_topic_counts[right_ee_topic] == 0 + ): + recorded_ee_poses = None + else: + recorded_ee_poses = np.concatenate( + [ + np.array(ee_pose_streams[left_ee_topic]["pose"]), + np.array(ee_pose_streams[right_ee_topic]["pose"]), + ], + axis=1, + ).reshape(-1, 2, 7) + recorded_ee_poses = self.normalize_recorded_ee_poses( + recorded_ee_poses, camera_parent_poses + ) + + if self.calibration_dict is not None: + image_extrinsic = copy.deepcopy(self.calibration_dict) + extrinsic = self.compute_camera_extrinsics( + image_extrinsic, camera_parent_poses + ) + + intrinsic = {} + for cam, t in zip(cameras, image_topics, strict=False): + intrinsic[cam] = images[t]["intrinsic"] + joint_positions = np.concatenate( + [ + np.array(joints[left_joint_topic]["position"]), + np.array(joints[right_joint_topic]["position"]), + ], + axis=-1, + ) + joint_velocity = np.concatenate( + [ + np.array(joints[left_joint_topic]["velocity"]), + np.array(joints[right_joint_topic]["velocity"]), + ], + axis=-1, + ) + joint_effort = np.concatenate( + [ + np.array(joints[left_joint_topic]["effort"]), + np.array(joints[right_joint_topic]["effort"]), + ], + axis=-1, + ) + + # UBT has no master/teleop arms; keep zero-width placeholders so the + # inspection model stays shape-compatible with the Piper payload. + master_joint_positions = np.zeros( + (joint_positions.shape[0], 0), dtype="float64" + ) + master_joint_velocity = np.zeros( + (joint_positions.shape[0], 0), dtype="float64" + ) + master_joint_effort = np.zeros( + (joint_positions.shape[0], 0), dtype="float64" + ) + + if left_ee_pose.shape[0] == 0: + ee_poses = np.zeros((0, 2, 7), dtype="float64") + else: + ee_poses = np.concatenate( + [ + left_ee_pose[:, :3, 3], + Rotation.from_matrix(left_ee_pose[:, :3, :3]).as_quat( + scalar_first=False + ), + right_ee_pose[:, :3, 3], + Rotation.from_matrix(right_ee_pose[:, :3, :3]).as_quat( + scalar_first=False + ), + ], + axis=1, + ).reshape(-1, 2, 7) + + # ==================== filter + if self.static_threshold is not None and self.static_threshold > 0: + static_mask = np.ones(joint_positions.shape[0], dtype=bool) + static_mask[1:] = np.any( + np.abs(np.diff(joint_positions, axis=0)) > 1e-3, axis=1 + ) + + time_mask = np.zeros(static_mask.shape[0], bool) + if ( + self.head_time_to_filter is not None + and self.head_time_to_filter > 0 + ): + head_time = base_time - base_time[0] + time_mask = np.logical_or( + time_mask, head_time < self.head_time_to_filter + ) + if ( + self.tile_time_to_filter is not None + and self.tile_time_to_filter > 0 + ): + tile_time = -base_time + base_time[-1] + time_mask = np.logical_or( + time_mask, tile_time < self.tile_time_to_filter + ) + static_mask = np.logical_or(static_mask, np.logical_not(time_mask)) + joint_positions = joint_positions[static_mask] + joint_velocity = joint_velocity[static_mask] + joint_effort = joint_effort[static_mask] + master_joint_positions = master_joint_positions[static_mask] + master_joint_velocity = master_joint_velocity[static_mask] + master_joint_effort = master_joint_effort[static_mask] + ee_poses = ee_poses[static_mask] + if recorded_ee_poses is not None: + recorded_ee_poses = recorded_ee_poses[static_mask] + logger.info( + f"{uuid} | all steps: {all_steps}, " + f"non static steps: {joint_positions.shape[0]}" + ) + + extrinsic["left"] = extrinsic["left"][static_mask] + extrinsic["right"] = extrinsic["right"][static_mask] + base_time = base_time[static_mask] + for t in image_topics: + images[t]["time"] = images[t]["time"][static_mask] + if "data" in images[t]: + images[t]["data"] = [ + x + for i, x in enumerate(images[t]["data"]) + if static_mask[i] + ] + for t in depth_topics: + depths[t]["time"] = depths[t]["time"][static_mask] + if "data" in depths[t]: + depths[t]["data"] = [ + x + for i, x in enumerate(depths[t]["data"]) + if static_mask[i] + ] + for t in ee_pose_streams: + ee_pose_streams[t]["pose"] = [ + x + for i, x in enumerate(ee_pose_streams[t]["pose"]) + if static_mask[i] + ] + ee_pose_streams[t]["time"] = [ + x + for i, x in enumerate(ee_pose_streams[t]["time"]) + if static_mask[i] + ] + + non_static_steps = len(base_time) + # ==================== filter finish + + topic_counts = raw_topic_counts + topic_frequencies = { + topic: payload["freq"] for topic, payload in images.items() + } + topic_frequencies.update( + {topic: payload["freq"] for topic, payload in depths.items()} + ) + topic_frequencies.update( + {topic: payload["freq"] for topic, payload in joints.items()} + ) + topic_frequencies.update( + { + topic: payload["freq"] + for topic, payload in ee_pose_streams.items() + } + ) + required_topics = ( + image_topics + + depth_topics + + [ + left_joint_topic, + right_joint_topic, + left_ee_topic, + right_ee_topic, + ] + ) + ( + joint_limit_names, + joint_lower_limits, + joint_upper_limits, + ) = self.get_recorded_joint_limits() + + return dict( + cam_names=cameras, + uuid=uuid, + source_path=path, + mcap_path=mcap, + images=images, + depths=depths, + joint_positions=joint_positions, + joint_velocity=joint_velocity, + joint_effort=joint_effort, + master_joint_positions=master_joint_positions, + master_joint_velocity=master_joint_velocity, + master_joint_effort=master_joint_effort, + ee_poses=ee_poses, + fk_ee_poses=ee_poses, + recorded_ee_poses=recorded_ee_poses, + extrinsic=extrinsic, + intrinsic=intrinsic, + meta=meta, + base_time=base_time, + all_steps=all_steps, + non_static_steps=non_static_steps, + topic_counts=topic_counts, + topic_frequencies=topic_frequencies, + required_topics=required_topics, + observed_topics=observed_topics, + joint_limit_names=joint_limit_names, + joint_lower_limits=joint_lower_limits, + joint_upper_limits=joint_upper_limits, + parse_warnings=parse_warnings, + topic_summaries=topic_summaries, + alignment_time_diff_stats=alignment_time_diff_stats, + static_filter_applied=( + self.static_threshold is not None and self.static_threshold > 0 + ), + ) + + # ------------------------------------------------------------------ + # Orchestration – episode model conversion + # ------------------------------------------------------------------ + + def build_episode_data(self, data) -> EpisodeData: + """Convert the parser payload into the typed inspection model.""" + + def _as_array(value): + if value is None: + return None + return np.asarray(value) + + return EpisodeData( + uuid=data["uuid"], + meta=data["meta"], + topic_counts=data.get("topic_counts", {}), + topic_frequencies=data.get("topic_frequencies", {}), + base_time=_as_array(data.get("base_time", [])), + joint_positions=_as_array(data["joint_positions"]), + master_joint_positions=_as_array(data["master_joint_positions"]), + fk_ee_poses=_as_array(data.get("fk_ee_poses", data["ee_poses"])), + recorded_ee_poses=_as_array(data.get("recorded_ee_poses")), + images=data["images"], + depths=data["depths"], + required_topics=data.get("required_topics", []), + observed_topics=set(data.get("observed_topics", set())), + joint_limit_names=data.get("joint_limit_names", []), + joint_lower_limits=_as_array(data.get("joint_lower_limits")), + joint_upper_limits=_as_array(data.get("joint_upper_limits")), + parse_warnings=data.get("parse_warnings", []), + cam_names=data.get("cam_names", []), + extrinsic=data.get("extrinsic", {}), + intrinsic=data.get("intrinsic", {}), + topic_summaries=data.get("topic_summaries", {}), + alignment_time_diff_stats=data.get( + "alignment_time_diff_stats", {} + ), + static_filter_applied=data.get("static_filter_applied", False), + ) + + # ------------------------------------------------------------------ + # Orchestration – video rendering + # ------------------------------------------------------------------ + + def decode_and_visualize( + self, data, episode_status: str = "pass", render_hints=None + ): + """Render one episode into a stitched visualization video.""" + + video_writer = None + uuid = data["uuid"] + os.makedirs(self.output_path, exist_ok=True) + file = os.path.join( + self.output_path, f"{uuid.replace(os.sep, '-')}.mp4" + ) + num_steps = len(data["joint_positions"]) + if num_steps == 0: + return None + frame_indices = list(range(0, num_steps, self.vis_interval)) + total_frames = len(frame_indices) + for frame_idx, i in enumerate(frame_indices): + if frame_idx == 0 or (frame_idx + 1) % 20 == 0 or frame_idx + 1 == total_frames: + logger.info( + f"{uuid} | rendering video frame " + f"{frame_idx + 1}/{total_frames} " + f"(step {i + 1}/{num_steps})" + ) + imgs = [] + depths_vis = [] + ee_poses = data["ee_poses"][i] + for cam in data["cam_names"]: + image_topic = ( + f"/observation/cameras/{cam}/color_image/image_raw" + ) + depth_topic = ( + f"/observation/cameras/{cam}/depth_image/image_raw" + ) + extrinsic = data["extrinsic"][cam] + if extrinsic.ndim == 3: + extrinsic = extrinsic[i] + intrinsic = data["intrinsic"][cam] + + img = decode_img(data["images"][image_topic]["data"][i]) + img = self.draw_ee_pose(img, intrinsic, extrinsic, ee_poses) + depth = decode_depth( + data["depths"][depth_topic]["data"][i], 1000 + ) + depth = depth_visualize(depth) + + imgs.append(img) + depths_vis.append(depth) + + shapes = [im.shape[:2] for im in imgs + depths_vis] + if not all(s == shapes[0] for s in shapes): + max_h = max(s[0] for s in shapes) + max_w = max(s[1] for s in shapes) + if i == 0: + logger.warning( + f"{uuid} | inconsistent image shapes detected, " + f"resizing all images to ({max_h},{max_w})" + ) + + def _resize(im, size): + return cv2.resize(im, size, interpolation=cv2.INTER_AREA) + + imgs = [_resize(im, (max_w, max_h)) for im in imgs] + depths_vis = [_resize(im, (max_w, max_h)) for im in depths_vis] + + if self.vis_depth: + vis_img = np.concatenate( + [ + np.concatenate(imgs, axis=1), + np.concatenate(depths_vis, axis=1), + ], + axis=0, + ) + else: + vis_img = np.concatenate(imgs, axis=1) + + if self.vis_resize_scale is not None: + size = ( + max(1, int(vis_img.shape[1] // self.vis_resize_scale)), + max(1, int(vis_img.shape[0] // self.vis_resize_scale)), + ) + vis_img = cv2.resize( + vis_img, size, interpolation=cv2.INTER_AREA + ) + + cv2.putText( + vis_img, + uuid, + (10, 30), + cv2.FONT_HERSHEY_SIMPLEX, + max(1 / self.vis_resize_scale, 0.8), + (0, 0, 255), + 2, + ) + + cv2.putText( + vis_img, + data["meta"]["instruction"], + (10, vis_img.shape[0] - 10), + cv2.FONT_HERSHEY_SIMPLEX, + max(1 / self.vis_resize_scale, 0.8), + (0, 0, 255), + 2, + ) + + if video_writer is None: + render_fps = max(1, round(self.vis_fps / self.vis_interval)) + video_writer = FfmpegVideoWriter( + file, + vis_img.shape[:2][::-1], + render_fps, + enable_ffmpeg_log=getattr( + self, "enable_ffmpeg_log", False + ), + ) + video_writer.write(vis_img) + if video_writer is not None: + video_writer.release() + return file + return None + + def render_episode_video(self, data, inspection): + """Render the visualization video for one inspected episode.""" + + return self.decode_and_visualize( + data, + episode_status=inspection.episode_status, + render_hints=inspection.render_hints, + ) + + def render_handler(self, item): + """Re-parse one episode and export its visualization video.""" + + ep_id = item["_episode_index"] + uuid = item.get("uuid", self.episodes[ep_id][0]) + try: + episode_raw = self.parse(ep_id, payload_mode="render") + video_file = self.decode_and_visualize( + episode_raw, + episode_status=item.get("episode_status", "pass"), + render_hints=item.get("render_hints", []), + ) + return { + "_episode_index": ep_id, + "video_file": video_file, + } + except Exception as e: + logger.exception( + f"failed to render episode {ep_id} ({uuid}) with error: {e}" + ) + return { + "_episode_index": ep_id, + "video_file": None, + "runtime_error": str(e), + } + + def render_videos(self, results): + """Run the second-stage video export after rule checking completes.""" + + render_jobs = [ + item + for item in results + if item.get("_episode_index") is not None + and not item.get("runtime_error") + ] + if not render_jobs: + return + + logger.info( + "starting video export for %d episode(s); " + "this re-parses mcap with images and may take several minutes", + len(render_jobs), + ) + if self.num_workers > 1: + with multiprocessing.Pool(processes=self.num_workers) as pool: + render_updates = pool.map(self.render_handler, render_jobs) + else: + render_updates = [ + self.render_handler(item) for item in render_jobs + ] + + updates_by_index = { + item["_episode_index"]: item for item in render_updates + } + for item in results: + episode_index = item.get("_episode_index") + if episode_index is None: + continue + update = updates_by_index.get(episode_index) + if update is None: + continue + item["video_file"] = update.get("video_file") + if update.get("runtime_error"): + item["video_runtime_error"] = update["runtime_error"] + + # ------------------------------------------------------------------ + # Orchestration – report / log formatting + # ------------------------------------------------------------------ + + def _iter_rule_results(self, item): + return iter_rule_results(item) + + def _count_topics(self, metric_value: str | None) -> int: + return count_topics(metric_value) + + def _build_report_row(self, item): + return build_report_row(item) + + def _write_mcap_lists(self, rows): + return write_mcap_lists(self.output_path, rows) + + def _format_numeric_rule_detail( + self, name: str, actual: float, limit: float + ) -> str: + return ( + f"{name} actual={actual:.6f}, " + f"limit={limit:.6f}, gap={actual - limit:.6f}" + ) + + def _format_numeric_detail_table( + self, label_header: str, rows: list[tuple[str, float, float, float]] + ) -> list[str]: + return format_numeric_detail_table(label_header, rows) + + def _format_text_detail_table( + self, headers: tuple[str, ...], rows: list[tuple[str, ...]] + ) -> list[str]: + return format_text_detail_table(headers, rows) + + def _format_rule_detail_lines(self, rule: dict) -> list[str]: + return format_rule_detail_lines(rule, self.inspect_config) + + def _write_rule_block(self, fh, rule: dict) -> None: + return write_rule_block(fh, rule, self.inspect_config) + + def _format_timestamp(self, timestamp_ns: int | None) -> str: + return format_timestamp(timestamp_ns) + + def _topic_sort_key(self, topic: str) -> tuple[int, str]: + return topic_sort_key(topic) + + def _iter_logged_topics( + self, topic_summaries: dict[str, dict] + ) -> list[tuple[str, dict]]: + return iter_logged_topics(topic_summaries) + + def _format_topic_summary_line(self, topic: str, summary: dict) -> str: + return format_topic_summary_line(topic, summary) + + def _write_full_log_episode(self, fh, item: dict) -> None: + return write_full_log_episode(fh, item, self.inspect_config) + + def _build_runtime_config(self) -> dict[str, object]: + return { + "input_path": getattr(self, "input_path", None), + "output_path": getattr(self, "output_path", None), + "task_names": getattr(self, "task_names", None), + "user_names": getattr(self, "user_names", None), + "date_prefix": getattr(self, "date_prefix", None), + "static_threshold": getattr(self, "static_threshold", None), + "head_time_to_filter": getattr(self, "head_time_to_filter", None), + "tile_time_to_filter": getattr(self, "tile_time_to_filter", None), + "num_workers": getattr(self, "num_workers", None), + "vis_interval": getattr(self, "vis_interval", None), + "vis_fps": getattr(self, "vis_fps", None), + "vis_resize_scale": getattr(self, "vis_resize_scale", None), + "vis_depth": getattr(self, "vis_depth", None), + "skip_video_export": getattr(self, "skip_video_export", None), + "enable_ffmpeg_log": getattr(self, "enable_ffmpeg_log", None), + "inspect_config_path": getattr(self, "inspect_config_path", None), + } + + def _write_error_logs(self, results): + return write_error_logs( + self.output_path, + results, + getattr(self, "inspect_config", None), + self._build_runtime_config(), + ) + + def write_manual_review_artifacts(self, results): + return write_manual_review_artifacts( + self.output_path, + results, + self.vis_interval, + self.vis_fps, + ) + + # ------------------------------------------------------------------ + # Orchestration – single-episode handler + # ------------------------------------------------------------------ + + def handler(self, ep_id): + """Parse and inspect one episode without exporting video.""" + + logger.info(f"start process [{ep_id + 1}/{len(self.episodes)}]") + try: + episode_raw = self.parse(ep_id, payload_mode="inspect") + episode_data = self.build_episode_data(episode_raw) + inspection = evaluate_episode_rules( + episode_data, self.inspect_config + ) + uuid = episode_data.uuid + source_path = episode_raw.get("source_path") + num_steps = len(episode_data.joint_positions) + mcap_path = episode_raw.get("mcap_path", source_path) + topic_summaries = episode_raw.get("topic_summaries", {}) + all_steps = episode_raw.get("all_steps", num_steps) + non_static_steps = episode_raw.get("non_static_steps", num_steps) + render_hints = inspection.render_hints + report = EpisodeReport( + uuid=uuid, + episode_status=inspection.episode_status, + rule_results=inspection.rule_results, + episode_metrics=inspection.episode_metrics, + video_file=None, + ) + except Exception as e: + uuid = self.episodes[ep_id][0] + source_path = uuid + logger.exception( + f"failed to process episode {ep_id} with error: {e}" + ) + report = EpisodeReport( + uuid=uuid, + episode_status="fail", + rule_results=[], + episode_metrics={"rule_hit_count": 0}, + video_file=None, + runtime_error=str(e), + ) + return { + **report.to_dict(), + "source_path": source_path, + "mcap_path": source_path, + "topic_summaries": {}, + "num_steps": 0, + "all_steps": 0, + "non_static_steps": 0, + "render_hints": [], + "_episode_index": ep_id, + } + else: + logger.info( + f"finish process [{ep_id + 1}/{len(self.episodes)}] {uuid}, " + f"num_steps:{num_steps} \n" + ) + del episode_data + del episode_raw + return { + **report.to_dict(), + "source_path": source_path, + "mcap_path": mcap_path, + "topic_summaries": topic_summaries, + "render_hints": render_hints, + "num_steps": num_steps, + "all_steps": all_steps, + "non_static_steps": non_static_steps, + "_episode_index": ep_id, + } + + # ------------------------------------------------------------------ + # Orchestration – batch job + # ------------------------------------------------------------------ + + def concat_videos(self, video_files): + """Concatenate rendered episode videos into one mp4.""" + + return concat_videos( + video_files, + self.output_path, + enable_ffmpeg_log=getattr(self, "enable_ffmpeg_log", False), + ) + + def build_job_summary(self, results): + """Build aggregate counters for a batch of episode reports.""" + + return build_job_summary(results, self.output_path) + + def write_job_outputs(self, results): + """Write batch-level log artifacts derived from inspection results.""" + + os.makedirs(self.output_path, exist_ok=True) + rows = [self._build_report_row(item) for item in results] + self._write_mcap_lists(rows) + self._write_error_logs(results) + + def __call__(self): + """Execute the full job: inspect first, then export videos if enabled.""" + + if self.num_workers > 1: + with multiprocessing.Pool(processes=self.num_workers) as pool: + results = pool.map( + self.handler, list(range(len(self.episodes))) + ) + else: + results = [] + for i in range(len(self.episodes)): + results.append(self.handler(i)) + + results.sort(key=lambda x: x["uuid"]) + total_steps = sum([x.get("num_steps", 0) for x in results]) + + logger.info( + f"total steps: {total_steps}, " + f"total times: {total_steps / 108000:.3f}h" + ) + self.write_job_outputs(results) + if not getattr(self, "skip_video_export", False): + self.render_videos(results) + self.concat_videos( + [x["video_file"] for x in results if x.get("video_file")] + ) + self.write_manual_review_artifacts(results) + + +if __name__ == "__main__": + import argparse + + from robo_orchard_lab.utils import log_basic_config + + log_basic_config( + format="%(asctime)s %(levelname)s-%(lineno)d: %(message)s", + level=logging.INFO, + ) + parser = argparse.ArgumentParser() + parser.add_argument("--input_path", type=str) + parser.add_argument("--output_path", type=str) + parser.add_argument("--urdf", type=str) + parser.add_argument("--user_names", type=str, default=None) + parser.add_argument("--task_names", type=str, default=None) + parser.add_argument("--date_prefix", type=str, default=None) + parser.add_argument("--num_workers", type=int, default=4) + parser.add_argument("--embodiment_meta_file", type=str, default=None) + parser.add_argument("--embodiment", type=str, default=None) + parser.add_argument("--use_extra_calibration", action="store_true") + parser.add_argument("--inspect_config", type=str, default=None) + parser.add_argument("--enable_ffmpeg_log", action="store_true") + parser.add_argument( + "--vis_interval", + type=int, + default=None, + help="Render every N steps (default: 3). Larger values export faster.", + ) + parser.add_argument( + "--vis_resize_scale", + type=float, + default=None, + help="Downscale rendered frames by this factor (UBT default: 1).", + ) + parser.add_argument( + "--vis_depth", + action=argparse.BooleanOptionalAction, + default=None, + help="Include depth row in rendered video (default: True).", + ) + parser.add_argument( + "--robot_type", + choices=["piper", "ubt"], + default="ubt", + help="Accepted for CLI compatibility; this script always uses UbtMcapChecker.", + ) + parser.add_argument( + "--skip_video_export", + action="store_true", + default=False, + help="Skip video rendering and only run data quality checks.", + ) + args = parser.parse_args() + + calibration_dict = None + if args.embodiment_meta_file is not None: + if args.embodiment is None: + parser.error( + "--embodiment is required when " + "--embodiment_meta_file is specified" + ) + with open(args.embodiment_meta_file, "r") as f: + embodiment_meta = json.load(f) + embodiment_meta = embodiment_meta[args.embodiment] + if args.use_extra_calibration: + calibration_dict = embodiment_meta["calibration"] + logger.info(f"use extra calibration: {calibration_dict}") + if args.urdf is None: + args.urdf = embodiment_meta["urdf"] + + if args.urdf is None: + parser.error( + "Either --urdf or --embodiment_meta_file " + "(with --embodiment) must be specified" + ) + logger.info(f"urdf: {args.urdf}") + + if args.task_names is None: + task_names = None + else: + task_names = args.task_names.split(",") + + checker_kwargs = {} + if args.vis_interval is not None: + checker_kwargs["vis_interval"] = args.vis_interval + if args.vis_resize_scale is not None: + checker_kwargs["vis_resize_scale"] = args.vis_resize_scale + if args.vis_depth is not None: + checker_kwargs["vis_depth"] = args.vis_depth + + checker = UbtMcapChecker( + input_path=args.input_path, + output_path=args.output_path, + urdf=args.urdf, + task_names=task_names, + user_names=args.user_names, + date_prefix=args.date_prefix, + calibration_dict=calibration_dict, + num_workers=args.num_workers, + inspect_config=build_default_inspect_config() + if args.inspect_config is None + else load_inspect_config(args.inspect_config), + inspect_config_path=args.inspect_config, + enable_ffmpeg_log=args.enable_ffmpeg_log, + skip_video_export=args.skip_video_export, + **checker_kwargs, + ) + checker() diff --git a/lightweight-operators/ubt-mcap-pipeline/robo_orchard_lab/dataset/ubt_manipulation/tools/templates/inspect_error_log.html b/lightweight-operators/ubt-mcap-pipeline/robo_orchard_lab/dataset/ubt_manipulation/tools/templates/inspect_error_log.html new file mode 100644 index 00000000..8102ef7d --- /dev/null +++ b/lightweight-operators/ubt-mcap-pipeline/robo_orchard_lab/dataset/ubt_manipulation/tools/templates/inspect_error_log.html @@ -0,0 +1,151 @@ + + + + + + Inspect Error Log + + + +
+
+
Inspect Error Log
+

错误摘要跳转页

+

+ 这个页面是 inspect_error_log.log 的 HTML 视图。人工复核页里的系统简写 + note 会直接跳到这里对应的错误 block。 +

+
Generated At: __INSPECT_ERROR_LOG_GENERATED_AT__
+
+ +
__INSPECT_ERROR_LOG_BLOCKS__
+
+ + diff --git a/lightweight-operators/ubt-mcap-pipeline/robo_orchard_lab/dataset/ubt_manipulation/tools/templates/manual_review.html b/lightweight-operators/ubt-mcap-pipeline/robo_orchard_lab/dataset/ubt_manipulation/tools/templates/manual_review.html new file mode 100644 index 00000000..17c45c92 --- /dev/null +++ b/lightweight-operators/ubt-mcap-pipeline/robo_orchard_lab/dataset/ubt_manipulation/tools/templates/manual_review.html @@ -0,0 +1,1159 @@ + + + + + + Manual Review + + + +
+
+
+
Manual Review
+

Concat 回放 + 人工失败标记

+

+ 通过 concat 视频回看整批数据,页面会实时识别当前片段对应的 + mcap_id。点击“不通过”即可把当前片段加入上方失败表格, + 本地状态会自动缓存到浏览器,完成后可手动导出 + manual_review_failures.json。 +

+
+ +
+ +
+
+
+
+

失败表格

+
+ 会先预填 inspect 结果里的 ERROR 项;同一个 mcap 只保留一条记录。 +
+
+
+
+ + + + + + + + + +
mcap_idmark_time_secnote
+
+ 当前没有待复查的不通过 mcap。 +
+
+
+ +
+
+
+
+
+

Concat 回放

+
+ 播放器时间轴会自动映射到当前片段。 +
+
+
+
+ +
+
+
+ + + + +
+
+
+ “不通过”会把当前片段对应的 mcap_id 加入失败表格。 +
+
+ “撤销当前”会将当前片段对应的记录移除。 +
+
+ 修改完成后点击 导出 JSON 下载最新的 + manual_review_failures.json。 +
+
+ 或点击 复制 mcap_id 一次性复制所有失败项。 +
+
+ 点击失败表格行可以回跳到对应片段。 +
+
+ 点击系统简写 note 可以跳到 inspect_error_log 对应报错。 +
+
+
+
+ + +
+
+
+
+ + + + diff --git a/lightweight-operators/ubt-mcap-pipeline/robo_orchard_lab/dataset/ubt_manipulation/tools/ubt_common.py b/lightweight-operators/ubt-mcap-pipeline/robo_orchard_lab/dataset/ubt_manipulation/tools/ubt_common.py new file mode 100644 index 00000000..be3a94cf --- /dev/null +++ b/lightweight-operators/ubt-mcap-pipeline/robo_orchard_lab/dataset/ubt_manipulation/tools/ubt_common.py @@ -0,0 +1,383 @@ +# Project RoboOrchard +# +# Copyright (c) 2024-2025 Horizon Robotics. All Rights Reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or +# implied. See the License for the specific language governing +# permissions and limitations under the License. + +"""Shared UBT dual 7-axis arm constants and helpers for checker / packer.""" + +from __future__ import annotations + +import logging +from typing import Any + +import numpy as np + +logger = logging.getLogger(__name__) + +ARM_DIM = 7 +POSITION_INDEX = 7 +STATE_INDEX = 8 +RECORDED_JOINT_DIM_PER_ARM = 9 +PACKED_JOINT_DIM_PER_ARM = 8 +PACKED_JOINT_DIM_DUAL = 16 + +LEFT_EE_LINK_NAME = "8_Link" +RIGHT_EE_LINK_NAME = "14_Link" +LEFT_CAMERA_PARENT_LINK_NAME = "L_wrist_pitch_link" +RIGHT_CAMERA_PARENT_LINK_NAME = "R_wrist_pitch_link" +MIDDLE_CAMERA_PARENT_LINK_NAME = "prismatic_link" + +LEFT_ARM_JOINT_NAMES = [ + "L_shoulder_pitch_joint", + "L_shoulder_roll_joint", + "L_shoulder_yaw_joint", + "L_elbow_roll_joint", + "L_elbow_yaw_joint", + "L_wrist_roll_joint", + "L_wrist_pitch_joint", +] +RIGHT_ARM_JOINT_NAMES = [ + "R_shoulder_pitch_joint", + "R_shoulder_roll_joint", + "R_shoulder_yaw_joint", + "R_elbow_roll_joint", + "R_elbow_yaw_joint", + "R_wrist_roll_joint", + "R_wrist_pitch_joint", +] + +GRIPPER_POSITION_LOWER = 0.0 +GRIPPER_POSITION_UPPER = 1.0 +# Gripper state: 0 = open, 1 = closed (right arm). +# Left (VR) arm firmware reports -1 when uninitialized, so lower bound is -1. +GRIPPER_STATE_LOWER = -1.0 +GRIPPER_STATE_UPPER = 1.0 + +LEFT_SLAVE_JOINT_TOPIC = "/observation/robot_state/left/joint" +RIGHT_SLAVE_JOINT_TOPIC = "/observation/robot_state/right/joint" +LEFT_MASTER_JOINT_TOPIC = "/observation/robot_state/left_master/joint" +RIGHT_MASTER_JOINT_TOPIC = "/observation/robot_state/right_master/joint" + +UBT_FRAME_ID_EXTRINSIC_MAP = { + "middle_camera_color_optical_frame/prismatic_link": "middle", + "left_camera_color_optical_frame/L_wrist_pitch_link": "left", + "right_camera_color_optical_frame/R_wrist_pitch_link": "right", +} + + +def get_slave_joint_topics() -> dict[str, str]: + """Return logical slave joint stream names.""" + + return { + "left": LEFT_SLAVE_JOINT_TOPIC, + "right": RIGHT_SLAVE_JOINT_TOPIC, + } + + +def get_master_joint_topics() -> dict[str, str]: + """Return logical master joint stream names.""" + + return { + "left": LEFT_MASTER_JOINT_TOPIC, + "right": RIGHT_MASTER_JOINT_TOPIC, + } + + +def recorded_joint_names() -> list[str]: + """Return joint names aligned with the 18-D checker joint vector.""" + + return ( + LEFT_ARM_JOINT_NAMES + + ["left_gripper_position"] + + ["left_gripper_state"] + + RIGHT_ARM_JOINT_NAMES + + ["right_gripper_position"] + + ["right_gripper_state"] + ) + + +def gripper_state_joint_indices(joint_limit_names: list[str]) -> list[int]: + """Return indices of gripper state columns in a recorded joint vector.""" + + return [ + idx + for idx, name in enumerate(joint_limit_names) + if name.endswith("_gripper_state") + ] + + +def gripper_position_joint_indices(joint_limit_names: list[str]) -> list[int]: + """Return indices of gripper width columns in a recorded joint vector.""" + + return [ + idx + for idx, name in enumerate(joint_limit_names) + if name.endswith("_gripper_position") + ] + + +def reshape_joint_component( + values: Any, + topic: str, + field_name: str, + uuid: str, + parse_warnings: list[str] | None, + *, + recorded_joint_dim: int = RECORDED_JOINT_DIM_PER_ARM, +) -> np.ndarray: + """Normalize UBT JointState components to 7 arm + position + state. + + Layout per arm (9-dim canonical): + + * ``idx 0–6``: arm joints + * ``idx 7``: gripper position (width feedback / cmd) + * ``idx 8``: gripper state (0=open, 1=closed) + + Any ``N > 9`` layout keeps the first 9 columns and discards the rest. + """ + + component = np.asarray(values, dtype="float64") + if component.ndim == 2 and component.shape[1] == 0: + return np.zeros( + (component.shape[0], recorded_joint_dim), + dtype="float64", + ) + if component.size == 0: + return component.reshape(-1, recorded_joint_dim) + + if component.ndim == 1: + for width in (10, recorded_joint_dim): + if component.size % width == 0: + component = component.reshape(-1, width) + break + + if component.ndim == 2 and component.shape[1] > recorded_joint_dim: + if field_name == "position": + extra = component.shape[1] - recorded_joint_dim + warning = ( + f"{uuid} | {topic} - position has {component.shape[1]} dims; " + f"keep first {recorded_joint_dim} " + f"(7 arm + position + state), drop {extra} trailing field(s)" + ) + logger.warning(warning) + if parse_warnings is not None: + parse_warnings.append(warning) + component = component[:, :recorded_joint_dim] + + if component.ndim == 2 and component.shape[1] == recorded_joint_dim: + return component + + return component.reshape(-1, recorded_joint_dim) + + +def zero_gripper_velocity_effort(component: np.ndarray) -> np.ndarray: + """Zero velocity/effort placeholders on gripper position/state columns.""" + + if component.ndim != 2 or component.shape[1] < RECORDED_JOINT_DIM_PER_ARM: + return component + + output = np.array(component, copy=True) + for arm_offset in (0, RECORDED_JOINT_DIM_PER_ARM): + for idx in (POSITION_INDEX, STATE_INDEX): + col = arm_offset + idx + if col < output.shape[1]: + output[:, col] = 0.0 + return output + + +def pack_slave_joint_positions(left: np.ndarray, right: np.ndarray) -> np.ndarray: + """Pack arm + position feedback into the 16-D stage-1 joint vector.""" + + return np.concatenate( + [left[:, :PACKED_JOINT_DIM_PER_ARM], right[:, :PACKED_JOINT_DIM_PER_ARM]], + axis=-1, + ) + + +def pack_master_joint_positions( + left_slave: np.ndarray, + right_slave: np.ndarray, +) -> np.ndarray: + """Build 16-D master joint vector with position cmd reused from slave.""" + + packed = np.zeros( + (left_slave.shape[0], PACKED_JOINT_DIM_DUAL), + dtype=np.float64, + ) + packed[:, POSITION_INDEX] = left_slave[:, POSITION_INDEX] + packed[:, PACKED_JOINT_DIM_PER_ARM + POSITION_INDEX] = right_slave[ + :, POSITION_INDEX + ] + return packed + + +def pack_gripper_state(left_slave: np.ndarray, right_slave: np.ndarray) -> np.ndarray: + """Pack left/right gripper state feedback into ``[T, 2]``.""" + + return np.concatenate( + [left_slave[:, STATE_INDEX : STATE_INDEX + 1], right_slave[:, STATE_INDEX : STATE_INDEX + 1]], + axis=-1, + ) + + +def pack_master_gripper_state( + left_master: np.ndarray, + right_master: np.ndarray, +) -> np.ndarray: + """Pack left/right gripper state cmd into ``[T, 2]``.""" + + return np.concatenate( + [ + left_master[:, STATE_INDEX : STATE_INDEX + 1], + right_master[:, STATE_INDEX : STATE_INDEX + 1], + ], + axis=-1, + ) + + +class UbtKinematicsMixin: + """Shared FK helpers for UBT checker and packer.""" + + left_ee_link_name = LEFT_EE_LINK_NAME + right_ee_link_name = RIGHT_EE_LINK_NAME + left_camera_parent_link_name = LEFT_CAMERA_PARENT_LINK_NAME + right_camera_parent_link_name = RIGHT_CAMERA_PARENT_LINK_NAME + middle_camera_parent_link_name = MIDDLE_CAMERA_PARENT_LINK_NAME + left_arm_joint_names = LEFT_ARM_JOINT_NAMES + right_arm_joint_names = RIGHT_ARM_JOINT_NAMES + + def _joint_parameter_index(self) -> dict[str, int]: + cached = getattr(self, "_joint_parameter_index_cache", None) + if cached is not None: + return cached + + joint_names = list( + self.chain.get_joint_parameter_names(exclude_fixed=True) + ) + self._joint_parameter_index_cache = { + name: idx for idx, name in enumerate(joint_names) + } + return self._joint_parameter_index_cache + + def forward_kinematics(self, left_joint, right_joint): + """Run FK for UBT dual 7-axis arm state (FK consumes arm joints only).""" + + index = self._joint_parameter_index() + joint_names = list( + self.chain.get_joint_parameter_names(exclude_fixed=True) + ) + joint = np.zeros([left_joint.shape[0], len(joint_names)]) + + if left_joint is not None: + for recorded_idx, name in enumerate(self.left_arm_joint_names): + joint[:, index[name]] = left_joint[:, recorded_idx] + if right_joint is not None: + for recorded_idx, name in enumerate(self.right_arm_joint_names): + joint[:, index[name]] = right_joint[:, recorded_idx] + + return self.chain.forward_kinematics(joint) + + def get_recorded_joint_limits( + self, + ) -> tuple[list[str], np.ndarray, np.ndarray]: + """Return limits aligned with UBT's 18-D recorded joint state.""" + + cached = getattr(self, "_recorded_joint_limit_cache", None) + if cached is not None: + return cached + + joint_names = list( + self.chain.get_joint_parameter_names(exclude_fixed=True) + ) + joint_lower_limits, joint_upper_limits = self.chain.get_joint_limits() + lower = np.asarray(joint_lower_limits, dtype=np.float64) + upper = np.asarray(joint_upper_limits, dtype=np.float64) + index = {name: idx for idx, name in enumerate(joint_names)} + + names = recorded_joint_names() + missing = [ + name + for name in self.left_arm_joint_names + self.right_arm_joint_names + if name not in index + ] + if missing: + raise ValueError( + "UBT URDF missing expected arm joints: " + ", ".join(missing) + ) + + recorded_joint_lower_limits = np.array( + [lower[index[name]] for name in self.left_arm_joint_names] + + [GRIPPER_POSITION_LOWER] + + [GRIPPER_STATE_LOWER] + + [lower[index[name]] for name in self.right_arm_joint_names] + + [GRIPPER_POSITION_LOWER] + + [GRIPPER_STATE_LOWER], + dtype=np.float64, + ) + recorded_joint_upper_limits = np.array( + [upper[index[name]] for name in self.left_arm_joint_names] + + [GRIPPER_POSITION_UPPER] + + [GRIPPER_STATE_UPPER] + + [upper[index[name]] for name in self.right_arm_joint_names] + + [GRIPPER_POSITION_UPPER] + + [GRIPPER_STATE_UPPER], + dtype=np.float64, + ) + + self._recorded_joint_limit_cache = ( + names, + recorded_joint_lower_limits, + recorded_joint_upper_limits, + ) + return self._recorded_joint_limit_cache + + def _link_pose_matrix(self, poses, link_name: str) -> np.ndarray: + return poses[link_name].get_matrix().numpy() + + def _camera_parent_poses( + self, + poses, + left_ee_pose: np.ndarray, + right_ee_pose: np.ndarray, + ) -> dict[str, np.ndarray | None]: + return { + "left": self._link_pose_matrix( + poses, self.left_camera_parent_link_name + ), + "right": self._link_pose_matrix( + poses, self.right_camera_parent_link_name + ), + "middle": self._link_pose_matrix( + poses, self.middle_camera_parent_link_name + ), + } + + def compute_camera_extrinsics( + self, + image_extrinsic: dict[str, np.ndarray], + camera_parent_poses: dict[str, np.ndarray | None], + ) -> dict[str, np.ndarray]: + """Convert static camera extrinsics into per-step camera-to-base matrices.""" + + middle_camera_parent_pose = camera_parent_poses.get("middle") + return { + "left": image_extrinsic["left"] + @ np.linalg.inv(camera_parent_poses["left"]), + "right": image_extrinsic["right"] + @ np.linalg.inv(camera_parent_poses["right"]), + "middle": image_extrinsic["middle"] + @ np.linalg.inv(middle_camera_parent_pose) + if middle_camera_parent_pose is not None + else np.copy(image_extrinsic["middle"]), + } diff --git a/lightweight-operators/ubt-mcap-pipeline/robo_orchard_lab/dataset/ubt_manipulation/tools/utils.py b/lightweight-operators/ubt-mcap-pipeline/robo_orchard_lab/dataset/ubt_manipulation/tools/utils.py new file mode 100644 index 00000000..ed70a2b5 --- /dev/null +++ b/lightweight-operators/ubt-mcap-pipeline/robo_orchard_lab/dataset/ubt_manipulation/tools/utils.py @@ -0,0 +1,1702 @@ +# Project RoboOrchard +# +# Copyright (c) 2025 Horizon Robotics. All Rights Reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or +# implied. See the License for the specific language governing +# permissions and limitations under the License. + +"""Utility helpers for UBT MCAP inspection and packing. + +This module contains a self-contained subset of functions extracted from +``horizon_manipulation.tools.utils`` that are required by +``mcap_checker_ubt`` and ``mcap_packer_ubt``. It deliberately avoids +importing from ``horizon_manipulation`` so that the ``ubt_manipulation`` +package can be delivered as a standalone unit. +""" + +import csv +import hashlib +import html +import json +import logging +import os +import subprocess +from dataclasses import asdict +from datetime import datetime, timezone +from pathlib import Path +from typing import Any, Iterator + +import cv2 +import matplotlib.pyplot as plt +import numpy as np +import torch +from scipy.spatial.transform import Rotation + +logger = logging.getLogger(__name__) + +FFMPEG_SILENT_LOGLEVEL = "error" +MANUAL_REVIEW_TEMPLATE_PATH = ( + Path(__file__).resolve().parent / "templates" / "manual_review.html" +) +INSPECT_ERROR_LOG_TEMPLATE_PATH = ( + Path(__file__).resolve().parent / "templates" / "inspect_error_log.html" +) + +MANUAL_REVIEW_RULE_SUMMARIES = { + "missing_topic": "missing topic", + "empty_stream": "empty stream", + "fps_out_of_range": "fps low", + "interval_spike_or_drop_frame": "fps spike/drop", + "start_ts_mismatch": "start ts", + "end_ts_mismatch": "end ts", + "duration_mismatch": "duration mismatch", + "alignment_time_diff_out_of_range": "time diff", + "timestamp_non_monotonic": "timestamp order", + "joint_limit_violation": "joint limit", + "joint_jump_violation": "joint jump", + "fk_ee_pose_mismatch": "ee mismatch", + "master_slave_joint_gap": "master/follower gap", +} + +REPORT_COLUMNS = [ + "mcap_path", + "topic_integrity: missing_topics", + "data_compression: average_size_error_count", + "topic_fps: camera_frame_rate_error_count", + "topic_fps: robot_state_frame_rate_error_count", + "message_timestamps: start_ts_error_count", + "message_timestamps: end_ts_error_count", + "message_timestamps: duration_error_count", + "robot_state: joint_out_of_bounds_count", + "robot_state: joint_sudden_change_count", + "robot_state: ee_pose_position_diff_count", + "robot_state: ee_pose_angle_diff_count", + "parse_exception", + "total_error_count", +] + +FULL_LOG_TOPIC_ORDER = [ + "/observation/cameras/left/color_image/image_raw", + "/observation/cameras/middle/color_image/image_raw", + "/observation/cameras/right/color_image/image_raw", + "/observation/cameras/left/depth_image/image_raw", + "/observation/cameras/middle/depth_image/image_raw", + "/observation/cameras/right/depth_image/image_raw", + "/observation/robot_state/left/joint", + "/observation/robot_state/right/joint", + "/observation/robot_state/left_master/joint", + "/observation/robot_state/right_master/joint", + "/observation/cameras/left/color_image/camera_info", + "/observation/cameras/middle/color_image/camera_info", + "/observation/cameras/right/color_image/camera_info", + "/observation/cameras/left/depth_image/camera_info", + "/observation/cameras/middle/depth_image/camera_info", + "/observation/cameras/right/depth_image/camera_info", +] + +TOPIC_SUMMARY_TOPIC_WIDTH = 60 +TOPIC_SUMMARY_FPS_WIDTH = 13 +TOPIC_SUMMARY_TS_WIDTH = 35 +TOPIC_SUMMARY_DURATION_WIDTH = 12 +TOPIC_SUMMARY_COUNT_WIDTH = 10 +TOPIC_SUMMARY_SIZE_WIDTH = 13 + + +# --------------------------------------------------------------------------- +# Video encoding helpers +# --------------------------------------------------------------------------- + + +def get_h264_encoder() -> str: + """Return an available ffmpeg H.264 encoder.""" + result = subprocess.run( + ["ffmpeg", "-hide_banner", "-encoders"], + check=True, + capture_output=True, + text=True, + ) + encoders = result.stdout + for encoder in ("libx264", "libopenh264", "h264_v4l2m2m"): + if encoder in encoders: + return encoder + raise RuntimeError("No supported H.264 encoder found in ffmpeg") + + +class FfmpegVideoWriter: + """Stream raw BGR frames to ffmpeg and encode them as mp4.""" + + def __init__( + self, + video_file: str, + frame_size, + fps: int, + enable_ffmpeg_log: bool = False, + ): + width, height = frame_size + cmd = [ + "ffmpeg", + ] + if not enable_ffmpeg_log: + cmd.extend(["-hide_banner", "-loglevel", FFMPEG_SILENT_LOGLEVEL]) + cmd.extend( + [ + "-y", + "-f", + "rawvideo", + "-vcodec", + "rawvideo", + "-pix_fmt", + "bgr24", + "-s", + f"{width}x{height}", + "-r", + str(fps), + "-i", + "-", + "-an", + "-c:v", + get_h264_encoder(), + "-pix_fmt", + "yuv420p", + "-movflags", + "+faststart", + video_file, + ] + ) + popen_kwargs = {"stdin": subprocess.PIPE} + if not enable_ffmpeg_log: + popen_kwargs["stdout"] = subprocess.DEVNULL + popen_kwargs["stderr"] = subprocess.DEVNULL + self.process = subprocess.Popen(cmd, **popen_kwargs) + self.stdin = self.process.stdin + + def write(self, frame: np.ndarray) -> None: + if self.stdin is None: + raise RuntimeError("ffmpeg stdin is not available") + self.stdin.write(frame.tobytes()) + + def release(self) -> None: + if self.stdin is not None and not self.stdin.closed: + self.stdin.close() + returncode = self.process.wait() + if returncode != 0: + raise RuntimeError( + f"ffmpeg exited with non-zero status: {returncode}" + ) + + +def concat_videos( + video_files, output_path: str, enable_ffmpeg_log: bool = False +) -> None: + """Concatenate rendered episode videos into one output mp4. + + Args: + video_files: Individual episode video paths. + output_path: Output directory for the concat artifact. + """ + + if not video_files: + return + list_file = os.path.join(output_path, "video_files.txt") + with open(list_file, "w") as f: + for file in video_files: + f.write(f"file '{os.path.abspath(file)}'\n") + cmd = [ + "ffmpeg", + ] + if not enable_ffmpeg_log: + cmd.extend(["-hide_banner", "-loglevel", FFMPEG_SILENT_LOGLEVEL]) + cmd.extend( + [ + "-f", + "concat", + "-safe", + "0", + "-i", + list_file, + "-c", + "copy", + os.path.join(output_path, "concat_videos.mp4"), + ] + ) + run_kwargs = {"check": True} + if not enable_ffmpeg_log: + run_kwargs["stdout"] = subprocess.DEVNULL + run_kwargs["stderr"] = subprocess.DEVNULL + subprocess.run(cmd, **run_kwargs) + + +# --------------------------------------------------------------------------- +# Report helpers +# --------------------------------------------------------------------------- + + +def build_job_summary(results, output_path: str | None = None): + """Aggregate batch-level counts and artifact lists from episode results. + + Args: + results: Episode report payloads for the batch. + output_path: Optional output directory used to resolve artifact paths. + + Returns: + dict: Aggregate batch summary. + """ + + if output_path is None: + output_path = "" + rule_hit_counts = {} + runtime_failure_count = 0 + video_files = [] + report_files = [] + problematic_data_files = [] + clean_data_files = [] + for item in results: + data_file = item.get("source_path") or item.get("uuid") + if item.get("runtime_error"): + runtime_failure_count += 1 + if item.get("video_file"): + video_files.append(item["video_file"]) + if item.get("report_file"): + report_files.append(item["report_file"]) + if item.get("episode_status") == "pass": + clean_data_files.append(data_file) + else: + problematic_data_files.append(data_file) + for rule in item.get("rule_results", []): + rule_id = ( + rule["rule_id"] if isinstance(rule, dict) else rule.rule_id + ) + rule_hit_counts[rule_id] = rule_hit_counts.get(rule_id, 0) + 1 + return { + "total_episodes": len(results), + "pass_count": sum( + item.get("episode_status") == "pass" for item in results + ), + "warning_count": sum( + item.get("episode_status") == "warning" for item in results + ), + "fail_count": sum( + item.get("episode_status") == "fail" for item in results + ), + "runtime_failure_count": runtime_failure_count, + "parse_error_count": runtime_failure_count, + "rule_hit_counts": rule_hit_counts, + "report_dir": os.path.join(output_path, "reports"), + "video_dir": output_path, + "report_files": report_files, + "video_files": video_files, + "problematic_data_files": problematic_data_files, + "clean_data_files": clean_data_files, + } + + +def iter_rule_results(item: dict) -> Iterator[dict]: + for rule in item.get("rule_results", []): + if isinstance(rule, dict): + yield rule + else: + yield rule.to_dict() + + +def count_topics( + metric_value: str | list[str] | tuple[str, ...] | None, +) -> int: + if not metric_value: + return 0 + if isinstance(metric_value, (list, tuple, set)): + return len([topic for topic in metric_value if str(topic).strip()]) + return len([topic for topic in metric_value.split(",") if topic]) + + +def build_report_row(item: dict) -> dict[str, int | float | str]: + row = {column: 0 for column in REPORT_COLUMNS} + row["mcap_path"] = ( + item.get("mcap_path") + or item.get("source_path") + or item.get("uuid") + or "" + ) + row["parse_exception"] = bool(item.get("runtime_error")) + + for rule in iter_rule_results(item): + rule_id = rule.get("rule_id") + metrics = rule.get("metrics") or {} + if rule_id == "missing_topic": + row["topic_integrity: missing_topics"] += count_topics( + metrics.get("missing_topics") + ) + elif rule_id == "fps_out_of_range": + for topic in metrics: + if "/observation/cameras/" in topic: + row["topic_fps: camera_frame_rate_error_count"] += 1 + elif "/observation/robot_state/" in topic: + row["topic_fps: robot_state_frame_rate_error_count"] += 1 + elif rule_id == "interval_spike_or_drop_frame": + for topic in metrics: + if "/observation/cameras/" in topic: + row["topic_fps: camera_frame_rate_error_count"] += 1 + elif "/observation/robot_state/" in topic: + row["topic_fps: robot_state_frame_rate_error_count"] += 1 + elif rule_id == "start_ts_mismatch": + row["message_timestamps: start_ts_error_count"] += len(metrics) + elif rule_id == "end_ts_mismatch": + row["message_timestamps: end_ts_error_count"] += len(metrics) + elif rule_id in { + "duration_mismatch", + "alignment_time_diff_out_of_range", + }: + row["message_timestamps: duration_error_count"] += len(metrics) + elif rule_id == "timestamp_non_monotonic": + row["message_timestamps: duration_error_count"] += 1 + elif rule_id in {"joint_limit_violation", "gripper_position_out_of_range"}: + row["robot_state: joint_out_of_bounds_count"] += 1 + elif rule_id == "joint_jump_violation": + row["robot_state: joint_sudden_change_count"] += 1 + elif rule_id == "fk_ee_pose_mismatch": + if metrics.get("position_gap", 0) > 0: + row["robot_state: ee_pose_position_diff_count"] += 1 + if metrics.get("orientation_gap", 0) > 0: + row["robot_state: ee_pose_angle_diff_count"] += 1 + + row["total_error_count"] = sum( + int(row[column]) + for column in REPORT_COLUMNS + if column not in {"mcap_path", "total_error_count"} + ) + return row + + +# --------------------------------------------------------------------------- +# Formatting helpers +# --------------------------------------------------------------------------- + + +def format_numeric_detail_table( + label_header: str, rows: list[tuple[str, float, float, float]] +) -> list[str]: + """Render aligned numeric rule details as a plain-text table. + + Args: + label_header: Header for the left-most label column. + rows: Numeric rows in ``(label, actual, limit, gap)`` format. + + Returns: + list[str]: Rendered table lines. + """ + + if not rows: + return [] + label_width = max( + len(label_header), + max(len(label) for label, *_ in rows), + ) + numeric_headers = ("actual", "limit", "gap") + numeric_width = max( + 10, + *( + len(f"{value:.6f}") + for _, actual, limit, gap in rows + for value in (actual, limit, gap) + ), + ) + header = ( + f" {label_header:<{label_width}} " + f"{numeric_headers[0]:>{numeric_width}} " + f"{numeric_headers[1]:>{numeric_width}} " + f"{numeric_headers[2]:>{numeric_width}}" + ) + separator = ( + f" {'-' * label_width} " + f"{'-' * numeric_width} " + f"{'-' * numeric_width} " + f"{'-' * numeric_width}" + ) + lines = [header, separator] + for label, actual, limit, gap in rows: + lines.append( + f" {label:<{label_width}} " + f"{actual:>{numeric_width}.6f} " + f"{limit:>{numeric_width}.6f} " + f"{gap:>{numeric_width}.6f}" + ) + return lines + + +def format_text_detail_table( + headers: tuple[str, ...], rows: list[tuple[str, ...]] +) -> list[str]: + """Render aligned text rule details as a plain-text table. + + Args: + headers: Table headers. + rows: Text rows matching the header layout. + + Returns: + list[str]: Rendered table lines. + """ + + if not rows: + return [] + widths = [ + max(len(headers[idx]), max(len(row[idx]) for row in rows)) + for idx in range(len(headers)) + ] + header = " " + " ".join( + f"{headers[idx]:<{widths[idx]}}" for idx in range(len(headers)) + ) + separator = " " + " ".join( + "-" * widths[idx] for idx in range(len(headers)) + ) + lines = [header, separator] + for row in rows: + lines.append( + " " + + " ".join( + f"{row[idx]:<{widths[idx]}}" for idx in range(len(headers)) + ) + ) + return lines + + +def format_rule_detail_lines(rule: dict, inspect_config) -> list[str]: + """Expand rule metrics into human-readable full-log detail lines. + + Args: + rule: Serialized rule result. + inspect_config: Effective inspection thresholds. + + Returns: + list[str]: Detail lines for the full log. + """ + + if inspect_config is None: + return [] + + rule_id = rule.get("rule_id") + metrics = rule.get("metrics") or {} + if rule_id == "fps_out_of_range": + lines = [] + numeric_rows = [] + for topic in sorted(metrics): + actual = metrics[topic] + if "/observation/cameras/" in topic: + mean_limit = inspect_config.camera_topics_mean_fps_limit + min_limit = inspect_config.camera_topics_min_fps_limit + elif "/observation/robot_state/" in topic: + mean_limit = inspect_config.robot_state_topics_mean_fps_limit + min_limit = inspect_config.robot_state_topics_min_fps_limit + else: + continue + if isinstance(actual, dict): + mean_fps = float(actual.get("mean_fps", 0.0)) + min_fps = float(actual.get("min_fps", 0.0)) + max_interval = float(actual.get("max_interval", 0.0)) + max_interval_limit = float( + actual.get("max_interval_limit", 0.0) + ) + exceed_count = int(actual.get("max_interval_exceed_count", 0)) + lines.append( + f" Topic '{topic}': mean_fps={mean_fps:.6f}, " + f"mean_fps_limit={mean_limit:.6f}, " + f"min_fps={min_fps:.6f}, min_fps_limit={min_limit:.6f}, " + f"max_interval={max_interval:.6f}, " + f"max_interval_limit={max_interval_limit:.6f}, " + f"max_interval_exceed_count={exceed_count}" + ) + continue + actual_value = float(actual) + numeric_rows.append( + (topic, actual_value, mean_limit, actual_value - mean_limit) + ) + return lines + format_numeric_detail_table("topic", numeric_rows) + if rule_id == "interval_spike_or_drop_frame": + lines = [] + for topic in sorted(metrics): + actual = metrics[topic] + if "/observation/cameras/" in topic: + min_limit = inspect_config.camera_topics_min_fps_limit + elif "/observation/robot_state/" in topic: + min_limit = inspect_config.robot_state_topics_min_fps_limit + else: + min_limit = float(actual.get("min_fps_limit", 0.0)) + lines.append( + f" Topic '{topic}': " + f"mean_fps={float(actual.get('mean_fps', 0.0)):.6f}, " + "mean_fps_limit=" + f"{float(actual.get('mean_fps_limit', 0.0)):.6f}, " + f"min_fps={float(actual.get('min_fps', 0.0)):.6f}, " + f"min_fps_limit={min_limit:.6f}" + ) + return lines + if rule_id in { + "start_ts_mismatch", + "end_ts_mismatch", + "duration_mismatch", + }: + label_map = { + "start_ts_mismatch": ("actual_start", "expected_start"), + "end_ts_mismatch": ("actual_end", "expected_end"), + "duration_mismatch": ("actual_duration", "expected_duration"), + } + actual_label, expected_label = label_map[rule_id] + lines = [] + for topic in sorted(metrics): + actual = metrics[topic] + lines.append( + f" Topic '{topic}': " + f"{actual_label}={float(actual.get('actual', 0.0)):.6f}, " + f"{expected_label}={float(actual.get('expected', 0.0)):.6f}, " + f"delta={float(actual.get('delta', 0.0)):.6f}" + ) + return lines + if rule_id == "alignment_time_diff_out_of_range": + lines = [] + for topic in sorted(metrics): + actual = metrics[topic] + lines.append( + f" Topic '{topic}': " + "max_time_diff=" + f"{float(actual.get('max_time_diff', 0.0)):.6f}, " + "mean_time_diff=" + f"{float(actual.get('mean_time_diff', 0.0)):.6f}, " + f"limit={float(actual.get('limit', 0.0)):.6f}" + ) + return lines + + if rule_id == "joint_jump_violation" and "max_joint_delta" in metrics: + actual = float(metrics["max_joint_delta"]) + limit = inspect_config.joint_change_tolerance + return format_numeric_detail_table( + "metric", + [("max_joint_delta", actual, limit, actual - limit)], + ) + if rule_id == "master_slave_joint_gap" and "max_gap" in metrics: + actual = float(metrics["max_gap"]) + limit = inspect_config.master_slave_joint_tolerance + return format_numeric_detail_table( + "metric", + [("max_gap", actual, limit, actual - limit)], + ) + if rule_id == "fk_ee_pose_mismatch": + rows = [] + if "position_gap" in metrics: + actual = float(metrics["position_gap"]) + limit = inspect_config.ee_pose_position_tolerance + rows.append(("position_gap", actual, limit, actual - limit)) + if "orientation_gap" in metrics: + actual = float(metrics["orientation_gap"]) + limit = inspect_config.ee_pose_orientation_tolerance + rows.append(("orientation_gap", actual, limit, actual - limit)) + return format_numeric_detail_table("metric", rows) + if rule_id == "joint_limit_violation": + if { + "joint_name", + "step_index", + "joint_index", + "actual", + "lower_limit", + "upper_limit", + "max_violation", + }.issubset(metrics): + return format_text_detail_table( + ( + "joint", + "step", + "index", + "actual", + "lower", + "upper", + "violation", + ), + [ + ( + str(metrics["joint_name"]), + str(metrics["step_index"]), + str(metrics["joint_index"]), + f"{float(metrics['actual']):.6f}", + f"{float(metrics['lower_limit']):.6f}", + f"{float(metrics['upper_limit']):.6f}", + f"{float(metrics['max_violation']):.6f}", + ) + ], + ) + if "max_joint_value" in metrics: + actual = float(metrics["max_joint_value"]) + limit = np.pi * 2 + inspect_config.joint_limit_tolerance + return format_numeric_detail_table( + "metric", + [("max_joint_value", actual, limit, actual - limit)], + ) + return [] + if rule_id == "timestamp_non_monotonic" and "min_delta" in metrics: + actual = float(metrics["min_delta"]) + limit = -inspect_config.timestamp_limit + return format_numeric_detail_table( + "metric", + [("min_delta", actual, limit, actual - limit)], + ) + if rule_id == "missing_topic" and "missing_topics" in metrics: + topics = [ + topic.strip() + for topic in str(metrics["missing_topics"]).split(",") + if topic.strip() + ] + return [ + f"Error: Topic '{topic}' is missing in the mcap file." + for topic in topics + ] + if rule_id == "empty_stream" and "empty_topics" in metrics: + topics = [ + topic.strip() + for topic in str(metrics["empty_topics"]).split(",") + if topic.strip() + ] + return [ + f"Error: Topic '{topic}' has no usable messages in the mcap file." + for topic in topics + ] + return [] + + +def write_rule_block(fh, rule: dict, inspect_config) -> None: + """Write one rule block with an Error/Warning prefix and detail lines.""" + + level = "Error" if rule.get("status") == "fail" else "Warning" + fh.write(f"{level}: {rule.get('rule_id')}: {rule.get('message', '')}\n") + for detail_line in format_rule_detail_lines(rule, inspect_config): + fh.write(f"{detail_line}\n") + fh.write("\n") + + +def format_timestamp(timestamp_ns: int | None) -> str: + """Format a nanosecond timestamp as an RFC3339 UTC string. + + Args: + timestamp_ns: Timestamp in nanoseconds. + + Returns: + str: RFC3339-formatted UTC timestamp, or an empty string. + """ + + if timestamp_ns is None: + return "" + sec = timestamp_ns // int(1e9) + nsec = timestamp_ns % int(1e9) + dt = datetime.fromtimestamp(sec, tz=timezone.utc) + return f"{dt.strftime('%Y-%m-%dT%H:%M:%S')}.{nsec:09d}Z" + + +def topic_sort_key(topic: str) -> tuple[int, str]: + """Sort known topics in a stable, inspection-friendly order.""" + + try: + return (FULL_LOG_TOPIC_ORDER.index(topic), topic) + except ValueError: + return (len(FULL_LOG_TOPIC_ORDER), topic) + + +def iter_logged_topics( + topic_summaries: dict[str, dict], +) -> list[tuple[str, dict]]: + """Filter and sort topics that should appear in the full log table.""" + + return sorted( + ( + (topic, summary) + for topic, summary in topic_summaries.items() + if topic != "/tf_static" and not topic.endswith("/end_pose") + ), + key=lambda item: topic_sort_key(item[0]), + ) + + +def _topic_summary_has_timestamps(summary: dict) -> bool: + return ( + summary.get("start_time_ns") is not None + and summary.get("end_time_ns") is not None + ) + + +def format_topic_summary_line(topic: str, summary: dict) -> str: + """Format one topic summary row for the full log FPS table.""" + + duration = ( + (summary["end_time_ns"] - summary["start_time_ns"]) / 1e9 + if summary["count"] > 1 and _topic_summary_has_timestamps(summary) + else 0.0 + ) + mean_fps = summary.get("mean_frequency", 0.0) + min_fps = summary.get("min_frequency", 0.0) + size_mb = summary["size_bytes"] / 1e6 + return ( + f"{topic:^{TOPIC_SUMMARY_TOPIC_WIDTH}} | " + f"{f'{mean_fps:.3f} hz':^{TOPIC_SUMMARY_FPS_WIDTH}} | " + f"{f'{min_fps:.3f} hz':^{TOPIC_SUMMARY_FPS_WIDTH}} | " + f"{format_timestamp(summary['start_time_ns']):^{TOPIC_SUMMARY_TS_WIDTH}}" + " | " + f"{format_timestamp(summary['end_time_ns']):^{TOPIC_SUMMARY_TS_WIDTH}}" + " | " + f"{f'{duration:.3f} s':^{TOPIC_SUMMARY_DURATION_WIDTH}} | " + f"{str(summary['count']):^{TOPIC_SUMMARY_COUNT_WIDTH}} | " + f"{f'{size_mb:.3f} MB':^{TOPIC_SUMMARY_SIZE_WIDTH}}" + ) + + +def format_topic_summary_header() -> str: + """Format the topic summary table header.""" + + return ( + f"{'Topic':^{TOPIC_SUMMARY_TOPIC_WIDTH}} | " + f"{'Mean FPS':^{TOPIC_SUMMARY_FPS_WIDTH}} | " + f"{'Min FPS':^{TOPIC_SUMMARY_FPS_WIDTH}} | " + f"{'Start TS':^{TOPIC_SUMMARY_TS_WIDTH}} | " + f"{'End TS':^{TOPIC_SUMMARY_TS_WIDTH}} | " + f"{'Duration':^{TOPIC_SUMMARY_DURATION_WIDTH}} | " + f"{'Count':^{TOPIC_SUMMARY_COUNT_WIDTH}} | " + f"{'Size':^{TOPIC_SUMMARY_SIZE_WIDTH}}" + ) + + +# --------------------------------------------------------------------------- +# Full-log writing +# --------------------------------------------------------------------------- + + +def write_full_log_episode(fh, item: dict, inspect_config) -> None: + """Write the full inspection log block for one episode.""" + + mcap_path = ( + item.get("mcap_path") + or item.get("source_path") + or item.get("uuid") + or "" + ) + topic_summaries = item.get("topic_summaries") or {} + separator = "=" * 24 + fh.write(f"{separator} BEGIN MCAP: {mcap_path} {separator}\n") + if not topic_summaries: + fh.write(f"Starting data checks: mcap_path is {mcap_path}\n") + for rule in iter_rule_results(item): + if rule.get("status") in {"warning", "fail"}: + write_rule_block(fh, rule, inspect_config) + if item.get("runtime_error"): + fh.write(f"ERROR parse_exception: {item['runtime_error']}\n") + fh.write(f"{separator} END MCAP: {mcap_path} {separator}\n") + return + + rule_results = list(iter_rule_results(item)) + rule_map = {rule.get("rule_id"): rule for rule in rule_results} + logged_topics = iter_logged_topics(topic_summaries) + all_steps = item.get("all_steps") + non_static_steps = item.get("non_static_steps") + + fh.write(f"Starting data checks: mcap_path is {mcap_path}\n") + if all_steps is not None and non_static_steps is not None: + fh.write( + f"all steps: {int(all_steps)}, non static steps: " + f"{int(non_static_steps)}\n" + ) + fh.write("Checking topic integrity...\n") + missing_rule = rule_map.get("missing_topic") + if missing_rule: + missing_topics = [ + topic.strip() + for topic in str( + (missing_rule.get("metrics") or {}).get("missing_topics", "") + ).split(",") + if topic.strip() + ] + for topic in missing_topics: + fh.write(f"Error: Topic '{topic}' is missing in the mcap file.\n") + fh.write(f"Error: Missing {len(missing_topics)} topic(s).\n") + else: + fh.write("All topics are present in the mcap file.\n") + empty_rule = rule_map.get("empty_stream") + if empty_rule: + empty_topics = [ + topic.strip() + for topic in str( + (empty_rule.get("metrics") or {}).get("empty_topics", "") + ).split(",") + if topic.strip() + ] + for topic in empty_topics: + fh.write( + "Error: Topic " + f"'{topic}' has no usable messages in the mcap file.\n" + ) + + fh.write("Checking camera FPS...\n") + fh.write(f"{format_topic_summary_header()}\n") + for topic, summary in logged_topics: + fh.write(f"{format_topic_summary_line(topic, summary)}\n") + fps_rule = rule_map.get("fps_out_of_range") + if fps_rule: + for topic, actual in sorted((fps_rule.get("metrics") or {}).items()): + if "/observation/cameras/" in topic: + mean_limit = inspect_config.camera_topics_mean_fps_limit + elif "/observation/robot_state/" in topic: + mean_limit = inspect_config.robot_state_topics_mean_fps_limit + else: + continue + if isinstance(actual, dict): + mean_fps = float(actual.get("mean_fps", 0.0)) + min_fps = float(actual.get("min_fps", 0.0)) + max_interval = float(actual.get("max_interval", 0.0)) + max_interval_exceed_count = int( + actual.get("max_interval_exceed_count", 0) + ) + fh.write( + "Error: Topic " + f"'{topic}' has a frame rate below the limit. " + f"mean_fps={mean_fps}, mean_fps_limit={mean_limit}, " + f"min_fps={min_fps}, " + f"max_interval={max_interval}, " + f"max_interval_exceed_count={max_interval_exceed_count}\n" + ) + continue + fh.write( + f"Error: Topic '{topic}' has a frame rate below the limit. " + f"mean_fps is {actual}, mean_fps_limit is {mean_limit}\n" + ) + interval_rule = rule_map.get("interval_spike_or_drop_frame") + if interval_rule: + for topic, actual in sorted( + (interval_rule.get("metrics") or {}).items() + ): + if "/observation/cameras/" in topic: + min_limit = inspect_config.camera_topics_min_fps_limit + elif "/observation/robot_state/" in topic: + min_limit = inspect_config.robot_state_topics_min_fps_limit + else: + min_limit = float(actual.get("min_fps_limit", 0.0)) + fh.write( + "Error: Topic " + f"'{topic}' has a min frame rate below the limit. " + f"mean_fps={actual.get('mean_fps')}, " + f"min_fps={actual.get('min_fps')}, " + f"min_fps_limit={min_limit}\n" + ) + + fh.write("Checking data compression...\n") + total_size = sum(summary["size_bytes"] for _, summary in logged_topics) + fh.write(f"Total size of the mcap file is {total_size / 1e6:.3f} MB.\n") + timed_topics = [ + (topic, summary) + for topic, summary in logged_topics + if _topic_summary_has_timestamps(summary) + ] + average_duration = ( + sum( + (summary["end_time_ns"] - summary["start_time_ns"]) / 1e9 + for _, summary in timed_topics + ) + / len(timed_topics) + if timed_topics + else 0.0 + ) + average_size = ( + total_size / average_duration if average_duration > 0 else 0.0 + ) + fh.write( + f"Average size of the mcap file is {average_size / 1e6:.3f} MB/s.\n" + ) + if average_size > 100 * 1e6: + fh.write("Warning: Data Average Size is too large.\n") + + fh.write("Checking message timestamps...\n") + start_ns = ( + min(summary["start_time_ns"] for _, summary in timed_topics) + if timed_topics + else None + ) + end_ns = ( + max(summary["end_time_ns"] for _, summary in timed_topics) + if timed_topics + else None + ) + duration = ( + (end_ns - start_ns) / 1e9 + if start_ns is not None and end_ns is not None + else 0.0 + ) + fh.write(f"Minimum start time is {format_timestamp(start_ns)}.\n") + fh.write(f"Maximum end time is {format_timestamp(end_ns)}.\n") + fh.write(f"Total duration is {duration} s.\n") + timestamp_rule = rule_map.get("timestamp_non_monotonic") + if timestamp_rule: + min_delta = (timestamp_rule.get("metrics") or {}).get("min_delta") + fh.write( + "Error: base timestamps are not strictly increasing. " + f"min_delta is {min_delta} s.\n" + ) + start_rule = rule_map.get("start_ts_mismatch") + if start_rule: + for topic, metric in sorted((start_rule.get("metrics") or {}).items()): + fh.write( + "Warning: start timestamp of topic " + f"'{topic}' differs from base timeline. " + f"expected_start={metric.get('expected')}, " + f"actual_start={metric.get('actual')}, " + f"delta={metric.get('delta')} s.\n" + ) + end_rule = rule_map.get("end_ts_mismatch") + if end_rule: + for topic, metric in sorted((end_rule.get("metrics") or {}).items()): + fh.write( + "Warning: end timestamp of topic " + f"'{topic}' differs from base timeline. " + f"expected_end={metric.get('expected')}, " + f"actual_end={metric.get('actual')}, " + f"delta={metric.get('delta')} s.\n" + ) + duration_rule = rule_map.get("duration_mismatch") + if duration_rule: + for topic, metric in sorted( + (duration_rule.get("metrics") or {}).items() + ): + fh.write( + "Warning: duration of topic " + f"'{topic}' differs from base timeline. " + f"expected_duration={metric.get('expected')}, " + f"actual_duration={metric.get('actual')}, " + f"delta={metric.get('delta')} s.\n" + ) + alignment_rule = rule_map.get("alignment_time_diff_out_of_range") + if alignment_rule: + for topic, metric in sorted( + (alignment_rule.get("metrics") or {}).items() + ): + fh.write( + "Warning: aligned timestamps of topic " + f"'{topic}' exceed tolerance. " + f"max_time_diff={metric.get('max_time_diff')}, " + f"mean_time_diff={metric.get('mean_time_diff')}, " + f"limit={metric.get('limit')} s.\n" + ) + + fh.write("Checking robot state with URDF joint limit...\n") + fh.write("Checking joint state smoothness...\n") + fh.write("Checking end-effector pose and joint state matchness...\n") + if "joint_limit_violation" in rule_map: + joint_limit_metrics = ( + rule_map["joint_limit_violation"].get("metrics") or {} + ) + if { + "joint_name", + "step_index", + "joint_index", + "actual", + "lower_limit", + "upper_limit", + "max_violation", + }.issubset(joint_limit_metrics): + fh.write( + "Error: Joint position exceeds URDF joint limit, " + f"joint={joint_limit_metrics['joint_name']}, " + f"step_index={joint_limit_metrics['step_index']}, " + f"joint_index={joint_limit_metrics['joint_index']}, " + f"actual={joint_limit_metrics['actual']}, " + f"lower_limit={joint_limit_metrics['lower_limit']}, " + f"upper_limit={joint_limit_metrics['upper_limit']}, " + "max_violation=" + f"{joint_limit_metrics['max_violation']}\n" + ) + else: + max_joint_value = joint_limit_metrics.get("max_joint_value") + fh.write( + "Error: Joint position exceeds configured limit, " + f"max_joint_value: {max_joint_value}\n" + ) + elif "gripper_position_out_of_range" in rule_map: + gp_metrics = ( + rule_map["gripper_position_out_of_range"].get("metrics") or {} + ) + fh.write( + "Error: Gripper position out of valid range [0, 1], " + f"joint={gp_metrics.get('joint_name')}, " + f"step_index={gp_metrics.get('step_index')}, " + f"actual={gp_metrics.get('actual')}, " + f"max_violation={gp_metrics.get('max_violation')}\n" + ) + else: + fh.write("All joint positions are within the limits.\n") + if "joint_jump_violation" in rule_map: + max_joint_delta = ( + rule_map["joint_jump_violation"].get("metrics") or {} + ).get("max_joint_delta") + fh.write( + "Warning: Adjacent joint delta exceeds tolerance, " + f"max_joint_delta: {max_joint_delta}\n" + ) + else: + fh.write("All joint positions are smooth.\n") + fk_rule = rule_map.get("fk_ee_pose_mismatch") + if fk_rule: + fk_metrics = fk_rule.get("metrics") or {} + if "position_gap" in fk_metrics: + fh.write( + "Warning: End-effector pose position is not consistent " + "with joint state, " + f"position difference: {fk_metrics['position_gap']} meters.\n" + ) + if "orientation_gap" in fk_metrics: + fh.write( + "Warning: End-effector pose orientation is not " + "consistent with joint state, " + f"angle difference: {fk_metrics['orientation_gap']} radians.\n" + ) + else: + fh.write("All end-effector pose positions are consistent.\n") + fh.write("All end-effector pose orientations are consistent.\n") + if "master_slave_joint_gap" in rule_map: + max_gap = ( + rule_map["master_slave_joint_gap"].get("metrics") or {} + ).get("max_gap") + fh.write( + "Warning: master and follower joints diverge, " + f"max_gap: {max_gap}\n" + ) + if item.get("runtime_error"): + runtime_error = item["runtime_error"] + fh.write( + f"When processing {mcap_path}, error occurred: {runtime_error}\n" + ) + else: + fh.write(f"{mcap_path} checks completed successfully.\n") + fh.write(f"{separator} END MCAP: {mcap_path} {separator}\n") + + +def build_manual_review_error_log_anchor(source_id: str) -> str: + """Build a stable DOM anchor for one error-log block.""" + + digest = hashlib.sha1(source_id.encode("utf-8")).hexdigest()[:12] + return f"error-{digest}" + + +def is_quick_screen_pass_row(row: dict[str, Any]) -> bool: + """Return whether a row passes the simplified PASS/ERROR screening.""" + + return ( + row["topic_integrity: missing_topics"] == 0 + and row["topic_fps: camera_frame_rate_error_count"] == 0 + and row["topic_fps: robot_state_frame_rate_error_count"] == 0 + and row["robot_state: joint_out_of_bounds_count"] == 0 + and row["parse_exception"] == 0 + ) + + +def write_mcap_lists(output_path: str, rows: list[dict[str, Any]]) -> None: + """Write one merged PASS/ERROR list using the simplified screening rules.""" + + merged_path = os.path.join(output_path, "inspect_mcap_result.log") + no_error_rows = [row for row in rows if is_quick_screen_pass_row(row)] + error_rows = [row for row in rows if row not in no_error_rows] + with open(merged_path, "w", encoding="utf-8") as fh: + for row in error_rows: + fh.write(f"ERROR {row['mcap_path']}\n") + for row in no_error_rows: + fh.write(f"PASS {row['mcap_path']}\n") + + +def _count_rendered_frames(num_steps: int, vis_interval: int) -> int: + """Return the number of frames rendered for one episode video.""" + + step_interval = max(1, int(vis_interval)) + if num_steps <= 0: + return 0 + return (num_steps + step_interval - 1) // step_interval + + +def build_manual_review_timeline( + results: list[dict[str, Any]], vis_interval: int, vis_fps: int +) -> dict[str, Any]: + """Build a concat timeline for manual review playback.""" + + render_fps = max(1, round(vis_fps / max(1, vis_interval))) + segments = [] + current_start = 0.0 + for index, item in enumerate( + item for item in results if item.get("video_file") + ): + video_file = item["video_file"] + num_steps = int(item.get("num_steps", 0)) + frame_count = _count_rendered_frames(num_steps, vis_interval) + duration_sec = frame_count / render_fps if frame_count > 0 else 0.0 + if duration_sec <= 0: + continue + mcap_id = item.get("uuid") or item.get("mcap_path") or "" + segment = { + "index": index, + "mcap_id": mcap_id, + "mcap_path": item.get("mcap_path") + or item.get("source_path") + or "", + "video_file": os.path.basename(video_file), + "start_sec": round(current_start, 6), + "end_sec": round(current_start + duration_sec, 6), + "duration_sec": round(duration_sec, 6), + } + segments.append(segment) + current_start += duration_sec + return { + "concat_video": "concat_videos.mp4", + "generated_at": datetime.now(timezone.utc) + .astimezone() + .isoformat(timespec="seconds"), + "segments": segments, + } + + +def build_manual_review_note_summary(item: dict[str, Any]) -> str: + """Build a short system-generated summary for manual review.""" + + labels: list[str] = [] + if item.get("runtime_error"): + labels.append("runtime error") + for rule in iter_rule_results(item): + rule_id = str(rule.get("rule_id")) + label = MANUAL_REVIEW_RULE_SUMMARIES.get(rule_id) + if not label or label in labels: + continue + labels.append(label) + if len(labels) >= 3: + break + return " / ".join(labels) + + +def build_manual_review_failures( + results: list[dict[str, Any]], timeline: dict[str, Any] +) -> dict[str, Any]: + """Build the initial manual review failure payload.""" + + segment_by_mcap_id = { + segment["mcap_id"]: segment for segment in timeline.get("segments", []) + } + items = [] + for item in results: + report_row = build_report_row(item) + if is_quick_screen_pass_row(report_row): + continue + mcap_id = item.get("uuid") or report_row["mcap_path"] or "" + segment = segment_by_mcap_id.get(mcap_id) + items.append( + { + "mcap_id": mcap_id, + "mcap_path": report_row["mcap_path"], + "video_file": segment["video_file"] if segment else "", + "mark_time_sec": segment["start_sec"] if segment else None, + "note_summary": build_manual_review_note_summary(item), + "error_log_anchor": ( + build_manual_review_error_log_anchor( + report_row["mcap_path"] or mcap_id + ) + if segment + else "" + ), + "note": "", + } + ) + items.sort( + key=lambda item: ( + float("inf") + if item["mark_time_sec"] is None + else item["mark_time_sec"], + item["mcap_id"], + ) + ) + return { + "concat_video": "concat_videos.mp4", + "updated_at": datetime.now(timezone.utc) + .astimezone() + .isoformat(timespec="seconds"), + "items": items, + } + + +def render_manual_review_page( + timeline: dict[str, Any], failures: dict[str, Any], storage_key: str +) -> str: + """Render the standalone manual review page with embedded data.""" + + template_html = MANUAL_REVIEW_TEMPLATE_PATH.read_text(encoding="utf-8") + bootstrap = { + "timeline": timeline, + "failures": failures, + "storage_key": storage_key, + } + return template_html.replace( + "__MANUAL_REVIEW_BOOTSTRAP__", + json.dumps(bootstrap, ensure_ascii=False), + ) + + +def write_manual_review_page( + output_path: str, + timeline: dict[str, Any], + failures: dict[str, Any], +) -> str: + """Write a standalone manual review HTML page into one output directory.""" + + html_path = os.path.join(output_path, "manual_review.html") + failure_seed = "|".join( + ( + f"{item['mcap_id']}::{item.get('note_summary', '')}" + f"::{item.get('error_log_anchor', '')}" + ) + for item in failures.get("items", []) + ) + failure_seed_digest = hashlib.sha1( + failure_seed.encode("utf-8") + ).hexdigest()[:12] + concat_video = timeline.get("concat_video", "concat_videos.mp4") + storage_key = ( + f"manualReviewFailures::{concat_video}" + f"::{len(timeline.get('segments', []))}" + f"::{len(failures.get('items', []))}" + f"::{failure_seed_digest}" + ) + template_html = render_manual_review_page(timeline, failures, storage_key) + with open(html_path, "w", encoding="utf-8") as fh: + fh.write(template_html) + return html_path + + +def write_manual_review_artifacts( + output_path: str, + results: list[dict[str, Any]], + vis_interval: int, + vis_fps: int, +) -> tuple[str, str, str] | tuple[None, None, None]: + """Write manual-review timeline and failure placeholders.""" + + timeline = build_manual_review_timeline(results, vis_interval, vis_fps) + if not timeline["segments"]: + return None, None, None + failures = build_manual_review_failures(results, timeline) + os.makedirs(output_path, exist_ok=True) + page_path = write_manual_review_page(output_path, timeline, failures) + timeline_path = os.path.join(output_path, "manual_review_timeline.json") + failures_path = os.path.join(output_path, "manual_review_failures.json") + with open(timeline_path, "w", encoding="utf-8") as fh: + json.dump(timeline, fh, indent=2, ensure_ascii=False) + fh.write("\n") + with open(failures_path, "w", encoding="utf-8") as fh: + json.dump(failures, fh, indent=2, ensure_ascii=False) + fh.write("\n") + return page_path, timeline_path, failures_path + + +def render_inspect_error_log_page( + blocks: list[dict[str, str]], generated_at: str +) -> str: + """Render a standalone HTML view for inspect_error_log blocks.""" + + template_html = INSPECT_ERROR_LOG_TEMPLATE_PATH.read_text(encoding="utf-8") + if blocks: + blocks_html = "\n".join( + "".join( + [ + '
', + '
', + '#', + f"{html.escape(block['mcap_path'])}", + "
", + ( + '
'
+                        f"{html.escape(block['body'])}"
+                        "
" + ), + "
", + ] + ) + for block in blocks + ) + else: + blocks_html = ( + '
' + "当前没有可展示的 inspect error block。" + "
" + ) + return template_html.replace( + "__INSPECT_ERROR_LOG_GENERATED_AT__", generated_at + ).replace("__INSPECT_ERROR_LOG_BLOCKS__", blocks_html) + + +def write_inspect_error_log_page( + output_path: str, blocks: list[dict[str, str]] +) -> str: + """Write the HTML companion page for inspect_error_log.log.""" + + html_path = os.path.join(output_path, "inspect_error_log.html") + generated_at = ( + datetime.now(timezone.utc).astimezone().isoformat(timespec="seconds") + ) + page_html = render_inspect_error_log_page(blocks, generated_at) + with open(html_path, "w", encoding="utf-8") as fh: + fh.write(page_html) + return html_path + + +def write_error_logs( + output_path: str, + results, + inspect_config, + runtime_config: dict[str, Any] | None = None, +) -> None: + """Write the full log and derive the compact signal log from it. + + Args: + output_path: Output directory for the log files. + results: Episode report payloads for the batch. + inspect_config: Effective inspection thresholds. + runtime_config: Optional runtime configuration payload for log headers. + """ + + full_log_path = os.path.join(output_path, "inspect_full_log.log") + with open(full_log_path, "w", encoding="utf-8") as fh: + if runtime_config is not None: + fh.write("Running with the following configurations:\n") + fh.write(json.dumps(runtime_config, indent=2) + "\n") + if inspect_config is not None: + fh.write("Inspecting with the following configurations:\n") + fh.write(json.dumps(asdict(inspect_config), indent=2) + "\n") + for item in results: + write_full_log_episode(fh, item, inspect_config) + fh.write("\n") + + with open(full_log_path, "r", encoding="utf-8") as full_fh: + full_lines = full_fh.readlines() + + block_separator = "=" * 24 + + def _extract_signal_lines(lines: list[str]) -> list[str]: + selected_indices: list[int] = [] + seen_indices: set[int] = set() + + def _append_index(index: int) -> None: + if index in seen_indices: + return + selected_indices.append(index) + seen_indices.add(index) + + has_camera_min_fps_error = any( + line.startswith("Error: Topic '/observation/cameras/") + and "has a min frame rate below the limit" in line + for line in lines + ) + if has_camera_min_fps_error: + camera_section_start = next( + ( + idx + for idx, line in enumerate(lines) + if line.startswith("Checking camera FPS...") + ), + None, + ) + if camera_section_start is not None: + camera_section_end = next( + ( + idx + for idx in range(camera_section_start + 1, len(lines)) + if lines[idx].startswith( + "Checking data compression..." + ) + ), + len(lines), + ) + for idx in range(camera_section_start, camera_section_end): + _append_index(idx) + + for idx, line in enumerate(lines): + if ( + line.startswith("Warning:") + or line.startswith("Error:") + or line.startswith("ERROR ") + ): + _append_index(idx) + detail_idx = idx + 1 + while detail_idx < len(lines): + detail_line = lines[detail_idx] + if not detail_line.strip(): + break + if ( + detail_line.startswith("Warning:") + or detail_line.startswith("Error:") + or detail_line.startswith("ERROR ") + or detail_line.startswith("Checking ") + ): + break + _append_index(detail_idx) + detail_idx += 1 + + return [lines[idx] for idx in selected_indices] + + def _iter_episodes(lines: list[str]): + begin_prefix = block_separator + " BEGIN MCAP:" + end_prefix = block_separator + " END MCAP:" + in_episode = False + current = [] + for line in lines: + if line.startswith(begin_prefix): + in_episode = True + current = [line] + continue + if in_episode: + current.append(line) + if line.startswith(end_prefix): + yield current + in_episode = False + current = [] + + def _extract_mcap_path(header: str) -> str: + prefix = block_separator + " BEGIN MCAP: " + suffix = " " + block_separator + header = header.strip() + if header.startswith(prefix) and header.endswith(suffix): + return header[len(prefix) : -len(suffix)] + return header + + error_blocks: list[dict[str, str]] = [] + for episode in _iter_episodes(full_lines): + signal_lines = _extract_signal_lines(episode) + if not signal_lines: + continue + header = next( + ( + line.strip() + for line in episode + if line.startswith(block_separator + " BEGIN MCAP:") + ), + None, + ) + if header is None: + continue + mcap_path = _extract_mcap_path(header) + error_blocks.append( + { + "header": header, + "mcap_path": mcap_path, + "anchor": build_manual_review_error_log_anchor(mcap_path), + "body": "".join(signal_lines).rstrip(), + } + ) + + error_log_path = os.path.join(output_path, "inspect_error_log.log") + with open(error_log_path, "w", encoding="utf-8") as error_fh: + for block in error_blocks: + error_fh.write(f"{block['header']}\n") + if block["body"]: + error_fh.write(f"{block['body']}\n") + error_fh.write("\n") + + write_inspect_error_log_page(output_path, error_blocks) + + +# --------------------------------------------------------------------------- +# Data processing helpers +# --------------------------------------------------------------------------- + + +def sample(tgt_time, src_time, src_data=None, prefix=""): + """Align a source timeline to the target timeline by nearest timestamp. + + Args: + tgt_time: Target timeline. + src_time: Source timeline to be sampled. + src_data: Optional source payloads aligned with ``src_time``. + prefix: Prefix used in logging. + + Returns: + np.ndarray | tuple[np.ndarray, list]: + Aligned timestamps, and aligned data when ``src_data`` is + provided. + """ + + insert_pos = np.searchsorted(src_time, tgt_time, side="left") + left_idx = np.clip(insert_pos - 1, 0, src_time.shape[0] - 1) + right_idx = np.clip(insert_pos, 0, src_time.shape[0] - 1) + left_diff = np.abs(tgt_time - src_time[left_idx]) + right_diff = np.abs(src_time[right_idx] - tgt_time) + index = np.where(left_diff <= right_diff, left_idx, right_idx) + output_time = src_time[index] + aligned_diff = np.abs(tgt_time - output_time) + logger.info( + f"{prefix:<50} - " + + f"max time diff: {aligned_diff.max():.4f}, " + + f"mean time diff: {aligned_diff.mean():.4f}" + ) + if src_data is not None: + output = [] + for src in src_data: + _output = [] + for i in index: + _output.append(src[i]) + output.append(_output) + return output_time, output + return output_time + + +def format_time(timestamp): + """Convert ROS-style sec/nsec timestamps to floating-point seconds. + + Args: + timestamp: Timestamp array in ``[sec, nsec]`` format. + + Returns: + np.ndarray: Timestamps expressed in seconds. + """ + + timestamp = np.asarray(timestamp, dtype="float64") + if timestamp.size == 0: + return np.array([], dtype=np.float64) + if timestamp.ndim == 1: + timestamp = timestamp.reshape(-1, 2) + timestamp = timestamp[:, 0] + timestamp[:, 1] / 1e9 + return timestamp + + +def pose_to_mat(pose): + """Convert a pose-like object or dict into a 4x4 transform matrix. + + Args: + pose: Pose payload expressed as a dict or ROS-like object. + + Returns: + np.ndarray: Homogeneous 4x4 transform matrix. + """ + + if isinstance(pose, dict): + x, y, z = pose["position"] + qx, qy, qz, w = pose["orientation"] + elif hasattr(pose, "position"): + x, y, z = pose.position.x, pose.position.y, pose.position.z + qx, qy, qz, w = ( + pose.orientation.x, + pose.orientation.y, + pose.orientation.z, + pose.orientation.w, + ) + else: + x, y, z = pose.translation.x, pose.translation.y, pose.translation.z + qx, qy, qz, w = ( + pose.rotation.x, + pose.rotation.y, + pose.rotation.z, + pose.rotation.w, + ) + + trans = np.array([x, y, z]) + rot = Rotation.from_quat([qx, qy, qz, w], scalar_first=False).as_matrix() + ret = np.eye(4) + ret[:3, 3] = trans + ret[:3, :3] = rot + return ret + + +def get_frequency(timestamp, prefix="", window_size=3): + """Estimate smoothed per-step frequency values from a timestamp series. + + Args: + timestamp: Timestamp series in seconds. + prefix: Prefix used in logging. + window_size: Moving-average window used before inverting intervals. + + Returns: + np.ndarray: Per-step frequency estimates. + """ + + if not isinstance(timestamp, np.ndarray): + timestamp = np.array(timestamp) + time_diff = np.diff(timestamp) + if time_diff.size == 0: + return np.array([], dtype=np.float64) + if time_diff.size < window_size: + freq = 1 / time_diff + logger.info( + f"{prefix:<50} - " + f"duration: {timestamp[-1] - timestamp[0]:.2f}s, " + + f"min frequency: {freq.min():.1f}Hz, " + + f"mean frequency: {freq.mean():.1f}Hz" + ) + return freq + time_diff = torch.from_numpy(time_diff)[None, None] + time_diff = torch.nn.functional.avg_pool1d( + time_diff, window_size, 1 + ).numpy()[0, 0] + freq = 1 / time_diff + logger.info( + f"{prefix:<50} - " + f"duration: {timestamp[-1] - timestamp[0]:.2f}s, " + + f"min frequency: {freq.min():.1f}Hz, " + + f"mean frequency: {freq.mean():.1f}Hz" + ) + return freq + + +# --------------------------------------------------------------------------- +# Image / depth decoding helpers +# --------------------------------------------------------------------------- + + +def decode_img(img_buffer): + """Decode a compressed image buffer into a BGR numpy array.""" + + img_buffer = np.ndarray( + shape=(1, len(img_buffer)), dtype=np.uint8, buffer=img_buffer + ) + return cv2.imdecode(img_buffer, cv2.IMREAD_ANYCOLOR) + + +def decode_depth(depth_buffer, depth_scale=1000): + """Decode a compressed depth buffer and convert to metres.""" + + return ( + cv2.imdecode( + np.frombuffer(depth_buffer, np.uint8), cv2.IMREAD_UNCHANGED + ) + / depth_scale + ) + + +def depth_visualize(depth, min_depth=0.01, max_depth=1.2, mode="bwr"): + """Render a depth map as a false-colour BGR image. + + Args: + depth: Depth values in metres. + min_depth: Near clip for colour mapping. + max_depth: Far clip for colour mapping. + mode: Matplotlib colormap name. + + Returns: + np.ndarray: uint8 BGR image. + """ + + mask = depth > 0 + cmap = plt.cm.get_cmap(mode, 256) + cmap = np.array([cmap(i) for i in range(256)])[:, :3] * 255 + cmap = cmap[::-1] + + depth_shape = depth.shape + if max_depth is None: + max_depth = depth.max() + if min_depth is None: + min_depth = depth.min() + + depth = (depth - min_depth) / (max_depth - min_depth) + index = np.int32(depth * 255) + index = np.clip(index, a_min=0, a_max=255) + depth_color = cmap[index].reshape(*depth_shape, 3) + depth_color = np.where(mask[..., None], depth_color, 0) + return np.uint8(depth_color) diff --git a/lightweight-operators/ubt-mcap-pipeline/ubt_pipeline/__init__.py b/lightweight-operators/ubt-mcap-pipeline/ubt_pipeline/__init__.py new file mode 100644 index 00000000..c9d4d6c6 --- /dev/null +++ b/lightweight-operators/ubt-mcap-pipeline/ubt_pipeline/__init__.py @@ -0,0 +1,3 @@ +"""Drobodata UBT MCAP preparation, checking, and reporting pipeline.""" + +__version__ = "1.0.0" diff --git a/lightweight-operators/ubt-mcap-pipeline/ubt_pipeline/check.py b/lightweight-operators/ubt-mcap-pipeline/ubt_pipeline/check.py new file mode 100644 index 00000000..292b8d0a --- /dev/null +++ b/lightweight-operators/ubt-mcap-pipeline/ubt_pipeline/check.py @@ -0,0 +1,263 @@ +from __future__ import annotations + +import json +import multiprocessing +import os +import pathlib +import shutil +from dataclasses import asdict +from typing import Any + +from .common import ( + artifact, + csv_values, + emit_result, + env_bool, + env_int, + env_text, + find_json_payload, + load_input_artifacts, + normalize_dates, + write_json, +) + +OPERATOR_ID = "ubt-mcap-batch-check" +ROBO_ORCHARD_SOURCE_COMMIT = "c742b39e0821590605385ee4ddabcafb258432d8" + + +def _inspect_config(): + from robo_orchard_lab.dataset.ubt_manipulation.tools.check_config import ( + InspectConfig, + build_default_inspect_config, + ) + + value = env_text("INSPECT_CONFIG_JSON") + defaults = asdict(build_default_inspect_config()) + if value: + overrides = json.loads(value) + if not isinstance(overrides, dict): + raise ValueError("INSPECT_CONFIG_JSON must decode to an object") + unknown = sorted(set(overrides) - set(defaults)) + if unknown: + raise ValueError( + f"INSPECT_CONFIG_JSON contains unknown keys: {', '.join(unknown)}" + ) + defaults.update(overrides) + return InspectConfig(**defaults) + + +def _run_handlers(checker: Any) -> list[dict[str, Any]]: + indexes = list(range(len(checker.episodes))) + if checker.num_workers > 1: + with multiprocessing.Pool(processes=checker.num_workers) as pool: + results = pool.map(checker.handler, indexes) + else: + results = [checker.handler(index) for index in indexes] + results.sort(key=lambda item: item["uuid"]) + return results + + +def classify_report(item: dict[str, Any]) -> str: + from robo_orchard_lab.dataset.ubt_manipulation.tools.utils import ( + build_report_row, + is_quick_screen_pass_row, + ) + + if item.get("runtime_error") or item.get("video_runtime_error"): + return "RUNTIME_ERROR" + return "PASS" if is_quick_screen_pass_row(build_report_row(item)) else "ERROR" + + +def _episode_name(item: dict[str, Any]) -> str: + source_path = pathlib.Path( + str(item.get("source_path") or item.get("mcap_path") or item.get("uuid")) + ) + for candidate in (source_path, *source_path.parents): + if candidate.name.startswith("episode_"): + return candidate.name + uuid = str(item.get("uuid", "")) + return uuid if uuid.startswith("episode_") else f"episode_{uuid}" + + +def _result_payload( + raw: dict[str, Any], prepared_entry: dict[str, Any] | None +) -> dict[str, Any]: + status = classify_report(raw) + episode = _episode_name(raw) + return { + "episode": episode, + "date": episode[len("episode_") : len("episode_") + 10], + "status": status, + "sourcePath": raw.get("source_path", ""), + "mcapPath": raw.get("mcap_path", ""), + "preparedPath": ( + prepared_entry.get("preparedPath", "") + if prepared_entry + else raw.get("source_path", "") + ), + "source": prepared_entry.get("source", {}) if prepared_entry else {}, + "runtimeError": raw.get("runtime_error") or raw.get("video_runtime_error"), + "episodeStatus": raw.get("episode_status"), + "ruleResults": [ + rule if isinstance(rule, dict) else rule.to_dict() + for rule in raw.get("rule_results", []) + ], + "episodeMetrics": raw.get("episode_metrics", {}), + "topicSummaries": raw.get("topic_summaries", {}), + "numSteps": raw.get("num_steps", 0), + "allSteps": raw.get("all_steps", 0), + "nonStaticSteps": raw.get("non_static_steps", 0), + "videoFile": raw.get("video_file"), + } + + +def _rule_statistics(results: list[dict[str, Any]]) -> dict[str, int]: + counts: dict[str, int] = {} + for result in results: + for rule in result["ruleResults"]: + if str(rule.get("status", "")).lower() in {"pass", "ok"}: + continue + rule_id = str(rule.get("rule_id", "unknown")) + counts[rule_id] = counts.get(rule_id, 0) + 1 + return dict(sorted(counts.items())) + + +def run_check() -> None: + try: + from robo_orchard_lab.dataset.ubt_manipulation.tools.mcap_checker_ubt import ( + UbtMcapChecker, + ) + except ImportError as exc: + raise RuntimeError(f"UBT checker dependency is missing: {exc}") from exc + + prepared = find_json_payload(load_input_artifacts(), "prepared-manifest.json") + prepared_root = pathlib.Path(str(prepared.get("preparedRoot", ""))) + urdf_path = pathlib.Path(str(prepared.get("urdfPath", ""))) + if not prepared_root.is_dir(): + raise RuntimeError(f"prepared data directory is unreadable: {prepared_root}") + if not urdf_path.is_file(): + raise RuntimeError(f"prepared URDF is unreadable: {urdf_path}") + + dates = normalize_dates(env_text("DATE_PREFIXES")) + users = csv_values(env_text("USER_NAMES", prepared.get("userName", "ubt"))) + tasks = csv_values(env_text("TASK_NAMES", prepared.get("taskName", "test_task"))) + num_workers = max(1, env_int("NUM_WORKERS", 1)) + max_episodes = max(0, env_int("MAX_EPISODES", 0)) + video_export = env_bool("VIDEO_EXPORT", False) + fail_on_quality_error = env_bool("FAIL_ON_QUALITY_ERROR", False) + vis_interval = max(1, env_int("VIS_INTERVAL", 3)) + vis_resize_scale = float(env_text("VIS_RESIZE_SCALE", "1.0")) + vis_depth = env_bool("VIS_DEPTH", True) + + output_dir = pathlib.Path(env_text("OUTPUT_DIR", "/tmp/droboticflow-ubt-check")) + raw_output = output_dir / "raw-checker" + if raw_output.exists(): + shutil.rmtree(raw_output) + raw_output.mkdir(parents=True) + + checker = UbtMcapChecker( + input_path=str(prepared_root), + output_path=str(raw_output), + urdf=str(urdf_path), + task_names=tasks or None, + user_names=users or None, + date_prefix=",".join(dates) if dates else None, + num_workers=num_workers, + vis_interval=vis_interval, + vis_resize_scale=vis_resize_scale, + vis_depth=vis_depth, + inspect_config=_inspect_config(), + skip_video_export=not video_export, + ) + matched_episode_count = len(checker.episodes) + if matched_episode_count == 0: + raise RuntimeError("UBT checker matched zero episodes") + if max_episodes: + checker.episodes = checker.episodes[:max_episodes] + + raw_results = _run_handlers(checker) + if video_export: + checker.render_videos(raw_results) + checker.concat_videos( + [item["video_file"] for item in raw_results if item.get("video_file")] + ) + checker.write_job_outputs(raw_results) + checker.write_manual_review_artifacts(raw_results) + + prepared_by_name = { + str(item.get("episode")): item + for item in prepared.get("episodes", []) + if isinstance(item, dict) + } + episode_results = [ + _result_payload(item, prepared_by_name.get(_episode_name(item))) + for item in raw_results + ] + status_counts = { + status: sum(1 for item in episode_results if item["status"] == status) + for status in ("PASS", "ERROR", "RUNTIME_ERROR") + } + summary = { + "schemaVersion": "1.0", + "datasetId": prepared.get("datasetId"), + "datasetFingerprint": prepared.get("datasetFingerprint"), + "urdfFingerprint": prepared.get("urdfFingerprint"), + "sourceCommit": ROBO_ORCHARD_SOURCE_COMMIT, + "imageDigest": env_text("IMAGE_DIGEST", env_text("DROBOTICFLOW_IMAGE_DIGEST")), + "datePrefixes": dates, + "userNames": users, + "taskNames": tasks, + "matchedEpisodeCount": matched_episode_count, + "processedEpisodeCount": len(episode_results), + "maxEpisodes": max_episodes, + "videoExport": video_export, + "statusCounts": status_counts, + "passRate": status_counts["PASS"] / len(episode_results), + "ruleStatistics": _rule_statistics(episode_results), + "inspectConfig": asdict(checker.inspect_config), + } + episode_results_path = output_dir / "episode-results.json" + checker_summary_path = output_dir / "checker-summary.json" + write_json(episode_results_path, episode_results) + write_json(checker_summary_path, summary) + + artifacts = [ + artifact( + checker_summary_path, + port_id="checker-results", + port_name="质检结果", + data=summary, + ), + artifact( + episode_results_path, + port_id="checker-results", + port_name="质检结果", + data=episode_results, + ), + ] + for path in sorted(raw_output.rglob("*")): + if path.is_file(): + artifacts.append( + artifact( + path, + port_id="checker-results", + port_name="质检结果", + relative_path=f"raw-checker/{path.relative_to(raw_output)}", + ) + ) + emit_result( + output_dir, + OPERATOR_ID, + artifacts, + [ + f"matched episodes: {matched_episode_count}", + f"processed episodes: {len(episode_results)}", + f"status counts: {status_counts}", + ], + ) + if status_counts["RUNTIME_ERROR"]: + raise RuntimeError( + f"{status_counts['RUNTIME_ERROR']} episode(s) failed at runtime" + ) + if fail_on_quality_error and status_counts["ERROR"]: + raise RuntimeError(f"{status_counts['ERROR']} episode(s) failed quality checks") diff --git a/lightweight-operators/ubt-mcap-pipeline/ubt_pipeline/common.py b/lightweight-operators/ubt-mcap-pipeline/ubt_pipeline/common.py new file mode 100644 index 00000000..1b6a8180 --- /dev/null +++ b/lightweight-operators/ubt-mcap-pipeline/ubt_pipeline/common.py @@ -0,0 +1,221 @@ +from __future__ import annotations + +import hashlib +import json +import mimetypes +import os +import pathlib +import re +import time +from typing import Any, Iterable + +DATE_PATTERN = re.compile(r"^\d{4}[-_]\d{2}[-_]\d{2}$") +EPISODE_PATTERN = re.compile(r"^episode_(\d{4}_\d{2}_\d{2})-.+") +RESULT_PREFIX = "DROBOTICFLOW_RESULT_PATH=" + + +def env_text(name: str, default: str = "") -> str: + return os.environ.get(name, default).strip() + + +def env_bool(name: str, default: bool = False) -> bool: + value = env_text(name) + if not value: + return default + if value.lower() in {"1", "true", "yes", "on"}: + return True + if value.lower() in {"0", "false", "no", "off"}: + return False + raise ValueError(f"{name} must be a boolean") + + +def env_int(name: str, default: int) -> int: + value = env_text(name) + return int(value) if value else default + + +def csv_values(value: str | None) -> list[str]: + if not value: + return [] + return [item.strip() for item in value.split(",") if item.strip()] + + +def normalize_dates(value: str | None) -> list[str]: + dates: list[str] = [] + for item in csv_values(value): + if not DATE_PATTERN.fullmatch(item): + raise ValueError( + f"invalid date prefix {item!r}; expected YYYY-MM-DD or YYYY_MM_DD" + ) + normalized = item.replace("-", "_") + if normalized not in dates: + dates.append(normalized) + return dates + + +def safe_identifier(value: str, field: str) -> str: + normalized = value.strip() + if not normalized or normalized in {".", ".."}: + raise ValueError(f"{field} is required") + if "/" in normalized or "\\" in normalized or "\x00" in normalized: + raise ValueError(f"{field} must be a single path segment") + return normalized + + +def read_json(path: pathlib.Path) -> Any: + with path.open("r", encoding="utf-8") as handle: + return json.load(handle) + + +def write_json(path: pathlib.Path, payload: Any) -> None: + path.parent.mkdir(parents=True, exist_ok=True) + with path.open("w", encoding="utf-8") as handle: + json.dump(payload, handle, ensure_ascii=False, indent=2, sort_keys=True) + handle.write("\n") + + +def sha256_file(path: pathlib.Path) -> str: + digest = hashlib.sha256() + with path.open("rb") as handle: + for chunk in iter(lambda: handle.read(4 * 1024 * 1024), b""): + digest.update(chunk) + return digest.hexdigest() + + +def stable_fingerprint(items: Iterable[dict[str, Any]], urdf_sha256: str) -> str: + normalized = [ + { + "key": str(item.get("key", "")), + "size": int(item.get("size", 0) or 0), + "etag": str(item.get("etag", "")).strip('"'), + } + for item in items + ] + normalized.sort(key=lambda item: item["key"]) + encoded = json.dumps( + {"objects": normalized, "urdfSha256": urdf_sha256}, + ensure_ascii=False, + separators=(",", ":"), + sort_keys=True, + ).encode("utf-8") + return hashlib.sha256(encoded).hexdigest() + + +def normalize_input_artifacts(raw: Any) -> list[dict[str, Any]]: + if isinstance(raw, list): + return [item for item in raw if isinstance(item, dict)] + if isinstance(raw, dict): + for key in ("items", "artifacts", "inputArtifacts"): + value = raw.get(key) + if isinstance(value, list): + return [item for item in value if isinstance(item, dict)] + return [] + + +def load_input_artifacts() -> list[dict[str, Any]]: + path = env_text("INPUT_ARTIFACTS_PATH") + if not path: + return [] + return normalize_input_artifacts(read_json(pathlib.Path(path))) + + +def artifact_payload(artifact: dict[str, Any]) -> Any: + data = artifact.get("data") + if data is not None: + return data + raw_path = str(artifact.get("path", "")).strip() + if raw_path and "://" not in raw_path: + path = pathlib.Path(raw_path) + if path.is_file() and path.suffix.lower() == ".json": + return read_json(path) + return None + + +def find_json_payload(artifacts: list[dict[str, Any]], filename: str) -> dict[str, Any]: + for artifact in artifacts: + if str(artifact.get("name", "")).strip() != filename: + continue + payload = artifact_payload(artifact) + if isinstance(payload, dict): + return payload + raise RuntimeError(f"required upstream artifact is missing: {filename}") + + +def now_iso() -> str: + return time.strftime("%Y-%m-%dT%H:%M:%SZ", time.gmtime()) + + +def artifact( + path: pathlib.Path, + *, + port_id: str, + port_name: str, + artifact_type: str | None = None, + data: Any = None, + relative_path: str = "", +) -> dict[str, Any]: + mime_type = mimetypes.guess_type(path.name)[0] or "application/octet-stream" + kind = artifact_type or ("json" if path.suffix.lower() == ".json" else "file") + result: dict[str, Any] = { + "name": path.name, + "type": kind, + "path": str(path), + "portId": port_id, + "portName": port_name, + "sizeBytes": path.stat().st_size, + "metadata": {"mimeType": mime_type, "sha256": sha256_file(path)}, + } + if data is not None: + result["data"] = data + if relative_path: + result["relativePath"] = relative_path + return result + + +def emit_result( + output_dir: pathlib.Path, + operator_id: str, + artifacts: list[dict[str, Any]], + messages: list[str], +) -> None: + output_dir.mkdir(parents=True, exist_ok=True) + timestamp = now_iso() + result = { + "logs": [ + { + "timestamp": timestamp, + "level": "INFO", + "message": message, + "operator": operator_id, + } + for message in messages + ], + "artifacts": artifacts, + "resourceSeries": [], + } + result_path = pathlib.Path( + env_text("RESULT_OUTPUT_PATH", str(output_dir / "result.json")) + ) + write_json(result_path, result) + stage_manifest_path = pathlib.Path( + env_text("STAGE_MANIFEST_PATH", str(output_dir / "stage-manifest.json")) + ) + write_json( + stage_manifest_path, + { + "operatorId": operator_id, + "dataRefs": [ + { + "name": item["name"], + "path": item["path"], + "type": item["type"], + "portId": item["portId"], + "portName": item["portName"], + "sha256": item["metadata"]["sha256"], + "sizeBytes": item["sizeBytes"], + } + for item in artifacts + ], + }, + ) + print(f"{RESULT_PREFIX}{result_path}", flush=True) diff --git a/lightweight-operators/ubt-mcap-pipeline/ubt_pipeline/prepare.py b/lightweight-operators/ubt-mcap-pipeline/ubt_pipeline/prepare.py new file mode 100644 index 00000000..8442d19a --- /dev/null +++ b/lightweight-operators/ubt-mcap-pipeline/ubt_pipeline/prepare.py @@ -0,0 +1,418 @@ +from __future__ import annotations + +import contextlib +import fnmatch +import os +import pathlib +import shutil +import stat +import tempfile +import urllib.parse +import xml.etree.ElementTree as ET +import zipfile +from typing import Any + +from .common import ( + artifact, + emit_result, + env_bool, + env_int, + env_text, + load_input_artifacts, + safe_identifier, + sha256_file, + stable_fingerprint, + write_json, +) + +OPERATOR_ID = "ubt-data-prepare" + + +def parse_bos_path(value: str) -> tuple[str, str]: + parsed = urllib.parse.urlparse(value) + if parsed.scheme != "bos" or not parsed.netloc or not parsed.path.lstrip("/"): + raise ValueError(f"invalid BOS path: {value}") + return parsed.netloc, parsed.path.lstrip("/") + + +def build_bos_client(): + try: + import baidubce.protocol as protocol + from baidubce.auth.bce_credentials import BceCredentials + from baidubce.bce_client_configuration import BceClientConfiguration + from baidubce.services.bos.bos_client import BosClient + except ImportError as exc: + raise RuntimeError("BOS SDK is required for UBT data preparation") from exc + + endpoint = env_text("BOS_ENDPOINT") + access_key = env_text("BOS_ACCESS_KEY_ID") + secret_key = env_text("BOS_SECRET_ACCESS_KEY") + if not endpoint or not access_key or not secret_key: + raise RuntimeError("BOS_ENDPOINT and BOS credentials are required") + parsed = urllib.parse.urlparse( + endpoint if "://" in endpoint else f"https://{endpoint}" + ) + config = BceClientConfiguration( + credentials=BceCredentials(access_key, secret_key), + endpoint=parsed.netloc or parsed.path, + protocol=( + protocol.HTTPS + if (parsed.scheme or "https").lower() == "https" + else protocol.HTTP + ), + ) + return BosClient(config) + + +def safe_extract_zip(archive_path: pathlib.Path, destination: pathlib.Path) -> None: + destination.mkdir(parents=True, exist_ok=True) + root = destination.resolve() + with zipfile.ZipFile(archive_path) as archive: + for member in archive.infolist(): + target = (destination / member.filename).resolve() + if target != root and root not in target.parents: + raise RuntimeError( + f"archive entry escapes extraction root: {member.filename}" + ) + unix_mode = member.external_attr >> 16 + if stat.S_ISLNK(unix_mode): + raise RuntimeError(f"archive symlink is not allowed: {member.filename}") + archive.extractall(destination) + + +def validate_urdf(path: pathlib.Path) -> str: + try: + root = ET.parse(path).getroot() + except (ET.ParseError, OSError) as exc: + raise RuntimeError(f"invalid URDF: {path.name}") from exc + if root.tag != "robot" or not list(root.iter("joint")): + raise RuntimeError("URDF must contain a robot root and at least one joint") + return sha256_file(path) + + +def discover_episode_dirs(root: pathlib.Path) -> list[pathlib.Path]: + return sorted( + path + for path in root.rglob("episode_*") + if path.is_dir() and path.name.startswith("episode_") + ) + + +def validate_episode(path: pathlib.Path) -> None: + mcap_files = list(path.glob("*.mcap")) + if not mcap_files: + raise RuntimeError(f"episode is missing MCAP: {path.name}") + if any(item.is_dir() for item in path.iterdir()): + raise RuntimeError(f"episode contains an unexpected directory: {path.name}") + + +def claim_episode(path: pathlib.Path, seen: set[str]) -> str: + validate_episode(path) + episode_name = path.name + if episode_name in seen: + raise RuntimeError(f"duplicate episode: {episode_name}") + seen.add(episode_name) + return episode_name + + +def _source_manifest_entries(artifacts: list[dict[str, Any]]) -> list[dict[str, Any]]: + entries: list[dict[str, Any]] = [] + for item in artifacts: + raw_path = str(item.get("path", "")).strip() + if not raw_path.startswith("bos://"): + continue + bucket, key = parse_bos_path(raw_path) + metadata = ( + item.get("metadata") if isinstance(item.get("metadata"), dict) else {} + ) + entries.append( + { + "bucket": bucket, + "key": key, + "path": raw_path, + "size": int(item.get("sizeBytes", item.get("size", 0)) or 0), + "etag": str(metadata.get("etag", metadata.get("eTag", ""))), + } + ) + return entries + + +def _metadata_value(response: Any, *names: str) -> Any: + for name in names: + if isinstance(response, dict) and response.get(name) is not None: + return response[name] + value = getattr(response, name, None) + if value is not None: + return value + return None + + +def enrich_object_metadata(client: Any, item: dict[str, Any]) -> dict[str, Any]: + if item["size"] and item["etag"]: + return item + try: + metadata = client.get_object_meta_data(item["bucket"], item["key"]) + except Exception as exc: + raise RuntimeError(f"cannot read BOS object metadata: {item['path']}") from exc + enriched = dict(item) + enriched["size"] = int( + enriched["size"] + or _metadata_value(metadata, "content_length", "contentLength", "size") + or 0 + ) + enriched["etag"] = str( + enriched["etag"] or _metadata_value(metadata, "etag", "e_tag", "eTag") or "" + ).strip('"') + return enriched + + +def download_object( + client: Any, item: dict[str, Any], destination: pathlib.Path +) -> None: + destination.parent.mkdir(parents=True, exist_ok=True) + partial = destination.with_name(destination.name + ".part") + with contextlib.suppress(FileNotFoundError): + partial.unlink() + try: + client.get_object_to_file(item["bucket"], item["key"], str(partial)) + if item["size"] and partial.stat().st_size != item["size"]: + raise RuntimeError( + f"download size mismatch for {item['path']}: " + f"{partial.stat().st_size} != {item['size']}" + ) + partial.replace(destination) + finally: + with contextlib.suppress(FileNotFoundError): + partial.unlink() + + +def verify_cache(cache_dir: pathlib.Path, fingerprint: str) -> dict[str, Any] | None: + manifest_path = cache_dir / "prepared-manifest.json" + complete_path = cache_dir / ".complete.json" + if not manifest_path.is_file() or not complete_path.is_file(): + return None + try: + manifest = __import__("json").loads(manifest_path.read_text("utf-8")) + complete = __import__("json").loads(complete_path.read_text("utf-8")) + except Exception: + return None + if ( + manifest.get("datasetFingerprint") != fingerprint + or complete.get("datasetFingerprint") != fingerprint + ): + return None + episode_paths = manifest.get("episodes") + if not isinstance(episode_paths, list) or len(episode_paths) != complete.get( + "episodeCount" + ): + return None + for episode in episode_paths: + prepared = pathlib.Path(str(episode.get("preparedPath", ""))) + if not prepared.is_dir() or not list(prepared.glob("*.mcap")): + return None + return manifest + + +def _disk_free_gib(path: pathlib.Path) -> float: + path.mkdir(parents=True, exist_ok=True) + return shutil.disk_usage(path).free / (1024**3) + + +def _select_objects( + entries: list[dict[str, Any]], archive_glob: str, urdf_source_path: str +) -> tuple[list[dict[str, Any]], dict[str, Any]]: + archives = [ + item + for item in entries + if fnmatch.fnmatch(pathlib.PurePosixPath(item["key"]).name, archive_glob) + ] + urdf_matches = [ + item + for item in entries + if item["path"] == urdf_source_path + or item["key"] == urdf_source_path.lstrip("/") + or pathlib.PurePosixPath(item["key"]).name + == pathlib.PurePosixPath(urdf_source_path).name + ] + if not archives: + raise RuntimeError(f"no archive matches ARCHIVE_GLOB={archive_glob!r}") + if len(urdf_matches) != 1: + raise RuntimeError( + f"URDF_SOURCE_PATH must match exactly one source object; got {len(urdf_matches)}" + ) + return sorted(archives, key=lambda item: item["key"]), urdf_matches[0] + + +def run_prepare() -> None: + dataset_id = safe_identifier(env_text("DATASET_ID"), "DATASET_ID") + user_name = safe_identifier(env_text("USER_NAME", "ubt"), "USER_NAME") + task_name = safe_identifier(env_text("TASK_NAME", "test_task"), "TASK_NAME") + archive_glob = env_text("ARCHIVE_GLOB", "*.zip") + urdf_source_path = env_text("URDF_SOURCE_PATH") + reuse_prepared = env_bool("REUSE_PREPARED", True) + min_free_gib = env_int("MIN_FREE_GIB", 50) + duplicate_policy = env_text("ON_DUPLICATE_EPISODE", "error").lower() + if duplicate_policy != "error": + raise ValueError("ON_DUPLICATE_EPISODE currently supports only 'error'") + + prepared_root = pathlib.Path( + env_text("PREPARED_ROOT", "/data/ubt-prepared") + ).resolve() + free_gib = _disk_free_gib(prepared_root) + if free_gib < min_free_gib: + raise RuntimeError( + f"shared prepared storage has {free_gib:.2f} GiB free; " + f"MIN_FREE_GIB={min_free_gib}" + ) + + client = build_bos_client() + entries = [ + enrich_object_metadata(client, item) + for item in _source_manifest_entries(load_input_artifacts()) + ] + archives, urdf_object = _select_objects(entries, archive_glob, urdf_source_path) + + scratch_root = prepared_root / ".staging" + scratch_root.mkdir(parents=True, exist_ok=True) + with tempfile.TemporaryDirectory( + prefix=f"{dataset_id}-", dir=scratch_root + ) as scratch_text: + scratch = pathlib.Path(scratch_text) + urdf_local = scratch / pathlib.PurePosixPath(urdf_object["key"]).name + download_object(client, urdf_object, urdf_local) + urdf_sha256 = validate_urdf(urdf_local) + fingerprint = stable_fingerprint(archives, urdf_sha256) + cache_dir = prepared_root / dataset_id / fingerprint + + if reuse_prepared: + cached = verify_cache(cache_dir, fingerprint) + if cached is not None: + cached = dict(cached) + cached["cacheHit"] = True + cached["freeGiBBefore"] = round(free_gib, 3) + output_dir = pathlib.Path( + env_text("OUTPUT_DIR", "/tmp/droboticflow-ubt-prepare") + ) + output_manifest = output_dir / "prepared-manifest.json" + write_json(output_manifest, cached) + emit_result( + output_dir, + OPERATOR_ID, + [ + artifact( + output_manifest, + port_id="prepared-manifest", + port_name="准备清单", + data=cached, + ) + ], + [ + f"prepared cache hit: {fingerprint}", + f"episode count: {cached['episodeCount']}", + ], + ) + return + + build_dir = prepared_root / dataset_id / f".{fingerprint}.building" + if build_dir.exists(): + shutil.rmtree(build_dir) + episode_root = build_dir / user_name / task_name + episode_root.mkdir(parents=True) + shutil.copy2(urdf_local, build_dir / urdf_local.name) + + episodes: list[dict[str, Any]] = [] + seen: set[str] = set() + for index, archive_object in enumerate(archives): + local_archive = ( + scratch + / f"{index:02d}-{pathlib.PurePosixPath(archive_object['key']).name}" + ) + extract_dir = scratch / f"extract-{index:02d}" + download_object(client, archive_object, local_archive) + safe_extract_zip(local_archive, extract_dir) + with contextlib.suppress(FileNotFoundError): + local_archive.unlink() + discovered = discover_episode_dirs(extract_dir) + if not discovered: + raise RuntimeError( + f"archive contains no episode directory: {archive_object['path']}" + ) + for source_episode in discovered: + episode_name = claim_episode(source_episode, seen) + destination = episode_root / episode_name + shutil.move(str(source_episode), destination) + episodes.append( + { + "episode": episode_name, + "date": episode_name[len("episode_") : len("episode_") + 10], + "preparedPath": str(destination), + "source": { + "path": archive_object["path"], + "key": archive_object["key"], + "size": archive_object["size"], + "etag": archive_object["etag"], + }, + } + ) + shutil.rmtree(extract_dir) + + episodes.sort(key=lambda item: item["episode"]) + manifest = { + "schemaVersion": "1.0", + "datasetId": dataset_id, + "datasetFingerprint": fingerprint, + "urdfFingerprint": urdf_sha256, + "urdfPath": str(build_dir / urdf_local.name), + "preparedRoot": str(build_dir), + "userName": user_name, + "taskName": task_name, + "archiveCount": len(archives), + "episodeCount": len(episodes), + "cacheHit": False, + "freeGiBBefore": round(free_gib, 3), + "sourceObjects": archives + [urdf_object], + "episodes": episodes, + } + write_json(build_dir / "prepared-manifest.json", manifest) + write_json( + build_dir / ".complete.json", + { + "datasetFingerprint": fingerprint, + "episodeCount": len(episodes), + }, + ) + cache_dir.parent.mkdir(parents=True, exist_ok=True) + if cache_dir.exists(): + shutil.rmtree(cache_dir) + build_dir.replace(cache_dir) + + # Paths are stable only after the atomic rename. + old_root = manifest["preparedRoot"] + manifest["preparedRoot"] = str(cache_dir) + manifest["urdfPath"] = str(cache_dir / urdf_local.name) + for episode in manifest["episodes"]: + episode["preparedPath"] = episode["preparedPath"].replace( + old_root, str(cache_dir), 1 + ) + write_json(cache_dir / "prepared-manifest.json", manifest) + + output_dir = pathlib.Path(env_text("OUTPUT_DIR", "/tmp/droboticflow-ubt-prepare")) + output_manifest = output_dir / "prepared-manifest.json" + write_json(output_manifest, manifest) + emit_result( + output_dir, + OPERATOR_ID, + [ + artifact( + output_manifest, + port_id="prepared-manifest", + port_name="准备清单", + data=manifest, + ) + ], + [ + f"prepared dataset fingerprint: {manifest['datasetFingerprint']}", + f"archive count: {manifest['archiveCount']}", + f"episode count: {manifest['episodeCount']}", + ], + ) diff --git a/lightweight-operators/ubt-mcap-pipeline/ubt_pipeline/report.py b/lightweight-operators/ubt-mcap-pipeline/ubt_pipeline/report.py new file mode 100644 index 00000000..3d9c9932 --- /dev/null +++ b/lightweight-operators/ubt-mcap-pipeline/ubt_pipeline/report.py @@ -0,0 +1,301 @@ +from __future__ import annotations + +import csv +import html +import pathlib +import shutil +from collections import Counter, defaultdict +from typing import Any + +from .common import ( + artifact, + artifact_payload, + emit_result, + env_text, + find_json_payload, + load_input_artifacts, + write_json, +) + +OPERATOR_ID = "ubt-quality-report" + + +def _episode_results(artifacts: list[dict[str, Any]]) -> list[dict[str, Any]]: + for item in artifacts: + if str(item.get("name", "")) != "episode-results.json": + continue + payload = artifact_payload(item) + if isinstance(payload, list) and all(isinstance(row, dict) for row in payload): + return payload + raise RuntimeError("required upstream artifact is missing: episode-results.json") + + +def _rule_failures(result: dict[str, Any]) -> list[str]: + return [ + str(rule.get("rule_id", "unknown")) + for rule in result.get("ruleResults", []) + if str(rule.get("status", "")).lower() not in {"pass", "ok"} + ] + + +def _date_statistics(results: list[dict[str, Any]]) -> dict[str, dict[str, int]]: + output: dict[str, Counter[str]] = defaultdict(Counter) + for item in results: + output[str(item.get("date", "unknown"))][str(item["status"])] += 1 + return { + date: { + status: counts.get(status, 0) + for status in ("PASS", "ERROR", "RUNTIME_ERROR") + } + for date, counts in sorted(output.items()) + } + + +def _manifest_entry(item: dict[str, Any]) -> dict[str, Any]: + return { + "episode": item.get("episode"), + "date": item.get("date"), + "status": item.get("status"), + "source": item.get("source", {}), + "preparedPath": item.get("preparedPath", ""), + "mcapPath": item.get("mcapPath", ""), + "failedRules": _rule_failures(item), + "runtimeError": item.get("runtimeError"), + } + + +def _write_csv(path: pathlib.Path, results: list[dict[str, Any]]) -> None: + path.parent.mkdir(parents=True, exist_ok=True) + with path.open("w", encoding="utf-8", newline="") as handle: + writer = csv.DictWriter( + handle, + fieldnames=[ + "episode", + "date", + "status", + "failed_rules", + "runtime_error", + "source_bos_path", + "prepared_path", + "mcap_path", + "num_steps", + ], + ) + writer.writeheader() + for item in results: + writer.writerow( + { + "episode": item.get("episode", ""), + "date": item.get("date", ""), + "status": item.get("status", ""), + "failed_rules": ",".join(_rule_failures(item)), + "runtime_error": item.get("runtimeError") or "", + "source_bos_path": (item.get("source") or {}).get("path", ""), + "prepared_path": item.get("preparedPath", ""), + "mcap_path": item.get("mcapPath", ""), + "num_steps": item.get("numSteps", 0), + } + ) + + +def _render_html(summary: dict[str, Any], results: list[dict[str, Any]]) -> str: + rows = [] + for item in results: + failed_rules = ", ".join(_rule_failures(item)) or "—" + rows.append( + "" + f"{html.escape(str(item.get('episode', '')))}" + f"{html.escape(str(item.get('date', '')))}" + f"" + f"{html.escape(str(item['status']))}" + f"{html.escape(failed_rules)}" + f"{html.escape(str(item.get('runtimeError') or '—'))}" + "" + ) + counts = summary["statusCounts"] + return f""" + + + + +UBT MCAP 数据质检报告 + + +
+

UBT MCAP 数据质检报告

+
数据集 {html.escape(str(summary.get("datasetId", "")))} · +日期 {html.escape(", ".join(summary.get("datePrefixes", [])))} · +源码 {html.escape(str(summary.get("sourceCommit", "")))}
+
+
总数
{summary["totalEpisodes"]}
+
PASS
{counts["PASS"]}
+
ERROR
{counts["ERROR"]}
+
通过率
{summary["passRate"]:.2%}
+
+

数据 fingerprint:{html.escape(str(summary.get("datasetFingerprint", "")))}

+

URDF fingerprint:{html.escape(str(summary.get("urdfFingerprint", "")))}

+ +{''.join(rows)}
Episode日期状态失败规则运行异常
+
+""" + + +def _copy_raw_artifacts( + artifacts: list[dict[str, Any]], destination: pathlib.Path +) -> list[pathlib.Path]: + copied: list[pathlib.Path] = [] + for item in artifacts: + relative = str(item.get("relativePath", "")) + if not relative.startswith("raw-checker/"): + continue + source = pathlib.Path(str(item.get("path", ""))) + if not source.is_file(): + continue + target = destination / relative + target.parent.mkdir(parents=True, exist_ok=True) + shutil.copy2(source, target) + copied.append(target) + return copied + + +def run_report() -> None: + input_artifacts = load_input_artifacts() + checker_summary = find_json_payload(input_artifacts, "checker-summary.json") + results = _episode_results(input_artifacts) + if not results: + raise RuntimeError("cannot report an empty UBT result set") + + status_counts = { + status: sum(1 for item in results if item["status"] == status) + for status in ("PASS", "ERROR", "RUNTIME_ERROR") + } + rule_counts = Counter( + rule_id for item in results for rule_id in _rule_failures(item) + ) + summary = { + "schemaVersion": "1.0", + "datasetId": checker_summary.get("datasetId"), + "datasetFingerprint": checker_summary.get("datasetFingerprint"), + "urdfFingerprint": checker_summary.get("urdfFingerprint"), + "sourceCommit": checker_summary.get("sourceCommit"), + "imageDigest": checker_summary.get("imageDigest"), + "datePrefixes": checker_summary.get("datePrefixes", []), + "totalEpisodes": len(results), + "statusCounts": status_counts, + "passRate": status_counts["PASS"] / len(results), + "dateStatistics": _date_statistics(results), + "ruleStatistics": dict(sorted(rule_counts.items())), + "checkerSummary": checker_summary, + } + pass_manifest = { + "datasetId": summary["datasetId"], + "datasetFingerprint": summary["datasetFingerprint"], + "episodeCount": status_counts["PASS"], + "episodes": [ + _manifest_entry(item) for item in results if item["status"] == "PASS" + ], + } + fail_manifest = { + "datasetId": summary["datasetId"], + "datasetFingerprint": summary["datasetFingerprint"], + "episodeCount": status_counts["ERROR"] + status_counts["RUNTIME_ERROR"], + "episodes": [ + _manifest_entry(item) for item in results if item["status"] != "PASS" + ], + } + + output_dir = pathlib.Path(env_text("OUTPUT_DIR", "/tmp/droboticflow-ubt-report")) + output_dir.mkdir(parents=True, exist_ok=True) + paths = { + "summary": output_dir / "ubt-quality-summary.json", + "csv": output_dir / "episode-results.csv", + "pass": output_dir / "pass-manifest.json", + "fail": output_dir / "fail-manifest.json", + "rules": output_dir / "rule-statistics.json", + "html": output_dir / "ubt-quality-report.html", + } + write_json(paths["summary"], summary) + _write_csv(paths["csv"], results) + write_json(paths["pass"], pass_manifest) + write_json(paths["fail"], fail_manifest) + write_json(paths["rules"], summary["ruleStatistics"]) + paths["html"].write_text(_render_html(summary, results), encoding="utf-8") + raw_paths = _copy_raw_artifacts(input_artifacts, output_dir) + + dataset_id = str(summary["datasetId"] or "ubt") + date_range = "_".join(summary["datePrefixes"]) or "all-dates" + run_id = env_text("RUN_ID", "run") + prefix = f"results/{dataset_id}/{date_range}/{run_id}" + output_artifacts = [ + artifact( + paths["summary"], + port_id="quality-bundle", + port_name="质检结果包", + data=summary, + relative_path=f"{prefix}/{paths['summary'].name}", + ), + artifact( + paths["csv"], + port_id="quality-bundle", + port_name="质检结果包", + relative_path=f"{prefix}/{paths['csv'].name}", + ), + artifact( + paths["pass"], + port_id="quality-bundle", + port_name="质检结果包", + data=pass_manifest, + relative_path=f"{prefix}/{paths['pass'].name}", + ), + artifact( + paths["fail"], + port_id="quality-bundle", + port_name="质检结果包", + data=fail_manifest, + relative_path=f"{prefix}/{paths['fail'].name}", + ), + artifact( + paths["rules"], + port_id="quality-bundle", + port_name="质检结果包", + data=summary["ruleStatistics"], + relative_path=f"{prefix}/{paths['rules'].name}", + ), + artifact( + paths["html"], + port_id="quality-bundle", + port_name="质检结果包", + relative_path=f"{prefix}/{paths['html'].name}", + ), + ] + for path in raw_paths: + output_artifacts.append( + artifact( + path, + port_id="quality-bundle", + port_name="质检结果包", + relative_path=f"{prefix}/{path.relative_to(output_dir)}", + ) + ) + emit_result( + output_dir, + OPERATOR_ID, + output_artifacts, + [ + f"quality summary: {status_counts}", + f"pass rate: {summary['passRate']:.6f}", + f"bundle prefix: {prefix}", + ], + ) diff --git a/operator-images/lightweight/ubt-mcap-runtime/Dockerfile b/operator-images/lightweight/ubt-mcap-runtime/Dockerfile new file mode 100644 index 00000000..18b19522 --- /dev/null +++ b/operator-images/lightweight/ubt-mcap-runtime/Dockerfile @@ -0,0 +1,25 @@ +FROM python:3.10-slim-bookworm + +ARG ROBO_ORCHARD_SOURCE_COMMIT=c742b39e0821590605385ee4ddabcafb258432d8 +LABEL org.opencontainers.image.title="Drobodata UBT MCAP runtime" \ + org.opencontainers.image.source="robot-lab-internal/open-source/robo_orchard_lab" \ + org.opencontainers.image.revision="${ROBO_ORCHARD_SOURCE_COMMIT}" \ + org.opencontainers.image.licenses="Apache-2.0" + +ENV PYTHONUNBUFFERED=1 \ + MPLBACKEND=Agg \ + PIP_NO_CACHE_DIR=1 + +RUN apt-get update \ + && apt-get install -y --no-install-recommends ffmpeg libgl1 libglib2.0-0 \ + && rm -rf /var/lib/apt/lists/* + +COPY lightweight-operators/ubt-mcap-pipeline/requirements.txt /tmp/requirements.txt +RUN python -m pip install --upgrade pip==25.1.1 \ + && python -m pip install \ + --extra-index-url https://download.pytorch.org/whl/cpu \ + -r /tmp/requirements.txt \ + && python -m pip check + +WORKDIR /opt/droboticflow/lightweight/operator +CMD ["python", "/opt/droboticflow/lightweight/operator/main.py"] diff --git a/tests/backend/ubt_mcap_pipeline_test.py b/tests/backend/ubt_mcap_pipeline_test.py new file mode 100644 index 00000000..4cfa83b3 --- /dev/null +++ b/tests/backend/ubt_mcap_pipeline_test.py @@ -0,0 +1,160 @@ +from __future__ import annotations + +import importlib +import json +import pathlib +import sys +import zipfile + +import pytest + +PIPELINE_ROOT = ( + pathlib.Path(__file__).resolve().parents[2] + / "lightweight-operators" + / "ubt-mcap-pipeline" +) +sys.path.insert(0, str(PIPELINE_ROOT)) + +common = importlib.import_module("ubt_pipeline.common") +prepare = importlib.import_module("ubt_pipeline.prepare") +report = importlib.import_module("ubt_pipeline.report") + + +def make_episode(root: pathlib.Path, name: str) -> pathlib.Path: + episode = root / name + episode.mkdir(parents=True) + (episode / f"{name}_0.mcap").write_bytes(b"mcap") + return episode + + +def test_safe_extract_zip_rejects_directory_traversal(tmp_path: pathlib.Path): + archive = tmp_path / "unsafe.zip" + with zipfile.ZipFile(archive, "w") as handle: + handle.writestr("../escape.txt", "no") + + with pytest.raises(RuntimeError, match="escapes extraction root"): + prepare.safe_extract_zip(archive, tmp_path / "output") + assert not (tmp_path / "escape.txt").exists() + + +def test_episode_validation_and_duplicate_detection(tmp_path: pathlib.Path): + missing = tmp_path / "episode_2026_06_16-00_00_00" + missing.mkdir() + with pytest.raises(RuntimeError, match="missing MCAP"): + prepare.validate_episode(missing) + + episode = make_episode(tmp_path, "episode_2026_06_16-00_00_01") + seen: set[str] = set() + assert prepare.claim_episode(episode, seen) == episode.name + with pytest.raises(RuntimeError, match="duplicate episode"): + prepare.claim_episode(episode, seen) + + +def test_date_prefixes_accept_dash_and_underscore_and_reject_other_formats(): + assert common.normalize_dates("2026-06-16,2026_06_17,2026-06-16") == [ + "2026_06_16", + "2026_06_17", + ] + with pytest.raises(ValueError, match="invalid date prefix"): + common.normalize_dates("2026/06/16") + + +def test_fingerprint_is_stable_and_changes_with_etag_or_urdf(): + objects = [ + {"key": "b.zip", "size": 2, "etag": '"two"'}, + {"key": "a.zip", "size": 1, "etag": "one"}, + ] + fingerprint = common.stable_fingerprint(objects, "urdf") + assert fingerprint == common.stable_fingerprint(list(reversed(objects)), "urdf") + assert fingerprint != common.stable_fingerprint( + [{**objects[0], "etag": "changed"}, objects[1]], "urdf" + ) + assert fingerprint != common.stable_fingerprint(objects, "different-urdf") + + +def test_complete_cache_requires_every_episode(tmp_path: pathlib.Path): + cache = tmp_path / "cache" + episode = make_episode(cache, "episode_2026_06_16-00_00_00") + manifest = { + "datasetFingerprint": "fp", + "episodeCount": 1, + "episodes": [{"preparedPath": str(episode)}], + } + common.write_json(cache / "prepared-manifest.json", manifest) + common.write_json( + cache / ".complete.json", + {"datasetFingerprint": "fp", "episodeCount": 1}, + ) + assert prepare.verify_cache(cache, "fp") == manifest + next(episode.glob("*.mcap")).unlink() + assert prepare.verify_cache(cache, "fp") is None + + +def test_report_manifest_preserves_bos_and_prepared_references(): + item = { + "episode": "episode_2026_06_17-14_36_27", + "date": "2026_06_17", + "status": "ERROR", + "source": { + "path": "bos://testxyz/projects/ubt/raw/test_task.zip", + "key": "projects/ubt/raw/test_task.zip", + }, + "preparedPath": "/data/ubt-prepared/dataset/fp/ubt/test_task/episode_x", + "mcapPath": "/data/ubt-prepared/dataset/fp/ubt/test_task/episode_x/x.mcap", + "ruleResults": [{"rule_id": "gripper_position_out_of_range", "status": "fail"}], + } + manifest = report._manifest_entry(item) + assert manifest["source"]["path"].startswith("bos://testxyz/") + assert manifest["preparedPath"].startswith("/data/ubt-prepared/") + assert manifest["failedRules"] == ["gripper_position_out_of_range"] + + +def test_html_report_escapes_untrusted_episode_and_error_text(): + summary = { + "datasetId": "", + "datePrefixes": ["2026_06_16"], + "sourceCommit": "commit", + "totalEpisodes": 1, + "statusCounts": {"PASS": 0, "ERROR": 0, "RUNTIME_ERROR": 1}, + "passRate": 0.0, + "datasetFingerprint": "data", + "urdfFingerprint": "urdf", + } + result = { + "episode": "", + "date": "2026_06_16", + "status": "RUNTIME_ERROR", + "runtimeError": "", + "ruleResults": [], + } + rendered = report._render_html(summary, [result]) + assert "