From d3edc4d515ab8305b20e799152c769eca12601d2 Mon Sep 17 00:00:00 2001 From: pascd Date: Tue, 3 Nov 2026 14:42:08 +0000 Subject: [PATCH 1/2] Add comprehensive tests for post-processing, pre-check, recorder loop, review, run, and spin functionalities - Introduced tests for the post-process page to validate table filling, conversion enabling, and folder collection. - Added pre-check tests to ensure correct behavior of YAMLDrivenConverter and CLI check outputs. - Implemented recorder loop tests to verify data recording, handling of errors, and image decoding. - Created review page tests to confirm playback settings and robot description persistence. - Developed run page tests to check command line arguments, start/stop functionality, and process management. - Added spin tests to validate the TopicSubscribers' threading and message handling performance. --- CHANGELOG.md | 111 ++- LICENSE | 5 - README.md | 60 +- THIRD_PARTY_LICENSES.md | 4 +- docs/reporting_template.md | 2 +- pyproject.toml | 4 +- relay_d/acquisition/api/api.py | 161 ++- relay_d/acquisition/convert_cli.py | 84 +- relay_d/acquisition/qt_app/app.py | 17 +- .../qt_app/ui/config/builder/builder_page.py | 110 +- .../qt_app/ui/config/builder/config_check.py | 102 +- .../qt_app/ui/config/builder/draft.py | 51 +- .../qt_app/ui/config/builder/urdf_limits.py | 23 +- .../qt_app/ui/config/config_page.py | 42 +- .../ui/config/templates/config_template.yaml | 306 +----- .../postprocess_config_template.yaml | 86 +- .../qt_app/ui/config/yaml_parser.py | 113 +-- .../qt_app/ui/containers/mainWindow.ui | 286 +----- .../qt_app/ui/containers/mainWindow_ui.py | 140 +-- .../qt_app/ui/containers/topic_containers.py | 14 +- .../qt_app/ui/postprocess/h5_file_parser.py | 55 - .../qt_app/ui/postprocess/lerobot_writer.py | 22 +- .../qt_app/ui/postprocess/offline_config.py | 56 +- .../postprocess/postprocess_config_loader.py | 214 +--- .../postprocess_configs/lerobot.yaml | 54 - .../postprocess_configs/robomimic.yaml | 118 --- .../ui/postprocess/postprocess_options.py | 103 +- .../qt_app/ui/postprocess/postprocess_page.py | 434 +++++--- .../postprocess/robomimic_config_handler.py | 35 +- .../ui/postprocess/yaml_driven_converter.py | 941 ++++++++---------- .../qt_app/ui/record/container_manager.py | 8 +- .../qt_app/ui/record/container_visualizer.py | 3 +- .../qt_app/ui/record/data_recorder.py | 258 ++--- .../qt_app/ui/record/record_page.py | 44 +- .../qt_app/ui/record/recording_controller.py | 102 +- .../qt_app/ui/record/topic_subscribers.py | 80 +- .../ui/review/demo_playback_reconstructor.py | 8 +- .../qt_app/ui/review/demo_validator.py | 17 +- .../qt_app/ui/review/lerobot_reader.py | 316 ------ .../qt_app/ui/review/review_page.py | 195 +--- relay_d/acquisition/qt_app/ui/run/__init__.py | 0 relay_d/acquisition/qt_app/ui/run/run_page.py | 231 +++++ .../qt_app/ui/styles/relayd_dark.qss | 47 +- .../qt_app/ui/styles/relayd_light.qss | 47 +- .../acquisition/qt_app/ui/train/train_page.py | 5 +- .../qt_app/utils/stream_builder.py | 237 ++--- relay_d/dispatch/action_dispatcher.py | 346 +++---- relay_d/dispatch/api.py | 104 +- .../action_config_template.yaml | 271 ++--- .../config_templates/obs_config_template.yaml | 108 +- relay_d/dispatch/inference_node.py | 19 +- relay_d/dispatch/lerobot_runner.py | 4 + relay_d/dispatch/obs_builder.py | 208 +--- relay_d/dispatch/robomimic_runner.py | 19 +- .../templates/inference_script_template.py | 7 +- relay_d/training/__init__.py | 22 +- relay_d/training/launcher.py | 20 +- relay_d/training/train_cli.py | 2 +- relay_d/utils/numerics.py | 29 +- relay_d/utils/ros_media_codec.py | 17 +- relay_d/utils/user_settings.py | 19 +- scripts/convert_dataset.py | 13 - scripts/extract_joint_limits.py | 46 +- tests/test_action_dispatcher.py | 267 +++-- tests/test_app_api.py | 141 ++- tests/test_builder_page.py | 7 - tests/test_config_draft.py | 56 +- tests/test_conversion_fixes.py | 225 +++++ tests/test_conversion_paths.py | 198 ++++ tests/test_convert_cli.py | 28 +- tests/test_dispatch_templates.py | 40 +- tests/test_format_buttons.py | 12 +- tests/test_inference_session.py | 190 +++- tests/test_lerobot_conversion.py | 20 +- tests/test_lerobot_reader.py | 179 ---- tests/test_lerobot_runner.py | 38 +- tests/test_numerics.py | 74 +- tests/test_offline_config.py | 8 + tests/test_postprocess_options.py | 92 +- tests/test_postprocess_page.py | 146 +++ tests/test_precheck.py | 139 +++ tests/test_recorder_loop.py | 220 ++++ tests/test_recording_controller.py | 53 +- tests/test_review_lerobot.py | 135 --- tests/test_review_page.py | 55 + tests/test_run_page.py | 91 ++ tests/test_settings_consumers.py | 24 +- tests/test_settings_live.py | 21 +- tests/test_settings_page.py | 18 +- tests/test_spin.py | 89 ++ tests/test_train_page.py | 4 +- tests/test_training_launcher.py | 20 +- tests/test_urdf_limits.py | 10 - tests/test_user_settings.py | 22 +- 94 files changed, 4249 insertions(+), 4978 deletions(-) delete mode 100644 relay_d/acquisition/qt_app/ui/postprocess/h5_file_parser.py delete mode 100644 relay_d/acquisition/qt_app/ui/postprocess/postprocess_configs/lerobot.yaml delete mode 100644 relay_d/acquisition/qt_app/ui/postprocess/postprocess_configs/robomimic.yaml delete mode 100644 relay_d/acquisition/qt_app/ui/review/lerobot_reader.py create mode 100644 relay_d/acquisition/qt_app/ui/run/__init__.py create mode 100644 relay_d/acquisition/qt_app/ui/run/run_page.py delete mode 100755 scripts/convert_dataset.py create mode 100644 tests/test_conversion_fixes.py create mode 100644 tests/test_conversion_paths.py delete mode 100644 tests/test_lerobot_reader.py create mode 100644 tests/test_postprocess_page.py create mode 100644 tests/test_precheck.py create mode 100644 tests/test_recorder_loop.py delete mode 100644 tests/test_review_lerobot.py create mode 100644 tests/test_review_page.py create mode 100644 tests/test_run_page.py create mode 100644 tests/test_spin.py diff --git a/CHANGELOG.md b/CHANGELOG.md index c1286e5..93eb73a 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -13,6 +13,21 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0 - **`relay_d.dispatch.DispatcherHelpers` is gone:** use `save_dispatch_templates(output_dir)`. - **`lfd-inference-node --hz`** defaults to the rate the LeRobot policy was trained at (its dataset fps), no longer the `infer_hz` setting (10 Hz). robomimic checkpoints still use the setting: pass `--hz `. - **Action configs copied from the template:** the `robotiq_gripper` entry must map `position: 0`, not `7` (see *Fixed*). +- **Relay-D no longer normalizes** (robomimic and LeRobot already normalize and un-normalize with the dataset's stats). Remove + `normalize` / `normalized_range` / `limits` from acquisition streams and `obs_config.yaml`, and `denormalize` / `normalized_range` / + `apply_limits` from `action_config.yaml`; configs that still have them are refused. `limits` on an action entry now only **clamps** + the real value: an old action config with `limits` and no `denormalize` key (it defaulted to true) used to rescale [-1, 1] and now + clamps, so it is only right for a policy retrained on this release. +- **Removed config keys now stop with "'' was removed: ...; "** instead of being ignored. Acquisition config: + `output_map` and `fill_default` (also refused at Start: they changed the recorded data), `transfer_rate`, `output.prefix_path`, + `config.data`, `metadata.*`, and the old post-process keys (`training_model`, `*_frac`, `lerobot`). `postprocess_config.yaml`: + `robomimic.metadata`, `robomimic.training_model`, `splits.test_frac`. `action_config.yaml`: `command:` entries, `dedupe`, and the + `scale` / `clip` / `vacuum` / `magnetic` transforms. `obs_config.yaml`: `static` inputs. `recording_frequency` is required in the + acquisition YAML, and the `rec_default_frequency` / `rec_default_sync_*` settings are gone (the YAML is the only source). +- **Acquisition template:** the streams that read inputs which are only commented examples (`tf_object`, `ee_pose`, ...) are + commented out too; before, a "Generate Defaults" config failed its own check and inference refused its obs_config. +- **Action template:** only `move_arm_joint` and `robotiq_gripper` are live (`move_eef` became a commented example); before, a copied + template commanded the arm twice per tick, as a pose and as joint deltas. - **New folder names:** recording sessions end in `_Recording`, Post-process outputs in `_Robomimic` / `_Lerobot`. Existing folders keep their names. ### Added @@ -23,8 +38,8 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0 the state size and cameras are taken from the training dataset, because the checkpoint config keeps the pretrained model's ones. - **Safe stop:** a command or topic entry of `action_config.yaml` can set `stop: [...]` (e.g. `[0, 0, 0]` for a velocity), sent once when inference stops sending: inputs go stale, the model fails, `on_timeout: raise`, Ctrl+C, the end of `run()`, or `close()`. Before, the node just went quiet and a controller without a watchdog kept the last command. A broken `stop` (wrong length, a mapping that does not fit, set on an `action: true` entry) stops startup. -- **Recorded-rate check:** a demo recorded more than 10% off `recording_frequency` (a PC that could not keep up) fails the after-recording check and is left out of LeRobot datasets, whose fps time base would make the policy move - faster than shown. +- **Recorded-rate check:** a demo recorded more than 10% off `recording_frequency` (a PC that could not keep up) fails the after-recording check and is left out of robomimic and LeRobot datasets: the policy runs at that frequency, so + it would move faster than shown. - **CI runs the ROS-dependent tests** in a `ros:jazzy` container job (dispatcher, recorder, `AppAPI`, inference session). `pytest` also runs locally with ROS sourced (`pyproject.toml` disables the ROS pytest plugins). - Verified end to end against LeRobot 0.6.1: `LeRobotDataset` loads a Relay-D dataset (frames, RGB video, `action[t] = state[t+1]`, ACT-style action chunks via `delta_timestamps`), `lerobot_train` trains on it with the Train tab's command (ACT and SmolVLA), and `LeRobotPolicyRunner` runs the checkpoint, with state and action normalized by the dataset's own stats. @@ -61,6 +76,98 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0 - `relay_d.dispatch.DispatcherHelpers` (replaced by `save_dispatch_templates`). - The converter's field-scan fallback (`_scan_for_field`) and the resampling of datasets of the wrong length (`_resize_data`): both hid broken data (see *Fixed*). +### Changed (Post-process) +- **Pre-check before converting** (`YAMLDrivenConverter.precheck`, `relay-d-convert --check`, the Post-process page): per demo the + samples, recorded rate, the config it was recorded with (from the recording's metadata) and whether it converts or why not, plus the + layout that will be written, computed by the converter's own rules, so the page and the conversion cannot disagree. Conversion and + pre-check use one set of per-demo checks. +- **Wrong config named plainly:** a demo whose metadata shows another `recording_frequency` says "recorded at 20 Hz with + config_peter.yaml; the loaded config says 30 Hz" instead of "this PC could not keep up"; a configured stream that is in none of the + selected recordings stops the conversion and lists the streams they do have (before, it was zero-filled with a warning). +- **Post-process page:** the raw HDF5 tree is replaced by the pre-check table and an output preview built from the real config and + recordings (the earlier preview came from hand-written description files that had drifted from what conversion writes); recordings + can be added by file or by folder and dropped again with Remove selected / Delete (the files stay on disk); Convert says how many demos it will convert and why it is disabled. + +### Fixed (GUI = API) +The GUI, `AppAPI`, `relay-d-convert` and `lfd-inference-node` / `InferenceSession` now run the same code at every step. +- **Recording through the GUI was about 100 ms stale:** its Qt timer ran one ROS callback per node every 10 ms, so with 4 topics at + 100 Hz it handled ~78 of 400 messages a second. `TopicSubscribers` now drains its callbacks in its own thread, in the GUI and in + `AppAPI` alike (measured: 400 msg/s, 5 ms median age); inference was already fresh, so training and run time now see the same data. +- **Recording start/stop is one function** (`start_demo`): `AppAPI` subscribes every input at each start and clears them at stop as the + GUI does, refuses an unavailable topic / TF or an unparsable stream spec at `initialize` (as the Config page), names demos + `{dataset_name}_{ms}`, and logs the after-recording check. +- **One writer for the files next to a dataset** (`write_dataset_files`): the CLI and `AppAPI` now also write + `acquisition_config.yaml`, `postprocess_config.yaml` and `action_dispatcher/` (before, only the Post-process page did, so their + datasets could not be run from the Run tab and lost `modality:` for training). The CLI writes them with plain names, and for LeRobot + beside the dataset folder, where the Train launcher looks. A broken options file stops the conversion everywhere; the GUI resets + the options at each config load; `AppAPI` writes a new `_` folder instead of overwriting `combined_dataset_*`, and + `shutdown()` uses the default-format setting. Converted robomimic files given as inputs are skipped; inputs sort by full path. A + three-way check converting the same recordings gave identical files and dataset contents. +- **Inference:** SIGTERM now sends the safe stop (it killed the node without it); the stop also cancels action goals still running; + `InferenceSession` uses the `infer_device` setting and the policy's rate by default (it ran unthrottled on the CPU); a model error + exits 1 (the Run tab showed "Stopped"); the Run tab's node runs in its own process group, so a terminal Ctrl+C reaches it once. + +### Added (checkpoint to robot) +- **Run tab:** start a policy from the GUI (checkpoint + the dataset's `action_dispatcher/` folder, device, Hz, Dry run on by default), + follow its log, and stop it with a red **STOP** that sends Ctrl+C, so the node sends each entry's `stop:`; if the node does not + exit within 5 s it is killed and the log says the stop may not have been sent. Closing Relay-D stops a running policy first. +- **`lfd-inference-node --dry-run`** (and `InferenceSession(dry_run=True)`): the whole loop runs on live sensors, but no publisher or + action client is created and every command and stop is logged instead of sent; config errors still stop startup. +- **Action layout check:** an `action_config.yaml` index past the model's action size stops startup; action columns no entry sends + are listed in a warning. + +### Fixed (checkpoint to robot) +- **robomimic policies run at their recording rate:** conversion stores `recording_frequency` in the dataset's `env_args` + (`env_kwargs.control_freq`), robomimic keeps it in the checkpoint, and `--hz` defaults to it as for LeRobot. Before, robomimic ran at the + `infer_hz` setting (10 Hz) whatever the demos were recorded at. Checkpoints trained on older conversions still need `--hz`. +- The recorded-rate check (more than 10% off `recording_frequency`) now also leaves such demos out of robomimic datasets. + +### Removed (pipeline cleanup) +One path from collection to inference; nothing kept that the trainers ignore or that could make the data mean two things. +- **Recording:** Pause / Resume (a resume made the action at that row a jump; stop and start a new demo instead), `output_map` (a + second name for an input's group; it also broke inference for a camera, whose obs `source` became the group name), `fill_default` + (made-up values before the first message; leading samples are back-filled with the first real one), the dead `transfer_rate`, + `prefix_path`, `config.data` keys, the old nested `output:` parsing, and `AppAPI.pause_recording` / `resume_recording` / + `get_recording_status`. Stale data is now flushed at every Start (the GUI did not, the API did). +- **Conversion:** the path for recordings without `streams/` (its datasets could not run at inference), `next_obs` and `states` in the + robomimic file (robomimic does not load them with the shipped configs; about half the size with cameras), robosuite env metadata + (`env_args` is a fixed stub, rollout is off), the test split (`mask/eval`, read by nothing), `robomimic_config.json` written at + Post-process (the Train launcher builds it) and the output-structure preview, `scripts/convert_dataset.py` (use `relay-d-convert`). + Every stream is now length-checked against the demo instead of being cut. +- **Training:** the robomimic policy is chosen only on the Train tab / `--policy` (no "From the folder's config"); an older folder's saved + `robomimic_config.json` is still used when no policy is given. +- **Review:** the LeRobot dataset viewer (LeRobot has its own); Review shows recorded demos. +- **Inference:** `command:` entries that shelled out (`ros2 topic pub`, about 1.5 s per call), `dedupe`, the `scale` / `clip` / + `vacuum` / `magnetic` transforms (`threshold` gains `on_value` / `off_value`; `limits` clamps), `static` obs inputs (the recorder + never produced them, so no model was trained on them). +- Settings `review_plot_max_points` and `postprocess_default_training_model`; unused entries in `LICENSE` / `THIRD_PARTY_LICENSES.md` + (xmltodict, scipy, jurigged, watchdog, cv_bridge; torch is listed as optional); the re-exports of `relay_d.training`. + +### Removed (normalization) +- Relay-D's own min/max normalization: the converter, `ObsBuilder`, the dispatcher's `denormalize`, the Config builder's + "Normalize" checkbox with its limits table and URDF fill, and `numerics.normalize`. Every supported trainer (robomimic with the + shipped configs, LeRobot ACT / Diffusion / SmolVLA) already normalizes with the dataset's stats and un-normalizes at inference; the + second layer only added a way to get it wrong (`action_config.yaml` limits were never checked against the conversion limits, so a + normalized arm action could reach the robot as [-1, 1]). `limits` in `action_config.yaml` remains, as a clamp in real units, and + `scripts/extract_joint_limits.py` prints such blocks from a URDF. + +### Fixed (audit 3) +- **Robot safety:** a non-finite action, or an error in `InferenceSession.step()`, now sends the configured `stop` (it went quiet and the last + command kept running). Command values are written as plain decimals (`0.00005`, not `5e-05`, which `ros2 topic pub` read as text). A command + entry with the wrong number of `limits` now stops startup, like topic entries. The exported `inference_script.py` uses the configs next to it, + not the ones in the current folder. +- **Recording:** loading, saving or removing a config while recording is refused (it corrupted the demo); a demo whose file fails to close is + kept for the retry; `recording_frequency <= 0` is refused at Start, and a recording loop that dies is reported like a full buffer; `fill_default` + seeds no longer split a stream in two; cameras whose encoding has no `bgr8` conversion (`16UC1`, `32FC1`, `mono16`, ...) are recorded as decoded + instead of failing every frame; `AppAPI` honours an input's `tf_topic` / `tf_static_topic`. Stream specs (`sin(a - b)`, `zero:N`, `euler.*`, + `rot6d.*`) are parsed the same way when recording and at inference. +- **Conversion:** a demo missing an action stream or a camera is skipped (it got all-zero actions or black frames); a config without + `actions_to_extract` fails (and the config check reports it) instead of writing zero actions; `modality: depth` is found through the output + key; a LeRobot episode that fails halfway aborts the dataset instead of leaving mis-indexed rows; the success message says how many demos were + skipped; robomimic image `next_obs` is written straight to the file (half the peak RAM). +- Removed: the unused `scripts` (`xmltodict`) extra, `_stop_procs`, `fill_none_gaps`, `gripper_key_map`, and duplicate helpers (now + `numerics.stamp_to_sec` / `numerics.enriched_tf`, `review.demo_validator.first_demo`, `robomimic_config_handler.explicit_modalities`). + ### Fixed (audit) - **Action dispatcher:** a `mapping` that does not fit its message (a wrong field name or index) now stops startup; before, the first tick sent the other entries (the arm) and skipped that one (the gripper). An output that is not finite after its `transform` (a `scale` with `in_min == in_max`) skips the whole tick instead of being published. diff --git a/LICENSE b/LICENSE index 51d6d30..6eb7025 100644 --- a/LICENSE +++ b/LICENSE @@ -19,11 +19,7 @@ This program makes use and is distributed with the following libraries: * PyQt5: (https://www.riverbankcomputing.com/software/pyqt/) * PyQt5-sip: (https://www.riverbankcomputing.com/software/sip/) * pyqtgraph: (https://www.pyqtgraph.org/) -* xmltodict: (https://github.com/martinblech/xmltodict) * robomimic: (https://robomimic.github.io/) -* scipy: (https://scipy.org/) -* jurigged: (https://github.com/breuleux/jurigged) -* watchdog: (https://github.com/gorakhargosh/watchdog) * rclpy: (https://github.com/ros2/rclpy) * tf2_ros: (https://github.com/ros2/geometry2) * tf2_msgs: (https://github.com/ros2/geometry2) @@ -34,7 +30,6 @@ This program makes use and is distributed with the following libraries: * builtin_interfaces: (https://github.com/ros2/rcl_interfaces) * sensor_msgs_py: (https://github.com/ros2/common_interfaces) * rosidl_runtime_py: (https://github.com/ros2/rosidl_runtime_py) -* cv_bridge: (https://github.com/ros-perception/vision_opencv) * xacro: (https://github.com/ros/xacro) * Qt5: (https://www.qt.io/) * libhdf5: (https://www.hdfgroup.org/solutions/hdf5/) diff --git a/README.md b/README.md index 5ce0d27..881ed94 100644 --- a/README.md +++ b/README.md @@ -24,14 +24,14 @@ This project is currently **in progress**: it is actively being developed and co ## 2. Technology stack - **Language:** Python ≥ 3.12 -- **Frameworks and libraries:** ROS 2 Jazzy (`rclpy`, `tf2_ros`), PyQt5 (`RelayDApp` GUI), PyTorch, [Robomimic](https://robomimic.github.io/) (optional, imitation-learning backend), OpenCV, h5py +- **Frameworks and libraries:** ROS 2 Jazzy (`rclpy`, `tf2_ros`), PyQt5 (`RelayDApp` GUI), PyTorch (installed with robomimic / LeRobot), [Robomimic](https://robomimic.github.io/) (optional, imitation-learning backend), OpenCV, h5py - **Other tools:** pip/setuptools packaging, GitHub Actions (CI) Relay-D is a plain pip package (not a colcon/ament package) intended to run standalone inside a Python environment where ROS 2 Jazzy has already been sourced — it is not a ROS 2 node package in its own right, but it depends on a sourced ROS 2 environment at runtime. It is composed of two subsystems that interoperate directly: -- **`relay_d.acquisition`** — the `RelayDApp` GUI: records robot sensor data (topics + TF) to HDF5 per a YAML config, and converts recordings to Robomimic format. Also exposes a headless `AppAPI` for programmatic recording. +- **`relay_d.acquisition`** — the `RelayDApp` GUI: records robot sensor data (topics + TF) to HDF5 per a YAML config, and converts recordings to Robomimic format. Also exposes a headless `AppAPI` for programmatic recording that runs the same code as the GUI (same config checks, same start/stop, same conversion and files). - **`relay_d.dispatch`** — the `lfd-inference-node` CLI: runs a trained LfD policy live on the robot. Also exposes a programmatic `InferenceSession` API. - **`relay_d.utils`** — small utilities shared by both subsystems (currently the colored console logger). @@ -62,8 +62,6 @@ See [Architecture diagrams](#6-architecture-diagrams) for how these pieces fit t in the Python that runs the node: `robomimic` (above) for `.pth` checkpoints, or `lerobot` (`pip install 'lerobot[training]'`, plus `'lerobot[diffusion]'` for Diffusion and `'lerobot[smolvla]'` for SmolVLA) for LeRobot checkpoints. See "Running inference on the robot". -**Optional extra** — `pip install ".[scripts]"`: `xmltodict`, used by `scripts/extract_joint_limits.py` to parse URDF XML. - **Optional extra** — `pip install ".[lerobot]"` (only needed to convert recordings to the [LeRobotDataset](https://huggingface.co/docs/lerobot/main/en/lerobot-dataset-v3) format): | Package | Used for | @@ -87,7 +85,7 @@ These must come from a sourced ROS 2 Jazzy environment (see [Installation](#4-in | `tf2_ros`, `tf2_msgs` | transform lookups | | `geometry_msgs`, `std_msgs`, `sensor_msgs`, `sensor_msgs_py`, `trajectory_msgs`, `builtin_interfaces` | ROS 2 message types | | `rosidl_runtime_py` | introspecting ROS 2 message/service definitions | -| `xacro` | resolving robot description macros (used by `scripts/extract_joint_limits.py`) | +| `xacro` | resolving robot description macros (used by `scripts/extract_joint_limits.py`, which prints URDF joint limits for the `limits:` clamp of an `action_config.yaml` entry, in real units) | ### Other system libraries @@ -142,7 +140,7 @@ This is a plain pip package (not a colcon/ament package) — no `colcon build` s RelayDApp ``` -Three pages: **Config** (load/validate your YAML), **Record** (start/pause/stop, live sensor feeds), **Post-process** (convert raw recordings to Robomimic or LeRobot format; a config can be loaded here without the robot running). +Three pages: **Config** (load/validate your YAML), **Record** (start/stop, live sensor feeds), **Post-process** (convert raw recordings to Robomimic or LeRobot format; a config can be loaded here without the robot running). Everything lands in the recordings folder (Settings), named by what it holds: @@ -159,11 +157,11 @@ On the **Config** page, the **Build from ROS** tab writes `acquisition_config.ya 1. **Sources**: topics of the live graph (filter, double-click to add; or type a topic the graph does not show yet) and TF frames (a parent and a child, any connected pair). 2. **Inputs**: output group, modality (`depth`, `scan`), `joint_names` (**Read joint names from the topic** takes them from one live message), gap filling. The first word of an input name (`image_`, `joint_`, `pose_`, `tf_`, ...) picks its live view on the Record page, and is suggested for you. -3. **Observations**: tick message fields to fill the specs of a stream (`joint_1.position[:]`, `tf_eef.translation.x`; edit the text for arithmetic, `sin()/cos()` or a single element), normalize with limits typed in or filled from a URDF (file, or the latched `/robot_description`), roll/pitch/yaw for quaternions. Images and point clouds are added as camera observations. +3. **Observations**: tick message fields to fill the specs of a stream (`joint_1.position[:]`, `tf_eef.translation.x`; edit the text for arithmetic, `sin()/cos()` or a single element), roll/pitch/yaw for quaternions. Values stay in real units: robomimic and LeRobot normalize with the dataset's stats. Images and point clouds are added as camera observations. 4. **Actions**: tick the streams that form the action, in order, with optional delta. 5. **Recording**: name, frequency, buffer, sync. 6. **Review**: the YAML, the problems found (errors block saving) and **Save and use now**, which loads it like any hand-written file. **Edit loaded config** brings an existing file back into the builder. -The headless `AppAPI`, `relay-d-convert` and the other tools still take a ready-made config file. Robomimic environment metadata, the LeRobot task and the training model stay in `postprocess_config.yaml`. +The headless `AppAPI`, `relay-d-convert` and the other tools still take a ready-made config file. The train / validation split and the LeRobot options (task, robot type, video settings) stay in `postprocess_config.yaml`; the robomimic policy is chosen on the Train tab. ### Converting recordings without the GUI @@ -178,9 +176,16 @@ relay-d-convert --config acquisition_config.yaml --format robomimic \ ``` No ROS or display is needed. `--input` takes `.h5` files and/or directories (searched recursively). -Format options (robomimic env metadata and training model, LeRobot task, ...) are read from +Format options (train / validation split, LeRobot task, ...) are read from `--options` or a `postprocess_config.yaml` next to the config; see -`relay_d/acquisition/qt_app/ui/config/templates/postprocess_config_template.yaml`. `python scripts/convert_dataset.py` is the same tool. +`relay_d/acquisition/qt_app/ui/config/templates/postprocess_config_template.yaml`. A broken options file stops the conversion (CLI, `AppAPI` +and the Post-process page alike), and all three write the same dataset and the same files next to it. +`relay-d-convert --check` (no `--output` needed) and the Post-process page run the same pre-check before anything is written: for each +demo its sample count, recorded rate, the config it was recorded with, and whether it converts or why not; and the layout that will be +written (robomimic keys and shapes, action columns, masks; LeRobot features, fps, task). A configured stream that is in none of the +selected recordings stops the conversion and names the streams the recordings do have and the config they were recorded with; a demo +recorded with another config or `recording_frequency` is skipped with that config named. The page adds recordings by file or by folder +(recursive, like `--input`), drops selected ones with **Remove selected** or the Delete key (nothing is deleted from disk), and its Convert button says how many demos will be converted. ### Running inference on the robot @@ -196,18 +201,29 @@ lfd-inference-node --checkpoint path/to/run/output/checkpoints/last/pretrained_m --obs-config path/to/obs_config.yaml --action-config path/to/action_config.yaml --device cpu ``` -Data Post Process writes `action_dispatcher/` next to every dataset (robomimic or LeRobot): `obs_config.yaml` made from your acquisition +Data Post Process, `relay-d-convert` and `AppAPI` write `acquisition_config.yaml`, `postprocess_config.yaml` (when one was used) and `action_dispatcher/` next to every dataset (next to the `.h5`, or beside the LeRobot dataset folder): `obs_config.yaml` made from your acquisition config, an `action_config.yaml` template to adapt, and an `inference_script.py`. For LeRobot the node feeds the policy what the dataset had: the vector observations side by side as `observation.state` (in the order of the dataset's `meta/info.json`, found through the checkpoint's `train_config.json`; `--state-keys a,b` overrides it) and each camera as `observation.images.` (BGR to RGB, scaled to [0, 1]). LeRobot normalizes with the statistics saved in the checkpoint. The node runs at the rate -the policy was trained at (the dataset's fps, i.e. the `recording_frequency`) unless `--hz` says otherwise, and warns when they differ: each +the policy was trained at (the `recording_frequency`, stored in the LeRobot dataset's fps or in the robomimic checkpoint) unless `--hz` says otherwise, and warns when they differ: each predicted action is one step of the demos, so another rate makes the robot move faster or slower than you showed it. For the same reason a -demo whose recorded rate is more than 10% off `recording_frequency` fails the after-recording check and is left out of LeRobot datasets. -robomimic checkpoints do not store a rate: pass `--hz` equal to the recording frequency. The Python that +demo whose recorded rate is more than 10% off `recording_frequency` fails the after-recording check and is left out of robomimic and LeRobot datasets. +robomimic checkpoints trained on data converted before this release have no rate: pass `--hz` equal to the recording frequency. The Python that runs the node needs `lerobot`; LeRobot 0.6 needs torch < 2.12, so it usually lives in its own virtual environment with ROS 2 sourced. +At startup the node checks `action_config.yaml` against the model: an index past the model's action stops it, and action columns no entry +sends are listed in a warning. `--dry-run` runs the whole loop on the live robot's sensors but logs every command and stop instead of +sending it (config errors still stop startup): use it for the first run of a new checkpoint or action config. + +The **Run** tab does the same from the GUI: choose the checkpoint and the dataset's `action_dispatcher/` folder (or the Post-process output +folder holding it), keep **Dry run** checked for a first look, and press **Start**; the log shows the node's output. **STOP** sends Ctrl+C, +so the node sends each entry's `stop:`, cancels any action goal still running, and closes; closing Relay-D stops a running policy the same way. +The node runs in its own process group, so Ctrl+C in the terminal that started Relay-D stops it once, through Relay-D. A model error ends +the run with exit code 1 (shown as Failed). The node also sends the stop on SIGTERM. `InferenceSession` used from Python has the same +defaults as the node (`device`: the `infer_device` setting; `run(hz=None)`: the policy's rate, else `infer_hz`). + See `relay_d/dispatch/inference_node.py` and `relay_d/dispatch/config_templates/*.yaml` for the full set of options and config syntax. ### Training a policy (Train tab or `relay-d-train`) @@ -236,7 +252,8 @@ to test it. LeRobot pins its own torch range (0.6.x needs torch below 2.12), so environment: `pip install 'lerobot[training]'` (plus `'lerobot[diffusion]'` for Diffusion, `'lerobot[smolvla]'` for SmolVLA; the **Set up lerobot environment** button installs all three, press it again to add SmolVLA to an older environment). All LeRobot policies need at least one camera in the dataset. Policies: robomimic `bc_rnn_gmm`, `bc_transformer_gmm`, `diffusion_policy`; LeRobot `act`, -`diffusion` (trained from scratch) and `smolvla`. Anything else goes in "Extra arguments". +`diffusion` (trained from scratch) and `smolvla`. The policy is chosen only here (`--policy`; robomimic default `bc_rnn_gmm`, or the +`robomimic_config.json` an older Post-process saved in the folder). Anything else goes in "Extra arguments". **SmolVLA** is a small vision-language-action model (about 450M parameters) pretrained on many community robot datasets: Relay-D fine-tunes it from `lerobot/smolvla_base` instead of training from zero, so it usually needs fewer demos. It follows the dataset's task @@ -250,19 +267,12 @@ Without a display (SSH) the run starts in the background and logs to `train.log` The **Settings** tab (tabs: General, Recording, Review, Post-process, Training, Inference) edits `~/.relayd_settings.json`. The same file is read by the command-line tools, so a value saved there is the default of `relay-d-train`, `relay-d-convert` (video quality, file sizes, splits), `lfd-inference-node` -(`--device`, `--hz` when the policy has no rate of its own, `--max-obs-age`, `--obs-wait`, `--backend`) and the headless `AppAPI`. Precedence is -**acquisition YAML > setting > built-in default**, and a command-line flag beats all of them. Settings marked +(`--device`, `--hz` when the policy has no rate of its own, `--max-obs-age`, `--obs-wait`, `--backend`) and the headless `AppAPI`. What changes the recorded data (`recording_frequency`, sync) comes only from the +acquisition YAML; for the rest a command-line flag beats the setting, which beats the built-in default. Settings marked "(restart)" apply the next time Relay-D starts; the others apply immediately. The list of settings, with their defaults and tooltips, is the table `SETTINGS` in `relay_d/utils/user_settings.py`; `RELAYD_SETTINGS_FILE` points the tools at another file, and `RELAYD_LOG_LEVEL` overrides the log-level setting. -### Reviewing a LeRobot dataset - -The **Review** tab also opens LeRobot v3.0 datasets (needs `pyarrow`, from `pip install ".[lerobot]"`): choose the -Post-process output folder or the dataset folder. You get the episode list with checks (frame counts, ordering, -NaN, missing videos), a camera selector with a frame slider, and the `observation.state` / `action` curves -(joint names from `info.json`, time axis in seconds). It is view-only: no delete and no rviz playback. - ### Package layout ``` @@ -272,7 +282,7 @@ relay_d/ ├── training/ # start/follow robomimic and LeRobot training runs (relay-d-train, Train tab) └── acquisition/ # RelayDApp GUI + AppAPI (data recording + Robomimic/LeRobot conversion) └── qt_app/ui/config/templates/ # default acquisition + post-process config templates -scripts/ # standalone CLIs (convert_dataset.py, extract_joint_limits.py, reprocess_recorded_data.py) +scripts/ # standalone CLIs (extract_joint_limits.py, reprocess_recorded_data.py) ``` --- diff --git a/THIRD_PARTY_LICENSES.md b/THIRD_PARTY_LICENSES.md index 7856f5d..16ccae9 100644 --- a/THIRD_PARTY_LICENSES.md +++ b/THIRD_PARTY_LICENSES.md @@ -12,18 +12,17 @@ or its upstream repository, not guessed from memory. |---|---|---| | `numpy` | BSD-3-Clause (+ small bundled 0BSD/MIT/Zlib/CC0-1.0 components) | | | `pyyaml` | MIT | | -| `torch` | BSD-3-Clause (+ bundled Apache-2.0/BSD-2-Clause/BSL-1.0/MIT components) | | | `h5py` | BSD-3-Clause | | | `opencv-python-headless` | Apache-2.0 | Packaging scripts (the `opencv-python` project itself) are MIT; the bundled OpenCV library is Apache-2.0 (OpenCV ≥4.5) | | `pyqt5` | **GPL-3.0** (or a paid Riverbank commercial license) | See ⚠️ note below | | `pyqtgraph` | MIT | | -| `xmltodict` | MIT | | ## Python — optional extra (`pip install ".[robomimic]"`) | Package | License | Notes | |---|---|---| | `robomimic` | MIT | This project requires the [ARISE-Initiative source checkout](https://github.com/ARISE-Initiative/robomimic) (`>=0.4`), not the PyPI release (which only goes up to 0.3.0) — both are MIT | +| `torch` | BSD-3-Clause (+ bundled Apache-2.0/BSD-2-Clause/BSL-1.0/MIT components) | Not installed by Relay-D: comes with robomimic / LeRobot, imported only to run or train a policy | ## Python — dev only (`[dependency-groups] dev`) @@ -38,7 +37,6 @@ or its upstream repository, not guessed from memory. | `tf2_ros`, `tf2_msgs` | BSD-3-Clause | (`ros2/geometry2`) | | `std_msgs`, `geometry_msgs`, `sensor_msgs`, `trajectory_msgs`, `builtin_interfaces` | Apache-2.0 | per each package's `package.xml` (`ros2/common_interfaces` and related) | | `sensor_msgs_py`, `rosidl_runtime_py` | Apache-2.0 | | -| `cv_bridge` | Apache-2.0 | (`ros-perception/vision_opencv`) | | `xacro` | BSD-3-Clause | (`ros/xacro`) | ## Other system libraries diff --git a/docs/reporting_template.md b/docs/reporting_template.md index 7b95a6f..86b21c0 100644 --- a/docs/reporting_template.md +++ b/docs/reporting_template.md @@ -1,6 +1,6 @@
-![logo](../profile/inesctec_banner.png) +![logo](../img/inesctec_banner.png) ## INESC TEC ### Open-Source Software: *Security Issue Reporting Template* diff --git a/pyproject.toml b/pyproject.toml index f65be0b..77bede2 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -45,8 +45,6 @@ Changelog = "https://github.com/pascd/Relay-D/blob/main/CHANGELOG.md" # dispatcher targets >=0.4, e.g. 0.5.0). Install your robomimic source # checkout in editable mode BEFORE installing this extra — see README.md. robomimic = ["robomimic>=0.4"] -# Only for scripts/extract_joint_limits.py. -scripts = ["xmltodict"] # LeRobotDataset v3.0: convert to it (postprocess/lerobot_writer.py) and open it in the Review tab. # pyarrow 21+ needs NumPy 2 at import, and ROS Jazzy's Python ships NumPy 1.26 (pyarrow does not declare it, so pip would not warn). lerobot = ["pyarrow>=15.0,<21", "av>=12.0"] @@ -76,6 +74,6 @@ include = ["relay_d*"] "relay_d.acquisition.qt_app.ui.config" = ["templates/*.yaml"] "relay_d.acquisition.qt_app" = ["resources/**/*", "resources.qrc"] "relay_d.acquisition.qt_app.ui.containers" = ["*.ui"] -"relay_d.acquisition.qt_app.ui.postprocess.postprocess_configs" = ["*.json", "*.yaml"] +"relay_d.acquisition.qt_app.ui.postprocess.postprocess_configs" = ["*.json"] "relay_d.acquisition.qt_app.ui.review" = ["*.rviz"] "relay_d.acquisition.qt_app.ui.styles" = ["*.qss"] diff --git a/relay_d/acquisition/api/api.py b/relay_d/acquisition/api/api.py index b14d099..aad4124 100644 --- a/relay_d/acquisition/api/api.py +++ b/relay_d/acquisition/api/api.py @@ -1,16 +1,21 @@ import os +from datetime import datetime import rclpy -from threading import Thread, current_thread from typing import Optional from ..qt_app.ui.config.yaml_parser import YamlParser from ..qt_app.ui.record.topic_subscribers import TopicSubscribers -from ..qt_app.ui.record.data_recorder import DataRecorder +from ..qt_app.ui.record.data_recorder import DataRecorder, start_demo +from ..qt_app.ui.config.builder.config_check import check_config +from ..qt_app.utils.stream_builder import StreamBuilder +from relay_d.utils.tf_topic import TfTopicSubscriptionError from ..qt_app.ui.postprocess.yaml_driven_converter import YAMLDrivenConverter from ..qt_app.ui.postprocess.postprocess_config_loader import PostprocessConfigLoader -from ..qt_app.ui.postprocess.postprocess_options import FORMAT_SUFFIX, PostprocessOptions -from ..qt_app.ui.review.demo_validator import DemoValidator +from ..qt_app.ui.postprocess.offline_config import load_conversion_config, write_dataset_files +from ..qt_app.ui.postprocess.postprocess_options import dataset_output_paths +from ..qt_app.ui.review.demo_validator import CheckStatus, DemoValidator +from relay_d.utils import user_settings from relay_d.utils.coloring_logger import logger class ContainerAdapter: @@ -20,7 +25,9 @@ def __init__(self, data: dict): self.input_name = data.get("name") self.parent_frame = data.get("parent_frame") self.child_frame = data.get("child_frame") - self.transfer_rate = data.get("transfer_rate", 10) + self.tf_topic = data.get("tf_topic") # None -> /tf, /tf_static (as in the GUI's ContainerManager) + self.tf_static_topic = data.get("tf_static_topic") + self.tf_wait_timeout = data.get("tf_wait_timeout") class AppAPI: @@ -30,6 +37,8 @@ def __init__( output_dir: Optional[str] = None, postprocess_config_path: Optional[str] = None, ): + # Conversion config + format options, checked like relay-d-convert: raises before any ROS setup + self._load_conversion_config(config_path, postprocess_config_path) self._rclpy_initialized = False try: rclpy.init() @@ -40,28 +49,13 @@ def __init__( self._yaml_parser = YamlParser() self._topic_subscribers = TopicSubscribers() self._data_recorder = DataRecorder(self._yaml_parser) - self._config_loader = PostprocessConfigLoader() - self._yaml_converter = YAMLDrivenConverter(self._config_loader) - if config_path: - self._yaml_converter.load_data_config(config_path) - # Format options for conversion: explicit path, else a postprocess_config.yaml - # next to the acquisition config, else defaults (legacy keys in the config still work). - options_path = postprocess_config_path or PostprocessOptions.find_sibling(config_path) - if options_path: - options, problems = PostprocessOptions.load(options_path) - if options is not None: - self._yaml_converter.options = options - else: - logger.error(f"Ignoring postprocess options {options_path}: {'; '.join(problems)}") - # An explicit output_dir wins; otherwise the recorder reads the hdf5_save_folder setting when a session starts, # so a changed setting applies to the next recording. self._data_recorder.output_directory = output_dir self._session_h5_files: list = [] self._validator = DemoValidator() - self._spin_thread = None - self._should_spin = False + self._containers: list = [] self._config_path = config_path if config_path is not None: @@ -73,76 +67,95 @@ def initialize(self, config_path: str) -> bool: if not self._yaml_parser.load_yaml_file(config_path): return False - for input_data in self._yaml_parser.get_input_data(): - container = ContainerAdapter(input_data) - self._topic_subscribers.create_subscriber(container) + # The Config page's checks: refuse a dead topic / unreachable TF and a stream spec that does not parse + # (either would leave that input silently missing); the rest of check_config only warns, as there. + failures = self._topic_subscribers.unavailable_inputs(self._yaml_parser.get_input_data()) + failures += [f" {e}" for e in StreamBuilder(self._yaml_parser).get_build_errors()] + if failures: + self._yaml_parser.clear() + self._yaml_parser.current_config = self._yaml_parser.yaml_data = None # unloaded, as the Config page does + logger.error("Config rejected:\n" + "\n".join(failures)) + return False + problems = check_config(self._yaml_parser.yaml_data) + if problems: + logger.warning("Config check:\n" + "\n".join(f" {p.message}" for p in problems)) + # Subscribed at every start_recording, as in the GUI + self._containers = [ContainerAdapter(d) for d in self._yaml_parser.get_input_data()] self.start_spinning() return True def start_spinning(self): - self._should_spin = True - self._spin_thread = Thread(target=self._spin_loop, daemon=True) - self._spin_thread.start() - - def _spin_loop(self): - while self._should_spin: - with self._topic_subscribers._spin_lock: - rclpy.spin_once(self._topic_subscribers, timeout_sec=0.01) - rclpy.spin_once(self._data_recorder, timeout_sec=0.0) + self._topic_subscribers.start_spinning() def start_recording(self, demo_name: Optional[str] = None) -> bool: - started = self._data_recorder.start_recording( - self._topic_subscribers, demo_name, flush_stale_data=True - ) + """Start a demo, named like the GUI's (``{dataset_name}_{ms}``) unless ``demo_name`` is given.""" + try: + started = start_demo( # same steps as the GUI's Start + self._data_recorder, self._topic_subscribers, self._containers, + self._yaml_parser.get_dataset_name(), demo_name, + ) is not None + except TfTopicSubscriptionError as e: + logger.error(f"Recording NOT started: a configured TF topic could not be read: {e}") + return False if self._data_recorder.retried_file: # an earlier demo saved only now: keep it in the session self._session_h5_files.append(self._data_recorder.retried_file) return started - def pause_recording(self) -> bool: - return self._data_recorder.pause_recording() - - def resume_recording(self) -> bool: - return self._data_recorder.resume_recording() - def stop_recording(self) -> bool: result = self._data_recorder.stop_recording() + if not self._data_recorder.is_recording: # saved, or kept in memory for a retry: subscribe again at the next start + self._topic_subscribers.clear_all_subscribers() if result and self._data_recorder.h5_file_path: file_path = self._data_recorder.h5_file_path self._session_h5_files.append(file_path) try: - self._validator.validate(file_path) # logs the result + check = self._validator.validate(file_path) + for c in check.checks: # the GUI shows these in a dialog + if c.status != CheckStatus.PASS: + (logger.error if c.status == CheckStatus.FAIL else logger.warning)( + f"Demo check [{c.status.value.upper()}] {c.name}: {c.message}" + ) except Exception as e: logger.error(f"Error validating recording: {e}") return result - def get_recording_status(self) -> dict: - return { - "is_recording": self._data_recorder.is_recording, - "is_paused": self._data_recorder.is_paused, - "buffer_full": self._data_recorder.buffer_full, - "file_path": self._data_recorder.h5_file_path, - "samples": len(self._data_recorder.timestamps), - } + def _load_conversion_config(self, config_path: Optional[str], options_path: Optional[str]) -> None: + """The converter, with the config and format options of relay-d-convert (options: ``options_path``, else a + postprocess_config.yaml next to the config, else the defaults). Raises ValueError if a file is unusable.""" + self._yaml_converter = YAMLDrivenConverter(PostprocessConfigLoader()) + self._conversion_parser = None + if config_path: + self._conversion_parser, self._yaml_converter.options = load_conversion_config(config_path, options_path) + self._yaml_converter.data_config = self._conversion_parser.yaml_data["config"] def convert_recording_to_dataset(self, output_format: str = "robomimic") -> bool: - """Convert this session's recordings. `output_format` is "robomimic" (one .h5 - file) or "lerobot" (a LeRobotDataset v3.0 directory).""" + """Convert this session's recordings like the Post-process page: a new ``_`` folder in the + recordings folder with the dataset (robomimic .h5 or LeRobot v3.0 folder), the configs used and action_dispatcher/.""" try: if not self._session_h5_files: logger.warning("No recordings in this session to convert.") return False - name = "combined_dataset" + FORMAT_SUFFIX[output_format] + ("" if output_format == "lerobot" else ".h5") - output_path = os.path.join(self._data_recorder.recorded_data_dir, name) + if self._conversion_parser is None: + logger.error("No config loaded: nothing to convert with.") + return False + recordings = os.path.dirname(self._data_recorder.recorded_data_dir) # the session folder's parent + output_dir, output_path = dataset_output_paths(recordings, output_format, datetime.now()) success = self._yaml_converter.convert_multiple_files( output_format, self._session_h5_files, output_path ) - if success: - logger.info( - f"Successfully converted {len(self._session_h5_files)} recording(s) " - f"to: {output_path}" + if not success: + logger.error(f"Dataset conversion failed: {self._yaml_converter.last_error or 'see the log above'}") + return False + logger.info(f"{self._yaml_converter.conversion_summary()} -> {output_path}") + try: + write_dataset_files(output_dir, self._conversion_parser, self._yaml_converter.options.source) + except Exception as e: + logger.warning( + "The dataset was converted, but some extra files (config copies / action_dispatcher files) " + f"could not be written: {str(e) or type(e).__name__}" ) - return success + return True except Exception as e: logger.error(f"Error converting recording to dataset: {e}") return False @@ -160,28 +173,8 @@ def shutdown(self) -> bool: ) return False self._session_h5_files.append(self._data_recorder.retried_file) - self.convert_recording_to_dataset() - self._should_spin = False - - called_from_spin_thread = self._spin_thread is current_thread() - - if called_from_spin_thread: - spin_thread = self._spin_thread - rclpy_initialized = self._rclpy_initialized - topic_subscribers = self._topic_subscribers - - def _deferred_shutdown(): - if spin_thread and spin_thread.is_alive(): - spin_thread.join(timeout=2.0) - topic_subscribers.close() - if rclpy_initialized: - rclpy.shutdown() - - Thread(target=_deferred_shutdown, daemon=True).start() - else: - if self._spin_thread and self._spin_thread.is_alive(): - self._spin_thread.join(timeout=2.0) - self._topic_subscribers.close() - if self._rclpy_initialized: - rclpy.shutdown() + self.convert_recording_to_dataset(user_settings.get("postprocess_default_format") or "robomimic") + self._topic_subscribers.close() # stops its spin thread (no self-join when called from a callback) + if self._rclpy_initialized: + rclpy.shutdown() return True diff --git a/relay_d/acquisition/convert_cli.py b/relay_d/acquisition/convert_cli.py index fcb1ef1..911f269 100644 --- a/relay_d/acquisition/convert_cli.py +++ b/relay_d/acquisition/convert_cli.py @@ -7,6 +7,9 @@ "Load Config..." on the Post-process page. Format options come from ``--options`` or a ``postprocess_config.yaml`` next to the config (see relay_d/acquisition/qt_app/ui/config/templates/postprocess_config_template.yaml). +With ``--check`` nothing is written: it prints what the conversion would do (each demo's verdict, the output layout) +and exits 0 if at least one demo would be converted, else 1. + Exit codes: 0 success, 1 conversion failed, 2 bad arguments / unusable config or inputs. """ @@ -14,14 +17,12 @@ import argparse import os -import shutil import sys from typing import List, Optional -from .qt_app.ui.postprocess.offline_config import load_offline_config +from .qt_app.ui.postprocess.offline_config import load_conversion_config, write_dataset_files from .qt_app.ui.postprocess.postprocess_config_loader import PostprocessConfigLoader -from .qt_app.ui.postprocess.postprocess_options import PostprocessOptions from .qt_app.ui.postprocess.yaml_driven_converter import YAMLDrivenConverter, natural_key EXIT_OK, EXIT_FAILED, EXIT_USAGE = 0, 1, 2 @@ -52,13 +53,32 @@ def build_parser() -> argparse.ArgumentParser: ap.add_argument("--input", required=True, nargs="+", help=".h5 files and/or directories to search") ap.add_argument( "--output", - required=True, help="robomimic: output .h5 file; lerobot: dataset directory (must not exist or be empty)", ) ap.add_argument("--task", help="LeRobot task text (overrides postprocess_config.yaml / dataset_name)") + ap.add_argument("--check", action="store_true", help="write nothing: print what the conversion would do") return ap +def print_report(report: dict) -> None: + """The pre-check report (YAMLDrivenConverter.precheck) as text.""" + rows = [(f"{os.path.basename(d['file'])}::{d['demo']}", str(d["samples"]), + f"{d['rate_hz']:.1f} Hz" if d["rate_hz"] else "-", d["recorded_with"] or "-", + "converts" if d["ok"] else f"skipped: {d['reason']}") for d in report["demos"]] + header = ("Demo", "Samples", "Rate", "Recorded with", "Result") + widths = [max(len(r[i]) for r in [header, *rows]) for i in range(4)] + for row in [header, *rows]: + print(" ".join(c.ljust(w) for c, w in zip(row, widths)) + " " + row[4]) + if report["error"]: + print(f"\nerror: {report['error']}") + if report["output"]: + print(f"\nOutput ({report['format']}):") + width = max(len(name) for name, _ in report["output"]) + for name, detail in report["output"]: + print(f" {name.ljust(width)} {detail}") + print(f"\n{report['converted']} of {report['total']} demos would be converted") + + def _fail(message: str, code: int) -> int: print(f"error: {message}", file=sys.stderr) return code @@ -67,20 +87,12 @@ def _fail(message: str, code: int) -> int: def main(argv: Optional[List[str]] = None) -> int: args = build_parser().parse_args(argv) - parser, problems = load_offline_config(args.config) - if parser is None: - return _fail("unusable config:\n " + "\n ".join(problems), EXIT_USAGE) - for problem in problems: - print(problem, file=sys.stderr) - - options_path = args.options or PostprocessOptions.find_sibling(args.config) - options = PostprocessOptions() - if options_path: - loaded, opt_problems = PostprocessOptions.load(options_path) - if loaded is None: - return _fail(f"unusable options file {options_path}:\n " + "\n ".join(opt_problems), EXIT_USAGE) - options = loaded - print(f"Using format options from {options_path}") + try: + parser, options = load_conversion_config(args.config, args.options) + except ValueError as e: + return _fail(str(e), EXIT_USAGE) + if options.source: + print(f"Using format options from {options.source}") try: inputs = collect_inputs(args.input) @@ -89,6 +101,18 @@ def main(argv: Optional[List[str]] = None) -> int: if not inputs: return _fail("no .h5 files found in the given inputs", EXIT_USAGE) + converter = YAMLDrivenConverter( + PostprocessConfigLoader(), data_config=parser.yaml_data["config"] + ) + converter.options = options + + if args.check: + report = converter.precheck(args.format, inputs) + print_report(report) + return EXIT_OK if report["converted"] and not report["error"] else EXIT_FAILED + if not args.output: + return _fail("--output is required (unless --check)", EXIT_USAGE) + output = os.path.abspath(args.output) if args.format == "lerobot": if os.path.exists(output) and (not os.path.isdir(output) or os.listdir(output)): @@ -98,11 +122,6 @@ def main(argv: Optional[List[str]] = None) -> int: return _fail(f"output already exists, refusing to overwrite: {output}", EXIT_USAGE) os.makedirs(os.path.dirname(output), exist_ok=True) - converter = YAMLDrivenConverter( - PostprocessConfigLoader(), data_config=parser.yaml_data["config"] - ) - converter.options = options - print(f"Converting {len(inputs)} file(s) to {args.format} -> {output}") if args.format == "lerobot": ok = converter.convert_to_lerobot(inputs, output, task=args.task) @@ -111,16 +130,17 @@ def main(argv: Optional[List[str]] = None) -> int: if not ok: return _fail(converter.last_error or "conversion failed (see the log above)", EXIT_FAILED) - # Keep the configs used next to the result, like the GUI does. - if args.format == "lerobot": - dest_dir, prefix = output, "" - else: - dest_dir = os.path.dirname(output) - prefix = os.path.splitext(os.path.basename(output))[0] + "_" - shutil.copy2(args.config, os.path.join(dest_dir, f"{prefix}acquisition_config.yaml")) - if options.source: - shutil.copy2(options.source, os.path.join(dest_dir, f"{prefix}postprocess_config.yaml")) + # The configs used and action_dispatcher/, like the GUI: next to the .h5, above the LeRobot folder. + dest_dir = os.path.dirname(output) + if os.path.exists(os.path.join(dest_dir, "acquisition_config.yaml")): + print(f"warning: replacing the config copies / action_dispatcher files already in {dest_dir}", file=sys.stderr) + try: + write_dataset_files(dest_dir, parser, options.source) + except Exception as e: + print(f"warning: the dataset was converted, but some extra files (config copies / action_dispatcher files) " + f"could not be written: {str(e) or type(e).__name__}", file=sys.stderr) + print(converter.conversion_summary()) print(f"Done: {output}") return EXIT_OK diff --git a/relay_d/acquisition/qt_app/app.py b/relay_d/acquisition/qt_app/app.py index ec828f1..f15e34d 100755 --- a/relay_d/acquisition/qt_app/app.py +++ b/relay_d/acquisition/qt_app/app.py @@ -44,9 +44,10 @@ def apply_user_level(): from .ui.settings.settings_page import SettingsPage from .ui.review.review_page import ReviewPage from .ui.train.train_page import TrainPage + from .ui.run.run_page import RunPage except ImportError as e: logger.error(f"Failed to import one or more UI pages: {e}") - ConfigPage = RecordPage = PostProcessPage = SettingsPage = ReviewPage = TrainPage = None + ConfigPage = RecordPage = PostProcessPage = SettingsPage = ReviewPage = TrainPage = RunPage = None class UI(QMainWindow): @@ -76,6 +77,7 @@ def __init__(self, node): "btn_settings", "btn_review", "btn_train", + "btn_run", ]) # After setup_menu_buttons(self, menu_buttons) @@ -138,6 +140,8 @@ def __init__(self, node): self.page_train_ui = TrainPage( self, settings_page=getattr(self, "page_settings_ui", None) ) + if RunPage is not None: + self.page_run_ui = RunPage(self) if hasattr(self, "page_record") and hasattr(self, "page_config"): self.page_record.connect_to_config_signals( self.page_config.signal_manager @@ -195,12 +199,7 @@ def spin_once(self): """Spin all ROS nodes to process callbacks""" for node in self.ros_nodes: try: - lock = getattr(node, "_spin_lock", None) - if lock is not None: - with lock: - rclpy.spin_once(node, timeout_sec=0) - else: - rclpy.spin_once(node, timeout_sec=0) + rclpy.spin_once(node, timeout_sec=0) except Exception as e: logger.debug(f"Error spinning ROS node: {e}") @@ -224,6 +223,7 @@ def on_menu_button_clicked(self, btn_name): "btn_help": self.page_help, "btn_review": self.page_review, "btn_train": self.page_train, + "btn_run": self.page_run, } if btn_name in page_mapping: try: @@ -321,6 +321,9 @@ def closeEvent(self, event): event.ignore() return try: + if getattr(self, "page_run_ui", None) is not None: # a policy driving the robot stops first + self.page_run_ui.shutdown() + if hasattr(self, "page_record") and self.page_record is not None: # Stop TF update timerexit_code if hasattr(self.page_record, "tf_update_timer"): diff --git a/relay_d/acquisition/qt_app/ui/config/builder/builder_page.py b/relay_d/acquisition/qt_app/ui/config/builder/builder_page.py index a64e76f..a0e4971 100644 --- a/relay_d/acquisition/qt_app/ui/config/builder/builder_page.py +++ b/relay_d/acquisition/qt_app/ui/config/builder/builder_page.py @@ -1,7 +1,7 @@ """Config page > "Build from ROS": build an acquisition config from the live ROS 2 graph (topics, TF frames, message fields). The edited state is a ``ConfigDraft``; the pure modules next to this file do the work (``ros_catalog`` lists what the graph -offers, ``config_check`` validates, ``urdf_limits`` fills normalization limits). This file only turns them into widgets. +offers, ``config_check`` validates). This file only turns them into widgets. Steps are sub-tabs: Sources, Inputs, Observations, Actions, Recording, Review. """ @@ -34,7 +34,7 @@ from relay_d.utils.coloring_logger import logger from ....utils.custom_message_box import CustomMessageBox -from . import ros_catalog, urdf_limits +from . import ros_catalog from .config_check import ERROR, Problem, check_config, has_errors, stream_build_errors, stream_width from .draft import ConfigDraft, InputDraft, StreamDraft, suggest_input_name @@ -211,7 +211,7 @@ def add_tf_input(self, parent, child, tf_topic="/tf", name=None): def _build_inputs(self): w = QWidget() lay = QVBoxLayout(w) - self.table_inputs = _table(["Name", "Source", "Output map", "Modality", "Joint names", "Fill gaps"], stretch=1) + self.table_inputs = _table(["Name", "Source", "Modality", "Joint names", "Fill gaps"], stretch=1) self.table_inputs.itemChanged.connect(self._input_edited) lay.addWidget(self.table_inputs, 1) row = QHBoxLayout() @@ -219,7 +219,7 @@ def _build_inputs(self): row.addWidget(_button("Remove input", self.remove_selected_input)) row.addStretch(1) lay.addLayout(row) - note = QLabel("Output map: HDF5 group name, if not the input name. Modality: depth for depth images, scan for laser scans (empty = rgb).") + note = QLabel("Modality: depth for depth images, scan for laser scans (empty = rgb).") note.setWordWrap(True) lay.addWidget(note) return w @@ -230,7 +230,7 @@ def refresh_inputs(self): table.setRowCount(len(self.draft.inputs)) for r, inp in enumerate(self.draft.inputs.values()): source = f"TF {inp.parent_frame} -> {inp.child_frame}" if inp.kind == "tf" else inp.topic - cells = [_item(inp.name), _item(source), _item(inp.output_map, True), _item(inp.modality, True), _item(", ".join(inp.joint_names), True)] + cells = [_item(inp.name), _item(source), _item(inp.modality, True), _item(", ".join(inp.joint_names), True)] gaps = QTableWidgetItem() gaps.setFlags(Qt.ItemIsUserCheckable | Qt.ItemIsEnabled) gaps.setCheckState(Qt.Checked if inp.fill_gaps else Qt.Unchecked) @@ -243,12 +243,10 @@ def _input_edited(self, item): text = item.text().strip() col = item.column() if col == 2: - inp.output_map = text - elif col == 3: inp.modality = text - elif col == 4: + elif col == 3: inp.joint_names = [n.strip() for n in text.split(",") if n.strip()] - elif col == 5: + elif col == 4: inp.fill_gaps = item.checkState() == Qt.Checked def _selected_input(self): @@ -277,11 +275,10 @@ def _joint_names_read(self, inp, msg): inp.joint_names = names self.refresh_inputs() - def peek(self, topic, msg_type, on_msg, latched=False): + def peek(self, topic, msg_type, on_msg): """Call ``on_msg(message)`` with the next message on ``topic``, or ``on_msg(None)`` after a few seconds. Uses the GUI node (already spun by the app's timer), so nothing blocks and no second spin runs. - ``latched``: also get the last message published before (``/robot_description``). """ node = self._get_node() if node is None: @@ -289,9 +286,9 @@ def peek(self, topic, msg_type, on_msg, latched=False): return box = {} try: - from rclpy.qos import QoSDurabilityPolicy, QoSProfile, ReliabilityPolicy + from rclpy.qos import QoSProfile, ReliabilityPolicy - qos = QoSProfile(depth=1, durability=QoSDurabilityPolicy.TRANSIENT_LOCAL) if latched else QoSProfile(depth=1, reliability=ReliabilityPolicy.BEST_EFFORT) + qos = QoSProfile(depth=1, reliability=ReliabilityPolicy.BEST_EFFORT) sub = node.create_subscription(self._loader(msg_type), topic, lambda m: box.setdefault("msg", m), qos) except Exception as e: logger.warning(f"Could not subscribe to {topic}: {e}") @@ -338,27 +335,13 @@ def _build_observations(self): self.text_specs = QPlainTextEdit() self.text_specs.setPlaceholderText("One spec per line, e.g. joint_1.position[:] or tf_eef.translation.x or a.x - b.x") self.text_specs.setMaximumHeight(80) - self.text_specs.textChanged.connect(self._resize_limits) form.addRow("Name", self.edit_stream_name) form.addRow("Specs", self.text_specs) lay.addLayout(form) row = QHBoxLayout() - self.check_normalize = QCheckBox("Normalize to [-1, 1] (needs limits)") - self.check_normalize.toggled.connect(lambda _on: self._resize_limits()) self.check_rpy = QCheckBox("Quaternion to roll / pitch / yaw (4 specs: x y z w)") - row.addWidget(self.check_normalize) row.addWidget(self.check_rpy) row.addStretch(1) - lay.addLayout(row) - self.table_limits = _table(["Lower", "Upper"], select_rows=False) - self.table_limits.setMaximumHeight(130) - self.table_limits.horizontalHeader().setSectionResizeMode(1, QHeaderView.Stretch) - self.table_limits.setVisible(False) # shown once "Normalize" is ticked - lay.addWidget(self.table_limits) - row = QHBoxLayout() - row.addWidget(_button("Fill limits from URDF file...", self.fill_limits_from_file)) - row.addWidget(_button("Fill limits from /robot_description", self.fill_limits_from_topic)) - row.addStretch(1) row.addWidget(_button("Add stream", self.add_stream)) lay.addLayout(row) @@ -423,43 +406,15 @@ def _specs(self): def _inputs_dict(self): return self.draft.to_config()["config"]["input"] - def _resize_limits(self): - width = stream_width(self._specs(), self._inputs_dict()) if self.check_normalize.isChecked() else 0 - rows = width if width else (1 if self.check_normalize.isChecked() else 0) - old = [(self.table_limits.item(r, 0), self.table_limits.item(r, 1)) for r in range(self.table_limits.rowCount())] - self.table_limits.setRowCount(rows) - for r in range(rows): - for c in (0, 1): - keep = old[r][c].text() if r < len(old) and old[r][c] else "" - self.table_limits.setItem(r, c, _item(keep, True)) - self.table_limits.setVisible(rows > 0) - - def _limits(self): - out = [] - for r in range(self.table_limits.rowCount()): - lo, hi = (self.table_limits.item(r, c).text().strip() for c in (0, 1)) - try: - out.append({"lower_lim": float(lo), "upper_lim": float(hi)}) - except ValueError: - raise ValueError(f"limit row {r + 1} needs two numbers (lower, upper)") from None - return out - def add_stream(self): name, specs = self.edit_stream_name.text().strip(), self._specs() if not name or not specs: CustomMessageBox.warning("A stream needs a name and at least one spec.") return None - try: - limits = self._limits() if self.check_normalize.isChecked() else [] - except ValueError as e: - CustomMessageBox.warning(str(e)) - return None - stream = StreamDraft(name=name, specs=specs, normalize=self.check_normalize.isChecked(), limits=limits, - rotation_format="rpy" if self.check_rpy.isChecked() else "quat", explicit_dict=self.check_normalize.isChecked()) + stream = StreamDraft(name=name, specs=specs, rotation_format="rpy" if self.check_rpy.isChecked() else "quat") self.draft.streams[name] = stream self.edit_stream_name.clear() self.text_specs.clear() - self.check_normalize.setChecked(False) self.check_rpy.setChecked(False) self._fill_fields() self.refresh_observations() @@ -501,46 +456,6 @@ def refresh_observations(self): for key, (inp, kind) in self.draft.cameras.items(): self.list_obs.addItem(f"{key}: {kind[:-1]} of {inp}") - # URDF limits for the stream being built - - def _fill_limits(self, xml): - specs = self._specs() - try: - if len(specs) != 1 or not specs[0].endswith("[:]"): - raise ValueError("limits from a URDF need exactly one spec like joint_1.position[:]") - input_name, field = specs[0][:-3].split(".", 1) - names = self.draft.inputs[input_name].joint_names - if not names: - raise ValueError(f"'{input_name}' has no joint_names: read them on the Inputs step first") - limits = urdf_limits.limits_for(field, urdf_limits.parse_joint_limits(xml, names), names) - except (ValueError, KeyError) as e: - CustomMessageBox.warning(f"Could not fill the limits: {e}") - return - self.check_normalize.setChecked(True) - self._resize_limits() - for r, lim in enumerate(limits): - self.table_limits.item(r, 0).setText(f"{lim['lower_lim']:.6g}") - self.table_limits.item(r, 1).setText(f"{lim['upper_lim']:.6g}") - - def fill_limits_from_file(self): - path, _ = QFileDialog.getOpenFileName(self, "Robot description", self._dialog_dir(), "URDF / xacro (*.urdf *.xacro *.xml)") - if path: - from ...review.robot_description_resolver import RobotDescriptionError, resolve_from_file - - try: - self._fill_limits(resolve_from_file(path)) - except RobotDescriptionError as e: - CustomMessageBox.warning(str(e)) - - def fill_limits_from_topic(self): - def done(msg): - if msg is None: - CustomMessageBox.warning(f"No /robot_description received within {_PEEK_SECONDS:.0f} s. Is robot_state_publisher running?") - else: - self._fill_limits(msg.data) - - self.peek("/robot_description", "std_msgs/msg/String", done, latched=True) - # ------------------------------------------------------------------ 4 Actions def _build_actions(self): @@ -622,7 +537,8 @@ def refresh_recording(self): for widget in (self.edit_dataset, self.spin_freq, self.spin_buffer, self.spin_tol, self.combo_sync): widget.blockSignals(True) self.edit_dataset.setText(self.draft.dataset_name) - self.spin_freq.setValue(self.draft.recording_frequency or 30) + self.draft.recording_frequency = self.draft.recording_frequency or 30 # the value on screen is the one written + self.spin_freq.setValue(self.draft.recording_frequency) self.spin_buffer.setValue(self.draft.max_buffer_mb or 0) self.spin_tol.setValue(self.draft.sync_tolerance_sec or 0) self.combo_sync.setCurrentText(self.draft.sync_mode) diff --git a/relay_d/acquisition/qt_app/ui/config/builder/config_check.py b/relay_d/acquisition/qt_app/ui/config/builder/config_check.py index c2807ad..ef1e845 100644 --- a/relay_d/acquisition/qt_app/ui/config/builder/config_check.py +++ b/relay_d/acquisition/qt_app/ui/config/builder/config_check.py @@ -1,12 +1,13 @@ """Checks of an acquisition config dict, shared by the Config builder, the Config page and the offline loader (no Qt, no ROS). -The builder treats ``error`` problems as blocking. The load paths (Config page insert / save, ``load_offline_config``) show them -all as warnings: hand-written configs that worked before must keep loading. +The builder treats ``error`` problems as blocking. The Config page shows them all as a warning when a file loads; the recorder +refuses the removed keys that changed the recorded data (``RECORDING_CHANGING``). """ from __future__ import annotations import math +import re from dataclasses import dataclass from typing import Any, Dict, List, Optional @@ -31,7 +32,9 @@ def stream_width(specs: List[str], inputs: Dict[str, dict]) -> Optional[int]: """Number of values one stream row has, or None if it cannot be known from the config (a ``[:]`` slice without joint_names).""" width = 0 for spec in specs: - if spec.rstrip().endswith("[:]"): + if re.fullmatch(r"zero:\d+", spec.strip()): # zero:N is N zeros + width += int(spec.strip()[5:]) + elif re.sub(r"^(sin|cos)\((.*)\)$", r"\2", spec.strip()).endswith("[:]"): # sin(a.position[:]) is as wide as its slice names = (inputs.get(next(iter(spec_inputs(spec)), ""), {}) or {}).get("joint_names") if not names: return None @@ -52,9 +55,11 @@ def check_config(data: Dict[str, Any]) -> List[Problem]: if not isinstance(config, dict): return [Problem(ERROR, "the file has no 'config:' section")] + for key, why in removed_keys(config): + add(ERROR, why) freq = config.get("recording_frequency") if freq is None: - add(WARNING, "recording_frequency is not set (30 Hz is used; LeRobot conversion needs a frequency in the config)") + add(ERROR, "recording_frequency is required (the rate in Hz every input is sampled at)") elif not _numbers(freq) or freq <= 0: add(ERROR, f"recording_frequency must be a positive number, got {freq!r}") if config.get("sync_mode") not in (None, "soft", "strict"): @@ -62,21 +67,14 @@ def check_config(data: Dict[str, Any]) -> List[Problem]: # ---- inputs inputs = {n: v for n, v in (config.get("input") or {}).items() if isinstance(v, dict)} - groups: Dict[str, str] = {} for name, raw in inputs.items(): is_tf = "parent_frame" in raw and "child_frame" in raw if not is_tf and not raw.get("topic"): add(WARNING, f"input '{name}' has neither a topic nor parent_frame + child_frame: it is ignored") - if str(raw.get("modality") or "rgb").split()[0] not in _MODALITIES: + if str(raw.get("modality") or "rgb") not in _MODALITIES: add(WARNING, f"input '{name}': modality '{raw.get('modality')}' is not one of {_MODALITIES}") - group = raw.get("output_map") or name - if group in groups: - add(ERROR, f"inputs '{groups[group]}' and '{name}' are both recorded into the group '{group}': give one an output_map") - groups[group] = name - if group == "streams": - add(ERROR, f"input '{name}' would be recorded into the group 'streams', which holds the output.streams data") - if raw.get("fill_default") is not None and not raw.get("fill_gaps"): - add(WARNING, f"input '{name}': fill_default has no effect without fill_gaps: true") + if name == "streams": + add(ERROR, "an input named 'streams' would be recorded into the group that holds the output.streams data: rename it") # ---- streams output = config.get("output") or {} @@ -89,25 +87,13 @@ def check_config(data: Dict[str, Any]) -> List[Problem]: continue for inp in sorted({i for s in specs for i in spec_inputs(s)} - set(inputs)): add(ERROR, f"stream '{name}' uses the input '{inp}', which is not defined under input:") - width = stream_width(specs, inputs) fmt = value_dict.get("rotation_format", "quat") if fmt not in ("quat", "rpy"): add(ERROR, f"stream '{name}': rotation_format must be 'quat' or 'rpy', got {fmt!r}") elif fmt == "rpy" and (len(specs) != 4 or any(s.rstrip().endswith("[:]") for s in specs)): add(ERROR, f"stream '{name}': rotation_format rpy needs exactly 4 scalar quaternion specs (x, y, z, w), has {len(specs)}") - if value_dict.get("normalize"): - limits = value_dict.get("limits") - columns = 3 if fmt == "rpy" else width - if not limits: - add(ERROR, f"stream '{name}': normalize is on but there are no limits") - continue - bad = [lim for lim in limits if not (isinstance(lim, dict) and _numbers(lim.get("lower_lim")) and _numbers(lim.get("upper_lim")))] - if bad: - add(ERROR, f"stream '{name}': every limit needs numeric lower_lim and upper_lim (found {bad[0]!r})") - elif any(lim["lower_lim"] >= lim["upper_lim"] for lim in limits): - add(ERROR, f"stream '{name}': lower_lim must be smaller than upper_lim") - elif columns is not None and len(limits) not in (1, columns): - add(ERROR, f"stream '{name}': {len(limits)} limits for {columns} values (use one per value, or one for all)") + if msg := normalization_error(name, value): + add(ERROR, msg) # ---- camera / point cloud entries for key, value in output.items(): @@ -117,26 +103,61 @@ def check_config(data: Dict[str, Any]) -> List[Problem]: add(WARNING, f"output entry '{key}' should look like '/images' or '/points'") continue group = value.split("/", 1)[0] - if group not in groups: - if group in inputs: - add(ERROR, f"output '{key}' points at '{group}', but that input is recorded as '{inputs[group].get('output_map')}' (its output_map)") - else: - add(WARNING, f"output '{key}' points at '{group}', which is not an input") + if group not in inputs: + add(WARNING, f"output '{key}' points at '{group}', which is not an input") # ---- actions - known = set(streams) | {k for k in output if k not in ("streams", "prefix_path")} actions = config.get("actions_to_extract") or [] + if not actions: + add(ERROR, "actions_to_extract is empty: name the stream(s) the policy outputs as its action") for name in actions: - if name not in known: - add(ERROR, f"actions_to_extract: '{name}' is not a stream or output entry") + if name not in streams: + add(ERROR, f"actions_to_extract: '{name}' is not a stream under output.streams") for name in config.get("delta_actions") or []: if name not in actions: add(ERROR, f"delta_actions: '{name}' is not listed in actions_to_extract") - elif isinstance(streams.get(name), dict) and streams[name].get("normalize"): - add(ERROR, f"delta_actions: '{name}' is normalized; the dispatcher cannot undo a normalized delta (use a raw stream for the action)") return problems +_DEAD = "it was never used" +# (section, key) -> why it was removed and what to do. An old config that still has one gets an error, never silent ignoring. +_REMOVED = { + ("input", "output_map"): "inputs are always recorded under their own name (output_map made recording, conversion and " + "dispatch disagree). Delete it and rename the input to the group name you want", + ("input", "fill_default"): "it wrote a made-up value before the first real message. Delete it: with fill_gaps the " + "first real message fills the leading samples", + ("input", "transfer_rate"): f"{_DEAD}. Delete it: recording_frequency sets the rate of every input", + ("output", "prefix_path"): f"{_DEAD}. Delete it", + ("config", "data"): f"{_DEAD}. Delete it", + ("metadata", "recording_frequency"): "the rate has one place. Delete it and set config.recording_frequency instead", + ("metadata", "transfer_rate"): f"{_DEAD}. Delete it: config.recording_frequency sets the rate", +} +RECORDING_CHANGING = ("output_map", "fill_default") # the recorder refuses these (they changed the recorded data) + + +def removed_keys(config: Dict[str, Any]) -> List[tuple]: + """``(key, message)`` for every removed key still in ``config`` (the ``config:`` section).""" + sections = {"config": config, "output": config.get("output"), "metadata": config.get("metadata")} + found = [] + for (section, key), why in _REMOVED.items(): + if section == "input": + for name, raw in (config.get("input") or {}).items(): + if isinstance(raw, dict) and key in raw: + found.append((key, f"input '{name}': '{key}' was removed: {why}.")) + elif isinstance(sections[section], dict) and key in sections[section]: + label = key if section in ("config", "output") else f"{section}.{key}" + found.append((key, f"'{label}' was removed: {why}.")) + return found + + +def normalization_error(name: str, value: Any) -> Optional[str]: + """The error for a stream that still has the removed ``normalize`` / ``normalized_range`` / ``limits`` keys, else None.""" + if isinstance(value, dict) and any(k in value for k in ("normalize", "normalized_range", "limits")): + return (f"Relay-D no longer normalizes: robomimic and LeRobot normalize with the dataset's stats. " + f"Remove normalize / normalized_range / limits from stream '{name}'.") + return None + + def stream_build_errors(data: Dict[str, Any]) -> List[str]: """Spec syntax errors, from the same ``StreamBuilder`` the recorder and the Config page use.""" from relay_d.acquisition.qt_app.ui.config.yaml_parser import YamlParser @@ -150,8 +171,3 @@ def stream_build_errors(data: Dict[str, Any]) -> List[str]: def has_errors(problems: List[Problem]) -> bool: return any(p.level == ERROR for p in problems) - - -def as_warnings(problems: List[Problem]) -> List[str]: - """For the load paths: every problem as a ``warning: ...`` line (they never reject a file that loaded before).""" - return [f"warning: {p.message}" for p in problems] diff --git a/relay_d/acquisition/qt_app/ui/config/builder/draft.py b/relay_d/acquisition/qt_app/ui/config/builder/draft.py index cefbbc6..20d1558 100644 --- a/relay_d/acquisition/qt_app/ui/config/builder/draft.py +++ b/relay_d/acquisition/qt_app/ui/config/builder/draft.py @@ -2,7 +2,7 @@ ``ConfigDraft`` holds what the Config builder edits and writes the same ``{"config": {...}}`` dict a hand-written ``acquisition_config.yaml`` has. ``from_config`` reads such a dict back, so an existing file can be edited in the builder; -keys the builder does not know (``metadata``, ``prefix_path``, ...) are kept and written back unchanged. +keys the builder does not know (``metadata``, ...) are kept and written back unchanged. """ from __future__ import annotations @@ -20,10 +20,9 @@ } _KNOWN_TOP = ("dataset_name", "recording_frequency", "max_buffer_mb", "sync_tolerance_sec", "sync_mode", "input", "output", "actions_to_extract", "delta_actions") -_KNOWN_TOPIC_INPUT = ("topic", "output_map", "modality", "joint_names", "fill_gaps", "fill_default", "transfer_rate") -_KNOWN_TF_INPUT = ("parent_frame", "child_frame", "tf_topic", "tf_static_topic", "tf_wait_timeout", - "output_map", "fill_gaps", "fill_default", "transfer_rate") -_STREAM_DICT_KEYS = ("specs", "normalize", "normalized_range", "limits", "rotation_format") +_KNOWN_TOPIC_INPUT = ("topic", "modality", "joint_names", "fill_gaps") +_KNOWN_TF_INPUT = ("parent_frame", "child_frame", "tf_topic", "tf_static_topic", "tf_wait_timeout", "fill_gaps") +_STREAM_DICT_KEYS = ("specs", "rotation_format") # anything else (e.g. the removed normalize keys) goes to extra @dataclass @@ -32,12 +31,9 @@ class InputDraft: kind: str = "topic" # "topic" | "tf" topic: str = "" msg_type: str = "" # only to remember what the live graph said; not written to the file - output_map: str = "" # "" = the input name modality: str = "" # "" = not written (rgb is the default); "depth" | "scan" joint_names: List[str] = field(default_factory=list) fill_gaps: bool = False - fill_default: Any = None - transfer_rate: Optional[float] = None parent_frame: str = "" child_frame: str = "" tf_topic: str = "" # "" = /tf @@ -45,11 +41,6 @@ class InputDraft: tf_wait_timeout: Optional[float] = None extra: Dict[str, Any] = field(default_factory=dict) - @property - def group(self) -> str: - """Name of the HDF5 group this input is recorded into.""" - return self.output_map or self.name - def to_dict(self) -> Dict[str, Any]: out: Dict[str, Any] = {} if self.kind == "tf": @@ -65,14 +56,8 @@ def to_dict(self) -> Dict[str, Any]: out["modality"] = self.modality if self.joint_names: out["joint_names"] = list(self.joint_names) - if self.output_map: - out["output_map"] = self.output_map - if self.transfer_rate is not None: - out["transfer_rate"] = self.transfer_rate if self.fill_gaps: out["fill_gaps"] = True - if self.fill_default is not None: - out["fill_default"] = self.fill_default out.update(self.extra) return out @@ -82,9 +67,8 @@ def from_dict(cls, name: str, raw: Dict[str, Any]) -> "InputDraft": known = _KNOWN_TF_INPUT if is_tf else _KNOWN_TOPIC_INPUT return cls( name=name, kind="tf" if is_tf else "topic", topic=raw.get("topic", ""), - output_map=raw.get("output_map", ""), modality=raw.get("modality", ""), + modality=raw.get("modality", ""), joint_names=list(raw.get("joint_names") or []), fill_gaps=bool(raw.get("fill_gaps", False)), - fill_default=raw.get("fill_default"), transfer_rate=raw.get("transfer_rate"), parent_frame=raw.get("parent_frame", ""), child_frame=raw.get("child_frame", ""), tf_topic=raw.get("tf_topic", ""), tf_static_topic=raw.get("tf_static_topic", ""), tf_wait_timeout=raw.get("tf_wait_timeout"), @@ -98,21 +82,14 @@ class StreamDraft: name: str specs: List[str] = field(default_factory=list) - normalize: bool = False - normalized_range: List[float] = field(default_factory=lambda: [-1.0, 1.0]) - limits: List[Dict[str, float]] = field(default_factory=list) rotation_format: str = "quat" # "rpy": 4 quaternion specs become roll, pitch, yaw explicit_dict: bool = False # written as a dict even if nothing needs it (keeps a loaded file's shape) extra: Dict[str, Any] = field(default_factory=dict) def to_value(self) -> Any: - if not (self.normalize or self.rotation_format != "quat" or self.explicit_dict or self.extra): + if not (self.rotation_format != "quat" or self.explicit_dict or self.extra): return list(self.specs) out: Dict[str, Any] = {"specs": list(self.specs)} - if self.normalize: - out["normalize"] = True - out["normalized_range"] = list(self.normalized_range) - out["limits"] = [dict(lim) for lim in self.limits] if self.rotation_format != "quat": out["rotation_format"] = self.rotation_format out.update(self.extra) @@ -123,9 +100,7 @@ def from_value(cls, name: str, value: Any) -> "StreamDraft": if not isinstance(value, dict): return cls(name=name, specs=[str(s) for s in (value or [])]) return cls( - name=name, specs=[str(s) for s in value.get("specs", [])], normalize=bool(value.get("normalize", False)), - normalized_range=list(value.get("normalized_range", [-1.0, 1.0])), - limits=[dict(lim) for lim in (value.get("limits") or []) if isinstance(lim, dict)], + name=name, specs=[str(s) for s in value.get("specs", [])], rotation_format=str(value.get("rotation_format", "quat")), explicit_dict=True, extra={k: v for k, v in value.items() if k not in _STREAM_DICT_KEYS}, ) @@ -144,7 +119,6 @@ class ConfigDraft: actions: List[str] = field(default_factory=list) # actions_to_extract: stream names, in order delta_actions: List[str] = field(default_factory=list) extra: Dict[str, Any] = field(default_factory=dict) # unknown keys of `config` - output_extra: Dict[str, Any] = field(default_factory=dict) # unknown keys of `config.output` (e.g. prefix_path) raw_bridges: Dict[str, Any] = field(default_factory=dict) # bridge entries that point at no known input: kept as they were # ------------------------------------------------------------------ editing helpers @@ -186,11 +160,9 @@ def to_config(self) -> Dict[str, Any]: config["input"] = {name: inp.to_dict() for name, inp in self.inputs.items()} output: Dict[str, Any] = dict(self.raw_bridges) for key, (inp_name, dataset) in self.cameras.items(): - inp = self.inputs.get(inp_name) - output[key] = f"{inp.group if inp else inp_name}/{dataset}" + output[key] = f"{inp_name}/{dataset}" if self.streams: output["streams"] = {name: s.to_value() for name, s in self.streams.items()} - output.update(self.output_extra) config["output"] = output if self.actions: config["actions_to_extract"] = list(self.actions) @@ -218,15 +190,12 @@ def from_config(cls, data: Dict[str, Any]) -> "ConfigDraft": for name, raw in (config.get("input") or {}).items(): if isinstance(raw, dict): draft.inputs[name] = InputDraft.from_dict(name, raw) - by_group = {inp.group: inp.name for inp in draft.inputs.values()} for key, value in (config.get("output") or {}).items(): if key == "streams": draft.streams = {n: StreamDraft.from_value(n, v) for n, v in (value or {}).items()} - elif isinstance(value, str) and "/" in value and value.split("/", 1)[0] in by_group \ + elif isinstance(value, str) and "/" in value and value.split("/", 1)[0] in draft.inputs \ and value.split("/", 1)[1] in ("images", "points"): - draft.cameras[key] = (by_group[value.split("/", 1)[0]], value.split("/", 1)[1]) - elif key == "prefix_path": - draft.output_extra[key] = value + draft.cameras[key] = tuple(value.split("/", 1)) else: draft.raw_bridges[key] = value return draft diff --git a/relay_d/acquisition/qt_app/ui/config/builder/urdf_limits.py b/relay_d/acquisition/qt_app/ui/config/builder/urdf_limits.py index fd8a02b..03b3c4e 100644 --- a/relay_d/acquisition/qt_app/ui/config/builder/urdf_limits.py +++ b/relay_d/acquisition/qt_app/ui/config/builder/urdf_limits.py @@ -1,4 +1,4 @@ -"""Joint limits from a URDF string (stdlib only), used by the Config builder and ``scripts/extract_joint_limits.py``.""" +"""Joint limits from a URDF string (stdlib only), used by ``scripts/extract_joint_limits.py``.""" from __future__ import annotations @@ -35,24 +35,3 @@ def parse_joint_limits(xml_string: str, joint_filter: Optional[List[str]] = None return [joints[n] for n in joint_filter if n in joints] return list(joints.values()) - -def limits_for(field: str, joints: List[Dict], names: List[str]) -> List[Dict]: - """``limits:`` entries for the stream field ``position`` / ``velocity`` / ``effort`` of the joints ``names``, in that order. - - Position uses the URDF lower/upper, velocity and effort are symmetric (±max), like ``scripts/extract_joint_limits.py``. - Raises ``ValueError`` for an unknown field, for joints missing from ``joints`` or for a joint whose URDF limit is 0 (not set). - """ - if field not in ("position", "velocity", "effort"): - raise ValueError(f"no URDF limit for '{field}' (only position, velocity and effort have one)") - by_name = {j["name"]: j for j in joints} - missing = [n for n in names if n not in by_name] - if missing: - raise ValueError(f"joints not in the URDF (or not revolute / prismatic): {missing}") - out = [] - for n in names: - j = by_name[n] - lo, hi = (j["lower"], j["upper"]) if field == "position" else (-j[field], j[field]) - if lo >= hi: - raise ValueError(f"the URDF has no usable {field} limit for joint '{n}' ({lo} .. {hi})") - out.append({"lower_lim": lo, "upper_lim": hi}) - return out diff --git a/relay_d/acquisition/qt_app/ui/config/config_page.py b/relay_d/acquisition/qt_app/ui/config/config_page.py index 2018a24..1d26b05 100644 --- a/relay_d/acquisition/qt_app/ui/config/config_page.py +++ b/relay_d/acquisition/qt_app/ui/config/config_page.py @@ -181,7 +181,7 @@ def load_config_path(self, fn): return def _warn_about_problems(self): - """Show what the shared config check finds (undefined inputs, limits count, ...). Warnings only: configs written + """Show what the shared config check finds (undefined inputs, removed normalize keys, ...). Warnings only: configs written by hand that worked before must still load.""" problems = check_config(self.yaml_parser.yaml_data) if problems: @@ -203,33 +203,7 @@ def _validate_input_availability(self): ) return [] - failures = [] - for item in self.yaml_parser.get_input_data(): - name = item["name"] - if item.get("is_tf"): - tf_topic = item.get("tf_topic", "/tf") - tf_static_topic = item.get("tf_static_topic", "/tf_static") - logger.info( - f"Validating TF input '{name}': {item['parent_frame']} -> " - f"{item['child_frame']} on {tf_topic}..." - ) - if not topic_subscribers.is_tf_available( - item["parent_frame"], - item["child_frame"], - tf_topic=tf_topic, - tf_static_topic=tf_static_topic, - timeout=item.get("tf_wait_timeout"), - ): - failures.append( - f" {name}: TF {item['parent_frame']} -> " - f"{item['child_frame']} not available on " - f"tf_topic={tf_topic}" - ) - elif item.get("topic"): - logger.info(f"Validating input '{name}': topic {item['topic']}...") - if not topic_subscribers.is_topic_available(item["topic"]): - failures.append(f" {name}: topic {item['topic']} not found") - return failures + return topic_subscribers.unavailable_inputs(self.yaml_parser.get_input_data()) def _validate_stream_specs(self): """Build a throwaway StreamBuilder against the currently-loaded @@ -319,11 +293,11 @@ def update_metrics(self): try: # Get input data input_data = self.yaml_parser.get_input_data() - output_data = self.yaml_parser.get_output_data() + output_names = list(self.yaml_parser.get_streams()) + list(self.yaml_parser.get_output_bridge_map()) # Count inputs and outputs num_inputs = len(input_data) if input_data else 0 - num_outputs = len(output_data) if output_data else 0 + num_outputs = len(output_names) # Get input types input_types = set() @@ -335,13 +309,7 @@ def update_metrics(self): input_types.add(base_type) # Get output types - output_types = set() - if output_data: - for item in output_data: - name = item.get("name", "") - if name: - base_type = name.split("_")[0] - output_types.add(base_type) + output_types = {name.split("_")[0] for name in output_names} # Update labels if hasattr(self.ui, "inputsCountLabel"): diff --git a/relay_d/acquisition/qt_app/ui/config/templates/config_template.yaml b/relay_d/acquisition/qt_app/ui/config/templates/config_template.yaml index ed1882c..5f87754 100644 --- a/relay_d/acquisition/qt_app/ui/config/templates/config_template.yaml +++ b/relay_d/acquisition/qt_app/ui/config/templates/config_template.yaml @@ -12,7 +12,7 @@ config: # --------------------------------------------------------------------------- dataset_name: my_robot_demo # prefix used for HDF5 group names (e.g. demo_0, demo_1) - recording_frequency: 20 # samples per second saved to disk (Hz) + recording_frequency: 20 # required: samples per second saved to disk (Hz) # max_buffer_mb: 4096 # optional: recording stops sampling past this much buffered image data (Stop still saves) # --------------------------------------------------------------------------- @@ -130,7 +130,7 @@ config: # when omitted — existing configs need no changes. camera_rgb: topic: /camera/color/image_raw - modality: rgb (default, can be omitted) + modality: rgb # the default, can be omitted # --- Depth image ------------------------------------------------------------ # Also a sensor_msgs/Image, but decoded with cv_bridge's "passthrough" @@ -208,7 +208,7 @@ config: # --------------------------------------------------------------------------- # Two stream formats are supported: # - # Plain list form (no normalization): + # Plain list form: # : # - . # - .[:] @@ -216,21 +216,14 @@ config: # - sin/cos(.[:]) # - zero:N # - # Dict form (with optional normalization applied at conversion time): + # Dict form: # : # specs: [...] # same spec syntax as plain list - # normalize: true # set false to skip normalization - # normalized_range: [-1.0, 1.0] # target output range - # limits: - # - {lower_lim: ..., upper_lim: ...} # one entry per column, OR - # - {lower_lim: ..., upper_lim: ...} # one entry → broadcast to all columns # rotation_format: quat | rpy # see "Rotation representation" below # - # Notes on normalization: - # - sin/cos-wrapped specs are already in [-1, 1]; no limits needed. - # - Quaternion streams (rotation.x/y/z/w) are already in [-1, 1]. - # - Use ??? as a placeholder for limits you will fill in after inspecting data. - # - Normalization is applied during HDF5 postprocessing, not at recording time. + # Values are stored in real units: robomimic and LeRobot normalize with the + # dataset's stats at training time (normalize / normalized_range / limits are + # no longer accepted here and fail the config check and the conversion). # # Rotation representation (quaternion vs RPY): # - Rotation is always *recorded* as quaternion (x,y,z,w) — TF/Pose messages @@ -261,10 +254,10 @@ config: # nested inside it) map an obs_key straight to a raw recorded HDF5 path: # : / # - # This is the only way to get raw pixel/point data into obs/next_obs, and it + # This is the only way to get raw pixel/point data into obs, and it # freely coexists with output.streams — a config can have both numeric # streams (joint positions, TF, ...) and bridge-map image/pointcloud obs at - # the same time, all ending up in the same demo's obs/next_obs. + # the same time, all ending up in the same demo's obs. # # Recorded dataset names by input type (see the `input:` examples above): # Image (rgb) -> /images (N, H, W, 3) uint8 @@ -314,63 +307,67 @@ config: - tf_eef.rotation.z - tf_eef.rotation.w + # The streams below read inputs that are only commented examples above + # (tf_object, ee_pose, transform_pub, force_z, gripper_state): uncomment + # the input together with its stream. + # Object position — shape (N, 3) - object_translation: - - tf_object.translation.x - - tf_object.translation.y - - tf_object.translation.z + # object_translation: + # - tf_object.translation.x + # - tf_object.translation.y + # - tf_object.translation.z # Object orientation — shape (N, 4) - object_rotation: - - tf_object.rotation.x - - tf_object.rotation.y - - tf_object.rotation.z - - tf_object.rotation.w + # object_rotation: + # - tf_object.rotation.x + # - tf_object.rotation.y + # - tf_object.rotation.z + # - tf_object.rotation.w # PoseStamped position — shape (N, 3) # Use this when subscribing a geometry_msgs/PoseStamped topic. - ee_pose_position: - - ee_pose.pose.position.x - - ee_pose.pose.position.y - - ee_pose.pose.position.z + # ee_pose_position: + # - ee_pose.pose.position.x + # - ee_pose.pose.position.y + # - ee_pose.pose.position.z # PoseStamped orientation — shape (N, 4) - ee_pose_orientation: - - ee_pose.pose.orientation.x - - ee_pose.pose.orientation.y - - ee_pose.pose.orientation.z - - ee_pose.pose.orientation.w + # ee_pose_orientation: + # - ee_pose.pose.orientation.x + # - ee_pose.pose.orientation.y + # - ee_pose.pose.orientation.z + # - ee_pose.pose.orientation.w # TransformStamped (topic) translation — shape (N, 3) # Note: "transform." prefix IS required here (unlike TF inputs). - ts_translation: - - transform_pub.transform.translation.x - transform_pub.transform.translation.y - - transform_pub.transform.translation.y - - transform_pub.transform.translation.z + # ts_translation: + # - transform_pub.transform.translation.x - transform_pub.transform.translation.y # arithmetic example: x - y + # - transform_pub.transform.translation.y + # - transform_pub.transform.translation.z # TransformStamped (topic) rotation — shape (N, 4) - ts_rotation: - - transform_pub.transform.rotation.x - - transform_pub.transform.rotation.y - - transform_pub.transform.rotation.z - - transform_pub.transform.rotation.w + # ts_rotation: + # - transform_pub.transform.rotation.x + # - transform_pub.transform.rotation.y + # - transform_pub.transform.rotation.z + # - transform_pub.transform.rotation.w # Force reading — shape (N, 1) - force_reading: - - force_z.data + # force_reading: + # - force_z.data # Gripper state — shape (N, 1) - gripper_pos: - - gripper_state.data + # gripper_pos: + # - gripper_state.data # Combined observation vector — shape (N, 7+3+1 = 11) # Mix any fields from any inputs into a single flat vector. - observation: - - joint_1.position[:] - - tf_eef.translation.x - - tf_eef.translation.y - - tf_eef.translation.z - - force_z.data + # observation: + # - joint_1.position[:] + # - tf_eef.translation.x + # - tf_eef.translation.y + # - tf_eef.translation.z + # - force_z.data # Full robot state — shape (N, 7+7+3+4 = 21) full_state: @@ -384,84 +381,6 @@ config: - tf_eef.rotation.z - tf_eef.rotation.w - # ----------------------------------------------------------------------- - # Normalized stream examples (dict form) - # Replace ??? with your actual observed min/max values before use. - # IIWA 14 R820 limits are pre-filled; workspace/sensor limits are left - # as ??? for you to fill in. - # ----------------------------------------------------------------------- - - # Joint positions — 7 DOF IIWA 14 (rad) - # robot_joint_pos: - # specs: - # - joint_1.position[:] - # normalize: true - # normalized_range: [-1.0, 1.0] - # limits: - # - {lower_lim: -2.9671, upper_lim: 2.9671} # J1 ±170° - # - {lower_lim: -2.0944, upper_lim: 2.0944} # J2 ±120° - # - {lower_lim: -2.9671, upper_lim: 2.9671} # J3 ±170° - # - {lower_lim: -2.0944, upper_lim: 2.0944} # J4 ±120° - # - {lower_lim: -2.9671, upper_lim: 2.9671} # J5 ±170° - # - {lower_lim: -2.0944, upper_lim: 2.0944} # J6 ±120° - # - {lower_lim: -3.0543, upper_lim: 3.0543} # J7 ±175° - - # Joint velocities — 7 DOF IIWA 14 (rad/s) - # robot_joint_vel: - # specs: - # - joint_1.velocity[:] - # normalize: true - # normalized_range: [-1.0, 1.0] - # limits: - # - {lower_lim: -1.4835, upper_lim: 1.4835} # J1 ±85°/s - # - {lower_lim: -1.4835, upper_lim: 1.4835} # J2 ±85°/s - # - {lower_lim: -1.7453, upper_lim: 1.7453} # J3 ±100°/s - # - {lower_lim: -1.3090, upper_lim: 1.3090} # J4 ±75°/s - # - {lower_lim: -2.2689, upper_lim: 2.2689} # J5 ±130°/s - # - {lower_lim: -2.3562, upper_lim: 2.3562} # J6 ±135°/s - # - {lower_lim: -2.3562, upper_lim: 2.3562} # J7 ±135°/s - - # Joint efforts — 7 DOF IIWA 14 (Nm) - # robot_joint_eff: - # specs: - # - joint_1.effort[:] - # normalize: true - # normalized_range: [-1.0, 1.0] - # limits: - # - {lower_lim: -320, upper_lim: 320} # J1 - # - {lower_lim: -320, upper_lim: 320} # J2 - # - {lower_lim: -176, upper_lim: 176} # J3 - # - {lower_lim: -176, upper_lim: 176} # J4 - # - {lower_lim: -110, upper_lim: 110} # J5 - # - {lower_lim: -40, upper_lim: 40} # J6 - # - {lower_lim: -40, upper_lim: 40} # J7 - - # End-effector position — world-frame (m). Fill ??? from your workspace. - # eef_translation: - # specs: - # - tf_eef.translation.x - # - tf_eef.translation.y - # - tf_eef.translation.z - # normalize: true - # normalized_range: [-1.0, 1.0] - # limits: - # - {lower_lim: ???, upper_lim: ???} # x (m) - # - {lower_lim: ???, upper_lim: ???} # y (m) - # - {lower_lim: ???, upper_lim: ???} # z (m) - - # End-effector rotation — quaternion already in [-1, 1]; normalize is optional. - # One broadcast entry covers all four components. - # eef_rotation: - # specs: - # - tf_eef.rotation.x - # - tf_eef.rotation.y - # - tf_eef.rotation.z - # - tf_eef.rotation.w - # normalize: true - # normalized_range: [-1.0, 1.0] - # limits: - # - {lower_lim: -1.0, upper_lim: 1.0} # broadcast to all 4 components - # ----------------------------------------------------------------------- # Per-use rotation representation: obs stays quaternion, action uses RPY. # Both streams read the SAME source fields (tf_eef.rotation.x/y/z/w) — @@ -486,116 +405,6 @@ config: # - tf_eef.rotation.z # - tf_eef.rotation.w # rotation_format: rpy - # normalize: true - # normalized_range: [-1.0, 1.0] - # limits: - # - {lower_lim: -3.14159265, upper_lim: 3.14159265} # roll (+/- pi) - # - {lower_lim: -1.57079633, upper_lim: 1.57079633} # pitch (+/- pi/2) - # - {lower_lim: -3.14159265, upper_lim: 3.14159265} # yaw (+/- pi) - - # Object translation — world-frame (m). Fill ??? from your workspace. - # object_translation: - # specs: - # - tf_object.translation.x - # - tf_object.translation.y - # - tf_object.translation.z - # normalize: true - # normalized_range: [-1.0, 1.0] - # limits: - # - {lower_lim: ???, upper_lim: ???} # x (m) - # - {lower_lim: ???, upper_lim: ???} # y (m) - # - {lower_lim: ???, upper_lim: ???} # z (m) - - # Object rotation — quaternion, same reasoning as eef_rotation. - # object_rotation: - # specs: - # - tf_object.rotation.x - # - tf_object.rotation.y - # - tf_object.rotation.z - # - tf_object.rotation.w - # normalize: true - # normalized_range: [-1.0, 1.0] - # limits: - # - {lower_lim: -1.0, upper_lim: 1.0} # broadcast - - # Gripper position — fill ??? with your driver's output range. - # Common ranges: 0–255 (raw Robotiq byte), 0.0–1.0 (normalised), 0.0–0.085 (metres). - # gripper_pos: - # specs: - # - gripper_state.data - # - gripper_state.data # duplicate if your obs key expects shape (N, 2) - # normalize: true - # normalized_range: [-1.0, 1.0] - # limits: - # - {lower_lim: ???, upper_lim: ???} # broadcast to both columns - - # Force / torque reading (single axis). Fill ??? with your sensor's rated range. - # force_reading: - # specs: - # - force_z.data - # normalize: true - # normalized_range: [-1.0, 1.0] - # limits: - # - {lower_lim: ???, upper_lim: ???} # e.g. -100.0 / 100.0 (N) - - # Mixed object stream (position + quaternion + relative position, 10D). - # Requires one limits entry per column because scales differ. - # Prefer splitting into separate streams; use this only when a single key is needed. - # object: - # specs: - # - tf_object.translation.x - # - tf_object.translation.y - # - tf_object.translation.z - # - tf_object.rotation.x - # - tf_object.rotation.y - # - tf_object.rotation.z - # - tf_object.rotation.w - # - tf_tool0_object.translation.x - # - tf_tool0_object.translation.y - # - tf_tool0_object.translation.z - # normalize: true - # normalized_range: [-1.0, 1.0] - # limits: - # - {lower_lim: ???, upper_lim: ???} # world x (m) - # - {lower_lim: ???, upper_lim: ???} # world y (m) - # - {lower_lim: ???, upper_lim: ???} # world z (m) - # - {lower_lim: -1.0, upper_lim: 1.0} # quat x - # - {lower_lim: -1.0, upper_lim: 1.0} # quat y - # - {lower_lim: -1.0, upper_lim: 1.0} # quat z - # - {lower_lim: -1.0, upper_lim: 1.0} # quat w - # - {lower_lim: ???, upper_lim: ???} # relative x (m) - # - {lower_lim: ???, upper_lim: ???} # relative y (m) - # - {lower_lim: ???, upper_lim: ???} # relative z (m) - - # Same 10D object stream, but computing the relative (gripper-to-object) - # position inline with arithmetic instead of a separate tf_tool0_object - # frame. cube_pos - eef_pos is the standard Robosuite/Robomimic - # object-observation convention. - # object: - # specs: - # - tf_object.translation.x - # - tf_object.translation.y - # - tf_object.translation.z - # - tf_object.rotation.x - # - tf_object.rotation.y - # - tf_object.rotation.z - # - tf_object.rotation.w - # - tf_object.translation.x - tf_eef.translation.x # gripper_to_cube x - # - tf_object.translation.y - tf_eef.translation.y # gripper_to_cube y - # - tf_object.translation.z - tf_eef.translation.z # gripper_to_cube z - # normalize: true - # normalized_range: [-1.0, 1.0] - # limits: - # - {lower_lim: ???, upper_lim: ???} # world x (m) - # - {lower_lim: ???, upper_lim: ???} # world y (m) - # - {lower_lim: ???, upper_lim: ???} # world z (m) - # - {lower_lim: -1.0, upper_lim: 1.0} # quat x - # - {lower_lim: -1.0, upper_lim: 1.0} # quat y - # - {lower_lim: -1.0, upper_lim: 1.0} # quat z - # - {lower_lim: -1.0, upper_lim: 1.0} # quat w - # - {lower_lim: ???, upper_lim: ???} # relative x (m) - # - {lower_lim: ???, upper_lim: ???} # relative y (m) - # - {lower_lim: ???, upper_lim: ???} # relative z (m) # --------------------------------------------------------------------------- # actions_to_extract @@ -604,11 +413,8 @@ config: # in the Robomimic output file. # # The streams are concatenated column-wise in the order listed. - # n_actions above must equal the total number of columns. # The action of step t is the target of the next step: the stream's value at - # t+1, or (in delta_actions) the change from t to t+1. A stream in - # delta_actions must not set `normalize: true` (the dispatcher could not turn - # a normalized delta back into real units): use a second, raw stream for it. + # t+1, or (in delta_actions) the change from t to t+1. # --------------------------------------------------------------------------- actions_to_extract: @@ -621,21 +427,19 @@ config: # actions_to_extract: # - robot_joint_pos # (N, 7) # - eef_translation # (N, 3) - # # → actions shape: (N, 10), so set n_actions: 10 + # # → actions shape: (N, 10) # To use RPY rotation for the action while the observation stays quaternion # (see the eef_rotation_obs / eef_rotation_action example under output.streams): # actions_to_extract: # - eef_translation # (N, 3) # - eef_rotation_action # (N, 3) — RPY radians, converted from quaternion - # # → actions shape: (N, 6), so set n_actions: 6 + # # → actions shape: (N, 6) # # eef_rotation_obs stays quaternion (N, 4) and is picked up automatically as # # an obs-only stream since it isn't listed here. # --------------------------------------------------------------------------- - # Format-specific settings (robomimic env metadata, training_model, split fractions, - # LeRobot task, ...) do NOT belong here. Put them in `postprocess_config.yaml` + # Format-specific settings (split fractions, LeRobot task, ...) do NOT belong here. Put them in `postprocess_config.yaml` # (see relay_d/acquisition/qt_app/ui/config/templates/postprocess_config_template.yaml); the Post-process page picks it up - # from the same folder as this file. Keys left in this file still work but are - # deprecated and log a warning at conversion time. + # from the same folder as this file. Such keys left in this file fail the conversion. # --------------------------------------------------------------------------- diff --git a/relay_d/acquisition/qt_app/ui/config/templates/postprocess_config_template.yaml b/relay_d/acquisition/qt_app/ui/config/templates/postprocess_config_template.yaml index c46b3ed..a3856e5 100644 --- a/relay_d/acquisition/qt_app/ui/config/templates/postprocess_config_template.yaml +++ b/relay_d/acquisition/qt_app/ui/config/templates/postprocess_config_template.yaml @@ -3,10 +3,11 @@ # ============================================================================= # Optional. Keep this file next to your acquisition config as # `postprocess_config.yaml` (it is picked up automatically) or load it with -# "Load Options..." on the Post-process page. Any key left out falls back to the -# same key inside the acquisition config (deprecated) and then to a default. +# "Load Options..." on the Post-process page. Any key left out takes its default. # The acquisition config itself only says what to record and how streams/actions # are built; nothing in here changes the recording. +# Removed keys (robomimic.metadata, robomimic.training_model, splits.test_frac) +# are refused: the robomimic policy is chosen on the Train tab. # ============================================================================= postprocess: @@ -16,89 +17,10 @@ postprocess: # --------------------------------------------------------------------------- robomimic: - # Training-config template written next to the dataset: - # bc_rnn_gmm (default), bc_transformer_gmm or diffusion_policy - training_model: bc_rnn_gmm - - # Fractions for the dataset's `mask/train|valid|test` groups + # Fractions for the dataset's `mask/train|valid` groups (any rest also trains) splits: train_frac: 0.8 val_frac: 0.2 - test_frac: 0.0 - - # Embedded into the output as env_args. - # Required for Robomimic compatibility: env_name, env_version, type (1 = Robosuite). - # env_kwargs mirrors the Robosuite environment constructor arguments. - - metadata: - env_name: Lift # Robosuite environment name - env_version: 1.5.1 - type: 1 # 1 = Robosuite - - env_kwargs: - robots: - - IIWA # robot model name in Robosuite - - control_freq: 20 # controller frequency (Hz) - - # Rendering — set both to false for headless recording - has_renderer: false - has_offscreen_renderer: false - - # Observations - use_camera_obs: false # set true if recording camera obs for policy - use_object_obs: true # set true to include object state in obs - - # Episode behaviour - ignore_done: true - reward_shaping: false - lite_physics: false - - # Camera resolution (used if use_camera_obs: true) - camera_heights: 84 - camera_widths: 84 - camera_depths: false - - # Controller configuration (example: operational space control) - controller_configs: - type: BASIC - body_parts: - right: - type: OSC_POSE - input_min: -1 - input_max: 1 - output_min: - - -0.05 - - -0.05 - - -0.05 - - -0.5 - - -0.5 - - -0.5 - output_max: - - 0.05 - - 0.05 - - 0.05 - - 0.5 - - 0.5 - - 0.5 - kp: 150 - kp_limits: - - 0 - - 300 - damping: 1 - damping_limits: - - 0 - - 10 - impedance_mode: fixed - uncouple_pos_ori: true - control_delta: true - ramp_ratio: 0.2 - input_ref_frame: world - interpolation: null - orientation_limits: null - position_limits: null - gripper: - type: GRIP # --------------------------------------------------------------------------- diff --git a/relay_d/acquisition/qt_app/ui/config/yaml_parser.py b/relay_d/acquisition/qt_app/ui/config/yaml_parser.py index 257bab1..e278c32 100644 --- a/relay_d/acquisition/qt_app/ui/config/yaml_parser.py +++ b/relay_d/acquisition/qt_app/ui/config/yaml_parser.py @@ -24,18 +24,8 @@ def __init__(self): logger.info("Create YamlParser object.") self.yaml_data = None - self.yaml_config_data = [] - self.yaml_input_data = [] - self.yaml_input_data_raw = {} - self.yaml_output_data = [] - self.yaml_streams_data = {} - self.yaml_actions_to_extract = [] self.current_config = {} - self.prefix_path = "" - self.dataset_name = "demo" - self.metadata = {} - self.sync_tolerance_sec = None - self.sync_mode = "soft" + self.clear() def load_yaml_file(self, file_path): try: @@ -61,22 +51,7 @@ def _retrieve_all_parameters(self): config = self.yaml_data["config"] self.dataset_name = config.get("dataset_name", "demo") - if "data" in config: - self.yaml_config_data = [ - {"name": name, "count": count} for name, count in config["data"].items() - ] - if "input" in config: - global_transfer_rate = -1 - if "recording_frequency" in config: - global_transfer_rate = config["recording_frequency"] - elif "metadata" in config and "transfer_rate" in config["metadata"]: - global_transfer_rate = config["metadata"]["transfer_rate"] - elif "metadata" in config and "recording_frequency" in config["metadata"]: - global_transfer_rate = config["metadata"]["recording_frequency"] - else: - global_transfer_rate = 30 - # Also add a raw for future procesing self.yaml_input_data_raw = config["input"] @@ -101,42 +76,17 @@ def _retrieve_all_parameters(self): "tf_wait_timeout", None ) input_data["topic"] = None - input_data["output_map"] = input_config.get("output_map", name) - input_data["transfer_rate"] = input_config.get( - "transfer_rate", global_transfer_rate - ) input_data["fill_gaps"] = input_config.get("fill_gaps", False) - input_data["fill_default"] = input_config.get("fill_default", None) elif "topic" in input_config: input_data["topic"] = input_config["topic"] input_data["is_tf"] = False - input_data["output_map"] = input_config.get("output_map", name) - input_data["transfer_rate"] = input_config.get( - "transfer_rate", global_transfer_rate - ) input_data["joint_names"] = input_config.get("joint_names", []) input_data["fill_gaps"] = input_config.get("fill_gaps", False) - input_data["fill_default"] = input_config.get("fill_default", None) else: continue self.yaml_input_data.append(input_data) - if "output" in config: - for output_name, output_fields in config["output"].items(): - if output_name == "prefix_path": - self.prefix_path = output_fields - continue - if isinstance(output_fields, dict): - for field_name, field_path in output_fields.items(): - self.yaml_output_data.append( - { - "name": output_name, - "field": field_name, - "source": field_path, - } - ) - if "metadata" in config: self.metadata = config["metadata"] @@ -146,17 +96,9 @@ def _retrieve_all_parameters(self): else: self.yaml_streams_data = {} - self.yaml_actions_to_extract = config.get("actions_to_extract", []) - self.sync_tolerance_sec = config.get("sync_tolerance_sec", None) - self.sync_mode = config.get("sync_mode", "soft") - def _print_parsed_data(self): """Print all parsed data for debugging. Lines with nothing meaningful to show are skipped rather than printed blank.""" - logger.info("\n*** CONFIG DATA ***") - for item in self.yaml_config_data: - logger.info(f" {item['name']}: {item['count']}") - logger.info("\n*** INPUT DATA ***") for item in self.yaml_input_data: if item.get("is_tf"): @@ -168,13 +110,10 @@ def _print_parsed_data(self): logger.info(f" {item['name']}: {item['topic']}") logger.info("\n*** OUTPUT DATA ***") - if self.prefix_path: - logger.info(f" Prefix path: {self.prefix_path}") - for item in self.yaml_output_data: - if not item.get("source"): - continue - parent_info = f" (parent: {item['parent']})" if "parent" in item else "" - logger.info(f" {item['name']}.{item['field']}: {item['source']}{parent_info}") + for key, path in self.get_output_bridge_map().items(): + logger.info(f" {key}: {path}") + for name in self.yaml_streams_data: + logger.info(f" stream {name}") # ------------------------------------------------------------------ @@ -324,35 +263,21 @@ def is_loaded(self): def clear(self): """Reset the parsed lists. ``yaml_data`` and ``current_config`` are kept: this also runs at the start of every load. To unload a config, reset those two as well.""" - self.yaml_config_data = [] self.yaml_input_data = [] self.yaml_input_data_raw = {} - self.yaml_output_data = [] self.yaml_streams_data = {} - self.yaml_actions_to_extract = [] - self.prefix_path = "" self.dataset_name = "demo" self.metadata = {} - self.sync_tolerance_sec = None - self.sync_mode = "soft" def get_current_config_path(self): return self.current_config - def get_raw_yaml(self): - if self.yaml_data: - return yaml.dump(self.yaml_data, default_flow_style=False) - return "" - def get_input_data(self): return self.yaml_input_data def get_input_data_raw(self): return self.yaml_input_data_raw - def get_output_data(self): - return self.yaml_output_data - def get_dataset_name(self): return self.dataset_name @@ -365,14 +290,8 @@ def get_streams(self) -> dict: def get_output_bridge_map(self) -> dict: """Flat {logical_key: recorded_path} bridge-map entries from `config.output` — top-level keys other than `streams`/`prefix_path`, - e.g. `agentview_image: camera_rgb/images`. - - NOTE: this is distinct from get_output_data()/yaml_output_data, which - parses an older, incompatible two-level nested convention - (`output: : {: }`) for UI display only and is - never consulted by the actual conversion engine - (yaml_driven_converter.DataTransformEngine._build_bridge_map, which - this method mirrors exactly) or by dispatch's get_obs_config() below. + e.g. `agentview_image: camera_rgb/images`. Mirrors the converter's + (yaml_driven_converter.DataTransformEngine._build_bridge_map). """ if not self.yaml_data: return {} @@ -383,25 +302,11 @@ def get_output_bridge_map(self) -> dict: if key not in ("streams", "prefix_path") and isinstance(value, str) } - def get_train_model_name(self, options=None): - """Robomimic training-config model, default bc_rnn_gmm. - - Read from the postprocess options (`robomimic.training_model`) when given, else - from the legacy `config.training_model` key. Matches the - `robomimic__config.json` files shipped in postprocess_configs/. - """ - from ..postprocess.postprocess_options import PostprocessOptions - - config = (self.yaml_data or {}).get("config") or {} - return (options or PostprocessOptions()).training_model(config) - - # This is for the action dispatcher def get_obs_config(self): # Bridge-map entries (camera/organized-pointcloud obs, e.g. # `agentview_image: camera_rgb/images`) become passthrough obs in - # obs_config.yaml — ObsBuilder only needs the source input name, not - # the recorded dataset name (it decodes live from the topic, not - # from an HDF5 path), so keep only the part before the first '/'. + # obs_config.yaml. The recorded group is always the input name, so the + # part before the first '/' is the input ObsBuilder decodes live. bridge_obs = { key: {"source": path.split("/")[0]} for key, path in self.get_output_bridge_map().items() diff --git a/relay_d/acquisition/qt_app/ui/containers/mainWindow.ui b/relay_d/acquisition/qt_app/ui/containers/mainWindow.ui index b1f7a3f..155234e 100644 --- a/relay_d/acquisition/qt_app/ui/containers/mainWindow.ui +++ b/relay_d/acquisition/qt_app/ui/containers/mainWindow.ui @@ -373,6 +373,45 @@ + + + + + 0 + 50 + + + + + 16777215 + 50 + + + + + 14 + 75 + true + + + + Run a Policy on the Robot + + + Run + + + + :/icons/resources/icons/play.png:/icons/resources/icons/play.png + + + + 24 + 24 + + + + @@ -474,39 +513,6 @@ - - - - - 0 - 50 - - - - - 14 - 75 - true - - - - Pause Recording - - - Pause Record - - - - :/icons/resources/icons/video-pause-button.png:/icons/resources/icons/video-pause-button.png - - - - 24 - 24 - - - - @@ -669,7 +675,6 @@ btn_import recordingLabel btn_start_record - btn_pause_record btn_stop_record widget_3 @@ -715,6 +720,7 @@ + @@ -999,216 +1005,6 @@ - - - - - 0 - 0 - - - - - 12 - - - 0 - - - 0 - - - 0 - - - 0 - - - - - - 4 - - - 0 - - - 0 - - - 0 - - - 0 - - - - - Input H5 Structure - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter - - - - - - - - 0 - 200 - - - - - - - - - - - - 4 - - - 0 - - - 0 - - - 0 - - - 0 - - - - - Output Preview - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter - - - - - - - - 0 - 200 - - - - - - - - - - - - - - - 0 - - - 4 - - - 0 - - - 0 - - - - - Qt::Horizontal - - - - 40 - 20 - - - - - - - - - 130 - 38 - - - - - 75 - true - - - - Clear - - - - - - - - 130 - 38 - - - - - 75 - true - - - - Load H5 File - - - - - - - - 160 - 38 - - - - - 75 - true - - - - Convert Dataset - - - - - - - Qt::Horizontal - - - - 40 - 20 - - - - - - - @@ -1518,7 +1314,7 @@ p, li { white-space: pre-wrap; } <ol style="margin-top: 0px; margin-bottom: 0px; margin-left: 0px; margin-right: 0px; -qt-list-indent: 1;"><li style=" font-family:'Segoe UI','sans-serif'; font-size:14px;" style=" margin-top:12px; margin-bottom:0px; margin-left:20px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-size:14px;">Launch the Relay-D application</span></li> <li style=" font-family:'Segoe UI','sans-serif'; font-size:14px;" style=" margin-top:0px; margin-bottom:0px; margin-left:20px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-size:14px;">Import a configuration file via the </span><span style=" font-size:14px; font-weight:600;">Import</span><span style=" font-size:14px;"> page</span></li> <li style=" font-family:'Segoe UI','sans-serif'; font-size:14px;" style=" margin-top:0px; margin-bottom:0px; margin-left:20px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-size:14px;">Go to </span><span style=" font-size:14px; font-weight:600;">Start Recording</span><span style=" font-size:14px;"> to open the sensor/camera view</span></li> -<li style=" font-family:'Segoe UI','sans-serif'; font-size:14px;" style=" margin-top:0px; margin-bottom:0px; margin-left:20px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-size:14px;">Use </span><span style=" font-size:14px; font-weight:600;">Recording Controls</span><span style=" font-size:14px;"> in the sidebar to start, pause, and stop</span></li> +<li style=" font-family:'Segoe UI','sans-serif'; font-size:14px;" style=" margin-top:0px; margin-bottom:0px; margin-left:20px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-size:14px;">Use </span><span style=" font-size:14px; font-weight:600;">Recording Controls</span><span style=" font-size:14px;"> in the sidebar to start and stop</span></li> <li style=" font-family:'Segoe UI','sans-serif'; font-size:14px;" style=" margin-top:0px; margin-bottom:12px; margin-left:20px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-size:14px;">Use </span><span style=" font-size:14px; font-weight:600;">Data Post Process</span><span style=" font-size:14px;"> to convert collected data </span></li></ol> <h2 style=" margin-top:20px; margin-bottom:8px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-family:'Segoe UI','sans-serif'; font-size:x-large; font-weight:600;">System Requirements</span></h2> <p style=" margin-top:12px; margin-bottom:12px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px; line-height:160%;"><span style=" font-family:'Segoe UI','sans-serif'; font-size:14px;">Qt 5.12+ • 4 GB RAM min (8 GB recommended) • 1 GB disk space • Compatible recording hardware </span></p> diff --git a/relay_d/acquisition/qt_app/ui/containers/mainWindow_ui.py b/relay_d/acquisition/qt_app/ui/containers/mainWindow_ui.py index d18e994..4715bb6 100644 --- a/relay_d/acquisition/qt_app/ui/containers/mainWindow_ui.py +++ b/relay_d/acquisition/qt_app/ui/containers/mainWindow_ui.py @@ -1,6 +1,6 @@ # -*- coding: utf-8 -*- -# Form implementation generated from reading ui file '/home/pascd/Documents/git-repos/relay-d/relay_d/acquisition/qt_app/ui/containers/mainWindow.ui' +# Form implementation generated from reading ui file 'mainWindow.ui' # # Created by: PyQt5 UI code generator 5.15.11 # @@ -141,6 +141,18 @@ def setupUi(self, MainWindow): self.btn_train.setIconSize(QtCore.QSize(24, 24)) self.btn_train.setObjectName("btn_train") self.verticalLayout_3.addWidget(self.btn_train) + self.btn_run = QtWidgets.QPushButton(self.widget_2) + self.btn_run.setMinimumSize(QtCore.QSize(0, 50)) + self.btn_run.setMaximumSize(QtCore.QSize(16777215, 50)) + font = QtGui.QFont() + font.setPointSize(14) + font.setBold(True) + font.setWeight(75) + self.btn_run.setFont(font) + self.btn_run.setIcon(icon3) + self.btn_run.setIconSize(QtCore.QSize(24, 24)) + self.btn_run.setObjectName("btn_run") + self.verticalLayout_3.addWidget(self.btn_run) self.verticalLayout_2.addWidget(self.widget_2) self.btn_import = QtWidgets.QPushButton(self.leftMenuSubContainer) self.btn_import.setMinimumSize(QtCore.QSize(0, 50)) @@ -173,19 +185,6 @@ def setupUi(self, MainWindow): self.btn_start_record.setIconSize(QtCore.QSize(24, 24)) self.btn_start_record.setObjectName("btn_start_record") self.verticalLayout_2.addWidget(self.btn_start_record) - self.btn_pause_record = QtWidgets.QPushButton(self.leftMenuSubContainer) - self.btn_pause_record.setMinimumSize(QtCore.QSize(0, 50)) - font = QtGui.QFont() - font.setPointSize(14) - font.setBold(True) - font.setWeight(75) - self.btn_pause_record.setFont(font) - icon5 = QtGui.QIcon() - icon5.addPixmap(QtGui.QPixmap(":/icons/resources/icons/video-pause-button.png"), QtGui.QIcon.Normal, QtGui.QIcon.Off) - self.btn_pause_record.setIcon(icon5) - self.btn_pause_record.setIconSize(QtCore.QSize(24, 24)) - self.btn_pause_record.setObjectName("btn_pause_record") - self.verticalLayout_2.addWidget(self.btn_pause_record) self.btn_stop_record = QtWidgets.QPushButton(self.leftMenuSubContainer) self.btn_stop_record.setMinimumSize(QtCore.QSize(0, 50)) font = QtGui.QFont() @@ -193,9 +192,9 @@ def setupUi(self, MainWindow): font.setBold(True) font.setWeight(75) self.btn_stop_record.setFont(font) - icon6 = QtGui.QIcon() - icon6.addPixmap(QtGui.QPixmap(":/icons/resources/icons/stop-button.png"), QtGui.QIcon.Normal, QtGui.QIcon.Off) - self.btn_stop_record.setIcon(icon6) + icon5 = QtGui.QIcon() + icon5.addPixmap(QtGui.QPixmap(":/icons/resources/icons/stop-button.png"), QtGui.QIcon.Normal, QtGui.QIcon.Off) + self.btn_stop_record.setIcon(icon5) self.btn_stop_record.setIconSize(QtCore.QSize(24, 24)) self.btn_stop_record.setObjectName("btn_stop_record") self.verticalLayout_2.addWidget(self.btn_stop_record) @@ -218,9 +217,9 @@ def setupUi(self, MainWindow): font.setBold(True) font.setWeight(75) self.btn_settings.setFont(font) - icon7 = QtGui.QIcon() - icon7.addPixmap(QtGui.QPixmap(":/icons/resources/icons/setting.png"), QtGui.QIcon.Normal, QtGui.QIcon.Off) - self.btn_settings.setIcon(icon7) + icon6 = QtGui.QIcon() + icon6.addPixmap(QtGui.QPixmap(":/icons/resources/icons/setting.png"), QtGui.QIcon.Normal, QtGui.QIcon.Off) + self.btn_settings.setIcon(icon6) self.btn_settings.setIconSize(QtCore.QSize(24, 24)) self.btn_settings.setObjectName("btn_settings") self.verticalLayout_4.addWidget(self.btn_settings) @@ -232,9 +231,9 @@ def setupUi(self, MainWindow): font.setBold(True) font.setWeight(75) self.btn_help.setFont(font) - icon8 = QtGui.QIcon() - icon8.addPixmap(QtGui.QPixmap(":/icons/resources/icons/question.png"), QtGui.QIcon.Normal, QtGui.QIcon.Off) - self.btn_help.setIcon(icon8) + icon7 = QtGui.QIcon() + icon7.addPixmap(QtGui.QPixmap(":/icons/resources/icons/question.png"), QtGui.QIcon.Normal, QtGui.QIcon.Off) + self.btn_help.setIcon(icon7) self.btn_help.setIconSize(QtCore.QSize(24, 24)) self.btn_help.setObjectName("btn_help") self.verticalLayout_4.addWidget(self.btn_help) @@ -245,7 +244,6 @@ def setupUi(self, MainWindow): self.btn_import.raise_() self.recordingLabel.raise_() self.btn_start_record.raise_() - self.btn_pause_record.raise_() self.btn_stop_record.raise_() self.widget_3.raise_() self.verticalLayout.addWidget(self.leftMenuSubContainer) @@ -273,6 +271,9 @@ def setupUi(self, MainWindow): self.page_train = QtWidgets.QWidget() self.page_train.setObjectName("page_train") self.stackedWidget.addWidget(self.page_train) + self.page_run = QtWidgets.QWidget() + self.page_run.setObjectName("page_run") + self.stackedWidget.addWidget(self.page_run) self.page_data = QtWidgets.QWidget() self.page_data.setObjectName("page_data") self.verticalLayout_9 = QtWidgets.QVBoxLayout(self.page_data) @@ -378,82 +379,6 @@ def setupUi(self, MainWindow): spacerItem3 = QtWidgets.QSpacerItem(40, 20, QtWidgets.QSizePolicy.Expanding, QtWidgets.QSizePolicy.Minimum) self.horizontalLayout_6.addItem(spacerItem3) self.verticalLayout_9.addWidget(self.formatSelectionWidget) - self.dataViewWidget = QtWidgets.QWidget(self.page_data) - sizePolicy = QtWidgets.QSizePolicy(QtWidgets.QSizePolicy.Expanding, QtWidgets.QSizePolicy.Expanding) - sizePolicy.setHorizontalStretch(0) - sizePolicy.setVerticalStretch(0) - sizePolicy.setHeightForWidth(self.dataViewWidget.sizePolicy().hasHeightForWidth()) - self.dataViewWidget.setSizePolicy(sizePolicy) - self.dataViewWidget.setObjectName("dataViewWidget") - self.horizontalLayout_7 = QtWidgets.QHBoxLayout(self.dataViewWidget) - self.horizontalLayout_7.setContentsMargins(0, 0, 0, 0) - self.horizontalLayout_7.setSpacing(12) - self.horizontalLayout_7.setObjectName("horizontalLayout_7") - self.inputStructureWidget = QtWidgets.QWidget(self.dataViewWidget) - self.inputStructureWidget.setObjectName("inputStructureWidget") - self.verticalLayout_10 = QtWidgets.QVBoxLayout(self.inputStructureWidget) - self.verticalLayout_10.setContentsMargins(0, 0, 0, 0) - self.verticalLayout_10.setSpacing(4) - self.verticalLayout_10.setObjectName("verticalLayout_10") - self.inputStructureLabel = QtWidgets.QLabel(self.inputStructureWidget) - self.inputStructureLabel.setAlignment(QtCore.Qt.AlignLeading|QtCore.Qt.AlignLeft|QtCore.Qt.AlignVCenter) - self.inputStructureLabel.setObjectName("inputStructureLabel") - self.verticalLayout_10.addWidget(self.inputStructureLabel) - self.txt_input_structure = QtWidgets.QTextBrowser(self.inputStructureWidget) - self.txt_input_structure.setMinimumSize(QtCore.QSize(0, 200)) - self.txt_input_structure.setObjectName("txt_input_structure") - self.verticalLayout_10.addWidget(self.txt_input_structure) - self.horizontalLayout_7.addWidget(self.inputStructureWidget) - self.outputPreviewWidget = QtWidgets.QWidget(self.dataViewWidget) - self.outputPreviewWidget.setObjectName("outputPreviewWidget") - self.verticalLayout_11 = QtWidgets.QVBoxLayout(self.outputPreviewWidget) - self.verticalLayout_11.setContentsMargins(0, 0, 0, 0) - self.verticalLayout_11.setSpacing(4) - self.verticalLayout_11.setObjectName("verticalLayout_11") - self.outputPreviewLabel = QtWidgets.QLabel(self.outputPreviewWidget) - self.outputPreviewLabel.setAlignment(QtCore.Qt.AlignLeading|QtCore.Qt.AlignLeft|QtCore.Qt.AlignVCenter) - self.outputPreviewLabel.setObjectName("outputPreviewLabel") - self.verticalLayout_11.addWidget(self.outputPreviewLabel) - self.txt_output_preview = QtWidgets.QTextBrowser(self.outputPreviewWidget) - self.txt_output_preview.setMinimumSize(QtCore.QSize(0, 200)) - self.txt_output_preview.setObjectName("txt_output_preview") - self.verticalLayout_11.addWidget(self.txt_output_preview) - self.horizontalLayout_7.addWidget(self.outputPreviewWidget) - self.verticalLayout_9.addWidget(self.dataViewWidget) - self.conversionControlsWidget = QtWidgets.QWidget(self.page_data) - self.conversionControlsWidget.setObjectName("conversionControlsWidget") - self.horizontalLayout_8 = QtWidgets.QHBoxLayout(self.conversionControlsWidget) - self.horizontalLayout_8.setContentsMargins(0, 4, 0, 0) - self.horizontalLayout_8.setObjectName("horizontalLayout_8") - spacerItem4 = QtWidgets.QSpacerItem(40, 20, QtWidgets.QSizePolicy.Expanding, QtWidgets.QSizePolicy.Minimum) - self.horizontalLayout_8.addItem(spacerItem4) - self.btn_clear_preview = QtWidgets.QPushButton(self.conversionControlsWidget) - self.btn_clear_preview.setMinimumSize(QtCore.QSize(130, 38)) - font = QtGui.QFont() - font.setBold(True) - font.setWeight(75) - self.btn_clear_preview.setFont(font) - self.btn_clear_preview.setObjectName("btn_clear_preview") - self.horizontalLayout_8.addWidget(self.btn_clear_preview) - self.btn_load_h5 = QtWidgets.QPushButton(self.conversionControlsWidget) - self.btn_load_h5.setMinimumSize(QtCore.QSize(130, 38)) - font = QtGui.QFont() - font.setBold(True) - font.setWeight(75) - self.btn_load_h5.setFont(font) - self.btn_load_h5.setObjectName("btn_load_h5") - self.horizontalLayout_8.addWidget(self.btn_load_h5) - self.btn_convert_dataset = QtWidgets.QPushButton(self.conversionControlsWidget) - self.btn_convert_dataset.setMinimumSize(QtCore.QSize(160, 38)) - font = QtGui.QFont() - font.setBold(True) - font.setWeight(75) - self.btn_convert_dataset.setFont(font) - self.btn_convert_dataset.setObjectName("btn_convert_dataset") - self.horizontalLayout_8.addWidget(self.btn_convert_dataset) - spacerItem5 = QtWidgets.QSpacerItem(40, 20, QtWidgets.QSizePolicy.Expanding, QtWidgets.QSizePolicy.Minimum) - self.horizontalLayout_8.addItem(spacerItem5) - self.verticalLayout_9.addWidget(self.conversionControlsWidget) self.stackedWidget.addWidget(self.page_data) self.page_import = QtWidgets.QWidget() self.page_import.setObjectName("page_import") @@ -471,8 +396,8 @@ def setupUi(self, MainWindow): self.importPageTitle.setAlignment(QtCore.Qt.AlignLeading|QtCore.Qt.AlignLeft|QtCore.Qt.AlignVCenter) self.importPageTitle.setObjectName("importPageTitle") self.importHeaderLayout.addWidget(self.importPageTitle) - spacerItem6 = QtWidgets.QSpacerItem(40, 20, QtWidgets.QSizePolicy.Expanding, QtWidgets.QSizePolicy.Minimum) - self.importHeaderLayout.addItem(spacerItem6) + spacerItem4 = QtWidgets.QSpacerItem(40, 20, QtWidgets.QSizePolicy.Expanding, QtWidgets.QSizePolicy.Minimum) + self.importHeaderLayout.addItem(spacerItem4) self.btn_generate_configs = QtWidgets.QPushButton(self.importHeaderWidget) self.btn_generate_configs.setMinimumSize(QtCore.QSize(140, 36)) self.btn_generate_configs.setObjectName("btn_generate_configs") @@ -591,13 +516,13 @@ def retranslateUi(self, MainWindow): self.btn_review.setText(_translate("MainWindow", " Review")) self.btn_train.setToolTip(_translate("MainWindow", "Train a Policy")) self.btn_train.setText(_translate("MainWindow", " Train")) + self.btn_run.setToolTip(_translate("MainWindow", "Run a Policy on the Robot")) + self.btn_run.setText(_translate("MainWindow", " Run")) self.btn_import.setToolTip(_translate("MainWindow", "Import Configuration Files")) self.btn_import.setText(_translate("MainWindow", " Import")) self.recordingLabel.setText(_translate("MainWindow", "Recording Controls")) self.btn_start_record.setToolTip(_translate("MainWindow", "Start Recording")) self.btn_start_record.setText(_translate("MainWindow", " Start Record")) - self.btn_pause_record.setToolTip(_translate("MainWindow", "Pause Recording")) - self.btn_pause_record.setText(_translate("MainWindow", " Pause Record")) self.btn_stop_record.setToolTip(_translate("MainWindow", "Stop Recording")) self.btn_stop_record.setText(_translate("MainWindow", " Stop Record")) self.btn_restart_subscribers.setText(_translate("MainWindow", "Restart Subscribers")) @@ -618,11 +543,6 @@ def retranslateUi(self, MainWindow): self.formatLabel.setText(_translate("MainWindow", "Output Format:")) self.btn_format_robomimic.setText(_translate("MainWindow", "RoboMimic")) self.btn_format_lerobot.setText(_translate("MainWindow", "LeRobot")) - self.inputStructureLabel.setText(_translate("MainWindow", "Input H5 Structure")) - self.outputPreviewLabel.setText(_translate("MainWindow", "Output Preview")) - self.btn_clear_preview.setText(_translate("MainWindow", "Clear")) - self.btn_load_h5.setText(_translate("MainWindow", "Load H5 File")) - self.btn_convert_dataset.setText(_translate("MainWindow", "Convert Dataset")) self.importPageTitle.setText(_translate("MainWindow", "Select Structure File")) self.btn_generate_configs.setText(_translate("MainWindow", "Generate Defaults...")) self.btn_insert_config.setText(_translate("MainWindow", "Insert")) @@ -657,7 +577,7 @@ def retranslateUi(self, MainWindow): "
  1. Launch the Relay-D application
  2. \n" "
  3. Import a configuration file via the Import page
  4. \n" "
  5. Go to Start Recording to open the sensor/camera view
  6. \n" -"
  7. Use Recording Controls in the sidebar to start, pause, and stop
  8. \n" +"
  9. Use Recording Controls in the sidebar to start and stop
  10. \n" "
  11. Use Data Post Process to convert collected data
\n" "

System Requirements

\n" "

Qt 5.12+ • 4 GB RAM min (8 GB recommended) • 1 GB disk space • Compatible recording hardware

\n" diff --git a/relay_d/acquisition/qt_app/ui/containers/topic_containers.py b/relay_d/acquisition/qt_app/ui/containers/topic_containers.py index f387c18..73f8cc8 100644 --- a/relay_d/acquisition/qt_app/ui/containers/topic_containers.py +++ b/relay_d/acquisition/qt_app/ui/containers/topic_containers.py @@ -62,17 +62,9 @@ def create_container(self, topic_type, container_data=None): # Set basic properties for identification container.topic_type = topic_type - # Use output_map from container_data if available, otherwise use input_name - output_map = None - input_name = None - if container_data: - output_map = container_data.get("output_map") - input_name = container_data.get("input_name") - - # Use output_map as container_id (this is the name used in HDF5 output) - if output_map: - container.container_id = output_map - elif input_name: + # The input name is the container id (and the HDF5 group it is recorded into) + input_name = container_data.get("input_name") if container_data else None + if input_name: container.container_id = input_name else: container.container_id = f"container_{self.container_count}_{topic_type}" diff --git a/relay_d/acquisition/qt_app/ui/postprocess/h5_file_parser.py b/relay_d/acquisition/qt_app/ui/postprocess/h5_file_parser.py deleted file mode 100644 index 2cf708c..0000000 --- a/relay_d/acquisition/qt_app/ui/postprocess/h5_file_parser.py +++ /dev/null @@ -1,55 +0,0 @@ -import html -import h5py - -H5_COLORS = { - "key": "#1A7DC4", - "value": "#6B7280", -} - -class h5FileParser: - def __init__(self): - pass - - def _h5_to_dict(self, group): - """Recursively convert an h5py Group/Dataset to a nested dict.""" - result = {} - for key, item in group.items(): - if isinstance(item, h5py.Group): - result[key] = self._h5_to_dict(item) - elif isinstance(item, h5py.Dataset): - result[key] = f"shape={item.shape} dtype={item.dtype}" - else: - result[key] = str(item) - return result - - def _dict_to_html(self, d, indent=0): - """Convert nested dict to colored HTML.""" - html_str = "" - for k, v in d.items(): - pad = " " * (indent * 4) - if isinstance(v, dict): - html_str += ( - f'{pad}' - f'{html.escape(str(k))}:
' - ) - html_str += self._dict_to_html(v, indent + 1) - else: - html_str += ( - f'{pad}' - f'{html.escape(str(k))}: ' - f'' - f'{html.escape(str(v))}
' - ) - return html_str - - def convert_h5_to_html(self, input_file): - """Convert H5 structure to colored HTML for display.""" - with h5py.File(input_file, "r") as f: - h5_dict = self._h5_to_dict(f) - html_str = self._dict_to_html(h5_dict) - return ( - '
' - f"{html_str}
" - ) diff --git a/relay_d/acquisition/qt_app/ui/postprocess/lerobot_writer.py b/relay_d/acquisition/qt_app/ui/postprocess/lerobot_writer.py index 73ffdcd..940105b 100644 --- a/relay_d/acquisition/qt_app/ui/postprocess/lerobot_writer.py +++ b/relay_d/acquisition/qt_app/ui/postprocess/lerobot_writer.py @@ -73,7 +73,11 @@ class LeRobotWriterError(RuntimeError): pass -class LeRobotDependencyError(LeRobotWriterError): +class LeRobotFatalError(LeRobotWriterError): + """The whole conversion must stop (the dataset was aborted), never a per-episode skip.""" + + +class LeRobotDependencyError(LeRobotFatalError): """A required library is missing in the running Python. Fatal for the whole conversion (every episode would fail the same way), never a per-episode skip.""" @@ -301,6 +305,7 @@ def __init__( # per-camera video state: key -> {"chunk", "file", "video": _VideoFile} self._videos: Dict[str, Dict] = {} self._finalized = False + self._aborted = False # ------------------------------------------------------------------ public @@ -370,9 +375,14 @@ def add_episode( "tasks": [task], "length": n, } - row.update(self._write_data(cols, n, start)) - for key, frames in images.items(): - row.update(self._write_video(key, frames)) + try: + row.update(self._write_data(cols, n, start)) + for key, frames in images.items(): + row.update(self._write_video(key, frames)) + except Exception as e: + # rows (and maybe some videos) of this episode are already on disk: the next one would reuse its indices + self._abort() + raise LeRobotFatalError(f"Writing episode {episode_index} failed, dataset deleted: {e}") from e row["meta/episodes/chunk_index"] = 0 row["meta/episodes/file_index"] = 0 @@ -383,6 +393,8 @@ def add_episode( def finalize(self) -> None: """Close writers and write metadata. Must be called, or the parquet files are corrupt.""" + if self._aborted: + raise LeRobotWriterError("The dataset was aborted and deleted") if self._finalized: return self._finalized = True @@ -658,7 +670,7 @@ def _write_info(self) -> None: def _abort(self) -> None: """Release file handles and delete the half-written dataset (the constructor checked that the folder was empty, so everything in it is ours).""" - self._finalized = True + self._finalized = self._aborted = True try: self._close_data_writer() except Exception: diff --git a/relay_d/acquisition/qt_app/ui/postprocess/offline_config.py b/relay_d/acquisition/qt_app/ui/postprocess/offline_config.py index bdf152b..a11ff7b 100644 --- a/relay_d/acquisition/qt_app/ui/postprocess/offline_config.py +++ b/relay_d/acquisition/qt_app/ui/postprocess/offline_config.py @@ -6,6 +6,7 @@ - the file must parse and contain a top-level ``config`` section - every ``output.streams`` spec must parse (same check the Config page runs) + - no removed key (``metadata``, ``training_model``, ...) is left in it The returned ``YamlParser`` is a separate instance from the one the Record page uses, so loading a config this way can never affect recording. @@ -14,11 +15,16 @@ from __future__ import annotations import os +import shutil from typing import List, Optional, Tuple +import yaml + +from relay_d.dispatch import save_dispatch_templates from relay_d.utils.coloring_logger import logger from ..config.yaml_parser import YamlParser +from .postprocess_options import OPTIONS_FILENAME, PostprocessOptions, removed_key_errors from ...utils.stream_builder import StreamBuilder @@ -40,7 +46,7 @@ def load_offline_config(path: str) -> Tuple[Optional[YamlParser], List[str]]: if not isinstance(data, dict) or not isinstance(data.get("config"), dict): return None, ["The file has no top-level 'config' section."] - errors = [f"output.streams: {e}" for e in StreamBuilder(parser).get_build_errors()] + errors = removed_key_errors(data["config"]) + [f"output.streams: {e}" for e in StreamBuilder(parser).get_build_errors()] if errors: return None, errors @@ -48,12 +54,54 @@ def load_offline_config(path: str) -> Tuple[Optional[YamlParser], List[str]]: output = data["config"].get("output") or {} if not output: problems.append("warning: the config has no 'output' entries, so nothing can be converted.") - if not data["config"].get("recording_frequency") and not ( - data["config"].get("metadata") or {} - ).get("recording_frequency"): + if not data["config"].get("recording_frequency"): problems.append( "warning: no 'recording_frequency' set (required as fps for the LeRobot format)." ) for problem in problems: logger.warning(f"Offline config {path}: {problem}") return parser, problems + + +def load_conversion_config(config_path: str, options_path: Optional[str] = None) -> Tuple[YamlParser, PostprocessOptions]: + """Config + format options of a headless conversion (relay-d-convert, AppAPI): options from ``options_path``, + else a postprocess_config.yaml next to the config, else the defaults. Raises ValueError if a file is unusable.""" + parser, problems = load_offline_config(config_path) + if parser is None: + raise ValueError("unusable config:\n " + "\n ".join(problems)) + options_path = options_path or PostprocessOptions.find_sibling(config_path) + if not options_path: + return parser, PostprocessOptions() + options, problems = PostprocessOptions.load(options_path) + if options is None: + raise ValueError(f"unusable options file {options_path}:\n " + "\n ".join(problems)) + return parser, options + + +def write_dataset_files(output_dir: str, parser: YamlParser, options_source: Optional[str] = None) -> None: + """The files kept next to a converted dataset, the same for the GUI, AppAPI and relay-d-convert: the configs used + (acquisition_config.yaml, postprocess_config.yaml) and action_dispatcher/ for lfd-inference-node. + ``output_dir`` is the folder holding the dataset (for LeRobot the one above the dataset folder: where the + training launcher looks). Raises on failure; the dataset itself is already written then.""" + os.makedirs(output_dir, exist_ok=True) + # A byte copy keeps the comments, but only while the file is still what was converted with + # (the Config page can edit the config in memory); otherwise the data used is dumped. + target = os.path.join(output_dir, "acquisition_config.yaml") + source = parser.get_current_config_path() + if source and os.path.isfile(source) and _file_data(source) == parser.yaml_data: + if os.path.abspath(source) != os.path.abspath(target): + shutil.copyfile(source, target) + elif not parser.save_yaml_file(file_path=target): + raise RuntimeError("could not write acquisition_config.yaml") + if options_source: + shutil.copy2(options_source, os.path.join(output_dir, OPTIONS_FILENAME)) + dispatcher = os.path.join(output_dir, "action_dispatcher") + os.makedirs(dispatcher, exist_ok=True) + if not parser.save_yaml_file(file_path=os.path.join(dispatcher, "obs_config.yaml"), data=parser.get_obs_config()): + raise RuntimeError("could not write action_dispatcher/obs_config.yaml") + save_dispatch_templates(dispatcher) + + +def _file_data(path: str): + with open(path, "r") as f: + return yaml.safe_load(f) diff --git a/relay_d/acquisition/qt_app/ui/postprocess/postprocess_config_loader.py b/relay_d/acquisition/qt_app/ui/postprocess/postprocess_config_loader.py index 120dd2b..062011f 100644 --- a/relay_d/acquisition/qt_app/ui/postprocess/postprocess_config_loader.py +++ b/relay_d/acquisition/qt_app/ui/postprocess/postprocess_config_loader.py @@ -1,215 +1,13 @@ import os -import yaml -from typing import Dict, List, Optional -from relay_d.utils.coloring_logger import logger +from typing import Optional class PostprocessConfigLoader: - """Loads and manages postprocessing format configurations from YAML files""" + """Finds the robomimic training-config templates (``postprocess_configs/robomimic__config.json``).""" def __init__(self, config_dir: Optional[str] = None): - if config_dir is None: - script_dir = os.path.dirname(os.path.abspath(__file__)) - config_dir = os.path.join(script_dir, "postprocess_configs") + self.config_dir = config_dir or os.path.join(os.path.dirname(os.path.abspath(__file__)), "postprocess_configs") - self.config_dir = config_dir - self.configs: Dict[str, Dict] = {} - self.load_all_configs() - - def load_all_configs(self): - """Load all YAML config files from the config directory""" - if not os.path.exists(self.config_dir): - logger.warning(f"Config directory does not exist: {self.config_dir}") - return - - logger.info(f"Loading postprocessing configs from: {self.config_dir}") - - for filename in os.listdir(self.config_dir): - if filename.endswith(".yaml") or filename.endswith(".yml"): - self.load_config(filename) - - def load_config(self, filename: str) -> Optional[Dict]: - """Load a single config file""" - filepath = os.path.join(self.config_dir, filename) - - try: - with open(filepath, "r") as f: - config = yaml.safe_load(f) - - format_name = config.get("format", {}).get( - "name", filename.replace(".yaml", "") - ) - self.configs[format_name] = config - - logger.info(f"Loaded config: {format_name} from {filename}") - return config - - except Exception as e: - logger.error(f"Error loading config {filename}: {e}") - return None - - def generate_html_structure_preview(self, format_name: str) -> str: - """Generate an HTML preview of the output structure""" - structure = self.get_output_structure(format_name) - if not structure: - return "

No structure information available

" - - format_info = self.get_format_info(format_name) - html_parts = [] - - html_parts.append("") - - if format_info is not None: - html_parts.append(f"

{format_info['name']} Format

") - html_parts.append(f"

Version: {format_info['version']}

") - html_parts.append(f"

Description: {format_info['description']}

") - else: - html_parts.append("

Format Preview

") - - html_parts.append(self._render_structure_to_html(structure)) - - html_parts.append("") - - return "\n".join(html_parts) - - def _render_structure_to_html(self, structure: Dict, indent: int = 0) -> str: - """Recursively render structure to HTML""" - html_parts = [] - root_groups = structure.get("root_groups", []) - - for group in root_groups: - html_parts.append(self._render_group(group, indent)) - - return "\n".join(html_parts) - - def _render_group(self, group: Dict, indent: int) -> str: - """Render a single group to HTML""" - indent_str = " " * indent - name = group.get("name", "unnamed") - description = group.get("description", "") - - html_parts = [] - html_parts.append(f"{indent_str}
") - html_parts.append(f"{indent_str}

{name}

") - if description: - html_parts.append(f"{indent_str}

{description}

") - - if "groups" in group: - html_parts.append(f"{indent_str}
") - for subgroup in group["groups"]: - html_parts.append(self._render_group(subgroup, indent + 1)) - html_parts.append(f"{indent_str}
") - - if "datasets" in group: - for dataset in group["datasets"]: - html_parts.append(self._render_dataset(dataset, indent + 1)) - - if "attributes" in group: - for attr in group["attributes"]: - html_parts.append(self._render_attribute(attr, indent + 2)) - - html_parts.append(f"{indent_str}
") - - return "\n".join(html_parts) - - def _render_dataset(self, dataset: Dict, indent: int) -> str: - """Render a dataset to HTML""" - indent_str = " " * indent - name = dataset.get("name", "unnamed") - dtype = dataset.get("dtype", "unknown") - shape = dataset.get("shape", "unknown") - description = dataset.get("description", "") - - html_parts = [] - html_parts.append(f"{indent_str}
") - html_parts.append(f"{indent_str} {name} [{dtype}]") - if description: - html_parts.append(f"{indent_str}

{description}

") - html_parts.append(f"{indent_str}

Shape: {shape}

") - html_parts.append(f"{indent_str}
") - - return "\n".join(html_parts) - - def _render_attribute(self, attr: Dict, indent: int) -> str: - """Render an attribute to HTML""" - indent_str = " " * indent - name = attr.get("name", "unnamed") - dtype = attr.get("type", "unknown") - description = attr.get("description", "") - - html_parts = [] - html_parts.append(f"{indent_str}
") - html_parts.append(f"{indent_str} @{name} [{dtype}]") - if description: - html_parts.append(f"{indent_str}

{description}

") - html_parts.append(f"{indent_str}
") - - return "\n".join(html_parts) - - ### Getters for config data ### - - def get_config(self, format_name: str) -> Optional[Dict]: - """Get a specific config by format name""" - return self.configs.get(format_name) - - def get_all_format_names(self) -> List[str]: - """Get list of all available format names""" - return list(self.configs.keys()) - - def get_format_info(self, format_name: str) -> Optional[Dict[str, str]]: - """Get basic info about a format""" - config = self.get_config(format_name) - if config and "format" in config: - return { - "name": config["format"].get("name", format_name), - "version": config["format"].get("version", "unknown"), - "description": config["format"].get( - "description", "No description available" - ), - } - return None - - def get_output_structure(self, format_name: str) -> Optional[Dict]: - """Get the output structure for a format""" - config = self.get_config(format_name) - return config.get("output_structure") if config else None - - def get_output_train_config(self, format_name: str, model: str="bc_rnn_gmm") -> Optional[Dict]: - """Get the output train config for a format""" - return os.path.join(self.config_dir, f"{format_name}_{model}_config.json") if self.config_dir else None + def get_output_train_config(self, format_name: str, model: str = "bc_rnn_gmm") -> str: + """Path of the training-config template of ``model`` for ``format_name``.""" + return os.path.join(self.config_dir, f"{format_name}_{model}_config.json") diff --git a/relay_d/acquisition/qt_app/ui/postprocess/postprocess_configs/lerobot.yaml b/relay_d/acquisition/qt_app/ui/postprocess/postprocess_configs/lerobot.yaml deleted file mode 100644 index 42ffa16..0000000 --- a/relay_d/acquisition/qt_app/ui/postprocess/postprocess_configs/lerobot.yaml +++ /dev/null @@ -1,54 +0,0 @@ -format: - name: "lerobot" - version: "3.0" - description: "LeRobotDataset v3.0 (parquet + mp4 + metadata) for Hugging Face LeRobot" - -# The structure below is a directory tree (groups = folders, datasets = files). -# It is used for the preview only; the writer is relay_d/.../lerobot_writer.py. -output_structure: - root_groups: - - name: "meta" - description: "Dataset metadata" - datasets: - - name: "info.json" - dtype: "json" - shape: "-" - description: "codebase_version v3.0, fps, features (state/action/cameras), totals, path templates, splits" - - name: "stats.json" - dtype: "json" - shape: "-" - description: "Global min/max/mean/std/count/quantiles per feature (used for normalization)" - - name: "tasks.parquet" - dtype: "parquet" - shape: "[num_tasks, 2]" - description: "Task text -> task_index" - groups: - - name: "episodes/chunk-000/file-000.parquet" - description: "One row per episode: length, tasks, data/video file + frame/time offsets, per-episode stats" - - name: "data" - description: "Frame-by-frame tabular data, many episodes per file" - groups: - - name: "chunk-000/file-000.parquet" - description: "Columns below; files roll over at ~100 MB" - datasets: - - name: "observation.state" - dtype: "float32" - shape: "[num_frames, state_dim]" - description: "Numeric output.streams concatenated in declaration order (column names in info.json)" - - name: "action" - dtype: "float32" - shape: "[num_frames, action_dim]" - description: "Built from actions_to_extract (same as the robomimic actions)" - - name: "timestamp" - dtype: "float32" - shape: "[num_frames]" - description: "frame_index / fps (seconds within the episode)" - - name: "frame_index, episode_index, index, task_index" - dtype: "int64" - shape: "[num_frames]" - description: "Frame within episode, episode id, global frame id, task id" - - name: "videos" - description: "One mp4 per camera, many episodes per file (located by from/to_timestamp in meta/episodes)" - groups: - - name: "observation.images./chunk-000/file-000.mp4" - description: "H.264, yuv420p, RGB frames from the image bridge-map entries; files roll over at ~200 MB" diff --git a/relay_d/acquisition/qt_app/ui/postprocess/postprocess_configs/robomimic.yaml b/relay_d/acquisition/qt_app/ui/postprocess/postprocess_configs/robomimic.yaml deleted file mode 100644 index 43d8649..0000000 --- a/relay_d/acquisition/qt_app/ui/postprocess/postprocess_configs/robomimic.yaml +++ /dev/null @@ -1,118 +0,0 @@ -format: - name: "robomimic" - version: "1.0" - description: "Robomimic dataset format for imitation learning" - -output_structure: - root_groups: - - name: "data" - groups: - - name: "demo_{i}" - datasets: - - name: "states" - shape: [num_samples, state_dim] - dtype: "float32" - description: "Environment states" - - name: "actions" - shape: [num_samples, action_dim] - dtype: "float32" - description: "Action sequences for robot control" - - name: "rewards" - shape: [num_samples] - dtype: "float32" - description: "Reward signals" - - name: "dones" - shape: [num_samples] - dtype: "float32" - description: "Episode termination flags" - groups: - - name: "obs" - description: "Observations at current timestep" - datasets: - - name: "*_image" - shape: [num_samples, height, width, channels] - dtype: "uint8" - description: "Image observations from cameras" - - name: "*_joint_pos" - shape: [num_samples, num_joints] - dtype: "float32" - description: "Joint position observations" - - name: "*_joint_vel" - shape: [num_samples, num_joints] - dtype: "float32" - description: "Joint velocity observations" - - name: "*_eef_pos" - shape: [num_samples, 3] - dtype: "float32" - description: "End effector position" - - name: "*_eef_quat" - shape: [num_samples, 4] - dtype: "float32" - description: "End effector orientation quaternion" - - name: "*_gripper_qpos" - shape: [num_samples, 2] - dtype: "float32" - description: "Gripper position" - - name: "object_pos" - shape: [num_samples, 3] - dtype: "float32" - description: "Object translation" - - name: "object_quat" - shape: [num_samples, 4] - dtype: "float32" - description: "Object rotation quaternion" - - name: "next_obs" - description: "Observations at next timestep" - datasets: - - name: "*_image" - shape: [num_samples, height, width, channels] - dtype: "uint8" - description: "Image observations from cameras (shifted)" - - name: "*_joint_pos" - shape: [num_samples, num_joints] - dtype: "float32" - description: "Joint position observations (shifted)" - - name: "*_joint_vel" - shape: [num_samples, num_joints] - dtype: "float32" - description: "Joint velocity observations (shifted)" - - name: "*_eef_pos" - shape: [num_samples, 3] - dtype: "float32" - description: "End effector position (shifted)" - - name: "*_eef_quat" - shape: [num_samples, 4] - dtype: "float32" - description: "End effector orientation quaternion (shifted)" - - name: "*_gripper_qpos" - shape: [num_samples, 2] - dtype: "float32" - description: "Gripper position (shifted)" - - name: "object_pos" - shape: [num_samples, 3] - dtype: "float32" - description: "Object translation (shifted)" - - name: "object_quat" - shape: [num_samples, 4] - dtype: "float32" - description: "Object rotation quaternion (shifted)" - attributes: - - name: "num_samples" - type: "int" - description: "Number of timesteps in trajectory" - - name: "model_file" - type: "string" - description: "Path to MJCF model file (for robosuite)" - attributes: - - name: "total" - type: "int" - description: "Total number of demos in dataset" - - name: "env_args" - type: "json" - description: "Environment arguments in JSON format" - - name: "mask" - description: "Dataset filtering mask" - datasets: - - name: "valid" - dtype: "string" - description: "List of valid demo names" diff --git a/relay_d/acquisition/qt_app/ui/postprocess/postprocess_options.py b/relay_d/acquisition/qt_app/ui/postprocess/postprocess_options.py index fc3f947..3eac255 100644 --- a/relay_d/acquisition/qt_app/ui/postprocess/postprocess_options.py +++ b/relay_d/acquisition/qt_app/ui/postprocess/postprocess_options.py @@ -1,35 +1,31 @@ """Conversion-format options, kept apart from the acquisition config. The acquisition config says what to record and how streams/actions are built. -Choices that only matter when converting to a particular format (robomimic env -metadata, the robomimic training-config model, train/val/test split fractions, -the LeRobot task text, ...) live in an optional ``postprocess_config.yaml``:: +Choices that only matter when converting to a particular format (train/val +split fractions, the LeRobot task text, ...) live in an optional +``postprocess_config.yaml``:: postprocess: robomimic: - training_model: bc_rnn_gmm - splits: {train_frac: 0.8, val_frac: 0.2, test_frac: 0.0} - metadata: {env_name: Lift, env_version: 1.5.1, type: 1, env_kwargs: {...}} + splits: {train_frac: 0.8, val_frac: 0.2} lerobot: task: "lift the box" robot_type: iiwa state_streams: [eef_translation, robot_joint_pos] -Lookup order for every key: this file, then the legacy location inside the -acquisition config (with a one-time deprecation warning), then a default. -No Qt or ROS needed. +Every key not set here takes its default. Removed keys (here or in the +acquisition config) are refused with what to do instead. No Qt or ROS needed. """ from __future__ import annotations import os from datetime import datetime -from typing import Any, Dict, List, Optional, Set, Tuple +from typing import Any, Dict, List, Optional, Tuple import yaml from relay_d.utils import user_settings -from relay_d.utils.coloring_logger import logger OPTIONS_FILENAME = "postprocess_config.yaml" @@ -47,17 +43,27 @@ def dataset_output_paths(save_folder: str, fmt: str, now: datetime) -> Tuple[str return output_dir, os.path.join(output_dir, f"dataset_{int(now.timestamp())}{suffix}{extension}") -_warned_legacy: Set[str] = set() +# Removed keys -> why and what to do. A config that still has one is refused, never silently ignored. +_SPLITS_MOVED = "move it to postprocess_config.yaml under robomimic.splits" +_TEST_SPLIT = "the test split was removed (robomimic never trains or validates on it); delete it" +REMOVED_ACQUISITION_KEYS = { + "metadata": "robomimic env metadata is no longer written (env_args is a fixed stub); " + "put recording_frequency at the top level of config and delete 'metadata'", + "training_model": "choose the policy on the Train tab (or relay-d-train --policy) and delete it", + "train_frac": _SPLITS_MOVED, + "val_frac": _SPLITS_MOVED, + "test_frac": _TEST_SPLIT, + "lerobot": "move it to postprocess_config.yaml under 'lerobot'", +} +REMOVED_OPTIONS = { + "metadata": "robomimic env metadata is no longer written (env_args is a fixed stub); delete it", + "training_model": "choose the policy on the Train tab (or relay-d-train --policy) and delete it", +} -def _warn_legacy(key: str, new_location: str) -> None: - if key in _warned_legacy: - return - _warned_legacy.add(key) - logger.warning( - f"'{key}' in the acquisition config is deprecated; move it to " - f"{OPTIONS_FILENAME} under '{new_location}'." - ) +def removed_key_errors(acq_config: Optional[Dict]) -> List[str]: + """One error per removed key still in the acquisition config's ``config`` section.""" + return [f"'{k}' was removed: {why}." for k, why in REMOVED_ACQUISITION_KEYS.items() if k in (acq_config or {})] class PostprocessOptions: @@ -89,6 +95,12 @@ def load(cls, path: str) -> Tuple[Optional["PostprocessOptions"], List[str]]: problems.append(f"'postprocess.{fmt}' must be a mapping.") if problems: return None, problems + robomimic = section.get("robomimic", {}) + problems = [f"'robomimic.{k}' was removed: {why}." for k, why in REMOVED_OPTIONS.items() if k in robomimic] + if "test_frac" in (robomimic.get("splits") or {}): + problems.append(f"'robomimic.splits.test_frac' was removed: {_TEST_SPLIT}.") + if problems: + return None, problems known = {"robomimic", "lerobot"} warnings = [ f"warning: unknown format section 'postprocess.{k}' (known: {sorted(known)})" @@ -113,54 +125,19 @@ def _section(self, fmt: str) -> Dict: value = self._data.get(fmt) return value if isinstance(value, dict) else {} - def robomimic_metadata(self, acq_config: Optional[Dict] = None) -> Dict: - """Environment metadata embedded as robomimic ``env_args``.""" - meta = self._section("robomimic").get("metadata") - if isinstance(meta, dict) and meta: - return meta - legacy = (acq_config or {}).get("metadata") - if isinstance(legacy, dict) and legacy: - _warn_legacy("metadata", "robomimic.metadata") - return legacy - return {} - - def training_model(self, acq_config: Optional[Dict] = None) -> str: - model = self._section("robomimic").get("training_model") - if model: - return str(model) - legacy = (acq_config or {}).get("training_model") - if legacy: - _warn_legacy("training_model", "robomimic.training_model") - return str(legacy) - default = user_settings.get("postprocess_default_training_model") # the Settings table owns the default - logger.info(f"No 'training_model' set; using the default '{default}'.") - return default - @staticmethod def _default_splits() -> Dict[str, float]: - """Defaults from the user settings (train/val; the rest is eval).""" - train, val = float(user_settings.get("split_train")), float(user_settings.get("split_val")) - return {"train_frac": train, "val_frac": val, "test_frac": round(max(0.0, 1.0 - train - val), 6)} + """Defaults from the user settings (train/val; any rest also trains).""" + return {"train_frac": float(user_settings.get("split_train")), "val_frac": float(user_settings.get("split_val"))} - def splits(self, acq_config: Optional[Dict] = None) -> Dict[str, float]: - """train/val/test fractions for the robomimic ``mask`` group.""" + def splits(self) -> Dict[str, float]: + """train/val fractions for the robomimic ``mask`` group.""" out = self._default_splits() file_splits = self._section("robomimic").get("splits") - file_splits = file_splits if isinstance(file_splits, dict) else {} - for key in list(out): - if key in file_splits: - out[key] = file_splits[key] - elif acq_config and key in acq_config: - _warn_legacy(key, "robomimic.splits") - out[key] = acq_config[key] + if isinstance(file_splits, dict): + out.update({k: file_splits[k] for k in out if k in file_splits}) return out - def lerobot(self, acq_config: Optional[Dict] = None) -> Dict[str, Any]: + def lerobot(self) -> Dict[str, Any]: """LeRobot options: ``task``, ``robot_type``, ``state_streams``.""" - legacy = (acq_config or {}).get("lerobot") - merged: Dict[str, Any] = {} - if isinstance(legacy, dict) and legacy: - _warn_legacy("lerobot", "lerobot") - merged.update(legacy) - merged.update({k: v for k, v in self._section("lerobot").items() if v is not None}) - return merged + return {k: v for k, v in self._section("lerobot").items() if v is not None} diff --git a/relay_d/acquisition/qt_app/ui/postprocess/postprocess_page.py b/relay_d/acquisition/qt_app/ui/postprocess/postprocess_page.py index b65e4b1..cd8b457 100644 --- a/relay_d/acquisition/qt_app/ui/postprocess/postprocess_page.py +++ b/relay_d/acquisition/qt_app/ui/postprocess/postprocess_page.py @@ -1,29 +1,57 @@ +import copy import os -import shutil -from PyQt5.QtWidgets import QWidget, QButtonGroup -from PyQt5.QtWidgets import QFileDialog +from PyQt5.QtCore import Qt, pyqtSignal +from PyQt5.QtGui import QBrush, QColor, QFont, QKeySequence +from PyQt5.QtWidgets import ( + QAbstractItemView, + QButtonGroup, + QFileDialog, + QGroupBox, + QHBoxLayout, + QHeaderView, + QLabel, + QPlainTextEdit, + QPushButton, + QSplitter, + QTableWidget, + QTableWidgetItem, + QVBoxLayout, + QWidget, +) from datetime import datetime -from relay_d.dispatch import save_dispatch_templates # Import utils from ..config.yaml_parser import YamlParser from ..postprocess.yaml_driven_converter import YAMLDrivenConverter -from ..postprocess.h5_file_parser import h5FileParser -from ..postprocess.robomimic_config_handler import ( - RobomimicConfigHandler, - classify_observations, -) from ..settings.config_manager import config from relay_d.utils import user_settings from relay_d.utils.coloring_logger import logger +from ...utils.background import run_in_background from ...utils.custom_message_box import CustomMessageBox +from relay_d.acquisition.convert_cli import collect_inputs from ...utils.default_configs import generate_default_configs from ..postprocess.postprocess_config_loader import PostprocessConfigLoader from ..postprocess.postprocess_options import dataset_output_paths -from ..postprocess.offline_config import load_offline_config +from ..postprocess.offline_config import load_offline_config, write_dataset_files from ..postprocess.postprocess_options import PostprocessOptions +PRECHECK_COLUMNS = ("Demo", "Samples", "Rate (Hz)", "Recorded with", "Result") +# Per-item colours cannot come from the QSS; the mid-tones the Review page uses read on both themes. +_OK_COLOR, _FAIL_COLOR = "#16A34A", "#DC2626" + +class _PrecheckTable(QTableWidget): + """The pre-check table; the Delete key removes the selected recordings from the conversion.""" + + delete_pressed = pyqtSignal() + + def keyPressEvent(self, event): + if event.matches(QKeySequence.Delete): + self.delete_pressed.emit() + else: + super().keyPressEvent(event) + + class PostProcessPage(QWidget): def __init__(self, ui_instance, yaml_parser: YamlParser = None, settings_page=None): super().__init__() @@ -32,17 +60,18 @@ def __init__(self, ui_instance, yaml_parser: YamlParser = None, settings_page=No # Config loaded on this page only (no live-ROS check, never shared with the # Record page); takes precedence over the shared parser while set. self._local_parser = None - # Format options (postprocess_config.yaml): loaded explicitly by the user, or - # auto-detected next to a loaded config. Neither -> built-in defaults. + # Format options (postprocess_config.yaml): each config load resets them to the one next + # to that config (auto-detected), else the defaults; a file the user picks applies until the next load. self._options_explicit = False + self._options_error = None # a broken postprocess_config.yaml next to the config: refuse to convert, as the CLI and AppAPI do self.settings_page = settings_page self.h5_files = [] - self.h5_parser = h5FileParser() self.output_format = "" + self._precheck_gen = 0 # bumped by every refresh: only the newest pre-check result is shown + self._report = None # last pre-check report (None: not run / running) - self.config_loader = PostprocessConfigLoader() - self.yaml_converter = YAMLDrivenConverter(self.config_loader) + self.yaml_converter = YAMLDrivenConverter(PostprocessConfigLoader()) self.format_buttons = {} self.format_names = { "btn_format_robomimic": "robomimic", @@ -54,6 +83,7 @@ def __init__(self, ui_instance, yaml_parser: YamlParser = None, settings_page=No if self.yaml_parser is not None: self._sync_data_config_from_parser() + self._build_precheck_widgets() self.setup_connections() self.setup_format_buttons() self._update_config_label() @@ -98,6 +128,7 @@ def _on_config_saved(self, file_path: str): self.yaml_parser.load_yaml_file(file_path) if self._local_parser is None: self._sync_data_config_from_parser() + self.refresh_precheck() # ------------------------------------------------------------------ # Config loaded on this page (works without any live ROS topics) @@ -117,6 +148,7 @@ def _update_config_label(self): else: label.setText("No config loaded") self.ui.btn_clear_postprocess_config.setEnabled(self._local_parser is not None) + self.refresh_precheck() # every config change passes here def _load_postprocess_config(self): """Load a config for post-processing only: no topic/TF availability check, @@ -154,6 +186,7 @@ def _clear_postprocess_config(self): self._local_parser = None if self.yaml_parser is not None and self.yaml_parser.yaml_data: self._sync_data_config_from_parser() + self._auto_load_sibling_options(self.yaml_parser.get_current_config_path()) else: self.yaml_converter.data_config = {} self._update_config_label() @@ -170,6 +203,7 @@ def _update_options_label(self): else: self.ui.label_postprocess_options.setText("defaults") self.ui.btn_clear_postprocess_options.setEnabled(bool(source)) + self.refresh_precheck() # every options change passes here def _load_postprocess_options(self): try: @@ -190,21 +224,23 @@ def load_postprocess_options_from_path(self, path: str, explicit: bool = True) - "Options rejected:\n" + "\n".join(f" {p}" for p in problems) ) else: - logger.warning(f"Ignoring {path}: {'; '.join(problems)}") + self._options_error = f"{path} is not usable:\n" + "\n".join(f" {p}" for p in problems) + logger.error(self._options_error) return False warnings = [p for p in problems if p.startswith("warning:")] if warnings and explicit: CustomMessageBox.warning("\n".join(w[len("warning: "):] for w in warnings)) self.yaml_converter.options = options self._options_explicit = explicit + self._options_error = None self._update_options_label() logger.info(f"PostProcessPage: using format options from {path}") return True def _auto_load_sibling_options(self, config_path: str): - """Use a postprocess_config.yaml next to the loaded config, unless the user chose one.""" - if self._options_explicit: - return + """A loaded config brings its options: the postprocess_config.yaml next to it, else the defaults + (as relay-d-convert and AppAPI).""" + self._clear_postprocess_options() sibling = PostprocessOptions.find_sibling(config_path) if sibling: self.load_postprocess_options_from_path(sibling, explicit=False) @@ -212,6 +248,7 @@ def _auto_load_sibling_options(self, config_path: str): def _clear_postprocess_options(self): self.yaml_converter.options = PostprocessOptions() self._options_explicit = False + self._options_error = None self._update_options_label() # ------------------------------------------------------------------ @@ -227,7 +264,6 @@ def _sync_data_config_from_parser(self): config: input: { ... } output: { robot0_eef_pos: robot0_eef/translations, ... } - metadata: { env_name: ..., n_actions: 7, ... } """ parser = self._active_parser() if parser is None: @@ -260,9 +296,15 @@ def _sync_data_config_from_parser(self): def setup_connections(self): """Setup all button connections""" ui = self.ui - ui.btn_convert_dataset.clicked.connect(self.convert_dataset) - ui.btn_load_h5.clicked.connect(self._select_files_to_convert) - ui.btn_clear_preview.clicked.connect(self.clear_h5_preview) + self.btn_convert.clicked.connect(self.convert_dataset) + self.btn_add_files.clicked.connect(self._select_files_to_convert) + self.btn_add_folder.clicked.connect(self._select_folder_to_convert) + self.btn_clear_files.clicked.connect(lambda: self.set_files([])) + self.btn_remove_files.clicked.connect(self._remove_selected_files) + self.table_precheck.delete_pressed.connect(self._remove_selected_files) + self.table_precheck.itemSelectionChanged.connect( + lambda: self.btn_remove_files.setEnabled(bool(self._selected_files())) + ) ui.btn_generate_postprocess_configs.clicked.connect(lambda: generate_default_configs(ui)) ui.btn_load_postprocess_config.clicked.connect(self._load_postprocess_config) ui.btn_clear_postprocess_config.clicked.connect(self._clear_postprocess_config) @@ -271,42 +313,22 @@ def setup_connections(self): def setup_format_buttons(self): """Setup format selection buttons defined in .ui file""" - formats = self.config_loader.get_all_format_names() - logger.info(f"Available formats: {formats}") - # Create button group for exclusive selection self.format_button_group = QButtonGroup() self.format_button_group.setExclusive(False) - # Setup each format button for button_name, format_name in self.format_names.items(): button = getattr(self.ui, button_name, None) - - if button is not None: - format_info = self.config_loader.get_format_info(format_name) - button.setCheckable(True) - - if format_info is not None: - button.setToolTip(format_info.get("description", "")) - button.clicked.connect( - lambda checked, name=format_name: self._select_output_format( - name - ) - ) - self.format_button_group.addButton(button) - self.format_buttons[format_name] = button - - if format_name == user_settings.get("postprocess_default_format"): # setting - button.setChecked(True) - self.output_format = format_name - self.show_output_structure_preview(format_name) - else: - logger.warning(f"Could not get format info for: {format_name}") - button.setEnabled(False) - else: + if button is None: logger.warning(f"Button {button_name} not found in UI") - - logger.info(f"Setup {len(self.format_buttons)} format buttons") + continue + button.setCheckable(True) + button.clicked.connect(lambda checked, name=format_name: self._select_output_format(name)) + self.format_button_group.addButton(button) + self.format_buttons[format_name] = button + if format_name == user_settings.get("postprocess_default_format"): # setting + button.setChecked(True) + self.output_format = format_name def _select_output_format(self, format_name): """Single-choice toggle: select `format_name`, or deselect it if it already is. @@ -321,13 +343,12 @@ def _select_output_format(self, format_name): if self.output_format == format_name: self.output_format = "" self._sync_format_buttons() - self.clear_output_preview() logger.info(f"Deselected format: {format_name}") else: self.output_format = format_name self._sync_format_buttons() - self.show_output_structure_preview(format_name) logger.info(f"Selected output format: {format_name}") + self.refresh_precheck() def _sync_format_buttons(self): """Check exactly the button of the selected format (none if nothing is selected).""" @@ -336,49 +357,231 @@ def _sync_format_buttons(self): button.setChecked(name == self.output_format) button.blockSignals(blocked) - def clear_output_preview(self): - self.ui.txt_output_preview.clear() - - def show_output_structure_preview(self, format_name): - self.ui.txt_output_preview.setHtml(self.config_loader.generate_html_structure_preview(format_name)) - self.ui.txt_output_preview.show() - # ------------------------------------------------------------------ # File selection # ------------------------------------------------------------------ - def _select_files_to_convert(self): - - logger.info("Opening file browser") - - # Get the same folder where datasets are saved by default, for better UX. Fall back to home if not set. + def _recordings_dir(self): + """Where recordings are saved (Settings page), for the file dialogs; else home.""" save_folder = self.settings_page.get_hdf5_save_folder() if self.settings_page else "" - processed_dir = os.path.join(save_folder, "recorded_data") if save_folder else os.path.expanduser("~") + return os.path.join(save_folder, "recorded_data") if save_folder else os.path.expanduser("~") + def _select_files_to_convert(self): fn, _ = QFileDialog.getOpenFileNames( - parent=None, - caption="Select H5 Files", - filter="H5 files (*.h5)", - directory=processed_dir, + parent=None, caption="Add recordings", filter="H5 files (*.h5)", directory=self._recordings_dir() ) + if fn: + self.add_inputs(fn) - if not fn: - logger.info("No file selected by user.") - return + def _select_folder_to_convert(self): + folder = QFileDialog.getExistingDirectory(None, "Add every recording under a folder", self._recordings_dir()) + if folder: + self.add_inputs([folder]) - self.h5_files = list(fn) - logger.info(f"Files loaded: {self.h5_files}. Total files: {len(self.h5_files)}") - self.show_h5_preview(self.h5_parser.convert_h5_to_html(self.h5_files[0])) + def add_inputs(self, paths): + """Add files and folders (searched recursively for *.h5, as relay-d-convert --input) to the recordings.""" + try: + self.set_files(collect_inputs(self.h5_files + list(paths))) + except FileNotFoundError as e: + CustomMessageBox.error(f"Not found: {e}") - def show_h5_preview(self, html_string): - self.ui.txt_input_structure.clear() - self.ui.txt_input_structure.setHtml(html_string) + def _selected_files(self): + rows = {i.row() for i in self.table_precheck.selectedIndexes()} + return {self.table_precheck.item(r, 0).data(Qt.UserRole) for r in rows if self.table_precheck.item(r, 0)} - def clear_h5_preview(self): - self.ui.txt_input_structure.clear() - self.h5_files = [] - CustomMessageBox.info("Cleared H5 file preview and reset loaded files.") - return + def _remove_selected_files(self): + """Leave the selected rows' files out of the conversion (a file holding several demos goes as a whole).""" + selected = self._selected_files() + if selected: + self.set_files([f for f in self.h5_files if f not in selected]) + + def set_files(self, files): + self.h5_files = list(files) + logger.info(f"Recordings to convert: {len(self.h5_files)} files") + self.refresh_precheck() + + # ------------------------------------------------------------------ + # Pre-check (what conversion would do, without writing anything) + # ------------------------------------------------------------------ + + def _build_precheck_widgets(self): + """Recordings row, pre-check table, output preview and Convert, under the .ui's config/options/format rows.""" + layout = self.ui.verticalLayout_9 + + row = QHBoxLayout() + row.setSpacing(12) + row.addWidget(QLabel("Recordings:")) + self.label_recordings = QLabel() + row.addWidget(self.label_recordings, 1) + self.btn_add_files = QPushButton("Add files...") + self.btn_add_folder = QPushButton("Add folder...") + self.btn_remove_files = QPushButton("Remove selected") + self.btn_remove_files.setToolTip("Leave the selected recordings out of this conversion (Delete key). Nothing is deleted from disk.") + self.btn_remove_files.setEnabled(False) + self.btn_clear_files = QPushButton("Clear") + for b in (self.btn_add_files, self.btn_add_folder, self.btn_remove_files, self.btn_clear_files): + b.setMinimumHeight(36) + row.addWidget(b) + layout.addLayout(row) + + self.label_precheck_error = QLabel() + self.label_precheck_error.setObjectName("precheckError") + self.label_precheck_error.setWordWrap(True) + self.label_precheck_error.setTextInteractionFlags(Qt.TextSelectableByMouse) + self.label_precheck_error.hide() + layout.addWidget(self.label_precheck_error) + + self.table_precheck = _PrecheckTable(0, len(PRECHECK_COLUMNS)) + self.table_precheck.setHorizontalHeaderLabels(PRECHECK_COLUMNS) + self.table_precheck.setEditTriggers(QAbstractItemView.NoEditTriggers) + self.table_precheck.setSelectionBehavior(QAbstractItemView.SelectRows) + self.table_precheck.setSelectionMode(QAbstractItemView.ExtendedSelection) + self.table_precheck.verticalHeader().hide() + header = self.table_precheck.horizontalHeader() + header.setSectionResizeMode(QHeaderView.ResizeToContents) + header.setSectionResizeMode(len(PRECHECK_COLUMNS) - 1, QHeaderView.Stretch) # long reasons wrap instead of being cut + self.table_precheck.setWordWrap(True) + precheck_box = QGroupBox("Pre-check") + QVBoxLayout(precheck_box).addWidget(self.table_precheck) + + self.output_box = QGroupBox("Output") + self.txt_output = QPlainTextEdit() + self.txt_output.setReadOnly(True) + self.txt_output.setLineWrapMode(QPlainTextEdit.NoWrap) + self.txt_output.setFont(QFont("monospace")) + QVBoxLayout(self.output_box).addWidget(self.txt_output) + + side = QWidget() + side_layout = QVBoxLayout(side) + side_layout.setContentsMargins(0, 14, 0, 0) + self.label_summary = QLabel() + self.label_summary.setWordWrap(True) + self.label_convert_reason = QLabel() + self.label_convert_reason.setObjectName("pageHint") + self.label_convert_reason.setWordWrap(True) + self.btn_convert = QPushButton("Convert") + self.btn_convert.setMinimumHeight(38) + bold = self.btn_convert.font() + bold.setBold(True) + self.btn_convert.setFont(bold) + side_layout.addWidget(self.label_summary) + side_layout.addWidget(self.label_convert_reason) + side_layout.addStretch(1) + side_layout.addWidget(self.btn_convert) + side.setMinimumWidth(180) + + bottom = QSplitter(Qt.Horizontal) + bottom.addWidget(self.output_box) + bottom.addWidget(side) + bottom.setStretchFactor(0, 4) + bottom.setStretchFactor(1, 1) + split = QSplitter(Qt.Vertical) + split.addWidget(precheck_box) + split.addWidget(bottom) + split.setStretchFactor(0, 3) + split.setStretchFactor(1, 2) + split.setChildrenCollapsible(False) + layout.addWidget(split, 1) + + def _blocked_before_check(self): + """Why there is nothing to pre-check yet ("" when there is).""" + if not self.output_format: + return "Select an output format." + if not self.h5_files: + return "Add recordings to convert." + if self._options_error: + return self._options_error + if not self.yaml_converter.data_config.get("output"): + return "No configuration loaded: load one on the Config page, or use 'Load Config...' here." + return "" + + def refresh_precheck(self): + """Re-run the pre-check in the background; a refresh started later replaces the result of an earlier one.""" + self._precheck_gen += 1 + gen = self._precheck_gen + self._report = None + if self._blocked_before_check(): + self._show_report() + return + # The pre-check sets per-batch state on the converter: give it its own copy, so a running + # check never mixes with another one or with a conversion. + conv = copy.copy(self.yaml_converter) + conv.data_config = copy.deepcopy(self.yaml_converter.data_config) + fmt, files = self.output_format, list(self.h5_files) + self._show_report(checking=True) + run_in_background( + self.ui.page_data, lambda: conv.precheck(fmt, files), lambda r, e: self._on_precheck_done(gen, r, e) + ) + + def _on_precheck_done(self, gen, report, error): + if gen != self._precheck_gen: + return # stale: inputs changed while it ran + if error is not None: + logger.error(f"Pre-check failed: {error}") + report = {"error": f"Pre-check failed: {error}", "demos": [], "output": [], "converted": 0, "total": 0} + self._report = report + self._show_report() + + def _convert_block_reason(self, checking=False): + """Why Convert is disabled ("" when it is enabled).""" + if reason := self._blocked_before_check(): + return reason + if checking or self._report is None: + return "Checking the recordings..." + if self._report.get("error"): + return self._report["error"] + if not self._report.get("converted"): + return "No demo would be converted: see the Result column." + return "" + + def _show_report(self, checking=False): + report = self._report or {} + demos = report.get("demos") or [] + n_files = len(self.h5_files) + files = f"{n_files} file{'s' * (n_files != 1)}" + demos_text = f"{report.get('total')} demo{'s' * (report.get('total') != 1)}" + self.label_recordings.setText(f"{demos_text} in {files}" if "total" in report else files) + + error = report.get("error") + self.label_precheck_error.setText(error or "") + self.label_precheck_error.setVisible(bool(error)) + + self.table_precheck.setRowCount(len(demos)) + for row, d in enumerate(demos): + name = d.get("demo", "") + rate, rec_hz = d.get("rate_hz"), d.get("recorded_hz") + cells = ( + name, + str(d.get("samples", "")), + f"{rate:.1f}" if rate else "-", + (d.get("recorded_with") or "-") + (f" ({rec_hz:g} Hz)" if rec_hz else ""), + "\u2714 converts" if d.get("ok") else f"\u2716 {d.get('reason', '')}", + ) + for col, text in enumerate(cells): + item = QTableWidgetItem(text) + item.setToolTip(d.get("file", "") if col == 0 else text) + if col == 0: + item.setData(Qt.UserRole, d.get("file", "")) # a row stands for its recording file + if col == len(cells) - 1: + item.setForeground(QBrush(QColor(_OK_COLOR if d.get("ok") else _FAIL_COLOR))) + self.table_precheck.setItem(row, col, item) + self.table_precheck.resizeRowsToContents() + + fmt = {"robomimic": "Robomimic", "lerobot": "LeRobot"}.get(self.output_format, self.output_format) + self.output_box.setTitle(f"Output ({fmt})" if fmt else "Output") + output = report.get("output") or [] + width = max((len(str(name)) for name, _ in output), default=0) + self.txt_output.setPlainText("\n".join(f"{str(name):<{width}} {detail}" for name, detail in output)) + self.label_summary.setText( + f"{report['converted']} of {demos_text} will be converted" if "total" in report else "" + ) + + reason = self._convert_block_reason(checking) + converted = report.get("converted") or 0 + self.btn_convert.setEnabled(not reason) + self.btn_convert.setText(f"Convert {converted} demo{'s' * (converted != 1)}" if not reason else "Convert") + self.btn_convert.setToolTip(reason) + self.label_convert_reason.setText("See the error above." if error and reason == error else reason) # ------------------------------------------------------------------ # Conversion @@ -399,6 +602,10 @@ def convert_dataset(self): CustomMessageBox.warning("Please select at least one H5 file to convert.") return + if self._options_error: + CustomMessageBox.error(self._options_error + "\n\nFix it, or load another options file with 'Load Options...'.") + return + # Guard: ensure bridge map is populated before converting bridge_entries = len(self.yaml_converter.data_config.get("output", {})) if bridge_entries == 0: @@ -410,10 +617,10 @@ def convert_dataset(self): logger.info(f"Converting dataset to {self.output_format} format.") - # The Recordings folder setting, else ~/.RelayDApp + # The Recordings folder setting, else ~/lfd_recordings (the recorder's and AppAPI's default too) save_folder = self.settings_page.get_hdf5_save_folder() if self.settings_page else "" if not save_folder: - save_folder = os.path.expanduser("~/.RelayDApp") + save_folder = os.path.expanduser("~/lfd_recordings") logger.warning(f"The recordings folder is not set (Settings page). Defaulting to {save_folder}") output_dir, combined_output_path = dataset_output_paths(save_folder, self.output_format, datetime.now()) os.makedirs(output_dir, exist_ok=True) @@ -424,68 +631,27 @@ def convert_dataset(self): combined_output_path, ) - # Populate the training config's observation modalities from the - # actual converted data and save it alongside the dataset, for - # future reference. + # Keep the configs used next to the dataset, plus the dispatcher files. post_error = None if success: try: - # Copy the acquisition_config used to build the dataset into the output folder for reference - if(not self._active_parser().save_yaml_file(file_path=f"{output_dir}/acquisition_config.yaml")): - raise RuntimeError("could not copy acquisition_config.yaml to the output folder") - - # Keep the format options used for this dataset next to the config - if self.yaml_converter.options.source: - shutil.copy2( - self.yaml_converter.options.source, - os.path.join(output_dir, "postprocess_config.yaml"), - ) - - # The robomimic training config only applies to the robomimic format - if self.output_format != "lerobot": - # Get the train config path from the format config and populate it with the actual train dir and observation modalities - train_config_path = self.config_loader.get_output_train_config( - self.output_format, - self._active_parser().get_train_model_name(self.yaml_converter.options), - ) - handler = RobomimicConfigHandler(train_config_path) - handler.set_train_dir(f"~/Documents/robomimic_trains/{os.path.basename(output_dir)}") - - # Classigy observations based on the actual converted data and populate the config - classification = classify_observations( - self.yaml_converter.data_config, - self.yaml_converter.get_last_shape_cache(), - ) - handler.set_observation_modalities(**classification) - handler.save( - os.path.join(output_dir, f"{self.output_format}_config.json") - ) - - # The dispatcher files (robomimic and LeRobot policies both run through lfd-inference-node): - # obs_config.yaml from the acquired config, an action_config to adapt, and the inference script - action_dispatcher_folder = os.path.join(output_dir, "action_dispatcher") - os.makedirs(action_dispatcher_folder, exist_ok=True) - parser = self._active_parser() - parser.save_yaml_file( - file_path=os.path.join(action_dispatcher_folder, "obs_config.yaml"), data=parser.get_obs_config() - ) - save_dispatch_templates(action_dispatcher_folder) - + write_dataset_files(output_dir, self._active_parser(), self.yaml_converter.options.source) except Exception as e: post_error = str(e) or type(e).__name__ logger.error( - f"Failed to write populated {self.output_format} training config: {e}" + f"Failed to write the config copies / action_dispatcher files: {e}" ) if success: logger.info("Dataset successfully converted and validated!") if post_error: CustomMessageBox.warning( - "The dataset was converted, but some extra files (training config / " + "The dataset was converted, but some extra files (config copies / " f"action_dispatcher files) could not be written:\n\n{post_error}" ) CustomMessageBox.info( - f"Dataset successfully converted to {self.output_format} format!\n\nOutput file: {combined_output_path}", + f"Dataset converted to {self.output_format} format.\n\n{self.yaml_converter.conversion_summary()}" + f"\n\nOutput file: {combined_output_path}", ) else: logger.error("Dataset conversion failed") diff --git a/relay_d/acquisition/qt_app/ui/postprocess/robomimic_config_handler.py b/relay_d/acquisition/qt_app/ui/postprocess/robomimic_config_handler.py index 9aa2b58..e86c26c 100644 --- a/relay_d/acquisition/qt_app/ui/postprocess/robomimic_config_handler.py +++ b/relay_d/acquisition/qt_app/ui/postprocess/robomimic_config_handler.py @@ -16,9 +16,6 @@ def _load_config(self): def get_config(self) -> Dict: return self.json_config - def set_train_dir(self, train_dir: str): - self.json_config.setdefault("train", {})["output_dir"] = train_dir - def set_observation_modalities( self, low_dim: Iterable[str], @@ -37,9 +34,24 @@ def set_observation_modalities( obs["depth"] = list(depth) obs["scan"] = list(scan) - def save(self, output_path: str = None): - with open(output_path or self.config_path, "w") as f: - json.dump(self.json_config, f, indent=4) + +def explicit_modalities(data_config: Dict) -> Dict[str, str]: + """Output (observation) key -> "rgb"|"depth"|"scan" for camera / point-cloud outputs whose input + sets ``modality``. ``output: agentview_depth: camera_depth/images`` takes the modality of the input + recorded into the group ``camera_depth`` (the input's name).""" + by_group = { + name: spec.get("modality") + for name, spec in (data_config.get("input") or {}).items() + if isinstance(spec, dict) + } + modalities: Dict[str, str] = {} + for key, path in (data_config.get("output") or {}).items(): + if key in ("streams", "prefix_path") or not isinstance(path, str): + continue + modality = by_group.get(path.split("/")[0]) + if modality in ("rgb", "depth", "scan"): + modalities[key] = modality + return modalities def classify_observations( @@ -48,7 +60,7 @@ def classify_observations( ) -> Dict[str, list]: """ Classify every key in shape_cache (the observations that actually - survived the last conversion) into Robomimic's low_dim/rgb/depth/scan + are in the converted dataset) into Robomimic's low_dim/rgb/depth/scan obs modalities. Priority: @@ -59,14 +71,7 @@ def classify_observations( inputs that never bothered setting `modality`). 3. Otherwise -> low_dim. """ - explicit_modality: Dict[str, str] = {} - for input_name, spec in data_config.get("input", {}).items(): - if not isinstance(spec, dict): - continue - modality = spec.get("modality") - if modality in ("rgb", "depth", "scan"): - key = spec.get("output_map") or input_name - explicit_modality[key] = modality + explicit_modality = explicit_modalities(data_config) low_dim, rgb, depth, scan = [], [], [], [] buckets = {"low_dim": low_dim, "rgb": rgb, "depth": depth, "scan": scan} diff --git a/relay_d/acquisition/qt_app/ui/postprocess/yaml_driven_converter.py b/relay_d/acquisition/qt_app/ui/postprocess/yaml_driven_converter.py index 7dd5453..30c7003 100644 --- a/relay_d/acquisition/qt_app/ui/postprocess/yaml_driven_converter.py +++ b/relay_d/acquisition/qt_app/ui/postprocess/yaml_driven_converter.py @@ -1,12 +1,9 @@ """ -YAML-driven dataset conversion engine -Reads conversion configs and applies transformation rules to convert datasets -to the Robomimic 1.0 format using a two-step bridge mapping strategy. - -Bridge Mapping (two-step lookup): - Step A: robomimic.yaml defines required observations (e.g. robot0_eef_pos) - Step B: data_config.yaml `output` section maps logical keys to recorded HDF5 paths - (e.g. robot0_eef_pos -> robot0_eef/translations) +YAML-driven dataset conversion: recorded HDF5 demos -> robomimic HDF5 or LeRobotDataset v3.0. + +Numeric obs and actions come from each demo's `streams/` group (`output.streams`); other +`output:` entries (the bridge map) point an obs key at a recorded HDF5 path, +e.g. agentview_image -> camera_rgb/images. """ import h5py @@ -14,8 +11,10 @@ from typing import Dict, List, Any, Optional, Tuple, Set from relay_d.utils.coloring_logger import logger from relay_d.utils import user_settings -from relay_d.utils.numerics import RATE_TOLERANCE, median_rate, normalize, quat_to_euler -from .postprocess_options import PostprocessOptions +from relay_d.utils.numerics import RATE_TOLERANCE, median_rate, quat_to_euler +from relay_d.acquisition.qt_app.ui.config.builder.config_check import normalization_error, stream_width +from .postprocess_options import FORMAT_SUFFIX, PostprocessOptions, removed_key_errors +from .robomimic_config_handler import explicit_modalities import re import json import os @@ -24,108 +23,27 @@ def _parse_stream_entry(stream_val): - """Return (spec_list, normalize, norm_range, limits, rotation_format). + """Return (spec_list, rotation_format). Accepts plain-list form (list of strings) or dict form: specs: [...] - normalize: true - normalized_range: [-1.0, 1.0] - limits: - - {lower_lim: ..., upper_lim: ...} # one per column, or one entry → broadcast rotation_format: quat | rpy # quat (default) or roll-pitch-yaw radians """ if isinstance(stream_val, dict): - return ( - stream_val.get("specs", []), - stream_val.get("normalize", False), - stream_val.get("normalized_range", [-1.0, 1.0]), - stream_val.get("limits", None), - str(stream_val.get("rotation_format", "quat")).lower(), - ) - return stream_val, False, None, None, "quat" - + return stream_val.get("specs", []), str(stream_val.get("rotation_format", "quat")).lower() + return stream_val, "quat" -def _apply_normalization( - data: "np.ndarray", - limits: List[Dict], - norm_range: List[float], -) -> "np.ndarray": - """Normalize each column of *data* using per-element limits. - If *limits* has a single entry it is broadcast to every column. - Columns whose span is zero are left unchanged. +def _stream_output_width(data_config: Dict, stream_name: str) -> Optional[int]: """ - lo, hi = norm_range - n_cols = data.shape[1] if data.ndim == 2 else 1 - - if data.ndim == 2 and n_cols == 0: - logger.warning( - "Normalization skipped: stream has 0 columns — data was recorded before " - "the dict-form fix. Re-record to get proper data." - ) - return data - - if len(limits) == 1: - limits = limits * n_cols - - if data.ndim == 2 and len(limits) != n_cols: - raise ValueError( - f"_apply_normalization: {len(limits)} limits but data has {n_cols} columns. " - f"Trim the limits list in acquisition_config.yaml to match the recorded columns " - f"(tip: use --joints with extract_joint_limits.py to filter to only the joints " - f"listed in joint_names)." - ) - - out = data.copy() - for i, lim in enumerate(limits): - lower = float(lim["lower_lim"]) - upper = float(lim["upper_lim"]) - if data.ndim == 2: - out[:, i] = normalize(data[:, i], lower, upper, lo, hi) - else: - out = normalize(data, lower, upper, lo, hi) - return out - - -def _infer_spec_width(data_config: Dict, stream_name: str) -> int: - """ - Column width for a single stream, derived purely from its configured - specs (no recorded data needed): - - slice spec ([:]): width = len(joint_names) for that input, or 1 - - sin/cos wrapper: same rule applied to the inner spec - - scalar spec: width = 1 - - Returns 0 if the stream has no configured specs at all (e.g. it isn't - an `output.streams` entry). + Post-conversion column width for a stream from its configured specs (config_check.stream_width: + None if a ``[:]`` slice has no joint_names), except a stream with rotation_format: rpy outputs + 3 columns (roll, pitch, yaw) from its 4 quaternion specs. """ streams = data_config.get("output", {}).get("streams", {}) - inputs = data_config.get("input", {}) - - spec_list, *_ = _parse_stream_entry(streams.get(stream_name, [])) - - width = 0 - for spec in spec_list: - inner = spec.strip() - if inner.startswith(("sin(", "cos(")) and inner.endswith(")"): - inner = inner[4:-1].strip() - if inner.endswith("[:]"): - input_name = inner.split(".")[0] - jnames = inputs.get(input_name, {}).get("joint_names", []) - width += len(jnames) if jnames else 1 - else: - width += 1 - return width - - -def _stream_output_width(data_config: Dict, stream_name: str) -> int: - """ - Post-conversion column width for a stream: same as _infer_spec_width, except - a stream with rotation_format: rpy outputs 3 columns (roll, pitch, yaw) from - its 4 quaternion specs. - """ - width = _infer_spec_width(data_config, stream_name) - streams = data_config.get("output", {}).get("streams", {}) - *_, rot_fmt = _parse_stream_entry(streams.get(stream_name, [])) + spec_list, rot_fmt = _parse_stream_entry(streams.get(stream_name, [])) + inputs = {n: v for n, v in (data_config.get("input") or {}).items() if isinstance(v, dict)} + width = stream_width([str(x) for x in spec_list], inputs) if rot_fmt == "rpy": if width != 4: raise ValueError( @@ -136,33 +54,15 @@ def _stream_output_width(data_config: Dict, stream_name: str) -> int: return width -def _infer_n_actions(data_config: Dict) -> int: - """ - Derive the total action width from actions_to_extract + stream specs. - See _stream_output_width for the per-stream width rule. - """ - to_extract = data_config.get("actions_to_extract", []) - total = sum(_stream_output_width(data_config, name) for name in to_extract) - return total if total > 0 else 1 - - def _action_part(part: np.ndarray, key: str, stream_configs: dict, delta_actions: list) -> np.ndarray: - """One `actions_to_extract` entry as (N, width) columns: its stream's rotation format and - normalization, then the target of step t: the delta to t+1 if listed in `delta_actions`, else - the value at t+1 (the value at t is the current state, which would teach the policy to stand still). - Shared by both conversion paths.""" + """One `actions_to_extract` entry as (N, width) columns: its stream's rotation format, + then the target of step t: the delta to t+1 if listed in `delta_actions`, else the value at t+1 + (the value at t is the current state, which would teach the policy to stand still). + Shared by both output formats.""" part = part if part.ndim > 1 else part[:, np.newaxis] - _, normalize, norm_range, limits, rot_fmt = _parse_stream_entry(stream_configs.get(key, [])) - if normalize and limits and key in delta_actions: - # A delta of normalized values has no offset: the dispatcher's denormalize (which adds lower_lim) cannot undo it. - raise ValueError( - f"stream '{key}' is both normalized and in delta_actions: the dispatcher cannot turn that back into " - f"real units. Add a second stream over the same specs without `normalize` and use it as the action." - ) + _, rot_fmt = _parse_stream_entry(stream_configs.get(key, [])) part = _apply_rotation_format(part, rot_fmt, key) - if normalize and limits: - part = _apply_normalization(part, limits, norm_range) - return _compute_delta(part) if key in delta_actions else _next_obs(part) + return _compute_delta(part) if key in delta_actions else _next_step(part) def _compute_delta(data: np.ndarray) -> np.ndarray: @@ -224,30 +124,93 @@ def _check_length(data: np.ndarray, num_samples: int, key: str) -> None: raise ValueError(f"'{key}' has {len(data)} rows but the demo has {num_samples} timesteps") -def _next_obs(obs: np.ndarray) -> np.ndarray: - """next_obs[t] = obs[t+1]; the last step repeats itself so the length stays N.""" +def _demo_metadata(demo_data: h5py.Group) -> Tuple[Optional[float], Optional[str]]: + """(recording_frequency, yaml_config file name) the recorder stored in data//metadata, None where absent.""" + meta = demo_data.get("metadata") + attrs = meta.attrs if isinstance(meta, h5py.Group) else {} + try: + hz = float(attrs["recording_frequency"]) if "recording_frequency" in attrs else None + except (TypeError, ValueError): + hz = None + config = attrs.get("yaml_config") + config = config.decode() if isinstance(config, bytes) else config + return hz, (os.path.basename(str(config)) or None) if config else None + + +def _names_text(names: List[str]) -> str: + """'(7) a.0..3, b.0..2' from LeRobot column names (a.0, a.1, ..., b).""" + groups: Dict[str, int] = {} + for name in names: + key, _, idx = name.rpartition(".") + key = key if key and idx.isdigit() else name + groups[key] = groups.get(key, 0) + 1 + parts = [k if k in names else f"{k}.0..{n - 1}" for k, n in groups.items()] + return f"({len(names)}) " + ", ".join(parts) + + +def _splits_error(train, val) -> Optional[str]: + """Why robomimic.splits cannot make the masks, None if they can.""" + for name, frac in (("train", train), ("val", val)): + if not isinstance(frac, (int, float)) or isinstance(frac, bool) or not 0 <= frac <= 1: + return f"robomimic.splits: '{name}' must be a number between 0 and 1, got {frac!r}" + if train == 0: # every robomimic template trains on the "train" mask: no mask, no training + return "robomimic.splits: 'train' must be more than 0" + if train + val > 1 + 1e-9: + return f"robomimic.splits: train ({train}) + val ({val}) is more than 1" + return None + + +def _split_counts(total: int, train: float, val: float) -> Tuple[int, int]: + """(train, valid) demo counts; demos left over by rounding or train + val < 1 also train.""" + # At least one training demo when training is asked for (round() would give 0 for few demos). + train_count = max(1, round(total * train)) if train > 0 and total else 0 + val_count = min(round(total * val), total - train_count) + if train_count: + train_count = total - val_count + return train_count, val_count + + +def _next_step(obs: np.ndarray) -> np.ndarray: + """value[t+1]; the last step repeats itself so the length stays N.""" return np.concatenate([obs[1:], obs[-1:]], axis=0) -def _stack_states(obs: Dict[str, Any], num_samples: int) -> np.ndarray: - """``states``: every float (N, M) obs array side by side in order; images (uint8) and 1-D arrays are skipped.""" - parts = [v for v in obs.values() if isinstance(v, np.ndarray) and v.dtype != np.uint8 and v.ndim == 2] - return np.concatenate(parts, axis=1) if parts else np.zeros((num_samples, 1), dtype=np.float32) +# robomimic's train.py always reads env_args from the dataset; with rollout disabled it only needs these fields +# (type 1 = robosuite keeps robomimic's depth processing as before). Each conversion adds +# env_kwargs.control_freq = recording_frequency: robomimic copies env_args into the checkpoint (env_metadata), +# where inference reads the rate the policy was trained at. +ENV_ARGS = {"env_name": "relay_d", "type": 1, "env_kwargs": {}} + +FPS_REQUIRED = "A positive 'recording_frequency' is required in the config (used as the fps)." def natural_key(path: str) -> List: - """Sort key for file names with numbers: rec_2.h5 before rec_10.h5.""" - parts = re.split(r"(\d+)", os.path.basename(path)) + """Sort key for paths with numbers: rec_2.h5 before rec_10.h5. The whole path counts, so same-named + files of several session folders keep a fixed order (folder first).""" + parts = re.split(r"(\d+)", path) return [int(p) if p.isdigit() else p.lower() for p in parts] +def _is_converted_robomimic(path: str) -> bool: + """True (and logged) for a robomimic dataset written by a conversion (it has a ``mask`` group): not a recording.""" + try: + with h5py.File(path, "r") as f: + converted = "mask" in f + except OSError: + return False # reported as unreadable by the conversion loop + if converted: + logger.warning(f"Skipping {path}: it is a converted robomimic dataset, not a recording.") + return converted + + +def _recordings(input_files: List[str]) -> List[str]: + """The input files in conversion order, without converted robomimic datasets.""" + return [f for f in sorted(input_files, key=natural_key) if not _is_converted_robomimic(f)] + + class DataTransformEngine: """ - Engine for applying transformation rules based on YAML config. - - Accepts: - config – parsed robomimic.yaml - data_config – parsed data_config.yaml (the `config` sub-key) + Bridge map (obs key -> recorded HDF5 path) of a data_config (the `config` sub-key). """ def __init__(self, data_config: Optional[Dict] = None): @@ -281,28 +244,6 @@ def _build_bridge_map(self) -> Dict[str, str]: ) return bridge_map - def _discover_eef_key(self) -> Optional[str]: - """Discover the EEF position key from bridge map (e.g., robot0_eef_pos).""" - candidates = [ - key for key in self.bridge_map if key.endswith("_pos") and "eef" in key.lower() - ] - if len(candidates) > 1: - logger.warning( - f"Multiple candidate eef keys found {candidates}; using '{candidates[0]}'." - ) - return candidates[0] if candidates else None - - def _discover_joint_key(self) -> Optional[str]: - """Discover the joint position key from bridge map (e.g., robot0_joint_pos).""" - candidates = [ - key for key in self.bridge_map if key.endswith("_pos") and "joint" in key.lower() - ] - if len(candidates) > 1: - logger.warning( - f"Multiple candidate joint keys found {candidates}; using '{candidates[0]}'." - ) - return candidates[0] if candidates else None - # ------------------------------------------------------------------ # Path resolution # ------------------------------------------------------------------ @@ -317,74 +258,7 @@ def resolve_path(self, logical_key: str) -> Optional[str]: Returns: Recorded path e.g. "robot0_eef/translations", or None if unmapped. """ - if logical_key in self.bridge_map: - return self.bridge_map[logical_key] - - # Fuzzy: try stripping known suffixes to find a parent key - for suffix in ("_pos", "_quat", "_vel", "_image"): - if logical_key.endswith(suffix): - stripped = logical_key[: -len(suffix)] - if stripped in self.bridge_map: - return self.bridge_map[stripped] - - logger.debug(f"No bridge mapping found for logical key: '{logical_key}'") - return None - - # ------------------------------------------------------------------ - # next_obs generation - # ------------------------------------------------------------------ - - # ------------------------------------------------------------------ - # Action generation - # ------------------------------------------------------------------ - - def generate_actions( - self, - input_data: h5py.Group, - obs_dict: Dict[str, np.ndarray], - num_samples: int, - ) -> np.ndarray: - """ - Build the actions array (N, n_actions) from keys in actions_to_extract. - - Each key's raw recorded value is used as-is (no per-name transform); - normalization is applied per-column only when that stream's own - `output.streams` entry configures `normalize`/`limits`. - """ - actions_to_extract = self.data_config.get("actions_to_extract", []) - if not actions_to_extract: - n_actions = _infer_n_actions(self.data_config) - return np.zeros((num_samples, n_actions), dtype=np.float32) - - delta_actions = self.data_config.get("delta_actions", []) - stream_configs = self.data_config.get("output", {}).get("streams", {}) - action_parts: List[np.ndarray] = [] - - for key in actions_to_extract: - # A missing action column would silently change the action size (or train on zeros), - # so fail the demo instead of skipping the key. - path = self.resolve_path(key) - if not path: - raise ValueError(f"actions_to_extract key '{key}' not found in bridge_map") - - data = self._get_nested_data(input_data, path) - if data is None: - raise ValueError(f"actions_to_extract key '{key}': path '{path}' not found in data") - - raw_data = np.array(data, dtype=np.float32) - if len(raw_data) == 0: - raise ValueError(f"actions_to_extract key '{key}': dataset '{path}' is empty") - - _check_length(raw_data, num_samples, key) - action_parts.append(_action_part(raw_data, key, stream_configs, delta_actions)) - - actions = np.concatenate(action_parts, axis=1) - - return actions - - # ------------------------------------------------------------------ - # Rewards / dones / states - # ------------------------------------------------------------------ + return self.bridge_map.get(logical_key) # ------------------------------------------------------------------ # HDF5 nested data reader @@ -432,19 +306,17 @@ def __init__( self._shape_cache: Dict[str, Tuple[Tuple[int, ...], Any]] = {} # Short human-readable reason for the last failed conversion (shown by the GUI). self.last_error: Optional[str] = None - # Per-format conversion options (postprocess_config.yaml). Without one, keys - # fall back to their legacy place in data_config (with a deprecation warning). + # Per-format conversion options (postprocess_config.yaml); defaults without one. self.options: PostprocessOptions = PostprocessOptions() - # Image next_obs are only used by robomimic; LeRobot conversion switches this off - # because a full copy of every camera stream is expensive for large frames. - self._need_next_obs: bool = True + # Demos of the last conversion: how many were written, and "file::demo" of each one dropped. + self.converted_demos: int = 0 + self.skipped_demos: List[str] = [] # Bridge-map keys with zero data anywhere in the current batch and no # way to infer a shape for them — excluded from output entirely. self._excluded_keys: Set[str] = set() - # Per-batch: logical_key -> "rgb"|"depth"|"scan", from data_config's - # `input` section's `modality` field. Only keys with an EXPLICIT - # modality are present here — plain rgb (the common case) is instead - # detected from the real recorded dtype (uint8). + # Per-batch: output key -> "rgb"|"depth"|"scan", from the `modality` of the + # input it reads. Only keys with an EXPLICIT modality are present here — + # plain rgb (the common case) is instead detected from the real recorded dtype (uint8). self._modality_map: Dict[str, str] = {} # ------------------------------------------------------------------ @@ -488,20 +360,33 @@ def _prepare_batch( (zero-fill widths, excluded keys). Returns (engine, sorted_files), or None if the format config is missing. """ - fmt_config = self.config_loader.get_config(format_name) - if not fmt_config: - logger.error(f"Format config not found: '{format_name}'") + if format_name not in FORMAT_SUFFIX: + self.last_error = f"Unknown format: '{format_name}'" + logger.error(self.last_error) return None if data_config: self.data_config = data_config + if removed := removed_key_errors(self.data_config): + self.last_error = " ".join(removed) + logger.error(self.last_error) + return None + if not self.data_config.get("actions_to_extract"): + self.last_error = "actions_to_extract is empty: the config names no stream to use as the action." + logger.error(self.last_error) + return None + for name, entry in (self.data_config.get("output", {}).get("streams") or {}).items(): + if msg := normalization_error(name, entry): + self.last_error = msg + logger.error(msg) + return None - sorted_files = sorted(input_files, key=natural_key) + sorted_files = _recordings(input_files) # bridge_map/required_observations depend only on data_config, which is # identical for every demo in this batch — build the engine once. engine = DataTransformEngine(self.data_config) - self._modality_map = self._build_modality_map() + self._modality_map = explicit_modalities(self.data_config) # Real-data shape/dtype cache: for every observation the config asks # for, find the first demo in the whole batch where it actually @@ -512,30 +397,48 @@ def _prepare_batch( self._shape_cache, unresolved = self._prescan_shapes( sorted_files, engine, required_keys ) - self._excluded_keys = set() + if missing := sorted(k for k in unresolved if k in streams_cfg): + # a config these recordings were not made with: every demo would get zero columns + have, configs = self._recorded_streams(sorted_files) + self.last_error = ( + f"Configured stream(s) {', '.join(missing)} are in none of the selected recordings; " + f"they have {', '.join(sorted(have)) or 'no streams'}" + + (f" (recorded with {', '.join(sorted(configs))})" if configs else "") + + ". Load the config these demos were recorded with." + ) + logger.error(self.last_error) + return None + self._excluded_keys = set(unresolved) for key in sorted(unresolved): - if key in streams_cfg: - width = _stream_output_width(self.data_config, key) or 1 - self._shape_cache[key] = ((width,), np.float32) - logger.warning( - f"Stream '{key}' has no recorded data anywhere in this batch; " - f"zero-filling every demo using its configured spec width ({width})." - ) - else: - self._excluded_keys.add(key) - logger.error( - f"Observation '{key}' has no recorded data anywhere in this batch " - f"and no inferable width; excluding it from the converted dataset." - ) + logger.error( + f"Observation '{key}' has no recorded data anywhere in this batch " + f"and no inferable width; excluding it from the converted dataset." + ) return engine, sorted_files + def _recorded_streams(self, files: List[str]) -> Tuple[Set[str], Set[str]]: + """Stream names and recorded-with config names found in these recordings (for the mismatch message).""" + streams: Set[str] = set() + configs: Set[str] = set() + for path in files: + try: + with h5py.File(path, "r") as f: + for name in self._find_demo_groups(f): + demo = f["data"][name] + if isinstance(demo.get("streams"), h5py.Group): + streams.update(demo["streams"].keys()) + if config := _demo_metadata(demo)[1]: + configs.add(config) + except OSError: + continue + return streams, configs + def convert_multiple_files( self, format_name: str, input_files: List[str], output_path: str, - env_metadata: Optional[Dict] = None, data_config: Optional[Dict] = None, ) -> bool: """ @@ -545,7 +448,6 @@ def convert_multiple_files( format_name: Target format key (e.g. "robomimic") input_files: List of input HDF5 file paths output_path: Destination HDF5 path - env_metadata: Optional metadata override dict data_config: Optional data_config override (the `config` sub-dict) Returns: @@ -554,6 +456,7 @@ def convert_multiple_files( dropped and the remaining ones are still converted. """ self.last_error = None + self.converted_demos, self.skipped_demos = 0, [] if format_name == "lerobot": return self.convert_to_lerobot( input_files, output_path, data_config=data_config @@ -563,11 +466,7 @@ def convert_multiple_files( if prepared is None: return False engine, sorted_files = prepared - - # Metadata: prefer explicit arg, then postprocess_config.yaml, then the legacy - # `metadata` key of data_config, then empty - meta = env_metadata if env_metadata else self.options.robomimic_metadata(self.data_config) - env_args = self._build_env_args(meta) + freq = float(self.data_config.get("recording_frequency") or 0) # required by the config check; 0 = no rate check os.makedirs(os.path.dirname(os.path.abspath(output_path)), exist_ok=True) @@ -595,6 +494,11 @@ def convert_multiple_files( if not isinstance(demo_data, h5py.Group): continue + if reason := self._demo_problem(demo_data, engine, freq): + logger.error(f"Skipped {file_path}::{raw_demo_name}: {reason}") + self.skipped_demos.append(f"{file_path}::{raw_demo_name}") + continue + out_name = f"demo_{counter}" converted = self._convert_single_demo( demo_data, engine, out_name @@ -610,6 +514,7 @@ def convert_multiple_files( if out_name in data_grp: del data_grp[out_name] logger.error(f"Skipped {file_path}::{raw_demo_name}: {e}") + self.skipped_demos.append(f"{file_path}::{raw_demo_name}") continue processed.append(out_name) total_demos += converted["num_samples"] @@ -622,6 +527,7 @@ def convert_multiple_files( logger.warning( f"Skipped {file_path}::{raw_demo_name} (conversion failed)" ) + self.skipped_demos.append(f"{file_path}::{raw_demo_name}") except Exception as e: fname = os.path.basename(file_path) @@ -638,14 +544,11 @@ def convert_multiple_files( # Robomimic requires `total` = number of demos (not total timesteps) data_grp.attrs["total"] = counter # env_args MUST be a JSON string (Robomimic ObsUtils requirement) + env_args = {**ENV_ARGS, "env_kwargs": {"control_freq": freq}} if freq > 0 else ENV_ARGS data_grp.attrs["env_args"] = json.dumps(env_args) - mask_grp = out_f.create_group("mask") - splits = self.options.splits(self.data_config) - train_frac = splits["train_frac"] - val_frac = splits["val_frac"] - test_frac = splits["test_frac"] - if not self._create_data_mask(mask_grp, processed, train=train_frac, val=val_frac, test=test_frac): + splits = self.options.splits() + if not self._create_data_mask(out_f.create_group("mask"), processed, splits["train_frac"], splits["val_frac"]): self._discard_output(output_path) return False @@ -653,6 +556,7 @@ def convert_multiple_files( logger.warning( f"Skipped {len(skipped)} file(s) (corrupted or unreadable): {skipped}" ) + self.converted_demos = counter logger.info( f"Successfully wrote {counter} demos ({total_demos} timesteps) -> {output_path}" ) @@ -688,7 +592,7 @@ def convert_to_lerobot( Convert recorded HDF5 files into a LeRobotDataset v3.0 directory. Uses the same acquisition config as the robomimic conversion: - - actions <- `actions_to_extract` (as in generate_actions) + - actions <- `actions_to_extract` (as for robomimic) - observation.state <- numeric `output.streams`, concatenated in declaration order (or `lerobot.state_streams`) - observation.images. <- uint8 image obs from bridge-map entries @@ -704,14 +608,15 @@ def convert_to_lerobot( from .lerobot_writer import ( LeRobotDatasetWriter, LeRobotDependencyError, + LeRobotFatalError, LeRobotWriterError, check_dependencies, ) self.last_error = None + self.converted_demos, self.skipped_demos = 0, [] prepared = self._prepare_batch("lerobot", input_files, data_config) if prepared is None: - self.last_error = "The 'lerobot' format config was not found." return False engine, sorted_files = prepared @@ -746,33 +651,15 @@ def _is_recorded_camera(key: str) -> bool: logger.error(self.last_error) return False - lerobot_cfg = self.options.lerobot(self.data_config) + lerobot_cfg = self.options.lerobot() fps = self._lerobot_fps() if fps is None: - self.last_error = ( - "A positive 'recording_frequency' is required in the config (used as the fps)." - ) + self.last_error = FPS_REQUIRED return False - task_text = ( - task - or lerobot_cfg.get("task") - or self.data_config.get("dataset_name") - or "unspecified task" - ) - - streams_cfg = self.data_config.get("output", {}).get("streams", {}) - for name, entry in streams_cfg.items(): - _, normalize, _, _, _ = _parse_stream_entry(entry) - if normalize: - logger.warning( - f"Stream '{name}' has normalize=true: the LeRobot dataset will contain " - f"normalized values. LeRobot computes its own stats (meta/stats.json) and " - f"normalizes at training time, so normalize is usually left off." - ) + task_text = self._lerobot_task(task) written = 0 skipped: List[str] = [] - self._need_next_obs = False try: with LeRobotDatasetWriter( output_dir, fps=fps, robot_type=lerobot_cfg.get("robot_type"), @@ -798,14 +685,9 @@ def _is_recorded_camera(key: str) -> bool: demo_data = in_data_grp.get(raw_demo_name) if not isinstance(demo_data, h5py.Group): continue - ts = demo_data.get("timestamps") - rate = median_rate(ts[:]) if isinstance(ts, h5py.Dataset) else None - if rate is not None and abs(rate - fps) > RATE_TOLERANCE * fps: - # fps is the episode's time base: a policy trained on it would move rate/fps faster - logger.error( - f"Skipped {file_path}::{raw_demo_name}: recorded at {rate:.1f} Hz, but the dataset " - f"fps is {fps}. Set recording_frequency to a rate this PC keeps up with and record again." - ) + if reason := self._demo_problem(demo_data, engine, fps): + logger.error(f"Skipped {file_path}::{raw_demo_name}: {reason}") + self.skipped_demos.append(f"{file_path}::{raw_demo_name}") continue demo = self._convert_single_demo( demo_data, engine, f"lerobot_ep_{written}" @@ -814,6 +696,7 @@ def _is_recorded_camera(key: str) -> bool: logger.warning( f"Skipped {file_path}::{raw_demo_name} (conversion failed)" ) + self.skipped_demos.append(f"{file_path}::{raw_demo_name}") continue try: state, state_names, images = self._lerobot_split_obs( @@ -829,19 +712,20 @@ def _is_recorded_camera(key: str) -> bool: demo["actions"].shape[1] ), ) - except LeRobotDependencyError: - raise # fatal: every episode would fail the same way + except LeRobotFatalError: + raise # missing library or aborted dataset: every later episode would be wrong too except LeRobotWriterError as e: logger.error( f"Skipped {file_path}::{raw_demo_name}: {e}" ) + self.skipped_demos.append(f"{file_path}::{raw_demo_name}") continue written += 1 logger.info( f"Converted {file_path}::{raw_demo_name} -> " f"episode {written - 1} ({demo['num_samples']} frames)" ) - except LeRobotDependencyError: + except LeRobotFatalError: raise except Exception as e: # skip a broken file: reaching the writer would delete the whole output fname = os.path.basename(file_path) @@ -856,6 +740,7 @@ def _is_recorded_camera(key: str) -> bool: logger.warning( f"Skipped {len(skipped)} file(s) (corrupted or unreadable): {skipped}" ) + self.converted_demos = written logger.info(f"Successfully wrote {written} episodes -> {output_dir}") return True @@ -868,14 +753,173 @@ def _is_recorded_camera(key: str) -> bool: logger.error(f"Fatal error during LeRobot conversion: {e}") traceback.print_exc() return False - finally: - self._need_next_obs = True + + def conversion_summary(self) -> str: + """One line for the GUI / CLI: how many demos of the last conversion were written.""" + total = self.converted_demos + len(self.skipped_demos) + text = f"Converted {self.converted_demos} of {total} demos" + return text + (f" ({len(self.skipped_demos)} skipped, see log)" if self.skipped_demos else "") + + # ------------------------------------------------------------------ + # Pre-check: what a conversion would do, nothing written + # ------------------------------------------------------------------ + + def precheck(self, format_name: str, input_files: List[str]) -> Dict[str, Any]: + """ + What converting ``input_files`` to ``format_name`` would do, without writing anything (no LeRobot/PyAV needed): + the batch error the conversion would stop with, each demo's verdict from the same `_demo_problem` the + conversion runs, and the output layout from the real config + recorded shapes. + """ + self.last_error = None + report: Dict[str, Any] = {"format": format_name, "error": None, "demos": [], "output": [], + "converted": 0, "total": 0} + prepared = self._prepare_batch(format_name, input_files) + engine, files = prepared if prepared else (None, _recordings(input_files)) + freq = float(self.data_config.get("recording_frequency") or 0) + if prepared and format_name == "lerobot": + freq = self._lerobot_fps() or 0 + self.last_error = None if freq else FPS_REQUIRED + elif prepared: + splits = self.options.splits() + self.last_error = _splits_error(splits["train_frac"], splits["val_frac"]) + report["error"] = self.last_error + + for path in files: + if not path.endswith(".h5"): + continue # as the conversion: a skipped file, not a demo + try: + with h5py.File(path, "r") as f: + for name in sorted(self._find_demo_groups(f)): + demo = f["data"][name] + ts = demo.get("timestamps") + ts = ts[:] if isinstance(ts, h5py.Dataset) else None + streams = demo.get("streams") + first = next(iter(streams.values()), None) if isinstance(streams, h5py.Group) else None + recorded_hz, config = _demo_metadata(demo) + if report["error"]: + reason = self._rate_problem(demo, freq) or "not converted (batch error)" + else: + reason = self._demo_problem(demo, engine, freq) + report["demos"].append({ + "file": path, "demo": name, + "samples": len(ts) if ts is not None else (len(first) if first is not None else 0), + "rate_hz": median_rate(ts) if ts is not None else None, + "recorded_with": config, "recorded_hz": recorded_hz, + "ok": not reason, "reason": reason, + }) + except OSError as e: + logger.warning(f"Pre-check: could not read {path}: {e}") + + report["total"] = len(report["demos"]) + report["converted"] = sum(d["ok"] for d in report["demos"]) + if not report["error"]: + report["output"] = self._output_preview(format_name, engine, report["converted"], freq) + return report + + def _output_preview(self, format_name: str, engine: DataTransformEngine, demos: int, freq: float) -> List[List[str]]: + """[name, detail] rows of what the conversion writes, from the shape cache of _prepare_batch.""" + streams_cfg = self.data_config.get("output", {}).get("streams", {}) + obs: Dict[str, Tuple[Tuple[int, ...], np.dtype]] = {} # key -> (feature shape, dtype) as written + for key, entry in streams_cfg.items(): + shape = self._shape_cache[key][0] + obs[key] = ((3,) if _parse_stream_entry(entry)[1] == "rpy" else shape, np.dtype(np.float32)) + for key in sorted(engine.bridge_map): + if key not in self._excluded_keys and key not in obs: + shape, dtype = self._shape_cache[key] + kept = np.dtype(dtype) == np.uint8 or key in self._modality_map # else read as float32 + obs[key] = (shape, np.dtype(dtype) if kept else np.dtype(np.float32)) + widths = [(k, (obs[k][0] or (1,))[0] if k in obs else 0) for k in self.data_config.get("actions_to_extract", [])] + dim = sum(w for _, w in widths) + cols, start = [], 0 + for key, w in widths: + cols.append(f"{key}[{start}]" if w == 1 else f"{key}[{start}:{start + w}]") + start += w + actions = f"({dim}) = " + ", ".join(cols) + + if format_name == "lerobot": + from .lerobot_writer import LeRobotWriterError + + cfg = self.options.lerobot() + fake = {"num_samples": 1, "obs": {k: np.zeros((1,) + s, d) for k, (s, d) in obs.items()}} + try: # the converter's own split, on one zero row per key + _, state_names, images = self._lerobot_split_obs(fake, cfg.get("state_streams")) + state = _names_text(state_names) + except LeRobotWriterError as e: + state, images = str(e), {} + action_names = self._lerobot_action_names(dim) + return ([["observation.state", state]] + + [[k, f"video 3x{a.shape[1]}x{a.shape[2]}"] for k, a in images.items()] + + [["action", _names_text(action_names) if action_names else actions], ["fps", f"{freq:g}"], + ["task", self._lerobot_task()], ["robot_type", str(cfg.get("robot_type") or "-")], + ["episodes", str(demos)]]) + + rows = [[f"data/demo_N/obs/{k}", f"(N{''.join(f', {x}' for x in s)}) {d}"] for k, (s, d) in obs.items()] + splits = self.options.splits() + train, val = _split_counts(demos, splits["train_frac"], splits["val_frac"]) + return rows + [ + ["data/demo_N/actions", actions.replace("(", "(N, ", 1)], + ["data/demo_N/rewards, dones", "(N,) float32, 1 at the last step"], + ["mask/train, mask/valid", + f"{train} / {val} demos (train_frac {splits['train_frac']:g}, val_frac {splits['val_frac']:g})"], + ["env_args", f"control_freq {freq:g}" if freq else "no control_freq"], + ] + + @staticmethod + def _rate_problem(demo_data: h5py.Group, freq: float) -> str: + """Why the demo's rate is unusable ("" if fine): more than RATE_TOLERANCE off ``freq`` means the policy, run at + ``freq``, would move rate/freq faster than shown. A demo without timestamps (or freq 0) is not checked.""" + ts = demo_data.get("timestamps") + rate = median_rate(ts[:]) if freq > 0 and isinstance(ts, h5py.Dataset) else None + if rate is None or abs(rate - freq) <= RATE_TOLERANCE * freq: + return "" + recorded_hz, config = _demo_metadata(demo_data) + if recorded_hz is not None and recorded_hz != freq: + return (f"recorded at {recorded_hz:g} Hz with {config or 'another config'}; the loaded config says " + f"{freq:g} Hz: load the config these demos were recorded with") + return (f"recorded at {rate:.1f} Hz, but recording_frequency is {freq:g}: this PC could not keep up. " + f"Set recording_frequency to a rate this PC keeps up with and record again.") + + def _demo_problem(self, demo_data: h5py.Group, engine: DataTransformEngine, freq: float) -> str: + """Why this demo would be skipped, "" if it converts. The one set of per-demo rules, used by the conversion + loops and by precheck(); reads dataset shapes only (and the timestamps), never the arrays.""" + if reason := self._rate_problem(demo_data, freq): + return reason + streams_grp = demo_data.get("streams") + if not isinstance(streams_grp, h5py.Group): + return "no 'streams' group (not recorded by Relay-D?)" + streams_cfg = self.data_config.get("output", {}).get("streams", {}) + present = [k for k in streams_cfg if k in streams_grp] + if not present or not len(streams_grp[present[0]]): + return "no data in the configured streams" + n = len(streams_grp[present[0]]) + actions = self.data_config.get("actions_to_extract", []) + for key in actions: + if key not in streams_grp: # zero actions would train on zeros: the demo is skipped + return f"no {key} (action stream)" + try: # the recorder writes one row per timestamp: another length means lost or extra frames + for key in dict.fromkeys(present + list(actions)): + _check_length(streams_grp[key], n, key) + for key in sorted(engine.bridge_map): + if key in self._excluded_keys or key in streams_cfg: + continue + ds = engine._get_nested_data(demo_data, engine.resolve_path(key) or "") + if ds is not None: + _check_length(ds, n, key) + continue + shape, dtype = self._shape_cache[key] + if np.dtype(dtype) == np.uint8 or len(shape) >= 2 or key in self._modality_map: + # black frames would teach the policy to act blind (a missing numeric obs is zero-filled) + return f"no {'point cloud' if self._modality_map.get(key) == 'scan' else 'camera'} {key}" + except ValueError as e: + return str(e) + return "" + + def _lerobot_task(self, task: Optional[str] = None) -> str: + return task or self.options.lerobot().get("task") or self.data_config.get("dataset_name") or "unspecified task" def _lerobot_fps(self) -> Optional[int]: - """fps from `recording_frequency` (top level or under `metadata`).""" + """fps from `recording_frequency`.""" raw = self.data_config.get("recording_frequency") - if raw is None: - raw = (self.data_config.get("metadata") or {}).get("recording_frequency") try: value = float(raw) except (TypeError, ValueError): @@ -969,7 +1013,7 @@ def _convert_single_demo_from_streams( """ Stream-based conversion path. Reads pre-computed stream datasets from raw HDF5 `streams/` group and - maps them directly to obs/next_obs. Actions are assembled from + maps them directly to obs. Actions are assembled from `actions_to_extract` stream names. """ try: @@ -996,32 +1040,17 @@ def _convert_single_demo_from_streams( demo: Dict = { "num_samples": num_samples, "obs": {}, - "next_obs": {}, "actions": None, "rewards": None, "dones": None, - "states": None, } stream_configs = self.data_config.get("output", {}).get("streams", {}) for stream_name in stream_names: - _, normalize, norm_range, limits, rot_fmt = _parse_stream_entry( - stream_configs.get(stream_name, []) - ) + _, rot_fmt = _parse_stream_entry(stream_configs.get(stream_name, [])) if stream_name in streams_grp: - dataset = streams_grp[stream_name] - # Defensive guard: a stream should never legitimately carry - # uint8 image data (output.streams is numeric-only — see - # StreamBuilder), but if a misconfigured spec ever put one - # here, preserve its dtype instead of corrupting it with an - # unconditional float32 cast + rotation/normalization math. - is_image = dataset.dtype == np.uint8 - data = np.array(dataset) if is_image else np.array(dataset, dtype=np.float32) - data = data[:num_samples] - if not is_image: - data = _apply_rotation_format(data, rot_fmt, stream_name) - if normalize and limits: - data = _apply_normalization(data, limits, norm_range) + data = np.array(streams_grp[stream_name], dtype=np.float32) + data = _apply_rotation_format(data, rot_fmt, stream_name) else: cached = self._shape_cache.get(stream_name) if cached is None: @@ -1042,38 +1071,19 @@ def _convert_single_demo_from_streams( data = np.zeros((num_samples,) + feat_shape, dtype=dtype) demo["obs"][stream_name] = data - demo["next_obs"][stream_name] = _next_obs(data) actions_to_extract = self.data_config.get("actions_to_extract", []) delta_actions = self.data_config.get("delta_actions", []) action_parts = [] - for stream_name in actions_to_extract: - if stream_name in streams_grp: - part = np.array(streams_grp[stream_name], dtype=np.float32)[:num_samples] - action_parts.append(_action_part(part, stream_name, stream_configs, delta_actions)) - elif stream_name in demo["obs"]: - # Already zero-filled above from the shape cache. - part = demo["obs"][stream_name] - if part.ndim == 1: - part = part[:, np.newaxis] - action_parts.append(part) - else: - # A missing action column would silently change the action size: fail the demo. - raise ValueError( - f"actions_to_extract: stream '{stream_name}' not found in {demo_name} " - f"(no data anywhere in the batch)" - ) + for stream_name in actions_to_extract: # present and the right length: checked by _demo_problem + part = np.array(streams_grp[stream_name], dtype=np.float32) + action_parts.append(_action_part(part, stream_name, stream_configs, delta_actions)) action_parts = [p for p in action_parts if p.ndim < 2 or p.shape[1] > 0] - if action_parts: - demo["actions"] = np.concatenate(action_parts, axis=1) - else: - n_actions = _infer_n_actions(self.data_config) - demo["actions"] = np.zeros((num_samples, n_actions), dtype=np.float32) + demo["actions"] = np.concatenate(action_parts, axis=1) # all zero-width: ValueError, the demo fails demo["rewards"] = _end_flags(num_samples) demo["dones"] = _end_flags(num_samples) - demo["states"] = _stack_states(demo["obs"], num_samples) logger.info( f"Stream-based conversion: {demo_name}, " @@ -1084,7 +1094,6 @@ def _convert_single_demo_from_streams( except Exception as e: logger.error(f"Error in stream-based conversion of '{demo_name}': {e}") - import traceback traceback.print_exc() return None @@ -1097,15 +1106,13 @@ def _populate_bridge_obs( demo_name: str, ) -> None: """ - Populate demo['obs']/['next_obs'] for every bridge-map key (top-level + Populate demo['obs'] for every bridge-map key (top-level `output:` entries other than `streams`) not already present in demo['obs']. - This is how Image / organized PointCloud2 data reaches obs/next_obs: + This is how Image / organized PointCloud2 data reaches obs: those types can't be expressed as output.streams specs (numeric-only — see StreamBuilder), so they're referenced directly by their raw HDF5 - path instead, e.g. `output: agentview_image: camera_rgb/images`. Called - from both branches of `_convert_single_demo` so bridge-map obs can - coexist with (or stand in for) output.streams-based obs. + path instead, e.g. `output: agentview_image: camera_rgb/images`. """ required_keys = sorted( k for k in engine.bridge_map @@ -1132,10 +1139,7 @@ def _populate_bridge_obs( is_image = dataset.dtype == np.uint8 or obs_key in self._modality_map raw = np.array(dataset) if is_image else np.array(dataset, dtype=np.float32) - _check_length(raw, num_samples, obs_key) demo["obs"][obs_key] = raw - if self._need_next_obs: - demo["next_obs"][obs_key] = _next_obs(raw) continue cached = self._shape_cache.get(obs_key) @@ -1145,11 +1149,9 @@ def _populate_bridge_obs( f"shape anywhere in the batch; skipping." ) continue - feat_shape, dtype = cached - fill = np.zeros((num_samples,) + feat_shape, dtype=dtype) - demo["obs"][obs_key] = fill - if self._need_next_obs: - demo["next_obs"][obs_key] = fill.copy() + feat_shape, dtype = cached # a missing camera / point cloud skipped the demo in _demo_problem + logger.warning(f"'{obs_key}' not found in {demo_name}; zero-filling with shape {feat_shape}.") + demo["obs"][obs_key] = np.zeros((num_samples,) + feat_shape, dtype=dtype) def _convert_single_demo( self, @@ -1160,62 +1162,18 @@ def _convert_single_demo( """ Convert one demo group to an in-memory dict ready for HDF5 writing. - Steps: - 1. If a `streams/` group is present, start from stream-based - conversion (numeric obs from output.streams). - 2. Always merge in bridge-map obs (top-level `output:` entries other - than `streams`) — this is how Image / organized PointCloud2 data - reaches obs/next_obs, and it coexists with output.streams rather - than being mutually exclusive with it. - 3. If there was no `streams/` group at all, determine master - timestep count N via the bridge map instead, then generate - actions, rewards, dones, states as usual. + Numeric obs and actions come from the `streams/` group, then bridge-map obs + (cameras / point clouds) are merged in. Only for a demo `_demo_problem` passed. """ - if "streams" in demo_data: - demo = self._convert_single_demo_from_streams(demo_data, demo_name) - if demo is None: - return None - try: - self._populate_bridge_obs( - demo_data, engine, demo, demo["num_samples"], demo_name - ) - except ValueError as e: # one bad demo is skipped, not the rest of its file - logger.error(f"Error converting demo '{demo_name}': {e}") - return None - return demo - + demo = self._convert_single_demo_from_streams(demo_data, demo_name) + if demo is None: + return None try: - num_samples = self._get_master_timesteps(demo_data, engine) - if num_samples is None or num_samples == 0: - logger.warning(f"Could not determine timesteps for {demo_name}") - return None - - demo: Dict[str, Any] = { - "num_samples": num_samples, - "obs": {}, - "next_obs": {}, - "actions": None, - "rewards": None, - "dones": None, - "states": None, - } - - self._populate_bridge_obs(demo_data, engine, demo, num_samples, demo_name) - - # --- Actions, rewards, dones, states --- - demo["actions"] = engine.generate_actions( - demo_data, demo["obs"], num_samples - ) - demo["rewards"] = _end_flags(num_samples) - demo["dones"] = _end_flags(num_samples) - demo["states"] = _stack_states(demo["obs"], num_samples) - - return demo - - except Exception as e: + self._populate_bridge_obs(demo_data, engine, demo, demo["num_samples"], demo_name) + except ValueError as e: # one bad demo is skipped, not the rest of its file logger.error(f"Error converting demo '{demo_name}': {e}") - traceback.print_exc() return None + return demo # ------------------------------------------------------------------ # HDF5 write helpers @@ -1239,49 +1197,33 @@ def _write_demo_to_output( dg.attrs["model_file"] = "" # Required by some Robomimic loaders # Top-level datasets - for ds_name in ("states", "actions", "rewards", "dones"): + for ds_name in ("actions", "rewards", "dones"): arr = demo[ds_name].astype(np.float32) dg.create_dataset(ds_name, data=arr, **user_settings.h5_compression()) - # obs and next_obs - for group_name in ("obs", "next_obs"): - grp = dg.create_group(group_name) - for key, data in demo[group_name].items(): - is_image = data.dtype == np.uint8 or key in self._modality_map + # obs (no next_obs: robomimic only loads it with hdf5_load_next_obs, false in every shipped config) + grp = dg.create_group("obs") + for key, data in demo["obs"].items(): + is_image = data.dtype == np.uint8 or key in self._modality_map - kwargs: Dict[str, Any] = {**user_settings.h5_compression(), "dtype": data.dtype} - if is_image and data.ndim >= 3: - # Per-frame chunking for efficient sequential reads during training - kwargs["chunks"] = (1,) + data.shape[1:] + kwargs: Dict[str, Any] = {**user_settings.h5_compression(), "dtype": data.dtype} + if is_image and data.ndim >= 3: + # Per-frame chunking for efficient sequential reads during training + kwargs["chunks"] = (1,) + data.shape[1:] - grp.create_dataset(key, data=data, **kwargs) + grp.create_dataset(key, data=data, **kwargs) - def _create_data_mask(self, mask_group: h5py.Group, demo_names: List[str], train: float, val: float, test: float): + def _create_data_mask(self, mask_group: h5py.Group, demo_names: List[str], train: float, val: float): + """mask/train and mask/valid; demos left over by rounding or train + val < 1 also train.""" vlen_str_type = h5py.special_dtype(vlen=str) - for name, frac in (("train", train), ("val", val), ("test", test)): - if not isinstance(frac, (int, float)) or isinstance(frac, bool) or not 0 <= frac <= 1: - self.last_error = f"robomimic.splits: '{name}' must be a number between 0 and 1, got {frac!r}" - logger.error(self.last_error) - return False - if train == 0: # every robomimic template trains on the "train" mask: no mask, no training - self.last_error = "robomimic.splits: 'train' must be more than 0" - logger.error(self.last_error) - return False - if train + val > 1 + 1e-9: - self.last_error = f"robomimic.splits: train ({train}) + val ({val}) is more than 1" + if error := _splits_error(train, val): + self.last_error = error logger.error(self.last_error) return False sorted_demo_names = sorted(demo_names, key=natural_key) - total_demos = len(sorted_demo_names) - - # At least one training demo when training is asked for (round() would give 0 for few demos). - train_count = max(1, round(total_demos * train)) if train > 0 and total_demos else 0 - val_count = min(round(total_demos * val), total_demos - train_count) - test_count = total_demos - train_count - val_count - if not test and train_count: # no eval split asked for: rounding leftovers train instead of sitting unused in "eval" - train_count, test_count = train_count + test_count, 0 + train_count, val_count = _split_counts(len(sorted_demo_names), train, val) if(train_count != 0): mask_group.create_dataset( @@ -1297,13 +1239,6 @@ def _create_data_mask(self, mask_group: h5py.Group, demo_names: List[str], train dtype=vlen_str_type, ) - if(test_count != 0): - mask_group.create_dataset( - "eval", - data=[n.encode("utf-8") for n in sorted_demo_names[train_count + val_count:]], - dtype=vlen_str_type, - ) - return True # ------------------------------------------------------------------ @@ -1311,7 +1246,7 @@ def _create_data_mask(self, mask_group: h5py.Group, demo_names: List[str], train # ------------------------------------------------------------------ def _find_demo_groups(self, input_file: h5py.File) -> List[str]: - """Return all group names directly under /data that start with 'demo_'.""" + """Return the names of all groups directly under /data (any name, not only 'demo_*').""" groups: List[str] = [] data_grp = input_file.get("data") if isinstance(data_grp, h5py.Group): @@ -1320,102 +1255,6 @@ def _find_demo_groups(self, input_file: h5py.File) -> List[str]: groups.append(key) return groups - def _get_master_timesteps( - self, - demo_data: h5py.Group, - engine: DataTransformEngine, - ) -> Optional[int]: - """ - Determine N (number of timesteps) for a demo group. - - Priority: - 1. Explicit 'timestamps' dataset at demo root - 2. detailed_timestamps/recording_times - 3. Primary observation dataset via bridge map (joint_positions) - 4. Any first dataset found by scanning sub-groups - """ - # 1. timestamps - if "timestamps" in demo_data: - ts = demo_data["timestamps"] - if isinstance(ts, h5py.Dataset) and ts.shape[0] > 0: - return ts.shape[0] - - # 2. detailed_timestamps - dt = demo_data.get("detailed_timestamps") - if isinstance(dt, h5py.Group): - rt = dt.get("recording_times") - if isinstance(rt, h5py.Dataset) and rt.shape[0] > 0: - return rt.shape[0] - - # 3. Primary obs from bridge map (dynamically discovered) - joint_key = engine._discover_joint_key() - eef_key = engine._discover_eef_key() - for primary_key in (joint_key, eef_key): - if not primary_key: - continue - path = engine.resolve_path(primary_key) - if path: - ds = engine._get_nested_data(demo_data, path) - if isinstance(ds, h5py.Dataset) and ds.shape[0] > 0: - return ds.shape[0] - - # 4. Scan any dataset in any sub-group - for key in demo_data.keys(): - item = demo_data[key] - if isinstance(item, h5py.Group): - for sub_key in item.keys(): - sub = item[sub_key] - if ( - isinstance(sub, h5py.Dataset) - and len(sub.shape) >= 1 - and sub.shape[0] > 0 - ): - return sub.shape[0] - elif isinstance(item, h5py.Dataset) and item.shape[0] > 0: - return item.shape[0] - - return None - - def _build_env_args(self, metadata: Dict) -> Dict: - """ - Build the env_args dictionary that will be JSON-serialised into - /data.attrs['env_args']. - - Structure required by Robomimic ObsUtils: - { - "env_name": str, - "type": int, (Robomimic env-family enum, e.g. 1 = robosuite) - "env_kwargs": { ... } - } - """ - return { - "env_name": metadata.get("env_name", "LFD_Demo_Environment"), - "env_version": metadata.get("env_version", ""), - "type": metadata.get("type", 1), - "env_kwargs": metadata.get("env_kwargs", {}), - } - - def _build_modality_map(self) -> Dict[str, str]: - """ - Build logical_key -> "rgb"|"depth"|"scan" from data_config's `input` - section's `modality` field. Only inputs with an explicit modality are - included — implicit rgb (the common case) is instead detected from - the real recorded dtype (uint8) wherever it's checked. - """ - modality_map: Dict[str, str] = {} - for input_name, spec in self.data_config.get("input", {}).items(): - if not isinstance(spec, dict): - continue - modality = spec.get("modality") - if modality in ("rgb", "depth", "scan"): - key = spec.get("output_map") or input_name - modality_map[key] = modality - return modality_map - - def get_last_shape_cache(self) -> Dict[str, Tuple[Tuple[int, ...], Any]]: - """Real (feature_shape, dtype) per key from the last convert_multiple_files call.""" - return dict(self._shape_cache) - # ------------------------------------------------------------------ # Real-data shape/dtype inference # ------------------------------------------------------------------ diff --git a/relay_d/acquisition/qt_app/ui/record/container_manager.py b/relay_d/acquisition/qt_app/ui/record/container_manager.py index ed817a8..5c344c8 100644 --- a/relay_d/acquisition/qt_app/ui/record/container_manager.py +++ b/relay_d/acquisition/qt_app/ui/record/container_manager.py @@ -76,7 +76,6 @@ def create_containers(self): "input_name": input_name, "topic_type_msg": None, # Will be auto-detected from topic "base_type": base_type, - "output_map": input_item.get("output_map", input_name), } # Check if this is a TF lookup configuration @@ -90,14 +89,11 @@ def create_containers(self): container_data["tf_wait_timeout"] = input_item.get( "tf_wait_timeout" ) - container_data["transfer_rate"] = input_item.get( - "transfer_rate", 10 - ) container_data["topic_path"] = None # No topic for TF lookup # For TF, use 'pose' or 'tf' as the container type container_topic_type = "pose" logger.info( - f"TF lookup config: {input_name} - {input_item.get('parent_frame')} -> {input_item.get('child_frame')} -> {input_item.get('output_map')}" + f"TF lookup config: {input_name} - {input_item.get('parent_frame')} -> {input_item.get('child_frame')}" ) # Add topic path for regular topic subscriptions elif "topic" in input_item: @@ -117,7 +113,7 @@ def create_containers(self): container_specs.append(container_spec) logger.info( - f"Added container spec: {input_name} -> {base_type} -> output_map: {container_data['output_map']}" + f"Added container spec: {input_name} -> {base_type}" ) logger.info(f"Total container specs prepared: {len(container_specs)}") diff --git a/relay_d/acquisition/qt_app/ui/record/container_visualizer.py b/relay_d/acquisition/qt_app/ui/record/container_visualizer.py index b621eea..aa8c0e4 100644 --- a/relay_d/acquisition/qt_app/ui/record/container_visualizer.py +++ b/relay_d/acquisition/qt_app/ui/record/container_visualizer.py @@ -7,6 +7,7 @@ import math from relay_d.utils.coloring_logger import logger +from relay_d.utils.numerics import stamp_to_sec from ..settings.config_manager import config from sensor_msgs.msg import Image, CompressedImage, PointCloud2, JointState @@ -144,7 +145,7 @@ def update_container_display(self, container, topic_subscribers): ): ros_time = latest_msg.header.stamp if ros_time: - time_sec = ros_time.sec + ros_time.nanosec * 1e-9 + time_sec = stamp_to_sec(ros_time) dt_str = datetime.datetime.fromtimestamp(time_sec).strftime( "%H:%M:%S.%f" )[:-3] diff --git a/relay_d/acquisition/qt_app/ui/record/data_recorder.py b/relay_d/acquisition/qt_app/ui/record/data_recorder.py index 7315a15..08f76d9 100644 --- a/relay_d/acquisition/qt_app/ui/record/data_recorder.py +++ b/relay_d/acquisition/qt_app/ui/record/data_recorder.py @@ -6,7 +6,9 @@ from rclpy.node import Node from relay_d.utils import user_settings from relay_d.utils.coloring_logger import logger -from ...utils.stream_builder import StreamBuilder, fill_none_gaps +from ...utils.stream_builder import StreamBuilder +from relay_d.utils.numerics import inputs_in_sync, stamp_to_sec +from ..config.builder.config_check import RECORDING_CHANGING, removed_keys import time import json from threading import Lock, Thread @@ -19,6 +21,18 @@ def _sample_nbytes(sample: dict) -> int: return sum(v.nbytes for v in sample.values() if isinstance(v, np.ndarray)) +def start_demo(recorder, topic_subscribers, containers, dataset_name, demo_name=None): + """The per-Start steps the GUI and AppAPI share: subscribe every input again (re-detects the message type and QoS, + resets custom TF buffers; a broken custom TF topic raises TfTopicSubscriptionError), then start the recorder (it + flushes the data from before Start and first retries an unsaved demo, see ``recorder.retried_file``). + Returns the demo name, or None if the recording did not start. An input that could not be subscribed only logs: + the recorder holds it empty until data arrives (as the GUI always did).""" + results = topic_subscribers.create_subscribers_for_containers(containers) + logger.info(f"Subscribers created: {sum(results.values())}/{len(results)}") + demo_name = demo_name or f"{dataset_name}_{time.time_ns() // 1_000_000}" # ms: two Starts in one second must not clash ("w-") + return demo_name if recorder.start_recording(topic_subscribers, demo_name) else None + + def _write_stacked(group, name: str, frames: list): """Write equally shaped arrays as one (N, ...) dataset, frame by frame. @@ -46,18 +60,17 @@ def __init__(self, yaml_parser): self.current_demo_number = 0 self.retried_file = None # path of the demo saved by the last retry_save() - self._apply_setting_defaults() # the config file's values win (below, and again at each start_recording) - - # Update from yaml_parser if available - if self.yaml_parser and self.yaml_parser.is_loaded(): - config = self.yaml_parser.yaml_data.get("config", {}) - self.recording_frequency = config.get("recording_frequency", self.recording_frequency) + self.recording_frequency = None # read from the config at each start_recording + self.max_buffer_bytes = int(user_settings.get("rec_default_max_buffer_mb")) * 1024**2 + self._sync_tolerance = None + self._sync_mode = "soft" # shortcut: samples live in RAM until Stop, so a crash loses the demo being recorded; upgrade to a journal or # chunked HDF5 writes if demos get long enough for that to hurt. Past this many bytes of frames the loop stops # sampling (Stop still saves what is buffered) instead of running the PC out of memory. self._buffered_bytes = 0 self._unsaved = False # a save failed and the samples are still only in memory self.buffer_full = False # the loop stopped sampling because max_buffer_bytes was reached + self.loop_failed = False # the loop stopped sampling because of repeated errors self.output_directory = None self.recorded_data_dir = None @@ -69,8 +82,8 @@ def __init__(self, yaml_parser): self._stream_builder = StreamBuilder(self.yaml_parser) self._fill_gaps_containers = set() - self._fill_defaults = {} self._fill_count_summary = {} + self._held_counts = defaultdict(int) # per input: ticks recorded without a fresh sample self._modalities = {} self.topic_subscribers = None @@ -80,59 +93,13 @@ def __init__(self, yaml_parser): self.recording_sample_count = 0 self.is_recording = False - self.is_paused = False self.recording_lock = Lock() self._clock = self.get_clock() logger.info("DataRecorder initialized") - def pause_recording(self): - try: - with self.recording_lock: - if not self.is_recording: - logger.warning("No recording in progress to pause") - return False - - if self.is_paused: - logger.warning("Recording is already paused") - return False - - self.is_paused = True - logger.info("Recording paused") - return True - - except Exception as e: - logger.error(f"Failed to pause recording: {e}") - return False - - def resume_recording(self): - try: - with self.recording_lock: - if not self.is_recording: - logger.warning("No recording in progress to resume") - return False - - if not self.is_paused: - logger.warning("Recording is not paused") - return False - - self.is_paused = False - logger.info("Recording resumed") - return True - - except Exception as e: - logger.error(f"Failed to resume recording: {e}") - return False - - def _apply_setting_defaults(self): - """The ``rec_default_*`` settings as they are now (a change in Settings applies to the next recording).""" - self.recording_frequency = user_settings.get("rec_default_frequency") - self.max_buffer_bytes = int(user_settings.get("rec_default_max_buffer_mb")) * 1024**2 - self._sync_tolerance = user_settings.get("rec_default_sync_tolerance_sec") or None # 0 = off - self._sync_mode = user_settings.get("rec_default_sync_mode") - - def start_recording(self, topic_subscribers, demo_name=None, flush_stale_data=False): + def start_recording(self, topic_subscribers, demo_name=None): try: with self.recording_lock: if self.is_recording: @@ -150,7 +117,7 @@ def start_recording(self, topic_subscribers, demo_name=None, flush_stale_data=Fa return False self.topic_subscribers = topic_subscribers - self._apply_setting_defaults() + self.recording_frequency = None # only the loaded config sets it: no config, no recording if demo_name is None: demo_name = f"demo_{self.current_demo_number}" @@ -173,23 +140,19 @@ def start_recording(self, topic_subscribers, demo_name=None, flush_stale_data=Fa logger.info( f"Loaded metadata from config: {list(metadata_dict.keys())}" ) - # Always re-read recording_frequency so it matches the loaded config, - # not the stale value from __init__ (which ran before config was loaded). config = self.yaml_parser.yaml_data.get("config", {}) - # YAML first, then the user setting (so a config without the key does not inherit the last one) - self.recording_frequency = config.get( - "recording_frequency", user_settings.get("rec_default_frequency") - ) + blocking = [msg for key, msg in removed_keys(config) if key in RECORDING_CHANGING] + if blocking: + logger.error("Cannot start: " + " ".join(blocking)) + return False + self.recording_frequency = config.get("recording_frequency") self.max_buffer_bytes = int( float(config.get("max_buffer_mb") or user_settings.get("rec_default_max_buffer_mb")) * 1024**2 ) self.recording_metadata["recording_frequency"] = self.recording_frequency logger.info(f"Recording frequency: {self.recording_frequency} Hz") - # 0 means off, in the YAML as in the setting - self._sync_tolerance = config.get( - "sync_tolerance_sec", user_settings.get("rec_default_sync_tolerance_sec") - ) or None - self._sync_mode = config.get("sync_mode", user_settings.get("rec_default_sync_mode")) + self._sync_tolerance = config.get("sync_tolerance_sec") or None # missing or 0 = off + self._sync_mode = config.get("sync_mode", "soft") self.recording_metadata["sync_tolerance_sec"] = self._sync_tolerance if self._sync_tolerance is not None: logger.info( @@ -197,9 +160,15 @@ def start_recording(self, topic_subscribers, demo_name=None, flush_stale_data=Fa f"mode={self._sync_mode}" ) + if not self.recording_frequency or self.recording_frequency <= 0: + logger.error(f"Cannot start: recording_frequency must be > 0 Hz, got {self.recording_frequency!r}") + return False + self.recorded_data.clear() + self._held_counts.clear() self._buffered_bytes = 0 self.buffer_full = False + self.loop_failed = False self.timestamps.clear() self.timestamp_spreads = [] self._last_valid_raw = {} @@ -209,39 +178,26 @@ def start_recording(self, topic_subscribers, demo_name=None, flush_stale_data=Fa if self._fill_gaps_containers: logger.info(f"Gap-fill (hold) enabled for: {sorted(self._fill_gaps_containers)}") - if flush_stale_data: - self.topic_subscribers.flush_latest_data() - logger.info("Flushed stale data before recording start") + self.topic_subscribers.flush_latest_data() # samples from before Start are not part of the demo + logger.info("Flushed stale data before recording start") self._create_h5_file(demo_name) - return self._start_standard_recording(demo_name) + self.is_recording = True + self.stop_recording_flag = False + self.recording_thread = Thread( + target=self._standard_recording_loop, + args=(self.topic_subscribers,), + daemon=True, + ) + self.recording_thread.start() + logger.info(f"Started recording session: {demo_name}") + return True except Exception as e: logger.error(f"Failed to start recording: {e}") return False - def _start_standard_recording(self, demo_name): - try: - logger.info("Starting standard (non-synchronized) recording") - - self.is_recording = True - self.stop_recording_flag = False - - self.recording_thread = Thread( - target=self._standard_recording_loop, - args=(self.topic_subscribers,), - daemon=True, - ) - self.recording_thread.start() - - logger.info(f"Started standard recording session: {demo_name}") - return True - - except Exception as e: - logger.error(f"Failed to start standard recording: {e}") - return False - def _standard_recording_loop(self, topic_subscribers): try: logger.info( @@ -273,24 +229,6 @@ def _standard_recording_loop(self, topic_subscribers): else: logger.info("Warmup complete — all inputs have initial data") - # Seed hold defaults for inputs with fill_gaps: true and a fill_default - # value configured. Without seeding, leading Nones accumulate until the - # first message arrives (which defeats gap-fill for the leading period). - for _cid in list(topic_subscribers.subscribers.keys()): - if _cid in self._fill_gaps_containers and _cid not in self._last_valid_raw: - _default = self._fill_defaults.get(_cid) - if _default is None: - continue # no default configured — skip seeding - _status = (topic_subscribers.subscriber_status or {}).get(_cid, {}) - _topic = _status.get("topic", "") - _cls = topic_subscribers.message_type_cache.get(_topic) if _topic else None - if _cls is not None: - _type_name = _cls.__name__.lower() - self._last_valid_raw[_cid] = {"type": _type_name, "data": _default} - logger.info( - f"Seeded hold default ({_default!r}) for '{_cid}' ({_type_name})" - ) - self.recording_sample_count = 0 interval = 1.0 / self.recording_frequency @@ -301,22 +239,8 @@ def _standard_recording_loop(self, topic_subscribers): consecutive_errors = 0 while not self.stop_recording_flag: try: - if self.is_paused: - time.sleep(0.01) - next_tick = time.monotonic() # don't try to catch up after a pause - continue - - # Flush any TF messages queued in DDS since the last external - # spin. Non-blocking: skip if the Qt timer or API spin thread - # already holds the lock. - if topic_subscribers._spin_lock.acquire(blocking=False): - try: - rclpy.spin_once(topic_subscribers, timeout_sec=0.0) - except Exception: - pass - finally: - topic_subscribers._spin_lock.release() - + # No spin_once here: TopicSubscribers' own spin thread drains the callbacks, and spinning the + # node on the global executor would take it out of that thread's executor. # Refresh all TF transforms before snapshotting data so that # every input (topics and TF alike) reflects the same moment. topic_subscribers.update_tf_lookups() @@ -358,8 +282,7 @@ def _standard_recording_loop(self, topic_subscribers): for tf_cid in topic_subscribers.tf_lookup_containers: tf_val = all_data.get(tf_cid) if tf_val is not None and hasattr(tf_val, "header"): - s = tf_val.header.stamp - snap_timestamps[tf_cid] = s.sec + s.nanosec * 1e-9 + snap_timestamps[tf_cid] = stamp_to_sec(tf_val.header.stamp) # Timestamp sync check — compares ROS message timestamps across # all inputs that are not fill_gaps. Soft mode logs a warning; @@ -374,7 +297,7 @@ def _standard_recording_loop(self, topic_subscribers): if len(sync_cids) >= 2: ts_vals = [snap_timestamps[cid] for cid in sync_cids] spread = max(ts_vals) - min(ts_vals) - if spread > self._sync_tolerance: + if not inputs_in_sync(sync_cids, snap_timestamps, self._sync_tolerance): ref_ts = max(ts_vals) sorted_cids = sorted( sync_cids, key=lambda c: snap_timestamps[c], reverse=True @@ -425,6 +348,8 @@ def _standard_recording_loop(self, topic_subscribers): self._last_valid_raw[container_id] = processed_data self._buffered_bytes += _sample_nbytes(processed_data) data_processed += 1 + else: + self._held_counts[container_id] += 1 # held (or None until the first sample) self.recorded_data[container_id].append( processed_data or self._last_valid_raw.get(container_id) ) @@ -464,6 +389,7 @@ def _standard_recording_loop(self, topic_subscribers): consecutive_errors += 1 if consecutive_errors >= 50: logger.error("Recording loop failed 50 times in a row; sampling stopped.") + self.loop_failed = True break time.sleep(interval) @@ -475,6 +401,7 @@ def _standard_recording_loop(self, topic_subscribers): ) except Exception as e: + self.loop_failed = True logger.error(f"Fatal error in standard recording loop: {e}") def stop_recording(self): @@ -491,7 +418,6 @@ def stop_recording(self): self.stop_recording_flag = True self.is_recording = False - self.is_paused = False if self.recording_thread and self.recording_thread.is_alive(): # The loop checks the stop flag every tick; wait long enough that it is really done @@ -514,6 +440,7 @@ def stop_recording(self): ) self.recording_metadata["total_samples"] = len(self.timestamps) + self._unsaved = True # until the file is saved AND closed: a failed close must not lose the demo at the next Start saved = self._save_recorded_data() if self._fill_count_summary: @@ -528,13 +455,15 @@ def stop_recording(self): ) if self.h5_file: - self.h5_file.close() - self.h5_file = None + try: + self.h5_file.close() + finally: + self.h5_file = None if not saved: - self._unsaved = True logger.error(f"Recording NOT saved to {self.h5_file_path}; data is still in memory") return False + self._unsaved = False logger.info( f"Recording stopped. Saved {len(self.timestamps)} samples in {self.h5_file_path}" @@ -555,12 +484,16 @@ def retry_save(self): try: self._create_h5_file(self.recording_metadata["demo_name"], mode="w") # replaces its own partial file saved = self._save_recorded_data() + self.h5_file.close() # saved only once closed except Exception as e: logger.error(f"Retry save failed: {e}") saved = False - if self.h5_file: - self.h5_file.close() - self.h5_file = None + try: + if self.h5_file: + self.h5_file.close() + except Exception: + pass + self.h5_file = None if saved: self._unsaved = False self.retried_file = self.h5_file_path # the caller adds it to its session list @@ -608,21 +541,6 @@ def _process_message_for_recording(self, msg, container_id=None): logger.error(f"Error processing {type(msg).__name__} for recording ({container_id}): {e}") return None - def _build_output_map_lookup(self) -> dict: - """ - Build container_id -> output_map lookup from the yaml config input section. - If output_map is not set for a container, falls back to the container_id itself. - """ - lookup = {} - if self.yaml_parser and self.yaml_parser.is_loaded(): - input_section = ( - self.yaml_parser.yaml_data.get("config", {}).get("input", {}) - ) - for container_id, spec in input_section.items(): - output_map = spec.get("output_map") - lookup[container_id] = output_map if output_map else container_id - return lookup - def _build_modality_lookup(self) -> dict: """ Build container_id -> "rgb"|"depth"|"scan" from the yaml config input @@ -638,19 +556,13 @@ def _build_modality_lookup(self) -> dict: return lookup def _build_fill_gaps_set(self) -> set: - """Return the set of container IDs that have fill_gaps: true in the config. - Also populates self._fill_defaults with any configured fill_default values. - """ + """Return the set of container IDs that have fill_gaps: true in the config.""" fill_set = set() - self._fill_defaults = {} if self.yaml_parser and self.yaml_parser.is_loaded(): input_section = self.yaml_parser.yaml_data.get("config", {}).get("input", {}) for container_id, spec in input_section.items(): if isinstance(spec, dict) and spec.get("fill_gaps", False): fill_set.add(container_id) - default_val = spec.get("fill_default", None) - if default_val is not None: - self._fill_defaults[container_id] = default_val return fill_set def _save_recorded_data(self): @@ -662,7 +574,6 @@ def _save_recorded_data(self): demo_name = self.recording_metadata["demo_name"] demo_group = self.h5_file.create_group(f"data/{demo_name}") - output_map_lookup = self._build_output_map_lookup() self._fill_count_summary = {} if self.timestamps: @@ -687,27 +598,20 @@ def _save_recorded_data(self): if not data_list: continue - non_null_raw = sum(1 for d in data_list if d is not None) - - if container_id in self._fill_gaps_containers: - filled_list = fill_none_gaps(data_list) - fill_count = sum( - 1 for a, b in zip(data_list, filled_list) - if a is None and b is not None - ) - if fill_count > 0: - self._fill_count_summary[container_id] = fill_count - else: - filled_list = data_list - fill_count = 0 + # The loop already held the last sample over gaps; held ticks are not real samples. + fill_count = self._held_counts.get(container_id, 0) + non_null_raw = len(data_list) - fill_count + if fill_count > 0 and container_id in self._fill_gaps_containers: + self._fill_count_summary[container_id] = fill_count # Samples taken before the first message arrived are None. Dropping them would # leave this container shorter than `timestamps` and shift it against the rest. - first_valid = next((d for d in filled_list if d is not None), None) + first_valid = next((d for d in data_list if d is not None), None) if first_valid is None: logger.error(f"{container_id}: no data received during the whole recording") + filled_list = data_list else: - filled_list = [first_valid if d is None else d for d in filled_list] + filled_list = [first_valid if d is None else d for d in data_list] fill_suffix = f", {fill_count} gap-filled by hold" if fill_count > 0 else "" logger.info( @@ -715,11 +619,7 @@ def _save_recorded_data(self): f"({non_null_raw} real{fill_suffix})" ) - # Use output_map name so the raw HDF5 matches what data_config.yaml expects - group_name = output_map_lookup.get(container_id, container_id) - if group_name != container_id: - logger.info(f"Saving '{container_id}' as '{group_name}' (output_map)") - container_group = demo_group.create_group(group_name) + container_group = demo_group.create_group(container_id) data_by_type = defaultdict(list) for data_item in filled_list: diff --git a/relay_d/acquisition/qt_app/ui/record/record_page.py b/relay_d/acquisition/qt_app/ui/record/record_page.py index a26bb48..3ed8445 100644 --- a/relay_d/acquisition/qt_app/ui/record/record_page.py +++ b/relay_d/acquisition/qt_app/ui/record/record_page.py @@ -38,6 +38,7 @@ def __init__(self, ui_instance, yaml_parser: YamlParser, settings_page=None): # Initialize the TopicSubscribers # with the main ROS node self.topic_subscribers = TopicSubscribers() + self.topic_subscribers.start_spinning() # its own thread, not the app's 10 ms Qt timer; closeEvent stops it # Initialize visualization and recording self.container_visualizer = ContainerVisualizer() @@ -50,7 +51,7 @@ def __init__(self, ui_instance, yaml_parser: YamlParser, settings_page=None): self.container_visualizer, ) - # Recording lifecycle: start/pause/resume/stop, session demos, record buttons + # Recording lifecycle: start/stop, session demos, record buttons self.recording = RecordingController( ui_instance, yaml_parser, @@ -91,8 +92,18 @@ def connect_to_config_signals(self, config_signal_manager): config_signal_manager.config_cleared.connect(self._on_config_cleared) config_signal_manager.config_saved.connect(self._on_config_saved) + def _refuse_while_recording(self, what): + """True (and a warning) while a demo is recording or saving: swapping the subscribers now would + repeat the last frame and save the demo under the wrong names.""" + if self.recording.is_recording: + logger.warning(f"Config {what} ignored: a recording is in progress. Stop it first, then reload the config.") + return True + return False + def _on_config_saved(self, file_path): """Handle config saved signal - reload containers""" + if self._refuse_while_recording("save"): + return logger.info(f"Config saved, reloading containers from: {file_path}") # Reload the config file @@ -103,6 +114,8 @@ def _on_config_saved(self, file_path): def _on_config_loaded(self, file_path): """Handle config loaded signal""" + if self._refuse_while_recording("load"): + return logger.info(f"Config loaded from: {file_path}") if self.label_no_config is not None: self.label_no_config.hide() @@ -110,17 +123,11 @@ def _on_config_loaded(self, file_path): # Load config first self.yaml_parser.load_yaml_file(file_path) - # Register the topic_subscribers node with the UI for spinning - if hasattr(self.ui, "register_ros_node"): - self.ui.register_ros_node(self.topic_subscribers) - self.ui.register_ros_node(self.data_recorder) - logger.info("Registered topic_subscribers and data_recorder nodes with UI") - # Create the containers with visualization self._create_containers() # Start TF update timer at recording_frequency (not hardcoded 10 Hz) - freq = self.data_recorder.recording_frequency if self.data_recorder else 20 + freq = ((self.yaml_parser.yaml_data or {}).get("config") or {}).get("recording_frequency") or 20 # the recorder reads it only at Start interval_ms = max(50, int(1000 / freq)) # floor at 50 ms (20 Hz max for UI) self.tf_update_timer.start(interval_ms) logger.info(f"TF update timer started at {freq} Hz ({interval_ms} ms)") @@ -128,21 +135,12 @@ def _on_config_loaded(self, file_path): # Start becomes available once a config is loaded self.recording.set_config_loaded(True) - ## Set grippers keys for state modification - self.gripper_key_map = {} - input_data = self.yaml_parser.get_input_data() - - for entry in input_data: - if entry["name"].startswith("gripper") and "topic" in entry: - logger.info(f"Found gripper entry: {entry['name']}") - # For gripper, we now use key events - open/close keys should be defined separately - # or handled through the GUI - - logger.info(f"Total gripper mappings: {len(self.gripper_key_map)}") def _on_config_cleared(self): """Handle config cleared signal""" + if self._refuse_while_recording("clear"): + return logger.info("Config cleared") if self.label_no_config is not None: self.label_no_config.show() @@ -176,14 +174,6 @@ def start_recording(self): """Start recording process""" return self.recording.start_recording() - def pause_recording(self): - """Pause recording process""" - return self.recording.pause_recording() - - def resume_recording(self): - """Resume recording process""" - return self.recording.resume_recording() - def stop_recording(self): """Stop recording process""" return self.recording.stop_recording() diff --git a/relay_d/acquisition/qt_app/ui/record/recording_controller.py b/relay_d/acquisition/qt_app/ui/record/recording_controller.py index 4c102a4..fe5ab60 100644 --- a/relay_d/acquisition/qt_app/ui/record/recording_controller.py +++ b/relay_d/acquisition/qt_app/ui/record/recording_controller.py @@ -6,18 +6,17 @@ from relay_d.utils.tf_topic import TfTopicSubscriptionError from ..review.demo_validator import CheckStatus +from .data_recorder import start_demo from ...utils.background import run_in_background from ...utils.custom_message_box import CustomMessageBox class RecordingController: - """Recording lifecycle for the record page: start / pause / resume / stop. + """Recording lifecycle for the record page: start / stop. - Owns the recording state (``is_recording``, ``is_paused``), the demos recorded in this - session and the Start/Pause/Stop buttons. The Pause button is connected once to - ``toggle_pause``; its text and enabled state follow the state via ``update_buttons`` - (no reconnecting of slots). ``RecordPage`` delegates to this class and keeps its public - method names. + Owns the recording state (``is_recording``), the demos recorded in this session and the + Start/Stop buttons, whose enabled state follows the state via ``update_buttons``. + ``RecordPage`` delegates to this class and keeps its public method names. """ def __init__( @@ -38,8 +37,7 @@ def __init__( self.container_manager = container_manager self._demo_validator = demo_validator - self.is_recording = False # stays True while paused, and while the demo is being saved - self.is_paused = False + self.is_recording = False # stays True while the demo is being saved self.is_saving = False # Stop was pressed; the save runs in a background thread self.recording_start_time = None self.config_loaded = False @@ -58,17 +56,14 @@ def __init__( # ------------------------------------------------------------------ def connect_buttons(self): - """Connect Start/Pause/Stop once and set their initial state.""" + """Connect Start/Stop once and set their initial state.""" self.ui.btn_start_record.clicked.connect(self.start_recording) - self.ui.btn_pause_record.clicked.connect(self.toggle_pause) self.ui.btn_stop_record.clicked.connect(self.stop_recording) self.update_buttons() def update_buttons(self): - """Enable/label the buttons from the current state.""" + """Enable the buttons from the current state.""" self.ui.btn_start_record.setEnabled(self.config_loaded and not self.is_recording) - self.ui.btn_pause_record.setEnabled(self.is_recording and not self.is_saving) - self.ui.btn_pause_record.setText("Resume" if self.is_paused else "Pause") self.ui.btn_stop_record.setEnabled(self.is_recording and not self.is_saving) def set_config_loaded(self, loaded: bool): @@ -77,7 +72,7 @@ def set_config_loaded(self, loaded: bool): self.update_buttons() def _check_buffer_full(self): - """Tell the operator once when the recorder stopped sampling because its buffer is full.""" + """Tell the operator once when the recorder stopped sampling (buffer full, or its loop failed).""" if self.data_recorder.buffer_full is True: self._buffer_timer.stop() CustomMessageBox.warning( @@ -85,12 +80,13 @@ def _check_buffer_full(self): "Press Stop to save what was recorded.", title="Recording Buffer Full", ) - - def toggle_pause(self): - """Pause button slot: pause while recording, resume while paused.""" - if self.is_paused: - return self.resume_recording() - return self.pause_recording() + elif self.data_recorder.loop_failed is True: + self._buffer_timer.stop() + CustomMessageBox.error( + "The recording loop kept failing and has stopped sampling (see the log). " + "Press Stop to save what was recorded.", + title="Recording Stopped", + ) # ------------------------------------------------------------------ # Lifecycle @@ -108,22 +104,11 @@ def start_recording(self): CustomMessageBox.warning("Load a config first: recording needs one (Configuration page).", title="No config loaded") return False - # Create subscribers now (at recording start, not at config load) - logger.info("Creating ROS subscribers for recording...") - subscriber_results = self.topic_subscribers.create_subscribers_for_containers( - self.topic_containers.containers - ) - successful = sum(subscriber_results.values()) - logger.info( - f"Subscribers created: {successful}/{len(self.topic_containers.containers)}" - ) - - # Start recording with DataRecorder - dataset_name = self.yaml_parser.get_dataset_name() - demo_name = f"{dataset_name}_{time.time_ns() // 1_000_000}" # ms: two Starts in one second must not clash ("w-") - success = self.data_recorder.start_recording( - self.topic_subscribers, demo_name + demo_name = start_demo( # same steps as AppAPI.start_recording + self.data_recorder, self.topic_subscribers, self.topic_containers.containers, + self.yaml_parser.get_dataset_name(), ) + success = demo_name is not None if self.data_recorder.retried_file: # an earlier demo saved only now: keep it in the session self.session_h5_files.append(self.data_recorder.retried_file) @@ -134,7 +119,6 @@ def start_recording(self): ) self.is_recording = True - self.is_paused = False self.recording_start_time = time.time() self._buffer_timer.start() @@ -172,50 +156,6 @@ def start_recording(self): logger.error(f"Error starting recording: {e}") return False - def pause_recording(self): - """Pause recording process""" - try: - if not self.is_recording: - logger.warning("No recording in progress to pause") - return False - - success = self.data_recorder.pause_recording() - - if success: - self.is_paused = True - self.container_manager.set_status_for_all("Paused") - logger.info("Recording paused") - self.update_buttons() - return True - else: - logger.error("Failed to pause recording") - return False - - except Exception as e: - logger.error(f"Error pausing recording: {e}") - return False - - def resume_recording(self): - """Resume recording process""" - try: - success = self.data_recorder.resume_recording() - - if success: - self.is_paused = False - self.container_manager.set_statuses_by_data( - "Recording (Data Available)", "Recording (Waiting for Data)" - ) - logger.info("Recording resumed") - self.update_buttons() - return True - else: - logger.error("Failed to resume recording") - return False - - except Exception as e: - logger.error(f"Error resuming recording: {e}") - return False - def stop_recording(self): """Stop button: start saving the demo. True if the save was started. @@ -242,7 +182,6 @@ def _on_stopped(self, success, error): try: if success: self.is_recording = False - self.is_paused = False recording_duration = ( time.time() - self.recording_start_time if self.recording_start_time @@ -284,7 +223,6 @@ def _on_stopped(self, success, error): # and retries the save on the next Start. Leave the UI in the "stopped" state so # Start is available again instead of stuck on "Recording". self.is_recording = False - self.is_paused = False self.topic_subscribers.clear_all_subscribers() self.container_manager.set_statuses_by_data( "Stopped (Data Available)", "Stopped (No Data)" diff --git a/relay_d/acquisition/qt_app/ui/record/topic_subscribers.py b/relay_d/acquisition/qt_app/ui/record/topic_subscribers.py index 05ceff5..a2e63ee 100644 --- a/relay_d/acquisition/qt_app/ui/record/topic_subscribers.py +++ b/relay_d/acquisition/qt_app/ui/record/topic_subscribers.py @@ -4,6 +4,7 @@ import rclpy.time from relay_d.utils.coloring_logger import logger +from relay_d.utils.numerics import stamp_to_sec from relay_d.utils import user_settings from relay_d.utils.qos import subscription_qos from relay_d.utils.tf_topic import ( @@ -12,7 +13,7 @@ get_or_create_tf_buffer, wait_for_transform, ) -from threading import Lock, Thread +from threading import Lock, Thread, current_thread from rclpy.executors import SingleThreadedExecutor import importlib import time @@ -44,7 +45,8 @@ def __init__(self): self.latest_data = {} self.latest_timestamps = {} self.data_lock = Lock() - self._spin_lock = Lock() + self._spin_thread = None + self._should_spin = False self.message_type_cache = {} self.subscriber_status = {} self.tf_lookup_containers = {} @@ -76,13 +78,8 @@ def __init__(self): self.tf_buffer = Buffer(cache_time=rclpy_duration(seconds=10.0)) - # TF gets its own node + dedicated spin thread so that /tf (which - # can arrive in much denser bursts than regular topics, especially - # during rosbag replay) doesn't have to compete one-callback-at-a-time - # with everything else on this node's manually-driven spin_once() - # calls. Using a separate Node (rather than spin_thread=True on - # `self`) avoids racing with the shared spin_once calls elsewhere - # (data_recorder, AppAPI, the Qt spin timer) that also spin `self`. + # TF gets its own node + spin thread: /tf arrives in dense bursts (rosbag replay) and must not + # queue behind the topic callbacks, which run on this node's own spin thread (start_spinning). self._tf_node = rclpy.create_node(f"topic_subscribers_tf_listener_{id(self)}") self.tf_listener = _TfTopicListener(self.tf_buffer, self._tf_node, *self._DEFAULT_TF_KEY) self._tf_executor = SingleThreadedExecutor() @@ -134,7 +131,6 @@ def create_subscriber(self, container): getattr(container, "tf_wait_timeout", None) or self._CUSTOM_TF_WAIT_TIMEOUT ) - transfer_rate = getattr(container, "transfer_rate", 10) if not container_id: logger.error("Container missing container_id") @@ -153,7 +149,6 @@ def create_subscriber(self, container): container_id, parent_frame, child_frame, - transfer_rate, tf_topic, tf_static_topic, tf_wait_timeout, @@ -181,7 +176,7 @@ def create_subscriber(self, container): callback = self._create_callback(container_id, input_name) qos = subscription_qos(self, topic_path) - subscriber = self.create_subscription( + subscriber = self.create_subscription( # safe while the spin thread runs: rclpy defers handle destruction/use msg_class, topic_path, callback, qos_profile=qos ) @@ -248,6 +243,26 @@ def is_topic_available(self, topic_path, timeout=None): validation before any subscriber is actually created.""" return self._detect_topic_type(topic_path, timeout=timeout) is not None + def unavailable_inputs(self, input_data): + """One line per configured input whose topic / TF transform is not available now (empty: all are). + The Config page and AppAPI refuse a config with any.""" + failures = [] + for item in input_data: + name = item["name"] + if item.get("is_tf"): + tf_topic = item.get("tf_topic", "/tf") + logger.info(f"Validating TF input '{name}': {item['parent_frame']} -> {item['child_frame']} on {tf_topic}...") + if not self.is_tf_available( + item["parent_frame"], item["child_frame"], tf_topic=tf_topic, + tf_static_topic=item.get("tf_static_topic", "/tf_static"), timeout=item.get("tf_wait_timeout"), + ): + failures.append(f" {name}: TF {item['parent_frame']} -> {item['child_frame']} not available on tf_topic={tf_topic}") + elif item.get("topic"): + logger.info(f"Validating input '{name}': topic {item['topic']}...") + if not self.is_topic_available(item["topic"]): + failures.append(f" {name}: topic {item['topic']} not found") + return failures + def is_tf_available( self, parent_frame, @@ -351,8 +366,7 @@ def callback(msg): with self.data_lock: self.latest_data[container_id] = msg if hasattr(msg, "header"): - s = msg.header.stamp - self.latest_timestamps[container_id] = s.sec + s.nanosec * 1e-9 + self.latest_timestamps[container_id] = stamp_to_sec(msg.header.stamp) else: self.latest_timestamps[container_id] = time.time() if container_id in self.subscriber_status: @@ -456,8 +470,36 @@ def clear_all_subscribers(self): logger.error(f"Failed to clear subscribers: {e}") return False + def start_spinning(self): + """Run this node's callbacks in a thread (GUI and AppAPI). A Qt timer doing one spin_once per node every 10 ms + handled ~78 of 400 msg/s, ~100 ms late. Own executor: the global one refuses a second spinning thread.""" + if self._spin_thread is not None and self._spin_thread.is_alive(): + return + self._should_spin = True + executor = SingleThreadedExecutor() + executor.add_node(self) + self._spin_thread = Thread(target=self._spin_loop, args=(executor,), daemon=True) + self._spin_thread.start() + + def _spin_loop(self, executor): + while self._should_spin: + try: + executor.spin_once(timeout_sec=0.01) + except Exception as e: + if not rclpy.ok(): + break + self._log_throttled(f"Error spinning topic_subscribers: {e}", "spin") + executor.remove_node(self) + + def stop_spinning(self): + """Stop the spin thread (no join when called from a callback on that thread).""" + self._should_spin = False + if self._spin_thread is not None and self._spin_thread is not current_thread(): + self._spin_thread.join(timeout=2.0) + def close(self): - """Stop the dedicated TF spin thread/executor and destroy its node.""" + """Stop the spin threads (this node's and the TF one) and destroy the TF node.""" + self.stop_spinning() try: if self._tf_executor is not None: self._tf_executor.shutdown() @@ -504,7 +546,6 @@ def _create_tf_lookup( container_id, parent_frame, child_frame, - transfer_rate=10, tf_topic="/tf", tf_static_topic="/tf_static", tf_wait_timeout=None, @@ -607,8 +648,7 @@ def _lookup_tf_transform(self, container_id, parent_frame, child_frame, buffer): with self.data_lock: self.latest_data[container_id] = transform - s = transform.header.stamp - self.latest_timestamps[container_id] = s.sec + s.nanosec * 1e-9 + self.latest_timestamps[container_id] = stamp_to_sec(transform.header.stamp) if container_id in self.subscriber_status: status = self.subscriber_status[container_id] status["message_count"] += 1 @@ -633,7 +673,7 @@ def _lookup_tf_transform(self, container_id, parent_frame, child_frame, buffer): f"TF waiting: {parent_frame} -> {child_frame} ({str(e)[:60]})", f"tf_{container_id}", ) - except Exception as e: + except Exception: pass def lookup_tf_at_time(self, container_id, ros_time, timeout_sec=0.05): @@ -669,7 +709,7 @@ def get_content(self, container_id=None): return self.latest_data.get(container_id, None) else: return dict(self.latest_data) - except Exception as e: + except Exception: return None def get_content_and_timestamps(self): diff --git a/relay_d/acquisition/qt_app/ui/review/demo_playback_reconstructor.py b/relay_d/acquisition/qt_app/ui/review/demo_playback_reconstructor.py index 5840b4d..2f9ef4c 100644 --- a/relay_d/acquisition/qt_app/ui/review/demo_playback_reconstructor.py +++ b/relay_d/acquisition/qt_app/ui/review/demo_playback_reconstructor.py @@ -19,7 +19,7 @@ from std_msgs.msg import String, Float32, Float64, Int32, Bool from relay_d.utils.coloring_logger import logger -from .demo_validator import NON_INPUT_GROUPS +from .demo_validator import NON_INPUT_GROUPS, first_demo _SIMPLE_MSG_CLASSES = { "string": String, @@ -257,13 +257,11 @@ def __init__(self, h5_path: str): self.h5_path = h5_path self._h5 = h5py.File(h5_path, "r") - if "data" not in self._h5 or not list(self._h5["data"].keys()): + demo_name, self.demo_group = first_demo(self._h5) + if self.demo_group is None: self._h5.close() raise ValueError(f"No data/ group found in {h5_path}") - demo_name = list(self._h5["data"].keys())[0] - self.demo_group = self._h5["data"][demo_name] - if "timestamps" not in self.demo_group or self.demo_group["timestamps"].shape[0] == 0: self._h5.close() raise ValueError(f"Demo '{demo_name}' has no timestamps to play back") diff --git a/relay_d/acquisition/qt_app/ui/review/demo_validator.py b/relay_d/acquisition/qt_app/ui/review/demo_validator.py index d98f000..722ad6b 100644 --- a/relay_d/acquisition/qt_app/ui/review/demo_validator.py +++ b/relay_d/acquisition/qt_app/ui/review/demo_validator.py @@ -59,6 +59,12 @@ def summary_text(self) -> str: ) +def first_demo(f): + """(name, group) of the first group under data/ (a recording holds one demo), or ("", None).""" + names = list(f["data"].keys()) if "data" in f else [] + return (names[0], f["data"][names[0]]) if names else ("", None) + + class DemoValidator: """Runs a fixed set of viability checks against a single demo's HDF5 group.""" @@ -88,7 +94,7 @@ def validate(self, h5_path: str) -> DemoValidationResult: try: with h5py.File(h5_path, "r") as f: - demo_name, demo_group = self._find_demo_group(f) + demo_name, demo_group = first_demo(f) if demo_group is None: result.checks.append( CheckResult( @@ -113,15 +119,6 @@ def validate(self, h5_path: str) -> DemoValidationResult: result.overall_status = self._aggregate(result.checks) return result - def _find_demo_group(self, f): - if "data" not in f: - return "", None - names = list(f["data"].keys()) - if not names: - return "", None - demo_name = names[0] - return demo_name, f["data"][demo_name] - def _aggregate(self, checks) -> CheckStatus: worst = CheckStatus.PASS for c in checks: diff --git a/relay_d/acquisition/qt_app/ui/review/lerobot_reader.py b/relay_d/acquisition/qt_app/ui/review/lerobot_reader.py deleted file mode 100644 index 25d39e2..0000000 --- a/relay_d/acquisition/qt_app/ui/review/lerobot_reader.py +++ /dev/null @@ -1,316 +0,0 @@ -"""Read a LeRobotDataset v3.0 folder for viewing: episodes, state / action curves, camera frames, checks. - -Pure Python (pyarrow + OpenCV, no Qt, no ROS). View-only: it never writes to the dataset. -``pyarrow`` comes with the ``lerobot`` extra; camera frames are decoded with OpenCV, which is a core -dependency (PyAV is the fallback if OpenCV cannot open a file). -""" - -from __future__ import annotations - -import json -from collections import OrderedDict -from dataclasses import dataclass, field -from pathlib import Path -from typing import Any, Dict, List, Optional - -import numpy as np - -from .demo_validator import CheckResult, CheckStatus, DemoValidationResult, _SEVERITY - -STATE_KEY, ACTION_KEY = "observation.state", "action" -_MAX_OPEN_VIDEOS = 4 - - -class LeRobotError(Exception): - """The folder is not a readable LeRobot dataset (or pyarrow is missing).""" - - -def is_lerobot_dataset(path) -> bool: - return (Path(path) / "meta" / "info.json").is_file() - - -def _pyarrow(): - try: - import pyarrow.parquet as pq - except ImportError: - raise LeRobotError("Reading LeRobot datasets needs pyarrow: pip install 'relay-d[lerobot]'") from None - return pq - - -@dataclass -class Episode: - index: int - length: int - tasks: List[str] - data_file: tuple # (chunk, file) of the data parquet - from_index: int - to_index: int - videos: Dict[str, dict] = field(default_factory=dict) # camera key -> {chunk, file, from_ts, to_ts} - - @property - def label(self) -> str: - task = f" · {self.tasks[0]}" if self.tasks else "" - return f"Episode {self.index} · {self.length} frames{task}" - - -def _vector_column(col) -> np.ndarray: - """A list column (fixed-size or plain ``list``) as an (N, D) float array; a scalar column as (N, 1).""" - import pyarrow as pa - - col = col.combine_chunks() if isinstance(col, pa.ChunkedArray) else col - n = len(col) - t = col.type - if pa.types.is_fixed_size_list(t): - return col.flatten().to_numpy(zero_copy_only=False).reshape(n, t.list_size) - if pa.types.is_list(t) or pa.types.is_large_list(t): - lengths = np.diff(col.offsets.to_numpy()) - if n and (lengths != lengths[0]).any(): - raise LeRobotError("a vector column has rows of different lengths") - width = int(lengths[0]) if n else 0 - return col.flatten().to_numpy(zero_copy_only=False).reshape(n, width) - return col.to_numpy(zero_copy_only=False).reshape(n, 1) - - -class LeRobotDataset: - def __init__(self, root): - self.root = Path(root) - try: - self.info: Dict[str, Any] = json.loads((self.root / "meta" / "info.json").read_text()) - except (OSError, ValueError) as e: - raise LeRobotError(f"{self.root} is not a LeRobot dataset: {e}") from None - self.fps = int(self.info.get("fps") or 0) - self.features: Dict[str, dict] = self.info.get("features", {}) - self.camera_keys = [k for k, v in self.features.items() if v.get("dtype") == "video"] - self._episodes: Optional[List[Episode]] = None - self._tables: Dict[tuple, Any] = {} - self._row_index: Dict[tuple, np.ndarray] = {} # (chunk, file) -> the global `index` column - self._status: Dict[int, CheckStatus] = {} # episode -> result of validate() - self._av: "OrderedDict[str, dict]" = OrderedDict() # PyAV fallback: open containers - self._videos: "OrderedDict[str, dict]" = OrderedDict() # path -> {"cap": VideoCapture, "next": frame no.} - - # ------------------------------------------------------------------ metadata - - def names(self, key: str) -> List[str]: - """Names of the columns of a vector feature (joint names, ...).""" - feature = self.features.get(key, {}) - names = feature.get("names") - width = (feature.get("shape") or [0])[0] - if isinstance(names, list) and len(names) == width: - return [str(n) for n in names] - return [f"{key}[{i}]" for i in range(width)] - - def episodes(self) -> List[Episode]: - if self._episodes is None: - pq = _pyarrow() - files = sorted((self.root / "meta" / "episodes").glob("chunk-*/file-*.parquet")) - if not files: - raise LeRobotError("meta/episodes has no parquet files") - rows = [] - for f in files: - rows.extend(pq.read_table(f).to_pylist()) - episodes = [] - for r in sorted(rows, key=lambda r: r["episode_index"]): - videos = { - k: { - "chunk": r.get(f"videos/{k}/chunk_index"), "file": r.get(f"videos/{k}/file_index"), - "from_ts": r.get(f"videos/{k}/from_timestamp"), "to_ts": r.get(f"videos/{k}/to_timestamp"), - } - for k in self.camera_keys - } - episodes.append(Episode( - index=int(r["episode_index"]), length=int(r["length"]), tasks=list(r.get("tasks") or []), - data_file=(int(r["data/chunk_index"]), int(r["data/file_index"])), - from_index=int(r["dataset_from_index"]), to_index=int(r["dataset_to_index"]), videos=videos, - )) - self._episodes = episodes - return self._episodes - - def summary(self) -> str: - eps = self.episodes() - tasks = sorted({t for e in eps for t in e.tasks}) - return "\n".join([ - f"LeRobot dataset {self.info.get('codebase_version', '?')}: {self.root.name}", - f"{len(eps)} episodes, {self.info.get('total_frames', sum(e.length for e in eps))} frames, {self.fps} fps", - f"Robot type: {self.info.get('robot_type') or 'not set'}", - f"Cameras: {', '.join(self.camera_keys) or 'none'}", - f"Tasks: {', '.join(tasks) or 'none'}", - ]) - - # ------------------------------------------------------------------ data - - def _table(self, chunk: int, file: int): - key = (chunk, file) - if key not in self._tables: - path = self.root / "data" / f"chunk-{chunk:03d}" / f"file-{file:03d}.parquet" - try: - self._tables[key] = _pyarrow().read_table(path) - except OSError as e: - raise LeRobotError(f"cannot read {path}: {e}") from None - if len(self._tables) > 3: # a few files are enough for browsing episodes in order - self._tables.pop(next(iter(self._tables))) - return self._tables[key] - - def _episode_rows(self, episode: Episode): - """The slice of the data file that holds one episode (rows are ordered by the global ``index``).""" - table = self._table(*episode.data_file) - if "index" in table.column_names: - idx = self._row_index.get(episode.data_file) - if idx is None: - idx = self._row_index[episode.data_file] = np.asarray(table.column("index").to_numpy()) - start, stop = np.searchsorted(idx, [episode.from_index, episode.to_index], side="left") - else: # no global index: fall back to the episode column - rows = np.nonzero(np.asarray(table.column("episode_index").to_numpy()) == episode.index)[0] - start, stop = (rows[0], rows[-1] + 1) if len(rows) else (0, 0) - return table.slice(int(start), int(stop - start)) - - def arrays(self, episode: Episode) -> Dict[str, np.ndarray]: - """``time`` (T,) plus ``observation.state`` / ``action`` (T, D) of one episode.""" - rows = self._episode_rows(episode) - out = {"time": np.asarray(rows.column("timestamp").to_numpy()).astype(np.float64)} - for key in (STATE_KEY, ACTION_KEY): - if key in rows.column_names: - out[key] = _vector_column(rows.column(key)) - return out - - # ------------------------------------------------------------------ video - - def _video_path(self, episode: Episode, camera: str) -> Path: - v = episode.videos.get(camera) - if not v or v["chunk"] is None: - raise LeRobotError(f"episode {episode.index} has no video for '{camera}'") - return self.root / "videos" / camera / f"chunk-{v['chunk']:03d}" / f"file-{v['file']:03d}.mp4" - - def frame(self, episode: Episode, camera: str, i: int) -> np.ndarray: - """Frame ``i`` of an episode as an RGB uint8 array.""" - import cv2 - - if not 0 <= i < episode.length: - raise IndexError(f"frame {i} out of range for an episode of {episode.length} frames") - path = self._video_path(episode, camera) - number = int(round(episode.videos[camera]["from_ts"] * self.fps)) + i - entry = self._videos.pop(str(path), None) - if entry is None: - entry = {"cap": cv2.VideoCapture(str(path)), "next": 0} - self._videos[str(path)] = entry # most recently used last - while len(self._videos) > _MAX_OPEN_VIDEOS: - _, old = self._videos.popitem(last=False) - old["cap"].release() - cap = entry["cap"] - if not cap.isOpened(): - return self._frame_av(path, number) - if entry["next"] != number: # sequential playback needs no seek - cap.set(cv2.CAP_PROP_POS_FRAMES, number) - ok, bgr = cap.read() - entry["next"] = number + 1 - if not ok: - return self._frame_av(path, number) - return cv2.cvtColor(bgr, cv2.COLOR_BGR2RGB) - - def _frame_av(self, path: Path, number: int) -> np.ndarray: - """Fallback decoder: one open container per file, seek to the keyframe before the frame, decode forward.""" - try: - import av - except ImportError: - raise LeRobotError(f"cannot decode {path.name}: OpenCV failed and PyAV is not installed") from None - entry = self._av.pop(str(path), None) - if entry is None: - container = av.open(str(path)) - entry = {"container": container, "stream": container.streams.video[0], "frames": None, "next": None} - self._av[str(path)] = entry - while len(self._av) > _MAX_OPEN_VIDEOS: - self._av.popitem(last=False)[1]["container"].close() - container, stream = entry["container"], entry["stream"] - fps = float(stream.average_rate or self.fps) - sequential = entry["frames"] is not None and entry["next"] is not None and entry["next"] <= number <= entry["next"] + 60 - if not sequential: - container.seek(int(number / fps / stream.time_base), stream=stream, backward=True) - entry["frames"], entry["next"] = container.decode(stream), None - for frame in entry["frames"]: - n = int(round(frame.pts * stream.time_base * fps)) - entry["next"] = n + 1 - if n == number: - return frame.to_ndarray(format="rgb24") - if n > number: - break - entry["frames"] = None - raise LeRobotError(f"{path.name} has no frame {number}") - - def close(self) -> None: - for entry in self._videos.values(): - entry["cap"].release() - self._videos.clear() - for entry in self._av.values(): - entry["container"].close() - self._av.clear() - self._tables.clear() - self._row_index.clear() - - # ------------------------------------------------------------------ checks - - def validate(self) -> DemoValidationResult: - """Structure checks of the whole dataset (same result type as the demo validator).""" - result = DemoValidationResult(file_path=str(self.root), demo_name=self.root.name) - add = lambda name, status, msg: result.checks.append(CheckResult(name, status, msg)) # noqa: E731 - P, W, F = CheckStatus.PASS, CheckStatus.WARN, CheckStatus.FAIL - - version = self.info.get("codebase_version") - add("version", P if version == "v3.0" else W, f"codebase_version {version!r}") - if self.fps <= 0: - add("fps", F, f"fps is {self.info.get('fps')!r}") - try: - episodes = self.episodes() - except LeRobotError as e: - add("episodes", F, str(e)) - return self._finish(result) - total = sum(e.length for e in episodes) - result.samples = total - result.duration = total / self.fps if self.fps > 0 else 0.0 - declared = self.info.get("total_episodes") - add("episodes", P if declared in (None, len(episodes)) else F, - f"{len(episodes)} episodes" + ("" if declared in (None, len(episodes)) else f", info.json says {declared}")) - declared = self.info.get("total_frames") - add("frames", P if declared in (None, total) else F, - f"{total} frames" + ("" if declared in (None, total) else f", info.json says {declared}")) - if not self.camera_keys: - add("cameras", W, "no camera in this dataset (ACT and Diffusion need one)") - - data_bad, missing = [], set() - self._status = {} - for e in episodes: - problems = [] # data problems of this episode - try: - a = self.arrays(e) - except LeRobotError as err: - add(f"episode {e.index}", F, str(err)) - self._status[e.index] = F - continue - n = len(a["time"]) - if n != e.length: - problems.append(f"episode {e.index}: {n} rows, expected {e.length}") - else: - if n > 1 and not (np.diff(a["time"]) > 0).all(): - problems.append(f"episode {e.index}: timestamps do not increase") - for key in (STATE_KEY, ACTION_KEY): - if key in a and not np.isfinite(a[key]).all(): - problems.append(f"episode {e.index}: NaN/inf in {key}") - gone = [str(self._video_path(e, k).relative_to(self.root)) for k in self.camera_keys - if e.videos[k]["chunk"] is not None and not self._video_path(e, k).is_file()] - missing.update(gone) - data_bad.extend(problems) - self._status[e.index] = F if problems or gone else P - shown = "; ".join(data_bad[:5]) + (f" (+{len(data_bad) - 5} more)" if len(data_bad) > 5 else "") - add("data", F if data_bad else P, shown or "state / action rows match the episode lengths, finite and ordered") - missing = sorted(missing) - add("videos", F if missing else P, f"missing: {', '.join(missing[:3])}" if missing else "all video files exist") - return self._finish(result) - - def episode_check(self, episode: Episode) -> CheckStatus: - """Status of one episode for the list colour (runs ``validate`` once for the whole dataset).""" - if not self._status: - self.validate() - return self._status.get(episode.index, CheckStatus.FAIL) - - @staticmethod - def _finish(result: DemoValidationResult) -> DemoValidationResult: - result.overall_status = max((c.status for c in result.checks), key=lambda s: _SEVERITY[s], default=CheckStatus.PASS) - return result diff --git a/relay_d/acquisition/qt_app/ui/review/review_page.py b/relay_d/acquisition/qt_app/ui/review/review_page.py index 734b31b..6dc0c76 100644 --- a/relay_d/acquisition/qt_app/ui/review/review_page.py +++ b/relay_d/acquisition/qt_app/ui/review/review_page.py @@ -26,7 +26,6 @@ QRadioButton, QLineEdit, QProgressBar, - QComboBox, ) from PyQt5.QtCore import Qt, QTimer from PyQt5.QtGui import QImage, QPixmap, QColor, QBrush @@ -37,8 +36,7 @@ from ..settings.config_manager import config from ...utils.custom_message_box import CustomMessageBox from ...utils.demo_file_ops import delete_demo_file -from .demo_validator import NON_INPUT_GROUPS, DemoValidator, CheckStatus -from .lerobot_reader import LeRobotDataset, LeRobotError, is_lerobot_dataset +from .demo_validator import NON_INPUT_GROUPS, DemoValidator, CheckStatus, first_demo _STATUS_COLORS = { CheckStatus.PASS: "#16A34A", @@ -68,13 +66,6 @@ def __init__(self, ui_instance, record_page=None, settings_page=None): self.session_dir = None self.demo_entries = {} # file_path -> DemoValidationResult - # LeRobot (view-only) source: opened dataset, selected episode and its curves - self._sources = [] # [("h5", None) | ("lerobot", dataset_path)] - self._lerobot = None - self._lerobot_episode = None - self._lerobot_arrays = None - self._lerobot_checked = {} # dataset path -> (info.json mtime, validation result, {episode: status}) - self._current_h5 = None self._current_demo_group = None self._current_image_dataset = None @@ -122,12 +113,6 @@ def _build_ui(self): self.btn_refresh.clicked.connect(self.refresh_demo_list) sidebar_layout.addWidget(self.btn_refresh) - self.combo_source = QComboBox() - self.combo_source.setToolTip("Recorded demos (.h5) or a LeRobot dataset found in this folder") - self.combo_source.currentIndexChanged.connect(self._on_source_changed) - self.combo_source.setVisible(False) - sidebar_layout.addWidget(self.combo_source) - self.list_demos = QListWidget() self.list_demos.currentItemChanged.connect(self._on_demo_selected) sidebar_layout.addWidget(self.list_demos, 1) @@ -169,10 +154,6 @@ def _build_ui(self): self.image_page = QWidget() layout_image = QVBoxLayout(self.image_page) - self.combo_camera = QComboBox() # LeRobot datasets can have several cameras - self.combo_camera.currentIndexChanged.connect(lambda _i: self._show_frame(self.slider_scrub.value())) - self.combo_camera.setVisible(False) - layout_image.addWidget(self.combo_camera) self.label_image = QLabel("No image data") self.label_image.setAlignment(Qt.AlignCenter) self.label_image.setMinimumHeight(300) @@ -305,69 +286,20 @@ def choose_session_folder(self): self.refresh_demo_list() def refresh_demo_list(self): - """Look for recorded demos and LeRobot datasets in the session folder and list the first source.""" + """List the recorded demos in the session folder.""" self._close_current_file() - self._close_lerobot() self.list_demos.clear() self.demo_entries = {} - self._sources = [] if not self.session_dir or not os.path.isdir(self.session_dir): self.label_session.setText("No session selected") - self._update_source_combo() return - if glob.glob(os.path.join(self.session_dir, "*.h5")): - self._sources.append(("h5", None)) - for path in self._find_lerobot_datasets(self.session_dir): - self._sources.append(("lerobot", path)) - self._update_source_combo() self.label_session.setText(f"Session: {self.session_dir}") self._populate_list() - @staticmethod - def _find_lerobot_datasets(folder): - """The folder itself if it is a LeRobot dataset, else its child folders that are.""" - if is_lerobot_dataset(folder): - return [folder] - children = sorted(os.path.join(folder, d) for d in os.listdir(folder)) - return [c for c in children if os.path.isdir(c) and is_lerobot_dataset(c)] - - def _update_source_combo(self): - self.combo_source.blockSignals(True) - self.combo_source.clear() - for kind, path in self._sources: - self.combo_source.addItem("Recorded demos (.h5)" if kind == "h5" else f"LeRobot: {os.path.basename(path)}") - self.combo_source.blockSignals(False) - self.combo_source.setVisible(len(self._sources) > 1) - - def _on_source_changed(self, _index): - self._close_current_file() - self._close_lerobot() - self._populate_list() - - def _current_source(self): - i = self.combo_source.currentIndex() - return self._sources[i] if 0 <= i < len(self._sources) else ("h5", None) - - def _set_mode(self, lerobot: bool): - """LeRobot datasets are view-only: no delete, no ROS playback.""" - self.btn_delete.setEnabled(not lerobot) - self.btn_delete.setToolTip("LeRobot datasets are view-only here" if lerobot else "") - self.tab_widget.setTabEnabled(1, not lerobot) - self.tab_widget.setTabToolTip(1, "Playback needs recorded .h5 demos" if lerobot else "") - if lerobot: - self.tab_widget.setCurrentIndex(0) - self.combo_camera.setVisible(lerobot) - def _populate_list(self): self.list_demos.clear() - kind, dataset_path = self._current_source() - self._set_mode(kind == "lerobot") - if kind == "lerobot": - self._populate_lerobot(dataset_path) - return - self.text_validation.clear() paths = sorted( p @@ -397,86 +329,6 @@ def _populate_list(self): if not paths: self.label_session.setText(f"Session: {self.session_dir} (no demos found)") - # ------------------------------------------------------------------ - # LeRobot datasets (view-only) - # ------------------------------------------------------------------ - - def _populate_lerobot(self, path): - try: - self._lerobot = LeRobotDataset(path) - episodes = self._lerobot.episodes() - except LeRobotError as e: - logger.error(f"Cannot open LeRobot dataset {path}: {e}") - self.text_validation.setPlainText(str(e)) - return - info_mtime = os.path.getmtime(os.path.join(path, "meta", "info.json")) - cached = self._lerobot_checked.get(path) - if cached is None or cached[0] != info_mtime: - result = self._lerobot.validate() - statuses = {e.index: self._lerobot.episode_check(e) for e in episodes} - cached = self._lerobot_checked[path] = (info_mtime, result, statuses) - _, result, statuses = cached - self.text_validation.setPlainText( - self._lerobot.summary() + f"\n\nChecks: {_STATUS_LABELS[result.overall_status]}\n" + result.summary_text() - ) - for ep in episodes: - status = statuses[ep.index] - item = QListWidgetItem(f"[{_STATUS_LABELS[status]}] {ep.label}") - item.setData(Qt.UserRole, ("lerobot", ep.index)) - item.setForeground(QBrush(QColor(_STATUS_COLORS[status]))) - self.list_demos.addItem(item) - - self.combo_camera.blockSignals(True) - self.combo_camera.clear() - self.combo_camera.addItems(self._lerobot.camera_keys) - self.combo_camera.blockSignals(False) - - def _close_lerobot(self): - if self._lerobot is not None: - self._lerobot.close() - self._lerobot = None - self._lerobot_episode = None - self._lerobot_arrays = None - - def load_lerobot_episode(self, index): - self._close_current_file() - ds = self._lerobot - episode = next((e for e in ds.episodes() if e.index == index), None) if ds else None - if episode is None: - return - try: - self._lerobot_arrays = ds.arrays(episode) - except LeRobotError as e: - logger.error(f"Cannot read episode {index}: {e}") - self.label_image.setText(str(e)) - return - self._lerobot_episode = episode - channels = [] - if ds.camera_keys: - channels.append(("image", None, "Image")) - for key in ("observation.state", "action"): - if key in self._lerobot_arrays: - channels.append(("lerobot", key, f"{key}\n({episode.length}x{self._lerobot_arrays[key].shape[1]})")) - self.slider_scrub.setMaximum(max(episode.length - 1, 0)) - self.slider_scrub.setValue(0) - self._show_frame(0) - self._populate_channel_buttons(channels) - - def _render_lerobot_plot(self, key): - arrays, ds = self._lerobot_arrays, self._lerobot - t, data = arrays["time"], arrays[key] - step = max(1, -(-len(t) // int(user_settings.get("review_plot_max_points")))) # decimate long episodes - names = ds.names(key) - plot = pg.PlotWidget(title=key) - plot.setLabel("bottom", "time", units="s") - plot.addLegend() - for i in range(data.shape[1]): - plot.plot(t[::step], data[::step, i], pen=pg.intColor(i, hues=max(data.shape[1], 1)), - name=names[i] if i < len(names) else f"{key}[{i}]") - self._add_playback_cursor(plot) - self._playback_cursor_line.setPos(t[min(self.slider_scrub.value(), len(t) - 1)]) - self.layout_display_graph.addWidget(plot) - # ------------------------------------------------------------------ # Demo loading / playback # ------------------------------------------------------------------ @@ -484,11 +336,7 @@ def _render_lerobot_plot(self, key): def _on_demo_selected(self, current, previous): if current is None: return - data = current.data(Qt.UserRole) - if isinstance(data, (tuple, list)): # ("lerobot", episode_index) - self.load_lerobot_episode(data[1]) - else: - self.load_demo(data) + self.load_demo(current.data(Qt.UserRole)) def load_demo(self, path): self._close_current_file() @@ -505,13 +353,9 @@ def load_demo(self, path): logger.error(f"Error opening demo file {path}: {e}") return - if "data" not in self._current_h5: - return - demo_names = list(self._current_h5["data"].keys()) - if not demo_names: + _, demo_group = first_demo(self._current_h5) + if demo_group is None: return - - demo_group = self._current_h5["data"][demo_names[0]] self._current_demo_group = demo_group self._load_image_frames(demo_group) @@ -527,8 +371,6 @@ def _close_current_file(self): self._current_demo_group = None self._current_image_dataset = None self._current_image_kind = None - self._lerobot_episode = None - self._lerobot_arrays = None self.slider_scrub.setMaximum(0) self.label_image.setText("No image data") self.label_frame_index.setText("Frame 0 / 0") @@ -636,11 +478,6 @@ def _select_channel(self, kind, name): self.display_stack.setCurrentWidget(self.graph_page) self._clear_layout(self.layout_display_graph) - if kind == "lerobot": - if self._lerobot_arrays is not None: - self._render_lerobot_plot(name) - return - demo_group = self._current_demo_group if demo_group is None: return @@ -717,14 +554,8 @@ def _load_image_frames(self, demo_group): def _on_scrub(self, value): self._show_frame(value) - if self._lerobot_arrays is not None and self._playback_cursor_line is not None: - t = self._lerobot_arrays["time"] - self._playback_cursor_line.setPos(t[min(value, len(t) - 1)]) def _show_frame(self, index): - if self._lerobot_episode is not None: - self._show_lerobot_frame(index) - return if self._current_image_dataset is None: return count = self._current_image_dataset.shape[0] @@ -750,20 +581,6 @@ def _show_frame(self, index): except Exception as e: logger.error(f"Error showing frame {index}: {e}") - def _show_lerobot_frame(self, index): - ep, ds = self._lerobot_episode, self._lerobot - camera = self.combo_camera.currentText() - if not camera: - self.label_image.setText("This dataset has no camera") - return - index = max(0, min(index, ep.length - 1)) - try: - self._display_rgb_frame(ds.frame(ep, camera, index)) - self.label_frame_index.setText(f"Frame {index} / {ep.length - 1} ({index / ds.fps:.2f} s)") - except (LeRobotError, IndexError) as e: - logger.error(f"Error showing frame {index}: {e}") - self.label_image.setText(str(e)) - def _display_cv_frame(self, cv_image): self._display_rgb_frame(cv2.cvtColor(cv_image, cv2.COLOR_BGR2RGB)) @@ -794,8 +611,6 @@ def delete_selected_demo(self): return path = item.data(Qt.UserRole) - if isinstance(path, (tuple, list)): # a LeRobot episode: view-only - return confirmed = not user_settings.get("confirm_destructive") or CustomMessageBox.question( f"Permanently delete this demo?\n\n{os.path.basename(path)}", title="Delete Demo", diff --git a/relay_d/acquisition/qt_app/ui/run/__init__.py b/relay_d/acquisition/qt_app/ui/run/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/relay_d/acquisition/qt_app/ui/run/run_page.py b/relay_d/acquisition/qt_app/ui/run/run_page.py new file mode 100644 index 0000000..9667af3 --- /dev/null +++ b/relay_d/acquisition/qt_app/ui/run/run_page.py @@ -0,0 +1,231 @@ +"""Run tab: start a trained policy on the robot (``lfd-inference-node``) and stop it safely. + +The node runs as a child process; its output is shown in the log view. Stop sends SIGINT, the node's +Ctrl+C path, which sends the safe stop and closes. +""" + +import os +import shutil +import signal +import sys + +from PyQt5.QtCore import QProcess, QProcessEnvironment, QTimer +from PyQt5.QtGui import QDoubleValidator +from PyQt5.QtWidgets import ( + QCheckBox, + QComboBox, + QFileDialog, + QFormLayout, + QGroupBox, + QHBoxLayout, + QLabel, + QLineEdit, + QPlainTextEdit, + QPushButton, + QWidget, +) + +from relay_d.utils import user_settings +from relay_d.utils.coloring_logger import logger +from ..page_style import add_page_header, page_layout +from ..settings.config_manager import config + +DISPATCH_FILES = ("obs_config.yaml", "action_config.yaml") +KILL_AFTER_MS = 5000 +LOG_MAX_LINES = 5000 + + +def build_args(checkpoint, dispatch_dir, device, hz, dry_run): + """Arguments of ``python -m relay_d.dispatch.inference_node``. Blank ``hz``: the policy's own rate.""" + args = ["-m", "relay_d.dispatch.inference_node", "--checkpoint", checkpoint, + "--obs-config", os.path.join(dispatch_dir, DISPATCH_FILES[0]), + "--action-config", os.path.join(dispatch_dir, DISPATCH_FILES[1]), + "--device", device] + if hz.strip(): + args += ["--hz", hz.strip()] + if dry_run: + args.append("--dry-run") + return args + + +def dispatch_dir_for(folder): + """A Post-process output folder means its ``action_dispatcher/`` subfolder.""" + sub = os.path.join(folder, "action_dispatcher") + return sub if os.path.isdir(sub) else folder + + +def missing_reason(checkpoint, dispatch_dir): + """Why Start cannot run yet ("" when it can).""" + if not checkpoint or not os.path.exists(checkpoint): + return "Choose a checkpoint (a robomimic .pth file or a LeRobot pretrained_model folder)." + missing = [f for f in DISPATCH_FILES if not os.path.isfile(os.path.join(dispatch_dir or "", f))] + if not dispatch_dir or missing: + return (f"The dispatcher folder needs {' and '.join(missing or DISPATCH_FILES)} " + "(Post-process writes them to /action_dispatcher).") + return "" + + +class RunPage: + def __init__(self, ui_instance): + self.ui = ui_instance + self.proc = QProcess(self.ui.page_run) + self.proc.setProcessChannelMode(QProcess.MergedChannels) + env = QProcessEnvironment.systemEnvironment() + env.insert("PYTHONUNBUFFERED", "1") # stream the log as it is written + self.proc.setProcessEnvironment(env) + self.proc.readyReadStandardOutput.connect(self._read_output) + self.proc.finished.connect(self._on_finished) + self.proc.errorOccurred.connect(self._on_error) + self._kill_timer = QTimer(self.ui.page_run) + self._kill_timer.setSingleShot(True) + self._kill_timer.timeout.connect(self._kill_if_alive) + self._build_ui() + self._update_buttons() + + def _build_ui(self): + root = page_layout(self.ui.page_run) + add_page_header(root, "Run a Policy") + + box = QGroupBox("Policy") + form = QFormLayout(box) + self.edit_checkpoint = QLineEdit(user_settings.get("run_last_checkpoint")) + self.edit_checkpoint.setPlaceholderText("robomimic .pth file or LeRobot pretrained_model folder") + btn_file, btn_folder = QPushButton("File..."), QPushButton("Folder...") + btn_file.clicked.connect(lambda: self._choose_checkpoint(folder=False)) + btn_folder.clicked.connect(lambda: self._choose_checkpoint(folder=True)) + form.addRow("Checkpoint", self._row(self.edit_checkpoint, btn_file, btn_folder)) + + self.edit_dispatch = QLineEdit(user_settings.get("run_last_dispatch_dir")) + self.edit_dispatch.setPlaceholderText("Folder with obs_config.yaml and action_config.yaml") + btn_dispatch = QPushButton("Browse...") + btn_dispatch.clicked.connect(self._choose_dispatch_dir) + form.addRow("Dispatcher folder", self._row(self.edit_dispatch, btn_dispatch)) + + self.combo_device = QComboBox() + self.combo_device.addItems(["cuda", "cpu"]) + self.combo_device.setCurrentText(user_settings.get("infer_device")) + form.addRow("Device", self.combo_device) + self.edit_hz = QLineEdit() + self.edit_hz.setValidator(QDoubleValidator(0.0, 1000.0, 3)) + self.edit_hz.setPlaceholderText("blank: the rate the policy was trained at") + form.addRow("Rate (Hz)", self.edit_hz) + self.check_dry_run = QCheckBox("Dry run (log commands, move nothing)") + self.check_dry_run.setChecked(True) + form.addRow("", self.check_dry_run) + root.addWidget(box) + for edit in (self.edit_checkpoint, self.edit_dispatch): + edit.textChanged.connect(self._update_buttons) + + self.label_reason = QLabel("") + self.label_reason.setWordWrap(True) + root.addWidget(self.label_reason) + + self.btn_start = QPushButton("Start") + self.btn_start.clicked.connect(self.start) + self.btn_stop = QPushButton("STOP") + self.btn_stop.setObjectName("btn_run_stop") # red in both themes (styles/*.qss) + self.btn_stop.setMinimumHeight(60) + self.btn_stop.clicked.connect(self.stop) + self.label_state = QLabel("Stopped") + root.addWidget(self._row(self.btn_start, self.label_state)) + root.addWidget(self.btn_stop) + + self.log = QPlainTextEdit() + self.log.setReadOnly(True) + self.log.setMaximumBlockCount(LOG_MAX_LINES) # a long run does not grow memory forever + root.addWidget(self.log, 1) + + @staticmethod + def _row(*widgets): + w = QWidget() + lay = QHBoxLayout(w) + lay.setContentsMargins(0, 0, 0, 0) + for i, x in enumerate(widgets): + lay.addWidget(x, 1 if i == 0 else 0) + return w + + def _choose_checkpoint(self, folder): + start = os.path.dirname(self.edit_checkpoint.text()) + path = (QFileDialog.getExistingDirectory(self.ui.page_run, "Choose a pretrained_model folder", start) if folder + else QFileDialog.getOpenFileName(self.ui.page_run, "Choose a checkpoint", start, "Checkpoints (*.pth *.pt);;All files (*)")[0]) + if path: + self.edit_checkpoint.setText(path) + + def _choose_dispatch_dir(self): + path = QFileDialog.getExistingDirectory(self.ui.page_run, "Choose the dispatcher (or Post-process output) folder", + self.edit_dispatch.text()) + if path: + self.edit_dispatch.setText(dispatch_dir_for(path)) + + # ------------------------------------------------------------------ process + + def is_running(self): + return self.proc.state() != QProcess.NotRunning + + def command(self): + """(program, arguments) of the node; the node re-executes itself in the backend's environment.""" + return sys.executable, build_args(self.edit_checkpoint.text(), self.edit_dispatch.text(), + self.combo_device.currentText(), self.edit_hz.text(), + self.check_dry_run.isChecked()) + + def _update_buttons(self): + running = self.is_running() + reason = "" if running else missing_reason(self.edit_checkpoint.text(), self.edit_dispatch.text()) + self.label_reason.setText(reason) + self.btn_start.setEnabled(not running and not reason) + self.btn_stop.setEnabled(running) + + def start(self): + if self.is_running(): + return + config.save_settings({"run_last_checkpoint": self.edit_checkpoint.text(), + "run_last_dispatch_dir": self.edit_dispatch.text()}) + program, args = self.command() + self.log.appendPlainText(f"$ {program} {' '.join(args)}") + if shutil.which("setsid"): # own process group: a terminal Ctrl+C reaches only the GUI, which stops the node once + program, args = "setsid", [program, *args] # the child is no group leader, so setsid execs: same PID + self.proc.start(program, args) + self.label_state.setText("Running") + self._update_buttons() + + def stop(self): + """SIGINT (the node's Ctrl+C: safe stop, then exit); killed if still alive after 5 s.""" + if not self.is_running(): + return + self.log.appendPlainText("Stopping (SIGINT)...") + os.kill(int(self.proc.processId()), signal.SIGINT) + self._kill_timer.start(KILL_AFTER_MS) + + def _kill_if_alive(self): + if self.is_running(): + msg = (f"The policy did not stop {KILL_AFTER_MS // 1000} s after SIGINT: KILLED. " + "The safe stop may NOT have been sent: check the robot.") + logger.error(msg) + self.log.appendPlainText(f"!!! {msg}") + self.proc.kill() + + def shutdown(self): + """Stop a running policy before the window closes (blocks at most ~5 s).""" + if not self.is_running(): + return + self.stop() + if not self.proc.waitForFinished(KILL_AFTER_MS): + self._kill_if_alive() + self.proc.waitForFinished(1000) + + def _read_output(self): + self.log.appendPlainText(bytes(self.proc.readAllStandardOutput()).decode(errors="replace").rstrip("\n")) + + def _on_finished(self, code, status): + self._kill_timer.stop() + ok = status == QProcess.NormalExit and code == 0 + self.label_state.setText("Stopped" if ok else "Failed (killed by a signal)" if status == QProcess.CrashExit + else f"Failed (exit {code})") + self.log.appendPlainText(f"Process ended, exit code {code}.") + self._update_buttons() + + def _on_error(self, error): + if error == QProcess.FailedToStart: + self.label_state.setText("Failed (did not start)") + self.log.appendPlainText(f"Could not start: {self.proc.errorString()}") + self._update_buttons() diff --git a/relay_d/acquisition/qt_app/ui/styles/relayd_dark.qss b/relay_d/acquisition/qt_app/ui/styles/relayd_dark.qss index 0ce83a1..1bdfaf5 100644 --- a/relay_d/acquisition/qt_app/ui/styles/relayd_dark.qss +++ b/relay_d/acquisition/qt_app/ui/styles/relayd_dark.qss @@ -157,34 +157,9 @@ QPushButton#btn_start_record:pressed { } -/* — Pause Record (amber) — */ - -QPushButton#btn_pause_record { - background-color: rgba(251, 191, 36, 0.10); - border: 1px solid rgba(251, 191, 36, 0.25); - border-left: 4px solid #FBBF24; - color: #FDE68A; - font-weight: 600; - border-radius: 8px; - padding: 10px 16px; - text-align: left; -} - -QPushButton#btn_pause_record:hover { - background-color: rgba(251, 191, 36, 0.18); - border-color: rgba(251, 191, 36, 0.40); - border-left: 5px solid #F59E0B; -} - -QPushButton#btn_pause_record:pressed { - background-color: rgba(251, 191, 36, 0.25); - border-left: 6px solid #D97706; -} - - /* — Stop Record (red) — */ -QPushButton#btn_stop_record { +QPushButton#btn_stop_record, QPushButton#btn_run_stop { background-color: rgba(248, 113, 113, 0.10); border: 1px solid rgba(248, 113, 113, 0.25); border-left: 4px solid #F87171; @@ -195,18 +170,18 @@ QPushButton#btn_stop_record { text-align: left; } -QPushButton#btn_stop_record:hover { +QPushButton#btn_stop_record:hover, QPushButton#btn_run_stop:hover { background-color: rgba(248, 113, 113, 0.18); border-color: rgba(248, 113, 113, 0.40); border-left: 5px solid #EF4444; } -QPushButton#btn_stop_record:pressed { +QPushButton#btn_stop_record:pressed, QPushButton#btn_run_stop:pressed { background-color: rgba(248, 113, 113, 0.25); border-left: 6px solid #DC2626; } -QPushButton#btn_stop_record:disabled { +QPushButton#btn_stop_record:disabled, QPushButton#btn_run_stop:disabled { background-color: #22262E; color: #4B5563; border: 1px solid #333942; @@ -316,14 +291,13 @@ QLabel#recordingSettingsLabel { font-weight: 600; } -QLabel#inputStructureLabel { - background-color: rgba(96, 165, 250, 0.10); - border: 1px solid rgba(96, 165, 250, 0.30); +QLabel#precheckError { + background-color: rgba(248, 113, 113, 0.10); + border: 1px solid rgba(248, 113, 113, 0.25); + border-left: 4px solid #F87171; border-radius: 8px; - color: #93C5FD; - padding: 10px; - font-size: 15px; - font-weight: 600; + color: #FCA5A5; + padding: 8px 12px; } QLabel#outputPreviewLabel { @@ -800,7 +774,6 @@ QFrame#importSeparator { * 17. PANEL SUB-LABELS (compact, left-aligned) * --------------------------------------------------------------------------- */ -QLabel#inputStructureLabel, QLabel#outputPreviewLabel { background: transparent; border: none; diff --git a/relay_d/acquisition/qt_app/ui/styles/relayd_light.qss b/relay_d/acquisition/qt_app/ui/styles/relayd_light.qss index c4c04c7..5777430 100644 --- a/relay_d/acquisition/qt_app/ui/styles/relayd_light.qss +++ b/relay_d/acquisition/qt_app/ui/styles/relayd_light.qss @@ -147,34 +147,9 @@ QPushButton#btn_start_record:pressed { } -/* — Pause Record (amber) — */ - -QPushButton#btn_pause_record { - background-color: #FFFBEB; - border: 1px solid #FDE68A; - border-left: 4px solid #F59E0B; - color: #92400E; - font-weight: 600; - border-radius: 8px; - padding: 10px 16px; - text-align: left; -} - -QPushButton#btn_pause_record:hover { - background-color: #FEF3C7; - border-color: #FCD34D; - border-left: 5px solid #D97706; -} - -QPushButton#btn_pause_record:pressed { - background-color: #FDE68A; - border-left: 6px solid #B45309; -} - - /* — Stop Record (red) — */ -QPushButton#btn_stop_record { +QPushButton#btn_stop_record, QPushButton#btn_run_stop { background-color: #FEF2F2; border: 1px solid #FECACA; border-left: 4px solid #EF4444; @@ -185,18 +160,18 @@ QPushButton#btn_stop_record { text-align: left; } -QPushButton#btn_stop_record:hover { +QPushButton#btn_stop_record:hover, QPushButton#btn_run_stop:hover { background-color: #FEE2E2; border-color: #FCA5A5; border-left: 5px solid #DC2626; } -QPushButton#btn_stop_record:pressed { +QPushButton#btn_stop_record:pressed, QPushButton#btn_run_stop:pressed { background-color: #FECACA; border-left: 6px solid #B91C1C; } -QPushButton#btn_stop_record:disabled { +QPushButton#btn_stop_record:disabled, QPushButton#btn_run_stop:disabled { background-color: #F9FAFB; color: #D1D5DB; border: 1px solid #E5E7EB; @@ -306,14 +281,13 @@ QLabel#recordingSettingsLabel { font-weight: 600; } -QLabel#inputStructureLabel { - background-color: #EFF6FF; - border: 1px solid #BFDBFE; +QLabel#precheckError { + background-color: #FEF2F2; + border: 1px solid #FECACA; + border-left: 4px solid #EF4444; border-radius: 8px; - color: #1E40AF; - padding: 10px; - font-size: 15px; - font-weight: 600; + color: #991B1B; + padding: 8px 12px; } QLabel#outputPreviewLabel { @@ -790,7 +764,6 @@ QFrame#importSeparator { * 17. PANEL SUB-LABELS (compact, left-aligned) * --------------------------------------------------------------------------- */ -QLabel#inputStructureLabel, QLabel#outputPreviewLabel { background: transparent; border: none; diff --git a/relay_d/acquisition/qt_app/ui/train/train_page.py b/relay_d/acquisition/qt_app/ui/train/train_page.py index bf25025..a39e004 100644 --- a/relay_d/acquisition/qt_app/ui/train/train_page.py +++ b/relay_d/acquisition/qt_app/ui/train/train_page.py @@ -36,7 +36,6 @@ from ..page_style import add_page_header, page_layout from ..settings.config_manager import config -_FROM_FOLDER = "From the folder's config" # robomimic: use robomimic_config.json / postprocess options _REFRESH_MS = 2000 # run table refresh @@ -185,7 +184,7 @@ def _on_dataset_changed(self): self.combo_policy.clear() if ds is not None: if ds.format == launcher.ROBOMIMIC: - self.combo_policy.addItems([_FROM_FOLDER, *launcher.ROBOMIMIC_POLICIES]) + self.combo_policy.addItems(launcher.ROBOMIMIC_POLICIES) else: self.combo_policy.addItems(launcher.LEROBOT_POLICIES) i = launcher.LEROBOT_POLICIES.index("smolvla") @@ -211,7 +210,7 @@ def _on_dataset_changed(self): def _policy(self): text = self.combo_policy.currentText() - return None if text in ("", _FROM_FOLDER) else text + return text or None def _on_policy_changed(self): for widget, label in self._param_widgets.values(): diff --git a/relay_d/acquisition/qt_app/utils/stream_builder.py b/relay_d/acquisition/qt_app/utils/stream_builder.py index dd7880b..9f6d384 100644 --- a/relay_d/acquisition/qt_app/utils/stream_builder.py +++ b/relay_d/acquisition/qt_app/utils/stream_builder.py @@ -14,6 +14,7 @@ import numpy as np from relay_d.utils.coloring_logger import logger +from relay_d.utils.numerics import enriched_tf, read_scalar, strip_trig try: from sensor_msgs.msg import CompressedImage, Image, PointCloud2 @@ -23,29 +24,10 @@ _UNSUPPORTED_STREAM_MSG_TYPES = () -def fill_none_gaps(data_list): - """Forward-fill then back-fill None entries so all streams have equal length. - - Forward-fill handles Nones in the middle/end; back-fill covers leading Nones - that arise when a stream's inputs haven't produced a valid row yet. - Returns the original list unchanged when all entries are None. - """ - if not any(item is not None for item in data_list): - return data_list - filled = list(data_list) - last_valid = None - for i, item in enumerate(filled): - if item is not None: - last_valid = item - elif last_valid is not None: - filled[i] = last_valid - last_valid = None - for i in range(len(filled) - 1, -1, -1): - if filled[i] is not None: - last_valid = filled[i] - elif last_valid is not None: - filled[i] = last_valid - return filled +# Same spec grammar as relay_d/dispatch/obs_builder.py, so a recorded stream has the width and values +# the live obs will have: " " needs spaces around the operator. +_BINOP_RE = re.compile(r"^(.+?)\s+([+\-*/])\s+(.+)$") +_BIN_OPS = {"+": operator.add, "-": operator.sub, "*": operator.mul, "/": operator.truediv} class StreamBuilder: @@ -87,10 +69,9 @@ def referenced_input_names(self) -> set: for extractor in extractors: if extractor[0] == "simple": names.add(extractor[1]) - else: # "binary" - _kind, _op, (in_a, _ga), (in_b, _gb) = extractor - names.add(in_a) - names.add(in_b) + elif extractor[0] == "binary": + names.add(extractor[2][0]) + names.add(extractor[3][0]) return names def _build_stream_extractors(self): @@ -115,97 +96,72 @@ def _build_stream_extractors(self): self._joint_name_filters[entry["name"]] = frozenset(names) logger.info(f"Joint filter for '{entry['name']}': {list(names)}") - # Matches " " with a single +/- surrounded by - # whitespace. Operand chars exclude space/+/-, so ROS-style identifiers, - # dotted field paths and a trailing [:] are captured but the operator is - # not, making the split unambiguous. - binary_re = re.compile( - r"^(?P[\w.\[\]:]+)\s+(?P[+\-])\s+(?P[\w.\[\]:]+)$" - ) - binary_ops = {"+": operator.add, "-": operator.sub} - - def _parse_operand(operand): - """Resolve one dotted operand to (input_name, getter, is_slice). + def _error(msg): + logger.error(msg) + self.build_errors.append(msg) - Returns None (and logs) on an operand with no dot separator so the - caller can skip the whole spec. - """ + def _parse_operand(operand, spec): + """Resolve one dotted operand to (input_name, getter, is_slice), or None (logged).""" operand = operand.strip() - try: - first_dot = operand.index(".") - except ValueError: - msg = f"Stream spec operand '{operand}' has no dot separator — skipped" - logger.error(msg) - self.build_errors.append(msg) + if "." not in operand: + _error(f"Stream spec '{spec}': operand '{operand}' has no dot separator — skipped") return None - input_name = operand[:first_dot] - raw_field = operand[first_dot + 1:] + input_name, raw_field = operand.split(".", 1) is_slice = raw_field.endswith("[:]") field_path = raw_field[:-3] if is_slice else raw_field + if any(c in field_path for c in "+-*/"): + _error( + f"Stream spec '{spec}': arithmetic operators must have spaces " + f"around them (e.g. 'a.x - b.x') — skipped" + ) + return None if input_name in self._tf_input_names: field_path = "transform." + field_path return input_name, operator.attrgetter(field_path), is_slice streams = self.yaml_parser.get_streams() for stream_name, specs in streams.items(): - if isinstance(specs, dict): # dict form — extract spec list, ignore normalize/limits + if isinstance(specs, dict): # dict form — extract spec list, ignore rotation_format specs = specs.get("specs", []) extractor_list = [] for spec in specs: - # Detect sin(...) / cos(...) wrappers, e.g. sin(joint_1.position[:]) - math_op = None - inner_spec = spec.strip() - if inner_spec.startswith(("sin(", "cos(")) and inner_spec.endswith(")"): - func_name = inner_spec[:3] - inner_spec = inner_spec[4:-1].strip() - math_op = np.sin if func_name == "sin" else np.cos + # sin(...)/cos(...) enclosing the whole spec, e.g. sin(joint_1.position[:]) or sin(a.x - b.x) + math_op, inner_spec = strip_trig(spec.strip()) - # Binary arithmetic spec: +|- - bin_match = binary_re.match(inner_spec) - if bin_match: - if math_op is not None: - msg = ( - f"Stream spec '{spec}' combines sin/cos with arithmetic " - f"— unsupported, skipped" - ) - logger.error(msg) - self.build_errors.append(msg) - continue - parsed_a = _parse_operand(bin_match.group("a")) - parsed_b = _parse_operand(bin_match.group("b")) - if parsed_a is None or parsed_b is None: - continue # _parse_operand already logged - in_a, get_a, slice_a = parsed_a - in_b, get_b, slice_b = parsed_b - if slice_a or slice_b: - msg = ( - f"Stream spec '{spec}': arithmetic operands must be scalars " - f"([:] not allowed) — skipped" - ) - logger.error(msg) - self.build_errors.append(msg) - continue - op_func = binary_ops[bin_match.group("op")] - extractor_list.append( - ("binary", op_func, (in_a, get_a), (in_b, get_b)) - ) + if inner_spec.startswith("zero:"): + try: + extractor_list.append(("zero", int(inner_spec[5:]))) + except ValueError: + _error(f"Stream spec '{spec}': zero:N needs an integer N — skipped") continue - # Guard against a +/- operator without surrounding spaces, which - # would otherwise mis-parse as a single garbage field path. - if ("+" in inner_spec or "-" in inner_spec): - msg = ( - f"Stream spec '{spec}': arithmetic operators must have spaces " - f"around them (e.g. 'a.x - b.x') — skipped" - ) - logger.error(msg) - self.build_errors.append(msg) + # Binary arithmetic spec: +|-|*|/ , each side may have its own sin/cos + bin_match = _BINOP_RE.match(inner_spec) + if bin_match: + sides = [] + for side in (bin_match.group(1).strip(), bin_match.group(3).strip()): + if _BINOP_RE.match(side): + _error(f"Stream spec '{spec}': only one arithmetic operation per spec — skipped") + break + side_op, side_spec = strip_trig(side) + parsed = _parse_operand(side_spec, spec) + if parsed is None: + break + if parsed[2]: + _error( + f"Stream spec '{spec}': arithmetic operands must be scalars " + f"([:] not allowed) — skipped" + ) + break + sides.append((parsed[0], parsed[1], side_op)) + if len(sides) == 2: + extractor_list.append(("binary", _BIN_OPS[bin_match.group(2)], sides[0], sides[1], math_op)) continue # Simple single-operand spec. - parsed = _parse_operand(inner_spec) + parsed = _parse_operand(inner_spec, spec) if parsed is None: - continue # _parse_operand already logged the no-dot error + continue input_name, getter, is_slice = parsed extractor_list.append(("simple", input_name, getter, is_slice, math_op)) if not extractor_list: @@ -269,57 +225,50 @@ def _warn_unsupported_type(self, stream_name: str, input_name: str, msg) -> None f"See the 'output.streams' comment block in config_template.yaml." ) + def _input(self, stream_name, input_name, all_data): + """The message of ``input_name`` ready for the getters, or None (no data / unsupported type).""" + msg = all_data.get(input_name) + if msg is None: + return None + if isinstance(msg, _UNSUPPORTED_STREAM_MSG_TYPES): + self._warn_unsupported_type(stream_name, input_name, msg) + return None + if input_name in self._tf_input_names: + return enriched_tf(msg) # euler.* / rot6d.* / rot_matrix.*, as obs_builder sees TF + return self.apply_joint_filter(input_name, msg) + def evaluate(self, all_data: dict): for stream_name, extractors in self._stream_extractors.items(): row = [] ok = True for extractor in extractors: - if extractor[0] == "simple": - _kind, input_name, getter, is_slice, math_op = extractor - msg = all_data.get(input_name) - if msg is None: - ok = False - break - if isinstance(msg, _UNSUPPORTED_STREAM_MSG_TYPES): - self._warn_unsupported_type(stream_name, input_name, msg) - ok = False - break - msg = self.apply_joint_filter(input_name, msg) - try: - val = getter(msg) + try: + if extractor[0] == "zero": + row.extend([0.0] * extractor[1]) + elif extractor[0] == "simple": + _kind, input_name, getter, is_slice, math_op = extractor + msg = self._input(stream_name, input_name, all_data) + if msg is None: + ok = False + break if is_slice: + val = getter(msg) vals = list(val) if isinstance(val, _array.array) else list(val[:]) - if math_op is not None: - vals = [float(math_op(v)) for v in vals] - row.extend(vals) + row.extend(float(math_op(v)) if math_op is not None else float(v) for v in vals) else: - v = float(val) - row.append(float(math_op(v)) if math_op is not None else v) - except (AttributeError, TypeError, IndexError, ValueError): # e.g. a short joint array: hold the last row - ok = False - break - else: # "binary": op_func(operand_a, operand_b), both scalars - _kind, op_func, (in_a, get_a), (in_b, get_b) = extractor - msg_a = all_data.get(in_a) - msg_b = all_data.get(in_b) - if msg_a is None or msg_b is None: - ok = False - break - if isinstance(msg_a, _UNSUPPORTED_STREAM_MSG_TYPES): - self._warn_unsupported_type(stream_name, in_a, msg_a) - ok = False - break - if isinstance(msg_b, _UNSUPPORTED_STREAM_MSG_TYPES): - self._warn_unsupported_type(stream_name, in_b, msg_b) - ok = False - break - msg_a = self.apply_joint_filter(in_a, msg_a) - msg_b = self.apply_joint_filter(in_b, msg_b) - try: - row.append(float(op_func(float(get_a(msg_a)), float(get_b(msg_b))))) - except (AttributeError, TypeError, IndexError, ValueError): # e.g. a short joint array: hold the last row - ok = False - break + row.append(read_scalar(getter, msg, math_op)) + else: # "binary": outer_op(side_op(a) side_op(b)), both scalars + _kind, op_func, (in_a, get_a, op_a), (in_b, get_b, op_b), math_op = extractor + msg_a = self._input(stream_name, in_a, all_data) + msg_b = self._input(stream_name, in_b, all_data) + if msg_a is None or msg_b is None: + ok = False + break + v = float(op_func(read_scalar(get_a, msg_a, op_a), read_scalar(get_b, msg_b, op_b))) + row.append(float(math_op(v)) if math_op is not None else v) + except (AttributeError, TypeError, IndexError, ValueError, ZeroDivisionError): # e.g. a short joint array: hold the last row + ok = False + break buf = self._stream_buffers.setdefault(stream_name, []) if ok: self._stream_last_valid[stream_name] = row @@ -335,8 +284,8 @@ def finalize(self) -> dict: """Return {stream_name: (N, M) float32 array} for every stream with data.""" result = {} for stream_name, rows in self._stream_buffers.items(): - filled_rows = fill_none_gaps(rows) - valid = [r for r in filled_rows if r is not None] - if valid: - result[stream_name] = np.array(valid, dtype=np.float32) + # evaluate() already holds the last row; only the rows before the first valid one are None + first = next((r for r in rows if r is not None), None) + if first is not None: + result[stream_name] = np.array([first if r is None else r for r in rows], dtype=np.float32) return result diff --git a/relay_d/dispatch/action_dispatcher.py b/relay_d/dispatch/action_dispatcher.py index 7ee4a43..ef98469 100644 --- a/relay_d/dispatch/action_dispatcher.py +++ b/relay_d/dispatch/action_dispatcher.py @@ -6,6 +6,16 @@ action: false (or omitted) -> node.create_publisher -> publish() action: true -> ActionClient -> send_goal_async() + +The model already outputs real units (robomimic/LeRobot un-normalize with the +checkpoint's stats). An entry's `limits` only clamps each value to +[lower_lim, upper_lim] before its transform; there is no de-normalization. + +dry_run=True: no publisher or action client is created; every message and goal +is still built and checked, then logged ("[DRY RUN] ...") instead of sent. + +send_stop() (inference stops) publishes each topic entry's `stop:` values and +cancels every goal it sent that is still active. """ import re @@ -13,9 +23,8 @@ import time import numpy as np -import subprocess -import shlex +from action_msgs.msg import GoalStatus from rclpy.node import Node from rclpy.action import ActionClient from typing import Dict, Any @@ -24,6 +33,20 @@ from relay_d.utils.coloring_logger import logger from relay_d.utils.config_loader import load_yaml_config +_GOAL_DONE = (GoalStatus.STATUS_SUCCEEDED, GoalStatus.STATUS_CANCELED, GoalStatus.STATUS_ABORTED) + +# Removed config: an old action_config that still has one fails at startup instead of being ignored. +_REMOVED_KEYS = { + "command": "shell commands are no longer run; use a topic entry (topic + msg_type + mapping) or an action entry (action: true)", + "dedupe": "topic entries publish every tick; delete the key", +} +_REMOVED_TRANSFORMS = { + "scale": "use limits for clamping, or record the action in the units the robot takes", + "clip": "use limits for clamping", + "vacuum": "use a threshold transform with on_value / off_value and a topic or action entry", + "magnetic": "use a threshold transform with on_value / off_value and a topic or action entry", +} + def get_action(action_type_str: str): """ Dynamically load a ROS2 action class from a string like @@ -176,8 +199,11 @@ def _fill_msg(msg: Any, cfg: dict, slice_values: np.ndarray, node: Node) -> bool class ActionDispatcher: - def __init__(self, node: Node, action_config_path: str): + def __init__(self, node: Node, action_config_path: str, dry_run: bool = False): self._node = node + self._dry_run = dry_run + if dry_run: + logger.warning("[ActionDispatcher] DRY RUN: nothing is published or sent to the robot, commands are only logged.") config = load_yaml_config(action_config_path) @@ -187,25 +213,29 @@ def __init__(self, node: Node, action_config_path: str): self._goal_classes: Dict[str, Any] = {} self._msg_classes: Dict[str, Any] = {} self._last_sent: Dict[str, str] = {} - self._procs: Dict[str, subprocess.Popen] = {} - self._stop_procs: list = [] # stop commands: close() must not terminate them self._stopped = True # nothing commanded yet, so there is nothing to stop (see send_stop) + self._goals = [] # (name, accepted goal handle; None in a dry run) since the last stop: send_stop cancels them + self._goals_lock = threading.Lock() # goals are accepted on the executor thread # feedback: -- opt-in ack-wait for topic entries (see _wait_for_ack) self._ack_latest: Dict[str, Any] = {} self._ack_events: Dict[str, threading.Event] = {} for name, cfg in self._action_configs.items(): - self._check_transform(name, cfg.get("transform")) - is_action = cfg.get("action", False) - is_command = "command" in cfg - - if is_command: - logger.info( - f"[ActionDispatcher] '{name}' -> COMMAND '{cfg['command'][:50]}...'" - ) - - elif is_action: + for key, why in _REMOVED_KEYS.items(): + if key in cfg: + raise ValueError(f"[ActionDispatcher] '{name}': '{key}' was removed: {why}.") + removed = (cfg.get("transform") or {}).get("type") + if removed in _REMOVED_TRANSFORMS: + raise ValueError(f"[ActionDispatcher] '{name}': transform '{removed}' was removed: " + f"{_REMOVED_TRANSFORMS[removed]}.") + if cfg.get("denormalize") or cfg.get("apply_limits") or "normalized_range" in cfg: + raise ValueError(f"[ActionDispatcher] '{name}': the model already outputs real units; remove " + f"denormalize / normalized_range (limits now only clamp).") + if "limits" in cfg and len(cfg["limits"]) not in (1, len(cfg["indices"])): + raise ValueError(f"[ActionDispatcher] '{name}': 'limits' needs 1 entry or one per index " + f"({len(cfg['indices'])}), got {len(cfg['limits'])}.") + if cfg.get("action", False): if "action_type" not in cfg: raise ValueError( f"[ActionDispatcher] '{name}' has action: true " @@ -221,9 +251,10 @@ def __init__(self, node: Node, action_config_path: str): action_class = get_action(cfg["action_type"]) self._check_mapping(name, cfg, action_class.Goal) self._goal_classes[name] = action_class.Goal - self._action_clients[name] = ActionClient( - node, action_class, cfg["topic"] - ) + if not dry_run: + self._action_clients[name] = ActionClient( + node, action_class, cfg["topic"] + ) logger.info( f"[ActionDispatcher] '{name}' -> ACTION {cfg['topic']} " f"({cfg['action_type']})" @@ -244,9 +275,10 @@ def __init__(self, node: Node, action_config_path: str): msg_class = get_message(cfg["msg_type"]) self._check_mapping(name, cfg, msg_class) self._msg_classes[name] = msg_class - self._publishers[name] = node.create_publisher( - msg_class, cfg["topic"], 10 - ) + if not dry_run: + self._publishers[name] = node.create_publisher( + msg_class, cfg["topic"], 10 + ) logger.info( f"[ActionDispatcher] '{name}' -> TOPIC {cfg['topic']} " f"({cfg['msg_type']})" @@ -283,6 +315,7 @@ def __init__(self, node: Node, action_config_path: str): def dispatch(self, action: np.ndarray) -> None: if not np.isfinite(action).all(): logger.error(f"[ActionDispatcher] Non-finite action, not dispatching: {action}") + self.send_stop() return # Phase 1: compute every output. If one config entry is broken (bad slice, unknown transform) @@ -291,42 +324,26 @@ def dispatch(self, action: np.ndarray) -> None: for name, cfg in self._action_configs.items(): slice_values = action[cfg["indices"]] - # 1. Per-index denormalization: model output range -> real-world range - if "limits" in cfg and cfg.get("denormalize", cfg.get("apply_limits", True)): - slice_values = self._apply_limits( - slice_values, - cfg["limits"], - cfg.get("normalized_range", [-1.0, 1.0]), - ) + # 1. Safety clamp to [lower_lim, upper_lim], in real units + if "limits" in cfg: + slice_values = self._apply_limits(slice_values, cfg["limits"]) - # 2. Optional further transform (vacuum, magnetic, scale, etc.) + # 2. Optional further transform (threshold, euler_to_quat) if "transform" in cfg: transformed = self._apply_transform(slice_values, cfg["transform"]) else: transformed = slice_values - values = list(transformed.values()) if isinstance(transformed, dict) else transformed - if not np.isfinite(values).all(): # e.g. a `scale` transform with in_min == in_max + if not np.isfinite(transformed).all(): # e.g. a NaN in `limits` logger.error(f"[ActionDispatcher] '{name}' output is not finite after its transform, not dispatching: {transformed}") + self.send_stop() return outputs.append((name, cfg, transformed)) # Phase 2: send. self._stopped = False for name, cfg, transformed in outputs: - # Command template path — simplest, just shell out - if "command" in cfg: - cmd = self._render_command(cfg["command"], transformed) - if self._last_sent.get(name) == cmd: - continue - proc = self._procs.get(name) - if proc is not None and proc.poll() is None: - continue # previous command still running: retry next tick, don't pile up - # Remember it only once it was started, so a failed start is retried. - if self._dispatch_command(name, cfg, cmd): - self._last_sent[name] = cmd - # ROS2 action client path - elif cfg.get("action", False): + if cfg.get("action", False): key = str(transformed) if self._last_sent.get(name) == key: continue @@ -336,14 +353,9 @@ def dispatch(self, action: np.ndarray) -> None: if not self._dispatch_action(name, cfg, transformed): self._last_sent.pop(name, None) - # Publisher path: sent every tick (controllers with a watchdog need a stream); - # set `dedupe: true` on the entry to skip repeats. + # Publisher path: sent every tick (controllers with a watchdog need a stream). else: - key = str(transformed) - if cfg.get("dedupe", False) and self._last_sent.get(name) == key: - continue - if self._dispatch_topic(name, cfg, transformed): - self._last_sent[name] = key + self._dispatch_topic(name, cfg, transformed) def send_stop(self) -> None: """Send each entry's ``stop:`` values once, e.g. a zero velocity: called when inference stops sending (inputs @@ -353,47 +365,50 @@ def send_stop(self) -> None: """ if self._stopped: return - self._stopped = True + with self._goals_lock: + self._stopped = True + goals, self._goals = self._goals, [] + for name, goal_handle in goals: + self._cancel_goal(name, goal_handle) for name, cfg in self._action_configs.items(): if "stop" not in cfg: continue - # The first command after the stop must go out even if it equals the one before (commands are deduped). - self._last_sent.pop(name, None) try: - values = np.asarray(cfg["stop"], dtype=np.float64) - if "command" in cfg: - running = self._procs.pop(name, None) - if running is not None and running.poll() is None: - running.terminate() # an older motion command must not keep running after its stop - self._stop_procs.append(subprocess.Popen( - shlex.split(self._render_command(cfg["command"], values)), - stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL, - )) - else: - msg = self._stop_message(name, cfg, values) - if msg is None: - raise ValueError("a mapped field could not be set") - self._publishers[name].publish(msg) + msg = self._stop_message(name, cfg, np.asarray(cfg["stop"], dtype=np.float64)) + if msg is None: + raise ValueError("a mapped field could not be set") + if self._dry_run: + logger.warning(f"[DRY RUN] '{name}' stop {cfg['stop']} -> {msg}") + continue + self._publishers[name].publish(msg) logger.warning(f"[ActionDispatcher] '{name}': sent stop {cfg['stop']}") except Exception as e: logger.error(f"[ActionDispatcher] '{name}': stop could not be sent: {e}") - def close(self) -> None: - """Stop the shell commands this dispatcher started and is still running (stop commands are left to finish).""" - for proc in self._procs.values(): - if proc.poll() is None: - proc.terminate() - # ------------------------------------------------------------------------- # Private # ------------------------------------------------------------------------- - @staticmethod - def _check_transform(name: str, transform: Any) -> None: - """A scale with in_min == in_max divides by zero. Found at startup, not the first time the gripper switches on.""" - for scale in (transform or {}, (transform or {}).get("strength_scale") or {}): - if "in_min" in scale and "in_max" in scale and float(scale["in_min"]) == float(scale["in_max"]): - raise ValueError(f"[ActionDispatcher] '{name}': transform needs in_min != in_max, both are {scale['in_min']}.") + def _track_goal(self, name: str, goal_handle: Any) -> None: + """Remember a sent goal for send_stop; one accepted after the stop is cancelled at once.""" + with self._goals_lock: + if not self._stopped: + self._goals = [g for g in self._goals if g[1] is None or g[1].status not in _GOAL_DONE] + if goal_handle is not None or not any(n == name and h is None for n, h in self._goals): + self._goals.append((name, goal_handle)) + return + self._cancel_goal(name, goal_handle) + + def _cancel_goal(self, name: str, goal_handle: Any) -> None: + """Ask the server to cancel the goal if still active. Not awaited: the stop path must not block. Never raises.""" + try: + if goal_handle is None: + logger.warning(f"[DRY RUN] '{name}' cancel the sent goal") + elif goal_handle.status not in _GOAL_DONE: + goal_handle.cancel_goal_async() + logger.warning(f"[ActionDispatcher] '{name}': cancelling the active goal") + except Exception as e: + logger.error(f"[ActionDispatcher] '{name}': goal could not be cancelled: {e}") def _stop_message(self, name: str, cfg: dict, values: np.ndarray) -> Any: """The topic message carrying ``stop:`` values (no limits, no transform), or None if the mapping does not fit.""" @@ -405,58 +420,29 @@ def _stop_message(self, name: str, cfg: dict, values: np.ndarray) -> Any: def _check_stop(self, name: str, cfg: dict) -> None: """A broken ``stop:`` must stop startup: it is only needed at the moment something already went wrong.""" stop = cfg["stop"] - if cfg.get("action", False) and "command" not in cfg: - raise ValueError(f"[ActionDispatcher] '{name}': 'stop' is for topic and command entries; " - f"an action goal is stopped by cancelling it on the action server.") + if cfg.get("action", False): + raise ValueError(f"[ActionDispatcher] '{name}': 'stop' is for topic entries; " + f"an action entry's still-active goal is cancelled when inference stops.") if not isinstance(stop, list) or not stop or not all( isinstance(v, (int, float)) and not isinstance(v, bool) and np.isfinite(v) for v in stop): raise ValueError(f"[ActionDispatcher] '{name}': 'stop' must be a list of numbers, got {stop!r}.") - values = np.asarray(stop, dtype=np.float64) - if "command" in cfg: - if re.search(r'\{I\[\d+\]\}', self._render_command(cfg["command"], values)): - raise ValueError(f"[ActionDispatcher] '{name}': 'stop' has {len(stop)} values, too few for the command's {{I[n]}}.") - elif self._stop_message(name, cfg, values) is None: + if self._stop_message(name, cfg, np.asarray(stop, dtype=np.float64)) is None: raise ValueError(f"[ActionDispatcher] '{name}': 'stop' does not fit 'mapping' (see the warnings above).") def _check_mapping(self, name: str, cfg: dict, msg_class: Any) -> None: """Fill one message of ``msg_class`` with dummy values, so a broken ``mapping`` / ``goal_init`` stops startup. Found at the first tick it would send the other entries (the arm) without this one.""" values = np.zeros(len(cfg["indices"]), dtype=np.float32) - if "limits" in cfg and len(cfg["limits"]) not in (1, len(cfg["indices"])): - raise ValueError(f"[ActionDispatcher] '{name}': 'limits' needs 1 entry or one per index " - f"({len(cfg['indices'])}), got {len(cfg['limits'])}.") - if "limits" in cfg and cfg.get("denormalize", cfg.get("apply_limits", True)): - values = self._apply_limits(values, cfg["limits"], cfg.get("normalized_range", [-1.0, 1.0])) + if "limits" in cfg: + values = self._apply_limits(values, cfg["limits"]) if "transform" in cfg: - with np.errstate(all="ignore"): - values = self._apply_transform(values, cfg["transform"]) - if isinstance(values, np.ndarray): - values = np.nan_to_num(values) # only the field names and indices are checked here + values = self._apply_transform(values, cfg["transform"]) msg = msg_class() if "goal_init" in cfg: self._init_goal(msg, cfg["goal_init"], name) if not _fill_msg(msg, cfg, values, self._node): raise ValueError(f"[ActionDispatcher] '{name}': 'mapping' does not fit {msg_class.__name__} (see the warnings above).") - def _dispatch_command( - self, name: str, cfg: dict, cmd: str - ) -> bool: - logger.debug( - f"[ActionDispatcher] Running command for '{name}':\n {cmd}" - ) - try: - self._procs[name] = subprocess.Popen( - shlex.split(cmd), - stdout=subprocess.DEVNULL, - stderr=subprocess.DEVNULL, - ) - return True - except Exception as e: - logger.warning( - f"[ActionDispatcher] Command failed for '{name}': {e}\n cmd: {cmd}" - ) - return False - def _dispatch_topic(self, name: str, cfg: dict, slice_values: np.ndarray) -> bool: msg = self._msg_classes[name]() if "goal_init" in cfg: @@ -464,6 +450,9 @@ def _dispatch_topic(self, name: str, cfg: dict, slice_values: np.ndarray) -> boo if not _fill_msg(msg, cfg, slice_values, self._node): logger.error(f"[ActionDispatcher] '{name}' not published: a mapped field could not be set.") return False + if self._dry_run: # nothing published, so no ack to wait for + logger.info(f"[DRY RUN] '{name}' -> {msg}") + return True logger.debug( f"[ActionDispatcher] Publishing '{name}' -> {cfg['topic']}:\n {msg}" ) @@ -526,9 +515,9 @@ def _wait_for_ack(self, name: str, fb_cfg: dict, sent_stamp: Any) -> None: def _handle_timeout(self, name: str, fb_cfg: dict, timeout: float) -> None: """ Shared timeout policy for both topic-ack and action feedback/result - waits. `on_timeout: raise` propagates a RuntimeError out of - dispatch() (uncaught by step()/run() today) for a hard-stop-on- - failure setup; the default "warn" logs and lets dispatch() proceed. + waits. `on_timeout: raise` raises a RuntimeError out of dispatch(): + step() sends the stop and re-raises, run() lets it through and ends. + The default "warn" logs and lets dispatch() proceed. """ msg = ( f"[ActionDispatcher] '{name}' timed out waiting for feedback " @@ -539,13 +528,13 @@ def _handle_timeout(self, name: str, fb_cfg: dict, timeout: float) -> None: logger.warning(msg + "; proceeding anyway.") def _dispatch_action(self, name: str, cfg: dict, slice_values: np.ndarray) -> bool: - client = self._action_clients[name] - - if not client.server_is_ready(): - logger.warning( - f"[ActionDispatcher] Action server '{cfg['topic']}' not ready, skipping." - ) - return False + if not self._dry_run: + client = self._action_clients[name] + if not client.server_is_ready(): + logger.warning( + f"[ActionDispatcher] Action server '{cfg['topic']}' not ready, skipping." + ) + return False goal = self._goal_classes[name]() @@ -557,6 +546,10 @@ def _dispatch_action(self, name: str, cfg: dict, slice_values: np.ndarray) -> bo if not _fill_msg(goal, cfg, slice_values, self._node): logger.error(f"[ActionDispatcher] '{name}' goal not sent: a mapped field could not be set.") return False + if self._dry_run: # no goal sent, so no result or feedback to wait for + logger.info(f"[DRY RUN] '{name}' -> {goal}") + self._track_goal(name, None) + return True logger.debug( f"[ActionDispatcher] Sending goal for '{name}':\n{goal}" @@ -612,6 +605,7 @@ def _on_goal_response(f): holder["rejected"] = True ev.set() return + self._track_goal(name, goal_handle) if source == "result": result_future = goal_handle.get_result_async() @@ -682,101 +676,27 @@ def _init_goal(self, goal: Any, init_cfg: dict, name: str) -> None: # Raise, never skip: a message missing a goal_init field (e.g. use_delta) moves the robot wrongly. raise ValueError(f"[ActionDispatcher] goal_init failed for '{name}.{field_path}': {e}") from e - def _render_command(self, template: str, values) -> str: - """ - Replace {I[0]}, {I[1]}, ... in a command template with actual values. - values can be np.ndarray or dict. - """ - result = template - for idx, val in (values.items() if isinstance(values, dict) else enumerate(values)): - # Format ints as ints, floats as floats - formatted = str(int(val)) if float(val).is_integer() else str(round(float(val), 6)) - result = result.replace(f"{{I[{idx}]}}", formatted) - - if re.search(r'\{I\[\d+\]\}', result): - logger.warning( - f"[ActionDispatcher] Command template has unreplaced placeholders " - f"(action slice has {len(values)} values): {result!r}" - ) - return result - - def _apply_limits( - self, - slice_values: np.ndarray, - limits: list, - normalized_range: list, - ) -> np.ndarray: - """ - Per-index linear denormalization. - - Maps each element of slice_values from normalized_range to the - [lower_lim, upper_lim] defined in limits. - - Broadcast: a single limits entry is applied to all indices. - Use null for a specific entry to leave that index unchanged. + @staticmethod + def _apply_limits(slice_values: np.ndarray, limits: list) -> np.ndarray: + """Clamp each value to its [lower_lim, upper_lim] (real units). - normalized_range: [model_min, model_max] (typically [-1.0, 1.0]) - limits: list of {lower_lim, upper_lim} dicts — one per index OR one entry → broadcast. + limits: list of {lower_lim, upper_lim} dicts, one per index, or one entry broadcast to all. + A null entry leaves that index unchanged. """ - model_min, model_max = float(normalized_range[0]), float(normalized_range[1]) - span = model_max - model_min - result = slice_values.copy().astype(np.float32) - - if span == 0.0: - logger.warning( - f"[ActionDispatcher] normalized_range has zero span " - f"({model_min} == {model_max}); skipping denormalization." - ) - return result - + result = slice_values.astype(np.float32) for i in range(len(result)): lim = limits[min(i, len(limits) - 1)] - if lim is None: - continue - lower = float(lim["lower_lim"]) - upper = float(lim["upper_lim"]) - norm = np.clip((result[i] - model_min) / span, 0.0, 1.0) - result[i] = lower + norm * (upper - lower) - + if lim is not None: + result[i] = np.clip(result[i], float(lim["lower_lim"]), float(lim["upper_lim"])) return result def _apply_transform(self, slice_values: np.ndarray, transform_cfg: dict): values = slice_values.copy() t = transform_cfg["type"] - if t == "scale": - in_min = transform_cfg["in_min"] - in_max = transform_cfg["in_max"] - out_min = transform_cfg["out_min"] - out_max = transform_cfg["out_max"] - values = (values - in_min) / (in_max - in_min) - values = values * (out_max - out_min) + out_min - values = np.clip(values, out_min, out_max) - return values - - elif t == "threshold": - return (values > transform_cfg.get("threshold", 0.0)).astype(np.float32) - - elif t == "clip": - return np.clip(values, transform_cfg["out_min"], transform_cfg["out_max"]) - - elif t == "vacuum": - activate = float(values[0]) > transform_cfg.get("threshold", 0.0) - max_suction = transform_cfg.get("max_suction", 255) - power_limit = transform_cfg.get("power_limit", 400) - channel_value = max_suction if activate else 0 - return {0: float(channel_value), 1: float(channel_value), 2: float(power_limit)} - - elif t == "magnetic": - activate = float(values[0]) > transform_cfg.get("threshold", 0.0) - mode = 1 if activate else 0 - strength = 0 - if activate and "strength_scale" in transform_cfg: - sc = transform_cfg["strength_scale"] - norm = (float(values[0]) - sc["in_min"]) / (sc["in_max"] - sc["in_min"]) - strength = int(np.clip(norm * (sc["out_max"] - sc["out_min"]) + sc["out_min"], - sc["out_min"], sc["out_max"])) - return {0: float(mode), 1: float(strength)} + if t == "threshold": + on = values > transform_cfg.get("threshold", 0.0) + return np.where(on, transform_cfg.get("on_value", 1.0), transform_cfg.get("off_value", 0.0)).astype(np.float32) elif t == "euler_to_quat": start = int(transform_cfg.get("euler_start", 3)) @@ -793,4 +713,6 @@ def _goal_response_callback(self, future: Any, name: str) -> None: logger.warning( f"[ActionDispatcher] Goal rejected by '{name}'" ) - self._last_sent.pop(name, None) # not sent after all: retry it next tick \ No newline at end of file + self._last_sent.pop(name, None) # not sent after all: retry it next tick + return + self._track_goal(name, goal_handle) diff --git a/relay_d/dispatch/api.py b/relay_d/dispatch/api.py index 2bceb97..632208f 100644 --- a/relay_d/dispatch/api.py +++ b/relay_d/dispatch/api.py @@ -14,13 +14,13 @@ checkpoint="model.pth", # robomimic .pth, or a LeRobot pretrained_model folder obs_config="config/obs_config.yaml", action_config="config/action_config.yaml", - device="cpu", + device="cpu", # default: the infer_device setting ) as session: ok, report = session.validate() session.reset() action = session.step() # one tick: obs -> predict -> dispatch - # or run the full loop, same as the CLI: + # or run the full loop, same as the CLI (default rate: the policy's, else the infer_hz setting): session.run(hz=20) The backend is chosen by `backend=` ("robomimic" / "lerobot"), by default from the @@ -113,15 +113,17 @@ def __init__( checkpoint: str, obs_config: str, action_config: str, - device: str = "cpu", + device: Optional[str] = None, backend: Optional[str] = None, state_keys: Optional[List[str]] = None, task: Optional[str] = None, + dry_run: bool = False, ): - """``backend``: "robomimic", "lerobot" or "auto" (default: the infer_backend setting, "auto" detects it from + """``device``: torch device (default: the infer_device setting). ``backend``: "robomimic", "lerobot" or "auto" (default: the infer_backend setting, "auto" detects it from the checkpoint). ``state_keys``: LeRobot only, the observations that form observation.state, in order (default: read from the dataset the policy was trained on). ``task``: LeRobot only, the instruction a language-conditioned - policy (SmolVLA) follows (default: the training dataset's task).""" + policy (SmolVLA) follows (default: the training dataset's task). ``dry_run``: log every command instead of + publishing it or sending a goal (the robot does not move).""" if _ROS_IMPORT_ERROR is not None: raise ImportError( "InferenceSession requires a sourced ROS2 environment " @@ -129,6 +131,7 @@ def __init__( "available in this Python environment." ) from _ROS_IMPORT_ERROR + device = device or user_settings.get("infer_device") backend = backend or user_settings.get("infer_backend") if backend == "auto": backend = detect_backend(checkpoint) @@ -149,7 +152,8 @@ def __init__( fallback_state_keys=self.obs_node.low_dim_obs_names, task=task, ) - self.dispatcher = ActionDispatcher(self.obs_node, action_config) + self.dispatcher = ActionDispatcher(self.obs_node, action_config, dry_run=dry_run) + self._check_action_layout() self._was_stale = False # after stale inputs the policy restarts: no queued actions or frames from before the gap self._executor = MultiThreadedExecutor() @@ -158,6 +162,24 @@ def __init__( self.obs_node.get_logger().info("=== InferenceSession ready. ===") + def _check_action_layout(self) -> None: + """action_config.yaml indices against the model's action size: an index past it stops startup, a column no + entry sends is warned about (a wrong list would drive the wrong joint silently). Skipped if the runner cannot tell.""" + dim = getattr(self.model, "action_dim", None) + if not dim: + return + used = set() + for name, cfg in self.dispatcher._action_configs.items(): + bad = [i for i in cfg["indices"] if not -dim <= i < dim] + if bad: + raise ValueError(f"action_config.yaml '{name}': indices {bad} are outside the model's action, " + f"which has {dim} values (indices 0..{dim - 1}).") + used.update(i % dim for i in cfg["indices"]) + unused = sorted(set(range(dim)) - used) + if unused: + logger.warning(f"[InferenceSession] action columns {unused} of the model's {dim} are sent by no entry " + "in action_config.yaml: check its indices.") + # ------------------------------------------------------------------- # Lifecycle # ------------------------------------------------------------------- @@ -171,7 +193,6 @@ def start(self) -> None: def close(self) -> None: self.dispatcher.send_stop() # once: nothing is sent if run() already stopped and nothing was dispatched since - self.dispatcher.close() self._executor.shutdown() self.obs_node.destroy_node() if rclpy.ok(): @@ -225,16 +246,31 @@ def step(self, max_obs_age: Optional[float] = None) -> Optional[np.ndarray]: """One full tick: get latest obs -> predict -> dispatch. Returns the action, or None (nothing dispatched) if an input is older than max_obs_age, like run().""" max_obs_age = user_settings.get("infer_max_obs_age_s") if max_obs_age is None else max_obs_age + self._model_failed = False obs = self.get_observation() + if logger.isEnabledFor(logging.DEBUG): # vectors only: never format whole camera frames + logger.debug( + "[InferenceSession] obs: " + + ", ".join(f"{k}={np.round(obs[k], 4)}" for k in self.model.obs_keys if k in obs and obs[k].ndim == 1) + ) stale = self._stale_inputs(max_obs_age) if stale: - logger.error(f"[InferenceSession] Stale inputs (> {max_obs_age}s), not dispatching: {stale}") + self.obs_node.get_logger().error( + f"Stale inputs (> {max_obs_age}s without new data), not dispatching: {stale}", + throttle_duration_sec=1.0, + ) self.dispatcher.send_stop() # once per stale period self._was_stale = True return None self._reset_after_stale() - action = self.predict(obs) - self.dispatch(action) + try: + self._model_failed = True # until predict returns: run() ends on a model error, lets dispatch errors through + action = self.predict(obs) + self._model_failed = False + self.dispatch(action) + except BaseException: + self.dispatcher.send_stop() # an error must not leave the last command running + raise return action def reset(self) -> None: @@ -249,15 +285,19 @@ def _reset_after_stale(self) -> None: # Full loop (same behavior as the inference_node.py CLI) # ------------------------------------------------------------------- - def run(self, hz: float = 0.0, max_steps: Optional[int] = None, - validate_timeout: Optional[float] = None, max_obs_age: Optional[float] = None) -> None: - # None = the user setting (infer_obs_wait_s, infer_max_obs_age_s). hz stays 0 here: only the - # command line takes its default rate from the infer_hz setting. + def run(self, hz: Optional[float] = None, max_steps: Optional[int] = None, + validate_timeout: Optional[float] = None, max_obs_age: Optional[float] = None) -> bool: + """The inference loop until Ctrl+C, max_steps or a model error; always ends with the safe stop. + ``hz``: control rate, 0 = as fast as possible (default: the rate the policy was trained at, else the infer_hz + setting). None for the others = the user setting (infer_obs_wait_s, infer_max_obs_age_s). + Returns False if it ended on a model error, True otherwise.""" + fps = getattr(self.model, "fps", None) + # Each predicted action is one step of the training data: at another rate the robot moves faster or slower than shown. + hz = (fps or user_settings.get("infer_hz")) if hz is None else hz validate_timeout = user_settings.get("infer_obs_wait_s") if validate_timeout is None else validate_timeout max_obs_age = user_settings.get("infer_max_obs_age_s") if max_obs_age is None else max_obs_age self.start() - fps = getattr(self.model, "fps", None) if fps and hz != fps: pace = ("as fast as the loop runs" if hz <= 0 else f"{fps / hz:.2g}x slower" if hz < fps else f"{hz / fps:.2g}x faster") @@ -288,47 +328,34 @@ def run(self, hz: float = 0.0, max_steps: Optional[int] = None, if not ok: raise RuntimeError("[Pre-flight] Validation failed. Fix obs_config.yaml and restart.") - self.obs_node.get_logger().info("[Pre-flight] All inputs and observations validated. Starting inference.") + self.obs_node.get_logger().info("[Pre-flight] All inputs and observations validated. Starting inference " + f"at {f'{hz} Hz' if hz > 0 else 'unthrottled rate'}.") # Start a fresh episode right before the rollout begins. self.reset() min_dt = (1.0 / hz) if hz > 0 else 0.0 step_count = 0 + model_failed = False try: while rclpy.ok(): t0 = time.monotonic() - obs = self.get_observation() - if logger.isEnabledFor(logging.DEBUG): # vectors only: never format whole camera frames - logger.debug( - "[InferenceSession] obs: " - + ", ".join(f"{k}={np.round(obs[k], 4)}" for k in self.model.obs_keys if k in obs and obs[k].ndim == 1) - ) - - stale = self._stale_inputs(max_obs_age) - if stale: - self.obs_node.get_logger().error( - f"Stale inputs (> {max_obs_age}s without new data), not dispatching: {stale}", - throttle_duration_sec=1.0, - ) - self.dispatcher.send_stop() # once per stale period - self._was_stale = True - time.sleep(0.05) - continue - self._reset_after_stale() - try: - action = self.predict(obs) + action = self.step(max_obs_age) except Exception as e: + if not self._model_failed: + raise self.obs_node.get_logger().error(f"Model inference failed ({type(e).__name__}): {e}") + model_failed = True break + if action is None: # stale inputs + time.sleep(0.05) + continue logger.debug(f"[InferenceSession] raw action (full {action.shape[0]}-dim): {action.round(5)}") - self.dispatch(action) - step_count += 1 if step_count % 100 == 0: elapsed = time.monotonic() - t0 @@ -349,3 +376,4 @@ def run(self, hz: float = 0.0, max_steps: Optional[int] = None, self.obs_node.get_logger().info("Shutting down inference session.") finally: # every way out of the loop (model error, on_timeout: raise, Ctrl+C, max_steps) stops the robot self.dispatcher.send_stop() + return not model_failed diff --git a/relay_d/dispatch/config_templates/action_config_template.yaml b/relay_d/dispatch/config_templates/action_config_template.yaml index cb7599b..ee0556c 100644 --- a/relay_d/dispatch/config_templates/action_config_template.yaml +++ b/relay_d/dispatch/config_templates/action_config_template.yaml @@ -6,59 +6,55 @@ # # Processing pipeline for each action entry: # 1. Extract slice: values = model_output[indices] -# 2. Denormalize: map each value from normalized_range → [lower_lim, upper_lim] -# (skipped if denormalize: false) +# 2. Clamp: clip each value to [lower_lim, upper_lim] (real units) +# when `limits` is set # 3. Apply transform: optional per-entry post-processing -# 4. Dispatch: send via command / topic / action +# 4. Dispatch: send via topic / action # -# DENORMALIZATION: -# Symmetric inverse of obs_config.yaml normalization — same limits syntax applies. +# LIMITS (safety clamp, real units): +# The model already outputs real units: robomimic and LeRobot un-normalize +# actions with the checkpoint's stats. `limits` does not rescale; it only +# clips each value, e.g. a joint target the model overshoots. # -# Per-index form (one limit per index): +# Per-index form (one limit per index; null = no clamp for that index): # my_action: # indices: [0, 1, 2] -# denormalize: true -# normalized_range: [-1.0, 1.0] # limits: # - {lower_lim: -2.9, upper_lim: 2.9} # index 0 -# - {lower_lim: -2.1, upper_lim: 2.1} # index 1 +# - null # index 1: not clamped # - {lower_lim: -2.9, upper_lim: 2.9} # index 2 # # Broadcast form (one entry → applied to all indices): # my_action: # indices: [0, 1, 2] -# denormalize: true -# normalized_range: [-1.0, 1.0] # limits: # - {lower_lim: -0.5, upper_lim: 0.5} # applied to indices 0, 1, and 2 # -# Backward compat: apply_limits: true/false is still accepted (alias for denormalize). +# Removed keys and transforms are refused at startup; the error says what +# to use instead. # ============================================================================= # # QUICK REFERENCE — every key an action entry can use # ----------------------------------------------------------------------------- # indices list[int] required. positions to slice from the action array -# denormalize bool optional. model range -> real-world (default true; alias: apply_limits) -# normalized_range [min, max] optional. model output range (default [-1.0, 1.0]) -# limits list[dict] required if denormalizing. {lower_lim, upper_lim} per index, or 1 entry to broadcast -# transform dict optional. one of: scale / threshold / clip / vacuum / magnetic / euler_to_quat +# limits list[dict] optional. clamp, real units: {lower_lim, upper_lim} per index (null = none), or 1 entry to broadcast +# transform dict optional. one of: threshold / euler_to_quat # (see TRANSFORMS section below) -# stop list[float] optional, command and topic entries. values sent ONCE when inference stops +# stop list[float] optional, topic entries. values sent ONCE when inference stops # sending (inputs stale, model error, Ctrl+C, exit), e.g. [0, 0, 0] for a # velocity: a controller without a watchdog keeps the last command. Real-world -# values fed straight to mapping / {I[n]} (no limits, no transform). An action -# entry cannot have one (cancel its goal on the server instead). +# values fed straight to mapping (no limits, no transform). An action +# entry cannot have one: at the same moments its still-active goal is cancelled. # # -- then pick exactly ONE dispatch method -- -# command METHOD 1. shell string, {I[0]}, {I[1]}, ... substituted, run via subprocess -# topic + msg_type METHOD 2. ROS2 publisher; needs `mapping` -# action: true + action_type + topic METHOD 3. ROS2 ActionClient; needs `mapping` +# topic + msg_type METHOD 1. ROS2 publisher, published every tick; needs `mapping` +# action: true + action_type + topic METHOD 2. ROS2 ActionClient; needs `mapping` # (action_type is "pkg/action/Name", topic is the action's name) # -# mapping dict Methods 2/3. field_path (dot/bracket) -> slice index, or list of indices -# goal_init dict Methods 2/3. pre-populate literal/static fields + pre-allocate arrays first -# frame_id str Methods 2/3. optional, fills header.frame_id if the message has one -# feedback dict Methods 2/3 only, opt-in. Block dispatch() until completion is confirmed +# mapping dict both. field_path (dot/bracket) -> slice index, or list of indices +# goal_init dict both. pre-populate literal/static fields + pre-allocate arrays first +# frame_id str both. optional, fills header.frame_id if the message has one +# feedback dict both, opt-in. Block dispatch() until completion is confirmed # (or timeout) before returning — see MOVE_ARM_JOINT_WAIT_ACK and # ROBOTIQ_GRIPPER examples below. Per-entry opt-in, NOT a global # topic/action setting — every other entry, and every message from @@ -66,8 +62,8 @@ # # -- common to both methods -- # success_field str dot-path field on the ack/result/feedback message checked for success. -# Method 2 defaults to "reached" (matches ArmCommandStatus); -# Method 3 has NO default — required, since Result/Feedback +# Method 1 defaults to "reached" (matches ArmCommandStatus); +# Method 2 has NO default — required, since Result/Feedback # shapes vary per action type. # success_value any value success_field must equal (default: true). Works for bool # (true/false), string ("REACHED"), or int (e.g. stop_mode: 1). @@ -76,7 +72,7 @@ # RuntimeError out of dispatch() — propagates through # step()/run() uncaught; use for a hard-stop-on-failure setup). # -# -- Method 2 (topic) only -- +# -- Method 1 (topic) only -- # ack_topic str required. topic the driver publishes its ack/status message on # ack_msg_type str required. "pkg/msg/Name" of that ack message # correlation_field str optional. field on the ack echoing back the command's header.stamp, so a @@ -84,7 +80,7 @@ # Default: "command_stamp" (matches ArmCommandStatus). Set to # null to disable correlation (accept whatever ack arrives next). # -# -- Method 3 (action) only -- +# -- Method 2 (action) only -- # source str "result" (default) — wait for the action's terminal Result via # get_result_async(), the correct completion signal for a # well-behaved action server. "feedback" — instead watch the @@ -95,40 +91,35 @@ actions: - # Already working for ibot setup - move_eef: - indices: [0, 1, 2, 3, 4, 5, 6] # Pose - denormalize: false - msg_type: kuka_iiwa_arm_sim_interface/msg/ArmCommand - topic: /kuka_iiwa_arm_interface/streaming_command - # Native in-process publisher — do NOT switch this back to a shelled-out - # `ros2 topic pub` command. A single `ros2 topic pub -1` invocation takes - # ~1.5s to spin up rclpy/DDS, publish, and exit; at 20Hz that backlogs - # into 7-8+ concurrent competing processes whose publish order is - # essentially random OS-scheduling luck, which — combined with - # use_delta: true — silently scrambles the order deltas are applied in. - goal_init: - use_delta: true - speed: 12.0 - acceleration: 0.6 - blending: 0.0 - duration: 0.0 - transforms: 1 # pre-allocate 1 TransformStamped - "transforms[0].header.frame_id": "base" - "transforms[0].child_frame_id": "robotiq_tcp" - mapping: - "transforms[0].transform.translation.x": 0 - "transforms[0].transform.translation.y": 1 - "transforms[0].transform.translation.z": 2 - "transforms[0].transform.rotation.x": 3 - "transforms[0].transform.rotation.y": 4 - "transforms[0].transform.rotation.z": 5 - "transforms[0].transform.rotation.w": 6 + # Example: the same arm commanded with an end-effector pose instead of joints. + # Use it INSTEAD of move_arm_joint, never next to it: both read indices 0-6 + # and publish on the same topic, so the arm would get two commands per tick. + # + # move_eef: + # indices: [0, 1, 2, 3, 4, 5, 6] # Pose: x, y, z, qx, qy, qz, qw + # msg_type: kuka_iiwa_arm_sim_interface/msg/ArmCommand + # topic: /kuka_iiwa_arm_interface/streaming_command + # goal_init: + # use_delta: true + # speed: 12.0 + # acceleration: 0.6 + # blending: 0.0 + # duration: 0.0 + # transforms: 1 # pre-allocate 1 TransformStamped + # "transforms[0].header.frame_id": "base" + # "transforms[0].child_frame_id": "robotiq_tcp" + # mapping: + # "transforms[0].transform.translation.x": 0 + # "transforms[0].transform.translation.y": 1 + # "transforms[0].transform.translation.z": 2 + # "transforms[0].transform.rotation.x": 3 + # "transforms[0].transform.rotation.y": 4 + # "transforms[0].transform.rotation.z": 5 + # "transforms[0].transform.rotation.w": 6 move_arm_joint: indices: [0, 1, 2, 3, 4, 5, 6] # 7 iiwa joint positions (or deltas) - denormalize: false # set true + add limits if the model - # outputs normalized joint values instead + # limits: (optional) clamp each joint value or delta, real units, e.g. [{lower_lim: -0.05, upper_lim: 0.05}] msg_type: kuka_iiwa_arm_sim_interface/msg/ArmCommand topic: /kuka_iiwa_arm_interface/streaming_command goal_init: @@ -149,7 +140,6 @@ actions: # # move_arm_joint_wait_ack: # indices: [0, 1, 2, 3, 4, 5, 6] - # denormalize: false # msg_type: ibot_common/msg/ArmCommand # topic: /kuka_iiwa_arm_interface/streaming_command # goal_init: @@ -171,13 +161,9 @@ actions: # # reached: true bool check # timeout: 15.0 # on_timeout: warn - # dedupe: false # topic entries are published every tick; - # # true = skip when the value is unchanged robotiq_gripper: indices: [7] - denormalize: true - normalized_range: [-1.0, 1.0] limits: - {lower_lim: 0.0, upper_lim: 255.0} action: true @@ -198,8 +184,6 @@ actions: # # robotiq_gripper_wait_ack: # indices: [7] - # denormalize: true - # normalized_range: [-1.0, 1.0] # limits: # - {lower_lim: 0.0, upper_lim: 255.0} # action: true @@ -220,43 +204,7 @@ actions: # timeout: 10.0 # =========================================================================== - # DISPATCH METHOD 1: command (shell string with {I[n]} placeholders) - # =========================================================================== - # Simplest method. The processed slice values are substituted into the - # command string as {I[0]}, {I[1]}, etc., then executed via shell. - # =========================================================================== - - # # --- 7-DOF arm via ros2 topic pub --- - # move_arm: - # indices: [0, 1, 2, 3, 4, 5, 6] - # denormalize: false # set true to enable denormalization - # normalized_range: [-1.0, 1.0] # model output range - # limits: - # - {lower_lim: -2.9, upper_lim: 2.9} # joint 0 (radians) - # - {lower_lim: -2.1, upper_lim: 2.1} # joint 1 - # - {lower_lim: -2.9, upper_lim: 2.9} # joint 2 - # - {lower_lim: -2.1, upper_lim: 2.1} # joint 3 - # - {lower_lim: -2.9, upper_lim: 2.9} # joint 4 - # - {lower_lim: -2.1, upper_lim: 2.1} # joint 5 - # - {lower_lim: -3.0, upper_lim: 3.0} # joint 6 - # command: > - # ros2 topic pub -1 /arm_interface/command pkg/msg/ArmCommand - # "{joints: [{I[0]}, {I[1]}, {I[2]}, {I[3]}, {I[4]}, {I[5]}, {I[6]}]}" - - # # --- Single-value action (e.g. screwdriver duration) --- - # screwdriver: - # indices: [7] - # denormalize: true - # normalized_range: [-1.0, 1.0] - # limits: - # - {lower_lim: 0.0, upper_lim: 5.0} # duration in seconds - # command: > - # ros2 action send_goal /ScrewdriverSkill screwdriver_msgs/action/Screwdriver - # "{duration: {I[0]}}" - - - # =========================================================================== - # DISPATCH METHOD 2: topic (ROS2 publisher with field mapping) + # DISPATCH METHOD 1: topic (ROS2 publisher with field mapping) # =========================================================================== # Creates a ROS2 publisher and fills message fields from slice indices. # msg_type: full ROS2 type string (e.g. geometry_msgs/msg/Point) @@ -266,8 +214,6 @@ actions: # arm_via_topic: # indices: [0, 1, 2] - # denormalize: true - # normalized_range: [-1.0, 1.0] # limits: # - {lower_lim: -0.5, upper_lim: 0.5} # - {lower_lim: -0.5, upper_lim: 0.5} @@ -300,7 +246,7 @@ actions: # =========================================================================== - # DISPATCH METHOD 3: action (ROS2 action client) + # DISPATCH METHOD 2: action (ROS2 action client) # =========================================================================== # Sends a ROS2 action goal via a persistent ActionClient (goal accept/reject # is logged; see _goal_response_callback). Requires: @@ -316,10 +262,6 @@ actions: # velocity are constants (goal_init, set as written); only `position` comes # from the model (mapping). A mapped value is cast to the type of the field # it fills, so integer fields such as uint8 work. - # - # For simple one-shot goals the `command` form (METHOD 1: - # `ros2 action send_goal ...`) is also fine; use `action: true` when you - # need to know that the goal was accepted / finished (see `feedback`). # =========================================================================== @@ -356,7 +298,7 @@ actions: # "transforms[0].header.frame_id": "base" # "transforms[0].child_frame_id": "robotiq_tcp" # - # Both ACTIVE entries at the top (move_eef, move_arm_joint) use this pattern. They target the same + # move_arm_joint and the move_eef example at the top use this pattern. They target the same # ArmCommand message: `transforms` (Cartesian pose) and `joint_trajectory` (joint space) are mutually # exclusive, leave the one you do not use untouched. # @@ -364,7 +306,7 @@ actions: # trajectory_msgs/JointTrajectoryPoint[] directly on ArmCommand), there is no # JointTrajectory.points[] wrapper. goal_init pre-allocates ONE point # (`joint_trajectory: 1`) and mapping sets all its `.positions` in a single - # list-valued entry (see the list-valued mapping note under DISPATCH METHOD 2). + # list-valued entry (see the list-valued mapping note under DISPATCH METHOD 1). # =========================================================================== @@ -372,101 +314,25 @@ actions: # =========================================================================== # Only one transform per action entry. Types: # - # scale — linear remap: [in_min, in_max] -> [out_min, out_max] - # threshold — binarize: value > threshold -> 1.0, else -> 0.0 - # clip — clamp to [out_min, out_max] - # vacuum — vacuum gripper: threshold -> channel value + power limit - # magnetic — magnetic gripper: threshold -> mode + strength scaling + # threshold — binarize: value > threshold -> on_value (default 1.0), + # else -> off_value (default 0.0) # euler_to_quat — splice: replace 3 Euler angles with 4 quaternion values # (see ROTATION CONVERSIONS section below for full examples) # =========================================================================== - # Example: threshold on a binary gripper command + # Example: threshold on an on/off gripper (e.g. a suction gripper whose driver + # takes 255 for full suction and 0 for off). `limits` would clamp first. # gripper_binary: # indices: [7] - # denormalize: false - # normalized_range: [-1.0, 1.0] - # limits: - # - {lower_lim: 0.0, upper_lim: 1.0} # transform: # type: threshold # threshold: 0.5 - # command: "ros2 topic pub -1 /gripper/cmd std_msgs/msg/Bool \"{data: {I[0]}}\"" - - # Example: scale transform - # velocity_scaled: - # indices: [0, 1, 2] - # denormalize: true - # normalized_range: [-1.0, 1.0] - # limits: - # - {lower_lim: -1.0, upper_lim: 1.0} - # - {lower_lim: -1.0, upper_lim: 1.0} - # - {lower_lim: -1.0, upper_lim: 1.0} - # transform: - # type: scale - # in_min: -1.0 - # in_max: 1.0 - # out_min: -0.1 - # out_max: 0.1 - # command: "ros2 topic pub -1 /cmd_vel ... \"{I[0]}, {I[1]}, {I[2]}\"" - # stop: [0.0, 0.0, 0.0] # sent once when inference stops: the base must not keep the last velocity - - # Example: clip transform - # safe_velocity: - # indices: [0, 1, 2] - # denormalize: true - # normalized_range: [-1.0, 1.0] - # limits: - # - {lower_lim: -0.5, upper_lim: 0.5} - # - {lower_lim: -0.5, upper_lim: 0.5} - # - {lower_lim: -0.5, upper_lim: 0.5} - # transform: - # type: clip - # out_min: -0.3 - # out_max: 0.3 - # command: "..." - - # Example: vacuum transform (VGC10-style gripper) - # Reads only values[0] (the first slice index) as the activation signal; - # returns a fixed-shape dict {0: channel, 1: channel, 2: power_limit} meant - # for {I[n]} command templating (or numeric-keyed mapping, less common). - # vacuum_gripper: - # indices: [7] - # denormalize: true - # normalized_range: [-1.0, 1.0] - # limits: - # - {lower_lim: 0.0, upper_lim: 1.0} - # transform: - # type: vacuum - # threshold: 0.5 # optional, default 0.0 — values[0] > threshold -> activate - # max_suction: 255 # optional, default 255 — channel value when active - # power_limit: 400 # optional, default 400 - # command: > - # ros2 action send_goal /OnrobotVgc10Skill - # onrobot_vgc10_skill_msgs/action/OnrobotVgc10Skill - # "{a: {I[0]}, b: {I[1]}, ma: {I[2]}}" - - # Example: magnetic transform (MG10-style gripper) - # Also reads only values[0]; returns {0: mode, 1: strength}. strength_scale - # is optional — omit it to always send strength 0 when active. - # magnetic_gripper: - # indices: [7] - # denormalize: true - # normalized_range: [-1.0, 1.0] - # limits: - # - {lower_lim: 0.0, upper_lim: 1.0} - # transform: - # type: magnetic - # threshold: 0.5 - # strength_scale: - # in_min: 0.5 - # in_max: 1.0 - # out_min: 0 - # out_max: 100 - # command: > - # ros2 action send_goal /OnrobotMg10Skill - # onrobot_mg10_skill_msgs/action/OnrobotMg10Skill - # "{gripper_mode: {I[0]}, magnet_strength: {I[1]}}" + # on_value: 255 + # off_value: 0 + # topic: /gripper/command + # msg_type: std_msgs/msg/Float64 + # mapping: + # data: 0 # =========================================================================== @@ -485,7 +351,7 @@ actions: # # Array layout example (euler_start: 3): # - # Model output (after denorm): + # Model output (real units): # index 0 1 2 3 4 5 6 # x y z roll pitch yaw gripper # @@ -501,8 +367,6 @@ actions: # --------------------------------------------------------------------------- # eef_pose_euler_to_quat: # indices: [0, 1, 2, 3, 4, 5] # x, y, z, roll, pitch, yaw - # denormalize: true - # normalized_range: [-1.0, 1.0] # limits: # - {lower_lim: -0.8, upper_lim: 0.8} # x (metres) # - {lower_lim: -0.8, upper_lim: 0.8} # y @@ -545,5 +409,4 @@ actions: # - tf_eef.euler.yaw # # The system converts the TF quaternion to ZYX Euler radians automatically. - # See obs_config_template.yaml → "Quat → Euler" section for normalization examples. # =========================================================================== diff --git a/relay_d/dispatch/config_templates/obs_config_template.yaml b/relay_d/dispatch/config_templates/obs_config_template.yaml index d35265a..58a919f 100644 --- a/relay_d/dispatch/config_templates/obs_config_template.yaml +++ b/relay_d/dispatch/config_templates/obs_config_template.yaml @@ -28,10 +28,9 @@ config: # =========================================================================== # Topic inputs carry their own timestamp (msg.header.stamp), or the arrival # time if the message has no header (e.g. std_msgs/Bool, Float64). TF inputs - # use the TransformStamped's header.stamp from the lookup. Static inputs - # never carry a real timestamp and are always treated as "now". + # use the TransformStamped's header.stamp from the lookup. # - # When an observation is built from 2+ distinct live (non-static) inputs, + # When an observation is built from 2+ distinct inputs, # their timestamps must fall within `threshold` seconds of each other or # the observation is skipped this cycle (sample-and-hold — it keeps its # last value until inputs realign). A warning is logged once per @@ -133,16 +132,6 @@ config: # # supported here (same restriction as the # # acquisition-side config_template.yaml). - # --- Static (constant) inputs — no topic required --- - # Use when a topic is not available in the deployment system but the model - # was trained expecting that observation. The value never changes at runtime. - # Reference in observations as: .data[:] (list) or .data (scalar). - # - # gripper_state: - # static: [1.0] # list form → use gripper_state.data[:] - # some_flag: - # static: 0.0 # scalar form → use some_flag.data - # --- TF inputs (parent_frame + child_frame) --- # tf_eef: # parent_frame: base @@ -181,26 +170,9 @@ config: # rot6d.i0 .. rot6d.i5 (first 2 cols of R, col-major) # rot_matrix.r00 .. rot_matrix.r22 (full 3x3, row-major) # - # NORMALIZATION: - # Plain list form → values passed through unchanged (no normalization). - # Dict form → optional linear mapping from real-world range to model range. - # - # Plain list (no normalization): - # my_obs: - # - input.field[:] - # - # Dict form with normalization: - # my_obs: - # specs: - # - input.field[:] - # normalize: true - # normalized_range: [-1.0, 1.0] # target range (default [-1, 1]) - # limits: - # - {lower_lim: -3.14, upper_lim: 3.14} # one per element, OR - # - {lower_lim: -2.09, upper_lim: 2.09} # one entry → broadcast to all - # - # Key names match action_config.yaml (lower_lim / upper_lim). - # Normalization and action denormalization are exact inverses — same limits apply. + # NORMALIZATION: none. Values are passed to the model as read; robomimic and + # LeRobot normalize them with the checkpoint's stats. An obs with + # normalize / normalized_range / limits is refused at startup. # # ROTATION FORMAT (dict form only): # rotation_format: rpy @@ -208,9 +180,7 @@ config: # converts it to (roll, pitch, yaw) — ZYX radians, same convention as the # .euler.* paths and action_config.yaml's euler_to_quat. # - # Conversion happens BEFORE normalization, so the observation is 3 - # elements wide (not 4) and `limits` takes 3 entries — one per axis — - # or 1 to broadcast. + # The observation is 3 elements wide (not 4). # # Unlike the .euler.roll shortcut, which only exists for TF # inputs, rotation_format works for ANY input that carries a quaternion: @@ -231,34 +201,16 @@ config: # ZYX convention matches the euler_to_quat transform in action_config.yaml, so the # model sees and outputs Euler angles in the same convention. # - # Plain (no normalization): # robot0_eef_euler: # - tf_eef.euler.roll # - tf_eef.euler.pitch # - tf_eef.euler.yaw - # - # With per-element normalization (recommended when limits differ per axis): - # robot0_eef_euler: - # specs: - # - tf_eef.euler.roll - # - tf_eef.euler.pitch - # - tf_eef.euler.yaw - # normalize: true - # normalized_range: [-1.0, 1.0] - # limits: - # - {lower_lim: -3.1416, upper_lim: 3.1416} # roll (±180°) - # - {lower_lim: -1.5708, upper_lim: 1.5708} # pitch (±90°, avoids gimbal lock zone) - # - {lower_lim: -3.1416, upper_lim: 3.1416} # yaw (±180°) - # - # NOTE: if the model also outputs Euler angles (action side), use the same limits - # in action_config.yaml → euler_to_quat entry so normalization and denormalization - # are exact inverses. # --- Quaternion -> roll/pitch/yaw via rotation_format (ANY input) --- # Same result as the euler.* block above, but works for non-TF inputs too. - # The 4 quaternion specs collapse to 3 values, so limits has 3 entries. + # The 4 quaternion specs collapse to 3 values. # - # From a PoseStamped topic (limits: same per-axis ranges as the euler.* example above): + # From a PoseStamped topic: # ee_rotation_rpy: # specs: # - ee_pose.pose.orientation.x @@ -266,12 +218,6 @@ config: # - ee_pose.pose.orientation.z # - ee_pose.pose.orientation.w # rotation_format: rpy - # normalize: true - # normalized_range: [-1.0, 1.0] - # limits: - # - {lower_lim: -3.14159265, upper_lim: 3.14159265} # roll - # - {lower_lim: -1.57079633, upper_lim: 1.57079633} # pitch - # - {lower_lim: -3.14159265, upper_lim: 3.14159265} # yaw # # A TF input works the same way (tf_eef.rotation.x / .y / .z / .w): the result # equals listing tf_eef.euler.roll / .pitch / .yaw. @@ -309,35 +255,15 @@ config: # - tf_eef.euler.pitch # - tf_eef.euler.yaw - # --- Joint positions — plain list (no normalization) --- + # --- Joint positions --- # robot0_joint_pos: # - joint_1.position[:] - # --- Joint positions — with per-element normalization --- - # robot0_joint_pos: - # specs: - # - joint_1.position[:] - # normalize: true - # normalized_range: [-1.0, 1.0] - # limits: # one entry per joint - # - {lower_lim: -2.9671, upper_lim: 2.9671} # iiwa_joint_1 (±170°) - # - {lower_lim: -2.0944, upper_lim: 2.0944} # iiwa_joint_2 (±120°) - # - {lower_lim: -2.9671, upper_lim: 2.9671} # iiwa_joint_3 (±170°) - # - {lower_lim: -2.0944, upper_lim: 2.0944} # iiwa_joint_4 (±120°) - # - {lower_lim: -2.9671, upper_lim: 2.9671} # iiwa_joint_5 (±170°) - # - {lower_lim: -2.0944, upper_lim: 2.0944} # iiwa_joint_6 (±120°) - # - {lower_lim: -3.0543, upper_lim: 3.0543} # iiwa_joint_7 (±175°) - - # --- EEF position — with broadcast normalization (same range for x, y, z) --- + # --- EEF position --- # robot0_eef_pos: - # specs: - # - tf_eef.translation.x - # - tf_eef.translation.y - # - tf_eef.translation.z - # normalize: true - # normalized_range: [-1.0, 1.0] - # limits: - # - {lower_lim: -2.0, upper_lim: 2.0} # one entry → applied to x, y, and z + # - tf_eef.translation.x + # - tf_eef.translation.y + # - tf_eef.translation.z # --- Joint velocities --- # robot0_joint_vel: @@ -374,7 +300,7 @@ config: # Each side is an ordinary scalar spec (optionally sin()/cos()-wrapped) and # the two sides may reference different inputs. The operator (+ - * /) must # have spaces around it. Mixes freely with ordinary specs, and works in the - # plain list form (no normalize/limits) as shown here. + # plain list form as shown here. # # object: # - tf_object.translation.x # (the object pose specs from the example above ...) @@ -391,9 +317,7 @@ config: # Rules and gotchas: # - One operation per spec: "a.x - b.x - c.x" is rejected at startup. # - Slices are not allowed as operands: "a.position[:] - b.position[:]". - # - Numeric literals are not supported — use a static input as a constant: - # input: z_offset: {static: 0.1} - # spec: tf_object.translation.z - z_offset.data + # - Numeric literals are not supported: both operands are input fields. # - Both operand inputs count towards the sync check for this observation # (see config.sync above), so an obs that used to depend on one input # may now sample-and-hold when the two inputs' stamps drift apart. @@ -444,7 +368,7 @@ config: # - force_z.data # --- Camera / organized-pointcloud passthrough obs --- - # No specs, no normalize/rotation_format — the decoded array is used + # No specs, no rotation_format — the decoded array is used # exactly as received (uint8 (H,W,3) for rgb, native dtype (H,W) for # depth, float32 (H,W,fields) for an organized pointcloud). `source` must # name an `input:` entry above resolving to sensor_msgs/Image or an diff --git a/relay_d/dispatch/inference_node.py b/relay_d/dispatch/inference_node.py index 813fc33..7829f3e 100644 --- a/relay_d/dispatch/inference_node.py +++ b/relay_d/dispatch/inference_node.py @@ -22,6 +22,7 @@ """ import argparse +import signal import sys from relay_d.utils import user_settings @@ -30,6 +31,12 @@ from .reexec import reexec_if_needed +def sigterm_runs_the_safe_stop(): + """SIGTERM (kill, systemd, a supervisor) raises KeyboardInterrupt like Ctrl+C, so the safe stop is sent: rclpy's + own handler ends the process without running ``finally``. Call it after rclpy.init(), which installs that handler.""" + signal.signal(signal.SIGTERM, signal.default_int_handler) + + def main(): parser = argparse.ArgumentParser(description="LfD Inference Node") parser.add_argument("--checkpoint", required=True, @@ -40,7 +47,7 @@ def main(): parser.add_argument("--device", default=user_settings.get("infer_device"), help="Torch device: cpu | cuda (default: the infer_device setting)") parser.add_argument("--hz", type=float, default=None, help="Control rate in Hz (0 = as fast as possible). Default: the rate the policy was trained at " - "(a LeRobot dataset's fps), else the infer_hz setting") + "(the recording_frequency, stored in the checkpoint / LeRobot dataset), else the infer_hz setting") parser.add_argument("--max-obs-age", type=float, default=user_settings.get("infer_max_obs_age_s"), help="Skip dispatching while any input is older than this many seconds (default: the infer_max_obs_age_s setting)") parser.add_argument("--obs-wait", type=float, default=user_settings.get("infer_obs_wait_s"), @@ -54,6 +61,8 @@ def main(): parser.add_argument("--task", default=None, help="lerobot only: the instruction a language-conditioned policy (SmolVLA) follows. " "Default: the task of the dataset the policy was trained on") + parser.add_argument("--dry-run", action="store_true", + help="Run the full loop but publish nothing and send no goals: every command is logged instead") args = parser.parse_args() try: # the package of the backend may live in another environment (the one built by the Train tab) @@ -72,13 +81,15 @@ def main(): backend=args.backend, state_keys=state_keys, task=args.task, + dry_run=args.dry_run, ) - # Each predicted action is one step of the training data: at another rate the robot moves faster or slower than shown. - hz = args.hz if args.hz is not None else (getattr(session.model, "fps", None) or user_settings.get("infer_hz")) try: - session.run(hz=hz, validate_timeout=args.obs_wait, max_obs_age=args.max_obs_age) + sigterm_runs_the_safe_stop() + ok = session.run(hz=args.hz, validate_timeout=args.obs_wait, max_obs_age=args.max_obs_age) finally: session.close() + if not ok: # a model error: the Run tab shows Failed, not Stopped + sys.exit(1) if __name__ == "__main__": diff --git a/relay_d/dispatch/lerobot_runner.py b/relay_d/dispatch/lerobot_runner.py index 150be13..e28b865 100644 --- a/relay_d/dispatch/lerobot_runner.py +++ b/relay_d/dispatch/lerobot_runner.py @@ -170,6 +170,8 @@ def __init__( raise ValueError(f"[LeRobotPolicyRunner] unsupported policy inputs {unsupported}: only {STATE} and {IMAGE_PREFIX}* are fed") self._state_dim = int(features[STATE].shape[0]) if STATE in features else 0 self._images = {k[len(IMAGE_PREFIX):]: (k, tuple(f.shape)) for k, f in features.items() if k.startswith(IMAGE_PREFIX)} + action = (getattr(cfg, "output_features", None) or {}).get("action") + self.action_dim: Optional[int] = int(action.shape[0]) if action is not None else None info = dataset_info(folder) if fine_tuned(folder): @@ -180,6 +182,8 @@ def __init__( "training dataset (train_config.json -> dataset.root), which was not found: put it back at that path." ) self._state_dim, self._images = dataset_inputs(info) + action = (info.get("features") or {}).get("action") or {} + self.action_dim = int(action["shape"][0]) if action.get("shape") else None # The control rate the policy learned: each predicted action is one step at the dataset's fps. self.fps: Optional[float] = (info or {}).get("fps") self.state_keys = self._resolve_state_keys(info, state_keys, fallback_state_keys) diff --git a/relay_d/dispatch/obs_builder.py b/relay_d/dispatch/obs_builder.py index 188802a..03da88d 100644 --- a/relay_d/dispatch/obs_builder.py +++ b/relay_d/dispatch/obs_builder.py @@ -3,7 +3,7 @@ -------------- Builds the observation dict for model inference by subscribing to ROS2 topics and performing TF lookups, as configured in obs_config.yaml. The config syntax -(inputs, observation specs, normalization, rotation_format, passthrough +(inputs, observation specs, rotation_format, passthrough `source:` entries, sync threshold) is documented, with examples, in config_templates/obs_config_template.yaml: that file is the reference. @@ -15,8 +15,6 @@ import re import threading import time -import types -from dataclasses import dataclass from typing import Any, Dict, List, Optional, Tuple import numpy as np @@ -28,11 +26,11 @@ from rosidl_runtime_py.utilities import get_message from relay_d.utils.config_loader import load_yaml_config from relay_d.utils.numerics import ( + enriched_tf, inputs_in_sync, - normalize, quat_to_euler, - quat_to_rot_matrix, read_scalar, + stamp_to_sec, strip_trig, ) from relay_d.utils.qos import subscription_qos @@ -40,34 +38,6 @@ from relay_d.utils.tf_topic import _TfTopicListener, get_or_create_tf_buffer -# --------------------------------------------------------------------------- -# Normalization -# --------------------------------------------------------------------------- - -@dataclass -class _NormConfig: - """Per-stream normalization parameters. - - limits: list of (lower_lim, upper_lim) tuples — one per element, or a single - entry that is broadcast to every element in the stream. - """ - limits: List[Tuple[float, float]] - norm_min: float = -1.0 - norm_max: float = 1.0 - - -def _apply_normalization(arr: np.ndarray, nc: _NormConfig) -> np.ndarray: - """Linearly map each element from its real-world range to normalized_range. - - Symmetric inverse of action_dispatcher._apply_limits. - """ - out = np.empty_like(arr) - for i, v in enumerate(arr): - lo, hi = nc.limits[min(i, len(nc.limits) - 1)] - out[i] = normalize(v, lo, hi, nc.norm_min, nc.norm_max) - return out - - def _get_specs(stream_def) -> List[str]: """Return the spec list from either a plain list or a dict stream definition.""" return stream_def if isinstance(stream_def, list) else stream_def.get("specs", []) @@ -90,64 +60,6 @@ def _get_specs(stream_def) -> List[str]: _BINOP_RE = re.compile(r"^(.+?)\s+([+\-*/])\s+(.+)$") -# --------------------------------------------------------------------------- -# Input synchronization -# --------------------------------------------------------------------------- - -def _stamp_to_seconds(stamp) -> float: - """builtin_interfaces/Time (sec, nanosec) -> float seconds.""" - return stamp.sec + stamp.nanosec * 1e-9 - - -# --------------------------------------------------------------------------- -# Quaternion helpers -# --------------------------------------------------------------------------- - -def _quat_to_euler(x: float, y: float, z: float, w: float): - """Quaternion (x,y,z,w) -> (roll, pitch, yaw) radians, ZYX convention.""" - return tuple(quat_to_euler([x, y, z, w])) - - -# --------------------------------------------------------------------------- -# TF enriched wrapper -# --------------------------------------------------------------------------- - -def _make_enriched_tf(tf_stamped) -> types.SimpleNamespace: - """ - Wrap a TransformStamped so that attrgetter works for all supported TF specs: - translation.x/y/z - rotation.x/y/z/w - euler.roll/pitch/yaw — ZYX radians - rot6d.i0..i5 — first two columns of R, column-major - rot_matrix.r00..r22 — full 3x3, row-major - """ - t = tf_stamped.transform.translation - r = tf_stamped.transform.rotation - qx, qy, qz, qw = r.x, r.y, r.z, r.w - - roll, pitch, yaw = _quat_to_euler(qx, qy, qz, qw) - euler = types.SimpleNamespace(roll=roll, pitch=pitch, yaw=yaw) - - R = quat_to_rot_matrix(qx, qy, qz, qw) - rot_matrix = types.SimpleNamespace( - r00=R[0, 0], r01=R[0, 1], r02=R[0, 2], - r10=R[1, 0], r11=R[1, 1], r12=R[1, 2], - r20=R[2, 0], r21=R[2, 1], r22=R[2, 2], - ) - rot6d_vals = list(R[:, 0]) + list(R[:, 1]) - rot6d = types.SimpleNamespace(**{f"i{i}": float(v) for i, v in enumerate(rot6d_vals)}) - - return types.SimpleNamespace( - transform=types.SimpleNamespace( - translation=t, - rotation=r, - euler=euler, - rot_matrix=rot_matrix, - rot6d=rot6d, - ) - ) - - # --------------------------------------------------------------------------- # JointState filtered view # --------------------------------------------------------------------------- @@ -196,9 +108,15 @@ class ObsBuilder(Node): """ def __init__(self, obs_config_path: str): - super().__init__("obs_builder") - cfg = load_yaml_config(obs_config_path)["config"] + for name, spec in cfg.get("input", {}).items(): + if "static" in spec: # checked before the node exists: refused even without rclpy.init() + raise ValueError( + f"input '{name}': 'static' inputs were removed: the recorder never wrote them, so no model " + f"was trained on one. Remove it from obs_config.yaml (and from the observations that use it)." + ) + + super().__init__("obs_builder") self._input_cfg: Dict[str, dict] = cfg.get("input", {}) self._obs_cfg: Dict[str, List[str]] = cfg.get("observations", {}) @@ -221,28 +139,15 @@ def __init__(self, obs_config_path: str): # Classify inputs self._tf_inputs: set = set() - self._static_inputs: set = set() self._joint_filters: Dict[str, List[str]] = {} for name, spec in self._input_cfg.items(): if "parent_frame" in spec and "child_frame" in spec: self._tf_inputs.add(name) if "joint_names" in spec: self._joint_filters[name] = spec["joint_names"] - if "static" in spec: - self._static_inputs.add(name) - - # Pre-populate static inputs — always available, never overwritten by callbacks - for name in self._static_inputs: - val = self._input_cfg[name]["static"] - if isinstance(val, (int, float)): - arr = np.array([float(val)], dtype=np.float32) - else: - arr = np.array(val, dtype=np.float32) - self._latest_inputs[name] = types.SimpleNamespace(data=arr) - # Build per-obs extractor lists and normalization configs + # Build per-obs extractor lists self._extractors: Dict[str, List[_Entry]] = {} - self._norm_params: Dict[str, Optional[_NormConfig]] = {} self._rotation_format: Dict[str, Optional[str]] = {} self._obs_input_names: Dict[str, set] = {} self._passthrough_obs: Dict[str, str] = {} # obs_name -> input_name @@ -307,8 +212,8 @@ def __init__(self, obs_config_path: str): def _build_extractors(self) -> None: for obs_name, stream_def in self._obs_cfg.items(): # Passthrough obs: {source: } — no specs, no - # normalization/rotation-format. Deliberately kept OUT of - # self._extractors/_norm_params/_rotation_format entirely (those + # rotation-format. Deliberately kept OUT of + # self._extractors/_rotation_format entirely (those # all assume a flat float row); handled by its own pass in # _topic_callback/_evaluate_all instead. if isinstance(stream_def, dict) and "source" in stream_def: @@ -321,25 +226,16 @@ def _build_extractors(self) -> None: self._passthrough_obs[obs_name] = input_name continue - # Parse normalization config (dict form only) if isinstance(stream_def, list): - self._norm_params[obs_name] = None rot_fmt = None else: - if stream_def.get("normalize", False): - norm_min, norm_max = stream_def.get("normalized_range", [-1.0, 1.0]) - limits = [ - (float(l["lower_lim"]), float(l["upper_lim"])) - for l in stream_def.get("limits", []) - ] - if not limits: - raise ValueError( - f"obs '{obs_name}': normalize=true but 'limits' is " - f"empty or missing in obs_config.yaml" - ) - self._norm_params[obs_name] = _NormConfig(limits, norm_min, norm_max) - else: - self._norm_params[obs_name] = None + old = [k for k in ("normalize", "normalized_range", "limits") if k in stream_def] + if old: # an old checkpoint trained on normalized obs would act wrongly on raw ones + raise ValueError( + f"obs '{obs_name}': Relay-D no longer normalizes observations " + f"(robomimic/LeRobot do it with the checkpoint's stats). Remove " + f"{', '.join(old)} from obs_config.yaml, then re-convert and retrain." + ) # rotation_format (dict form only) rot_fmt = stream_def.get("rotation_format") @@ -390,9 +286,7 @@ def _build_extractors(self) -> None: real_inputs.update((payload[1], payload[5])) else: real_inputs.add(inp_name) - self._obs_input_names[obs_name] = { - n for n in real_inputs if n not in self._static_inputs - } + self._obs_input_names[obs_name] = real_inputs def _parse_atomic(self, obs_name: str, raw_spec: str, spec: str, math_op) -> _Entry: """Parse one atomic '.' or '.[:]'. @@ -481,14 +375,6 @@ def _validate_rpy(self, obs_name: str, specs: List[str], f"(w LAST, not first); a wrong order silently yields wrong angles." ) - nc = self._norm_params.get(obs_name) - if nc is not None and len(nc.limits) not in (1, 3): - raise ValueError( - f"obs '{obs_name}': rotation_format: rpy produces 3 values " - f"(roll, pitch, yaw) but 'limits' has {len(nc.limits)} entries — " - f"provide 3 (one per axis) or 1 (broadcast to all)." - ) - # ------------------------------------------------------------------ # Subscription setup with auto type discovery # ------------------------------------------------------------------ @@ -496,7 +382,7 @@ def _validate_rpy(self, obs_name: str, specs: List[str], def _setup_subscriptions(self) -> None: topic_map = {name: tlist for name, tlist in self.get_topic_names_and_types()} for input_name, spec in self._input_cfg.items(): - if input_name in self._tf_inputs or input_name in self._static_inputs: + if input_name in self._tf_inputs: continue topic = spec["topic"] if topic in topic_map and topic_map[topic]: @@ -540,7 +426,7 @@ def _retry_subscriptions(self) -> None: def _decode_passthrough(self, input_name: str, msg: Any) -> Optional[np.ndarray]: """Decode a camera/pointcloud message for a passthrough obs into a - plain numpy array (no flatten/normalize) via the shared codec module, + plain numpy array (no flatten) via the shared codec module, so a live obs matches what a trained model saw during postprocessing (relay_d/acquisition/qt_app/ui/postprocess/yaml_driven_converter.py's bridge-map obs). Returns None (and warns once) on anything it can't @@ -576,9 +462,9 @@ def _topic_callback(self, msg: Any, input_name: str) -> None: # Capture timestamp before any wrapping (e.g. _JointStateView) hides # the original message's header. if hasattr(msg, "header"): - ts = _stamp_to_seconds(msg.header.stamp) + ts = stamp_to_sec(msg.header.stamp) else: - ts = _stamp_to_seconds(self.get_clock().now().to_msg()) + ts = stamp_to_sec(self.get_clock().now().to_msg()) if input_name in self._passthrough_sources: decoded = self._decode_passthrough(input_name, msg) @@ -625,11 +511,11 @@ def _tf_callback(self) -> None: spec["child_frame"], rclpy.time.Time(), ) - updates[name] = _make_enriched_tf(tf) - ts = _stamp_to_seconds(tf.header.stamp) + updates[name] = enriched_tf(tf) + ts = stamp_to_sec(tf.header.stamp) # A chain of only static transforms comes back stamped 0 and never changes: - # treat it as always fresh (like a `static:` input) instead of stale forever. - ts_updates[name] = ts if ts != 0.0 else _stamp_to_seconds(self.get_clock().now().to_msg()) + # treat it as always fresh instead of stale forever. + ts_updates[name] = ts if ts != 0.0 else stamp_to_sec(self.get_clock().now().to_msg()) except Exception as exc: if name not in self._tf_warned: self._tf_warned.add(name) @@ -651,12 +537,9 @@ def _tf_callback(self) -> None: # ------------------------------------------------------------------ def _evaluate_all(self) -> None: - now = _stamp_to_seconds(self.get_clock().now().to_msg()) with self._lock: snapshot = dict(self._latest_inputs) ts_snapshot = dict(self._input_timestamps) - for name in self._static_inputs: - ts_snapshot[name] = now new_vals: Dict[str, np.ndarray] = {} @@ -719,25 +602,12 @@ def _evaluate_all(self) -> None: if ok: if self._rotation_format.get(obs_name) == "rpy": - # 4 raw quaternion floats -> (roll, pitch, yaw), BEFORE - # normalization, so `limits` describes the Euler ranges. - row = [float(v) for v in _quat_to_euler(*row)] - arr = np.array(row, dtype=np.float32) - nc = self._norm_params.get(obs_name) - if nc is not None: - if len(nc.limits) not in (1, len(arr)): - # Never reuse the last limit for the rest: leave the obs unset so pre-flight fails. - self.get_logger().error( - f"obs '{obs_name}': 'limits' needs 1 entry or one per value ({len(arr)}), " - f"got {len(nc.limits)}.", - throttle_duration_sec=5.0, - ) - continue - arr = _apply_normalization(arr, nc) - new_vals[obs_name] = arr + # 4 raw quaternion floats -> (roll, pitch, yaw) + row = [float(v) for v in quat_to_euler(row)] + new_vals[obs_name] = np.array(row, dtype=np.float32) # Passthrough obs (camera/organized-pointcloud): the decoded array is - # used as-is — no flatten, no float32 cast, no normalization/rotation. + # used as-is — no flatten, no float32 cast, no rotation. for obs_name, input_name in self._passthrough_obs.items(): if not inputs_in_sync({input_name}, ts_snapshot, self._sync_threshold): self._warn_desync(obs_name, {input_name}, ts_snapshot) @@ -809,19 +679,11 @@ def validate_inputs(self, timeout: float = 10.0) -> Tuple[bool, dict]: deadline = time.monotonic() + timeout report: dict = {} - # Static inputs — always available - for name in self._static_inputs: - report[name] = { - "ok": True, - "kind": "static", - "detail": str(self._input_cfg[name]["static"]), - } - # Topic inputs — probe ROS graph with retries topic_inputs = { name: self._input_cfg[name]["topic"] for name in self._input_cfg - if name not in self._tf_inputs and name not in self._static_inputs + if name not in self._tf_inputs } pending_topics = set(topic_inputs) diff --git a/relay_d/dispatch/robomimic_runner.py b/relay_d/dispatch/robomimic_runner.py index bd37629..3fbc066 100644 --- a/relay_d/dispatch/robomimic_runner.py +++ b/relay_d/dispatch/robomimic_runner.py @@ -6,9 +6,8 @@ LeRobotPolicyRunner: get_action(obs_dict) -> np.ndarray, reset(), and an .obs_keys list. -- RolloutPolicy wants raw observations and normalizes internally with the - statistics in the checkpoint: do not set `normalize: true` in obs_config.yaml - for keys this backend uses, or they are normalized twice. +- robomimic normalizes observations and un-normalizes actions with the + checkpoint's stats; obs_config has no normalization. - Camera / organized point cloud observations (`source:` entries) reach the policy unchanged: RolloutPolicy runs the checkpoint's own encoder. - Multi-frame policies (e.g. Diffusion Policy) get their frame history in @@ -21,7 +20,7 @@ from __future__ import annotations from collections import deque -from typing import Dict, List +from typing import Dict, List, Optional import numpy as np @@ -52,6 +51,13 @@ def _safe_cfg_get(obj, attr, default=None): return default +def checkpoint_fps(ckpt: dict) -> Optional[float]: + """The rate the training data was recorded at: the converter writes it to the dataset's env_args + (env_kwargs.control_freq), which robomimic saves in the checkpoint as env_metadata. None for older checkpoints.""" + env_meta = ckpt.get("env_metadata") + return (env_meta.get("env_kwargs") or {}).get("control_freq") if isinstance(env_meta, dict) else None + + class RobomimicPolicyRunner: """Runs inference via robomimic's RolloutPolicy, loaded straight from a checkpoint.""" @@ -59,6 +65,9 @@ def __init__(self, checkpoint_path: str, device: str = "cpu"): torch_device = TorchUtils.get_torch_device(try_to_use_cuda=device.startswith("cuda")) self._policy, ckpt = FileUtils.policy_from_checkpoint(ckpt_path=checkpoint_path, device=torch_device) self.obs_keys: List[str] = list(ckpt["shape_metadata"]["all_obs_keys"]) + self.action_dim: int = int(ckpt["shape_metadata"]["ac_dim"]) + # Each predicted action is one step at this rate (None: older checkpoint, the caller picks the rate). + self.fps: Optional[float] = checkpoint_fps(ckpt) # observation_horizon (To) drives DiffusionPolicyUNet._get_action_trajectory # directly; robomimic's own shipped configs keep config.train.frame_stack @@ -82,7 +91,7 @@ def __init__(self, checkpoint_path: str, device: str = "cpu"): logger.info( f"[RobomimicPolicyRunner] algo={ckpt.get('algo_name')} device={device} " - f"obs_horizon={self._obs_horizon} checkpoint={checkpoint_path}" + f"obs_horizon={self._obs_horizon} fps={self.fps} checkpoint={checkpoint_path}" ) logger.info(f"[RobomimicPolicyRunner] obs_keys: {self.obs_keys}") diff --git a/relay_d/dispatch/templates/inference_script_template.py b/relay_d/dispatch/templates/inference_script_template.py index 181f9ae..4cc7095 100644 --- a/relay_d/dispatch/templates/inference_script_template.py +++ b/relay_d/dispatch/templates/inference_script_template.py @@ -1,17 +1,18 @@ """Run a trained policy on the robot with the configs next to this script. Same options and defaults as `lfd-inference-node` (rate, stale-input limit, cleanup on Ctrl-C); -only --obs-config / --action-config default to the files in the current folder. +only --obs-config / --action-config default to the files next to this script. Example: python inference_script.py --checkpoint path/to/model.pth """ import sys +from pathlib import Path from relay_d.dispatch.inference_node import main # Default configuration file paths -ACTION_CONFIG = "./action_config.yaml" -OBS_CONFIG = "./obs_config.yaml" +ACTION_CONFIG = str(Path(__file__).with_name("action_config.yaml")) +OBS_CONFIG = str(Path(__file__).with_name("obs_config.yaml")) if __name__ == "__main__": for flag, path in (("--obs-config", OBS_CONFIG), ("--action-config", ACTION_CONFIG)): diff --git a/relay_d/training/__init__.py b/relay_d/training/__init__.py index a28326e..656d5e5 100644 --- a/relay_d/training/__init__.py +++ b/relay_d/training/__init__.py @@ -1,20 +1,4 @@ -"""Launch and follow imitation-learning training runs (robomimic, LeRobot). No Qt, no ROS.""" +"""Launch and follow imitation-learning training runs (robomimic, LeRobot). No Qt, no ROS. -from relay_d.training.launcher import ( - LEROBOT, - ROBOMIMIC, - DatasetInfo, - TrainingError, - TrainJob, - build_command, - check_environment, - default_params, - find_datasets, - start, -) -from relay_d.training.runs import Run, list_runs, read_run, stop - -__all__ = [ - "LEROBOT", "ROBOMIMIC", "DatasetInfo", "Run", "TrainJob", "TrainingError", "build_command", - "check_environment", "default_params", "find_datasets", "list_runs", "read_run", "start", "stop", -] +Import the modules directly: ``launcher``, ``runs``, ``envsetup``, ``train_cli``. +""" diff --git a/relay_d/training/launcher.py b/relay_d/training/launcher.py index 7dc6279..9c466a2 100644 --- a/relay_d/training/launcher.py +++ b/relay_d/training/launcher.py @@ -23,7 +23,6 @@ from relay_d.utils import user_settings from relay_d.acquisition.qt_app.ui.postprocess.postprocess_config_loader import PostprocessConfigLoader -from relay_d.acquisition.qt_app.ui.postprocess.postprocess_options import PostprocessOptions from relay_d.acquisition.qt_app.ui.postprocess.robomimic_config_handler import ( RobomimicConfigHandler, classify_observations, @@ -90,7 +89,7 @@ def name(self) -> str: class TrainJob: dataset: DatasetInfo python: str = sys.executable - policy: Optional[str] = None # robomimic: None = the folder's robomimic_config.json / postprocess options + policy: Optional[str] = None # robomimic: None = the folder's saved robomimic_config.json, else the first template params: Dict[str, Any] = field(default_factory=dict) # only the keys the user changed extra_args: str = "" @@ -170,7 +169,7 @@ def _set_path(cfg: dict, dotted: str, value) -> None: def _config_file(dataset: "DatasetInfo", name: str) -> Optional[Path]: - """``name`` (e.g. ``postprocess_config.yaml``) that belongs to ``dataset``, next to it. + """``name`` (e.g. ``acquisition_config.yaml``) that belongs to ``dataset``, next to it. ``relay-d-convert`` writes ``_``, so a folder can hold several pairs: take the one named after this dataset, else the plain file the GUI writes, else any ``*``. @@ -182,17 +181,10 @@ def _config_file(dataset: "DatasetInfo", name: str) -> Optional[Path]: def _robomimic_policy(dataset: DatasetInfo, policy: Optional[str]) -> str: - """The template name to use: explicit, else the folder's postprocess options, else the default.""" - if policy: - if policy not in ROBOMIMIC_POLICIES: - raise TrainingError(f"unknown robomimic policy '{policy}' (choose from {ROBOMIMIC_POLICIES})") - return policy - options_file = _config_file(dataset, "postprocess_config.yaml") - if options_file is not None: - options, _ = PostprocessOptions.load(str(options_file)) - if options is not None: - return options.training_model() - return PostprocessOptions().training_model() + """The template name to use: explicit (the Train tab's Policy box / --policy), else the first one.""" + if policy and policy not in ROBOMIMIC_POLICIES: + raise TrainingError(f"unknown robomimic policy '{policy}' (choose from {ROBOMIMIC_POLICIES})") + return policy or ROBOMIMIC_POLICIES[0] def _robomimic_modalities(dataset: DatasetInfo) -> Dict[str, list]: diff --git a/relay_d/training/train_cli.py b/relay_d/training/train_cli.py index 4622128..c7555a6 100644 --- a/relay_d/training/train_cli.py +++ b/relay_d/training/train_cli.py @@ -30,7 +30,7 @@ def build_parser() -> argparse.ArgumentParser: ap = argparse.ArgumentParser(prog="relay-d-train", description="Start imitation-learning training on a dataset.") ap.add_argument("--folder", required=True, help="Post-process output folder, or the dataset itself") ap.add_argument("--format", choices=["auto", "robomimic", "lerobot"], default="auto") - ap.add_argument("--policy", help=f"robomimic: {launcher.ROBOMIMIC_POLICIES} (default: the folder's config); " + ap.add_argument("--policy", help=f"robomimic: {launcher.ROBOMIMIC_POLICIES} (default: the folder's saved robomimic_config.json, else {launcher.ROBOMIMIC_POLICIES[0]}); " f"lerobot: {launcher.LEROBOT_POLICIES} (default: act)") ap.add_argument("--python", help="interpreter with robomimic / lerobot installed (default: the python_ setting, else this one)") ap.add_argument("--epochs", type=int, help="robomimic: number of epochs") diff --git a/relay_d/utils/numerics.py b/relay_d/utils/numerics.py index 704c56f..e02592a 100644 --- a/relay_d/utils/numerics.py +++ b/relay_d/utils/numerics.py @@ -4,20 +4,15 @@ than it was trained on. Keep them here, not in two copies. """ +import types from typing import Optional import numpy as np -def normalize(x, lower: float, upper: float, norm_min: float = -1.0, norm_max: float = 1.0): - """Linearly map ``x`` from [lower, upper] to [norm_min, norm_max]. - - A degenerate range (upper <= lower) leaves ``x`` unchanged. ``x`` may be a scalar or an array. - """ - span = upper - lower - if span <= 0: - return x - return (x - lower) / span * (norm_max - norm_min) + norm_min +def stamp_to_sec(stamp) -> float: + """builtin_interfaces/Time (sec, nanosec), e.g. a header.stamp -> float seconds.""" + return stamp.sec + stamp.nanosec * 1e-9 def quat_to_euler(q) -> np.ndarray: @@ -39,6 +34,22 @@ def quat_to_rot_matrix(x: float, y: float, z: float, w: float) -> np.ndarray: ], dtype=np.float32) +def enriched_tf(tf_stamped) -> types.SimpleNamespace: + """TransformStamped -> object whose ``transform`` also has euler.roll/pitch/yaw, rot6d.i0..i5 + (first two columns of R, column-major) and rot_matrix.r00..r22, so stream specs such as + ``tf.euler.yaw`` read the same values when recording and at inference.""" + t, r = tf_stamped.transform.translation, tf_stamped.transform.rotation + roll, pitch, yaw = (float(v) for v in quat_to_euler([r.x, r.y, r.z, r.w])) + R = quat_to_rot_matrix(r.x, r.y, r.z, r.w) + return types.SimpleNamespace(transform=types.SimpleNamespace( + translation=t, + rotation=r, + euler=types.SimpleNamespace(roll=roll, pitch=pitch, yaw=yaw), + rot_matrix=types.SimpleNamespace(**{f"r{i}{j}": R[i, j] for i in range(3) for j in range(3)}), + rot6d=types.SimpleNamespace(**{f"i{i}": float(v) for i, v in enumerate(list(R[:, 0]) + list(R[:, 1]))}), + )) + + def inputs_in_sync(names, timestamps, threshold: Optional[float]) -> bool: """True if the given inputs' timestamps all fall within `threshold` seconds of each other. diff --git a/relay_d/utils/ros_media_codec.py b/relay_d/utils/ros_media_codec.py index cac15fd..e1b6b6a 100644 --- a/relay_d/utils/ros_media_codec.py +++ b/relay_d/utils/ros_media_codec.py @@ -84,17 +84,30 @@ def cv2_to_imgmsg(arr, encoding: str = "bgr8"): return msg +_no_bgr8_warned = set() # (encoding, frame_id) already warned about: once per camera, not once per frame + + def decode_image(img_msg, is_depth: bool = False): """Decode a sensor_msgs/Image into an (H, W, C) or (H, W) numpy array. `is_depth` selects "passthrough" mode instead of "bgr8" so the native depth encoding (typically 32FC1 float meters or 16UC1 uint16 millimeters) is - preserved instead of being reinterpreted as color. + preserved instead of being reinterpreted as color. An encoding with no bgr8 + conversion (16UC1, 32FC1, mono16, 8UC3, rgb16, ...) is also kept as decoded. Returns None on decode failure. """ try: - return imgmsg_to_cv2(img_msg, "passthrough" if is_depth else "bgr8") + if is_depth or img_msg.encoding not in _ENCODINGS: + return imgmsg_to_cv2(img_msg, "passthrough") # an unknown encoding raises here + try: + return imgmsg_to_cv2(img_msg, "bgr8") + except ValueError as e: + key = (img_msg.encoding, getattr(getattr(img_msg, "header", None), "frame_id", "")) + if key not in _no_bgr8_warned: + _no_bgr8_warned.add(key) + logger.warning(f"{e}: keeping '{img_msg.encoding}' images as decoded (frame '{key[1]}')") + return imgmsg_to_cv2(img_msg, "passthrough") except Exception as e: logger.error(f"Error decoding image message: {e}") return None diff --git a/relay_d/utils/user_settings.py b/relay_d/utils/user_settings.py index 32159d4..e377840 100644 --- a/relay_d/utils/user_settings.py +++ b/relay_d/utils/user_settings.py @@ -55,23 +55,15 @@ def _s(*args, **kwargs) -> Setting: _s("hdf5_load_folder", "General", "Open-files folder", "folder", "", "Where file dialogs for recordings start."), _s("config_folder", "General", "Config folder", "folder", "", "Where the config / options file dialogs start."), _s("ros_spin_ms", "General", "ROS spin interval (ms)", "int", 10, - "How often the GUI processes incoming ROS messages. Lower is more responsive and uses more CPU.", + "How often the GUI processes ROS messages for its own pages (recorded topics have their own thread). Lower is more responsive and uses more CPU.", minimum=1, maximum=100), _s("ros_domain_id", "General", "ROS domain id (-1 = environment)", "int", -1, "Sets ROS_DOMAIN_ID for this app. Ignored if the variable is already set, or -1.", minimum=-1, maximum=232, applies=RESTART), # ---- Recording - _s("rec_default_frequency", "Recording", "Default recording frequency (Hz)", "int", 30, - "Used when the acquisition config has no recording_frequency.", minimum=1, maximum=1000), _s("rec_default_max_buffer_mb", "Recording", "Default memory buffer (MB)", "int", 4096, "Samples stay in RAM until Stop (a crash before Stop loses the demo being recorded); sampling stops at this size. Used when the config has no max_buffer_mb.", minimum=64, maximum=1_000_000), - _s("rec_default_sync_mode", "Recording", "Default sync mode", "choice", "soft", - "soft: warn when inputs drift apart; strict: skip the sample. Used when the config has no sync_mode.", - ("soft", "strict")), - _s("rec_default_sync_tolerance_sec", "Recording", "Default sync tolerance (s, 0 = off)", "float", 0.0, - "Largest allowed spread between input timestamps. Used when the config has no sync_tolerance_sec.", - minimum=0.0, maximum=10.0), _s("rec_warmup_s", "Recording", "Warm-up wait before recording (s)", "float", 2.0, "Time to wait for every input to deliver a first sample after Start.", minimum=0.0, maximum=60.0), _s("session_folder_format", "Recording", "Session folder name format", "text", "%Y-%m-%d_%H-%M-%S", @@ -100,16 +92,11 @@ def _s(*args, **kwargs) -> Setting: "How long to wait for a robot description from a topic.", minimum=0.5, maximum=120.0), _s("review_playback_tick_ms", "Review", "Playback UI refresh (ms)", "int", 150, "How often the slider and plot cursor follow the playback.", minimum=20, maximum=2000), - _s("review_plot_max_points", "Review", "Plot decimation (max points)", "int", 5000, - "Long state / action curves are thinned to this many points.", minimum=200, maximum=200_000), _s("review_last_robot_description", "Review", "Last robot description file", "file", "", "Remembered URDF / xacro of the last playback."), # ---- Post-process _s("postprocess_default_format", "Post-process", "Default output format", "choice", "robomimic", "Format selected when the Post-process page opens.", ("robomimic", "lerobot")), - _s("postprocess_default_training_model", "Post-process", "Default robomimic training model", "choice", - "bc_rnn_gmm", "Used when postprocess_config.yaml has no training_model.", - ("bc_rnn_gmm", "bc_transformer_gmm", "diffusion_policy")), _s("split_train", "Post-process", "Default train fraction", "float", 0.8, "Used when postprocess_config.yaml has no robomimic.splits.", minimum=0.0, maximum=1.0), _s("split_val", "Post-process", "Default validation fraction", "float", 0.2, @@ -150,6 +137,10 @@ def _s(*args, **kwargs) -> Setting: _s("infer_backend", "Inference", "Default backend", "choice", "auto", "Default of --backend. auto: robomimic for a .pth file, LeRobot for a pretrained_model folder.", ("auto", "robomimic", "lerobot")), + _s("run_last_checkpoint", "Inference", "Last Run checkpoint", "file", "", + "Remembered checkpoint of the last policy started from the Run tab."), + _s("run_last_dispatch_dir", "Inference", "Last Run dispatcher folder", "folder", "", + "Remembered folder with obs_config.yaml and action_config.yaml of the last Run."), ) BY_KEY: Dict[str, Setting] = {s.key: s for s in SETTINGS} diff --git a/scripts/convert_dataset.py b/scripts/convert_dataset.py deleted file mode 100755 index 4d93c25..0000000 --- a/scripts/convert_dataset.py +++ /dev/null @@ -1,13 +0,0 @@ -#!/usr/bin/env python3 -"""Headless dataset conversion (no GUI, no ROS). See relay_d/acquisition/convert_cli.py. - - python scripts/convert_dataset.py --config acquisition.yaml --format lerobot \ - --input recordings/ --output my_lerobot_dataset -""" - -import sys - -from relay_d.acquisition.convert_cli import main - -if __name__ == "__main__": - sys.exit(main()) diff --git a/scripts/extract_joint_limits.py b/scripts/extract_joint_limits.py index fbca16c..9e35f9b 100755 --- a/scripts/extract_joint_limits.py +++ b/scripts/extract_joint_limits.py @@ -1,7 +1,11 @@ #!/usr/bin/env python3 """ Extract joint position, velocity, and effort limits from a URDF robot description -and print them as ready-to-paste YAML blocks for acquisition_config.yaml. +and print them as ready-to-paste `limits:` blocks for action_config.yaml entries. + +The dispatcher clamps each model output value to [lower_lim, upper_lim] before sending it, +so these are real units (rad, m, rad/s, N·m), the same units the model outputs. +Relay-D does not normalize: robomimic and LeRobot normalize with the dataset's stats. Usage ----- @@ -11,16 +15,13 @@ From the live ROS 2 parameter server (robot_state_publisher must be running): python3 scripts/extract_joint_limits.py --ros2 -Filter / reorder joints (must match the joint_names order in acquisition_config.yaml): +Filter / reorder joints (must match the order of the entry's indices): python3 scripts/extract_joint_limits.py --urdf robot.urdf \\ --joints iiwa_joint_1 iiwa_joint_2 iiwa_joint_3 \\ iiwa_joint_4 iiwa_joint_5 iiwa_joint_6 iiwa_joint_7 -Change the output stream-name prefix (default: robot0): +Change the output entry-name prefix (default: robot0): python3 scripts/extract_joint_limits.py --urdf robot.urdf --prefix robot0 - -Change the input name used in the spec (default: joint_1): - python3 scripts/extract_joint_limits.py --urdf robot.urdf --input-name joint_1 """ import argparse @@ -101,21 +102,18 @@ def _fmt(value: float) -> str: return f"{value:8.4f}" -def _limits_block(stream_name: str, spec: str, joints: list, +def _limits_block(entry_name: str, joints: list, lo_key: str, hi_key: str, symmetric: bool) -> str: """ - Produce one YAML dict-form stream block. + Produce one action_config.yaml entry with its `limits:` clamp (add indices and a dispatch method). symmetric=True → lower_lim = -value, upper_lim = +value (vel / eff) symmetric=False → lower_lim = joint[lo_key], upper_lim = joint[hi_key] (pos) """ lines = [ - f" {stream_name}:", - f" specs:", - f" - {spec}", - f" normalize: true", - f" normalized_range: [-1.0, 1.0]", - f" limits:", + f" {entry_name}:", + f" indices: [{', '.join(str(i) for i in range(len(joints)))}] # positions in the model output", + f" limits: # clamp, real units", ] for j in joints: if symmetric: @@ -126,40 +124,37 @@ def _limits_block(stream_name: str, spec: str, joints: list, lo = j[lo_key] hi = j[hi_key] lines.append( - f" - {{lower_lim: {_fmt(lo)}, upper_lim: {_fmt(hi)}}}" + f" - {{lower_lim: {_fmt(lo)}, upper_lim: {_fmt(hi)}}}" f" # {j['name']} ({j['type']})" ) return "\n".join(lines) -def print_yaml_blocks(joints: list, prefix: str, input_name: str) -> None: +def print_yaml_blocks(joints: list, prefix: str) -> None: if not joints: sys.exit("ERROR: no matching revolute/prismatic joints found.") pos_block = _limits_block( f"{prefix}_joint_pos", - f"{input_name}.position[:]", joints, lo_key="lower", hi_key="upper", symmetric=False, ) vel_block = _limits_block( f"{prefix}_joint_vel", - f"{input_name}.velocity[:]", joints, lo_key=None, hi_key="velocity", symmetric=True, ) eff_block = _limits_block( f"{prefix}_joint_eff", - f"{input_name}.effort[:]", joints, lo_key=None, hi_key="effort", symmetric=True, ) separator = "\n" - print("# ── paste into acquisition_config.yaml → output.streams ──────────────────\n") + print("# ── paste the limits you need into action_config.yaml entries (clamp, real units) ──\n") print(pos_block) print(separator) print(vel_block) @@ -191,19 +186,14 @@ def build_parser() -> argparse.ArgumentParser: p.add_argument( "--joints", nargs="+", metavar="JOINT_NAME", help="Filter and order output to these joint names " - "(must match joint_names in acquisition_config.yaml). " + "(must match the order of the action_config entry's indices). " "Defaults to all revolute/prismatic joints in URDF order.", ) p.add_argument( "--prefix", default="robot0", metavar="PREFIX", - help="Stream-name prefix (default: robot0). " + help="Entry-name prefix (default: robot0). " "Produces _joint_pos, _joint_vel, _joint_eff.", ) - p.add_argument( - "--input-name", default="joint_1", metavar="NAME", - help="Input source name used in the spec string " - "(default: joint_1, matching acquisition_config.yaml).", - ) p.add_argument( "--node-name", default="robot_state_publisher", metavar="NODE", help="ROS 2 node that owns the robot_description parameter " @@ -221,7 +211,7 @@ def main() -> None: xml_string = _load_from_file(args.urdf) joints = parse_joint_limits(xml_string, joint_filter=args.joints) - print_yaml_blocks(joints, prefix=args.prefix, input_name=args.input_name) + print_yaml_blocks(joints, prefix=args.prefix) if __name__ == "__main__": diff --git a/tests/test_action_dispatcher.py b/tests/test_action_dispatcher.py index c8fce2a..bf8eaf4 100644 --- a/tests/test_action_dispatcher.py +++ b/tests/test_action_dispatcher.py @@ -1,5 +1,7 @@ """Action math and safe stop of ActionDispatcher. Needs a sourced ROS 2 (rclpy): skipped without it, run by the "Tests (ROS 2 Jazzy)" CI job.""" +import threading + import numpy as np import pytest @@ -9,21 +11,17 @@ @pytest.fixture def d(): - return ActionDispatcher.__new__(ActionDispatcher) # skip ROS setup: only pure methods are used - - -def test_limits_broadcast_clip_and_none(d): - lims = [{"lower_lim": 0.0, "upper_lim": 10.0}] - out = d._apply_limits(np.array([-1.0, 0.0, 1.0, 5.0]), lims, [-1.0, 1.0]) - assert out.tolist() == [0.0, 5.0, 10.0, 10.0] # out-of-range input is clipped - out = d._apply_limits(np.array([1.0, 0.3]), [None, {"lower_lim": 0, "upper_lim": 2}], [-1.0, 1.0]) - assert out[0] == 1.0 # None entry: unchanged - assert d._apply_limits(np.array([0.5]), lims, [1.0, 1.0]).tolist() == [0.5] # zero span: unchanged + d = ActionDispatcher.__new__(ActionDispatcher) # skip ROS setup: only pure methods are used + d._stopped, d._dry_run = True, False # as __init__ sets them + d._goals, d._goals_lock = [], threading.Lock() + return d -def test_render_command(d): - cmd = d._render_command("run {I[0]} {I[1]} {I[10]}", np.array([3.0, 0.1234567])) - assert cmd == "run 3 0.123457 {I[10]}" # {I[1]} must not eat {I[10]} +def test_limits_clamp_broadcast_and_none(d): + lims = [{"lower_lim": 0.0, "upper_lim": 2.0}] + assert d._apply_limits(np.array([5.0, -1.0, 1.5]), lims).tolist() == [2.0, 0.0, 1.5] # clamp, not rescale + out = d._apply_limits(np.array([5.0, 5.0]), [None, {"lower_lim": 0, "upper_lim": 2}]) + assert out.tolist() == [5.0, 2.0] # None entry: unchanged def test_dispatch_drops_non_finite_action(d): @@ -31,6 +29,10 @@ def test_dispatch_drops_non_finite_action(d): d._last_sent = {} d.dispatch(np.array([np.nan])) # would raise (no publishers set up) if it got past the guard d.dispatch(np.array([np.inf])) + published = _stop_dispatcher(d, {"base": {"indices": [0], "topic": "/cmd_vel", "mapping": {"x": 0}, "stop": [0]}}) + d._stopped = False + d.dispatch(np.array([np.nan])) + assert published == [("base", 0.0, 0.0)] # a velocity controller must not keep the last command def test_set_nested_casts_to_field_type(): @@ -59,7 +61,7 @@ def test_fill_msg_reports_failed_mapping(): def test_failed_send_is_not_remembered_so_it_is_retried(d): d._action_configs = {"grip": {"indices": [0], "action": True}} - d._last_sent, d._procs = {}, {} + d._last_sent = {} sent = [] results = iter([False, True, True]) d._dispatch_action = lambda name, cfg, values: sent.append(name) or next(results) @@ -73,7 +75,7 @@ def test_dispatch_sends_nothing_if_one_entry_cannot_be_computed(d): "arm": {"indices": [0], "topic": "/arm"}, "tool": {"indices": [0], "transform": {"type": "nope"}}, } - d._last_sent, d._procs = {}, {} + d._last_sent = {} d._dispatch_topic = lambda *a: pytest.fail("arm must not move while the tool entry is broken") with pytest.raises(ValueError): d.dispatch(np.array([1.0])) @@ -92,11 +94,16 @@ def test_check_mapping_rejects_a_field_the_message_does_not_have(d): def test_dispatch_sends_nothing_if_a_transform_gives_nan(d): d._action_configs = { "arm": {"indices": [0], "topic": "/arm"}, - "tool": {"indices": [0], "transform": {"type": "scale", "in_min": 1, "in_max": 1, "out_min": 0, "out_max": 1}}, + "tool": {"indices": [0], "limits": [{"lower_lim": float("nan"), "upper_lim": 1.0}]}, # e.g. a NaN limit } - d._last_sent, d._procs = {}, {} + d._last_sent = {} d._dispatch_topic = lambda *a: pytest.fail("nothing may be sent when one output is NaN") d.dispatch(np.array([1.0])) + d._action_configs["arm"].update(mapping={"x": 0}, stop=[0]) + published = _stop_dispatcher(d, d._action_configs) + d._stopped = False + d.dispatch(np.array([1.0])) + assert published == [("arm", 0.0, 0.0)] # the stop goes out instead def test_bool_field_is_true_only_above_half(): @@ -145,7 +152,7 @@ def test_rejected_goal_is_retried_even_if_rejected_before_send_returns(d): from types import SimpleNamespace d._action_configs = {"grip": {"indices": [0], "action": True}} - d._last_sent, d._procs = {}, {} + d._last_sent = {} sent = [] def send_and_reject(name, cfg, values): # the server answers before _dispatch_action returns @@ -170,13 +177,87 @@ def test_fill_msg_keeps_the_frame_set_by_goal_init(): assert (msg.header.frame_id, msg.header.stamp) == ("base_link", "now") -def test_check_mapping_rejects_a_short_limits_list(d): +@pytest.mark.parametrize("entry", [ + {"action": True, "action_type": "pkg/action/Name", "topic": "/arm", "mapping": {}}, + {"topic": "/arm", "msg_type": "std_msgs/msg/Float64MultiArray", "mapping": {}}, +]) +def test_short_limits_list_is_rejected_at_startup(tmp_path, entry): + import yaml + + lims = [{"lower_lim": 0.0, "upper_lim": 1.0}] * 2 + path = tmp_path / "action_config.yaml" + path.write_text(yaml.safe_dump({"actions": {"arm": {"indices": [0, 1, 2], "limits": lims, **entry}}})) + with pytest.raises(ValueError, match="limits"): # index 2 would reuse index 1's range + ActionDispatcher(None, str(path)) + + +@pytest.mark.parametrize("key", [{"denormalize": True}, {"apply_limits": True}, {"normalized_range": [-1.0, 1.0]}]) +def test_old_denormalize_keys_are_rejected_at_startup(tmp_path, key): + import yaml + + path = tmp_path / "action_config.yaml" + entry = {"indices": [0], "topic": "/grip", "msg_type": "std_msgs/msg/Float64", "mapping": {"data": 0}, + "limits": [{"lower_lim": 0.0, "upper_lim": 2.0}], **key} + path.write_text(yaml.safe_dump({"actions": {"grip": entry}})) + with pytest.raises(ValueError, match="grip.*denormalize"): # the model already outputs real units + ActionDispatcher(None, str(path)) + + +def _fake_node(sent): + """A node whose publishers append every published message to ``sent``.""" from types import SimpleNamespace - d._node = None - lims = [{"lower_lim": 0.0, "upper_lim": 1.0}] * 3 - with pytest.raises(ValueError, match="limits"): # joints 3-5 would reuse joint 2's range - d._check_mapping("arm", {"indices": list(range(6)), "limits": lims, "mapping": {}}, lambda: SimpleNamespace()) + return SimpleNamespace(create_publisher=lambda cls, topic, qos: SimpleNamespace(publish=sent.append)) + + +def _load(tmp_path, actions, node=None, **kwargs): + import yaml + + path = tmp_path / "action_config.yaml" + path.write_text(yaml.safe_dump({"actions": actions})) + return ActionDispatcher(node, str(path), **kwargs) + + +_GRIP = {"indices": [0], "topic": "/grip", "msg_type": "std_msgs/msg/Float64", "mapping": {"data": 0}} + + +def test_limits_clamp_the_dispatched_value(tmp_path): + sent = [] + entry = {**_GRIP, "limits": [{"lower_lim": 0.0, "upper_lim": 2.0}], + "denormalize": False} # accepted and ignored: old configs that never de-normalized still start + d = _load(tmp_path, {"grip": entry}, _fake_node(sent)) + d.dispatch(np.array([5.0])) + assert [m.data for m in sent] == [2.0] # clamped to upper_lim, not sent as 5 + + +def test_topic_entry_is_published_every_tick(tmp_path): + sent = [] + d = _load(tmp_path, {"grip": _GRIP}, _fake_node(sent)) + d.dispatch(np.array([1.0])) + d.dispatch(np.array([1.0])) + assert [m.data for m in sent] == [1.0, 1.0] # a controller with a watchdog needs the stream + + +@pytest.mark.parametrize("extra, match", [ + ({"command": "go {I[0]}"}, "command.*removed.*topic entry.*action entry"), + ({"dedupe": True}, "dedupe.*removed.*every tick"), + ({"transform": {"type": "scale", "in_min": 0, "in_max": 1, "out_min": 0, "out_max": 2}}, "scale.*removed.*limits"), + ({"transform": {"type": "clip", "out_min": 0, "out_max": 1}}, "clip.*removed.*limits"), + ({"transform": {"type": "vacuum"}}, "vacuum.*removed.*threshold"), + ({"transform": {"type": "magnetic"}}, "magnetic.*removed.*threshold"), +]) +def test_removed_keys_are_rejected_at_startup(tmp_path, extra, match): + with pytest.raises(ValueError, match="'grip'.*" + match): # never silently ignored: it would change what the robot gets + _load(tmp_path, {"grip": {**_GRIP, **extra}}) + + +@pytest.mark.parametrize("transform, expected", [ + ({"type": "threshold", "threshold": 0.5}, [0.0, 0.0, 1.0]), # default: 1 / 0, as before + ({"type": "threshold", "threshold": 0.5, "on_value": 255, "off_value": 10}, [10.0, 10.0, 255.0]), +]) +def test_threshold_on_and_off_values(d, transform, expected): + out = d._apply_transform(np.array([0.2, 0.5, 0.9], dtype=np.float32), transform) + assert out.dtype == np.float32 and out.tolist() == expected def test_ack_arriving_while_a_stale_one_is_read_is_not_lost(d): @@ -203,21 +284,10 @@ def get(self, key, default=None): # the right ack lands just as the stale one i assert time.monotonic() - t0 < 0.4 # was: the fresh ack's signal was cleared and the wait timed out -@pytest.mark.parametrize("transform", [ - {"type": "scale", "in_min": 1, "in_max": 1, "out_min": 0, "out_max": 1}, - {"type": "magnetic", "strength_scale": {"in_min": 0.5, "in_max": 0.5, "out_min": 0, "out_max": 100}}, -]) -def test_zero_input_span_is_rejected_at_startup(transform): - with pytest.raises(ValueError, match="in_min != in_max"): # magnetic would raise ZeroDivisionError mid-task - ActionDispatcher._check_transform("tool", transform) - ActionDispatcher._check_transform("tool", {"type": "threshold"}) - ActionDispatcher._check_transform("tool", None) - - def _stop_dispatcher(d, entries): from types import SimpleNamespace - d._action_configs, d._last_sent, d._procs, d._stop_procs, d._node = entries, {}, {}, [], None + d._action_configs, d._last_sent, d._node = entries, {}, None d._msg_classes = {n: (lambda: SimpleNamespace(x=0.0, y=0.0)) for n, c in entries.items() if "topic" in c} published = [] d._publishers = {n: SimpleNamespace(publish=lambda m, n=n: published.append((n, m.x, m.y))) for n in d._msg_classes} @@ -239,34 +309,6 @@ def test_stop_is_sent_once_until_the_next_dispatch(d): assert published[-1] == ("base", 0.0, 0.0) and len(published) == 4 -def test_command_after_a_stop_is_not_skipped_as_a_repeat(d, monkeypatch): - import relay_d.dispatch.action_dispatcher as ad - - started = [] - - class Proc: - def __init__(self, argv, **kw): - self.argv, self.terminated = argv, False - started.append(self) - - def poll(self): - return 0 if self.terminated else None - - def terminate(self): - self.terminated = True - - monkeypatch.setattr(ad.subprocess, "Popen", Proc) - _stop_dispatcher(d, {"vel": {"indices": [0], "command": "vel {I[0]}", "stop": [0]}}) - d.dispatch(np.array([0.5])) - motion = started[0] - d.send_stop() - assert motion.terminated and started[1].argv == ["vel", "0"] # the old motion ends, the stop command runs - d.close() - assert not started[1].terminated # close() must not kill the stop command - d.dispatch(np.array([0.5])) - assert started[-1].argv == ["vel", "0.5"] # same as before the stop, but sent: the robot was stopped since - - def test_one_failing_stop_does_not_block_the_others(d): from types import SimpleNamespace @@ -282,12 +324,105 @@ def test_one_failing_stop_does_not_block_the_others(d): @pytest.mark.parametrize("cfg, match", [ ({"indices": [0], "action": True, "stop": [0]}, "cancel"), - ({"indices": [0, 1], "command": "go {I[0]} {I[1]}", "stop": [0]}, "too few"), - ({"indices": [0], "command": "go {I[0]}", "stop": "zero"}, "list of numbers"), - ({"indices": [0], "command": "go {I[0]}", "stop": [float("nan")]}, "list of numbers"), + ({"indices": [0, 1], "topic": "/a", "mapping": {"x": 0, "y": 1}, "stop": [0]}, "does not fit"), + ({"indices": [0], "topic": "/a", "mapping": {"x": 0}, "stop": "zero"}, "list of numbers"), + ({"indices": [0], "topic": "/a", "mapping": {"x": 0}, "stop": [float("nan")]}, "list of numbers"), ({"indices": [0], "topic": "/a", "mapping": {"x": 0, "nope": 1}, "stop": [0, 0]}, "does not fit"), ]) def test_a_broken_stop_is_rejected_at_startup(d, cfg, match): _stop_dispatcher(d, {"e": cfg}) with pytest.raises(ValueError, match=match): d._check_stop("e", cfg) + + +def test_dry_run_creates_no_publisher_and_logs_dispatch_and_stop(tmp_path, monkeypatch): + from types import SimpleNamespace + + import relay_d.dispatch.action_dispatcher as ad + + logged = [] + monkeypatch.setattr(ad.logger, "info", logged.append) + monkeypatch.setattr(ad.logger, "warning", logged.append) + node = SimpleNamespace(create_publisher=lambda *a: pytest.fail("dry run must not create a publisher")) + d = _load(tmp_path, {"grip": {**_GRIP, "stop": [0.25]}}, node, dry_run=True) + d.dispatch(np.array([1.5])) + d.send_stop() + dry = [m for m in logged if "[DRY RUN]" in m] + assert len(dry) == 2 and "'grip'" in dry[0] and "1.5" in dry[0] and "stop" in dry[1] and "0.25" in dry[1] + + +def test_dry_run_creates_no_action_client_and_does_not_wait_for_feedback(tmp_path, monkeypatch): + import time + + import relay_d.dispatch.action_dispatcher as ad + + logged = [] + monkeypatch.setattr(ad.logger, "info", logged.append) + monkeypatch.setattr(ad, "ActionClient", lambda *a: pytest.fail("dry run must not create an action client")) + entry = {"indices": [0], "action": True, "topic": "/fib", "action_type": "example_interfaces/action/Fibonacci", + "mapping": {"order": 0}, "feedback": {"success_field": "sequence", "timeout": 5.0, "on_timeout": "raise"}} + with pytest.raises(ValueError, match="mapping"): # messages are still built: a mapping error still stops startup + _load(tmp_path, {"fib": {**entry, "mapping": {"nope": 0}}}, dry_run=True) + d = _load(tmp_path, {"fib": entry}, dry_run=True) + t0 = time.monotonic() + d.dispatch(np.array([3.0])) # no server answers in a dry run: no wait for its result + assert time.monotonic() - t0 < 1.0 + assert any("[DRY RUN]" in m and "'fib'" in m and "order=3" in m for m in logged) + + +def _goal_handle(status=1): # 1 = STATUS_ACCEPTED + from types import SimpleNamespace + + h = SimpleNamespace(accepted=True, status=status, cancels=[]) + h.cancel_goal_async = lambda: h.cancels.append(1) + return h + + +def _accept(d, handle, name="fib"): + from types import SimpleNamespace + + d._goal_response_callback(SimpleNamespace(result=lambda: handle), name) + + +def test_stop_cancels_each_active_goal_once(d): + _stop_dispatcher(d, {"fib": {"indices": [0], "action": True}}) + active, done = _goal_handle(), _goal_handle(status=4) # 4 = STATUS_SUCCEEDED + d._stopped = False # a goal was sent + _accept(d, active) + _accept(d, done) + d.send_stop() + d.send_stop() + assert active.cancels == [1] and done.cancels == [] # a finished goal has nothing to cancel + late = _goal_handle() + _accept(d, late) # sent before the stop, accepted after it: still cancelled + assert late.cancels == [1] + + +def test_stop_cancels_a_goal_sent_by_the_waiting_path(d): + from types import SimpleNamespace + + _stop_dispatcher(d, {"fib": {"indices": [0], "action": True}}) + handle = _goal_handle() + handle.get_result_async = lambda: SimpleNamespace(add_done_callback=lambda cb: None) # never finishes + future = SimpleNamespace(add_done_callback=lambda cb: cb(SimpleNamespace(result=lambda: handle))) + client = SimpleNamespace(send_goal_async=lambda goal, feedback_callback=None: future) + d._stopped = False + d._dispatch_action_and_wait("fib", client, object(), {"timeout": 0.05}) + d.send_stop() + assert handle.cancels == [1] + + +def test_dry_run_logs_the_cancel_of_a_sent_goal(tmp_path, monkeypatch): + import relay_d.dispatch.action_dispatcher as ad + + logged = [] + monkeypatch.setattr(ad.logger, "info", logged.append) + monkeypatch.setattr(ad.logger, "warning", logged.append) + entry = {"indices": [0], "action": True, "topic": "/fib", "action_type": "example_interfaces/action/Fibonacci", + "mapping": {"order": 0}} + d = _load(tmp_path, {"fib": entry}, dry_run=True) + d.send_stop() + assert not [m for m in logged if "cancel" in m] # nothing sent yet + d.dispatch(np.array([3.0])) + d.send_stop() + assert len([m for m in logged if "[DRY RUN]" in m and "'fib'" in m and "cancel" in m]) == 1 diff --git a/tests/test_app_api.py b/tests/test_app_api.py index bc18b6a..f680238 100644 --- a/tests/test_app_api.py +++ b/tests/test_app_api.py @@ -1,11 +1,20 @@ """AppAPI session handling. Needs a sourced ROS 2 (rclpy): skipped without it, run by the "Tests (ROS 2 Jazzy)" CI job.""" +import copy +import logging +import re from types import SimpleNamespace import pytest +import yaml + +from helpers import DATA_CONFIG pytest.importorskip("rclpy") from relay_d.acquisition.api.api import AppAPI # noqa: E402 +from relay_d.acquisition.qt_app.ui.config.yaml_parser import YamlParser # noqa: E402 +from relay_d.acquisition.qt_app.ui.review.demo_validator import CheckStatus # noqa: E402 +from relay_d.utils.tf_topic import TfTopicSubscriptionError # noqa: E402 def test_shutdown_saves_a_running_recording_before_converting(): @@ -13,18 +22,16 @@ def test_shutdown_saves_a_running_recording_before_converting(): calls = [] api._data_recorder = SimpleNamespace(is_recording=True) api.stop_recording = lambda: calls.append("stop") - api.convert_recording_to_dataset = lambda: calls.append("convert") - api._spin_thread, api._rclpy_initialized = None, False + api.convert_recording_to_dataset = lambda *a: calls.append("convert") + api._rclpy_initialized = False api._topic_subscribers = SimpleNamespace(close=lambda: None) api.shutdown() assert calls == ["stop", "convert"] # the demo was only in memory def test_demo_saved_by_a_retry_at_start_joins_the_session(): - api = AppAPI.__new__(AppAPI) - api._session_h5_files = [] - api._topic_subscribers = None - api._data_recorder = SimpleNamespace(start_recording=lambda *a, **k: True, retried_file="/data/demo_0.h5") + api = _recording_api() + api._data_recorder.retried_file = "/data/demo_0.h5" assert api.start_recording("demo_1") assert api._session_h5_files == ["/data/demo_0.h5"] # was never converted @@ -34,19 +41,119 @@ def test_shutdown_keeps_running_when_the_recording_could_not_be_saved(): calls = [] api._data_recorder = SimpleNamespace(is_recording=True, _unsaved=True, retry_save=lambda: False, h5_file_path="/full/disk.h5") api.stop_recording = lambda: calls.append("stop") - api.convert_recording_to_dataset = lambda: calls.append("convert") - api._spin_thread, api._rclpy_initialized = None, False + api.convert_recording_to_dataset = lambda *a: calls.append("convert") + api._rclpy_initialized = False api._topic_subscribers = SimpleNamespace(close=lambda: calls.append("close")) assert api.shutdown() is False assert calls == ["stop"] # no convert, no close: the demo is still only in memory -@pytest.mark.parametrize("fmt, name", [("robomimic", "combined_dataset_Robomimic.h5"), ("lerobot", "combined_dataset_Lerobot")]) -def test_session_dataset_is_named_after_the_format(fmt, name): - api = AppAPI.__new__(AppAPI) - seen = [] - api._session_h5_files = ["/rec/2026-11-02_15-00-00_Recording/d.h5"] - api._data_recorder = SimpleNamespace(recorded_data_dir="/rec/2026-11-02_15-00-00_Recording") - api._yaml_converter = SimpleNamespace(convert_multiple_files=lambda f, files, out: seen.append((f, out)) or True) - assert api.convert_recording_to_dataset(fmt) - assert seen == [(fmt, f"/rec/2026-11-02_15-00-00_Recording/{name}")] +def test_api_subscribes_tf_inputs_on_their_configured_tf_topic(): + tf_input = {"name": "ee", "is_tf": True, "parent_frame": "base", "child_frame": "tool", + "tf_topic": "/robot/tf", "tf_static_topic": "/robot/tf_static", "tf_wait_timeout": 7.0} + api = _recording_api(inputs=[tf_input]) + assert api.initialize("cfg.yaml") + assert api.start_recording() + c = api._topic_subscribers.subscribed[0][0] + assert (c.tf_topic, c.tf_static_topic, c.tf_wait_timeout) == ("/robot/tf", "/robot/tf_static", 7.0) # was /tf + + +# --- recording: the same steps as the GUI's Start / Stop + + +class _Subscribers: + """TopicSubscribers stand-in: records what AppAPI asks of it.""" + + def __init__(self, unavailable=(), fail=None): + self.subscribed, self.cleared, self.unavailable, self.fail = [], 0, list(unavailable), fail + + def unavailable_inputs(self, items): + return self.unavailable + + def create_subscriber(self, c): # the old initialize() subscribed here once + self.subscribed.append([c]) + + def create_subscribers_for_containers(self, containers): + if self.fail: + raise self.fail + self.subscribed.append(list(containers)) + return {c.container_id: True for c in containers} + + def clear_all_subscribers(self): + self.cleared += 1 + + def start_spinning(self): + pass + + +def _recording_api(inputs=({"name": "cam", "topic": "/cam"},), subscribers=None, parser=None): + api = AppAPI.__new__(AppAPI) # skip ROS setup + api._session_h5_files, api._containers = [], [] + api._topic_subscribers = subscribers or _Subscribers() + api._yaml_parser = parser or SimpleNamespace( + load_yaml_file=lambda p: True, get_input_data=lambda: list(inputs), get_dataset_name=lambda: "stack", + yaml_data={"config": {}}, get_streams=dict, clear=lambda: None, + ) + api.start_spinning = lambda: None + names = [] + api._data_recorder = SimpleNamespace( + start_recording=lambda subs, name=None: names.append(name) or True, retried_file=None, names=names, + is_recording=False, stop_recording=lambda: True, h5_file_path=None, + ) + return api + + +def test_every_start_subscribes_every_input_and_names_the_demo_like_the_gui(): + api = _recording_api() + assert api.initialize("cfg.yaml") + assert api.start_recording() and api.start_recording() + assert [[c.container_id for c in cs] for cs in api._topic_subscribers.subscribed] == [["cam"], ["cam"]] # was once, at load + assert all(re.fullmatch(r"stack_\d{13}", n) for n in api._data_recorder.names) # was demo_0, demo_1 + + +def test_start_is_refused_when_a_configured_tf_topic_cannot_be_read(): + api = _recording_api(subscribers=_Subscribers(fail=TfTopicSubscriptionError("/robot/tf"))) + assert api.initialize("cfg.yaml") + assert api.start_recording() is False + assert api._data_recorder.names == [] # as the GUI: no silent fallback to /tf + + +def test_stop_clears_the_subscribers_like_the_gui(): + api = _recording_api() + api.initialize("cfg.yaml") + api.start_recording() + api.stop_recording() + assert api._topic_subscribers.cleared == 1 + + +def test_initialize_refuses_an_input_that_is_not_available(): + api = _recording_api(subscribers=_Subscribers(unavailable=[" cam: topic /cam not found"])) + assert api.initialize("cfg.yaml") is False + + +def test_initialize_refuses_a_stream_spec_that_does_not_parse(tmp_path): + config = copy.deepcopy(DATA_CONFIG) + config["output"]["streams"]["robot_joint_pos"] = ["joint_1.position[:] + "] + path = tmp_path / "cfg.yaml" + path.write_text(yaml.safe_dump({"config": config})) + api = _recording_api(parser=YamlParser()) + assert api.initialize(str(path)) is False + assert not api._yaml_parser.is_loaded() + + +def test_the_after_recording_check_is_logged(caplog): + api = _recording_api() + api._data_recorder.h5_file_path = "/rec/stack_1.h5" + api._validator = SimpleNamespace(validate=lambda p: SimpleNamespace(checks=[ + SimpleNamespace(status=CheckStatus.PASS, name="ok", message="fine"), + SimpleNamespace(status=CheckStatus.WARN, name="rate", message="9.1 Hz"), + SimpleNamespace(status=CheckStatus.FAIL, name="samples", message="no samples"), + ])) + relay_logger = logging.getLogger("relay_d") # does not propagate to the root logger + relay_logger.addHandler(caplog.handler) + try: + assert api.stop_recording() + finally: + relay_logger.removeHandler(caplog.handler) + lines = [(r.levelname, r.getMessage()) for r in caplog.records if "Demo check" in r.getMessage()] + assert lines == [("WARNING", "Demo check [WARN] rate: 9.1 Hz"), ("ERROR", "Demo check [FAIL] samples: no samples")] diff --git a/tests/test_builder_page.py b/tests/test_builder_page.py index 84e4b93..f4aaefc 100644 --- a/tests/test_builder_page.py +++ b/tests/test_builder_page.py @@ -9,9 +9,6 @@ from relay_d.acquisition.qt_app.ui.postprocess.offline_config import load_offline_config # noqa: E402 from test_ros_catalog import MESSAGES, _Buffer, _Node # noqa: E402 -URDF = '' \ - '' - @pytest.fixture def page(qapp, monkeypatch): @@ -44,10 +41,6 @@ def test_build_save_and_load_like_a_handwritten_config(page, tmp_path, monkeypat _tick(page, "position") assert page._specs() == ["joint_1.position[:]"] page.edit_stream_name.setText("robot_joint_pos") - page.check_normalize.setChecked(True) - assert page.table_limits.rowCount() == 2 # one row per joint - page._fill_limits(URDF) # what "Fill limits from URDF" does with the file's text - assert page._limits() == [{"lower_lim": -1.0, "upper_lim": 1.0}, {"lower_lim": -2.0, "upper_lim": 2.0}] page.add_stream() page.combo_obs_input.setCurrentText("tf_eef") diff --git a/tests/test_config_draft.py b/tests/test_config_draft.py index dab4dd9..3523ad5 100644 --- a/tests/test_config_draft.py +++ b/tests/test_config_draft.py @@ -40,7 +40,7 @@ def test_suggested_names_use_the_prefix_the_record_page_needs(): def _good(): d = ConfigDraft(recording_frequency=30) d.add_input(InputDraft(name="joint_a", topic="/js", joint_names=["j1", "j2"])) - d.streams["q"] = StreamDraft(name="q", specs=["joint_a.position[:]"], normalize=True, limits=[{"lower_lim": -1, "upper_lim": 1}] * 2) + d.streams["q"] = StreamDraft(name="q", specs=["joint_a.position[:]"], explicit_dict=True) d.actions = ["q"] return d.to_config() @@ -58,11 +58,57 @@ def bad(mutate): return _messages(cfg, ERROR) assert any("not defined" in m for m in bad(lambda c: c["input"].pop("joint_a"))) - assert any("limits for 2 values" in m for m in bad(lambda c: c["output"]["streams"]["q"].update(limits=[{"lower_lim": 0, "upper_lim": 1}] * 3))) - assert any("lower_lim must be smaller" in m for m in bad(lambda c: c["output"]["streams"]["q"].update(limits=[{"lower_lim": 1, "upper_lim": 1}] * 2))) assert any("actions_to_extract" in m for m in bad(lambda c: c.update(actions_to_extract=["nope"]))) assert any("delta_actions" in m for m in bad(lambda c: c.update(delta_actions=["other"]))) - assert any("is normalized" in m for m in bad(lambda c: c.update(delta_actions=["q"]))) # q is normalized + for key in ("normalize", "normalized_range", "limits"): # Relay-D no longer normalizes: old keys must not be ignored + msgs = bad(lambda c: c["output"]["streams"]["q"].update({key: True})) + assert any("Relay-D no longer normalizes" in m and "'q'" in m for m in msgs), key assert any("positive" in m for m in bad(lambda c: c.update(recording_frequency=0))) assert any("rpy needs exactly 4" in m for m in bad(lambda c: c["output"]["streams"]["q"].update(rotation_format="rpy"))) - assert any("both recorded into" in m for m in bad(lambda c: c["input"].update(joint_b={"topic": "/x", "output_map": "joint_a"}))) + assert any("'streams'" in m for m in bad(lambda c: c["input"].update(streams={"topic": "/x"}))) + assert any("recording_frequency is required" in m for m in bad(lambda c: c.pop("recording_frequency"))) + # actions are streams only (a camera entry is an observation, the converter cannot use it as an action) + assert any("not a stream" in m for m in bad(lambda c: (c["output"].update(cam="joint_a/images"), c.update(actions_to_extract=["cam"])))) + + +def test_removed_keys_are_errors_that_say_what_to_do(): + """Every removed key fails loudly (never silently ignored) and names what to do.""" + def bad(mutate): + cfg = _good() + mutate(cfg["config"]) + return _messages(cfg, ERROR) + + cases = { + "output_map": lambda c: c["input"]["joint_a"].update(output_map="g"), + "fill_default": lambda c: c["input"]["joint_a"].update(fill_gaps=True, fill_default=0), + "transfer_rate": lambda c: c["input"]["joint_a"].update(transfer_rate=10), + "prefix_path": lambda c: c["output"].update(prefix_path="x"), + "data": lambda c: c.update(data={"a": 1}), + "metadata.recording_frequency": lambda c: c.update(metadata={"recording_frequency": 30}), + "metadata.transfer_rate": lambda c: c.update(metadata={"transfer_rate": 30}), + } + for key, mutate in cases.items(): + assert any(f"'{key}' was removed" in m and "elete" in m for m in bad(mutate)), key + + +def test_stream_width_counts_zero_n_as_n_values(): + assert stream_width(["zero:3", "a.x"], {}) == 4 + + +def test_camera_obs_for_dispatch_names_an_existing_input(): + from relay_d.acquisition.qt_app.ui.config.yaml_parser import YamlParser + + cfg = _good() + cfg["config"]["input"]["image_cam"] = {"topic": "/img"} + cfg["config"]["output"]["agentview_image"] = "image_cam/images" + parser = YamlParser() + assert parser.load_from_yaml_string(yaml.safe_dump(cfg)) + obs = parser.get_obs_config()["config"] + assert obs["observations"]["agentview_image"] == {"source": "image_cam"} + assert obs["observations"]["agentview_image"]["source"] in obs["input"] + + +def test_shipped_acquisition_template_passes_the_check(): + # "Generate Defaults" hands this file to users: it must load without errors and turn into an obs_config inference accepts. + path = Path(__file__).parents[1] / "relay_d/acquisition/qt_app/ui/config/templates/config_template.yaml" + assert [str(p) for p in check_config(yaml.safe_load(path.read_text())) if p.level == ERROR] == [] diff --git a/tests/test_conversion_fixes.py b/tests/test_conversion_fixes.py new file mode 100644 index 0000000..4195ebd --- /dev/null +++ b/tests/test_conversion_fixes.py @@ -0,0 +1,225 @@ +"""Regression tests for the audited conversion bugs (robomimic + LeRobot paths).""" + +import json + +import h5py +import numpy as np +import pytest + +pytest.importorskip("av") + +from relay_d.acquisition.convert_cli import main as cli_main # noqa: E402 +from relay_d.acquisition.qt_app.ui.config.builder.config_check import ERROR, check_config # noqa: E402 +from relay_d.acquisition.qt_app.ui.postprocess import lerobot_writer # noqa: E402 +from relay_d.acquisition.qt_app.ui.postprocess.postprocess_config_loader import PostprocessConfigLoader # noqa: E402 +from relay_d.acquisition.qt_app.ui.postprocess.robomimic_config_handler import classify_observations # noqa: E402 +from relay_d.acquisition.qt_app.ui.postprocess.yaml_driven_converter import YAMLDrivenConverter # noqa: E402 + +from helpers import DATA_CONFIG, LENGTHS, write_recording # noqa: E402 + + +def _cfg(): + return json.loads(json.dumps(DATA_CONFIG)) + + +def _converter(cfg=None): + return YAMLDrivenConverter(PostprocessConfigLoader(), data_config=cfg or _cfg()) + + +def _robomimic(tmp_path, conv, rec): + out = tmp_path / "out.h5" + ok = conv.convert_multiple_files("robomimic", [str(rec)], str(out)) + return ok, out + + +def test_demo_missing_the_action_stream_is_skipped_not_given_zero_actions(tmp_path): + rec = tmp_path / "rec.h5" + write_recording(rec, LENGTHS) + with h5py.File(rec, "a") as f: + del f["data/demo_1/streams/robot_joint_pos"] + conv = _converter() + ok, out = _robomimic(tmp_path, conv, rec) + assert ok + with h5py.File(out, "r") as f: + assert f["data"].attrs["total"] == 1 + assert f["data/demo_0/actions"].shape[0] == LENGTHS[0] + assert len(conv.skipped_demos) == 1 and conv.converted_demos == 1 + assert "Converted 1 of 2 demos (1 skipped" in conv.conversion_summary() + + +def test_demo_missing_a_camera_is_skipped_not_given_black_frames(tmp_path): + rec = tmp_path / "rec.h5" + write_recording(rec, LENGTHS) + with h5py.File(rec, "a") as f: + del f["data/demo_1/camera_rgb"] + ok, out = _robomimic(tmp_path, _converter(), rec) + assert ok + with h5py.File(out, "r") as f: + assert f["data"].attrs["total"] == 1 + assert f["data/demo_0/obs/agentview_image"][:].any() + + +@pytest.mark.parametrize("actions", [[], None]) +def test_no_actions_to_extract_fails_instead_of_zero_actions(tmp_path, actions): + cfg = _cfg() + if actions is None: + del cfg["actions_to_extract"] + else: + cfg["actions_to_extract"] = actions + rec = tmp_path / "rec.h5" + write_recording(rec, LENGTHS) + conv = _converter(cfg) + ok, out = _robomimic(tmp_path, conv, rec) + assert not ok and not out.exists() + assert "actions_to_extract" in conv.last_error + assert any(p.level == ERROR and "actions_to_extract" in p.message for p in check_config({"config": cfg})) + + +@pytest.mark.parametrize("fmt", ["robomimic", "lerobot"]) +def test_stream_with_normalize_fails_the_conversion(tmp_path, fmt): + cfg = _cfg() + cfg["output"]["streams"]["robot_joint_pos"] = {"specs": ["joint_1.position[:]"], "normalize": True} + rec = tmp_path / "rec.h5" + write_recording(rec, LENGTHS) + conv = _converter(cfg) + out = tmp_path / ("out.h5" if fmt == "robomimic" else "lerobot_out") + assert not conv.convert_multiple_files(fmt, [str(rec)], str(out)) + assert not out.exists() + assert "Relay-D no longer normalizes" in conv.last_error and "'robot_joint_pos'" in conv.last_error + + +def _depth_config(): + cfg = _cfg() + cfg["input"]["camera_depth"] = {"topic": "/depth", "modality": "depth"} + cfg["output"]["agentview_depth"] = "camera_depth/images" + return cfg + + +def test_modality_is_looked_up_through_the_output_key(tmp_path): + rec = tmp_path / "rec.h5" + write_recording(rec, LENGTHS) + with h5py.File(rec, "a") as f: + for i, n in enumerate(LENGTHS): + f[f"data/demo_{i}"].create_group("camera_depth").create_dataset( + "images", data=np.ones((n, 6, 8), np.float32)) + cfg = _depth_config() + conv = _converter(cfg) + ok, out = _robomimic(tmp_path, conv, rec) + assert ok + classes = classify_observations(cfg, conv._shape_cache) + assert classes["depth"] == ["agentview_depth"] and "agentview_depth" not in classes["low_dim"] + with h5py.File(out, "r") as f: + assert f["data/demo_0/obs/agentview_depth"].chunks == (1, 6, 8) + + +def test_robomimic_output_has_only_what_training_reads(tmp_path): + rec = tmp_path / "rec.h5" + write_recording(rec, LENGTHS) + ok, out = _robomimic(tmp_path, _converter(), rec) + assert ok + with h5py.File(out, "r") as f: + assert set(f["data/demo_0"].keys()) == {"actions", "obs", "rewards", "dones"} # no next_obs / states + # robomimic copies env_args into the checkpoint (env_metadata): inference reads its rate from control_freq + assert json.loads(f["data"].attrs["env_args"]) == {"env_name": "relay_d", "type": 1, + "env_kwargs": {"control_freq": float(DATA_CONFIG["recording_frequency"])}} + assert set(f["mask"].keys()) <= {"train", "valid"} + + +@pytest.mark.parametrize("fmt", ["robomimic", "lerobot"]) +def test_demo_without_streams_is_skipped(tmp_path, fmt): + rec = tmp_path / "rec.h5" + write_recording(rec, LENGTHS) + with h5py.File(rec, "a") as f: + del f["data/demo_1/streams"] + conv = _converter() + out = tmp_path / ("out.h5" if fmt == "robomimic" else "lerobot_out") + assert conv.convert_multiple_files(fmt, [str(rec)], str(out)) + assert conv.converted_demos == 1 and conv.skipped_demos == [f"{rec}::demo_1"] + + +def test_stream_shorter_than_the_demo_fails_the_demo(tmp_path): + rec = tmp_path / "rec.h5" + write_recording(rec, LENGTHS) + with h5py.File(rec, "a") as f: + short = f["data/demo_1/streams/eef_translation"][:-2] + del f["data/demo_1/streams/eef_translation"] + f["data/demo_1/streams"].create_dataset("eef_translation", data=short) + conv = _converter() + ok, out = _robomimic(tmp_path, conv, rec) + assert ok and conv.converted_demos == 1 and len(conv.skipped_demos) == 1 + + +def test_skipped_demos_are_counted_for_lerobot_and_shown_by_the_cli(tmp_path, capsys): + import yaml + + rec = tmp_path / "rec.h5" + write_recording(rec, LENGTHS) + with h5py.File(rec, "a") as f: + del f["data/demo_1/streams/robot_joint_pos"] + cfg_path = tmp_path / "acq.yaml" + cfg_path.write_text(yaml.safe_dump({"config": _cfg()})) + assert cli_main(["--config", str(cfg_path), "--format", "lerobot", "--input", str(rec), + "--output", str(tmp_path / "ds")]) == 0 + assert "Converted 1 of 2 demos (1 skipped, see log)" in capsys.readouterr().out + + +def _two_camera_recording(path, lengths, seed): + write_recording(path, lengths, seed=seed) + with h5py.File(path, "a") as f: + for i, n in enumerate(lengths): + f[f"data/demo_{i}"].create_group("camera_wrist").create_dataset( + "images", data=np.full((n, 24, 32, 3), 9, np.uint8)) + + +def test_video_error_mid_episode_aborts_the_whole_lerobot_dataset(tmp_path, monkeypatch): + cfg = _cfg() + cfg["input"]["camera_wrist"] = {"topic": "/wrist", "modality": "rgb"} + cfg["output"]["wrist_image"] = "camera_wrist/images" + a, b = tmp_path / "rec_0.h5", tmp_path / "rec_1.h5" + _two_camera_recording(a, [6], 0) + _two_camera_recording(b, [5], 1) + + real = lerobot_writer.LeRobotDatasetWriter._write_video + calls = [] + + def flaky(self, key, frames): + calls.append(key) + if len(calls) == 2: # the 2nd camera of the 1st episode, after its rows were written + raise OSError("encoder died") + return real(self, key, frames) + + monkeypatch.setattr(lerobot_writer.LeRobotDatasetWriter, "_write_video", flaky) + conv = _converter(cfg) + out = tmp_path / "ds" + assert not conv.convert_multiple_files("lerobot", [str(a), str(b)], str(out)) + assert "encoder died" in conv.last_error + assert not out.exists() + + +def test_writer_cannot_finalize_after_a_failed_episode(tmp_path, monkeypatch): + def broken(self, key, frames): + raise OSError("disk full") + + monkeypatch.setattr(lerobot_writer.LeRobotDatasetWriter, "_write_video", broken) + writer = lerobot_writer.LeRobotDatasetWriter(tmp_path / "ds", fps=10) + frames = np.zeros((4, 8, 8, 3), np.uint8) + with pytest.raises(lerobot_writer.LeRobotFatalError): + writer.add_episode(np.zeros((4, 2)), np.zeros((4, 1)), {"observation.images.cam": frames}, task="t") + with pytest.raises(lerobot_writer.LeRobotWriterError): + writer.finalize() + assert not (tmp_path / "ds").exists() + + +@pytest.mark.parametrize("fmt", ["robomimic", "lerobot"]) +def test_demo_recorded_off_rate_is_skipped(tmp_path, fmt): + from helpers import FPS + + rec = tmp_path / "rec.h5" + write_recording(rec, LENGTHS) + with h5py.File(rec, "a") as f: # demo_1 recorded at 70% of recording_frequency: a PC that could not keep up + for name, rate in (("demo_0", FPS), ("demo_1", 0.7 * FPS)): + f[f"data/{name}"].create_dataset("timestamps", data=(np.arange(f[f"data/{name}/streams/robot_joint_pos"].shape[0]) * 1e9 / rate).astype(np.int64)) + conv = _converter() + out = tmp_path / ("out.h5" if fmt == "robomimic" else "lerobot_out") + assert conv.convert_multiple_files(fmt, [str(rec)], str(out)) + assert conv.converted_demos == 1 and conv.skipped_demos == [f"{rec}::demo_1"] diff --git a/tests/test_conversion_paths.py b/tests/test_conversion_paths.py new file mode 100644 index 0000000..c0cc433 --- /dev/null +++ b/tests/test_conversion_paths.py @@ -0,0 +1,198 @@ +"""The GUI Post-process page, AppAPI and relay-d-convert write the same dataset and the same files next to it.""" + +import os +from datetime import datetime +from types import SimpleNamespace + +import h5py +import pytest +import yaml + +pytest.importorskip("av") + +from relay_d.acquisition.convert_cli import main as cli_main # noqa: E402 +from relay_d.acquisition.qt_app.ui.postprocess import offline_config # noqa: E402 +from relay_d.acquisition.qt_app.ui.postprocess.postprocess_config_loader import PostprocessConfigLoader # noqa: E402 +from relay_d.acquisition.qt_app.ui.postprocess.yaml_driven_converter import YAMLDrivenConverter # noqa: E402 +from helpers import DATA_CONFIG, LENGTHS, write_recording # noqa: E402 + +SIDECARS = ["acquisition_config.yaml", "action_dispatcher/action_config.yaml", + "action_dispatcher/inference_script.py", "action_dispatcher/obs_config.yaml"] + + +@pytest.fixture +def workspace(tmp_path): + rec_dir = tmp_path / "recordings" / "2026-11-03_10-00-00_Recording" + rec_dir.mkdir(parents=True) + write_recording(rec_dir / "stack_cubes_1.h5", LENGTHS) + cfg = tmp_path / "configs" / "acquisition.yaml" + cfg.parent.mkdir() + cfg.write_text("# my comment\n" + yaml.safe_dump({"config": DATA_CONFIG})) + return tmp_path, cfg, rec_dir + + +def _files(folder): + return sorted(os.path.relpath(os.path.join(r, f), folder) for r, _, fs in os.walk(folder) for f in fs) + + +# --- B1: one writer for the files next to a dataset +@pytest.mark.parametrize("fmt", ["robomimic", "lerobot"]) +def test_cli_writes_the_dispatcher_files_where_the_launcher_looks(workspace, fmt): + tmp, cfg, rec_dir = workspace + out = tmp / "out" / ("ds.h5" if fmt == "robomimic" else "ds") + assert cli_main(["--config", str(cfg), "--format", fmt, "--input", str(rec_dir), "--output", str(out)]) == 0 + sidecars = [f for f in _files(tmp / "out") if not f.startswith("ds")] + assert sidecars == SIDECARS # next to the .h5 / in the folder above the LeRobot folder (launcher _config_file) + assert (tmp / "out" / "acquisition_config.yaml").read_bytes() == cfg.read_bytes() # byte copy, comments kept + + +def test_cli_keeps_the_dataset_when_the_extra_files_fail(workspace, monkeypatch, capsys): + tmp, cfg, rec_dir = workspace + + def boom(folder): + raise OSError("disk full") + + monkeypatch.setattr(offline_config, "save_dispatch_templates", boom) + out = tmp / "out" / "ds.h5" + assert cli_main(["--config", str(cfg), "--format", "robomimic", "--input", str(rec_dir), "--output", str(out)]) == 0 + assert out.exists() + err = capsys.readouterr().err + assert "disk full" in err and "Traceback" not in err + + +def test_acquisition_config_is_dumped_when_the_parser_was_edited_after_loading(workspace, tmp_path): + _, cfg, _ = workspace + parser, _ = offline_config.load_offline_config(str(cfg)) + parser.yaml_data["config"]["dataset_name"] = "edited" + offline_config.write_dataset_files(str(tmp_path / "o"), parser) + assert yaml.safe_load((tmp_path / "o" / "acquisition_config.yaml").read_text())["config"]["dataset_name"] == "edited" + + +# --- B2: AppAPI converts like the GUI +def _api(cfg, rec_dir, options=None): + pytest.importorskip("rclpy") + from relay_d.acquisition.api.api import AppAPI + + api = AppAPI.__new__(AppAPI) # no ROS: only the conversion part + api._load_conversion_config(str(cfg), options) + api._session_h5_files = [str(p) for p in sorted(rec_dir.glob("*.h5"))] + api._data_recorder = SimpleNamespace(recorded_data_dir=str(rec_dir)) + return api + + +@pytest.mark.parametrize("fmt", ["robomimic", "lerobot"]) +def test_api_writes_a_new_dataset_folder_with_the_dispatcher_files(workspace, fmt, monkeypatch): + tmp, cfg, rec_dir = workspace + api = _api(cfg, rec_dir) + from relay_d.acquisition.api import api as api_mod + + now = datetime(2026, 11, 3, 11, 0, 0) + monkeypatch.setattr(api_mod, "datetime", SimpleNamespace(now=lambda: now)) + assert api.convert_recording_to_dataset(fmt) + out_dir = tmp / "recordings" / now.strftime("%Y-%m-%d_%H-%M-%S" + ("_Robomimic" if fmt == "robomimic" else "_Lerobot")) + assert [f for f in _files(out_dir) if not f.startswith("dataset_")] == SIDECARS + assert _files(rec_dir) == ["stack_cubes_1.h5"] # nothing written into the recording session + + +def test_api_refuses_a_bad_options_file(workspace): + tmp, cfg, rec_dir = workspace + bad = tmp / "opts.yaml" + bad.write_text("postprocess:\n robomimic:\n training_model: bc\n") + with pytest.raises(ValueError, match="training_model"): + _api(cfg, rec_dir, str(bad)) + + +def test_api_shutdown_converts_to_the_default_format_setting(settings_file): + pytest.importorskip("rclpy") + from relay_d.acquisition.api.api import AppAPI + from relay_d.utils import user_settings + + user_settings.save({**user_settings.values(), "postprocess_default_format": "lerobot"}) + api = AppAPI.__new__(AppAPI) + seen = [] + api._data_recorder = SimpleNamespace(is_recording=False) + api.convert_recording_to_dataset = lambda *a: seen.append(a) + api._rclpy_initialized = False + api._topic_subscribers = SimpleNamespace(close=lambda: None) + api.shutdown() + assert seen == [("lerobot",)] + + +# --- B3: GUI options follow the loaded config +@pytest.fixture +def page(monkeypatch, qapp): + pytest.importorskip("PyQt5") + from PyQt5.QtWidgets import QMainWindow + import relay_d.acquisition.qt_app as qt_app + + monkeypatch.syspath_prepend(os.path.dirname(qt_app.__file__)) + from relay_d.acquisition.qt_app.ui.config.yaml_parser import YamlParser + from relay_d.acquisition.qt_app.ui.containers.mainWindow_ui import Ui_MainWindow + from relay_d.acquisition.qt_app.ui.postprocess import postprocess_page as pp + + monkeypatch.setattr(pp.CustomMessageBox, "warning", lambda *a, **k: None) + window = QMainWindow() + ui = Ui_MainWindow() + ui.setupUi(window) + p = pp.PostProcessPage(ui, YamlParser()) + p._window = window + return p + + +def test_gui_options_reset_on_each_config_load(workspace, page): + tmp, cfg, _ = workspace + picked = tmp / "picked.yaml" + picked.write_text("postprocess:\n lerobot:\n task: picked\n") + assert page.load_postprocess_options_from_path(str(picked)) + assert page.load_postprocess_config_from_path(str(cfg)) # no postprocess_config.yaml next to it + assert page.yaml_converter.options.source is None # defaults, not the earlier file + sibling = cfg.parent / "postprocess_config.yaml" + sibling.write_text("postprocess:\n lerobot:\n task: sibling\n") + assert page.load_postprocess_options_from_path(str(picked)) + assert page.load_postprocess_config_from_path(str(cfg)) + assert page.yaml_converter.options.source == str(sibling) + + +def test_gui_refuses_to_convert_with_a_broken_options_file_next_to_the_config(workspace, page, monkeypatch): + # as relay-d-convert and AppAPI: a broken postprocess_config.yaml is not silently replaced by the defaults + tmp, cfg, rec_dir = workspace + (cfg.parent / "postprocess_config.yaml").write_text("postprocess:\n robomimic:\n training_model: bc\n") + errors = [] + from relay_d.acquisition.qt_app.ui.postprocess import postprocess_page as pp + + monkeypatch.setattr(pp.CustomMessageBox, "error", lambda msg, *a, **k: errors.append(msg)) + page.load_postprocess_config_from_path(str(cfg)) + page.output_format, page.h5_files = "robomimic", [str(rec_dir / "stack_cubes_1.h5")] + converted = [] + monkeypatch.setattr(page.yaml_converter, "convert_multiple_files", lambda *a, **k: converted.append(a)) + page.convert_dataset() + assert converted == [] and errors and "training_model" in errors[0] + + +# --- B4: inputs +def _converter(): + import json + + return YAMLDrivenConverter(PostprocessConfigLoader(), data_config=json.loads(json.dumps(DATA_CONFIG))) + + +def test_converted_robomimic_files_in_the_inputs_are_skipped(tmp_path): + rec = tmp_path / "rec.h5" + write_recording(rec, LENGTHS) + conv = _converter() + first = tmp_path / "first.h5" + assert conv.convert_multiple_files("robomimic", [str(rec)], str(first)) + assert conv.convert_multiple_files("robomimic", [str(rec), str(first)], str(tmp_path / "second.h5")) + with h5py.File(tmp_path / "second.h5", "r") as f: + assert f["data"].attrs["total"] == len(LENGTHS) + assert conv.conversion_summary() == f"Converted {len(LENGTHS)} of {len(LENGTHS)} demos" # not "failed demos" + + +def test_same_named_files_of_several_sessions_sort_by_folder(tmp_path): + for session, n in (("s1", 5), ("s2", 7)): + (tmp_path / session).mkdir() + write_recording(tmp_path / session / "rec.h5", [n]) + out = tmp_path / "out.h5" + assert _converter().convert_multiple_files("robomimic", [str(tmp_path / "s2/rec.h5"), str(tmp_path / "s1/rec.h5")], str(out)) + with h5py.File(out, "r") as f: + assert f["data/demo_0/actions"].shape[0] == 5 diff --git a/tests/test_convert_cli.py b/tests/test_convert_cli.py index 576f50a..d9a886c 100644 --- a/tests/test_convert_cli.py +++ b/tests/test_convert_cli.py @@ -1,7 +1,6 @@ """Headless converter (relay_d.acquisition.convert_cli).""" import json -import subprocess import sys import h5py @@ -43,22 +42,22 @@ def test_lerobot_conversion(workspace, capsys): info = json.loads((out / "meta/info.json").read_text()) assert info["total_episodes"] == 1 + len(LENGTHS) assert pq.read_table(out / "meta/tasks.parquet")["task"].to_pylist() == ["stack the cubes"] - assert (out / "acquisition_config.yaml").exists() + assert (tmp / "acquisition_config.yaml").exists() # above the dataset folder, where the launcher looks def test_robomimic_conversion_with_sibling_options(workspace): tmp, cfg = workspace (tmp / "postprocess_config.yaml").write_text(yaml.safe_dump({ - "postprocess": {"robomimic": {"metadata": {"env_name": "CliEnv", "env_version": "1", "type": 1, "env_kwargs": {}}}} + "postprocess": {"robomimic": {"splits": {"train_frac": 1.0, "val_frac": 0.0}}} })) out = tmp / "out" / "dataset.h5" assert main(["--config", str(cfg), "--format", "robomimic", "--input", str(tmp / "recordings"), "--output", str(out)]) == 0 with h5py.File(out, "r") as f: assert f["data"].attrs["total"] == 1 + len(LENGTHS) - assert json.loads(f["data"].attrs["env_args"])["env_name"] == "CliEnv" - assert (tmp / "out" / "dataset_acquisition_config.yaml").exists() - assert (tmp / "out" / "dataset_postprocess_config.yaml").exists() + assert list(f["mask"].keys()) == ["train"] # the options file was used + assert (tmp / "out" / "acquisition_config.yaml").exists() + assert (tmp / "out" / "postprocess_config.yaml").exists() def test_usage_errors_exit_2(workspace, tmp_path, capsys): @@ -90,15 +89,6 @@ def test_failed_conversion_exits_1_with_reason(workspace, monkeypatch, capsys): assert "av" in capsys.readouterr().err -def test_script_wrapper_help_runs_without_ros_or_qt(tmp_path): - root = __import__("os").path.dirname(__import__("os").path.dirname(__file__)) - result = subprocess.run( - [sys.executable, "scripts/convert_dataset.py", "--help"], - cwd=root, env={"PYTHONPATH": root, "PATH": ""}, capture_output=True, text=True, - ) - assert result.returncode == 0 and "--format" in result.stdout - - def test_robomimic_refuses_to_overwrite(workspace): tmp, cfg = workspace out = tmp / "out" / "dataset.h5" @@ -108,3 +98,11 @@ def test_robomimic_refuses_to_overwrite(workspace): before = out.read_bytes() assert main(args) == 2 assert out.read_bytes() == before + + +def test_removed_option_exits_2(workspace, capsys): + tmp, cfg = workspace + (tmp / "postprocess_config.yaml").write_text("postprocess:\n robomimic:\n training_model: bc_rnn_gmm\n") + assert main(["--config", str(cfg), "--format", "robomimic", "--input", str(tmp / "recordings"), + "--output", str(tmp / "out" / "d.h5")]) == 2 + assert "robomimic.training_model" in capsys.readouterr().err diff --git a/tests/test_dispatch_templates.py b/tests/test_dispatch_templates.py index 1d4cef0..d94eca3 100644 --- a/tests/test_dispatch_templates.py +++ b/tests/test_dispatch_templates.py @@ -1,6 +1,7 @@ """The dispatch config templates users copy: they must parse, and their live entries must stay as documented.""" from importlib import resources +from pathlib import Path import yaml @@ -9,10 +10,10 @@ def _load(name): return yaml.safe_load(resources.files("relay_d.dispatch").joinpath(f"config_templates/{name}").read_text()) -def test_action_template_keeps_its_three_live_entries(): +def test_action_template_has_one_live_arm_entry(): actions = _load("action_config_template.yaml")["actions"] - assert list(actions) == ["move_eef", "move_arm_joint", "robotiq_gripper"] - assert actions["move_eef"]["goal_init"]["transforms"] == 1 # array pre-allocation, see the goal_init notes + # move_eef is only an example: live next to move_arm_joint (same topic, same indices) it moved the arm twice per tick + assert list(actions) == ["move_arm_joint", "robotiq_gripper"] assert actions["move_arm_joint"]["mapping"] == {"joint_trajectory[0].positions": [0, 1, 2, 3, 4, 5, 6]} assert actions["robotiq_gripper"]["action"] is True and actions["robotiq_gripper"]["mapping"] == {"position": 0} @@ -28,8 +29,12 @@ def test_live_template_mappings_point_inside_their_slice(): def test_obs_template_parses_and_every_documented_dispatch_key_is_known(): assert "config" in _load("obs_config_template.yaml") text = resources.files("relay_d.dispatch").joinpath("config_templates/action_config_template.yaml").read_text() - for key in ("indices", "denormalize", "normalized_range", "limits", "transform", "goal_init", "feedback", "mapping", "stop"): + for key in ("indices", "limits", "transform", "goal_init", "feedback", "mapping", "stop"): assert key in text # the quick reference still documents every entry key + for key in ("denormalize", "normalized_range", "apply_limits", "dedupe", "command:", "{I[", "vacuum", "magnetic", + "type: scale", "type: clip", "after denorm"): + assert key not in text # removed: the dispatcher refuses them + assert "static:" not in resources.files("relay_d.dispatch").joinpath("config_templates/obs_config_template.yaml").read_text() def test_inference_script_runs_the_cli_with_the_local_configs(monkeypatch): @@ -44,4 +49,29 @@ def test_inference_script_runs_the_cli_with_the_local_configs(monkeypatch): monkeypatch.setattr(sys, "argv", ["inference_script.py", "--checkpoint", "m.pth", "--obs-config=o.yaml"]) path = resources.files("relay_d.dispatch").joinpath("templates/inference_script_template.py") runpy.run_path(str(path), run_name="__main__") - assert seen == [["--checkpoint", "m.pth", "--obs-config=o.yaml", "--action-config", "./action_config.yaml"]] + # the files next to the script, not the current folder + assert seen == [["--checkpoint", "m.pth", "--obs-config=o.yaml", "--action-config", str(Path(str(path)).with_name("action_config.yaml"))]] + + +def test_cli_dry_run_flag_reaches_the_session(monkeypatch): + import sys + from types import SimpleNamespace + + from relay_d.dispatch import inference_node + + seen = [] + + def fake_session(**kwargs): + seen.append(kwargs["dry_run"]) + return SimpleNamespace(model=SimpleNamespace(fps=30.0), run=lambda **k: seen.append(k["hz"]) or True, close=lambda: None) + + monkeypatch.setattr(inference_node, "InferenceSession", fake_session) + monkeypatch.setattr(inference_node, "reexec_if_needed", lambda *a: None) + monkeypatch.setattr(inference_node, "sigterm_runs_the_safe_stop", lambda: None) # keep this process's SIGTERM + base = ["lfd-inference-node", "--checkpoint", "m.pth", "--obs-config", "o.yaml", "--action-config", "a.yaml", + "--backend", "robomimic"] + for extra, expected in (([], False), (["--dry-run"], True)): # the Run tab passes exactly --dry-run + seen.clear() + monkeypatch.setattr(sys, "argv", base + extra) + inference_node.main() + assert seen == [expected, None] # and no rate: run() takes the policy's own diff --git a/tests/test_format_buttons.py b/tests/test_format_buttons.py index 234e69c..ee16f66 100644 --- a/tests/test_format_buttons.py +++ b/tests/test_format_buttons.py @@ -48,11 +48,7 @@ def test_only_one_format_button_is_checked_at_a_time(page): assert _state(p, ui) == ("robomimic", True, False) -def test_preview_follows_the_selected_format(page): - p, ui, _ = page - ui.btn_format_lerobot.click() - assert "lerobot" in ui.txt_output_preview.toPlainText().lower() - ui.btn_format_robomimic.click() - assert "robomimic" in ui.txt_output_preview.toPlainText().lower() - ui.btn_format_robomimic.click() # deselect clears the preview - assert ui.txt_output_preview.toPlainText() == "" +def test_output_preview_is_gone(page): + _, ui, _ = page + assert not hasattr(ui, "outputPreviewWidget") + assert not hasattr(ui, "txt_input_structure") # replaced by the pre-check table diff --git a/tests/test_inference_session.py b/tests/test_inference_session.py index 5ec57a4..b0cc8f2 100644 --- a/tests/test_inference_session.py +++ b/tests/test_inference_session.py @@ -13,7 +13,8 @@ def test_policy_is_reset_when_inputs_come_back_after_going_stale(): s = InferenceSession.__new__(InferenceSession) # skip ROS setup ages = {"cam": 5.0} resets = [] - s.obs_node = SimpleNamespace(get_input_ages=lambda: dict(ages), get_obs_ages=lambda: {}) + s.obs_node = SimpleNamespace(get_input_ages=lambda: dict(ages), get_obs_ages=lambda: {}, + get_logger=lambda: SimpleNamespace(error=lambda *a, **k: None)) s.model = SimpleNamespace(obs_keys=[], reset=lambda: resets.append(1)) s._was_stale = False s.get_observation = lambda: {} @@ -27,6 +28,39 @@ def test_policy_is_reset_when_inputs_come_back_after_going_stale(): assert resets == [1] # once, on the first fresh tick: no actions queued before the gap +def test_step_sends_the_stop_when_predict_raises(): + s = InferenceSession.__new__(InferenceSession) + stops = [] + s.dispatcher = SimpleNamespace(send_stop=lambda: stops.append(1)) + s.obs_node = SimpleNamespace(get_input_ages=lambda: {}, get_obs_ages=lambda: {}) + s.model = SimpleNamespace(obs_keys=[], reset=lambda: None) + s._was_stale = False + s.get_observation = lambda: {} + s.predict = lambda obs: (_ for _ in ()).throw(RuntimeError("model broke")) + with pytest.raises(RuntimeError, match="model broke"): + s.step(max_obs_age=1.0) + assert stops == [1] + + +def test_run_lets_a_dispatch_error_through(monkeypatch): + s = InferenceSession.__new__(InferenceSession) + stops = [] + s.dispatcher = SimpleNamespace(send_stop=lambda: stops.append(1)) + s.obs_node = SimpleNamespace(get_input_ages=lambda: {}, get_obs_ages=lambda: {}, + get_logger=lambda: SimpleNamespace(info=print, error=print)) + s.model = SimpleNamespace(obs_keys=[], reset=lambda: None) + s._was_stale = False + s.start = lambda: None + s.validate = lambda timeout: (True, {"inputs": {}, "obs_keys_ready": True}) + s.get_observation = lambda: {} + s.predict = lambda obs: np.zeros(1) + s.dispatch = lambda a: (_ for _ in ()).throw(RuntimeError("ack timeout")) # feedback on_timeout: raise + monkeypatch.setattr("relay_d.dispatch.api.rclpy.ok", lambda: True) + with pytest.raises(RuntimeError, match="ack timeout"): # only model errors end run() quietly + s.run(validate_timeout=0.1, max_obs_age=1.0) + assert stops + + def test_joint_view_keeps_configured_order_and_skips_partial_arrays(): from relay_d.dispatch.obs_builder import _JointStateView @@ -57,7 +91,8 @@ def test_stale_inputs_send_the_stop_and_run_always_ends_with_one(monkeypatch): s = InferenceSession.__new__(InferenceSession) stops = [] s.dispatcher = SimpleNamespace(send_stop=lambda: stops.append(1)) - s.obs_node = SimpleNamespace(get_input_ages=lambda: {"cam": 5.0}, get_obs_ages=lambda: {}) + s.obs_node = SimpleNamespace(get_input_ages=lambda: {"cam": 5.0}, get_obs_ages=lambda: {}, + get_logger=lambda: SimpleNamespace(error=lambda *a, **k: None)) s.model = SimpleNamespace(obs_keys=[], reset=lambda: None) s._was_stale = False s.get_observation = lambda: {} @@ -72,8 +107,8 @@ def test_stale_inputs_send_the_stop_and_run_always_ends_with_one(monkeypatch): predicted = [] s.predict = lambda obs: predicted.append(1) or (_ for _ in ()).throw(RuntimeError("model broke")) monkeypatch.setattr("relay_d.dispatch.api.rclpy.ok", lambda: True) # no rclpy.init() here - s.run(validate_timeout=0.1, max_obs_age=1.0) - assert predicted == [1] and stops == [1] # the loop broke on the model error: the robot is told to stop + assert s.run(validate_timeout=0.1, max_obs_age=1.0) is False # the node exits 1: the Run tab shows Failed + assert predicted == [1] and stops # (the real send_stop sends once) the loop broke on the model error: the robot is told to stop @pytest.mark.parametrize("hz, warned", [(10.0, "3x slower"), (30.0, None), (60.0, "2x faster"), (0.0, "as fast as")]) @@ -95,3 +130,150 @@ def test_run_warns_when_the_rate_differs_from_the_policy(monkeypatch, hz, warned assert rate_warnings == [] else: assert len(rate_warnings) == 1 and warned in rate_warnings[0] + + +@pytest.mark.parametrize("key", [{"normalize": True}, {"normalized_range": [-1.0, 1.0]}, + {"limits": [{"lower_lim": 0.0, "upper_lim": 1.0}]}]) +def test_obs_normalization_keys_are_rejected_at_startup(key): + from relay_d.dispatch.obs_builder import ObsBuilder + + ob = ObsBuilder.__new__(ObsBuilder) # skip ROS setup + ob._obs_cfg, ob._input_cfg = {"q": {"specs": ["js.position[:]"], **key}}, {"js": {"topic": "/js"}} + ob._tf_inputs, ob._static_inputs = set(), set() + ob._extractors, ob._rotation_format, ob._obs_input_names, ob._passthrough_obs = {}, {}, {}, {} + with pytest.raises(ValueError, match="no longer normalizes"): # robomimic/LeRobot normalize with the checkpoint's stats + ob._build_extractors() + + +def test_static_obs_input_is_rejected_at_startup(tmp_path): + import yaml + + from relay_d.dispatch.obs_builder import ObsBuilder + + path = tmp_path / "obs_config.yaml" + path.write_text(yaml.safe_dump({"config": {"input": {"grip": {"static": [1.0]}}, "observations": {"g": ["grip.data[:]"]}}})) + with pytest.raises(ValueError, match="'grip'.*static.*removed"): # the recorder never wrote one: no model was trained on it + ObsBuilder(str(path)) # raises before the node is created: no rclpy.init needed + + +def _layout_session(action_dim, actions): + s = InferenceSession.__new__(InferenceSession) # skip ROS setup + s.model = SimpleNamespace(action_dim=action_dim) + s.dispatcher = SimpleNamespace(_action_configs=actions) + return s + + +def test_action_index_past_the_model_output_stops_startup(): + s = _layout_session(7, {"arm": {"indices": [0, 1, 2, 3, 4, 5]}, "grip": {"indices": [7]}}) + with pytest.raises(ValueError, match="'grip'.*7.*7 values"): # would drive nothing (or the wrong joint) silently + s._check_action_layout() + + +def test_action_columns_no_entry_sends_are_warned_about(monkeypatch): + import relay_d.dispatch.api as api + + warnings = [] + monkeypatch.setattr(api.logger, "warning", warnings.append) + _layout_session(7, {"arm": {"indices": [0, 1, 2, 3, 4, 5]}})._check_action_layout() + assert len(warnings) == 1 and "[6]" in warnings[0] + warnings.clear() + _layout_session(None, {"arm": {"indices": [99]}})._check_action_layout() # the runner cannot tell: no check + _layout_session(2, {"arm": {"indices": [0, 1]}})._check_action_layout() + assert warnings == [] + + +def _rate_session(monkeypatch, fps, infer_hz=7.0): + import relay_d.dispatch.api as api + + s = InferenceSession.__new__(InferenceSession) + infos = [] + monkeypatch.setattr(api.rclpy, "ok", lambda: False) # stop before the first tick + real_get = api.user_settings.get + monkeypatch.setattr(api.user_settings, "get", lambda k: infer_hz if k == "infer_hz" else real_get(k)) + s.model = SimpleNamespace(fps=fps, obs_keys=[], reset=lambda: None) + s.dispatcher = SimpleNamespace(send_stop=lambda: None) + s.obs_node = SimpleNamespace(get_logger=lambda: SimpleNamespace(info=infos.append, error=lambda *a, **k: None)) + s.start = lambda: None + s.validate = lambda timeout: (True, {"inputs": {}, "obs_keys_ready": True}) + return s, infos + + +@pytest.mark.parametrize("fps, hz, expected", [(30, None, "30 Hz"), (None, None, "7.0 Hz"), (30, 0.0, "unthrottled"), + (30, 10.0, "10.0 Hz")]) +def test_run_defaults_to_the_policy_rate_else_the_infer_hz_setting(monkeypatch, fps, hz, expected): + s, infos = _rate_session(monkeypatch, fps) + assert s.run(hz=hz, validate_timeout=0.1, max_obs_age=1.0) is True # ended without a model error + assert any("Starting inference" in m and expected in m for m in infos), infos + + +def test_device_defaults_to_the_infer_device_setting(monkeypatch): + import relay_d.dispatch.api as api + import relay_d.dispatch.lerobot_runner as lr + + devices = [] + real_get = api.user_settings.get + monkeypatch.setattr(api.user_settings, "get", lambda k: "cuda:7" if k == "infer_device" else real_get(k)) + monkeypatch.setattr(api.rclpy, "ok", lambda: True) + monkeypatch.setattr(api, "ObsBuilder", lambda cfg: SimpleNamespace(low_dim_obs_names=[], + get_logger=lambda: SimpleNamespace(info=print))) + monkeypatch.setattr(api, "ActionDispatcher", lambda *a, **k: SimpleNamespace(_action_configs={})) + monkeypatch.setattr(api, "MultiThreadedExecutor", lambda: SimpleNamespace(add_node=lambda n: None)) + monkeypatch.setattr(lr, "LeRobotPolicyRunner", lambda ckpt, device, **k: devices.append(device) or SimpleNamespace()) + InferenceSession("m", "o.yaml", "a.yaml", backend="lerobot") + InferenceSession("m", "o.yaml", "a.yaml", device="cpu", backend="lerobot") + assert devices == ["cuda:7", "cpu"] # the API runs where the CLI and the Run tab run + + +def test_node_passes_its_rate_through_and_exits_1_on_a_model_error(monkeypatch): + import relay_d.dispatch.inference_node as node + + calls = [] + + class FakeSession: + def __init__(self, **kw): + calls.append(("device", kw["device"])) + + def run(self, hz, validate_timeout, max_obs_age): + calls.append(("hz", hz)) + return False + + def close(self): + calls.append("close") + + monkeypatch.setattr(node, "InferenceSession", FakeSession) + monkeypatch.setattr(node, "sigterm_runs_the_safe_stop", lambda: calls.append("sigterm")) + monkeypatch.setattr(node.sys, "argv", ["node", "--checkpoint", "/nope", "--obs-config", "o", "--action-config", "a"]) + with pytest.raises(SystemExit) as e: + node.main() + assert e.value.code == 1 + assert ("hz", None) in calls and "sigterm" in calls and calls[-1] == "close" # run() picks the rate; the stop still runs + + +SIGTERM_PROBE = """ +import time, rclpy +from std_msgs.msg import Float64 +from relay_d.dispatch.inference_node import sigterm_runs_the_safe_stop +rclpy.init() # installs rclpy's own handlers, as InferenceSession does +sigterm_runs_the_safe_stop() +node = rclpy.create_node("sigterm_probe") +pub = node.create_publisher(Float64, "/sigterm_probe_stop", 1) +print("ready", flush=True) +try: + while True: + time.sleep(0.05) +finally: + pub.publish(Float64(data=0.0)) + print("STOP PUBLISHED", flush=True) +""" + + +def test_sigterm_takes_the_safe_stop_path(): + import signal + import subprocess + import sys + + p = subprocess.Popen([sys.executable, "-c", SIGTERM_PROBE], stdout=subprocess.PIPE, stderr=subprocess.STDOUT, text=True) + assert p.stdout.readline().strip() == "ready" + p.send_signal(signal.SIGTERM) + out, _ = p.communicate(timeout=20) + assert "STOP PUBLISHED" in out, out # rclpy's default SIGTERM handling exits without running finally diff --git a/tests/test_lerobot_conversion.py b/tests/test_lerobot_conversion.py index 7b225b8..e22cfc8 100644 --- a/tests/test_lerobot_conversion.py +++ b/tests/test_lerobot_conversion.py @@ -21,9 +21,7 @@ @pytest.fixture def converter(): - loader = PostprocessConfigLoader() - assert "lerobot" in loader.get_all_format_names() - return YAMLDrivenConverter(loader, data_config=json.loads(json.dumps(DATA_CONFIG))) + return YAMLDrivenConverter(PostprocessConfigLoader(), data_config=json.loads(json.dumps(DATA_CONFIG))) def test_convert_to_lerobot(tmp_path, converter): @@ -63,7 +61,9 @@ def test_convert_to_lerobot(tmp_path, converter): def test_config_overrides(tmp_path, converter): rec = tmp_path / "recording_0.h5" write_recording(rec, [6]) - converter.data_config["lerobot"] = {"task": "pick up the red cube", "state_streams": ["eef_translation"]} + from relay_d.acquisition.qt_app.ui.postprocess.postprocess_options import PostprocessOptions + + converter.options = PostprocessOptions({"lerobot": {"task": "pick up the red cube", "state_streams": ["eef_translation"]}}) out = tmp_path / "ds" assert converter.convert_multiple_files("lerobot", [str(rec)], str(out)) info = json.loads((out / "meta/info.json").read_text()) @@ -86,7 +86,7 @@ def test_robomimic_path_still_works(tmp_path, converter): assert converter.convert_multiple_files("robomimic", [str(rec)], str(out)) with h5py.File(out, "r") as f: assert f["data"].attrs["total"] == len(LENGTHS) - assert set(f["data/demo_0"].keys()) >= {"actions", "obs", "next_obs", "rewards", "dones", "states"} + assert set(f["data/demo_0"].keys()) == {"actions", "obs", "rewards", "dones"} assert f["data/demo_0/actions"].shape == (LENGTHS[0], 4) assert "agentview_image" in f["data/demo_0/obs"] @@ -148,16 +148,6 @@ def test_recording_without_images_needs_no_pyav(tmp_path, converter, monkeypatch assert not any(k.startswith("observation.images") for k in info["features"]) -def test_get_train_model_name_default_and_explicit(tmp_path): - from relay_d.acquisition.qt_app.ui.config.yaml_parser import YamlParser - - p = YamlParser() - p.yaml_data = {"config": {}} - assert p.get_train_model_name() == "bc_rnn_gmm" - p.yaml_data = {"config": {"training_model": "diffusion_policy"}} - assert p.get_train_model_name() == "diffusion_policy" - - def test_missing_pyarrow_fails_clearly_without_output(tmp_path, converter, monkeypatch): import sys diff --git a/tests/test_lerobot_reader.py b/tests/test_lerobot_reader.py deleted file mode 100644 index 89f68ee..0000000 --- a/tests/test_lerobot_reader.py +++ /dev/null @@ -1,179 +0,0 @@ -"""LeRobot reader used by the Review page: episodes, curves, camera frames, checks (no Qt, no ROS).""" - -import shutil -import types - -import numpy as np -import pytest - -pytest.importorskip("av") -pytest.importorskip("pyarrow") -pytest.importorskip("cv2") -import pyarrow.parquet as pq # noqa: E402 - -from relay_d.acquisition.qt_app.ui.postprocess.lerobot_writer import LeRobotDatasetWriter # noqa: E402 -from relay_d.acquisition.qt_app.ui.review.demo_validator import CheckStatus # noqa: E402 -from relay_d.acquisition.qt_app.ui.review.lerobot_reader import ( # noqa: E402 - LeRobotDataset, - LeRobotError, - is_lerobot_dataset, -) - -FPS = 10 -LENGTHS = [12, 7, 9] - - -def _frames(n, episode): - """BGR frames (writer default): B=200, G grows with the frame number, R=30.""" - frames = np.zeros((n, 32, 48, 3), np.uint8) - for i in range(n): - frames[i, :, :, 0] = 200 - frames[i, :, :, 1] = 20 + 8 * i + 40 * episode - frames[i, :, :, 2] = 30 - return frames - - -def _make(root, with_camera=True): - with LeRobotDatasetWriter(root, fps=FPS, robot_type="arm") as writer: - for ep, n in enumerate(LENGTHS): - rng = np.random.default_rng(ep) - writer.add_episode( - rng.normal(size=(n, 5)).astype(np.float32), rng.normal(size=(n, 3)).astype(np.float32), - {"observation.images.cam": _frames(n, ep)} if with_camera else {}, - task="pick the cube", state_names=["a", "b", "c", "d", "e"], - ) - return root - - -@pytest.fixture -def ds(tmp_path): - reader = LeRobotDataset(_make(tmp_path / "ds")) - yield reader - reader.close() - - -def test_metadata_and_episodes(ds, tmp_path): - assert is_lerobot_dataset(tmp_path / "ds") and not is_lerobot_dataset(tmp_path) - assert ds.fps == FPS and ds.camera_keys == ["observation.images.cam"] - assert ds.names("observation.state") == ["a", "b", "c", "d", "e"] - assert ds.names("action") == ["action_0", "action_1", "action_2"] # no names in info.json: generic ones - eps = ds.episodes() - assert [e.length for e in eps] == LENGTHS and eps[0].tasks == ["pick the cube"] - assert eps[1].label == "Episode 1 · 7 frames · pick the cube" - assert "3 episodes" in ds.summary() and "arm" in ds.summary() - - -def test_arrays_are_sliced_per_episode(ds): - for ep, n in zip(ds.episodes(), LENGTHS): - a = ds.arrays(ep) - assert a["observation.state"].shape == (n, 5) and a["action"].shape == (n, 3) - assert a["time"].shape == (n,) and a["time"][0] == 0.0 and np.allclose(np.diff(a["time"]), 1 / FPS, atol=1e-4) - rng = np.random.default_rng(1) - assert np.allclose(ds.arrays(ds.episodes()[1])["observation.state"], rng.normal(size=(7, 5)).astype(np.float32)) - - -def test_frames_decode_to_the_right_picture_in_rgb(ds): - cam = ds.camera_keys[0] - eps = ds.episodes() - for ep_index, i in ((1, 3), (0, 11), (2, 0), (1, 4), (1, 5)): # jumps, then sequential reads - frame = ds.frame(eps[ep_index], cam, i) - assert frame.shape == (32, 48, 3) and frame.dtype == np.uint8 - r, g, b = frame[8:24, 12:36].reshape(-1, 3).mean(axis=0) - assert abs(r - 30) < 12 and abs(b - 200) < 12 # RGB order, not BGR - assert abs(g - (20 + 8 * i + 40 * ep_index)) < 14 - with pytest.raises(IndexError): - ds.frame(eps[0], cam, 12) - - -def test_validation_passes_on_a_good_dataset(ds): - result = ds.validate() - assert result.overall_status == CheckStatus.PASS, result.summary_text() - assert result.samples == sum(LENGTHS) and result.duration == pytest.approx(sum(LENGTHS) / FPS) - assert all(ds.episode_check(e) == CheckStatus.PASS for e in ds.episodes()) - - -def test_validation_fails_when_a_video_is_missing(tmp_path): - root = _make(tmp_path / "ds") - shutil.rmtree(root / "videos") - reader = LeRobotDataset(root) - result = reader.validate() - assert result.overall_status == CheckStatus.FAIL and "missing" in result.summary_text() - assert reader.episode_check(reader.episodes()[0]) == CheckStatus.FAIL - - -def test_validation_fails_when_data_rows_are_missing(tmp_path): - root = _make(tmp_path / "ds") - path = root / "data" / "chunk-000" / "file-000.parquet" - table = pq.read_table(path) - pq.write_table(table.slice(0, table.num_rows - 3), path) # the last episode loses 3 rows - reader = LeRobotDataset(root) - result = reader.validate() - assert result.overall_status == CheckStatus.FAIL and "episode 2" in result.summary_text() - - -def test_dataset_without_camera_warns(tmp_path): - reader = LeRobotDataset(_make(tmp_path / "ds", with_camera=False)) - assert reader.camera_keys == [] - result = reader.validate() - assert result.overall_status == CheckStatus.WARN and "no camera" in result.summary_text() - - -def test_not_a_dataset_is_a_clear_error(tmp_path): - with pytest.raises(LeRobotError, match="not a LeRobot dataset"): - LeRobotDataset(tmp_path) - - -# ---------------------------------------------------------------- datasets from other tools / big datasets - - -def test_plain_list_columns_are_read_as_matrices(tmp_path): - """Datasets from other tools store state/action as list, not fixed-size lists.""" - import pyarrow as pa - - from relay_d.acquisition.qt_app.ui.review import lerobot_reader as lr - - plain = pa.chunked_array([pa.array([[1.0, 2.0, 3.0], [4.0, 5.0, 6.0]], type=pa.list_(pa.float32()))]) - out = lr._vector_column(plain) - assert out.shape == (2, 3) and out.dtype == np.float32 and out[1, 2] == 6.0 - assert lr._vector_column(pa.chunked_array([pa.array([1.5, 2.5])])).shape == (2, 1) - ragged = pa.chunked_array([pa.array([[1.0], [2.0, 3.0]], type=pa.list_(pa.float32()))]) - with pytest.raises(LeRobotError, match="different lengths"): - lr._vector_column(ragged) - - -def test_every_row_is_converted_once_not_once_per_episode(tmp_path, monkeypatch): - from relay_d.acquisition.qt_app.ui.review import lerobot_reader as lr - - root = tmp_path / "ds" - with LeRobotDatasetWriter(root, fps=FPS) as writer: - for ep in range(30): - rng = np.random.default_rng(ep) - writer.add_episode(rng.normal(size=(20, 4)).astype(np.float32), rng.normal(size=(20, 2)).astype(np.float32), - {}, task="t") - converted = [] - original = lr._vector_column - monkeypatch.setattr(lr, "_vector_column", lambda col: converted.append(len(col)) or original(col)) - reader = LeRobotDataset(root) - result = reader.validate() - statuses = [reader.episode_check(e) for e in reader.episodes()] # reuse validate()'s result - assert result.overall_status == CheckStatus.WARN and all(s == CheckStatus.PASS for s in statuses) - assert sum(converted) == 30 * 20 * 2 # state + action of every episode, once (was episodes x all rows, twice) - rng = np.random.default_rng(7) # the slice of episode 7 holds exactly what was written for it - state, action = rng.normal(size=(20, 4)).astype(np.float32), rng.normal(size=(20, 2)).astype(np.float32) - got = reader.arrays(reader.episodes()[7]) - assert np.allclose(got["observation.state"], state) and np.allclose(got["action"], action) - - -def test_pyav_fallback_decodes_the_same_frames_and_seeks_both_ways(tmp_path, monkeypatch): - import cv2 - - root = _make(tmp_path / "ds") - monkeypatch.setattr(cv2, "VideoCapture", lambda path: types.SimpleNamespace(isOpened=lambda: False, release=lambda: None)) - reader = LeRobotDataset(root) - cam, eps = reader.camera_keys[0], reader.episodes() - for ep_index, i in ((1, 3), (1, 4), (1, 5), (0, 11), (0, 2), (2, 8)): # forward, sequential, backward, jumps - frame = reader.frame(eps[ep_index], cam, i) - r, g, b = frame[8:24, 12:36].reshape(-1, 3).mean(axis=0) - assert abs(r - 30) < 12 and abs(b - 200) < 12 and abs(g - (20 + 8 * i + 40 * ep_index)) < 14 - reader.close() - assert not reader._av diff --git a/tests/test_lerobot_runner.py b/tests/test_lerobot_runner.py index 68ba60b..a432445 100644 --- a/tests/test_lerobot_runner.py +++ b/tests/test_lerobot_runner.py @@ -136,6 +136,7 @@ def reset(self): cfg = types.SimpleNamespace( type="act", device="cpu", input_features={STATE: feature((3,)), "observation.images.cam": feature((3, 4, 6))}, + output_features={"action": feature((3,))}, ) _lerobot_folder(tmp_path / "pretrained_model") ck = tmp_path / "pretrained_model" @@ -162,6 +163,7 @@ def reset(self): with pytest.raises(FileNotFoundError, match="not a LeRobot checkpoint"): lr.LeRobotPolicyRunner(str(tmp_path / "nowhere")) assert runner.fps is None # no training dataset to read the rate from + assert runner.action_dim == 3 # checked against action_config.yaml indices at startup root = tmp_path / "ds" (root / "meta").mkdir(parents=True) @@ -184,11 +186,11 @@ class TokenizerProcessorStep: # what a language-conditioned policy (SmolVLA) ha # real inputs are the dataset's, fed under the dataset's camera name (the preprocessor renames it to camera1) cfg.input_features = {STATE: feature((6,)), "observation.images.camera1": feature((3, 256, 256))} (root / "meta" / "info.json").write_text(json.dumps({"fps": 15, "features": { - STATE: {"names": ["x", "y.0", "y.1"], "shape": [3]}, + STATE: {"names": ["x", "y.0", "y.1"], "shape": [3]}, "action": {"shape": [2]}, "observation.images.cam": {"dtype": "video", "shape": [4, 6, 3]}}})) (ck / "train_config.json").write_text(json.dumps({"dataset": {"root": str(root)}, "policy": {"pretrained_path": "lerobot/smolvla_base"}})) runner = lr.LeRobotPolicyRunner(str(ck), device="cpu", task="stack the cubes") - assert runner.obs_keys == ["x", "y", "cam"] + assert runner.obs_keys == ["x", "y", "cam"] and runner.action_dim == 2 # the dataset's action, not the base model's runner.get_action({"x": np.array([1.0]), "y": np.array([2.0, 3.0]), "cam": _frame(4, 6)}) assert seen["batch"][STATE].shape == (1, 3) and seen["batch"]["observation.images.cam"].shape == (1, 3, 4, 6) (ck / "train_config.json").write_text(json.dumps({"dataset": {"root": str(tmp_path / "gone")}, "policy": {"pretrained_path": "x"}})) @@ -201,3 +203,35 @@ def test_old_custom_backend_setting_falls_back_to_auto(settings_file): settings_file.write_text(json.dumps({"infer_backend": "custom"})) assert us.reload()["infer_backend"] == "auto" + + +def _robomimic_runner_module(monkeypatch, ckpt): + """relay_d.dispatch.robomimic_runner loaded against stand-in robomimic modules (robomimic is not installed here).""" + import importlib.util + import sys + + policy = types.SimpleNamespace(policy=None, start_episode=lambda: None) + file_utils = types.ModuleType("robomimic.utils.file_utils") + file_utils.policy_from_checkpoint = lambda ckpt_path, device: (policy, ckpt) + torch_utils = types.ModuleType("robomimic.utils.torch_utils") + torch_utils.get_torch_device = lambda try_to_use_cuda: "cpu" + utils = types.ModuleType("robomimic.utils") + utils.file_utils, utils.torch_utils = file_utils, torch_utils + robomimic = types.ModuleType("robomimic") + robomimic.utils = utils + for module in (robomimic, utils, file_utils, torch_utils): + monkeypatch.setitem(sys.modules, module.__name__, module) + spec = importlib.util.find_spec("relay_d.dispatch.robomimic_runner") + module = importlib.util.module_from_spec(spec) # not put in sys.modules: other tests never see the stand-ins + spec.loader.exec_module(module) + return module + + +def test_robomimic_runner_reads_the_rate_and_action_size_from_the_checkpoint(monkeypatch): + ckpt = {"shape_metadata": {"all_obs_keys": ["q"], "ac_dim": 7}, + "env_metadata": {"env_name": "relay_d", "type": 1, "env_kwargs": {"control_freq": 30.0}}} + rr = _robomimic_runner_module(monkeypatch, ckpt) + runner = rr.RobomimicPolicyRunner("model.pth") + assert runner.fps == 30.0 and runner.action_dim == 7 # the rate the data was recorded at, as for LeRobot + assert rr.checkpoint_fps({"env_metadata": {"env_name": "relay_d", "type": 1, "env_kwargs": {}}}) is None # older checkpoint + assert rr.checkpoint_fps({}) is None and rr.checkpoint_fps({"env_metadata": [ckpt["env_metadata"]] * 2}) is None diff --git a/tests/test_numerics.py b/tests/test_numerics.py index 2925a8a..947aeca 100644 --- a/tests/test_numerics.py +++ b/tests/test_numerics.py @@ -6,13 +6,10 @@ from relay_d.acquisition.qt_app.ui.postprocess.postprocess_config_loader import PostprocessConfigLoader from relay_d.acquisition.qt_app.ui.postprocess.yaml_driven_converter import ( - DataTransformEngine, YAMLDrivenConverter, - _apply_normalization, ) from relay_d.utils.numerics import ( inputs_in_sync, - normalize, quat_to_euler, quat_to_rot_matrix, read_scalar, @@ -20,21 +17,6 @@ ) -def test_normalize_maps_range_and_leaves_degenerate_range_alone(): - assert normalize(5.0, 0.0, 10.0) == 0.0 - assert normalize(10.0, 0.0, 10.0, 0.0, 1.0) == 1.0 - assert normalize(7.0, 3.0, 3.0) == 7.0 # upper == lower - assert normalize(7.0, 3.0, 1.0) == 7.0 # upper < lower - - -def test_training_normalization_matches_shared_rule_for_degenerate_limits(): - data = np.array([[7.0, 5.0], [7.0, 10.0]], dtype=np.float32) - limits = [{"lower_lim": 3.0, "upper_lim": 3.0}, {"lower_lim": 0.0, "upper_lim": 10.0}] - out = _apply_normalization(data, limits, [-1.0, 1.0]) - assert out[:, 0].tolist() == [7.0, 7.0] # same as inference: the value is passed through - assert out[:, 1].tolist() == [0.0, 1.0] - - def test_quat_to_euler_known_rotations(): s = np.sqrt(0.5) q = np.array([[0, 0, 0, 1], [0, 0, s, s], [s, 0, 0, s]], dtype=np.float64) # id, yaw 90, roll 90 @@ -77,42 +59,30 @@ def converter(): return YAMLDrivenConverter(PostprocessConfigLoader()) -def _mask(converter, n, train, val, test): +def _mask(converter, n, train, val): with h5py.File("m.h5", "w", driver="core", backing_store=False) as f: g = f.create_group("mask") - ok = converter._create_data_mask(g, [f"demo_{i}" for i in range(n)], train, val, test) + ok = converter._create_data_mask(g, [f"demo_{i}" for i in range(n)], train, val) return ok, {k: len(g[k]) for k in g} def test_split_mask_normal_and_few_demos(converter): - assert _mask(converter, 10, 0.8, 0.1, 0.1) == (True, {"train": 8, "valid": 1, "eval": 1}) - ok, sizes = _mask(converter, 2, 0.8, 0.1, 0.1) + assert _mask(converter, 10, 0.8, 0.2) == (True, {"train": 8, "valid": 2}) + ok, sizes = _mask(converter, 2, 0.8, 0.1) assert ok and sizes["train"] >= 1 # int(2 * 0.8) would be 1 here, int(1 * 0.8) would be 0 -def test_split_mask_without_test_split_trains_on_the_leftovers(converter): - assert _mask(converter, 10, 0.8, 0.1, 0.0) == (True, {"train": 9, "valid": 1}) # was 8 / 1 / 1 unused "eval" - assert _mask(converter, 100, 0.29, 0.71, 0.0)[1] == {"train": 29, "valid": 71} # int(100 * 0.29) is 28 +def test_split_mask_trains_on_the_leftovers(converter): + assert _mask(converter, 10, 0.8, 0.1) == (True, {"train": 9, "valid": 1}) # no unused "eval" split + assert _mask(converter, 100, 0.29, 0.71)[1] == {"train": 29, "valid": 71} # int(100 * 0.29) is 28 def test_split_mask_rejects_bad_fractions(converter): - assert _mask(converter, 10, 0.9, 0.2, 0.0)[0] is False + assert _mask(converter, 10, 0.9, 0.2)[0] is False assert "more than 1" in converter.last_error - assert _mask(converter, 10, -0.1, 0.1, 0.1)[0] is False - assert _mask(converter, 10, "0.8", 0.1, 0.1)[0] is False - assert _mask(converter, 10, 0.0, 0.5, 0.5)[0] is False # no train mask: robomimic could not start - - -def test_generate_actions_fails_on_missing_key(): - cfg = {"actions_to_extract": ["robot_joint_pos"], "output": {"robot_joint_pos": "robot/joint_positions"}} - engine = DataTransformEngine(cfg) - with h5py.File("a.h5", "w", driver="core", backing_store=False) as f: - with pytest.raises(ValueError, match="not found in data"): - engine.generate_actions(f, {}, 5) - f.create_dataset("robot/joint_positions", data=np.ones((5, 3), dtype=np.float32)) - assert engine.generate_actions(f, {}, 5).shape == (5, 3) - with pytest.raises(ValueError, match="not found in bridge_map"): - DataTransformEngine({"actions_to_extract": ["nope"]}).generate_actions(None, {}, 5) + assert _mask(converter, 10, -0.1, 0.1)[0] is False + assert _mask(converter, 10, "0.8", 0.1)[0] is False + assert _mask(converter, 10, 0.0, 0.5)[0] is False # no train mask: robomimic could not start def test_zero_quaternion_in_an_rpy_stream_fails_instead_of_writing_nan(): @@ -133,31 +103,11 @@ def test_action_is_the_next_step_target(): assert _action_part(x, "q", raw, []).ravel().tolist() == [1.0, 3.0, 3.0] # delta: change from t to t+1; the last step is zero assert _action_part(x, "q", raw, ["q"]).ravel().tolist() == [1.0, 2.0, 0.0] - norm = {"q": {"specs": ["j.position[:]"], "normalize": True, "limits": [{"lower_lim": 0.0, "upper_lim": 4.0}]}} - assert _action_part(x, "q", norm, []).ravel().tolist() == [-0.5, 0.5, 0.5] - with pytest.raises(ValueError, match="delta_actions"): # a normalized delta cannot be denormalized at run time - _action_part(x, "q", norm, ["q"]) - - -def test_generate_actions_fails_on_a_length_mismatch_instead_of_stretching(): - cfg = {"actions_to_extract": ["q"], "output": {"q": "robot/joint_positions"}} - with h5py.File("b.h5", "w", driver="core", backing_store=False) as f: - f.create_dataset("robot/joint_positions", data=np.ones((4, 2), dtype=np.float32)) - with pytest.raises(ValueError, match="4 rows"): - DataTransformEngine(cfg).generate_actions(f, {}, 5) - - -def test_generate_actions_does_not_guess_another_group_with_the_same_field(): - cfg = {"actions_to_extract": ["q"], "output": {"q": "arm/joint_positions"}} - with h5py.File("c.h5", "w", driver="core", backing_store=False) as f: - f.create_dataset("gripper/joint_positions", data=np.ones((5, 1), dtype=np.float32)) - with pytest.raises(ValueError, match="not found in data"): # was: silently used the gripper - DataTransformEngine(cfg).generate_actions(f, {}, 5) def test_split_mask_sorts_demo_numbers_naturally(converter): with h5py.File("d.h5", "w", driver="core", backing_store=False) as f: - assert converter._create_data_mask(f.create_group("mask"), [f"demo_{i}" for i in range(11)], 0.9, 0.1, 0.0) + assert converter._create_data_mask(f.create_group("mask"), [f"demo_{i}" for i in range(11)], 0.9, 0.1) assert [n.decode() for n in f["mask/valid"][:]] == ["demo_10"] # the last demo, not demo_9 diff --git a/tests/test_offline_config.py b/tests/test_offline_config.py index 097a5bc..3f703f5 100644 --- a/tests/test_offline_config.py +++ b/tests/test_offline_config.py @@ -40,6 +40,14 @@ def test_warnings_for_missing_output_and_fps(tmp_path): assert any("recording_frequency" in p for p in problems) +@pytest.mark.parametrize("key", ["metadata", "training_model", "test_frac"]) +def test_removed_keys_are_rejected(tmp_path, key): + cfg = json.loads(json.dumps(DATA_CONFIG)) + cfg[key] = {"recording_frequency": 10} if key == "metadata" else 0.1 + parser, problems = load_offline_config(_write_cfg(tmp_path / "c.yaml", cfg)) + assert parser is None and any(f"'{key}'" in p and "removed" in p for p in problems) + + def test_malformed_stream_spec_is_rejected(tmp_path): cfg = json.loads(json.dumps(DATA_CONFIG)) cfg["output"]["streams"]["bad"] = ["a.x - b.y - c.z"] # arithmetic allows exactly two operands diff --git a/tests/test_postprocess_options.py b/tests/test_postprocess_options.py index 008f81c..2e9f6a1 100644 --- a/tests/test_postprocess_options.py +++ b/tests/test_postprocess_options.py @@ -22,64 +22,47 @@ FILE_OPTIONS = { "postprocess": { - "robomimic": { - "training_model": "diffusion_policy", - "splits": {"train_frac": 0.5, "val_frac": 0.5}, - "metadata": {"env_name": "FromFile", "env_version": "9", "type": 1, "env_kwargs": {"a": 1}}, - }, + "robomimic": {"splits": {"train_frac": 0.5, "val_frac": 0.5}}, "lerobot": {"task": "task from file", "robot_type": "iiwa"}, } } -@pytest.fixture(autouse=True) -def _reset_warned(): - po._warned_legacy.clear() - - def _write(path, data): path.write_text(yaml.safe_dump(data)) return str(path) -def test_file_values_win_over_legacy_and_defaults(tmp_path): +def test_file_values_and_defaults(tmp_path): opts, problems = PostprocessOptions.load(_write(tmp_path / "postprocess_config.yaml", FILE_OPTIONS)) assert opts is not None and problems == [] - legacy = {"metadata": {"env_name": "Legacy"}, "training_model": "bc_rnn_gmm", "train_frac": 0.1} - assert opts.robomimic_metadata(legacy)["env_name"] == "FromFile" - assert opts.training_model(legacy) == "diffusion_policy" - assert opts.splits(legacy) == {"train_frac": 0.5, "val_frac": 0.5, "test_frac": 0.0} - assert opts.lerobot({"lerobot": {"task": "legacy", "state_streams": ["x"]}}) == { - "task": "task from file", "robot_type": "iiwa", "state_streams": ["x"], - } + assert opts.splits() == {"train_frac": 0.5, "val_frac": 0.5} + assert opts.lerobot() == {"task": "task from file", "robot_type": "iiwa"} + assert PostprocessOptions().splits() == {"train_frac": 0.8, "val_frac": 0.2} + assert PostprocessOptions().lerobot() == {} -def test_legacy_fallback_warns_once(caplog): - legacy = {"metadata": {"env_name": "Legacy"}, "training_model": "bc_transformer_gmm", "val_frac": 0.3} - opts = PostprocessOptions() - messages = [] - import relay_d.acquisition.qt_app.ui.postprocess.postprocess_options as mod +@pytest.mark.parametrize("robomimic, key", [ + ({"metadata": {"env_name": "Lift"}}, "robomimic.metadata"), + ({"training_model": "diffusion_policy"}, "robomimic.training_model"), + ({"splits": {"train_frac": 0.8, "val_frac": 0.1, "test_frac": 0.1}}, "robomimic.splits.test_frac"), +]) +def test_removed_options_are_refused(tmp_path, robomimic, key): + opts, problems = PostprocessOptions.load(_write(tmp_path / "o.yaml", {"postprocess": {"robomimic": robomimic}})) + assert opts is None and any(key in p and "removed" in p for p in problems) - mod.logger = type("L", (), {"warning": staticmethod(messages.append), "info": staticmethod(lambda *_: None)}) - try: - assert opts.robomimic_metadata(legacy)["env_name"] == "Legacy" - assert opts.robomimic_metadata(legacy)["env_name"] == "Legacy" - assert opts.training_model(legacy) == "bc_transformer_gmm" - assert opts.splits(legacy)["val_frac"] == 0.3 - finally: - import relay_d.utils.coloring_logger as cl - mod.logger = cl.logger - assert sum("'metadata'" in m for m in messages) == 1 # warned once, not per call - assert any("deprecated" in m and "postprocess_config.yaml" in m for m in messages) - - -def test_defaults_when_nothing_set(): - opts = PostprocessOptions() - assert opts.robomimic_metadata({}) == {} - assert opts.training_model({}) == "bc_rnn_gmm" - assert opts.splits({}) == {"train_frac": 0.8, "val_frac": 0.2, "test_frac": 0.0} - assert opts.lerobot({}) == {} +@pytest.mark.parametrize("key", ["metadata", "training_model", "train_frac", "val_frac", "test_frac", "lerobot"]) +def test_removed_acquisition_keys_fail_the_conversion(tmp_path, key): + rec = tmp_path / "recording_0.h5" + write_recording(rec, LENGTHS) + cfg = json.loads(json.dumps(DATA_CONFIG)) + cfg[key] = {"recording_frequency": 10} if key in ("metadata", "lerobot") else 0.5 + conv = _converter(cfg) + assert not conv.convert_multiple_files("robomimic", [str(rec)], str(tmp_path / "rm.h5")) + assert f"'{key}'" in conv.last_error and "removed" in conv.last_error + assert not conv.convert_multiple_files("lerobot", [str(rec)], str(tmp_path / "lr")) + assert f"'{key}'" in conv.last_error def test_load_rejects_bad_files(tmp_path): @@ -103,7 +86,6 @@ def test_find_sibling(tmp_path): def test_templates_are_valid(): opts, problems = PostprocessOptions.load("relay_d/acquisition/qt_app/ui/config/templates/postprocess_config_template.yaml") assert opts is not None and problems == [] - assert opts.robomimic_metadata({})["env_name"] == "Lift" acq = yaml.safe_load(open("relay_d/acquisition/qt_app/ui/config/templates/config_template.yaml"))["config"] for legacy_key in ("metadata", "training_model", "train_frac", "lerobot"): assert legacy_key not in acq @@ -122,24 +104,12 @@ def test_robomimic_uses_options_file(tmp_path): out = tmp_path / "rm.h5" assert conv.convert_multiple_files("robomimic", [str(rec)], str(out)) with h5py.File(out, "r") as f: - env_args = json.loads(f["data"].attrs["env_args"]) - assert env_args["env_name"] == "FromFile" + assert json.loads(f["data"].attrs["env_args"]) == {"env_name": "relay_d", "type": 1, "env_kwargs": {"control_freq": float(DATA_CONFIG["recording_frequency"])}} + assert "next_obs" not in f["data/demo_0"] and "states" not in f["data/demo_0"] and "eval" not in f["mask"] # train 0.5 / valid 0.5 of 2 demos assert len(f["mask/train"]) == 1 and len(f["mask/valid"]) == 1 -def test_robomimic_legacy_config_still_works(tmp_path): - rec = tmp_path / "recording_0.h5" - write_recording(rec, LENGTHS) - cfg = json.loads(json.dumps(DATA_CONFIG)) - cfg["metadata"] = {"env_name": "LegacyEnv", "env_version": "1", "type": 1, "env_kwargs": {}} - conv = _converter(cfg) - out = tmp_path / "rm.h5" - assert conv.convert_multiple_files("robomimic", [str(rec)], str(out)) - with h5py.File(out, "r") as f: - assert json.loads(f["data"].attrs["env_args"])["env_name"] == "LegacyEnv" - - def test_lerobot_uses_options_file(tmp_path): rec = tmp_path / "recording_0.h5" write_recording(rec, LENGTHS) @@ -154,14 +124,6 @@ def test_lerobot_uses_options_file(tmp_path): assert pq.read_table(out / "meta/tasks.parquet")["task"].to_pylist() == ["task from file"] -def test_get_train_model_name_with_options(tmp_path): - p = YamlParser() - p.yaml_data = {"config": {"training_model": "legacy_model"}} - assert p.get_train_model_name() == "legacy_model" - opts, _ = PostprocessOptions.load(_write(tmp_path / "o.yaml", FILE_OPTIONS)) - assert p.get_train_model_name(opts) == "diffusion_policy" - - def test_get_obs_config_without_sync_tolerance(): p = YamlParser() p.yaml_data = {"config": {"input": {}, "output": {"streams": {}}}} diff --git a/tests/test_postprocess_page.py b/tests/test_postprocess_page.py new file mode 100644 index 0000000..9453835 --- /dev/null +++ b/tests/test_postprocess_page.py @@ -0,0 +1,146 @@ +"""Post-process page: pre-check table, output preview, Convert state and the recordings row.""" + +import os +import time + +import pytest + +pytest.importorskip("PyQt5") +from PyQt5.QtWidgets import QMainWindow # noqa: E402 + +from relay_d.acquisition.qt_app.ui.config.yaml_parser import YamlParser # noqa: E402 +from relay_d.acquisition.qt_app.ui.postprocess import postprocess_page as pp # noqa: E402 + + +def _report(ok=(True, False), error=None): + demos = [ + {"file": "/rec/a.h5", "demo": f"demo_{i}", "samples": 50 + i, "rate_hz": 20.0, + "recorded_with": "config_peter.yaml", "recorded_hz": 20.0, "ok": good, + "reason": "" if good else "no robot_gripper_data (action)"} + for i, good in enumerate(ok) + ] + converted = sum(ok) + return {"format": "robomimic", "error": error, "demos": [] if error else demos, + "output": [] if error else [["data/demo_0/actions", "(N,8)"], ["mask/train", f"{converted} demos"]], + "converted": 0 if error else converted, "total": len(ok)} + + +@pytest.fixture +def page(monkeypatch, qapp): + import relay_d.acquisition.qt_app as qt_app + + monkeypatch.syspath_prepend(os.path.dirname(qt_app.__file__)) + from relay_d.acquisition.qt_app.ui.containers.mainWindow_ui import Ui_MainWindow + + window = QMainWindow() + ui = Ui_MainWindow() + ui.setupUi(window) + p = pp.PostProcessPage(ui, YamlParser()) + p._window = window + p.yaml_converter.data_config = {"output": {"streams": {"x": {}}}, "actions_to_extract": ["x"]} + p.output_format = "robomimic" + return p + + +def _wait(qapp, cond, timeout=5.0): + end = time.time() + timeout + while not cond() and time.time() < end: + qapp.processEvents() + time.sleep(0.01) + assert cond() + + +def test_good_report_fills_table_and_enables_convert(page, qapp, monkeypatch): + monkeypatch.setattr(page.yaml_converter, "precheck", lambda fmt, files: _report(), raising=False) + page.set_files(["/rec/a.h5"]) + _wait(qapp, lambda: page._report is not None) # the real background runner + t = page.table_precheck + assert t.rowCount() == 2 + assert [t.item(0, c).text() for c in range(4)] == ["demo_0", "50", "20.0", "config_peter.yaml (20 Hz)"] + assert t.item(0, 4).text().startswith("✔") and "robot_gripper_data" in t.item(1, 4).text() + assert page.btn_convert.isEnabled() and page.btn_convert.text() == "Convert 1 demo" + assert page.label_summary.text() == "1 of 2 demos will be converted" + assert "data/demo_0/actions" in page.txt_output.toPlainText() + assert page.label_recordings.text() == "2 demos in 1 file" + assert page.label_precheck_error.isHidden() + + +@pytest.mark.parametrize("report, reason", [ + (_report(error="robot0_gripper is in none of the recordings"), "robot0_gripper is in none"), + (_report(ok=(False, False)), "No demo would be converted"), +]) +def test_convert_disabled_with_reason(page, qapp, monkeypatch, report, reason): + monkeypatch.setattr(page.yaml_converter, "precheck", lambda fmt, files: report, raising=False) + page.set_files(["/rec/a.h5"]) + _wait(qapp, lambda: page._report is not None) + assert not page.btn_convert.isEnabled() + assert reason in page.btn_convert.toolTip() + if report["error"]: # shown once, in the error box; the side label points to it + assert not page.label_precheck_error.isHidden() and reason in page.label_precheck_error.text() + assert page.label_convert_reason.text() == "See the error above." + else: + assert reason in page.label_convert_reason.text() + + +def test_nothing_to_check_disables_convert_without_running(page, monkeypatch): + monkeypatch.setattr(pp, "run_in_background", lambda *a: pytest.fail("no pre-check without files")) + page.set_files([]) + assert not page.btn_convert.isEnabled() and "Add recordings" in page.btn_convert.toolTip() + page._select_output_format("robomimic") # deselect + assert "output format" in page.label_convert_reason.text() + + +def test_add_folder_collects_recursively(page, tmp_path, monkeypatch): + (tmp_path / "s1" / "deep").mkdir(parents=True) + for rel in ("top.h5", "s1/a.h5", "s1/deep/b.h5", "s1/notes.txt"): + (tmp_path / rel).write_text("") + monkeypatch.setattr(pp, "run_in_background", lambda *a: None) + monkeypatch.setattr(pp.QFileDialog, "getExistingDirectory", lambda *a, **k: str(tmp_path)) + page._select_folder_to_convert() + assert page.h5_files == pp.collect_inputs([str(tmp_path)]) + assert sorted(os.path.relpath(f, tmp_path) for f in page.h5_files) == ["s1/a.h5", "s1/deep/b.h5", "top.h5"] + page.btn_clear_files.click() + assert page.h5_files == [] + + +def test_later_refresh_wins(page, monkeypatch): + jobs = [] + monkeypatch.setattr(pp, "run_in_background", lambda parent, work, on_done: jobs.append(on_done)) + page.set_files(["/rec/a.h5"]) + page.set_files(["/rec/a.h5", "/rec/b.h5"]) + first, second = jobs + second(_report(ok=(True, True)), None) + first(_report(ok=(False,)), None) # finishes last but was started first: ignored + assert page.table_precheck.rowCount() == 2 and page.btn_convert.text() == "Convert 2 demos" + + +def test_selected_recordings_can_be_removed_before_converting(page, qapp, monkeypatch): + from PyQt5.QtCore import QItemSelectionModel, Qt + from PyQt5.QtTest import QTest + + files = ["/rec/a.h5", "/rec/b.h5", "/rec/c.h5"] + checked = [] + + def precheck(fmt, chosen): + checked.append(list(chosen)) + demos = [{"file": f, "demo": os.path.basename(f), "samples": 50, "rate_hz": 20.0, "recorded_with": None, + "recorded_hz": None, "ok": True, "reason": ""} for f in chosen] + return {"format": fmt, "error": None, "demos": demos, "output": [], "converted": len(demos), "total": len(demos)} + + monkeypatch.setattr(page.yaml_converter, "precheck", precheck, raising=False) + page.set_files(files) + _wait(qapp, lambda: page._report is not None and page.table_precheck.rowCount() == 3) + assert not page.btn_remove_files.isEnabled() # nothing selected yet + + table = page.table_precheck + for row in (0, 2): + table.selectionModel().select(table.model().index(row, 0), QItemSelectionModel.Select | QItemSelectionModel.Rows) + assert page.btn_remove_files.isEnabled() + page.btn_remove_files.click() + assert page.h5_files == ["/rec/b.h5"] # only left out of the selection: nothing is deleted from disk + _wait(qapp, lambda: checked[-1] == ["/rec/b.h5"] and table.rowCount() == 1) + + table.selectRow(0) + table.setFocus() + QTest.keyClick(table, Qt.Key_Delete) + assert page.h5_files == [] diff --git a/tests/test_precheck.py b/tests/test_precheck.py new file mode 100644 index 0000000..bdcb050 --- /dev/null +++ b/tests/test_precheck.py @@ -0,0 +1,139 @@ +"""Pre-check (YAMLDrivenConverter.precheck / relay-d-convert --check): same verdicts as the conversion, nothing written.""" + +import json + +import h5py +import numpy as np +import pytest +import yaml + +from relay_d.acquisition.convert_cli import main as cli_main +from relay_d.acquisition.qt_app.ui.postprocess.postprocess_config_loader import PostprocessConfigLoader +from relay_d.acquisition.qt_app.ui.postprocess.postprocess_options import PostprocessOptions +from relay_d.acquisition.qt_app.ui.postprocess.yaml_driven_converter import YAMLDrivenConverter + +from helpers import DATA_CONFIG, FPS, write_recording + + +def _converter(cfg=None): + return YAMLDrivenConverter(PostprocessConfigLoader(), data_config=cfg or json.loads(json.dumps(DATA_CONFIG))) + + +def _stamp(demo, rate, recorded_hz=None, config="/somewhere/config_peter.yaml"): + n = demo["streams/eef_translation"].shape[0] + demo.create_dataset("timestamps", data=(np.arange(n) * 1e9 / rate).astype(np.int64)) + if recorded_hz is not None: + meta = demo.create_group("metadata") + meta.attrs["recording_frequency"] = float(recorded_hz) + meta.attrs["yaml_config"] = config + + +def _problem_recording(tmp_path): + """demo_0 converts; 1 off-rate, 2 no action stream, 3 no camera, 4 a short stream.""" + rec = tmp_path / "rec.h5" + write_recording(rec, [14, 9, 10, 11, 12]) + with h5py.File(rec, "a") as f: + d = f["data"] + for i in range(5): + _stamp(d[f"demo_{i}"], 0.7 * FPS if i == 1 else FPS, recorded_hz=FPS) + del d["demo_2/streams/robot_joint_pos"] + del d["demo_3/camera_rgb"] + short = d["demo_4/streams/eef_translation"][:-3] + del d["demo_4/streams/eef_translation"] + d["demo_4/streams"].create_dataset("eef_translation", data=short) + return rec + + +@pytest.mark.parametrize("fmt", ["robomimic", "lerobot"]) +def test_precheck_skips_exactly_what_conversion_skips(tmp_path, fmt): + rec = _problem_recording(tmp_path) + before = sorted(p.name for p in tmp_path.iterdir()) + report = _converter().precheck(fmt, [str(rec)]) + assert sorted(p.name for p in tmp_path.iterdir()) == before # nothing written + assert report["error"] is None and report["total"] == 5 and report["converted"] == 1 + reasons = {d["demo"]: d["reason"] for d in report["demos"]} + assert reasons["demo_0"] == "" + assert "keep up" in reasons["demo_1"] # metadata matches the config: a slow PC + assert reasons["demo_2"] == "no robot_joint_pos (action stream)" + assert reasons["demo_3"] == "no camera agentview_image" + assert "eef_translation" in reasons["demo_4"] and "9 rows" in reasons["demo_4"] + + if fmt == "lerobot": + pytest.importorskip("av") + conv = _converter() + out = tmp_path / ("out.h5" if fmt == "robomimic" else "lerobot_out") + assert conv.convert_multiple_files(fmt, [str(rec)], str(out)) + assert {f"{d['file']}::{d['demo']}" for d in report["demos"] if not d["ok"]} == set(conv.skipped_demos) + assert conv.converted_demos == report["converted"] + + +def test_rate_mismatch_names_the_config_the_demo_was_recorded_with(tmp_path): + rec = tmp_path / "rec.h5" + write_recording(rec, [14]) + with h5py.File(rec, "a") as f: + _stamp(f["data/demo_0"], 2 * FPS, recorded_hz=2 * FPS) + report = _converter().precheck("robomimic", [str(rec)]) + demo = report["demos"][0] + assert demo["recorded_with"] == "config_peter.yaml" and demo["recorded_hz"] == 2 * FPS + assert demo["rate_hz"] == pytest.approx(2 * FPS) + assert not demo["ok"] and "keep up" not in demo["reason"] + assert f"recorded at {2 * FPS} Hz with config_peter.yaml; the loaded config says {FPS} Hz" in demo["reason"] + + +@pytest.mark.parametrize("fmt", ["robomimic", "lerobot"]) +def test_stream_in_no_recording_stops_the_conversion(tmp_path, fmt): + cfg = json.loads(json.dumps(DATA_CONFIG)) + cfg["output"]["streams"]["robot0_gripper"] = ["gripper.data"] + rec = tmp_path / "rec.h5" + write_recording(rec, [14, 9]) + with h5py.File(rec, "a") as f: + _stamp(f["data/demo_0"], FPS, recorded_hz=FPS) + report = _converter(cfg).precheck(fmt, [str(rec)]) + conv = _converter(cfg) + out = tmp_path / "out" + assert not conv.convert_multiple_files(fmt, [str(rec)], str(out)) and not out.exists() + assert report["error"] == conv.last_error and report["output"] == [] and report["converted"] == 0 + assert "robot0_gripper" in conv.last_error and "eef_translation, robot_joint_pos" in conv.last_error + assert "config_peter.yaml" in conv.last_error + + +def test_output_layout_robomimic(tmp_path): + rec = tmp_path / "rec.h5" + write_recording(rec, [14, 9]) + conv = _converter() + conv.options = PostprocessOptions({"robomimic": {"splits": {"train_frac": 0.9, "val_frac": 0.1}}}) + out = dict(map(tuple, conv.precheck("robomimic", [str(rec)])["output"])) + assert out["data/demo_N/obs/robot_joint_pos"] == "(N, 4) float32" + assert out["data/demo_N/obs/agentview_image"] == "(N, 24, 32, 3) uint8" + assert out["data/demo_N/actions"] == "(N, 4) = robot_joint_pos[0:4]" + assert out["mask/train, mask/valid"] == "2 / 0 demos (train_frac 0.9, val_frac 0.1)" + assert out["env_args"] == f"control_freq {FPS}" + + +def test_output_layout_lerobot(tmp_path): + rec = tmp_path / "rec.h5" + write_recording(rec, [14, 9]) + out = dict(map(tuple, _converter().precheck("lerobot", [str(rec)])["output"])) + assert out["observation.state"] == "(7) robot_joint_pos.0..3, eef_translation.0..2" + assert out["observation.images.agentview_image"] == "video 3x24x32" + assert out["action"] == "(4) = robot_joint_pos[0:4]" # [:] without joint_names: the writer names them action_i + assert out["fps"] == str(FPS) and out["task"] == "stack_cubes" and out["episodes"] == "2" + + +def test_cli_check_prints_the_report_and_writes_nothing(tmp_path, capsys): + rec = _problem_recording(tmp_path) + cfg = tmp_path / "acq.yaml" + cfg.write_text(yaml.safe_dump({"config": DATA_CONFIG})) + before = sorted(p.name for p in tmp_path.iterdir()) + assert cli_main(["--config", str(cfg), "--format", "robomimic", "--input", str(rec), "--check"]) == 0 + assert sorted(p.name for p in tmp_path.iterdir()) == before + text = capsys.readouterr().out + assert "1 of 5 demos would be converted" in text and "no camera agentview_image" in text + assert "data/demo_N/actions" in text + + bad = json.loads(json.dumps(DATA_CONFIG)) + bad["output"]["streams"]["robot0_gripper"] = ["gripper.data"] + bad["input"]["gripper"] = {"topic": "/gripper"} + cfg.write_text(yaml.safe_dump({"config": bad})) + assert cli_main(["--config", str(cfg), "--format", "robomimic", "--input", str(rec), "--check"]) == 1 + assert "robot0_gripper" in capsys.readouterr().out diff --git a/tests/test_recorder_loop.py b/tests/test_recorder_loop.py new file mode 100644 index 0000000..485c849 --- /dev/null +++ b/tests/test_recorder_loop.py @@ -0,0 +1,220 @@ +"""DataRecorder's real sampling loop, stop/save path and image decoding, plus recording/inference stream parity.""" + +import sys +import threading +from types import SimpleNamespace +from unittest.mock import MagicMock + +import h5py +import numpy as np +import pytest +from helpers import joint_sample, make_recorder + +_HDR = SimpleNamespace(stamp=SimpleNamespace(sec=0, nanosec=0), frame_id="") + + +def _real_message_classes(recorder_cls): + """The encoder's message classes are MagicMocks under recorder_cls; isinstance() needs real classes.""" + module = sys.modules[recorder_cls.__module__.replace(".data_recorder", ".message_encoding")] + for name in ("Image", "CompressedImage", "PointCloud2", "PoseStamped", "Twist", + "TransformStamped", "String", "Float32", "Int32", "Bool"): + setattr(module, name, type(name, (), {})) + module.JointState = type("JointState", (), {"name": [], "position": [], "velocity": [], "effort": [], "header": _HDR}) + module.Float64 = type("Float64", (), {"data": 0.0}) + return module + + +class _FakeSubscribers: + """Feeds one dict of messages per tick to the loop, then stops it.""" + + def __init__(self, rec, frames, classes): + self.rec, self.frames, self.ticks = rec, frames, 0 + self.subscribers = dict.fromkeys(classes) + self.subscriber_status = {cid: {"topic": "/" + cid} for cid in classes} + self.message_type_cache = {"/" + cid: cls for cid, cls in classes.items()} + self.tf_lookup_containers = {} + + def update_tf_lookups(self): + pass + + def get_content(self): + return dict.fromkeys(self.subscribers) + + def get_content_and_timestamps(self): + frame = self.frames[min(self.ticks, len(self.frames) - 1)] + self.ticks += 1 + if self.ticks >= len(self.frames): + self.rec.stop_recording_flag = True + return dict(frame), {} + + +def _loop_ready(recorder_cls, monkeypatch, parser=None): + module = sys.modules[recorder_cls.__module__] + get = module.user_settings.get + monkeypatch.setattr(module.user_settings, "get", lambda k: 0 if k == "rec_warmup_s" else get(k)) + rec = recorder_cls(parser or SimpleNamespace(is_loaded=lambda: False, yaml_data=None)) + rec._clock = SimpleNamespace(now=lambda: SimpleNamespace(to_msg=lambda: SimpleNamespace(sec=1, nanosec=0))) + rec.recording_frequency = 1000 + rec._last_valid_raw = {} + rec.stop_recording_flag = False + return rec + + +def test_a_failing_loop_sets_loop_failed(recorder_cls, monkeypatch): + rec = _loop_ready(recorder_cls, monkeypatch) + subs = _FakeSubscribers(rec, [{}], {}) + subs.get_content_and_timestamps = MagicMock(side_effect=RuntimeError("broken")) + rec.topic_subscribers = subs + rec._standard_recording_loop(subs) # returns after 50 errors in a row + assert subs.get_content_and_timestamps.call_count == 50 + assert rec.loop_failed is True + + +def _config_parser(config): + return SimpleNamespace(is_loaded=lambda: True, yaml_data=config, get_current_config_path=lambda: "c.yaml", + get_metadata=lambda: {}, get_input_data=lambda: [], get_streams=lambda: {}) + + +@pytest.mark.parametrize("recording_frequency", [None, 0]) +def test_missing_or_zero_recording_frequency_is_refused_at_start(recorder_cls, monkeypatch, tmp_path, recording_frequency): + config = {"config": {"input": {"arm": {"topic": "/arm"}}}} + if recording_frequency is not None: + config["config"]["recording_frequency"] = recording_frequency + rec = _loop_ready(recorder_cls, monkeypatch, _config_parser(config)) + rec.output_directory = str(tmp_path) + assert rec.start_recording(_FakeSubscribers(rec, [{}], {})) is False # missing: was the 30 Hz setting + assert rec.is_recording is False + + +@pytest.mark.parametrize("removed", [{"output_map": "robot"}, {"fill_gaps": True, "fill_default": 0.5}]) +def test_removed_keys_that_changed_the_recorded_data_are_refused_at_start(recorder_cls, monkeypatch, tmp_path, removed): + config = {"config": {"recording_frequency": 1000, "input": {"arm": {"topic": "/arm", **removed}}}} + rec = _loop_ready(recorder_cls, monkeypatch, _config_parser(config)) + rec.output_directory = str(tmp_path) + errors = [] + monkeypatch.setattr(sys.modules[recorder_cls.__module__].logger, "error", errors.append) + assert rec.start_recording(_FakeSubscribers(rec, [{}], {}), "d") is False # was True: groups renamed / made-up values + assert rec.is_recording is False and rec.h5_file is None + assert any("was removed" in e for e in errors) + + +def test_start_always_flushes_stale_data(recorder_cls, monkeypatch, tmp_path): + """The GUI passes no flag: it must drop pre-start messages just like the API.""" + msgs = _real_message_classes(recorder_cls) + config = {"config": {"recording_frequency": 1000, "input": {"arm": {"topic": "/arm"}}}} + rec = _loop_ready(recorder_cls, monkeypatch, _config_parser(config)) + rec.output_directory = str(tmp_path) + js = msgs.JointState() + js.name, js.position = ["a"], [1.0] + subs = _FakeSubscribers(rec, [{"arm": js}] * 3, {"arm": msgs.JointState}) + subs.flush_latest_data = MagicMock() + assert rec.start_recording(subs, "d") is True + rec.recording_thread.join(5) + assert rec.stop_recording() is True + subs.flush_latest_data.assert_called_once() # was not called from the GUI path + + +def test_a_failed_close_keeps_the_demo_for_a_retry(recorder_cls, tmp_path): + rec = make_recorder(recorder_cls, tmp_path, [1, 2], {"arm": [joint_sample()] * 2}) + real = rec.h5_file + + class _CloseFails: + def __getattr__(self, name): + return getattr(real, name) + + def close(self): + raise OSError("disk full") + + rec.h5_file = _CloseFails() + rec.is_recording = True + rec.recording_metadata["start_time"] = 0.0 + assert rec.stop_recording() is False + assert rec._unsaved is True # was False: the next Start would have cleared the buffer + real.close() + rec.output_directory = str(tmp_path / "retry") + assert rec.retry_save() is True and rec._unsaved is False + with h5py.File(rec.retried_file) as f: + assert f["data/d/arm/joint_positions"].shape == (2, 1) + + +@pytest.mark.parametrize("encoding, image", [ + ("16UC1", np.arange(12, dtype=np.uint16).reshape(3, 4)), + ("8UC3", np.arange(36, dtype=np.uint8).reshape(3, 4, 3)), +]) +def test_an_image_without_bgr8_conversion_is_kept_as_decoded(encoding, image, monkeypatch): + pytest.importorskip("sensor_msgs_py") + from relay_d.utils import ros_media_codec as codec + + warnings = [] + monkeypatch.setattr(codec.logger, "warning", warnings.append) + msg = codec.cv2_to_imgmsg(image, encoding) + msg.header.frame_id = f"cam_{encoding}" + for _ in range(3): + assert np.array_equal(codec.decode_image(msg), image) # was None: nothing saved + assert len(warnings) == 1 # once per camera, not once per frame + + +def test_record_page_ignores_config_changes_while_recording(qapp, monkeypatch): + pytest.importorskip("rclpy") + for name in ("PyQt5.QtMultimedia", "PyQt5.QtMultimediaWidgets"): # not in every PyQt5 install; unused here + try: + __import__(name) + except ImportError: + monkeypatch.setitem(sys.modules, name, MagicMock()) + from relay_d.acquisition.qt_app.ui.record.record_page import RecordPage + + page = RecordPage.__new__(RecordPage) + page.recording = SimpleNamespace(is_recording=True) + page.yaml_parser, page.topic_subscribers, page.topic_containers = MagicMock(), MagicMock(), MagicMock() + page.container_manager, page.tf_update_timer = MagicMock(), MagicMock() + page.label_no_config, page.ui = None, SimpleNamespace() + page._on_config_saved("a.yaml") + page._on_config_loaded("a.yaml") + page._on_config_cleared() + page.yaml_parser.load_yaml_file.assert_not_called() + page.topic_subscribers.clear_all_subscribers.assert_not_called() + page.container_manager.create_containers.assert_not_called() + + +def test_recorded_streams_match_the_live_obs(monkeypatch): + pytest.importorskip("rclpy") + from geometry_msgs.msg import TransformStamped + from sensor_msgs.msg import CompressedImage, Image, JointState, PointCloud2 + + from relay_d.acquisition.qt_app.utils import stream_builder + from relay_d.acquisition.qt_app.utils.stream_builder import StreamBuilder + # first imported under recorder_cls's stubbed sensor_msgs in a full run + monkeypatch.setattr(stream_builder, "_UNSUPPORTED_STREAM_MSG_TYPES", (Image, CompressedImage, PointCloud2)) + from relay_d.dispatch.obs_builder import ObsBuilder + from relay_d.utils.numerics import enriched_tf + + inputs = {"js": {"topic": "/js"}, "tf": {"parent_frame": "base", "child_frame": "ee"}} + specs = ["js.position[:]", "cos(js.position[:])", "sin(tf.translation.x - tf.translation.y)", + "sin(tf.translation.x) * cos(tf.translation.z)", "tf.translation.z / tf.translation.x", "zero:2", + "tf.euler.yaw", "tf.rot6d.i1", "tf.rot_matrix.r02", "js.velocity"] + js = JointState(name=["a", "b"], position=[0.1, -0.4], velocity=[0.7, 0.0]) + tf = TransformStamped() + tf.transform.translation.x, tf.transform.translation.y, tf.transform.translation.z = 0.5, -0.2, 0.9 + q = np.array([0.1, 0.3, -0.2, 0.9]) + q /= np.linalg.norm(q) + r = tf.transform.rotation + r.x, r.y, r.z, r.w = (float(v) for v in q) + + sb = StreamBuilder(SimpleNamespace(yaml_data={"config": {"input": inputs}}, get_input_data=lambda: [], + get_streams=lambda: {"s": specs})) + assert sb.get_build_errors() == [] + sb.evaluate({"js": js, "tf": tf}) + recorded = sb.finalize()["s"][0] + + ob = ObsBuilder.__new__(ObsBuilder) # skip ROS setup + ob._obs_cfg, ob._input_cfg, ob._tf_inputs, ob._static_inputs = {"s": specs}, inputs, {"tf"}, set() + ob._extractors, ob._rotation_format, ob._obs_input_names, ob._passthrough_obs = {}, {}, {}, {} + ob._build_extractors() + ob._lock, ob._sync_threshold, ob._latest, ob._obs_refreshed = threading.Lock(), None, {}, {} + ob._latest_inputs, ob._input_timestamps = {"js": js, "tf": enriched_tf(tf)}, {} + ob.get_clock = lambda: SimpleNamespace(now=lambda: SimpleNamespace(to_msg=lambda: SimpleNamespace(sec=0, nanosec=0))) + ob._evaluate_all() + live = ob._latest["s"] + + assert recorded.shape == live.shape == (13,) + np.testing.assert_allclose(recorded, live, rtol=1e-6) diff --git a/tests/test_recording_controller.py b/tests/test_recording_controller.py index ca92f23..8375545 100644 --- a/tests/test_recording_controller.py +++ b/tests/test_recording_controller.py @@ -1,4 +1,4 @@ -"""RecordingController (start/pause/resume/stop + buttons) and ContainerManager's non-blocking wait. +"""RecordingController (start/stop + buttons) and ContainerManager's non-blocking wait. Qt runs offscreen; the ROS-facing objects are mocks. Skipped when PyQt5 is not installed. """ @@ -36,7 +36,6 @@ class TfTopicSubscriptionError(Exception): def env(qapp, monkeypatch): ui = SimpleNamespace( btn_start_record=QPushButton("Start"), - btn_pause_record=QPushButton("Pause"), btn_stop_record=QPushButton("Stop"), ) yaml_parser = MagicMock() @@ -44,8 +43,6 @@ def env(qapp, monkeypatch): yaml_parser.get_dataset_name.return_value = "my_demo" recorder = MagicMock() recorder.start_recording.return_value = True - recorder.pause_recording.return_value = True - recorder.resume_recording.return_value = True recorder.stop_recording.return_value = True recorder.h5_file_path = "/data/my_demo_1.h5" recorder.retried_file = None @@ -78,46 +75,32 @@ def _run_now(parent, work, on_done, busy=None): def _states(ui): return ( ui.btn_start_record.isEnabled(), - ui.btn_pause_record.isEnabled(), ui.btn_pause_record.text(), ui.btn_stop_record.isEnabled(), ) def test_initial_and_config_loaded_button_states(env): - assert _states(env.ui) == (False, False, "Pause", False) + assert _states(env.ui) == (False, False) env.ctrl.set_config_loaded(True) - assert _states(env.ui) == (True, False, "Pause", False) + assert _states(env.ui) == (True, False) env.ctrl.set_config_loaded(False) - assert _states(env.ui) == (False, False, "Pause", False) + assert _states(env.ui) == (False, False) def test_full_cycle_through_button_clicks(env): env.ctrl.set_config_loaded(True) env.ui.btn_start_record.click() - assert env.ctrl.is_recording and not env.ctrl.is_paused + assert env.ctrl.is_recording assert env.recorder.start_recording.call_args[0][1].startswith("my_demo_") - assert _states(env.ui) == (False, True, "Pause", True) - - env.ui.btn_pause_record.click() - assert env.ctrl.is_paused and env.ctrl.is_recording - env.recorder.pause_recording.assert_called_once() - env.manager.set_status_for_all.assert_called_with("Paused") - assert _states(env.ui) == (False, True, "Resume", True) - - env.ui.btn_pause_record.click() # same slot, now resumes - assert not env.ctrl.is_paused - env.recorder.resume_recording.assert_called_once() - env.recorder.pause_recording.assert_called_once() # not called a second time - assert _states(env.ui) == (False, True, "Pause", True) - - env.ui.btn_pause_record.click() - env.ui.btn_stop_record.click() # stop while paused resets the label - assert not env.ctrl.is_recording and not env.ctrl.is_paused + assert _states(env.ui) == (False, True) + + env.ui.btn_stop_record.click() + assert not env.ctrl.is_recording env.recorder.stop_recording.assert_called_once() env.subscribers.clear_all_subscribers.assert_called_once() env.validator.validate.assert_called_once_with("/data/my_demo_1.h5") assert env.ctrl.session_h5_files == ["/data/my_demo_1.h5"] - assert _states(env.ui) == (True, False, "Pause", False) + assert _states(env.ui) == (True, False) env.ui.btn_start_record.click() # a second take works assert env.ctrl.is_recording @@ -126,7 +109,6 @@ def test_full_cycle_through_button_clicks(env): def test_guards(env): assert env.ctrl.stop_recording() is False # nothing to stop - assert env.ctrl.pause_recording() is False env.yaml_parser.is_loaded.return_value = False assert env.ctrl.start_recording() is False env.recorder.start_recording.assert_not_called() @@ -183,12 +165,12 @@ def test_save_runs_in_the_background_with_the_buttons_locked(env, monkeypatch): env.ctrl.set_config_loaded(True) env.ctrl.start_recording() assert env.ctrl.stop_recording() is True - assert _states(env.ui) == (False, False, "Pause", False) # nothing to click while the demo is written + assert _states(env.ui) == (False, False) # nothing to click while the demo is written assert env.ctrl.stop_recording() is False # a second Stop does not start a second save work, on_done = pending.pop() on_done(work(), None) # the thread finished assert not env.ctrl.is_recording and env.ctrl.session_h5_files == ["/data/my_demo_1.h5"] - assert _states(env.ui) == (True, False, "Pause", False) + assert _states(env.ui) == (True, False) def test_a_crash_in_the_save_thread_is_a_failed_stop(env): @@ -229,3 +211,14 @@ def test_demo_saved_by_a_retry_at_start_joins_the_session(env): env.ui.btn_start_record.click() env.ui.btn_stop_record.click() assert env.ctrl.session_h5_files == ["/data/my_demo_0.h5", "/data/my_demo_1.h5"] + + +def test_a_failed_recording_loop_is_reported_like_a_full_buffer(env): + env.recorder.buffer_full, env.recorder.loop_failed = False, False + env.ctrl._check_buffer_full() + env.box.error.assert_not_called() + env.recorder.loop_failed = True # the sampling thread gave up after repeated errors + env.ctrl._buffer_timer.start() + env.ctrl._check_buffer_full() + assert "Press Stop" in env.box.error.call_args.args[0] + assert not env.ctrl._buffer_timer.isActive() # told once diff --git a/tests/test_review_lerobot.py b/tests/test_review_lerobot.py deleted file mode 100644 index 26c65f1..0000000 --- a/tests/test_review_lerobot.py +++ /dev/null @@ -1,135 +0,0 @@ -"""Review page with a LeRobot dataset: episode list, camera image, state / action curves, view-only.""" - -from types import SimpleNamespace - -import h5py -import numpy as np -import pytest - -pytest.importorskip("PyQt5") -pytest.importorskip("pyqtgraph") -pytest.importorskip("av") -pytest.importorskip("pyarrow") -pytest.importorskip("cv2") -from PyQt5.QtWidgets import QWidget # noqa: E402 - -from relay_d.acquisition.qt_app.ui.postprocess.lerobot_writer import LeRobotDatasetWriter # noqa: E402 -from relay_d.acquisition.qt_app.ui.review import review_page as rp # noqa: E402 - -LENGTHS = [12, 7] - - -def _make_dataset(root, cameras=("cam_a", "cam_b")): - with LeRobotDatasetWriter(root, fps=10, robot_type="arm") as writer: - for ep, n in enumerate(LENGTHS): - rng = np.random.default_rng(ep) - images = {f"observation.images.{c}": rng.integers(0, 255, (n, 32, 48, 3), dtype=np.uint8) for c in cameras} - writer.add_episode(rng.normal(size=(n, 4)).astype(np.float32), rng.normal(size=(n, 2)).astype(np.float32), - images, task="stack", state_names=["j1", "j2", "j3", "j4"]) - - -@pytest.fixture -def make_page(qapp): - pages = [] - - def make(folder): - page = rp.ReviewPage(SimpleNamespace(page_review=QWidget())) - page.session_dir = str(folder) - page.refresh_demo_list() - pages.append(page) - return page - - yield make - for p in pages: - p._close_current_file() - p._close_lerobot() - - -def test_lists_episodes_of_a_dataset_in_the_folder(make_page, tmp_path): - _make_dataset(tmp_path / "lerobot_dataset_1") # the layout Post-process writes: dataset inside the folder - page = make_page(tmp_path) - assert page.list_demos.count() == 2 and not page.combo_source.isVisibleTo(page.combo_source.parent() or page.combo_source) - assert "Episode 0 · 12 frames · stack" in page.list_demos.item(0).text() and "PASS" in page.list_demos.item(0).text() - assert "2 episodes" in page.text_validation.toPlainText() and "cam_a" in page.text_validation.toPlainText() - assert not page.btn_delete.isEnabled() and not page.tab_widget.isTabEnabled(1) # view-only, no ROS playback - - -def test_selecting_an_episode_shows_camera_frames_and_curves(make_page, tmp_path): - _make_dataset(tmp_path / "ds") - page = make_page(tmp_path / "ds") # the dataset folder itself - page.list_demos.setCurrentRow(1) - assert [b.text().split("\n")[0] for b in page.channel_buttons] == ["Image", "observation.state", "action"] - assert page.slider_scrub.maximum() == 6 and page.label_frame_index.text().startswith("Frame 0 / 6") - assert page.label_image.pixmap() is not None and not page.label_image.pixmap().isNull() - assert [page.combo_camera.itemText(i) for i in range(page.combo_camera.count())] == [ - "observation.images.cam_a", "observation.images.cam_b"] - - page.slider_scrub.setValue(4) - assert page.label_frame_index.text().startswith("Frame 4 / 6") - page.combo_camera.setCurrentIndex(1) # another camera, same frame - assert page.label_frame_index.text().startswith("Frame 4 / 6") - - page.channel_buttons[1].click() # observation.state: 4 curves named after the joints, x axis in seconds - plot = page.layout_display_graph.itemAt(0).widget() - assert len(plot.getPlotItem().listDataItems()) == 4 - assert [i.name() for i in plot.getPlotItem().listDataItems()] == ["j1", "j2", "j3", "j4"] - assert page._playback_cursor_line.value() == pytest.approx(0.4) - page.slider_scrub.setValue(6) # the cursor follows the slider - assert page._playback_cursor_line.value() == pytest.approx(0.6) - - -def test_long_episodes_are_decimated(make_page, tmp_path, monkeypatch): - _make_dataset(tmp_path / "ds", cameras=("c",)) - real_get = rp.user_settings.get - monkeypatch.setattr(rp.user_settings, "get", lambda key, default=None: 5 if key == "review_plot_max_points" else real_get(key, default)) - page = make_page(tmp_path / "ds") - page.list_demos.setCurrentRow(0) - page.channel_buttons[1].click() - item = page.layout_display_graph.itemAt(0).widget().getPlotItem().listDataItems()[0] - assert len(item.xData) <= 5 + 1 - - -def test_h5_demos_and_a_dataset_in_the_same_folder_are_two_sources(make_page, tmp_path): - _make_dataset(tmp_path / "ds") - with h5py.File(tmp_path / "demo.h5", "w") as f: - f.create_group("data/demo_0") - page = make_page(tmp_path) - assert [page.combo_source.itemText(i) for i in range(2)] == ["Recorded demos (.h5)", "LeRobot: ds"] - assert page.btn_delete.isEnabled() # the first source is the h5 demos - page.combo_source.setCurrentIndex(1) - assert page.list_demos.count() == 2 and not page.btn_delete.isEnabled() - page.combo_source.setCurrentIndex(0) - assert page.btn_delete.isEnabled() and page.tab_widget.isTabEnabled(1) - - -def test_a_broken_dataset_is_reported_not_raised(make_page, tmp_path): - (tmp_path / "ds" / "meta").mkdir(parents=True) - (tmp_path / "ds" / "meta" / "info.json").write_text("{}") - page = make_page(tmp_path / "ds") - assert page.list_demos.count() == 0 and page.text_validation.toPlainText() - - -def test_the_robot_description_of_a_playback_is_remembered(make_page, tmp_path, monkeypatch): - """The setting says 'last playback': a typed path counts. (The ROS-only modules are replaced by stubs.)""" - import sys - import types - - package = "relay_d.acquisition.qt_app.ui.review" - resolver = types.ModuleType(f"{package}.robot_description_resolver") - resolver.resolve_from_file = lambda path: "" - resolver.RobotDescriptionError = type("RobotDescriptionError", (Exception,), {}) - player = types.ModuleType(f"{package}.data_player") - player.PlaybackState = SimpleNamespace(PAUSED="paused") - monkeypatch.setitem(sys.modules, resolver.__name__, resolver) - monkeypatch.setitem(sys.modules, player.__name__, player) - - with h5py.File(tmp_path / "demo.h5", "w") as f: - f.create_group("data/demo_0") - page = make_page(tmp_path) - page.list_demos.setCurrentRow(0) - saved, launched = {}, [] - monkeypatch.setattr(rp.config, "save_settings", lambda values: saved.update(values)) - monkeypatch.setattr(page, "_launch_playback", lambda demo, xml: launched.append((demo, xml))) - page.edit_urdf_path.setText("/robots/arm.urdf") - page._on_play_clicked() - assert saved == {"review_last_robot_description": "/robots/arm.urdf"} and len(launched) == 1 diff --git a/tests/test_review_page.py b/tests/test_review_page.py new file mode 100644 index 0000000..0787e98 --- /dev/null +++ b/tests/test_review_page.py @@ -0,0 +1,55 @@ +"""Review page: recorded-demo list and playback settings.""" + +from types import SimpleNamespace + +import h5py +import pytest + +pytest.importorskip("PyQt5") +pytest.importorskip("pyqtgraph") +pytest.importorskip("cv2") +from PyQt5.QtWidgets import QWidget # noqa: E402 + +from relay_d.acquisition.qt_app.ui.review import review_page as rp # noqa: E402 + + +@pytest.fixture +def make_page(qapp): + pages = [] + + def make(folder): + page = rp.ReviewPage(SimpleNamespace(page_review=QWidget())) + page.session_dir = str(folder) + page.refresh_demo_list() + pages.append(page) + return page + + yield make + for p in pages: + p._close_current_file() + + +def test_the_robot_description_of_a_playback_is_remembered(make_page, tmp_path, monkeypatch): + """The setting says 'last playback': a typed path counts. (The ROS-only modules are replaced by stubs.)""" + import sys + import types + + package = "relay_d.acquisition.qt_app.ui.review" + resolver = types.ModuleType(f"{package}.robot_description_resolver") + resolver.resolve_from_file = lambda path: "" + resolver.RobotDescriptionError = type("RobotDescriptionError", (Exception,), {}) + player = types.ModuleType(f"{package}.data_player") + player.PlaybackState = SimpleNamespace(PAUSED="paused") + monkeypatch.setitem(sys.modules, resolver.__name__, resolver) + monkeypatch.setitem(sys.modules, player.__name__, player) + + with h5py.File(tmp_path / "demo.h5", "w") as f: + f.create_group("data/demo_0") + page = make_page(tmp_path) + page.list_demos.setCurrentRow(0) + saved, launched = {}, [] + monkeypatch.setattr(rp.config, "save_settings", lambda values: saved.update(values)) + monkeypatch.setattr(page, "_launch_playback", lambda demo, xml: launched.append((demo, xml))) + page.edit_urdf_path.setText("/robots/arm.urdf") + page._on_play_clicked() + assert saved == {"review_last_robot_description": "/robots/arm.urdf"} and len(launched) == 1 diff --git a/tests/test_run_page.py b/tests/test_run_page.py new file mode 100644 index 0000000..aedc3ec --- /dev/null +++ b/tests/test_run_page.py @@ -0,0 +1,91 @@ +"""Run tab: command line, and Start / Stop on a stand-in process (no policy, no ROS).""" + +import shutil +import sys +import time +from types import SimpleNamespace + +import pytest + +pytest.importorskip("PyQt5") +from PyQt5.QtWidgets import QApplication, QWidget # noqa: E402 + +from relay_d.acquisition.qt_app.ui.run import run_page as rp # noqa: E402 + +# Prints until SIGINT, then exits 0 like the node's Ctrl+C path. +STAND_IN = ("import time\ntry:\n while True:\n print('tick', flush=True)\n time.sleep(0.05)\n" + "except KeyboardInterrupt:\n print('safe stop sent', flush=True)\n") + + +def _wait(cond, seconds=10): + deadline = time.time() + seconds + while not cond() and time.time() < deadline: + QApplication.processEvents() + time.sleep(0.02) + QApplication.processEvents() + return cond() + + +@pytest.fixture +def page(qapp, tmp_path, monkeypatch): + saved = [] + monkeypatch.setattr(rp.config, "save_settings", saved.append) # do not touch ~/.relayd_settings.json + disp = tmp_path / "out" / "action_dispatcher" + disp.mkdir(parents=True) + ckpt = tmp_path / "model.pth" + ckpt.write_text("") + p = rp.RunPage(SimpleNamespace(page_run=QWidget())) + p.saved, p.ckpt, p.disp = saved, str(ckpt), disp + yield p + p.shutdown() + + +def test_args_dry_run_and_hz(): + args = rp.build_args("/c.pth", "/d", "cpu", "", True) + assert args[:2] == ["-m", "relay_d.dispatch.inference_node"] + assert "--dry-run" in args and "--hz" not in args + assert args[args.index("--obs-config") + 1] == "/d/obs_config.yaml" + args = rp.build_args("/c.pth", "/d", "cuda", " 30 ", False) + assert "--dry-run" not in args and args[args.index("--hz") + 1] == "30" + + +def test_start_needs_both_dispatch_files_and_postprocess_folder_maps_to_subfolder(page): + page.edit_checkpoint.setText(page.ckpt) + assert rp.dispatch_dir_for(str(page.disp.parent)) == str(page.disp) + page.edit_dispatch.setText(str(page.disp)) + assert not page.btn_start.isEnabled() and "obs_config.yaml and action_config.yaml" in page.label_reason.text() + (page.disp / "obs_config.yaml").write_text("") + (page.disp / "action_config.yaml").write_text("") + page._update_buttons() + assert page.btn_start.isEnabled() and page.label_reason.text() == "" + assert page.check_dry_run.isChecked() + assert "--dry-run" in page.command()[1] + + +def test_start_then_stop_sends_sigint_and_exits_cleanly(page, monkeypatch): + page.edit_checkpoint.setText(page.ckpt) + monkeypatch.setattr(page, "command", lambda: (sys.executable, ["-c", STAND_IN])) + page.start() + assert page.saved and page.label_state.text() == "Running" + assert not page.btn_start.isEnabled() and page.btn_stop.isEnabled() + assert _wait(lambda: "tick" in page.log.toPlainText().splitlines()) # the child is up (its handler is set) + page.stop() + assert _wait(lambda: not page.is_running()) + assert page.proc.exitCode() == 0 and page.label_state.text() == "Stopped" + assert "safe stop sent" in page.log.toPlainText() and not page.btn_stop.isEnabled() + + +GROUP_PROBE = ("import os, time\nprint('pid', os.getpid(), os.getpgid(0) == os.getpid(), flush=True)\n" + STAND_IN) + + +@pytest.mark.skipif(not shutil.which("setsid"), reason="no setsid (util-linux)") +def test_node_runs_in_its_own_process_group_and_stop_still_reaches_it(page, monkeypatch): + page.edit_checkpoint.setText(page.ckpt) + monkeypatch.setattr(page, "command", lambda: (sys.executable, ["-c", GROUP_PROBE])) + page.start() + assert _wait(lambda: "tick" in page.log.toPlainText().splitlines()) + # a terminal Ctrl+C reaches only the GUI (which stops the node once), and the PID is the node's, not setsid's + assert f"pid {page.proc.processId()} True" in page.log.toPlainText() + page.stop() + assert _wait(lambda: not page.is_running()) + assert page.proc.exitCode() == 0 and "safe stop sent" in page.log.toPlainText() diff --git a/tests/test_settings_consumers.py b/tests/test_settings_consumers.py index 500cfad..3bf36ae 100644 --- a/tests/test_settings_consumers.py +++ b/tests/test_settings_consumers.py @@ -31,18 +31,11 @@ def _yaml_parser(config=None): # ------------------------------------------------------------------ recorder -def test_recorder_uses_the_settings_when_the_yaml_is_silent(recorder_cls, settings): - settings({"rec_default_frequency": 12, "rec_default_max_buffer_mb": 128, - "rec_default_sync_mode": "strict", "rec_default_sync_tolerance_sec": 0.25}) +def test_recorder_takes_the_buffer_setting_and_the_frequency_only_from_the_yaml(recorder_cls, settings): + settings({"rec_default_max_buffer_mb": 128, "rec_default_frequency": 12}) # an old settings file: the key is ignored rec = recorder_cls(_yaml_parser({})) - assert rec.recording_frequency == 12 and rec.max_buffer_bytes == 128 * 1024**2 - assert (rec._sync_mode, rec._sync_tolerance) == ("strict", 0.25) - - -def test_recorder_yaml_wins_over_the_settings(recorder_cls, settings): - settings({"rec_default_frequency": 12}) - rec = recorder_cls(_yaml_parser({"recording_frequency": 5})) - assert rec.recording_frequency == 5 + assert rec.max_buffer_bytes == 128 * 1024**2 + assert rec.recording_frequency is None and (rec._sync_mode, rec._sync_tolerance) == ("soft", None) def test_recorder_sync_tolerance_zero_means_off(recorder_cls, settings): @@ -92,14 +85,13 @@ def test_log_level_setting_and_environment_override(settings, monkeypatch): def test_postprocess_defaults_come_from_settings_and_the_file_wins(settings, tmp_path): from relay_d.acquisition.qt_app.ui.postprocess.postprocess_options import PostprocessOptions - settings({"postprocess_default_training_model": "diffusion_policy", "split_train": 0.6, "split_val": 0.3}) + settings({"split_train": 0.6, "split_val": 0.3}) opts = PostprocessOptions() - assert opts.training_model() == "diffusion_policy" - assert opts.splits() == {"train_frac": 0.6, "val_frac": 0.3, "test_frac": 0.1} + assert opts.splits() == {"train_frac": 0.6, "val_frac": 0.3} f = tmp_path / "postprocess_config.yaml" - f.write_text("postprocess:\n robomimic:\n training_model: bc_rnn_gmm\n splits: {train_frac: 0.5, val_frac: 0.5, test_frac: 0.0}\n") + f.write_text("postprocess:\n robomimic:\n splits: {train_frac: 0.5, val_frac: 0.5}\n") loaded, _ = PostprocessOptions.load(str(f)) - assert loaded.training_model() == "bc_rnn_gmm" and loaded.splits()["train_frac"] == 0.5 + assert loaded.splits()["train_frac"] == 0.5 def test_lerobot_encoder_settings_reach_the_dataset(settings, tmp_path): diff --git a/tests/test_settings_live.py b/tests/test_settings_live.py index 5535781..89ae01f 100644 --- a/tests/test_settings_live.py +++ b/tests/test_settings_live.py @@ -18,21 +18,24 @@ def test_one_store_and_other_pages_saves_show_in_settings(qapp, settings_file): assert config.get("python_lerobot") == user_settings.get("python_lerobot") == "/venv/bin/python" if "python_lerobot" in page.widgets: assert page.widgets["python_lerobot"][1]() == "/venv/bin/python" - page.widgets["rec_default_frequency"][2](42) # an unsaved edit elsewhere survives another page's save + page.widgets["rec_default_max_buffer_mb"][2](420) # an unsaved edit elsewhere survives another page's save config.save_settings({"review_default_fixed_frame": "base_link"}) - assert page.widgets["rec_default_frequency"][1]() == 42 + assert page.widgets["rec_default_max_buffer_mb"][1]() == 420 assert page.widgets["review_default_fixed_frame"][1]() == "base_link" -def test_recorder_uses_the_current_defaults_at_each_start(recorder_cls, settings_file): - rec = recorder_cls(SimpleNamespace(is_loaded=lambda: False, yaml_data=None)) - user_settings.save({"rec_default_frequency": 17, "rec_default_max_buffer_mb": 64}) - rec._apply_setting_defaults() # what start_recording does first - assert rec.recording_frequency == 17 and rec.max_buffer_bytes == 64 * 1024**2 +def test_recorder_uses_the_current_buffer_setting_at_each_start(recorder_cls, settings_file): + config = {"config": {"recording_frequency": 0}} + rec = recorder_cls(SimpleNamespace(is_loaded=lambda: True, yaml_data=config, get_metadata=lambda: {}, + get_current_config_path=lambda: "c.yaml", get_input_data=lambda: [], + get_streams=lambda: {})) + user_settings.save({"rec_default_max_buffer_mb": 64}) + assert rec.start_recording(SimpleNamespace()) is False # 0 Hz is refused, after the settings were read + assert rec.max_buffer_bytes == 64 * 1024**2 def test_save_button_saves_the_values_on_screen(qapp, settings_file): page = SettingsPage(SimpleNamespace(page_settings=QWidget())) - page.widgets["rec_default_frequency"][2](25) + page.widgets["rec_default_max_buffer_mb"][2](250) page.btn_save_settings.click() # `clicked` passes `checked`; it must not be taken as the keys to save - assert user_settings.get("rec_default_frequency") == 25 + assert user_settings.get("rec_default_max_buffer_mb") == 250 diff --git a/tests/test_settings_page.py b/tests/test_settings_page.py index 09c2375..67c2e54 100644 --- a/tests/test_settings_page.py +++ b/tests/test_settings_page.py @@ -33,11 +33,11 @@ def test_every_setting_has_an_editor_and_shows_its_value(page): def test_save_writes_the_file_emits_and_notes_restart_only_changes(page): seen = [] config.settings_changed.connect(seen.append) - page.widgets["rec_default_frequency"][2](45) + page.widgets["rec_default_max_buffer_mb"][2](450) page.widgets["theme"][2]("dark") page.widgets["ros_domain_id"][2](7) assert page.save_settings_from_ui() is True - assert json.loads(page.path.read_text())["rec_default_frequency"] == 45 + assert json.loads(page.path.read_text())["rec_default_max_buffer_mb"] == 450 assert us.get("theme") == "dark" and config.get("theme") == "dark" assert seen and seen[-1]["theme"] == "dark" assert "Restart" in page.label_status.text() and "ROS domain" in page.label_status.text() @@ -51,11 +51,11 @@ def test_live_only_changes_do_not_ask_for_a_restart(page): def test_reset_one_tab_only_resets_that_tab(page): - page.widgets["rec_default_frequency"][2](99) + page.widgets["rec_default_max_buffer_mb"][2](990) page.widgets["infer_hz"][2](33.0) page.save_settings_from_ui() page.reset_to_default("Recording") - assert us.get("rec_default_frequency") == 30 and us.get("infer_hz") == 33.0 + assert us.get("rec_default_max_buffer_mb") == 4096 and us.get("infer_hz") == 33.0 page.reset_to_default(None) assert us.get("infer_hz") == 10.0 @@ -73,16 +73,16 @@ def test_load_settings_shows_saved_values_and_notifies(page): def test_saving_unchanged_settings_keeps_the_file_free_of_defaults(page): page.save_settings_from_ui() assert not page.path.exists() or json.loads(page.path.read_text()) == {} - page.widgets["rec_default_frequency"][2](45) + page.widgets["rec_default_max_buffer_mb"][2](450) page.save_settings_from_ui() - assert json.loads(page.path.read_text()) == {"rec_default_frequency": 45} + assert json.loads(page.path.read_text()) == {"rec_default_max_buffer_mb": 450} def test_resetting_one_tab_neither_saves_nor_discards_edits_on_other_tabs(page): - page.widgets["rec_default_frequency"][2](60) # an edit on the Recording tab, not saved yet + page.widgets["rec_default_max_buffer_mb"][2](600) # an edit on the Recording tab, not saved yet page.widgets["infer_hz"][2](33.0) page.save_settings_from_ui(keys=["infer_hz"]) page.reset_to_default("Inference") assert us.get("infer_hz") == 10.0 - assert us.get("rec_default_frequency") == 30 # was saved by the old reset - assert page.widgets["rec_default_frequency"][1]() == 60 # and the edit is still on screen + assert us.get("rec_default_max_buffer_mb") == 4096 # was saved by the old reset + assert page.widgets["rec_default_max_buffer_mb"][1]() == 600 # and the edit is still on screen diff --git a/tests/test_spin.py b/tests/test_spin.py new file mode 100644 index 0000000..7e5287a --- /dev/null +++ b/tests/test_spin.py @@ -0,0 +1,89 @@ +"""TopicSubscribers' spin thread, shared by the GUI's Record page and AppAPI. Needs a sourced ROS 2 (rclpy).""" + +import statistics +import sys +import threading +import time +from types import SimpleNamespace +from unittest.mock import MagicMock + +import pytest + +rclpy = pytest.importorskip("rclpy") +from rclpy.executors import SingleThreadedExecutor # noqa: E402 +from sensor_msgs.msg import JointState # noqa: E402 + +from relay_d.acquisition.qt_app.ui.record.topic_subscribers import TopicSubscribers # noqa: E402 + + +@pytest.fixture +def ros(): + rclpy.init() + yield + rclpy.shutdown() + + +def test_the_spin_thread_keeps_up_with_four_topics_at_100_hz(ros): + topics = [f"/spin_test_{i}" for i in range(4)] + pub_node = rclpy.create_node("spin_test_pub") + pubs = [pub_node.create_publisher(JointState, t, 10) for t in topics] + + def publish(): + for p in pubs: + msg = JointState() + msg.header.stamp = pub_node.get_clock().now().to_msg() + p.publish(msg) + + pub_node.create_timer(0.01, publish) + executor = SingleThreadedExecutor() + executor.add_node(pub_node) + threading.Thread(target=executor.spin, daemon=True).start() + + subs = TopicSubscribers() + try: + subs.start_spinning() # before subscribing, as in the GUI and AppAPI: new subscriptions must still be served + containers = [SimpleNamespace(container_id=f"c{i}", topic_path=t, input_name=f"c{i}") for i, t in enumerate(topics)] + assert all(subs.create_subscribers_for_containers(containers).values()) + time.sleep(1.0) + counts0 = {c.container_id: subs.get_message_count(c.container_id) for c in containers} + ages, t0 = [], time.time() + while time.time() - t0 < 2.0: + _, stamps = subs.get_content_and_timestamps() + ages += [time.time() - s for s in stamps.values()] + time.sleep(0.007) + handled = sum(subs.get_message_count(c.container_id) - counts0[c.container_id] for c in containers) / (time.time() - t0) + median_ms = statistics.median(ages) * 1000 + print(f"spin thread: {handled:.0f} msg/s handled of ~400, median age {median_ms:.1f} ms") + assert median_ms < 30 # the GUI's 10 ms Qt-timer spin_once: ~98 ms + assert handled > 300 # the Qt timer: ~78 msg/s + finally: + subs.close() + executor.shutdown() + pub_node.destroy_node() + assert not subs._spin_thread.is_alive() + + +def test_the_record_page_spins_its_subscribers_in_their_own_thread_not_the_qt_timer(qapp, ros, monkeypatch, tmp_path): + for name in ("PyQt5.QtMultimedia", "PyQt5.QtMultimediaWidgets"): # not in every PyQt5 install; unused here + try: + __import__(name) + except ImportError: + monkeypatch.setitem(sys.modules, name, MagicMock()) + from relay_d.acquisition.qt_app.ui.config.yaml_parser import YamlParser + from relay_d.acquisition.qt_app.ui.record.record_page import RecordPage + + ros_nodes = [] # the app's 10 ms Qt-timer list (app.register_ros_node) + ui = MagicMock() + ui.register_ros_node = ros_nodes.append + page = RecordPage(ui, YamlParser()) + try: + page.container_manager = MagicMock() + cfg = tmp_path / "cfg.yaml" + cfg.write_text("config:\n dataset_name: d\n recording_frequency: 10\n input: {}\n") + page._on_config_loaded(str(cfg)) + assert page.topic_subscribers not in ros_nodes # was registered: one callback per 10 ms tick + assert page.topic_subscribers._spin_thread.is_alive() + finally: + page.tf_update_timer.stop() + page.topic_subscribers.close() # what MainWindow.closeEvent calls + assert not page.topic_subscribers._spin_thread.is_alive() diff --git a/tests/test_train_page.py b/tests/test_train_page.py index c97db6d..599a95f 100644 --- a/tests/test_train_page.py +++ b/tests/test_train_page.py @@ -12,7 +12,7 @@ from PyQt5.QtWidgets import QApplication, QWidget # noqa: E402 from relay_d.acquisition.qt_app.ui.train import train_page as tp # noqa: E402 -from relay_d.training import runs # noqa: E402 +from relay_d.training import launcher, runs # noqa: E402 def _settle(page, seconds=10): @@ -48,7 +48,7 @@ def page(qapp, tmp_path, monkeypatch): def test_loading_a_folder_fills_policy_and_parameters(page): page.load_folder(str(page.folder_path)) - assert page.combo_policy.itemText(0) == tp._FROM_FOLDER + assert [page.combo_policy.itemText(i) for i in range(page.combo_policy.count())] == launcher.ROBOMIMIC_POLICIES assert {"epochs", "batch_size", "learning_rate", "device"} <= set(page._param_widgets) assert page.btn_start.isEnabled() and "1 episodes" in page.label_summary.text() page.combo_policy.setCurrentText("diffusion_policy") # another template: the form is rebuilt diff --git a/tests/test_training_launcher.py b/tests/test_training_launcher.py index 4847dd4..68ba5e6 100644 --- a/tests/test_training_launcher.py +++ b/tests/test_training_launcher.py @@ -235,16 +235,24 @@ def test_run_falls_back_to_background_when_every_terminal_fails(folder, tmp_path def test_the_config_files_of_the_selected_dataset_are_used(tmp_path): """relay-d-convert writes _acquisition_config.yaml, so one folder can hold several datasets.""" - for name, model in (("a", "bc_rnn_gmm"), ("b", "diffusion_policy")): + for name in ("a", "b"): _robomimic_file(tmp_path / f"{name}.h5") - (tmp_path / f"{name}_postprocess_config.yaml").write_text(f"postprocess:\n robomimic:\n training_model: {model}\n") + (tmp_path / f"{name}_acquisition_config.yaml").write_text("config: {}\n") by_name = {d.path.stem: d for d in launcher.find_datasets(tmp_path)} - assert launcher._robomimic_policy(by_name["b"], None) == "diffusion_policy" # not the alphabetically first file - assert launcher._robomimic_policy(by_name["a"], None) == "bc_rnn_gmm" - (tmp_path / "postprocess_config.yaml").write_text("postprocess:\n robomimic:\n training_model: bc_transformer_gmm\n") + assert launcher._config_file(by_name["b"], "acquisition_config.yaml").name == "b_acquisition_config.yaml" + assert launcher._config_file(by_name["a"], "acquisition_config.yaml").name == "a_acquisition_config.yaml" + (tmp_path / "acquisition_config.yaml").write_text("config: {}\n") _robomimic_file(tmp_path / "c.h5") c = next(d for d in launcher.find_datasets(tmp_path) if d.path.stem == "c") - assert launcher._robomimic_policy(c, None) == "bc_transformer_gmm" # no c_* file: the plain one the GUI writes + assert launcher._config_file(c, "acquisition_config.yaml").name == "acquisition_config.yaml" # the plain one the GUI writes + + +def test_robomimic_policy_is_chosen_on_the_train_tab_not_by_postprocess_options(folder): + """robomimic.training_model was removed: a stray one in the folder changes nothing; no policy = the first template.""" + (folder / "postprocess_config.yaml").write_text("postprocess:\n robomimic:\n training_model: diffusion_policy\n") + ds = _robomimic(folder) + assert launcher._robomimic_policy(ds, None) == launcher.ROBOMIMIC_POLICIES[0] + assert launcher._robomimic_policy(ds, "diffusion_policy") == "diffusion_policy" def test_ctrl_c_in_the_training_window_ends_the_run_and_closes_the_window(folder, tmp_path): diff --git a/tests/test_urdf_limits.py b/tests/test_urdf_limits.py index fd205ba..2924ef6 100644 --- a/tests/test_urdf_limits.py +++ b/tests/test_urdf_limits.py @@ -19,13 +19,3 @@ def test_parse_joint_limits_order_filter_and_errors(): with pytest.raises(ValueError): parse_joint_limits("not xml") - -def test_limits_for_position_velocity_and_errors(): - from relay_d.acquisition.qt_app.ui.config.builder.urdf_limits import limits_for - - joints = parse_joint_limits(URDF) - assert limits_for("position", joints, ["j2", "j1"]) == [{"lower_lim": 0.0, "upper_lim": 0.1}, {"lower_lim": -1.5, "upper_lim": 1.5}] - assert limits_for("velocity", joints, ["j1"]) == [{"lower_lim": -2.0, "upper_lim": 2.0}] - for args in (("position", ["nope"]), ("acceleration", ["j1"])): - with pytest.raises(ValueError): - limits_for(args[0], joints, args[1]) diff --git a/tests/test_user_settings.py b/tests/test_user_settings.py index 9d9068f..56d0f88 100644 --- a/tests/test_user_settings.py +++ b/tests/test_user_settings.py @@ -21,31 +21,31 @@ def test_table_is_consistent(): def test_defaults_without_a_file(settings_file): assert not settings_file.exists() - assert us.get("rec_default_frequency") == 30 and us.get("theme") == "light" + assert us.get("rec_default_max_buffer_mb") == 4096 and us.get("theme") == "light" assert us.values() == {**us.defaults()} def test_save_merges_keeps_unknown_keys_and_leaves_no_temp_file(settings_file): settings_file.write_text(json.dumps({"some_future_key": 1, "theme": "dark"})) us.reload() - result = us.save({"rec_default_frequency": 60}) + result = us.save({"rec_default_max_buffer_mb": 60}) on_disk = json.loads(settings_file.read_text()) - assert on_disk == {"some_future_key": 1, "theme": "dark", "rec_default_frequency": 60} - assert result["rec_default_frequency"] == 60 and us.get("theme") == "dark" + assert on_disk == {"some_future_key": 1, "theme": "dark", "rec_default_max_buffer_mb": 60} + assert result["rec_default_max_buffer_mb"] == 60 and us.get("theme") == "dark" assert [p.name for p in settings_file.parent.iterdir()] == ["settings.json"] def test_invalid_values_are_rejected_on_save_and_ignored_on_load(settings_file): with pytest.raises(ValueError): - us.save({"rec_default_frequency": "fast"}) + us.save({"rec_default_max_buffer_mb": "fast"}) with pytest.raises(ValueError): us.save({"theme": "purple"}) with pytest.raises(ValueError): us.save({"confirm_destructive": 1}) # an int is not a bool assert not settings_file.exists() - settings_file.write_text(json.dumps({"rec_default_frequency": "x", "theme": "dark"})) + settings_file.write_text(json.dumps({"rec_default_max_buffer_mb": "x", "theme": "dark"})) us.reload() - assert us.get("rec_default_frequency") == 30 and us.get("theme") == "dark" + assert us.get("rec_default_max_buffer_mb") == 4096 and us.get("theme") == "dark" def test_corrupt_or_wrong_file_means_defaults(settings_file): @@ -77,11 +77,11 @@ def test_restart_keys_and_compression_kwargs(settings_file): def test_values_equal_to_the_default_are_not_stored(settings_file): - us.save({"rec_default_frequency": 60, "theme": "light", "infer_hz": 10.0}) - assert json.loads(settings_file.read_text()) == {"rec_default_frequency": 60} - us.save({"rec_default_frequency": 30}) # back to the default: the key disappears + us.save({"rec_default_max_buffer_mb": 60, "theme": "light", "infer_hz": 10.0}) + assert json.loads(settings_file.read_text()) == {"rec_default_max_buffer_mb": 60} + us.save({"rec_default_max_buffer_mb": 4096}) # back to the default: the key disappears assert json.loads(settings_file.read_text()) == {} - assert us.get("rec_default_frequency") == 30 + assert us.get("rec_default_max_buffer_mb") == 4096 def test_resetting_a_migrated_setting_does_not_bring_the_old_value_back(settings_file): From 7e63f5b76cade7dd4733c8bba079bb9d4029436e Mon Sep 17 00:00:00 2001 From: pascd Date: Tue, 3 Nov 2026 14:57:26 +0000 Subject: [PATCH 2/2] Refactor start_demo function into its own module and update imports --- .github/workflows/ci.yml | 2 +- relay_d/acquisition/api/api.py | 3 ++- .../acquisition/qt_app/ui/record/data_recorder.py | 12 ------------ .../acquisition/qt_app/ui/record/demo_start.py | 15 +++++++++++++++ .../qt_app/ui/record/recording_controller.py | 2 +- 5 files changed, 19 insertions(+), 15 deletions(-) create mode 100644 relay_d/acquisition/qt_app/ui/record/demo_start.py diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index f71bfeb..bc22393 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -71,7 +71,7 @@ jobs: - name: Install test dependencies run: | apt-get update - apt-get install -y --no-install-recommends python3-pip python3-pytest python3-opencv python3-pyqt5 + apt-get install -y --no-install-recommends python3-pip python3-pytest python3-opencv python3-pyqt5 ros-jazzy-example-interfaces pip install --break-system-packages h5py "pyarrow<21" av pyqtgraph # pyarrow 21+ needs NumPy 2 - name: Run tests diff --git a/relay_d/acquisition/api/api.py b/relay_d/acquisition/api/api.py index aad4124..d42f1a7 100644 --- a/relay_d/acquisition/api/api.py +++ b/relay_d/acquisition/api/api.py @@ -6,7 +6,8 @@ from ..qt_app.ui.config.yaml_parser import YamlParser from ..qt_app.ui.record.topic_subscribers import TopicSubscribers -from ..qt_app.ui.record.data_recorder import DataRecorder, start_demo +from ..qt_app.ui.record.data_recorder import DataRecorder +from ..qt_app.ui.record.demo_start import start_demo from ..qt_app.ui.config.builder.config_check import check_config from ..qt_app.utils.stream_builder import StreamBuilder from relay_d.utils.tf_topic import TfTopicSubscriptionError diff --git a/relay_d/acquisition/qt_app/ui/record/data_recorder.py b/relay_d/acquisition/qt_app/ui/record/data_recorder.py index 08f76d9..607975b 100644 --- a/relay_d/acquisition/qt_app/ui/record/data_recorder.py +++ b/relay_d/acquisition/qt_app/ui/record/data_recorder.py @@ -21,18 +21,6 @@ def _sample_nbytes(sample: dict) -> int: return sum(v.nbytes for v in sample.values() if isinstance(v, np.ndarray)) -def start_demo(recorder, topic_subscribers, containers, dataset_name, demo_name=None): - """The per-Start steps the GUI and AppAPI share: subscribe every input again (re-detects the message type and QoS, - resets custom TF buffers; a broken custom TF topic raises TfTopicSubscriptionError), then start the recorder (it - flushes the data from before Start and first retries an unsaved demo, see ``recorder.retried_file``). - Returns the demo name, or None if the recording did not start. An input that could not be subscribed only logs: - the recorder holds it empty until data arrives (as the GUI always did).""" - results = topic_subscribers.create_subscribers_for_containers(containers) - logger.info(f"Subscribers created: {sum(results.values())}/{len(results)}") - demo_name = demo_name or f"{dataset_name}_{time.time_ns() // 1_000_000}" # ms: two Starts in one second must not clash ("w-") - return demo_name if recorder.start_recording(topic_subscribers, demo_name) else None - - def _write_stacked(group, name: str, frames: list): """Write equally shaped arrays as one (N, ...) dataset, frame by frame. diff --git a/relay_d/acquisition/qt_app/ui/record/demo_start.py b/relay_d/acquisition/qt_app/ui/record/demo_start.py new file mode 100644 index 0000000..621a2bc --- /dev/null +++ b/relay_d/acquisition/qt_app/ui/record/demo_start.py @@ -0,0 +1,15 @@ +import time + +from relay_d.utils.coloring_logger import logger + + +def start_demo(recorder, topic_subscribers, containers, dataset_name, demo_name=None): + """The per-Start steps the GUI and AppAPI share: subscribe every input again (re-detects the message type and QoS, + resets custom TF buffers; a broken custom TF topic raises TfTopicSubscriptionError), then start the recorder (it + flushes the data from before Start and first retries an unsaved demo, see ``recorder.retried_file``). + Returns the demo name, or None if the recording did not start. An input that could not be subscribed only logs: + the recorder holds it empty until data arrives (as the GUI always did).""" + results = topic_subscribers.create_subscribers_for_containers(containers) + logger.info(f"Subscribers created: {sum(results.values())}/{len(results)}") + demo_name = demo_name or f"{dataset_name}_{time.time_ns() // 1_000_000}" # ms: two Starts in one second must not clash ("w-") + return demo_name if recorder.start_recording(topic_subscribers, demo_name) else None diff --git a/relay_d/acquisition/qt_app/ui/record/recording_controller.py b/relay_d/acquisition/qt_app/ui/record/recording_controller.py index fe5ab60..f17c1a3 100644 --- a/relay_d/acquisition/qt_app/ui/record/recording_controller.py +++ b/relay_d/acquisition/qt_app/ui/record/recording_controller.py @@ -6,7 +6,7 @@ from relay_d.utils.tf_topic import TfTopicSubscriptionError from ..review.demo_validator import CheckStatus -from .data_recorder import start_demo +from .demo_start import start_demo from ...utils.background import run_in_background from ...utils.custom_message_box import CustomMessageBox