diff --git a/Justfile b/Justfile index d8687a5..1ea1a15 100644 --- a/Justfile +++ b/Justfile @@ -164,15 +164,16 @@ docker-build-x86: # (ros-tee / perception-tee / nvmm-source) moved to openral-pro with it. # GStreamer-free deploy smoke inside the open x86 image: asserts `import gi` -# is absent, cv2 / feetech (scservo_sdk) / onnxruntime / omdet are present, and -# `deploy run --config so101_bench.yaml --dry-run` resolves the launch. This is -# what CI (`docker-build.yml`) runs on the built image. Exits non-zero on failure. +# is absent, cv2 / feetech (scservo_sdk) / onnxruntime / omdet plus the LingBot +# msgpack/websocket protocol dependencies are present, and `deploy run --config +# so101_bench.yaml --dry-run` resolves the launch. This is what CI +# (`docker-build.yml`) runs on the built image. Exits non-zero on failure. docker-smoke-x86-deploy: docker-build-x86 docker run --rm --entrypoint /entrypoint.sh openral:x86 \ bash -lc 'set -e; \ - python -c "import cv2, scservo_sdk, onnxruntime; from transformers import AutoModelForZeroShotObjectDetection; \ + python -c "import cv2, msgpack, scservo_sdk, onnxruntime, websockets.sync.client; from transformers import AutoModelForZeroShotObjectDetection; \ import importlib.util as u; assert u.find_spec(\"gi\") is None, \"gi present — expected gstreamer-free\"; \ - print(\"OK: cv2\", cv2.__version__, \"| feetech + onnxruntime + omdet present | gi absent\")"; \ + print(\"OK: cv2\", cv2.__version__, \"| feetech + onnxruntime + omdet + LingBot protocol present | gi absent\")"; \ openral deploy run --config scenes/deploy/so101_bench.yaml --dry-run | grep -q "hal_mode:=real" \ && echo "OK: deploy run --dry-run resolves"' @@ -395,6 +396,7 @@ ros2-build: --packages-select openral_msgs \ opentelemetry_cpp_vendor \ openral_hal_so100 \ + openral_hal_galaxea_a1 \ openral_hal_panda_mobile \ openral_hal_openarm \ openral_hal_franka \ @@ -494,6 +496,7 @@ ros2-test: --packages-select openral_msgs \ opentelemetry_cpp_vendor \ openral_hal_so100 \ + openral_hal_galaxea_a1 \ openral_hal_panda_mobile \ openral_hal_openarm \ openral_hal_franka \ diff --git a/README.md b/README.md index 83b8567..8c2a575 100644 --- a/README.md +++ b/README.md @@ -46,7 +46,7 @@ We compose ROS 2, tf2, MoveIt 2 (with optional CUDA-accelerated **cuMotion** pla **Shipped today** (all workspace packages at `0.1.0`): - `openral_core` schemas + the `openral` CLI (bare `openral` drops into a REPL) -- HAL adapters for [17 robot platforms](docs/reference/robots.md) — manipulators, mobile manipulators, bimanual arms, humanoids +- HAL adapters for [18 robot platforms](docs/reference/robots.md) — manipulators, mobile manipulators, bimanual arms, humanoids - [Sensor catalog](docs/reference/sensors_landscape.md) — RGB-D, F/T, and USB-UVC adapters - `WorldStateAggregator` — 30 Hz tf2-aware snapshot with lifted object detections - [rSkill packages](docs/reference/rskills.md) spanning every kind — VLA policies (SmolVLA, π0.5, xVLA, MolmoAct2, ACT, Diffusion Policy, 3D Diffuser Actor, RLDX-1, OpenVLA-OFT, GR00T N1.7), open-vocabulary detectors (RT-DETR, OmDet-Turbo, LocateAnything), the Qwen3.5-4B scene VLM (`kind: vlm`), the Robometer-4B reward/progress monitor (`kind: reward`), MoveIt / Nav2 classical-control skills (`kind: ros_action`), and human-authored reasoner playbooks (`kind: playbook`) diff --git a/docker/galaxea_a1_sidecar/Dockerfile b/docker/galaxea_a1_sidecar/Dockerfile new file mode 100644 index 0000000..681b908 --- /dev/null +++ b/docker/galaxea_a1_sidecar/Dockerfile @@ -0,0 +1,40 @@ +FROM ros:noetic-ros-base-focal + +ENV DEBIAN_FRONTEND=noninteractive +ENV LANG=C.UTF-8 +ENV LC_ALL=C.UTF-8 + +# The vendor SDK is never copied into this image. Operators mount a separately +# obtained, already-built A1_SDK tree read-only at run time. +RUN apt-get update && apt-get install -y --no-install-recommends \ + libconsole-bridge0.4 \ + libassimp5 \ + liboctomap-dev \ + liborocos-kdl1.4 \ + python3-serial \ + ros-noetic-controller-interface \ + ros-noetic-controller-manager \ + ros-noetic-joint-state-controller \ + ros-noetic-interactive-markers \ + ros-noetic-kdl-parser \ + ros-noetic-pluginlib \ + ros-noetic-robot-state-publisher \ + ros-noetic-ros-control \ + ros-noetic-ros-controllers \ + ros-noetic-tf \ + ros-noetic-tf-conversions \ + ros-noetic-tf2-ros \ + ros-noetic-trac-ik-lib \ + ros-noetic-urdf \ + ros-noetic-urdf-parser-plugin \ + ros-noetic-xacro \ + && rm -rf /var/lib/apt/lists/* + +ENV ROS_MASTER_URI=http://127.0.0.1:11311 +ENV ROS_HOSTNAME=127.0.0.1 + +COPY entrypoint.sh /usr/local/bin/openral-a1-entrypoint +RUN chmod 0755 /usr/local/bin/openral-a1-entrypoint + +ENTRYPOINT ["/usr/local/bin/openral-a1-entrypoint"] +CMD ["bash"] diff --git a/docker/galaxea_a1_sidecar/entrypoint.sh b/docker/galaxea_a1_sidecar/entrypoint.sh new file mode 100644 index 0000000..ac68ab7 --- /dev/null +++ b/docker/galaxea_a1_sidecar/entrypoint.sh @@ -0,0 +1,14 @@ +#!/usr/bin/env bash +set -euo pipefail + +# shellcheck disable=SC1091 +source /opt/ros/noetic/setup.bash + +if [[ -z "${A1_SDK_ROOT:-}" || ! -f "${A1_SDK_ROOT}/install/setup.bash" ]]; then + echo "A1_SDK_ROOT must point to a mounted, built Galaxea A1 SDK" >&2 + exit 2 +fi + +# shellcheck disable=SC1090 +source "${A1_SDK_ROOT}/install/setup.bash" +exec "$@" diff --git a/docker/inference/Dockerfile.x86 b/docker/inference/Dockerfile.x86 index dee24b8..20cc14a 100644 --- a/docker/inference/Dockerfile.x86 +++ b/docker/inference/Dockerfile.x86 @@ -137,6 +137,8 @@ COPY python/ python/ # load time (needed on REAL hardware too). # omdet — the open-vocabulary object detector (omdet-turbo, transformers-based) # the ros_image_detector_node runs when enable_object_detector is on. +# lingbot — the msgpack + synchronous-websocket protocol dependencies used by +# the out-of-process LingBot policy adapters, including Galaxea A1. # The `opencv_thread` camera path needs cv2 (opencv-python). It is a per-member # EXTRA (openral-runner `opencv`), NOT pulled by `uv sync --all-packages`, so it # is installed explicitly post-sync alongside onnxruntime (which lives only in @@ -147,7 +149,7 @@ COPY python/ python/ # uv.lock. TensorRT (`--group tensorrt`) is an OpenRAL Pro plugin — never here. RUN --mount=type=cache,target=/root/.cache/uv \ uv sync --frozen --all-packages \ - --group sim --group omdet \ + --group sim --group omdet --group lingbot \ && uv pip install --python /workspace/.venv/bin/python onnxruntime opencv-python # Copy the colcon-buildable trees AFTER the venv resolve so a packages/ tweak @@ -185,6 +187,7 @@ RUN bash -lc 'set -eo pipefail \ --packages-select openral_msgs \ opentelemetry_cpp_vendor \ openral_hal_so100 \ + openral_hal_galaxea_a1 \ openral_hal_openarm \ openral_world_state \ openral_reasoner_ros \ diff --git a/docs/architecture/repo-state-map.html b/docs/architecture/repo-state-map.html index 08c8bbf..ca8c63e 100644 --- a/docs/architecture/repo-state-map.html +++ b/docs/architecture/repo-state-map.html @@ -700,6 +700,15 @@

Schemas registry

outputs: ["JointTrajectory", "/error_recovery/goal"], schemas: ["RobotDescription", "JointState", "Action"], }, + { + title: "HAL · Galaxea A1 (real-HW)", + pkg: "python/hal · galaxea_a1.py · tools/galaxea_a1_ros1_sidecar.py · docker/galaxea_a1_sidecar · packages/openral_hal_galaxea_a1", + status: "yellow", + desc: "Real-only six-axis A1 HAL over an isolated ROS 1 Noetic sidecar. The operator provides the official SDK; OpenRAL bundles no vendor code. A background transport accepts only literal IPv4 loopback endpoints and keeps network I/O off the HAL hot path. The sidecar owns roscore + official serial driver + joint tracker and stops the full process group on command-lease expiry, disconnect, motor fault, or e-stop. Its locked staged-command relay blocks the tracker's compiled startup pose until it aligns with fresh measured feedback. The manifest at robots/galaxea_a1/robot.yaml includes the official URDF's joint axes and transforms; collision primitives remain uncommitted pending cleared lowering provenance. Unit, socket, ROS lifecycle, and read-only preflight coverage are complete. Real HIL has passed observation, current hold, a bounded 0.01 rad joint round trip, a 10 mm gripper round trip, and current hold through candidate_action → the C++ kernel → safe_action → the real HAL. The LingBot-VA A1 rSkill consumes the external A1 Runtime's paired-camera/model/EEF-IK contracts, explicitly subdivides IK solutions by its own policy_extras bound under the independent HAL ceiling, and returns typed joint + gripper actions to the same OpenRAL safety path.", + inputs: ["Action (6-D + gripper)", "/joint_states_host", "/arm_status_host"], + outputs: ["/arm_joint_target_position", "/gripper_position_control_host", "JointState"], + schemas: ["RobotDescription", "JointState", "Action"], + }, { title: "HAL · Sawyer (real-HW)", pkg: "python/hal · sawyer_real.py", @@ -1569,7 +1578,7 @@

Schemas registry

title: "Tests · HIL", pkg: "tests/hil", status: "yellow", - desc: "7 files: test_ur5e, test_ur10e, test_franka_panda, test_sawyer, test_aloha (real-hardware HALs gated by self-hosted [lab-] runner labels + UR{5,10}E_HOST / *_FCI_HOST env vars), plus test_detect_jetson_live for the auto-provisioning probe. SO-100 / SO-101 / RealSense HIL gates were removed until matching lab hardware exists. The shared rclpy bridge lives in _ros_control_transport.py (RosControlHILTransport) and _aloha_ros_transport.py (4-way ALOHA fan-out). The L4T HIL workflow (`hil-l4t.yml`) that PR #93 added was removed by the 2026-05-14 single-Dockerfile consolidation amendment — L4T returns when the `[self-hosted, l4t]` runner pool is online.", + desc: "8 files: test_ur5e, test_ur10e, test_franka_panda, test_sawyer, test_aloha (real-hardware HALs gated by self-hosted [lab-] runner labels + UR{5,10}E_HOST / *_FCI_HOST env vars), test_galaxea_a1 (direct HAL observation/hold/joint/gripper gates), test_galaxea_a1_deploy (full candidate_action → C++ kernel → safe_action → real HAL → ROS 1 relay hold + mandatory e-stop), plus test_detect_jetson_live for the auto-provisioning probe. SO-100 / SO-101 / RealSense HIL gates were removed until matching lab hardware exists. The shared rclpy bridge lives in _ros_control_transport.py (RosControlHILTransport) and _aloha_ros_transport.py (4-way ALOHA fan-out). The L4T HIL workflow (`hil-l4t.yml`) that PR #93 added was removed by the 2026-05-14 single-Dockerfile consolidation amendment — L4T returns when the `[self-hosted, l4t]` runner pool is online.", }, { title: "CI · Docker image build", diff --git a/docs/methods/00-core-schemas.md b/docs/methods/00-core-schemas.md index 142f27e..b3867e9 100644 --- a/docs/methods/00-core-schemas.md +++ b/docs/methods/00-core-schemas.md @@ -45,7 +45,7 @@ _openral schema v0 — normative Pydantic v2 contracts for all layers._ `APACHE_2_0, MIT, BSD, PERMISSIVE_RESEARCH, NVIDIA_NON_COMMERCIAL, NVIDIA_OPEN_MODEL, RLWRLD_NON_COMMERCIAL, PROPRIETARY, UNKNOWN` (NVIDIA_OPEN_MODEL = GR00T N1.7+, commercial OK) - `class RSkillRuntime(str, Enum)` — Manifest runtime hint. (L2775) `PYTORCH, ONNX, TENSORRT, TRT_LLM, VLLM, GGUF, MLX, JAX` -- `class PhysicsBackend(str, Enum)` — Sim backend. (L6520) +- `class PhysicsBackend(str, Enum)` — Sim backend. (L6532) `MUJOCO, MUJOCO_MJX, PYBULLET, SAPIEN, ISAACSIM, COPPELIASIM, GENESIS, MOCK` (SAPIEN = ManiSkill3 / RoboTwin engine; RoboTwin uses it via a py3.10 sidecar; `COPPELIASIM` = CoppeliaSim/PyRep RLBench backend, out-of-process py3.10 sidecar) **Pydantic models — robot manifest hierarchy** @@ -66,7 +66,7 @@ _openral schema v0 — normative Pydantic v2 contracts for all layers._ - `class ComputeSpec(BaseModel)` — Compute profile for one deployment tier (edge / local / cloud). Populated by `openral_detect._enrich_compute` from GPU probe results; attached to `RobotDescription.compute_edge`, `compute_local`, or `compute_cloud`. (L667) fields: `compute_tops, system_memory_gb, num_gpus, gpu_vram_gb, cuda_compute_capability, cuda_toolkit_version, tensorrt_version, gpu_supported_runtimes, gpu_supported_dtypes, nvmm_available, endpoint, network_latency_ms`. Shared across all three tiers — `endpoint` and `network_latency_ms` are `None` for edge/local. `nvmm_available` is probed on all tiers (returns `False` gracefully when absent). `num_gpus > 1` captures cloud multi-GPU pods. GPU/runtime fields moved here from `RobotCapabilities` to separate physical robot capabilities from host compute properties. - `supports_cumotion() -> bool` — True when the host meets the cuMotion (Isaac ROS) GPU floor: compute capability >= (8, 0), CUDA toolkit major >= 13, and `gpu_vram_gb` >= `_CUMOTION_MIN_VRAM_GIB` (7.5; nominal-8 GB cards report ~7.99 GiB). The MoveIt planner gate uses it to pick cuMotion vs OMPL. False on non-CUDA hosts or when the CUDA toolkit version is unknown. -- `class ReasonerModel(BaseModel)` — Frozen curated S2 model registry entry (ADR-0088). Fields: `id`, `display_name`, `dialect (anthropic|openai)`, `hosting (cloud|managed_local|byo_local)`, `served_model_id`, `default_endpoint`, `auth_required`, `tool_choice`, `max_tokens_default`, `min_gpu_vram_gb`, `required_dtype`, `weights_license`; property `is_local`. `REASONER_MODELS` is the curated map (`claude-opus-4-8`, `gpt-5.5`, `gpt-5.6`, `cosmos3-edge`); membership means the model passed the robotics tool-calling contract. `REASONER_MANAGED_ENDPOINT="managed"` is the managed-local sentinel. (L8065) +- `class ReasonerModel(BaseModel)` — Frozen curated S2 model registry entry (ADR-0088). Fields: `id`, `display_name`, `dialect (anthropic|openai)`, `hosting (cloud|managed_local|byo_local)`, `served_model_id`, `default_endpoint`, `auth_required`, `tool_choice`, `max_tokens_default`, `min_gpu_vram_gb`, `required_dtype`, `weights_license`; property `is_local`. `REASONER_MODELS` is the curated map (`claude-opus-4-8`, `gpt-5.5`, `gpt-5.6`, `cosmos3-edge`); membership means the model passed the robotics tool-calling contract. `REASONER_MANAGED_ENDPOINT="managed"` is the managed-local sentinel. (L8082) - `class RobotCapabilities(BaseModel)` — Physical capability flags for skill compatibility. (L760) fields: `locomotion, can_lift_kg, has_dexterous_hands, has_tactile, has_force_control, has_vision, has_lidar, has_vision_slam, has_audio, bimanual, supported_control_modes, supported_vla_embodiments, embodiment_tags`. GPU/compute fields moved to `ComputeSpec` (attached at `RobotDescription.compute`). **`has_vision_slam`** gates the camera-based cuVSLAM+nvblox SLAM backend for lidar-less robots; independent of `has_lidar` (lidar backend wins when both set). - `class SafetyEnvelope(BaseModel)` — Constraints enforced by C++ safety kernel. (L808) @@ -210,7 +210,7 @@ _Advisory, queryable Layer-2 world model the S2 Reasoner consults to recall wher - `WRAPPED_TASK_SPACE_LAYOUTS: frozenset[StateLayout]` — Subset of `StateLayout` covering Cartesian/FK-derived composites: `{rc365, human300_16d, libero_eef8d}`. These layouts REQUIRE `StateContract.bindings`; the cross-validator on `StateContract` enforces this at manifest load (`human300_16d`/`rc365` require `eef_frame`+`base_frame`; `libero_eef8d` requires `eef_frame`+`gripper_qpos_joints` — world-frame absolute EE pose, so no `base_frame`). Joint-space layouts (`smolvla_9d`, `gr1`, `simpler_*`) are excluded — they're served verbatim from raw `JointState.position`. - `StateContractBindings` (Pydantic model) — Per-robot source bindings for an rSkill's `state_contract.layout`. Fields: `eef_frame: str | None`, `base_frame: str | None`, `world_frame: str | None = "map"`, `gripper_qpos_joints: list[str]`, `quaternion_convention: Literal["xyzw","wxyz"] = "xyzw"`. Symmetric to `ControlModeSemantics` on the action side. Required when `StateContract.layout` is in `WRAPPED_TASK_SPACE_LAYOUTS`, forbidden otherwise. - `BenchmarkName` (TypeAlias = Literal[...]) — Closed canonical benchmark ids matching `benchmarks/*.yaml` suites (plus retained `aloha_insertion`/`aloha_transfer_cube` task-level ids cited by the act-aloha* manifests after the two suites were unified into `aloha.yaml`). Members: `aloha, aloha_insertion, aloha_transfer_cube, gr1_tabletop, libero_10, libero_goal, libero_object, libero_spatial, maniskill3_panda, metaworld_mt10, metaworld_mt50, pusht, rlbench, robocasa_pnp, robotwin, simpler_env_widowx`. (L2633) -- `ModelFamily` (TypeAlias = Literal["smolvla","pi05","xvla","act","diffusion","rldx","molmoact2","gr00t","diffuser_actor","openvla","lingbot_vla2","lingbot_vla","internvla_n1"]) — Closed VLA/policy family used by the eval/runner adapter dispatch. Required only when `RSkillManifest.kind == "vla"`. `gr00t` (NVIDIA Isaac GR00T) runs out-of-process via a ZMQ sidecar, reusing the `rldx` adapter. `diffuser_actor` (3D Diffuser Actor, RLBench keyframe policy) runs out-of-process via its own py3.10 sidecar. `openvla` (OpenVLA / OpenVLA-OFT) loads in-process as a transformers custom-code model and de-normalizes discrete action tokens via the checkpoint's `unnorm_key`. `lingbot_vla2` (Robbyant LingBot-VLA 2.0, Qwen3-VL-4B + sparse-MoE flow-matching expert, Apache-2.0) runs out-of-process via its own auto-provisioned py3.12 + torch-2.8 sidecar (`openral_sim.policies.lingbot_vla2`); `lingbot_vla` (LingBot-VLA 1.0, 4B Qwen2.5-VL + dense Qwen2 flow-matching expert, the RoboTwin post-train) shares that sidecar via `--variant v1` (a separate V1 repo + transformers==4.51.3 / lerobot==0.4.2 venv). `internvla_n1` (InternRobotics InternVLA-N1 / DualVLN, arXiv:2512.08186) is a dual-system vision-language **navigation** policy (Qwen2.5-VL-7B System-2 + NavDP DiT System-1); it runs out-of-process via a py3.11 ZMQ sidecar (upstream pins transformers 4.51) and emits a 6-D `BODY_TWIST` base velocity from RGB-D + instruction. (L2655) +- `ModelFamily` (TypeAlias = Literal["smolvla","pi05","xvla","act","diffusion","rldx","molmoact2","gr00t","diffuser_actor","openvla","lingbot_vla2","lingbot_vla","lingbot_va_a1","internvla_n1"]) — Closed VLA/policy family used by the eval/runner adapter dispatch. Required only when `RSkillManifest.kind == "vla"`. `gr00t` (NVIDIA Isaac GR00T) runs out-of-process via a ZMQ sidecar, reusing the `rldx` adapter. `diffuser_actor` (3D Diffuser Actor, RLBench keyframe policy) runs out-of-process via its own py3.10 sidecar. `openvla` (OpenVLA / OpenVLA-OFT) loads in-process as a transformers custom-code model and de-normalizes discrete action tokens via the checkpoint's `unnorm_key`. `lingbot_vla2` (Robbyant LingBot-VLA 2.0, Qwen3-VL-4B + sparse-MoE flow-matching expert, Apache-2.0) runs out-of-process via its own auto-provisioned py3.12 + torch-2.8 sidecar (`openral_sim.policies.lingbot_vla2`); `lingbot_vla` (LingBot-VLA 1.0, 4B Qwen2.5-VL + dense Qwen2 flow-matching expert, the RoboTwin post-train) shares that sidecar via `--variant v1`. `lingbot_va_a1` connects to the Runtime-owned versioned policy gateway; Runtime owns model configuration, EEF/cache semantics, and IK while OpenRAL owns candidate-action safety and HAL execution. `internvla_n1` is a dual-system vision-language navigation policy that runs out-of-process and emits a 6-D `BODY_TWIST`. (L2655) - `RSkillKind` (TypeAlias = Literal["vla","wam","ros_action","ros_service","detector","vlm","reward","playbook"]) — Discriminator selecting the loader / runner branch. `"vla"` is today's learnable policy path; `"ros_action"` / `"ros_service"` route through `ROSActionRskill`; `"wam"` is reserved (loader rejects at resolve time); `"detector"` is a perception producer that runs an exported ONNX/TRT detection model and publishes `ObjectsMetadata` — emits no `Action`; `"vlm"` is a video-language model answering natural-language scene queries from camera frames — emits text, no actions/boxes, `role: s2` (reached via the read-only `query_scene` tool, not `ExecuteRskill`); `"reward"` is a progress/reward monitor; `"playbook"` is a symbolic, human-authored S2 decision procedure (Markdown SOP) the reasoner *reads* into its system prompt — no weights, actuators, or `Action`, `role: s2`. (L2752) **Pydantic models — skill benchmark results (`rskills//eval/.json`)** @@ -258,7 +258,7 @@ On-disk + runtime contracts for the hardware inference runner (`openral deploy - - `_decode_data(cls, value: Any) -> bytes | None` [@field_validator("data", mode="before")] — Accept raw `bytes` or a base64-encoded `str` on JSON parse. (L614) - `_encode_data(self, value: bytes | None) -> str | None` [@field_serializer("data", when_used="json")] — JSON-serialize the binary payload as base64. (L632) - `model_post_init(_context: object) -> None` — Cross-field validation: exactly one of `(data, topic, handle)` must be set. (L637) -- `class SensorReaderBackend(str, Enum)` — Which `SensorReader` implementation a sensor uses. (L1691) +- `class SensorReaderBackend(str, Enum)` — Which `SensorReader` implementation a sensor uses. Includes `galaxea_a1_camera_bridge`, which consumes the versioned paired raw-frame owner exposed by an external A1 Runtime process without importing Runtime or reopening either RealSense device. (L1691) `OPENCV_THREAD, ROS2_IMAGE, GSTREAMER` - `class DeadlineOverrunPolicy(str, Enum)` — Behaviour when a tick exceeds `1 / rate_hz`. (L1706) `WARN, DROP, RAISE` diff --git a/docs/methods/01-hal.md b/docs/methods/01-hal.md index 3fe5480..ee22436 100644 --- a/docs/methods/01-hal.md +++ b/docs/methods/01-hal.md @@ -3,15 +3,23 @@ > Part of the OpenRAL [public-symbol inventory](../METHODS.md). Hand-curated; `(LNN)` markers are refreshed by `tools/refresh_methods_linenos.py`. ### `python/hal/src/openral_hal/protocol.py` -_HAL Protocol — the normative interface every HAL adapter must satisfy._ +_Normative HAL protocol plus explicit optional lifecycle extensions._ -- `class HAL(Protocol)` — Structural protocol every HAL adapter must satisfy. (L26) +- `class HAL(Protocol)` — Structural protocol every HAL adapter must satisfy. - attr `description: RobotDescription` - - `connect() -> None` — Open connection to robot/sim. (L43) - - `disconnect() -> None` — Close connection (idempotent). (L53) - - `read_state() -> JointState` — Latest joint state snapshot (hot path). (L61) - - `send_action(action: Action) -> None` — Forward action chunk to controller (hot path). (L75) - - `estop() -> None` — Trigger emergency stop, always raises `ROSEStopRequested`. (L91) + - `connect() -> None` — Open connection to robot/sim. + - `disconnect() -> None` — Close connection (idempotent). + - `read_state() -> JointState` — Latest joint state snapshot (hot path). + - `send_action(action: Action) -> None` — Forward action chunk to controller (hot path). + - `estop() -> None` — Trigger emergency stop, always raises `ROSEStopRequested`. +- `class LifecycleEStopHAL(Protocol)` — Opt-in propagation of the generic + lifecycle e-stop to downstream hardware or owned processes. +- `class ResettableLifecycleEStopHAL(LifecycleEStopHAL, Protocol)` — Opt-in + in-process recovery contract. +- `class EStopRecovery(StrEnum)` — Declares whether recovery is resettable or + requires a full lifecycle restart. +- `class HALHealthProvider(Protocol)` / `class HALHealthReport` — Cached, + I/O-free diagnostics consumed by the generic lifecycle heartbeat. ### `python/hal/src/openral_hal/_mujoco_arm.py` _Internal MuJoCo-backed HAL implementation shared by UR / Franka / SO-100 / G1 / H1 / Rizon-4 / OpenArm / ALOHA adapters. Reads its wiring from `RobotDescription.sim`._ @@ -169,6 +177,249 @@ _SO100FollowerHAL — wraps lerobot's SO-100 follower arm USB driver._ - `_rad_to_deg(rad) -> float` (L238) - const `SO100_DESCRIPTION = RobotDescription(...)` (L88) +### `python/hal/src/openral_hal/galaxea_a1.py` +_Real-only Galaxea A1 HAL. OpenRAL stays ROS 2 / Python 3.12; the operator's +official ROS 1 Noetic SDK runs out of process behind a literal IPv4-loopback +JSON-lines sidecar. No vendor source, binary, or message package is distributed._ + +- `class GalaxeaA1HAL(HALBase)` — six-axis joint-position + normalized + gripper adapter. `read_state` and `send_action` use a cached snapshot/latest + target so network I/O stays off the HAL hot path. Commands fail closed on + stale state/status, unaccepted motor bits, non-finite values, initial target + misalignment, or an excessive feedback-relative target step. Command limits + remain exact; a separately tracked 0.01 rad feedback-only endpoint tolerance + absorbs encoder zero/quantization at a nominal URDF boundary. `estop` asks + the sidecar to stop its owned ROS 1 stack and always raises + `ROSEStopRequested`. +- const `GALAXEA_A1_DESCRIPTION` — real-only `RobotDescription`, mirrored by + `robots/galaxea_a1/robot.yaml`; official A1 URDF joint names/limits, explicit + sidecar deadlines, motor masks, 0..104 mm normalized gripper mapping, and + the calibrated D455 front / D405 wrist RGB observation contracts. The six + joint origins, orientations, and axes are transcribed from the official A1 + URDF; collision primitives remain absent until their redistribution and + lowering provenance is cleared. +- `tools/galaxea_a1_ros1_sidecar.py` — Python-3.8-compatible ROS 1 process that + owns `roscore`, `signal_arm/single_arm_node.launch`, and the official + `mobiman/jointTracker_demo_node` binary. The tracker publishes to + `/openral/arm_joint_command_staged`; a sidecar-owned relay is the sole + publisher to `/arm_joint_command_host`. The relay stays `LOCKED` until the + first target, then `ARMING` until a fresh, valid tracker command is aligned + with both that target and measured joint feedback. Only `ACTIVE` forwards + unchanged tracker commands, and gripper setpoints are gated on the same + machine — a gripper command while the relay is `LOCKED`/`ARMING` is refused + fail-closed (the gripper bypasses the tracker's staged-hold interpolation, + so it must never actuate before alignment). Repeated identical joint and gripper setpoints + refresh the command lease without restarting the official tracker. A command + lease, alignment timeout, malformed command, stale feedback, motor fault, + client disconnect, or e-stop stops the complete owned process group. +- `tools/run_galaxea_a1_sidecar.sh` — Docker launcher. Requires an explicit + operator-provided image and SDK path; mounts both read-only and claims only the + selected serial device. The SDK remains read-only; the official tracker's + generated CppAD files go to + `$XDG_CACHE_HOME/openral/galaxea-a1/x1_robot` (or the equivalent path below + `~/.cache`). `--check-only` verifies the local image, required SDK files, + cache parent, serial ownership, loopback port, container name, and process + lock without opening the serial device or starting a container. + +#### Galaxea A1 hardware bring-up + +The first session is observation-only until the HAL graph is healthy. Ensure no +other process/container owns the serial device, the arm workspace is clear, and +the physical e-stop is reachable. + +```bash +# One-time: build OpenRAL's vendor-free Noetic runtime image. The official SDK +# is mounted at run time and is never copied into the image. +docker build \ + -t openral/galaxea-a1-sidecar:noetic \ + docker/galaxea_a1_sidecar + +# One-time: build OpenRAL's standard public x86 deploy image (Jazzy/Python 3.12). +just docker-build-x86 + +# Read-only gate — checks the image, SDK, serial ownership, port, and lock. +tools/run_galaxea_a1_sidecar.sh \ + --image openral/galaxea-a1-sidecar:noetic \ + --sdk-root /absolute/path/to/A1_SDK \ + --serial /dev/a1 \ + --check-only + +# Terminal 1 — isolated ROS 1 bridge network; only TCP 46011 reaches loopback. +tools/run_galaxea_a1_sidecar.sh \ + --image openral/galaxea-a1-sidecar:noetic \ + --sdk-root /absolute/path/to/A1_SDK \ + --serial /dev/a1 + +# Terminal 2 — OpenRAL's standard real-hardware path. The OpenRAL container uses +# host networking only for ROS 2 DDS and the sidecar's loopback TCP port; it owns +# no Galaxea serial device and cannot see the ROS 1 master inside the sidecar. +docker run --rm --name openral-galaxea-a1 --network host \ + --volume "$(pwd)/robots:/workspace/robots:ro" \ + --volume "$(pwd)/scenes:/workspace/scenes:ro" \ + --volume "$(pwd)/tests:/workspace/tests:ro" \ + openral:x86 \ + --config scenes/deploy/galaxea_a1_bench.yaml + +# Terminal 3 — observation gate: six named joints update; diagnostics are clean. +docker exec openral-galaxea-a1 bash -lc \ + 'source /opt/ros/jazzy/setup.bash && source /workspace/install/setup.bash && \ + ros2 topic echo /joint_states --once && ros2 topic echo /diagnostics --once' +``` + +An optional HAL-level HIL gate can run between Terminal 1 and the full deploy. +It opens one sidecar session, validates three fresh finite named-joint samples +plus cached motor health, and ends by verifying that downstream e-stop stops the +owned ROS 1 stack. Restart Terminal 1 afterwards: + +```bash +GALAXEA_A1_HIL=1 just hil galaxea_a1 +``` + +Only after that observation-only run passes, opt into a measured-current-pose +hold. Feedback within the tracked 0.01 rad endpoint tolerance is projected to +the exact command limit; any larger projection fails before publication. The +test also waits for the sidecar relay to report `ACTIVE`, proving the official +tracker has converged from its compiled `task.info` initial pose before any +host motor command is forwarded: + +```bash +GALAXEA_A1_HIL=1 GALAXEA_A1_ALLOW_HOLD=1 just hil galaxea_a1 +``` + +After the hold passes, a separate lab opt-in moves `arm_joint1` by +0.01 rad, +requires it to settle within 0.008 rad (covering the measured 0.007 rad +small-command residual), continuously bounds all six joint excursions, returns +to the measured start, and then performs the same downstream e-stop: + +```bash +GALAXEA_A1_HIL=1 GALAXEA_A1_ALLOW_NUDGE=1 just hil galaxea_a1 +``` + +The G2 gripper has its own opt-in. It uses the vendor example's 10 mm step, +mapped through the normalized `0..1` contract over the configured 104 mm +stroke, chooses the direction away from the nearest endpoint, verifies feedback +within the measured 2.5 mm steady-state tolerance, and returns to the measured +opening even when the outbound-leg assertion fails: + +```bash +GALAXEA_A1_HIL=1 GALAXEA_A1_ALLOW_GRIPPER=1 just hil galaxea_a1 +``` + +After the HAL-level gates pass, the full-graph HIL runs inside the deploy +container. It captures the current named-joint feedback itself, requires the +C++ kernel and real HAL to be active while the relay is still `LOCKED`, then +publishes only that measured hold through `candidate_action`. It verifies the +matching `safe_action`, exact staged/forwarded targets, zero kernel drops, and +less than one degree of drift. Its `finally` path publishes `/openral/estop` +three times and requires the HAL diagnostics to confirm the latch: + +```bash +docker exec \ + --env GALAXEA_A1_DEPLOY_HIL=1 \ + --env GALAXEA_A1_ALLOW_HOLD=1 \ + openral-galaxea-a1 \ + bash -lc 'source /opt/ros/jazzy/setup.bash && \ + source /workspace/install/setup.bash && \ + pytest -q /workspace/tests/hil/test_galaxea_a1_deploy.py' +``` + +After the current-pose full-graph gate passes, the same fixture has a separate +motion opt-in. It moves `arm_joint1` by +0.01 rad through +`candidate_action -> C++ safety kernel -> safe_action`, bounds all six joint +excursions, and returns to the measured start before the downstream e-stop: + +```bash +docker exec \ + --env GALAXEA_A1_DEPLOY_HIL=1 \ + --env GALAXEA_A1_ALLOW_HOLD=1 \ + --env GALAXEA_A1_ALLOW_NUDGE=1 \ + openral-galaxea-a1 \ + bash -lc 'source /opt/ros/jazzy/setup.bash && \ + source /workspace/install/setup.bash && \ + pytest -q /workspace/tests/hil/test_galaxea_a1_deploy.py' +``` + +This test intentionally ends the hardware session. Restart both the sidecar +and deploy container before any later motion test. + +Do not start a policy on the first pass. Stop both commands and investigate if +feedback/status becomes stale, a motor code other than the manifest's explicit +idle/gripper masks appears, joint order differs, the sidecar exits, or the arm +moves before an approved safe action. Motion validation then proceeds with a +current-pose hold and a single <=0.01 rad joint increment through OpenRAL's +standard candidate-action → C++ kernel → safe-action path, then return-to-start; +only afterwards run an A1-specific rSkill. + +#### LingBot-VA rSkill through the complete OpenRAL path + +`rskills/lingbot-va-galaxea-a1-fruit-placement/rskill.yaml` is the first +checkpoint-specific A1 rSkill. The dependency direction is deliberate: + +```text +A1 Camera Bridge -> OpenRAL WorldState -> LingBot-VA rSkill + -> A1 Runtime policy gateway (model contract + EEF/cache + IK) + -> OpenRAL candidate_action -> C++ safety kernel -> safe_action + -> GalaxeaA1HAL -> isolated ROS 1 sidecar -> official A1 driver +``` + +The A1 Runtime is a public capability provider, not a second controller: +start only its persistent camera owner, LingBot policy server, and OpenRAL +policy gateway. The gateway has no ROS imports or command publisher. Do not +start its LingBot ROS execution bridge or A1 joint runtime while OpenRAL owns +the deployment. The rSkill owns its `policy_extras.max_joint_substep_rad` replay +setting and reads the independent `max_target_step_rad` ceiling from the same +`RobotDescription` used to construct the HAL. Startup rejects a policy bound +that exceeds either the HAL's live target-step ceiling or locked-relay +alignment tolerance. The policy bound is 0.045 rad, below the 0.05 rad +locked-relay alignment threshold; the independent HAL/sidecar live limit is +0.08 rad. The gateway constructs Runtime's IK implementation with the active +OpenRAL `RobotDescription`'s ordered command limits after verifying they are no +wider than Runtime's envelope. Runtime calibration margins therefore cannot +widen the typed OpenRAL, safety-kernel, or official sidecar command envelope. + +The gateway emits one bounded target per 30 Hz control tick. When its IK +solution is farther than 0.045 rad from fresh feedback, it keeps advancing +toward that same solved target on subsequent ticks and only consumes the next +model action after the solved target has been dispatched. The FK of the actual +dispatched target is written into the KV cache, so the policy state reflects +what OpenRAL commanded rather than an unreachable ideal. The official tracker's +steady-state error cannot widen the command envelope or bypass the bounded +step. The A1 Runtime's 1.70 rad IK-solution validation remains an upstream +reachability check, not a motor-command step limit. + +```bash +# Terminal A — A1 Runtime capability providers only (no ROS command publisher). +cd /absolute/path/to/A1-Research +just cameras start +scripts/apps/lingbot/a1_lingbot_runtime.sh server +uv run galaxea-a1-openral-policy \ + --config configs/deployments/lingbot/fruit_placement_eef.toml \ + --repo-root . + +# Terminal B — official ROS 1 sidecar, as in the bring-up section above. +cd /absolute/path/to/OpenRAL +tools/run_galaxea_a1_sidecar.sh \ + --image openral/galaxea-a1-sidecar:noetic \ + --sdk-root /absolute/path/to/A1_SDK \ + --serial /dev/a1 + +# Terminal C — the complete OpenRAL real deployment. +cd /absolute/path/to/OpenRAL +uv run --group lingbot openral deploy run \ + --config scenes/deploy/galaxea_a1_bench.yaml +``` + +Submit the exact trained prompt (for example, `put the red mango into the blue +plate`) through the dashboard. Before allowing a task motion, first repeat the +observation, hold, joint-nudge, gripper, and full-graph gates above. Stop the +LingBot server afterwards with +`scripts/apps/lingbot/a1_lingbot_runtime.sh server-stop`. + +The A1 opts into hardware-downstream e-stop: `/openral/estop` stops the +sidecar-owned tracker and driver immediately. The generic +`/openral/estop_cleared` broadcast cannot re-arm this HAL; restart the lifecycle +and sidecar, re-read motor health, and repeat initial alignment instead. + ### `python/hal/src/openral_hal/h1.py` _MuJoCo digital twin for the Unitree H1 humanoid (Menagerie MJCF). Contract validator only — falls without an S0 cerebellum; gravity must be disabled in closed-loop tests (CLAUDE.md §6.2). Unlike the G1 / UR / Franka / SO-100 MJCFs, the H1 menagerie ships ``motor`` (torque) actuators, so this HAL runs a software PD position loop every physics step._ @@ -294,15 +545,15 @@ _Generic ROS 2 managed lifecycle node wrapper for every HAL adapter — UR5e / U - `class HALLifecycleNodeBase(LifecycleNode)` — Public base class. Owns the standard `/joint_states` + `~/joint_states` publishers, the `/openral/safe_action` + `/openral/estop` subscribers, the 1 Hz `DiagnosticsHeartbeat`, the per-tick `hal.read_state` + `hal.send_action` OTel spans, the estop latch, and the full configure → activate → deactivate → cleanup → shutdown transition wiring. The formerly used `~/command` (`trajectory_msgs/JointTrajectory`) subscriber + its `_on_command` callback + the `_subscriber` field were removed; `_send_action_traced` is now driven only by `_on_safe_action`. (L322) - `_create_hal(self) -> HAL` — **Subclass hook (required)**: construct and return a HAL instance. Reads ROS-parameter-driven constructor args via `self.get_parameter(...)`. (L381) - `_heartbeat_extra_fields(self) -> dict[str, str]` — Subclass hook (optional): extra key/values for the `/diagnostics` payload (e.g. `{"port": "/dev/ttyUSB0"}` for SO-100, `{"mjcf": "..."}` for OpenArm). Default: `{}`. (L393) - - `on_configure_post_hal(self) -> TransitionCallbackReturn` — Subclass hook (optional): robot-specific setup after the HAL connects (e.g. opening a camera renderer on OpenArm). Default: `SUCCESS`. (L405) - - `on_activate_post_subs(self) -> TransitionCallbackReturn` — Subclass hook (optional): robot-specific timers/publishers after the base wires its subs (e.g. the OpenArm camera-render timer). Default: `SUCCESS`. (L414) - - `on_deactivate_pre_teardown(self) -> None` — Subclass hook (optional): stop robot-specific timers before base teardown. Default: no-op. (L422) - - `on_cleanup_pre_disconnect(self) -> None` — Subclass hook (optional): tear down robot-specific resources (viewers, renderers) before HAL.disconnect(). Default: no-op. (L429) - - `_publish_joint_state(self) -> None` — Timer callback. Wraps `self._hal.read_state()` in a `hal.read_state` span (identity attrs + `producer.record_joint_state`) and publishes the standard `/joint_states` + `~/joint_states` messages. Subclasses may override + call `super()._publish_joint_state()` to extend (OpenArm does this for viewer-sync). (L744) - - `_on_safe_action(self, msg) -> None` — `/openral/safe_action` callback. Decodes the `openral_msgs/ActionChunk` into an `openral_core.Action` and forwards through `_send_action_traced(action, source="safe_action")`. (L808) - - `_send_action_traced(self, action, *, source) -> None` — Forward `action` to `self._hal.send_action` inside a `hal.send_action` span. The `source` attribute disambiguates the origin on the dashboard's Commands card (kept on the span so future subscriber additions can fan in without changing the span shape). (L828) + - `on_configure_post_hal(self) -> TransitionCallbackReturn` — Subclass hook (optional): robot-specific setup after the HAL connects (e.g. opening a camera renderer on OpenArm). Default: `SUCCESS`. (L445) + - `on_activate_post_subs(self) -> TransitionCallbackReturn` — Subclass hook (optional): robot-specific timers/publishers after the base wires its subs (e.g. the OpenArm camera-render timer). Default: `SUCCESS`. (L454) + - `on_deactivate_pre_teardown(self) -> None` — Subclass hook (optional): stop robot-specific timers before base teardown. Default: no-op. (L462) + - `on_cleanup_pre_disconnect(self) -> None` — Subclass hook (optional): tear down robot-specific resources (viewers, renderers) before HAL.disconnect(). Default: no-op. (L469) + - `_publish_joint_state(self) -> None` — Timer callback. Wraps `self._hal.read_state()` in a `hal.read_state` span (identity attrs + `producer.record_joint_state`) and publishes the standard `/joint_states` + `~/joint_states` messages. Subclasses may override + call `super()._publish_joint_state()` to extend (OpenArm does this for viewer-sync). (L775) + - `_on_safe_action(self, msg) -> None` — `/openral/safe_action` callback. Decodes the `openral_msgs/ActionChunk` into an `openral_core.Action` and forwards through `_send_action_traced(action, source="safe_action")`. (L846) + - `_send_action_traced(self, action, *, source) -> None` — Forward `action` to `self._hal.send_action` inside a `hal.send_action` span. The `source` attribute disambiguates the origin on the dashboard's Commands card (kept on the span so future subscriber additions can fan in without changing the span shape). (L866) - `make_lifecycle_main(node_name, hal_factory) -> Callable[[], None]` — Build a `main()` entry point for a zero-parameter HAL adapter. Internally constructs a `_FactoryHALLifecycleNode(HALLifecycleNodeBase)` whose `_create_hal()` returns `hal_factory()`. Superseded for the standard arms by `make_lifecycle_main_from_manifest`; retained for bespoke nodes. (L194) -- `class ManifestHALLifecycleNode(HALLifecycleNodeBase)` — Public generic manifest-driven lifecycle node (promoted from the private `_ManifestHALLifecycleNode` under issue #191). Reads `robot_yaml` + `hal_mode` + sensor knobs as ROS params and builds its HAL via `openral_hal.build_hal`, so a robot's construction kwargs come from the manifest's `hal.parameters.defaults` — no bespoke `_create_hal` subclass. Attaches `SimSensorBridge` (cameras / depth / scan / viewer) in `on_activate_post_subs`. In `on_configure_post_hal`, **reflects** on the built HAL and opens `/openral//reset_to_pose` (`openral_msgs/srv/ResetToPose`) iff it exposes `reset_to_pose` — generalising the openarm-only service to every `MujocoArmHAL` sim arm (issue #191 Phase 2); HALs without the method (panda_mobile, scene-attached twins) get no service. In `_create_hal`, when a scene composition is declared (and not scene-attaching), calls the named composer and threads the composed MJCF in as the HAL's `mjcf_path` (issue #191 Phase 3b — openarm tabletop); the composition is read from the `scene_composition_json` ROS param (the DeployScene's own `composition`) which **takes precedence** over the robot manifest's `scene_defaults.composition` (back-compat fallback) — so the scene owns its arena, the robot manifest describes the robot. Bare-twin camera robots (so100/so101) need no composition: their cameras are spliced by the generic camera rig at HAL connect from `sensors[].sim_placement`. In `on_activate_post_subs`, when the manifest declares a planar base (`base_joints`), also attaches a `MobileBaseBridge` (`/odom` + `odom->base_link` TF + `/cmd_vel`→BODY_TWIST) — so panda_mobile runs on this node with no subclass (issue #191 Phase 3a). The per-robot lifecycle packages collapse into this node (issue #191 Phases 2-3). A back-compat alias `_ManifestHALLifecycleNode` is retained. (L940) +- `class ManifestHALLifecycleNode(HALLifecycleNodeBase)` — Public generic manifest-driven lifecycle node (promoted from the private `_ManifestHALLifecycleNode` under issue #191). Reads `robot_yaml` + `hal_mode` + sensor knobs as ROS params and builds its HAL via `openral_hal.build_hal`, so a robot's construction kwargs come from the manifest's `hal.parameters.defaults` — no bespoke `_create_hal` subclass. Attaches `SimSensorBridge` (cameras / depth / scan / viewer) in `on_activate_post_subs`. In `on_configure_post_hal`, **reflects** on the built HAL and opens `/openral//reset_to_pose` (`openral_msgs/srv/ResetToPose`) iff it exposes `reset_to_pose` — generalising the openarm-only service to every `MujocoArmHAL` sim arm (issue #191 Phase 2); HALs without the method (panda_mobile, scene-attached twins) get no service. In `_create_hal`, when a scene composition is declared (and not scene-attaching), calls the named composer and threads the composed MJCF in as the HAL's `mjcf_path` (issue #191 Phase 3b — openarm tabletop); the composition is read from the `scene_composition_json` ROS param (the DeployScene's own `composition`) which **takes precedence** over the robot manifest's `scene_defaults.composition` (back-compat fallback) — so the scene owns its arena, the robot manifest describes the robot. Bare-twin camera robots (so100/so101) need no composition: their cameras are spliced by the generic camera rig at HAL connect from `sensors[].sim_placement`. In `on_activate_post_subs`, when the manifest declares a planar base (`base_joints`), also attaches a `MobileBaseBridge` (`/odom` + `odom->base_link` TF + `/cmd_vel`→BODY_TWIST) — so panda_mobile runs on this node with no subclass (issue #191 Phase 3a). The per-robot lifecycle packages collapse into this node (issue #191 Phases 2-3). A back-compat alias `_ManifestHALLifecycleNode` is retained. (L1028) - `make_lifecycle_main_from_manifest(node_name) -> Callable[[], None]` — Build a `main()` that spins up `ManifestHALLifecycleNode`. The node reads `robot_yaml` + `hal_mode` ("sim"|"real") ROS params and constructs its HAL via `openral_hal.build_hal(description, mode=hal_mode)` — one node class serves both modes for every robot. Used by franka / ur5e / ur10e / aloha / g1 / h1 / rizon4 / so100 / so101 (issue #191 Phase 2 migrated so100/so101 off their bespoke node); `openral deploy sim` injects `hal_mode="sim"`, `openral deploy run` injects `hal_mode="real"`. A robot lacking the requested mode raises `ROSCapabilityMismatch`. (L252) - `decode_action_chunk(msg) -> Action | None` — Inverse of `ros_publishing_hal._flatten_action_payload`. Decodes the `ActionChunk` wire shape (`flat` + `n_dof` + `horizon` + `control_mode`) back into a typed `openral_core.Action` with the per-mode payload field populated (`cartesian_delta` / `gripper` / `body_twist` / `joint_*`). Returns `None` for degenerate chunks (`flat=[]`, `n_dof≤0`) and for modes the F1/F5 publisher doesn't encode (`CARTESIAN_POSE`, `FOOT_PLACEMENT`, `DEX_HAND_JOINT`). Used by `HALLifecycleNodeBase._on_safe_action`; lives at module scope so unit tests in `tests/unit/test_lifecycle_action_chunk_decoder.py` exercise it without a ROS 2 install. (L103) diff --git a/docs/methods/02-sensors.md b/docs/methods/02-sensors.md index 73db36b..f577ac6 100644 --- a/docs/methods/02-sensors.md +++ b/docs/methods/02-sensors.md @@ -55,9 +55,9 @@ _Sensor catalog — vendor-agnostic registry of `SensorSpec` / `SensorBundle` fa _Generalised sensor → ROS 2 image publisher; non-GStreamer fallback to `RosImagePublisher`._ - `class SensorRosPublisher(*, reader, topic, rate_hz, node_name=None, frame_id=None, qos_depth=5, camera_info=None, info_topic=None)` — Background-thread publisher that polls any `SensorReader.read_latest()` and republishes as `sensor_msgs/Image`. Lazy-imports rclpy; raises `RuntimeError` at `start()` with install hint when ROS 2 isn't sourced. Optional `CameraInfo` companion (from an `IntrinsicsPinhole`) with RELIABLE QoS on `info_topic` — `None` derives `/camera_info` (camera_info_manager convention); the deploy sensor leg overrides it to the OpenRAL sibling layout `/openral/cameras//camera_info` so real cameras match the sim HAL (mono visual SLAM subscribes there). Reader lifecycle (open/close) is owned by the caller. (L79) - - `start() -> None` — Init rclpy if needed, create publishers, spawn the pump thread. (L190) - - `stop() -> None` — Signal the pump thread, tear down publishers + node; idempotent. (L259) - - prop `is_started`, `n_published`, `n_stale_skipped`, `topic`, `info_topic` — Diagnostics surface consumed by the `openral_sensors_ros` lifecycle node. (L166) + - `start() -> None` — Init rclpy if needed, create publishers, spawn the pump thread. (L251) + - `stop() -> None` — Signal the pump thread, tear down publishers + node; idempotent. (L272) + - prop `is_started`, `n_published`, `n_stale_skipped`, `topic`, `info_topic` — Diagnostics surface consumed by the `openral_sensors_ros` lifecycle node. (L169) ### `python/sensors/src/openral_sensors/_reader_protocol.py` _Internal Protocol shim mirroring `openral_runner.SensorReader` to avoid a sensors↔runner import cycle._ diff --git a/docs/methods/05-inference-runner.md b/docs/methods/05-inference-runner.md index 676dcec..8ba84b0 100644 --- a/docs/methods/05-inference-runner.md +++ b/docs/methods/05-inference-runner.md @@ -61,6 +61,27 @@ _:class:`OpenCVThreadSensorReader` — default backend. Mirrors lerobot's per-ca - `read_latest(max_age_ms: int | None = None) -> SensorFrame` — Lock-protected snapshot of the `_latest_frame` slot; constructs a `SensorFrame` with inlined raw bytes; raises `ROSPerceptionStale` on no-frame-yet or staleness, `RuntimeError` on closed reader. (L173) - `_read_loop()` — Background daemon: `cv2.VideoCapture.read` → `_latest_frame + _latest_stamp_*_ns` under lock; sleeps `1/fps` on read failure / EOF. (L228) +### `python/runner/src/openral_runner/backends/galaxea_a1_camera_bridge.py` +_Real-deploy reader for the public A1 Runtime paired-frame bridge. It never +opens a camera device or imports the Runtime checkout; the A1 camera monitor +stays the only RealSense owner._ + +- `class GalaxeaA1CameraBridgeReader` — Reads either the `front` or `wrist` + member of a native versioned Unix-socket session and returns an inline RGB8 + `SensorFrame`. The client discovers and pins the Runtime contract digest and + camera shapes before reading; stale pairs, wrong shapes, and drift fail + explicitly. + +### `python/runner/src/openral_runner/backends/galaxea_a1_ipc.py` +_Shared native transport for public A1 Runtime local services._ + +- `runtime_socket_path(name) -> Path` — Resolves a private per-user Runtime + endpoint from `A1_PROCESS_STATE_ROOT` or the standard runtime directory. +- `class RuntimeLocalClient` — Bounded, synchronous length-prefixed MessagePack + client with typed connect-time and request-time failures. +- `encode_array(value) -> dict[str, Any]` / `decode_array(...) -> NDArray` — + Exact shape/dtype/data ndarray wire helpers. + ### `python/runner/src/openral_runner/backends/__init__.py` _Per-backend `SensorReader` implementations. Default `OpenCVThreadSensorReader` is always available; `GStreamerSensorReader` (PR I) + `Ros2ImageSensorReader` gate on optional deps._ @@ -165,12 +186,18 @@ _Public surface of the inference runner. Imports are PEP 562 lazy (M8 PR I/8): h ### `python/runner/src/openral_runner/factory.py` _Library deploy runner used by runtime nodes; the public deploy CLI now shells the ROS graph from a `DeployScene`._ -- `SKILL_REGISTRY: dict[str, Callable[[dict[str, object]], rSkillBase]]` — `vla.id` → skill factory. Today: `hello`, `gpu_passthrough` (M8 PR I/10). (L84) -- `SENSOR_BACKEND_REGISTRY: dict[str, Callable[[SensorReaderConfig], SensorReader]]` — `backend` id → reader factory. Today: `opencv_thread`, `gstreamer`. (L246) -- `_to_int(value, *, field, sensor_id) -> int` — YAML `object` → `int` coercion helper used across factories; rejects bools explicitly. (L40) -- `_make_gpu_passthrough_skill(extra) -> rSkillBase` — Builds `GpuPassthroughSkill`; recognised `extra`: `sensor_id` (default `"wrist_rgb"`), `n_joints`, `horizon`, `device` (default `"cuda"`, raises if unavailable). (L61) -- `_make_opencv_thread_reader(cfg) -> SensorReader` — Builds `OpenCVThreadSensorReader` from a `SensorReaderConfig`; requires `backend_params.device`. (L90) -- `_make_gstreamer_reader(cfg) -> SensorReader` — Builds `GStreamerSensorReader` from a `SensorReaderConfig`. Translates `publish_to_ros` / `publish_topic` / `publish_rate_hz` → `PipelineSpec.enable_ros_tee`. (M8 PR I/2 + I/4.) (L130) +- `SKILL_REGISTRY: dict[str, Callable[[dict[str, object]], rSkillBase]]` — `vla.id` → skill factory. Today: `hello`, `gpu_passthrough` (M8 PR I/10). (L92) +- `SENSOR_BACKEND_REGISTRY: dict[str, Callable[[SensorReaderConfig], SensorReader]]` — `backend` id → reader factory. Today: `opencv_thread`, `gstreamer`, `galaxea_a1_camera_bridge`. (L325) +- `_to_int(value, *, field, sensor_id) -> int` — YAML `object` → `int` coercion helper used across factories; rejects bools explicitly. (L48) +- `_make_gpu_passthrough_skill(extra) -> rSkillBase` — Builds `GpuPassthroughSkill`; recognised `extra`: `sensor_id` (default `"wrist_rgb"`), `n_joints`, `horizon`, `device` (default `"cuda"`, raises if unavailable). (L69) +- `_make_opencv_thread_reader(cfg) -> SensorReader` — Builds `OpenCVThreadSensorReader` from a `SensorReaderConfig`; requires `backend_params.device`. (L98) +- `_make_gstreamer_reader(cfg) -> SensorReader` — Builds `GStreamerSensorReader` from a `SensorReaderConfig`. Translates `publish_to_ros` / `publish_topic` / `publish_rate_hz` → `PipelineSpec.enable_ros_tee`. (M8 PR I/2 + I/4.) (L138) +- `_make_galaxea_a1_camera_bridge_reader(cfg) -> SensorReader` — Builds the + native A1 Runtime paired-camera connector. Accepts only `camera`; unknown + values are rejected. +- `make_sensor_readers(configs) -> list[SensorReader]` — Batch constructor that + preserves config order and shares one A1 paired-camera session across both + views. Other backends still dispatch through `SENSOR_BACKEND_REGISTRY`. ### `python/runner/src/openral_runner/deploy_runner.py` _:class:`DeployRunner` — concrete `InferenceRunnerBase` subclass composing HAL + Skill + WorldStateAggregator + SensorReaders + SafetyClient._ diff --git a/docs/methods/07-eval-sim.md b/docs/methods/07-eval-sim.md index 654dc2a..67c0da2 100644 --- a/docs/methods/07-eval-sim.md +++ b/docs/methods/07-eval-sim.md @@ -402,6 +402,25 @@ _Robbyant LingBot-VLA 2.0 policy adapter (Apache-2.0 code + weights). Proxies th - `_build_lingbot_vla2(env_cfg)` / `_build_lingbot_vla(env_cfg)` — `@POLICIES.register("lingbot_vla2")` (6B) and `@POLICIES.register("lingbot_vla")` (4B RoboTwin post-train, `model_family: "lingbot_vla"` in `rskills/lingbot-vla-4b-robotwin/rskill.yaml`) thin wrappers over `_build_lingbot`. (L385) - `_policy_default_port(model_id, robo_name, variant="v2") -> int` / `_resolve_camera_keys(env_cfg, extra) -> tuple[str, ...]` / `_resolve_model_id(spec, extra, default_model_id) -> str` / `_locate_sidecar_script() -> Path` / `_opt_int(value, default) -> int` — per-(variant, model, embodiment) SHA-256-bucketed default port (20000–39999); scene-camera resolution (order load-bearing: top/left-wrist/right-wrist); model-id resolution (`vla.extra.model_id` > manifest `weights_uri` > `spec.weights_uri` > per-variant default); boot-helper locator; `vla.extra` int coercion. (L119) +#### `python/sim/src/openral_sim/policies/lingbot_va_a1.py` +_Thin LingBot-VA real-deployment adapter for the Galaxea A1 embodiment. It +connects to the Runtime-owned versioned policy gateway and returns its absolute +six-joint plus normalized-gripper proposals through OpenRAL's normal +candidate-action and safety path._ + +- `_LingBotVaA1Adapter(spec, *, robot_description)` — Validates the A1 + embodiment and rSkill model identity, negotiates the active OpenRAL joint + envelope and policy-owned step bound, sends joints plus paired RGB + observations, and validates the returned 7-D action proposal. Runtime owns + its exact deployment config, EEF transforms, chunk/cache replay, and IK. +- `_joint_contract(spec, robot_description)` — Extracts ordered finite command + limits and rejects a policy substep above either active HAL phase limit. +- `_model_identity(spec) -> tuple[str, str]` — Resolves the rSkill's immutable + model repo/revision for the gateway identity handshake. +- `_build_lingbot_va_a1(env_cfg) -> _LingBotVaA1Adapter` — + `@POLICIES.register("lingbot_va_a1")` factory. The adapter explicitly accepts + only the `galaxea_a1` embodiment. + #### `tools/lingbot_vla2_sidecar.py` + `tools/_lingbot_vla2_server.py` _Boot helper + server for the LingBot-VLA 2.0 sidecar, companion to `openral_sim.policies.lingbot_vla2`._ The boot helper runs under the openral interpreter: it clones `github.com/robbyant/lingbot-vla-v2` at the pinned SHA `69729b4` into `/source` (shallow-fetch of the exact commit; `$OPENRAL_LINGBOT_VLA2_REPO` reuses a checkout), builds a Python 3.12 venv from the upstream fully-pinned `requirements.txt` (torch==2.8.0 / transformers==4.57.3 / triton==3.4.0 / numpy==1.26.4) + `pyzmq` + `bitsandbytes` via the shared `ensure_pip_venv` (`$OPENRAL_LINGBOT_VLA2_SIDECAR_PYTHON` reuses an interpreter), stamps the sidecar identity record, then `os.execvpe`s into the server with `make_isolated_env`. The server (sidecar venv, no openral import) loads `LingbotVLAv2Server` (NF4 Qwen3-VL backbone via `_nf4_backbone_in_place`, bf16 MoE expert), reconstructs the missing `lingbotvla_cli.yaml` from `configs/vla/robotwin/robotwin.yaml`, and answers `ping`/`reset`/`get_action`/`close` over the same msgpack ndarray wire as `openral_sim.sidecar`. flash-attn is NOT installed; `_install_attn_fallback` / `_coerce_attn_config` patch the upstream `flash_attention_2` hardcode to `sdpa` (or `eager`) before the model is built. `_install_lerobot_stub` installs a meta-path finder that stubs the training-only `lerobot.*` imports the model class pulls in (uninstallable here — it wants transformers 5.x), and `_write_cli_yaml` stringifies the `joints`/`norm_type` entries the upstream loader `ast.literal_eval`s; both are required for the inference-only load to succeed (verified live). A `--variant {v2,v1}` switch selects the model family end-to-end. **v1** (LingBot-VLA 1.0, 4B) clones the separate V1 repo `github.com/robbyant/lingbot-vla` at pinned SHA `4eb34b7` and provisions its own `transformers==4.51.3` / `lerobot==0.4.2` (flat layout) venv via `_install_v1` — it uses **real lerobot** (no stub) and its checkpoint ships real `config.json` + `lingbotvla_cli.yaml` (no `_write_cli_yaml` reconstruction). The V1 model (`_LingBotV1Policy`, upstream `LingbotVLAServer`) is a Qwen2.5-VL-3B backbone + a *dense* Qwen2 flow-matching expert; three server-side patches make its flash-free path correct: inject the missing `rotate_half` into the eager vision attention, force `attn=eager` (its custom attention has no `sdpa` kernel), and skip `o_proj` in NF4 (the interleaved attention reads `o_proj.weight.dtype`). diff --git a/docs/methods/11-ros2-nodes.md b/docs/methods/11-ros2-nodes.md index 52be00d..2de2910 100644 --- a/docs/methods/11-ros2-nodes.md +++ b/docs/methods/11-ros2-nodes.md @@ -32,7 +32,7 @@ matching Python HAL class and call `openral_hal.lifecycle.make_lifecycle_main(.. - `packages/openral_rskill_ros/scripts/runtime_node` — Composed-runtime entry point installed as `lib/openral_rskill_ros/runtime_node`. Reads ROS parameter `robot_yaml` (absolute path), calls `compose_runtime`, and spins both lifecycle nodes on a `MultiThreadedExecutor`. Spawned once per `openral deploy sim/run` invocation by `sim_e2e.launch.py` (one generic launch — the per-robot `openarm_e2e.launch.py` / `so100_e2e.launch.py` were unified into `sim_e2e.launch.py` in 2026-05-24). The dashboard SLAM Map bridge is now always part of `compose_runtime`; there is no `enable_slam_bridge` launch parameter. Params `world_cloud_topic` + `slam_source_node` (both default `""`) thread the mono visual-SLAM dashboard wiring through to `compose_runtime` (nvblox voxel-cloud topic + SLAM-card source label). Real deploys (`openral deploy run`): the `deploy_config` parameter carries the DeployScene YAML path; when set the script opens every deploy-bound SensorSpec (robot manifest ∪ scene `sensors:`) via `open_deploy_sensor_readers` before spinning and closes them in the teardown `finally`. Script top eagerly runs `gi` + `Gst.init()` BEFORE any numpy/pydantic/rclpy import — under Fast-DDS a later `rclpy.Node()` segfaults when numpy/pydantic loaded first (x86 Ubuntu 24.04 / system PyGObject; bisected 2026-07-02). - `packages/openral_rskill_ros/launch/sim_e2e.launch.py` — Generic deploy ROS graph. Reasoner launch args are model-first: `reasoner_model` (default `OPENRAL_REASONER_MODEL` or curated `gpt-5.5`) and optional `reasoner_endpoint`; these become `OPENRAL_REASONER_{MODEL,ENDPOINT,MAX_TOKENS}` on the reasoner node. Deprecated `reasoner_provider` remains for one release and switches the node back to the legacy `OPENRAL_REASONER_LLM_*` shim. Launch arg `workcell_json` carries the `DeployScene` safety/ACM subset when non-empty; the launch parses it, calls `compute_intersection(robot, skill=None, deploy=...)`, applies `merge_extra_allowed_pairs`, and forwards the resulting kernel params. Visual-SLAM args: `slam_visual_impl`, `slam_stereo_cameras`, `slam_mono_camera` (non-empty → `_build_visual_slam_includes` composes the **mono RGBD** leg: pycuvslam in RGBD mode + the DA3 depth provider (framed at the camera's manifest `frame_id`) + nvblox — always, not gated on nav2), and `slam_depth_sidecar_autostart` (default true → an `ExecuteProcess` runs `tools/da3_depth_sidecar.py --port 5771`; a port-conflicting operator sidecar wins because the child's death never tears down the launch). - `packages/openral_rskill_ros/openral_rskill_ros/sensor_leg.py` — Real-mode camera leg for `openral deploy run` (the real-hardware counterpart of the sim HAL's `SimSensorBridge`). - - `open_deploy_sensor_readers(sensors, *, topic_prefix="/openral/cameras", aggregator=None) -> SensorLeg` (L113) — Opens one `SensorReader` per deploy-bound `SensorSpec` (specs without a `deploy_binding` are skipped) via the runner's `SENSOR_BACKEND_REGISTRY` and guarantees each camera publishes on `//image` (the WorldState subscription): `gstreamer` backends get the in-pipeline ROS tee forced on (`publish_to_ros=True`); other backends (`opencv_thread`) are wrapped in a polling `SensorRosPublisher` — stamped with the spec's manifest `frame_id` and, when the spec carries calibrated `intrinsics`, publishing a companion `CameraInfo` on `//camera_info` (the sim HAL's layout), which is what lets mono visual SLAM run on real hardware. When the composed runtime's shared `aggregator` is passed, every reader also gets an `_AggregatorPump` writing `read_latest()` frames straight into `WorldStateAggregator.update_image_frame` — zero-copy NVMM `SensorFrame.handle`s survive (no intra-process ROS round trip on the policy leg) — and the sensor lands in `SensorLeg.direct_sensors`. Half-open legs are closed before the raising error propagates. + - `open_deploy_sensor_readers(sensors, *, topic_prefix="/openral/cameras", aggregator=None) -> SensorLeg` (L113) — Builds the complete deploy-bound batch through `make_sensor_readers`, so multi-view backends can share explicitly scoped resources (the A1 front/wrist readers share one paired session) without module-global state. Specs without a `deploy_binding` are skipped. Every camera publishes on `//image`: `gstreamer` backends get the in-pipeline ROS tee forced on (`publish_to_ros=True`); other backends are wrapped in a polling `SensorRosPublisher`, with manifest frame/intrinsics and companion `CameraInfo`. When the composed runtime's shared `aggregator` is passed, every reader also gets an `_AggregatorPump` writing frames directly into `WorldStateAggregator.update_image_frame`; half-open legs are closed before errors propagate. - `_AggregatorPump` — daemon-thread pump (start/stop like `SensorRosPublisher`) polling `read_latest` at the spec rate into the aggregator; deduplicates on the frame's monotonic stamp so a re-polled latched frame never refreshes the staleness stamp. - `merge_deploy_sensors(manifest_sensors, scene_sensors) -> list[SensorSpec]` — Robot-manifest ∪ scene sensors with the scene entry winning on name collision (a same-named scene entry is that robot sensor's deploy binding — keeping both would double-open the device). - `SensorLeg` (L62) — dataclass holding `readers` + `publishers` + `direct_sensors` (forward to WorldState's `direct_image_frame_sensors` parameter so `_on_image` keeps serving observability but doesn't double-write the aggregator for directly-fed sensors); `close()` (L78) stops publishers first, then closes readers, idempotently and exception-safe (teardown must always reach the HAL shutdown behind it). diff --git a/docs/methods/12-tests-hil.md b/docs/methods/12-tests-hil.md index 784cbee..95d1455 100644 --- a/docs/methods/12-tests-hil.md +++ b/docs/methods/12-tests-hil.md @@ -9,6 +9,14 @@ injected `publish_fn` / `state_fn` callables. Off-lab they are guarded behind `importlib.util.find_spec("rclpy") is None` plus a per-robot env probe; the unit-lane conformance tests use `SimTransport` instead. +The Galaxea A1 uses an isolated ROS 1 sidecar rather than a +`ros2_control` bridge. Its HAL-level gate lives in +`tests/hil/test_galaxea_a1.py`; the separate +`tests/hil/test_galaxea_a1_deploy.py` gate runs inside an active deploy +container and proves a measured hold across the real C++ kernel, ROS 2 HAL, +and staged ROS 1 command relay. Both are environment-gated, time-bounded, and +end by stopping the sidecar through the downstream e-stop path. + ### `tests/hil/_ros_control_transport.py` _Single-controller bridge. Used by UR5e, UR10e, Franka Panda, Sawyer._ @@ -21,4 +29,3 @@ _4-way fan-out bridge for the bimanual ALOHA HAL._ - `AlohaHILTransport(node, joint_names, *, left_arm_command_topic, right_arm_command_topic, left_gripper_command_topic, right_gripper_command_topic, joint_state_topic)` — Owns four `JointTrajectory` publishers (two arms 6-DOF + two grippers 1-DOF — Trossen gripper modeled as a 1-DOF `JointTrajectoryController`) and one aggregated `JointState` subscriber. `publish(topic, msg)` dispatches by topic match; unknown topics raise `ValueError`. (L67) - `make_aloha_hil_transport(node_name, *, left/right arm + gripper command topics, joint_state_topic) -> tuple[Node, AlohaHILTransport, Callable[[], None]]` — Factory; derives the canonical 14 joint names from the public `ALOHA_REAL_DESCRIPTION.joints` so the bridge stays in lock-step with the HAL manifest. (L228) - diff --git a/docs/reference/robots.md b/docs/reference/robots.md index e35f4cd..6a1da65 100644 --- a/docs/reference/robots.md +++ b/docs/reference/robots.md @@ -8,6 +8,7 @@ Every embodiment is a typed `RobotDescription` manifest under `robots/ |---|---|---|---| | SO-100 (LeRobot follower arm) | [`robots/so100_follower/`](https://github.com/OpenRAL/openral/tree/master/robots/so100_follower/) | `SO100FollowerHAL` (real) + `SO100MujocoHAL` (sim) + `openral_hal_so100` lifecycle node | ✓ HW + sim | | SO-101 (LeRobot follower arm) | [`robots/so101_follower/`](https://github.com/OpenRAL/openral/tree/master/robots/so101_follower/) | shares SO-100 family — `SO100FollowerHAL` (real) + `SO100MujocoHAL` (sim) + `openral_hal_so100` lifecycle node | ✓ HW + sim | +| Galaxea A1 | [`robots/galaxea_a1/`](https://github.com/OpenRAL/openral/tree/master/robots/galaxea_a1/) | `GalaxeaA1HAL` (real-only) + isolated operator-provided ROS 1 SDK sidecar + `openral_hal_galaxea_a1` lifecycle node | ✓ unit + offline integration + real observation/hold/joint/gripper + C++ kernel HIL | | Franka Panda | [`robots/franka_panda/`](https://github.com/OpenRAL/openral/tree/master/robots/franka_panda/) | `FrankaPandaHAL` (`MujocoArmHAL`) + `openral_hal_franka` | ✓ sim · HW bring-up M3 (#56) | | UR5e | [`robots/ur5e/`](https://github.com/OpenRAL/openral/tree/master/robots/ur5e/) | `UR5eHAL` (`MujocoArmHAL`) + `openral_hal_ur5e` | ✓ sim · HW bring-up M3 | | UR10e | [`robots/ur10e/`](https://github.com/OpenRAL/openral/tree/master/robots/ur10e/) | `UR10eHAL` (`MujocoArmHAL`) + `openral_hal_ur10e` | ✓ sim · HW bring-up M3 | diff --git a/docs/reference/schemas/BenchmarkScene.json b/docs/reference/schemas/BenchmarkScene.json index 6c18d21..5633bed 100644 --- a/docs/reference/schemas/BenchmarkScene.json +++ b/docs/reference/schemas/BenchmarkScene.json @@ -140,6 +140,18 @@ "additionalProperties": false, "description": "Committed deploy-posture toggles for a workcell scene.\n\nEvery runtime leg ``openral deploy sim`` / ``deploy run`` can bring up is\ndeclared here so a committed :class:`DeployScene` pins its posture\nexplicitly instead of relying on per-invocation CLI flags. Precedence is\n``CLI flag > scene runtime > auto`` \u2014 an explicit CLI flag always wins,\nand ``None`` means \"auto\" (the manifest-derived / built-in default the CLI\ndocuments for that flag: SLAM from ``capabilities.has_lidar``/vision-SLAM,\nNav2 tracks SLAM, octomap from a depth SensorSpec, detector on,\nkernel-check on, reward monitor / critic off).\n\nHost-operational knobs (dashboard, foxglove, ports, dataset recording,\n``--initial-task``, ``--dry-run``) stay CLI-only \u2014 they describe the\ninvocation, not the workcell.\n\nRelative ``*_manifest`` / ``object_detector_onnx`` paths that exist next\nto the scene YAML are resolved against the scene's directory; otherwise\nthe value is passed through verbatim (repo-relative default / alias).", "properties": { + "enable_reasoner": { + "anyOf": [ + { + "type": "boolean" + }, + { + "type": "null" + } + ], + "default": null, + "title": "Enable Reasoner" + }, "enable_slam": { "anyOf": [ { @@ -827,12 +839,13 @@ "type": "string" }, "SensorReaderBackend": { - "description": "Which :class:`SensorReader` implementation to instantiate.\n\nFour backends are reserved. ``opencv_thread``\nis the default and mirrors lerobot's per-camera background-thread\npattern. ``ros2_image`` subscribes to a ROS 2 image topic published by\na vendor driver. ``gstreamer`` runs a GStreamer pipeline whose appsink\ndelivers frames (NVMM / DMA-BUF on Jetson; CPU bytes on x86).\n\n``holoscan`` is **reserved-but-unimplemented**: a 2026-05-12 evaluation\nof NVIDIA Holoscan SDK as a parallel ingest backbone\ndeferred adoption (lean GStreamer with custom NvBufSurface glue\nwon the comparison). The enum value exists so a future PR can add\nthe backend additively without bumping the schema again; configs\nthat select it today raise ``ROSConfigError`` at factory time.", + "description": "Which :class:`SensorReader` implementation to instantiate.\n\n``opencv_thread`` is the default and mirrors lerobot's per-camera\nbackground-thread pattern. ``galaxea_a1_camera_bridge`` consumes the\nversioned paired raw-frame service owned by an external A1 Runtime process\nwithout importing Runtime or opening either RealSense device. Three\nadditional backends are reserved.\n``ros2_image`` subscribes to a ROS 2 image topic published by\na vendor driver. ``gstreamer`` runs a GStreamer pipeline whose appsink\ndelivers frames (NVMM / DMA-BUF on Jetson; CPU bytes on x86).\n\n``holoscan`` is **reserved-but-unimplemented**: a 2026-05-12 evaluation\nof NVIDIA Holoscan SDK as a parallel ingest backbone\ndeferred adoption (lean GStreamer with custom NvBufSurface glue\nwon the comparison). The enum value exists so a future PR can add\nthe backend additively without bumping the schema again; configs\nthat select it today raise ``ROSConfigError`` at factory time.", "enum": [ "opencv_thread", "ros2_image", "gstreamer", - "holoscan" + "holoscan", + "galaxea_a1_camera_bridge" ], "title": "SensorReaderBackend", "type": "string" diff --git a/docs/reference/schemas/DeployScene.json b/docs/reference/schemas/DeployScene.json index 65b5340..404bf82 100644 --- a/docs/reference/schemas/DeployScene.json +++ b/docs/reference/schemas/DeployScene.json @@ -96,6 +96,18 @@ "additionalProperties": false, "description": "Committed deploy-posture toggles for a workcell scene.\n\nEvery runtime leg ``openral deploy sim`` / ``deploy run`` can bring up is\ndeclared here so a committed :class:`DeployScene` pins its posture\nexplicitly instead of relying on per-invocation CLI flags. Precedence is\n``CLI flag > scene runtime > auto`` \u2014 an explicit CLI flag always wins,\nand ``None`` means \"auto\" (the manifest-derived / built-in default the CLI\ndocuments for that flag: SLAM from ``capabilities.has_lidar``/vision-SLAM,\nNav2 tracks SLAM, octomap from a depth SensorSpec, detector on,\nkernel-check on, reward monitor / critic off).\n\nHost-operational knobs (dashboard, foxglove, ports, dataset recording,\n``--initial-task``, ``--dry-run``) stay CLI-only \u2014 they describe the\ninvocation, not the workcell.\n\nRelative ``*_manifest`` / ``object_detector_onnx`` paths that exist next\nto the scene YAML are resolved against the scene's directory; otherwise\nthe value is passed through verbatim (repo-relative default / alias).", "properties": { + "enable_reasoner": { + "anyOf": [ + { + "type": "boolean" + }, + { + "type": "null" + } + ], + "default": null, + "title": "Enable Reasoner" + }, "enable_slam": { "anyOf": [ { @@ -783,12 +795,13 @@ "type": "string" }, "SensorReaderBackend": { - "description": "Which :class:`SensorReader` implementation to instantiate.\n\nFour backends are reserved. ``opencv_thread``\nis the default and mirrors lerobot's per-camera background-thread\npattern. ``ros2_image`` subscribes to a ROS 2 image topic published by\na vendor driver. ``gstreamer`` runs a GStreamer pipeline whose appsink\ndelivers frames (NVMM / DMA-BUF on Jetson; CPU bytes on x86).\n\n``holoscan`` is **reserved-but-unimplemented**: a 2026-05-12 evaluation\nof NVIDIA Holoscan SDK as a parallel ingest backbone\ndeferred adoption (lean GStreamer with custom NvBufSurface glue\nwon the comparison). The enum value exists so a future PR can add\nthe backend additively without bumping the schema again; configs\nthat select it today raise ``ROSConfigError`` at factory time.", + "description": "Which :class:`SensorReader` implementation to instantiate.\n\n``opencv_thread`` is the default and mirrors lerobot's per-camera\nbackground-thread pattern. ``galaxea_a1_camera_bridge`` consumes the\nversioned paired raw-frame service owned by an external A1 Runtime process\nwithout importing Runtime or opening either RealSense device. Three\nadditional backends are reserved.\n``ros2_image`` subscribes to a ROS 2 image topic published by\na vendor driver. ``gstreamer`` runs a GStreamer pipeline whose appsink\ndelivers frames (NVMM / DMA-BUF on Jetson; CPU bytes on x86).\n\n``holoscan`` is **reserved-but-unimplemented**: a 2026-05-12 evaluation\nof NVIDIA Holoscan SDK as a parallel ingest backbone\ndeferred adoption (lean GStreamer with custom NvBufSurface glue\nwon the comparison). The enum value exists so a future PR can add\nthe backend additively without bumping the schema again; configs\nthat select it today raise ``ROSConfigError`` at factory time.", "enum": [ "opencv_thread", "ros2_image", "gstreamer", - "holoscan" + "holoscan", + "galaxea_a1_camera_bridge" ], "title": "SensorReaderBackend", "type": "string" diff --git a/docs/reference/schemas/RSkillManifest.json b/docs/reference/schemas/RSkillManifest.json index a1051ef..f612516 100644 --- a/docs/reference/schemas/RSkillManifest.json +++ b/docs/reference/schemas/RSkillManifest.json @@ -1312,6 +1312,7 @@ "openvla", "lingbot_vla2", "lingbot_vla", + "lingbot_va_a1", "internvla_n1" ], "type": "string" @@ -1332,6 +1333,7 @@ "custom", "franka_panda", "g1", + "galaxea_a1", "google_robot", "gr1", "h1", diff --git a/docs/reference/schemas/RobotDescription.json b/docs/reference/schemas/RobotDescription.json index ee71b80..cfac77b 100644 --- a/docs/reference/schemas/RobotDescription.json +++ b/docs/reference/schemas/RobotDescription.json @@ -1392,12 +1392,13 @@ "type": "string" }, "SensorReaderBackend": { - "description": "Which :class:`SensorReader` implementation to instantiate.\n\nFour backends are reserved. ``opencv_thread``\nis the default and mirrors lerobot's per-camera background-thread\npattern. ``ros2_image`` subscribes to a ROS 2 image topic published by\na vendor driver. ``gstreamer`` runs a GStreamer pipeline whose appsink\ndelivers frames (NVMM / DMA-BUF on Jetson; CPU bytes on x86).\n\n``holoscan`` is **reserved-but-unimplemented**: a 2026-05-12 evaluation\nof NVIDIA Holoscan SDK as a parallel ingest backbone\ndeferred adoption (lean GStreamer with custom NvBufSurface glue\nwon the comparison). The enum value exists so a future PR can add\nthe backend additively without bumping the schema again; configs\nthat select it today raise ``ROSConfigError`` at factory time.", + "description": "Which :class:`SensorReader` implementation to instantiate.\n\n``opencv_thread`` is the default and mirrors lerobot's per-camera\nbackground-thread pattern. ``galaxea_a1_camera_bridge`` consumes the\nversioned paired raw-frame service owned by an external A1 Runtime process\nwithout importing Runtime or opening either RealSense device. Three\nadditional backends are reserved.\n``ros2_image`` subscribes to a ROS 2 image topic published by\na vendor driver. ``gstreamer`` runs a GStreamer pipeline whose appsink\ndelivers frames (NVMM / DMA-BUF on Jetson; CPU bytes on x86).\n\n``holoscan`` is **reserved-but-unimplemented**: a 2026-05-12 evaluation\nof NVIDIA Holoscan SDK as a parallel ingest backbone\ndeferred adoption (lean GStreamer with custom NvBufSurface glue\nwon the comparison). The enum value exists so a future PR can add\nthe backend additively without bumping the schema again; configs\nthat select it today raise ``ROSConfigError`` at factory time.", "enum": [ "opencv_thread", "ros2_image", "gstreamer", - "holoscan" + "holoscan", + "galaxea_a1_camera_bridge" ], "title": "SensorReaderBackend", "type": "string" diff --git a/docs/reference/schemas/SensorBundle.json b/docs/reference/schemas/SensorBundle.json index 8470df0..2edfdc2 100644 --- a/docs/reference/schemas/SensorBundle.json +++ b/docs/reference/schemas/SensorBundle.json @@ -193,12 +193,13 @@ "type": "string" }, "SensorReaderBackend": { - "description": "Which :class:`SensorReader` implementation to instantiate.\n\nFour backends are reserved. ``opencv_thread``\nis the default and mirrors lerobot's per-camera background-thread\npattern. ``ros2_image`` subscribes to a ROS 2 image topic published by\na vendor driver. ``gstreamer`` runs a GStreamer pipeline whose appsink\ndelivers frames (NVMM / DMA-BUF on Jetson; CPU bytes on x86).\n\n``holoscan`` is **reserved-but-unimplemented**: a 2026-05-12 evaluation\nof NVIDIA Holoscan SDK as a parallel ingest backbone\ndeferred adoption (lean GStreamer with custom NvBufSurface glue\nwon the comparison). The enum value exists so a future PR can add\nthe backend additively without bumping the schema again; configs\nthat select it today raise ``ROSConfigError`` at factory time.", + "description": "Which :class:`SensorReader` implementation to instantiate.\n\n``opencv_thread`` is the default and mirrors lerobot's per-camera\nbackground-thread pattern. ``galaxea_a1_camera_bridge`` consumes the\nversioned paired raw-frame service owned by an external A1 Runtime process\nwithout importing Runtime or opening either RealSense device. Three\nadditional backends are reserved.\n``ros2_image`` subscribes to a ROS 2 image topic published by\na vendor driver. ``gstreamer`` runs a GStreamer pipeline whose appsink\ndelivers frames (NVMM / DMA-BUF on Jetson; CPU bytes on x86).\n\n``holoscan`` is **reserved-but-unimplemented**: a 2026-05-12 evaluation\nof NVIDIA Holoscan SDK as a parallel ingest backbone\ndeferred adoption (lean GStreamer with custom NvBufSurface glue\nwon the comparison). The enum value exists so a future PR can add\nthe backend additively without bumping the schema again; configs\nthat select it today raise ``ROSConfigError`` at factory time.", "enum": [ "opencv_thread", "ros2_image", "gstreamer", - "holoscan" + "holoscan", + "galaxea_a1_camera_bridge" ], "title": "SensorReaderBackend", "type": "string" diff --git a/docs/reference/schemas/SensorDeployBinding.json b/docs/reference/schemas/SensorDeployBinding.json index 73d36bd..6ca84eb 100644 --- a/docs/reference/schemas/SensorDeployBinding.json +++ b/docs/reference/schemas/SensorDeployBinding.json @@ -1,12 +1,13 @@ { "$defs": { "SensorReaderBackend": { - "description": "Which :class:`SensorReader` implementation to instantiate.\n\nFour backends are reserved. ``opencv_thread``\nis the default and mirrors lerobot's per-camera background-thread\npattern. ``ros2_image`` subscribes to a ROS 2 image topic published by\na vendor driver. ``gstreamer`` runs a GStreamer pipeline whose appsink\ndelivers frames (NVMM / DMA-BUF on Jetson; CPU bytes on x86).\n\n``holoscan`` is **reserved-but-unimplemented**: a 2026-05-12 evaluation\nof NVIDIA Holoscan SDK as a parallel ingest backbone\ndeferred adoption (lean GStreamer with custom NvBufSurface glue\nwon the comparison). The enum value exists so a future PR can add\nthe backend additively without bumping the schema again; configs\nthat select it today raise ``ROSConfigError`` at factory time.", + "description": "Which :class:`SensorReader` implementation to instantiate.\n\n``opencv_thread`` is the default and mirrors lerobot's per-camera\nbackground-thread pattern. ``galaxea_a1_camera_bridge`` consumes the\nversioned paired raw-frame service owned by an external A1 Runtime process\nwithout importing Runtime or opening either RealSense device. Three\nadditional backends are reserved.\n``ros2_image`` subscribes to a ROS 2 image topic published by\na vendor driver. ``gstreamer`` runs a GStreamer pipeline whose appsink\ndelivers frames (NVMM / DMA-BUF on Jetson; CPU bytes on x86).\n\n``holoscan`` is **reserved-but-unimplemented**: a 2026-05-12 evaluation\nof NVIDIA Holoscan SDK as a parallel ingest backbone\ndeferred adoption (lean GStreamer with custom NvBufSurface glue\nwon the comparison). The enum value exists so a future PR can add\nthe backend additively without bumping the schema again; configs\nthat select it today raise ``ROSConfigError`` at factory time.", "enum": [ "opencv_thread", "ros2_image", "gstreamer", - "holoscan" + "holoscan", + "galaxea_a1_camera_bridge" ], "title": "SensorReaderBackend", "type": "string" diff --git a/docs/reference/schemas/SensorReaderBackend.json b/docs/reference/schemas/SensorReaderBackend.json index 9067bf9..2c57a64 100644 --- a/docs/reference/schemas/SensorReaderBackend.json +++ b/docs/reference/schemas/SensorReaderBackend.json @@ -7,6 +7,7 @@ "opencv_thread", "ros2_image", "gstreamer", - "holoscan" + "holoscan", + "galaxea_a1_camera_bridge" ] } diff --git a/docs/reference/schemas/SensorReaderConfig.json b/docs/reference/schemas/SensorReaderConfig.json index b9d1247..edaa538 100644 --- a/docs/reference/schemas/SensorReaderConfig.json +++ b/docs/reference/schemas/SensorReaderConfig.json @@ -1,12 +1,13 @@ { "$defs": { "SensorReaderBackend": { - "description": "Which :class:`SensorReader` implementation to instantiate.\n\nFour backends are reserved. ``opencv_thread``\nis the default and mirrors lerobot's per-camera background-thread\npattern. ``ros2_image`` subscribes to a ROS 2 image topic published by\na vendor driver. ``gstreamer`` runs a GStreamer pipeline whose appsink\ndelivers frames (NVMM / DMA-BUF on Jetson; CPU bytes on x86).\n\n``holoscan`` is **reserved-but-unimplemented**: a 2026-05-12 evaluation\nof NVIDIA Holoscan SDK as a parallel ingest backbone\ndeferred adoption (lean GStreamer with custom NvBufSurface glue\nwon the comparison). The enum value exists so a future PR can add\nthe backend additively without bumping the schema again; configs\nthat select it today raise ``ROSConfigError`` at factory time.", + "description": "Which :class:`SensorReader` implementation to instantiate.\n\n``opencv_thread`` is the default and mirrors lerobot's per-camera\nbackground-thread pattern. ``galaxea_a1_camera_bridge`` consumes the\nversioned paired raw-frame service owned by an external A1 Runtime process\nwithout importing Runtime or opening either RealSense device. Three\nadditional backends are reserved.\n``ros2_image`` subscribes to a ROS 2 image topic published by\na vendor driver. ``gstreamer`` runs a GStreamer pipeline whose appsink\ndelivers frames (NVMM / DMA-BUF on Jetson; CPU bytes on x86).\n\n``holoscan`` is **reserved-but-unimplemented**: a 2026-05-12 evaluation\nof NVIDIA Holoscan SDK as a parallel ingest backbone\ndeferred adoption (lean GStreamer with custom NvBufSurface glue\nwon the comparison). The enum value exists so a future PR can add\nthe backend additively without bumping the schema again; configs\nthat select it today raise ``ROSConfigError`` at factory time.", "enum": [ "opencv_thread", "ros2_image", "gstreamer", - "holoscan" + "holoscan", + "galaxea_a1_camera_bridge" ], "title": "SensorReaderBackend", "type": "string" diff --git a/docs/reference/schemas/SensorSpec.json b/docs/reference/schemas/SensorSpec.json index 02aaebc..a46ba75 100644 --- a/docs/reference/schemas/SensorSpec.json +++ b/docs/reference/schemas/SensorSpec.json @@ -193,12 +193,13 @@ "type": "string" }, "SensorReaderBackend": { - "description": "Which :class:`SensorReader` implementation to instantiate.\n\nFour backends are reserved. ``opencv_thread``\nis the default and mirrors lerobot's per-camera background-thread\npattern. ``ros2_image`` subscribes to a ROS 2 image topic published by\na vendor driver. ``gstreamer`` runs a GStreamer pipeline whose appsink\ndelivers frames (NVMM / DMA-BUF on Jetson; CPU bytes on x86).\n\n``holoscan`` is **reserved-but-unimplemented**: a 2026-05-12 evaluation\nof NVIDIA Holoscan SDK as a parallel ingest backbone\ndeferred adoption (lean GStreamer with custom NvBufSurface glue\nwon the comparison). The enum value exists so a future PR can add\nthe backend additively without bumping the schema again; configs\nthat select it today raise ``ROSConfigError`` at factory time.", + "description": "Which :class:`SensorReader` implementation to instantiate.\n\n``opencv_thread`` is the default and mirrors lerobot's per-camera\nbackground-thread pattern. ``galaxea_a1_camera_bridge`` consumes the\nversioned paired raw-frame service owned by an external A1 Runtime process\nwithout importing Runtime or opening either RealSense device. Three\nadditional backends are reserved.\n``ros2_image`` subscribes to a ROS 2 image topic published by\na vendor driver. ``gstreamer`` runs a GStreamer pipeline whose appsink\ndelivers frames (NVMM / DMA-BUF on Jetson; CPU bytes on x86).\n\n``holoscan`` is **reserved-but-unimplemented**: a 2026-05-12 evaluation\nof NVIDIA Holoscan SDK as a parallel ingest backbone\ndeferred adoption (lean GStreamer with custom NvBufSurface glue\nwon the comparison). The enum value exists so a future PR can add\nthe backend additively without bumping the schema again; configs\nthat select it today raise ``ROSConfigError`` at factory time.", "enum": [ "opencv_thread", "ros2_image", "gstreamer", - "holoscan" + "holoscan", + "galaxea_a1_camera_bridge" ], "title": "SensorReaderBackend", "type": "string" diff --git a/docs/reference/schemas/SimScene.json b/docs/reference/schemas/SimScene.json index a6bcaa2..0a12154 100644 --- a/docs/reference/schemas/SimScene.json +++ b/docs/reference/schemas/SimScene.json @@ -140,6 +140,18 @@ "additionalProperties": false, "description": "Committed deploy-posture toggles for a workcell scene.\n\nEvery runtime leg ``openral deploy sim`` / ``deploy run`` can bring up is\ndeclared here so a committed :class:`DeployScene` pins its posture\nexplicitly instead of relying on per-invocation CLI flags. Precedence is\n``CLI flag > scene runtime > auto`` \u2014 an explicit CLI flag always wins,\nand ``None`` means \"auto\" (the manifest-derived / built-in default the CLI\ndocuments for that flag: SLAM from ``capabilities.has_lidar``/vision-SLAM,\nNav2 tracks SLAM, octomap from a depth SensorSpec, detector on,\nkernel-check on, reward monitor / critic off).\n\nHost-operational knobs (dashboard, foxglove, ports, dataset recording,\n``--initial-task``, ``--dry-run``) stay CLI-only \u2014 they describe the\ninvocation, not the workcell.\n\nRelative ``*_manifest`` / ``object_detector_onnx`` paths that exist next\nto the scene YAML are resolved against the scene's directory; otherwise\nthe value is passed through verbatim (repo-relative default / alias).", "properties": { + "enable_reasoner": { + "anyOf": [ + { + "type": "boolean" + }, + { + "type": "null" + } + ], + "default": null, + "title": "Enable Reasoner" + }, "enable_slam": { "anyOf": [ { @@ -827,12 +839,13 @@ "type": "string" }, "SensorReaderBackend": { - "description": "Which :class:`SensorReader` implementation to instantiate.\n\nFour backends are reserved. ``opencv_thread``\nis the default and mirrors lerobot's per-camera background-thread\npattern. ``ros2_image`` subscribes to a ROS 2 image topic published by\na vendor driver. ``gstreamer`` runs a GStreamer pipeline whose appsink\ndelivers frames (NVMM / DMA-BUF on Jetson; CPU bytes on x86).\n\n``holoscan`` is **reserved-but-unimplemented**: a 2026-05-12 evaluation\nof NVIDIA Holoscan SDK as a parallel ingest backbone\ndeferred adoption (lean GStreamer with custom NvBufSurface glue\nwon the comparison). The enum value exists so a future PR can add\nthe backend additively without bumping the schema again; configs\nthat select it today raise ``ROSConfigError`` at factory time.", + "description": "Which :class:`SensorReader` implementation to instantiate.\n\n``opencv_thread`` is the default and mirrors lerobot's per-camera\nbackground-thread pattern. ``galaxea_a1_camera_bridge`` consumes the\nversioned paired raw-frame service owned by an external A1 Runtime process\nwithout importing Runtime or opening either RealSense device. Three\nadditional backends are reserved.\n``ros2_image`` subscribes to a ROS 2 image topic published by\na vendor driver. ``gstreamer`` runs a GStreamer pipeline whose appsink\ndelivers frames (NVMM / DMA-BUF on Jetson; CPU bytes on x86).\n\n``holoscan`` is **reserved-but-unimplemented**: a 2026-05-12 evaluation\nof NVIDIA Holoscan SDK as a parallel ingest backbone\ndeferred adoption (lean GStreamer with custom NvBufSurface glue\nwon the comparison). The enum value exists so a future PR can add\nthe backend additively without bumping the schema again; configs\nthat select it today raise ``ROSConfigError`` at factory time.", "enum": [ "opencv_thread", "ros2_image", "gstreamer", - "holoscan" + "holoscan", + "galaxea_a1_camera_bridge" ], "title": "SensorReaderBackend", "type": "string" diff --git a/docs/reference/schemas/all.json b/docs/reference/schemas/all.json index 4fae408..3ebfda4 100644 --- a/docs/reference/schemas/all.json +++ b/docs/reference/schemas/all.json @@ -155,7 +155,8 @@ "opencv_thread", "ros2_image", "gstreamer", - "holoscan" + "holoscan", + "galaxea_a1_camera_bridge" ] }, "DeadlineOverrunPolicy": { @@ -514,12 +515,13 @@ "type": "string" }, "SensorReaderBackend": { - "description": "Which :class:`SensorReader` implementation to instantiate.\n\nFour backends are reserved. ``opencv_thread``\nis the default and mirrors lerobot's per-camera background-thread\npattern. ``ros2_image`` subscribes to a ROS 2 image topic published by\na vendor driver. ``gstreamer`` runs a GStreamer pipeline whose appsink\ndelivers frames (NVMM / DMA-BUF on Jetson; CPU bytes on x86).\n\n``holoscan`` is **reserved-but-unimplemented**: a 2026-05-12 evaluation\nof NVIDIA Holoscan SDK as a parallel ingest backbone\ndeferred adoption (lean GStreamer with custom NvBufSurface glue\nwon the comparison). The enum value exists so a future PR can add\nthe backend additively without bumping the schema again; configs\nthat select it today raise ``ROSConfigError`` at factory time.", + "description": "Which :class:`SensorReader` implementation to instantiate.\n\n``opencv_thread`` is the default and mirrors lerobot's per-camera\nbackground-thread pattern. ``galaxea_a1_camera_bridge`` consumes the\nversioned paired raw-frame service owned by an external A1 Runtime process\nwithout importing Runtime or opening either RealSense device. Three\nadditional backends are reserved.\n``ros2_image`` subscribes to a ROS 2 image topic published by\na vendor driver. ``gstreamer`` runs a GStreamer pipeline whose appsink\ndelivers frames (NVMM / DMA-BUF on Jetson; CPU bytes on x86).\n\n``holoscan`` is **reserved-but-unimplemented**: a 2026-05-12 evaluation\nof NVIDIA Holoscan SDK as a parallel ingest backbone\ndeferred adoption (lean GStreamer with custom NvBufSurface glue\nwon the comparison). The enum value exists so a future PR can add\nthe backend additively without bumping the schema again; configs\nthat select it today raise ``ROSConfigError`` at factory time.", "enum": [ "opencv_thread", "ros2_image", "gstreamer", - "holoscan" + "holoscan", + "galaxea_a1_camera_bridge" ], "title": "SensorReaderBackend", "type": "string" @@ -1071,12 +1073,13 @@ "type": "string" }, "SensorReaderBackend": { - "description": "Which :class:`SensorReader` implementation to instantiate.\n\nFour backends are reserved. ``opencv_thread``\nis the default and mirrors lerobot's per-camera background-thread\npattern. ``ros2_image`` subscribes to a ROS 2 image topic published by\na vendor driver. ``gstreamer`` runs a GStreamer pipeline whose appsink\ndelivers frames (NVMM / DMA-BUF on Jetson; CPU bytes on x86).\n\n``holoscan`` is **reserved-but-unimplemented**: a 2026-05-12 evaluation\nof NVIDIA Holoscan SDK as a parallel ingest backbone\ndeferred adoption (lean GStreamer with custom NvBufSurface glue\nwon the comparison). The enum value exists so a future PR can add\nthe backend additively without bumping the schema again; configs\nthat select it today raise ``ROSConfigError`` at factory time.", + "description": "Which :class:`SensorReader` implementation to instantiate.\n\n``opencv_thread`` is the default and mirrors lerobot's per-camera\nbackground-thread pattern. ``galaxea_a1_camera_bridge`` consumes the\nversioned paired raw-frame service owned by an external A1 Runtime process\nwithout importing Runtime or opening either RealSense device. Three\nadditional backends are reserved.\n``ros2_image`` subscribes to a ROS 2 image topic published by\na vendor driver. ``gstreamer`` runs a GStreamer pipeline whose appsink\ndelivers frames (NVMM / DMA-BUF on Jetson; CPU bytes on x86).\n\n``holoscan`` is **reserved-but-unimplemented**: a 2026-05-12 evaluation\nof NVIDIA Holoscan SDK as a parallel ingest backbone\ndeferred adoption (lean GStreamer with custom NvBufSurface glue\nwon the comparison). The enum value exists so a future PR can add\nthe backend additively without bumping the schema again; configs\nthat select it today raise ``ROSConfigError`` at factory time.", "enum": [ "opencv_thread", "ros2_image", "gstreamer", - "holoscan" + "holoscan", + "galaxea_a1_camera_bridge" ], "title": "SensorReaderBackend", "type": "string" @@ -3607,12 +3610,13 @@ "type": "string" }, "SensorReaderBackend": { - "description": "Which :class:`SensorReader` implementation to instantiate.\n\nFour backends are reserved. ``opencv_thread``\nis the default and mirrors lerobot's per-camera background-thread\npattern. ``ros2_image`` subscribes to a ROS 2 image topic published by\na vendor driver. ``gstreamer`` runs a GStreamer pipeline whose appsink\ndelivers frames (NVMM / DMA-BUF on Jetson; CPU bytes on x86).\n\n``holoscan`` is **reserved-but-unimplemented**: a 2026-05-12 evaluation\nof NVIDIA Holoscan SDK as a parallel ingest backbone\ndeferred adoption (lean GStreamer with custom NvBufSurface glue\nwon the comparison). The enum value exists so a future PR can add\nthe backend additively without bumping the schema again; configs\nthat select it today raise ``ROSConfigError`` at factory time.", + "description": "Which :class:`SensorReader` implementation to instantiate.\n\n``opencv_thread`` is the default and mirrors lerobot's per-camera\nbackground-thread pattern. ``galaxea_a1_camera_bridge`` consumes the\nversioned paired raw-frame service owned by an external A1 Runtime process\nwithout importing Runtime or opening either RealSense device. Three\nadditional backends are reserved.\n``ros2_image`` subscribes to a ROS 2 image topic published by\na vendor driver. ``gstreamer`` runs a GStreamer pipeline whose appsink\ndelivers frames (NVMM / DMA-BUF on Jetson; CPU bytes on x86).\n\n``holoscan`` is **reserved-but-unimplemented**: a 2026-05-12 evaluation\nof NVIDIA Holoscan SDK as a parallel ingest backbone\ndeferred adoption (lean GStreamer with custom NvBufSurface glue\nwon the comparison). The enum value exists so a future PR can add\nthe backend additively without bumping the schema again; configs\nthat select it today raise ``ROSConfigError`` at factory time.", "enum": [ "opencv_thread", "ros2_image", "gstreamer", - "holoscan" + "holoscan", + "galaxea_a1_camera_bridge" ], "title": "SensorReaderBackend", "type": "string" @@ -6182,6 +6186,7 @@ "openvla", "lingbot_vla2", "lingbot_vla", + "lingbot_va_a1", "internvla_n1" ], "type": "string" @@ -6202,6 +6207,7 @@ "custom", "franka_panda", "g1", + "galaxea_a1", "google_robot", "gr1", "h1", @@ -8851,6 +8857,18 @@ "additionalProperties": false, "description": "Committed deploy-posture toggles for a workcell scene.\n\nEvery runtime leg ``openral deploy sim`` / ``deploy run`` can bring up is\ndeclared here so a committed :class:`DeployScene` pins its posture\nexplicitly instead of relying on per-invocation CLI flags. Precedence is\n``CLI flag > scene runtime > auto`` \u2014 an explicit CLI flag always wins,\nand ``None`` means \"auto\" (the manifest-derived / built-in default the CLI\ndocuments for that flag: SLAM from ``capabilities.has_lidar``/vision-SLAM,\nNav2 tracks SLAM, octomap from a depth SensorSpec, detector on,\nkernel-check on, reward monitor / critic off).\n\nHost-operational knobs (dashboard, foxglove, ports, dataset recording,\n``--initial-task``, ``--dry-run``) stay CLI-only \u2014 they describe the\ninvocation, not the workcell.\n\nRelative ``*_manifest`` / ``object_detector_onnx`` paths that exist next\nto the scene YAML are resolved against the scene's directory; otherwise\nthe value is passed through verbatim (repo-relative default / alias).", "properties": { + "enable_reasoner": { + "anyOf": [ + { + "type": "boolean" + }, + { + "type": "null" + } + ], + "default": null, + "title": "Enable Reasoner" + }, "enable_slam": { "anyOf": [ { @@ -9538,12 +9556,13 @@ "type": "string" }, "SensorReaderBackend": { - "description": "Which :class:`SensorReader` implementation to instantiate.\n\nFour backends are reserved. ``opencv_thread``\nis the default and mirrors lerobot's per-camera background-thread\npattern. ``ros2_image`` subscribes to a ROS 2 image topic published by\na vendor driver. ``gstreamer`` runs a GStreamer pipeline whose appsink\ndelivers frames (NVMM / DMA-BUF on Jetson; CPU bytes on x86).\n\n``holoscan`` is **reserved-but-unimplemented**: a 2026-05-12 evaluation\nof NVIDIA Holoscan SDK as a parallel ingest backbone\ndeferred adoption (lean GStreamer with custom NvBufSurface glue\nwon the comparison). The enum value exists so a future PR can add\nthe backend additively without bumping the schema again; configs\nthat select it today raise ``ROSConfigError`` at factory time.", + "description": "Which :class:`SensorReader` implementation to instantiate.\n\n``opencv_thread`` is the default and mirrors lerobot's per-camera\nbackground-thread pattern. ``galaxea_a1_camera_bridge`` consumes the\nversioned paired raw-frame service owned by an external A1 Runtime process\nwithout importing Runtime or opening either RealSense device. Three\nadditional backends are reserved.\n``ros2_image`` subscribes to a ROS 2 image topic published by\na vendor driver. ``gstreamer`` runs a GStreamer pipeline whose appsink\ndelivers frames (NVMM / DMA-BUF on Jetson; CPU bytes on x86).\n\n``holoscan`` is **reserved-but-unimplemented**: a 2026-05-12 evaluation\nof NVIDIA Holoscan SDK as a parallel ingest backbone\ndeferred adoption (lean GStreamer with custom NvBufSurface glue\nwon the comparison). The enum value exists so a future PR can add\nthe backend additively without bumping the schema again; configs\nthat select it today raise ``ROSConfigError`` at factory time.", "enum": [ "opencv_thread", "ros2_image", "gstreamer", - "holoscan" + "holoscan", + "galaxea_a1_camera_bridge" ], "title": "SensorReaderBackend", "type": "string" @@ -10159,6 +10178,18 @@ "additionalProperties": false, "description": "Committed deploy-posture toggles for a workcell scene.\n\nEvery runtime leg ``openral deploy sim`` / ``deploy run`` can bring up is\ndeclared here so a committed :class:`DeployScene` pins its posture\nexplicitly instead of relying on per-invocation CLI flags. Precedence is\n``CLI flag > scene runtime > auto`` \u2014 an explicit CLI flag always wins,\nand ``None`` means \"auto\" (the manifest-derived / built-in default the CLI\ndocuments for that flag: SLAM from ``capabilities.has_lidar``/vision-SLAM,\nNav2 tracks SLAM, octomap from a depth SensorSpec, detector on,\nkernel-check on, reward monitor / critic off).\n\nHost-operational knobs (dashboard, foxglove, ports, dataset recording,\n``--initial-task``, ``--dry-run``) stay CLI-only \u2014 they describe the\ninvocation, not the workcell.\n\nRelative ``*_manifest`` / ``object_detector_onnx`` paths that exist next\nto the scene YAML are resolved against the scene's directory; otherwise\nthe value is passed through verbatim (repo-relative default / alias).", "properties": { + "enable_reasoner": { + "anyOf": [ + { + "type": "boolean" + }, + { + "type": "null" + } + ], + "default": null, + "title": "Enable Reasoner" + }, "enable_slam": { "anyOf": [ { @@ -10846,12 +10877,13 @@ "type": "string" }, "SensorReaderBackend": { - "description": "Which :class:`SensorReader` implementation to instantiate.\n\nFour backends are reserved. ``opencv_thread``\nis the default and mirrors lerobot's per-camera background-thread\npattern. ``ros2_image`` subscribes to a ROS 2 image topic published by\na vendor driver. ``gstreamer`` runs a GStreamer pipeline whose appsink\ndelivers frames (NVMM / DMA-BUF on Jetson; CPU bytes on x86).\n\n``holoscan`` is **reserved-but-unimplemented**: a 2026-05-12 evaluation\nof NVIDIA Holoscan SDK as a parallel ingest backbone\ndeferred adoption (lean GStreamer with custom NvBufSurface glue\nwon the comparison). The enum value exists so a future PR can add\nthe backend additively without bumping the schema again; configs\nthat select it today raise ``ROSConfigError`` at factory time.", + "description": "Which :class:`SensorReader` implementation to instantiate.\n\n``opencv_thread`` is the default and mirrors lerobot's per-camera\nbackground-thread pattern. ``galaxea_a1_camera_bridge`` consumes the\nversioned paired raw-frame service owned by an external A1 Runtime process\nwithout importing Runtime or opening either RealSense device. Three\nadditional backends are reserved.\n``ros2_image`` subscribes to a ROS 2 image topic published by\na vendor driver. ``gstreamer`` runs a GStreamer pipeline whose appsink\ndelivers frames (NVMM / DMA-BUF on Jetson; CPU bytes on x86).\n\n``holoscan`` is **reserved-but-unimplemented**: a 2026-05-12 evaluation\nof NVIDIA Holoscan SDK as a parallel ingest backbone\ndeferred adoption (lean GStreamer with custom NvBufSurface glue\nwon the comparison). The enum value exists so a future PR can add\nthe backend additively without bumping the schema again; configs\nthat select it today raise ``ROSConfigError`` at factory time.", "enum": [ "opencv_thread", "ros2_image", "gstreamer", - "holoscan" + "holoscan", + "galaxea_a1_camera_bridge" ], "title": "SensorReaderBackend", "type": "string" @@ -11611,6 +11643,18 @@ "additionalProperties": false, "description": "Committed deploy-posture toggles for a workcell scene.\n\nEvery runtime leg ``openral deploy sim`` / ``deploy run`` can bring up is\ndeclared here so a committed :class:`DeployScene` pins its posture\nexplicitly instead of relying on per-invocation CLI flags. Precedence is\n``CLI flag > scene runtime > auto`` \u2014 an explicit CLI flag always wins,\nand ``None`` means \"auto\" (the manifest-derived / built-in default the CLI\ndocuments for that flag: SLAM from ``capabilities.has_lidar``/vision-SLAM,\nNav2 tracks SLAM, octomap from a depth SensorSpec, detector on,\nkernel-check on, reward monitor / critic off).\n\nHost-operational knobs (dashboard, foxglove, ports, dataset recording,\n``--initial-task``, ``--dry-run``) stay CLI-only \u2014 they describe the\ninvocation, not the workcell.\n\nRelative ``*_manifest`` / ``object_detector_onnx`` paths that exist next\nto the scene YAML are resolved against the scene's directory; otherwise\nthe value is passed through verbatim (repo-relative default / alias).", "properties": { + "enable_reasoner": { + "anyOf": [ + { + "type": "boolean" + }, + { + "type": "null" + } + ], + "default": null, + "title": "Enable Reasoner" + }, "enable_slam": { "anyOf": [ { @@ -12298,12 +12342,13 @@ "type": "string" }, "SensorReaderBackend": { - "description": "Which :class:`SensorReader` implementation to instantiate.\n\nFour backends are reserved. ``opencv_thread``\nis the default and mirrors lerobot's per-camera background-thread\npattern. ``ros2_image`` subscribes to a ROS 2 image topic published by\na vendor driver. ``gstreamer`` runs a GStreamer pipeline whose appsink\ndelivers frames (NVMM / DMA-BUF on Jetson; CPU bytes on x86).\n\n``holoscan`` is **reserved-but-unimplemented**: a 2026-05-12 evaluation\nof NVIDIA Holoscan SDK as a parallel ingest backbone\ndeferred adoption (lean GStreamer with custom NvBufSurface glue\nwon the comparison). The enum value exists so a future PR can add\nthe backend additively without bumping the schema again; configs\nthat select it today raise ``ROSConfigError`` at factory time.", + "description": "Which :class:`SensorReader` implementation to instantiate.\n\n``opencv_thread`` is the default and mirrors lerobot's per-camera\nbackground-thread pattern. ``galaxea_a1_camera_bridge`` consumes the\nversioned paired raw-frame service owned by an external A1 Runtime process\nwithout importing Runtime or opening either RealSense device. Three\nadditional backends are reserved.\n``ros2_image`` subscribes to a ROS 2 image topic published by\na vendor driver. ``gstreamer`` runs a GStreamer pipeline whose appsink\ndelivers frames (NVMM / DMA-BUF on Jetson; CPU bytes on x86).\n\n``holoscan`` is **reserved-but-unimplemented**: a 2026-05-12 evaluation\nof NVIDIA Holoscan SDK as a parallel ingest backbone\ndeferred adoption (lean GStreamer with custom NvBufSurface glue\nwon the comparison). The enum value exists so a future PR can add\nthe backend additively without bumping the schema again; configs\nthat select it today raise ``ROSConfigError`` at factory time.", "enum": [ "opencv_thread", "ros2_image", "gstreamer", - "holoscan" + "holoscan", + "galaxea_a1_camera_bridge" ], "title": "SensorReaderBackend", "type": "string" @@ -13246,12 +13291,13 @@ "SensorReaderConfig": { "$defs": { "SensorReaderBackend": { - "description": "Which :class:`SensorReader` implementation to instantiate.\n\nFour backends are reserved. ``opencv_thread``\nis the default and mirrors lerobot's per-camera background-thread\npattern. ``ros2_image`` subscribes to a ROS 2 image topic published by\na vendor driver. ``gstreamer`` runs a GStreamer pipeline whose appsink\ndelivers frames (NVMM / DMA-BUF on Jetson; CPU bytes on x86).\n\n``holoscan`` is **reserved-but-unimplemented**: a 2026-05-12 evaluation\nof NVIDIA Holoscan SDK as a parallel ingest backbone\ndeferred adoption (lean GStreamer with custom NvBufSurface glue\nwon the comparison). The enum value exists so a future PR can add\nthe backend additively without bumping the schema again; configs\nthat select it today raise ``ROSConfigError`` at factory time.", + "description": "Which :class:`SensorReader` implementation to instantiate.\n\n``opencv_thread`` is the default and mirrors lerobot's per-camera\nbackground-thread pattern. ``galaxea_a1_camera_bridge`` consumes the\nversioned paired raw-frame service owned by an external A1 Runtime process\nwithout importing Runtime or opening either RealSense device. Three\nadditional backends are reserved.\n``ros2_image`` subscribes to a ROS 2 image topic published by\na vendor driver. ``gstreamer`` runs a GStreamer pipeline whose appsink\ndelivers frames (NVMM / DMA-BUF on Jetson; CPU bytes on x86).\n\n``holoscan`` is **reserved-but-unimplemented**: a 2026-05-12 evaluation\nof NVIDIA Holoscan SDK as a parallel ingest backbone\ndeferred adoption (lean GStreamer with custom NvBufSurface glue\nwon the comparison). The enum value exists so a future PR can add\nthe backend additively without bumping the schema again; configs\nthat select it today raise ``ROSConfigError`` at factory time.", "enum": [ "opencv_thread", "ros2_image", "gstreamer", - "holoscan" + "holoscan", + "galaxea_a1_camera_bridge" ], "title": "SensorReaderBackend", "type": "string" @@ -13320,12 +13366,13 @@ "SensorDeployBinding": { "$defs": { "SensorReaderBackend": { - "description": "Which :class:`SensorReader` implementation to instantiate.\n\nFour backends are reserved. ``opencv_thread``\nis the default and mirrors lerobot's per-camera background-thread\npattern. ``ros2_image`` subscribes to a ROS 2 image topic published by\na vendor driver. ``gstreamer`` runs a GStreamer pipeline whose appsink\ndelivers frames (NVMM / DMA-BUF on Jetson; CPU bytes on x86).\n\n``holoscan`` is **reserved-but-unimplemented**: a 2026-05-12 evaluation\nof NVIDIA Holoscan SDK as a parallel ingest backbone\ndeferred adoption (lean GStreamer with custom NvBufSurface glue\nwon the comparison). The enum value exists so a future PR can add\nthe backend additively without bumping the schema again; configs\nthat select it today raise ``ROSConfigError`` at factory time.", + "description": "Which :class:`SensorReader` implementation to instantiate.\n\n``opencv_thread`` is the default and mirrors lerobot's per-camera\nbackground-thread pattern. ``galaxea_a1_camera_bridge`` consumes the\nversioned paired raw-frame service owned by an external A1 Runtime process\nwithout importing Runtime or opening either RealSense device. Three\nadditional backends are reserved.\n``ros2_image`` subscribes to a ROS 2 image topic published by\na vendor driver. ``gstreamer`` runs a GStreamer pipeline whose appsink\ndelivers frames (NVMM / DMA-BUF on Jetson; CPU bytes on x86).\n\n``holoscan`` is **reserved-but-unimplemented**: a 2026-05-12 evaluation\nof NVIDIA Holoscan SDK as a parallel ingest backbone\ndeferred adoption (lean GStreamer with custom NvBufSurface glue\nwon the comparison). The enum value exists so a future PR can add\nthe backend additively without bumping the schema again; configs\nthat select it today raise ``ROSConfigError`` at factory time.", "enum": [ "opencv_thread", "ros2_image", "gstreamer", - "holoscan" + "holoscan", + "galaxea_a1_camera_bridge" ], "title": "SensorReaderBackend", "type": "string" diff --git a/packages/openral_hal_galaxea_a1/CMakeLists.txt b/packages/openral_hal_galaxea_a1/CMakeLists.txt new file mode 100644 index 0000000..f9a92c8 --- /dev/null +++ b/packages/openral_hal_galaxea_a1/CMakeLists.txt @@ -0,0 +1,25 @@ +cmake_minimum_required(VERSION 3.8) +project(openral_hal_galaxea_a1) + +find_package(ament_cmake REQUIRED) +find_package(ament_cmake_python REQUIRED) + +ament_python_install_package(${PROJECT_NAME}) + +install( + PROGRAMS openral_hal_galaxea_a1/lifecycle_node.py + DESTINATION lib/${PROJECT_NAME} +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + find_package(ament_cmake_pytest REQUIRED) + set(ament_cmake_copyright_FOUND TRUE) + ament_lint_auto_find_test_dependencies() + + ament_add_pytest_test(test_lifecycle_safety test/test_lifecycle_safety.py + TIMEOUT 60 + ) +endif() + +ament_package() diff --git a/packages/openral_hal_galaxea_a1/README.md b/packages/openral_hal_galaxea_a1/README.md new file mode 100644 index 0000000..04abfec --- /dev/null +++ b/packages/openral_hal_galaxea_a1/README.md @@ -0,0 +1,49 @@ +# `openral_hal_galaxea_a1` + +ROS 2 lifecycle host for the real-only Galaxea A1 HAL. The Python HAL talks to +`tools/galaxea_a1_ros1_sidecar.py`; the sidecar runs in an operator-provisioned +ROS Noetic environment and owns the official SDK driver and joint tracker. +The HAL accepts only literal IPv4 loopback sidecar addresses and strict integer +TCP ports; remote, IPv6, and DNS-resolved endpoints are rejected before any +connection attempt. + +The vendor SDK is not bundled. See `docs/methods/01-hal.md` for the deployment +and hardware bring-up sequence. + +Before opening the serial device, run the launcher's read-only preflight: + +```bash +tools/run_galaxea_a1_sidecar.sh \ + --image openral/galaxea-a1-sidecar:noetic \ + --sdk-root /absolute/path/to/A1_SDK \ + --serial /dev/a1 \ + --check-only +``` + +The SDK mount stays read-only. The official joint tracker writes its generated +CppAD artifacts only to the launcher's dedicated XDG cache under the operator's +home directory. + +The lab-gated HIL test uses one sidecar session and ends by stopping the owned +ROS 1 stack: + +```bash +GALAXEA_A1_HIL=1 just hil galaxea_a1 +``` + +Set `GALAXEA_A1_ALLOW_HOLD=1` only after the observation-only run passes. It +replays the measured current joint pose and verifies less than one degree of +drift. A feedback value within the tracked endpoint tolerance is projected to +the exact command limit; a larger projection is rejected before publication. + +After the hold gate passes, `GALAXEA_A1_ALLOW_NUDGE=1` performs a bounded +`arm_joint1` +0.01 rad round trip, while `GALAXEA_A1_ALLOW_GRIPPER=1` performs +the official G2 example's 10 mm gripper step and returns to the measured +opening. Each gate implies the hold gate and ends through the downstream +e-stop path. + +The final deployment gate is `tests/hil/test_galaxea_a1_deploy.py`. Run it +inside an active `openral deploy run` container with both +`GALAXEA_A1_DEPLOY_HIL=1` and `GALAXEA_A1_ALLOW_HOLD=1`. It verifies the +measured hold through the C++ Safety Kernel and ROS 1 relay, then intentionally +ends the session through `/openral/estop`. diff --git a/packages/openral_hal_galaxea_a1/openral_hal_galaxea_a1/__init__.py b/packages/openral_hal_galaxea_a1/openral_hal_galaxea_a1/__init__.py new file mode 100644 index 0000000..2da1bb6 --- /dev/null +++ b/packages/openral_hal_galaxea_a1/openral_hal_galaxea_a1/__init__.py @@ -0,0 +1 @@ +"""ROS 2 lifecycle package for the Galaxea A1 HAL.""" diff --git a/packages/openral_hal_galaxea_a1/openral_hal_galaxea_a1/lifecycle_node.py b/packages/openral_hal_galaxea_a1/openral_hal_galaxea_a1/lifecycle_node.py new file mode 100755 index 0000000..72da8a1 --- /dev/null +++ b/packages/openral_hal_galaxea_a1/openral_hal_galaxea_a1/lifecycle_node.py @@ -0,0 +1,9 @@ +#!/usr/bin/env python3 +"""Manifest-driven lifecycle entrypoint for the Galaxea A1 real HAL.""" + +from openral_hal.lifecycle import make_lifecycle_main_from_manifest + +main = make_lifecycle_main_from_manifest(node_name="openral_hal_galaxea_a1") + +if __name__ == "__main__": + main() diff --git a/packages/openral_hal_galaxea_a1/package.xml b/packages/openral_hal_galaxea_a1/package.xml new file mode 100644 index 0000000..bf1e147 --- /dev/null +++ b/packages/openral_hal_galaxea_a1/package.xml @@ -0,0 +1,31 @@ + + + + openral_hal_galaxea_a1 + 0.1.0 + Manifest-driven OpenRAL lifecycle node for the Galaxea A1 arm. + OpenRAL Contributors + Apache-2.0 + + ament_cmake + ament_cmake_python + rclpy + sensor_msgs + trajectory_msgs + diagnostic_msgs + std_msgs + openral_msgs + lifecycle_msgs + rclpy_lifecycle + python3-openral-hal + python3-openral-observability + ament_copyright + ament_cmake_pytest + ament_flake8 + ament_pep257 + python3-pytest + + + ament_cmake + + diff --git a/packages/openral_hal_galaxea_a1/test/test_lifecycle_safety.py b/packages/openral_hal_galaxea_a1/test/test_lifecycle_safety.py new file mode 100644 index 0000000..0c11b1c --- /dev/null +++ b/packages/openral_hal_galaxea_a1/test/test_lifecycle_safety.py @@ -0,0 +1,220 @@ +"""Hardware-free lifecycle safety wiring for the Galaxea A1 package.""" + +from __future__ import annotations + +from types import SimpleNamespace +from typing import cast + +import pytest + +rclpy = pytest.importorskip("rclpy") + +from openral_core.exceptions import ROSEStopRequested +from openral_hal.lifecycle import ManifestHALLifecycleNode +from openral_hal.protocol import HAL, EStopRecovery, HALHealthReport +from openral_observability import Level + + +class _FakeRealHAL: + description = SimpleNamespace(name="galaxea_a1") + estop_recovery = EStopRecovery.RESTART_REQUIRED + + def __init__(self) -> None: + self.estop_calls = 0 + + def estop(self) -> None: + self.estop_calls += 1 + raise ROSEStopRequested("fake A1 sidecar stopped") + + def health(self) -> HALHealthReport: + return HALHealthReport( + message="A1 healthy", + fields={"motor_status_codes": "0,0,0,0,0,0,0"}, + ) + + +class _FakeLatchOnlyHAL: + description = SimpleNamespace(name="legacy_hal") + + def __init__(self) -> None: + self.estop_calls = 0 + + def estop(self) -> None: + self.estop_calls += 1 + raise ROSEStopRequested("must not be forwarded without explicit opt-in") + + +class _FakeResettableHAL(_FakeRealHAL): + estop_recovery = EStopRecovery.RESETTABLE + + def __init__(self) -> None: + super().__init__() + self.reset_calls = 0 + + def reset_estop(self) -> None: + self.reset_calls += 1 + + +class _FakeFailingEStopHAL(_FakeRealHAL): + def estop(self) -> None: + self.estop_calls += 1 + raise RuntimeError("fake downstream stop failed") + + +def test_estop_reaches_real_hal_and_reset_requires_restart() -> None: + rclpy.init() + try: + node = ManifestHALLifecycleNode("openral_hal_galaxea_a1") + hal = _FakeRealHAL() + node._hal = cast(HAL, hal) + try: + node._on_estop(object()) + assert node._estopped + assert hal.estop_calls == 1 + + node._on_estop_cleared(object()) + assert node._estopped + finally: + node.destroy_node() + finally: + rclpy.try_shutdown() + + +def test_estopped_node_does_not_poll_disconnected_real_hal() -> None: + rclpy.init() + try: + node = ManifestHALLifecycleNode("openral_hal_galaxea_a1") + node._hal = cast(HAL, _FakeRealHAL()) + node._publisher = object() + node._estopped = True + try: + node._publish_joint_state() + finally: + node.destroy_node() + finally: + rclpy.try_shutdown() + + +def test_estopped_snapshot_backed_node_keeps_publishing_joint_state() -> None: + """Snapshot-backed (sim-attached) HALs keep /joint_states alive through the latch. + + Only real HALs skip the publish while ``_estopped`` — their transport may + already be torn down. A proprio snapshot is plain data, so operators keep + joint visibility during an e-stop instead of the topic going dark. + """ + import time + + from openral_core.schemas import JointState + from openral_hal.galaxea_a1 import GALAXEA_A1_DESCRIPTION, GalaxeaA1HAL + from openral_hal.proprio_snapshot import ProprioFrame, ProprioSnapshot + from sensor_msgs.msg import JointState as RosJointState + + rclpy.init() + try: + node = ManifestHALLifecycleNode("openral_hal_galaxea_a1_snapshot_estop") + listener = rclpy.create_node("t_estop_joint_state_listener") + received: list[RosJointState] = [] + listener.create_subscription(RosJointState, "/t_estop_joint_states", received.append, 5) + try: + node._publisher = node.create_publisher(RosJointState, "/t_estop_joint_states", 5) + node._hal = cast(HAL, GalaxeaA1HAL()) + names = [joint.name for joint in GALAXEA_A1_DESCRIPTION.joints] + positions = [0.25] * len(names) + snapshot = ProprioSnapshot() + snapshot.set( + ProprioFrame( + state=JointState(name=names, position=positions, stamp_ns=1), + base_pose=(0.0, 0.0, 0.0), + base_pose_6dof=None, + base_twist=(0.0, 0.0, 0.0, 0.0, 0.0, 0.0), + ) + ) + node._proprio = snapshot + node._estopped = True + node._publish_joint_state() + deadline = time.monotonic() + 2.0 + while not received and time.monotonic() < deadline: + rclpy.spin_once(listener, timeout_sec=0.1) + assert received, "estopped snapshot-backed node stopped publishing joint state" + assert list(received[0].position) == positions + finally: + node.destroy_node() + listener.destroy_node() + finally: + rclpy.try_shutdown() + + +def test_downstream_estop_failure_stays_latched_without_crashing_callback() -> None: + rclpy.init() + try: + node = ManifestHALLifecycleNode("openral_hal_galaxea_a1") + hal = _FakeFailingEStopHAL() + node._hal = cast(HAL, hal) + try: + node._on_estop(object()) + assert node._estopped + assert hal.estop_calls == 1 + finally: + node.destroy_node() + finally: + rclpy.try_shutdown() + + +def test_diagnostics_include_cached_a1_health() -> None: + rclpy.init() + try: + node = ManifestHALLifecycleNode("openral_hal_galaxea_a1") + node._hal = cast(HAL, _FakeRealHAL()) + try: + level, message, fields = node._heartbeat_status("galaxea_a1") + assert level == Level.OK + assert message == "A1 healthy" + assert fields["motor_status_codes"] == "0,0,0,0,0,0,0" + + node._estopped = True + level, message, fields = node._heartbeat_status("galaxea_a1") + assert level == Level.ERROR + assert message == "estop latched" + assert fields["estopped"] == "true" + finally: + node.destroy_node() + finally: + rclpy.try_shutdown() + + +def test_hal_without_lifecycle_estop_extension_keeps_latch_only_behavior() -> None: + rclpy.init() + try: + node = ManifestHALLifecycleNode("openral_hal_legacy") + hal = _FakeLatchOnlyHAL() + node._hal = cast(HAL, hal) + try: + node._on_estop(object()) + assert node._estopped + assert hal.estop_calls == 0 + + node._on_estop_cleared(object()) + assert not node._estopped + finally: + node.destroy_node() + finally: + rclpy.try_shutdown() + + +def test_resettable_lifecycle_estop_clears_only_after_hal_reset() -> None: + rclpy.init() + try: + node = ManifestHALLifecycleNode("openral_hal_resettable") + hal = _FakeResettableHAL() + node._hal = cast(HAL, hal) + try: + node._on_estop(object()) + node._on_estop_cleared(object()) + + assert hal.estop_calls == 1 + assert hal.reset_calls == 1 + assert not node._estopped + finally: + node.destroy_node() + finally: + rclpy.try_shutdown() diff --git a/packages/openral_rskill_ros/launch/sim_e2e.launch.py b/packages/openral_rskill_ros/launch/sim_e2e.launch.py index e397a92..56aff82 100644 --- a/packages/openral_rskill_ros/launch/sim_e2e.launch.py +++ b/packages/openral_rskill_ros/launch/sim_e2e.launch.py @@ -608,6 +608,11 @@ def compose_runtime_graph(context: LaunchContext, *_args: object, **_kwargs: obj "true", "yes", ) + enable_reasoner = LaunchConfiguration("enable_reasoner").perform(context).lower() in ( + "1", + "true", + "yes", + ) # Read-only Foxglove live-scene bridge. Off by default; # ``openral deploy sim --foxglove`` opts in. enable_foxglove = LaunchConfiguration("enable_foxglove").perform(context).lower() in ( @@ -1055,24 +1060,25 @@ def compose_runtime_graph(context: LaunchContext, *_args: object, **_kwargs: obj # (VRAM eviction only evicts the detectors, not the reasoner), so a one-shot drive to active is # behaviour-preserving — exactly as for the HAL block below. _reasoner_autostart_path = str(_REPO_ROOT / "tools" / "lifecycle_autostart.py") - autostart.append( - ExecuteProcess( - cmd=[ - sys.executable, - _reasoner_autostart_path, - "--node", - "/openral_reasoner", - "--target", - "active", - "--service-timeout-s", - "60.0", - "--transition-timeout-s", - "300.0", - ], - output="log", + if enable_reasoner: + autostart.append( + ExecuteProcess( + cmd=[ + sys.executable, + _reasoner_autostart_path, + "--node", + "/openral_reasoner", + "--target", + "active", + "--service-timeout-s", + "60.0", + "--transition-timeout-s", + "300.0", + ], + output="log", + ) ) - ) - autostart += _autostart_lifecycle(prompt_router, "openral_prompt_router") + autostart += _autostart_lifecycle(prompt_router, "openral_prompt_router") # HAL autostart goes through ``tools/lifecycle_autostart.py`` rather # than ``_autostart_lifecycle`` because launch_ros's # ``lifecycle_event_manager`` race on Jazzy (same one documented for @@ -1695,11 +1701,11 @@ def compose_runtime_graph(context: LaunchContext, *_args: object, **_kwargs: obj nodes: list = [ safety_kernel, runtime, - reasoner, - prompt_router, hal, *extra_nodes, ] + if enable_reasoner: + nodes[2:2] = [reasoner, prompt_router] # Dashboard is opt-out (default on). ``openral deploy sim --no-dashboard`` # threads ``enable_dashboard:=false`` to skip the spawn entirely — # useful for headless CI and avoids the @@ -2175,6 +2181,15 @@ def generate_launch_description() -> LaunchDescription: "locator. Ignored unless enable_object_detector is true." ), ), + DeclareLaunchArgument( + "enable_reasoner", + default_value="true", + description=( + "Spawn the reasoner and prompt-router lifecycle nodes. Disable " + "for direct-rSkill deployments that submit an explicit " + "/openral/execute_rskill goal and do not require LLM planning." + ), + ), DeclareLaunchArgument( "enable_dashboard", default_value="true", diff --git a/packages/openral_rskill_ros/openral_rskill_ros/rskill_runner_node.py b/packages/openral_rskill_ros/openral_rskill_ros/rskill_runner_node.py index a2f08f2..314ad3f 100644 --- a/packages/openral_rskill_ros/openral_rskill_ros/rskill_runner_node.py +++ b/packages/openral_rskill_ros/openral_rskill_ros/rskill_runner_node.py @@ -1846,13 +1846,15 @@ def _build_runtime_skill_from_manifest( "are resolved via make_default_skill_resolver(), which branches " "on manifest.kind before reaching this helper." ) + policy_extra = dict(manifest.policy_extras) + policy_extra["latency_budget_ms"] = manifest.latency_budget.per_chunk_ms vla = VLASpec( id=manifest.model_family, # Pass the absolute local directory so the same resolver that # `openral sim run` uses can find the manifest on disk. weights_uri=str(yaml_path.parent), device="auto", - extra={}, + extra=policy_extra, ) # The policy factories only access `env_cfg.vla`; a SimpleNamespace # wrapper is enough to avoid building a full Pydantic SimEnvironment @@ -1870,7 +1872,11 @@ def _build_runtime_skill_from_manifest( # which `resolve_camera_keys` still honours first. The obs-image keys are # realigned to match in `_PolicyAdapterSkill._step_impl`. effective_scene_cameras = _required_vla_camera_slots(manifest, description) or scene_cameras - env_stub = _SimpleEnvCfg(vla=vla, cameras=effective_scene_cameras) + env_stub = _SimpleEnvCfg( + vla=vla, + cameras=effective_scene_cameras, + robot_description=description, + ) try: adapter = make_policy(env_stub) # type: ignore[arg-type] except ImportError as exc: @@ -1915,11 +1921,18 @@ def __init__(self, *, cameras: list[str] | tuple[str, ...]) -> None: class _SimpleEnvCfg: - """Minimal `env_cfg` carrier — `.vla` and `.scene.cameras` are read by policy factories.""" + """Minimal policy factory input with VLA, camera, and robot contracts.""" - def __init__(self, *, vla: object, cameras: list[str] | tuple[str, ...] = ()) -> None: + def __init__( + self, + *, + vla: object, + cameras: list[str] | tuple[str, ...] = (), + robot_description: RobotDescription | None = None, + ) -> None: self.vla = vla self.scene = _SimpleSceneCfg(cameras=cameras) + self.robot_description = robot_description def _effective_perm(robot_to_policy: list[int] | None, n: int) -> list[int]: diff --git a/packages/openral_rskill_ros/openral_rskill_ros/sensor_leg.py b/packages/openral_rskill_ros/openral_rskill_ros/sensor_leg.py index 40be584..c9b17a5 100644 --- a/packages/openral_rskill_ros/openral_rskill_ros/sensor_leg.py +++ b/packages/openral_rskill_ros/openral_rskill_ros/sensor_leg.py @@ -11,7 +11,7 @@ This module is that leg. :func:`open_deploy_sensor_readers` builds one reader per bound spec via the runner's ``SENSOR_BACKEND_REGISTRY`` and -guarantees every camera ends up on the WorldState subscription topic +prepares every camera for the WorldState subscription topic ``//image``: * ``gstreamer`` backend — the reader's built-in ROS tee publishes @@ -38,10 +38,11 @@ WorldState's ``direct_image_frame_sensors`` parameter stops ``_on_image`` from double-writing those sensors into the aggregator. -The caller owns teardown: :meth:`SensorLeg.close` stops publishers and -closes readers idempotently. ``runtime_node`` wires this in when its -``deploy_config`` parameter is set (real deploys only — sim keeps the -HAL bridge as its single camera source). +The caller starts the prepared pumps with :meth:`SensorLeg.start` only after +the composed ROS lifecycle nodes are configured, then owns teardown through +:meth:`SensorLeg.close`. ``runtime_node`` wires this in when its +``deploy_config`` parameter is set (real deploys only — sim keeps the HAL +bridge as its single camera source). """ from __future__ import annotations @@ -130,13 +131,13 @@ def _run(self) -> None: @dataclass class SensorLeg: - """Open readers + started publishers for one deploy session. + """Open readers + prepared publishers for one deploy session. Attributes: readers: Open :class:`SensorReader` instances, one per deploy-bound :class:`SensorSpec` (gstreamer readers publish via their internal ROS tee). - publishers: Started :class:`SensorRosPublisher` pumps for the + publishers: Prepared :class:`SensorRosPublisher` pumps for the readers without a native ROS tee. Parallel list, NOT index-aligned with ``readers``. """ @@ -148,6 +149,19 @@ class SensorLeg: #: so ``_on_image`` doesn't double-write them from the ROS tee. direct_sensors: list[str] = field(default_factory=list) + def start(self) -> None: + """Start every prepared publisher after ROS lifecycle configuration. + + Entity creation and lifecycle transitions remain single-threaded. + Only after those complete may camera pumps publish concurrently. + """ + try: + for publisher in self.publishers: + publisher.start() # type: ignore[attr-defined] # reason: duck-typed pump + except Exception: + self.close() + raise + def close(self) -> None: """Stop publishers first (they poll the readers), then close readers. @@ -248,8 +262,9 @@ def open_deploy_sensor_readers( *, topic_prefix: str = DEFAULT_TOPIC_PREFIX, aggregator: Any | None = None, # reason: WorldStateAggregator — deferred import + ros_node: Any | None = None, # reason: composed rclpy node — deferred import ) -> SensorLeg: - """Open every deploy-bound sensor in ``sensors`` and publish each onto ROS. + """Open deploy-bound sensors and prepare their ROS publishing resources. Args: sensors: Robot-manifest sensors plus :attr:`DeployScene.sensors` @@ -264,10 +279,13 @@ def open_deploy_sensor_readers( intact) straight into it, and the sensor is recorded in :attr:`SensorLeg.direct_sensors` — forward that list to WorldState's ``direct_image_frame_sensors`` parameter. + ros_node: Existing composed ROS node that owns image publishers. When + omitted, each fallback publisher owns its historical private node. Returns: - A :class:`SensorLeg` holding the open readers + started - publishers. Call :meth:`SensorLeg.close` on shutdown. + A :class:`SensorLeg` holding open readers and prepared publishers. + Call :meth:`SensorLeg.start` after the owning ROS lifecycle nodes are + configured, then :meth:`SensorLeg.close` on shutdown. Raises: ROSConfigError: A binding names an unknown backend, or a backend's @@ -278,6 +296,7 @@ def open_deploy_sensor_readers( >>> desc = RobotDescription.from_yaml("robots/so101_follower/robot.yaml") # doctest: +SKIP >>> scene = DeployScene.from_yaml("scenes/deploy/so101_bench.yaml") # doctest: +SKIP >>> leg = open_deploy_sensor_readers([*desc.sensors, *scene.sensors]) # doctest: +SKIP + >>> leg.start() # doctest: +SKIP >>> try: # doctest: +SKIP ... ... # spin the graph ... finally: @@ -286,27 +305,39 @@ def open_deploy_sensor_readers( # Deferred imports — openral_runner pulls torch-adjacent modules; keep # this module importable for AST/shape tests on minimal hosts. from openral_core import SensorReaderBackend, SensorReaderConfig - from openral_runner.factory import SENSOR_BACKEND_REGISTRY + from openral_runner.factory import make_sensor_readers leg = SensorLeg() - try: - for spec in sensors: - binding = spec.deploy_binding - if binding is None: - continue - topic = f"{topic_prefix}/{spec.name}/image" - if binding.backend == SensorReaderBackend.GSTREAMER: - # Native in-pipeline tee: force it on so the frames reach ROS. - cfg = SensorReaderConfig( + prepared: list[tuple[SensorSpec, str, SensorReaderConfig, bool]] = [] + for spec in sensors: + binding = spec.deploy_binding + if binding is None: + continue + topic = f"{topic_prefix}/{spec.name}/image" + native_tee = binding.backend == SensorReaderBackend.GSTREAMER + prepared.append( + ( + spec, + topic, + SensorReaderConfig( sensor_id=spec.name, backend=binding.backend, backend_params=binding.backend_params, max_age_ms=binding.max_age_ms, - publish_to_ros=True, - publish_topic=topic, - publish_rate_hz=_publish_rate_hz(spec), - ) - reader = SENSOR_BACKEND_REGISTRY[cfg.backend.value](cfg) + publish_to_ros=native_tee, + publish_topic=topic if native_tee else None, + publish_rate_hz=_publish_rate_hz(spec) if native_tee else None, + ), + native_tee, + ) + ) + readers = make_sensor_readers([cfg for _, _, cfg, _ in prepared]) + try: + for (spec, topic, _cfg, native_tee), reader in zip(prepared, readers, strict=True): + binding = spec.deploy_binding + assert binding is not None # reason: prepared filters unbound specs + if native_tee: + # Native in-pipeline tee: force it on so the frames reach ROS. reader.open() # Serialise cold-start: block until this camera streams a frame # before opening the next one (reopening on a transient bus @@ -321,13 +352,6 @@ def open_deploy_sensor_readers( # attach the polling ROS publisher pump. from openral_sensors.ros_publisher import SensorRosPublisher - cfg = SensorReaderConfig( - sensor_id=spec.name, - backend=binding.backend, - backend_params=binding.backend_params, - max_age_ms=binding.max_age_ms, - ) - reader = SENSOR_BACKEND_REGISTRY[cfg.backend.value](cfg) reader.open() leg.readers.append(reader) # Manifest-calibrated intrinsics → companion CameraInfo on the @@ -342,15 +366,14 @@ def open_deploy_sensor_readers( frame_id=spec.frame_id, camera_info=spec.intrinsics, info_topic=f"{topic_prefix}/{spec.name}/camera_info", + node=ros_node, ) - publisher.start() leg.publishers.append(publisher) if aggregator is not None: # Zero-copy vision path: in-process reader → aggregator, no ROS hop # for the policy leg (NVMM handles survive). The ROS tee above # keeps serving observability consumers. pump = _AggregatorPump(reader, spec.name, aggregator, _publish_rate_hz(spec)) - pump.start() leg.publishers.append(pump) leg.direct_sensors.append(spec.name) log.info( @@ -360,6 +383,14 @@ def open_deploy_sensor_readers( topic=topic, direct_to_aggregator=aggregator is not None, ) + # Prepare every ROS entity while execution is still single-threaded. + # SensorLeg.start() runs only after the composed lifecycle nodes have + # also finished creating their publishers, subscriptions, and action + # servers. + for publisher in leg.publishers: + prepare = getattr(publisher, "prepare", None) + if prepare is not None: + prepare() except Exception: # Half-open leg → close what we already opened before re-raising; # a failed camera must not leak a v4l2 handle past the error. diff --git a/packages/openral_rskill_ros/scripts/runtime_node b/packages/openral_rskill_ros/scripts/runtime_node index 4bcdf6a..1c22287 100755 --- a/packages/openral_rskill_ros/scripts/runtime_node +++ b/packages/openral_rskill_ros/scripts/runtime_node @@ -147,9 +147,7 @@ def _main() -> int: # noqa: PLR0911, PLR0912, PLR0915 # reason: linear bootstr dataset_license: str = ( bootstrap.get_parameter("dataset_license").get_parameter_value().string_value ) - deploy_config: str = ( - bootstrap.get_parameter("deploy_config").get_parameter_value().string_value - ) + deploy_config: str = bootstrap.get_parameter("deploy_config").get_parameter_value().string_value bootstrap.destroy_node() if not robot_yaml: @@ -238,6 +236,7 @@ def _main() -> int: # noqa: PLR0911, PLR0912, PLR0915 # reason: linear bootstr sensor_leg = open_deploy_sensor_readers( merge_deploy_sensors(runtime.description.sensors, scene.sensors), aggregator=runtime.aggregator, + ros_node=runtime.world_state_node, ) if sensor_leg.direct_sensors: from rclpy.parameter import Parameter @@ -297,6 +296,17 @@ def _main() -> int: # noqa: PLR0911, PLR0912, PLR0915 # reason: linear bootstr rclpy.try_shutdown() return 5 + # Start camera publication only after both composed lifecycle nodes have + # created all ROS entities. Concurrent publishing during runner configure + # can block rclpy entity creation before the ExecuteRskill server exists. + if sensor_leg is not None: + try: + sensor_leg.start() + except Exception as exc: # surface as exit code, not stack trace + print(f"runtime_node: sensor leg failed to start: {exc!s}", file=sys.stderr) + rclpy.try_shutdown() + return 6 + try: executor.spin() except (KeyboardInterrupt, ExternalShutdownException): diff --git a/packages/openral_rskill_ros/test/test_no_dashboard_otlp_env.py b/packages/openral_rskill_ros/test/test_no_dashboard_otlp_env.py index 35c38c7..d93331a 100644 --- a/packages/openral_rskill_ros/test/test_no_dashboard_otlp_env.py +++ b/packages/openral_rskill_ros/test/test_no_dashboard_otlp_env.py @@ -87,37 +87,26 @@ def _import_launch_module() -> Any: return module -def _make_launch_context(*, enable_dashboard: bool) -> Any: - """Return a :class:`launch.LaunchContext` pre-populated with the defaults. - - Mirrors the ``DeclareLaunchArgument(default_value=…)`` block at the - bottom of ``sim_e2e.launch.py`` and pins ``enable_dashboard`` to the - value the test cares about. Only ``robot_yaml`` / ``hal_*`` lack - defaults (CLI-required) so we set them by hand to a representative HAL. - """ +def _make_launch_context(*, enable_dashboard: bool, enable_reasoner: bool = True) -> Any: + """Return a launch context populated from the launch's own declarations.""" from launch import LaunchContext + from launch.actions import DeclareLaunchArgument + module = _import_launch_module() ctx = LaunchContext() cfg = ctx.launch_configurations + # Required CLI-provided arguments have no defaults. cfg["robot_yaml"] = str(_REPO_ROOT / "robots" / _REPRESENTATIVE_ROBOT / "robot.yaml") cfg["hal_package"] = "openral_hal_openarm" cfg["hal_executable"] = "lifecycle_node.py" cfg["hal_node_name"] = "openral_hal_test" cfg["hal_params_file"] = "/tmp/openral-test-hal-params.yaml" - cfg["reset_to_pose_service"] = "" - cfg["dashboard_port"] = "4318" - cfg["reasoner_provider"] = "" - cfg["reasoner_model"] = "gpt-5.5" - cfg["reasoner_endpoint"] = "" - cfg["spatial_memory_path"] = "" - cfg["spatial_memory_ingest"] = "false" - cfg["hal_mode"] = "sim" - cfg["enable_slam"] = "false" - cfg["enable_nav2"] = "false" - cfg["enable_octomap"] = "false" - cfg["octomap_cloud_topic"] = "/openral/cameras/front_depth/points" - cfg["enable_object_detector"] = "false" - cfg["object_detector_onnx"] = str(_REPO_ROOT / "rskills" / "rtdetr-coco-r18" / "model.onnx") + + for entity in module.generate_launch_description().entities: + if isinstance(entity, DeclareLaunchArgument): + entity.execute(ctx) + + cfg["enable_reasoner"] = "true" if enable_reasoner else "false" cfg["enable_dashboard"] = "true" if enable_dashboard else "false" return ctx @@ -221,6 +210,23 @@ def test_reasoner_uses_model_first_env() -> None: assert "OPENRAL_REASONER_LLM_PROVIDER" not in reasoner_env +def test_direct_rskill_mode_omits_reasoner_and_prompt_router() -> None: + from launch_ros.actions import LifecycleNode, Node + + module = _import_launch_module() + ctx = _make_launch_context(enable_dashboard=False, enable_reasoner=False) + entities = module.compose_runtime_graph(ctx) + packages = { + getattr(entity, "_Node__package", None) + for entity in entities + if isinstance(entity, Node | LifecycleNode) + } + assert "openral_reasoner_ros" not in packages + assert "openral_prompt_router" not in packages + assert "openral_safety_kernel" in packages + assert "openral_rskill_ros" in packages + + def test_dashboard_enabled_forwards_otlp_endpoint_to_every_node() -> None: """Default (``--dashboard``) → every node points OTLP at the dashboard port. diff --git a/pyproject.toml b/pyproject.toml index f272d64..78fc5b8 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -338,6 +338,7 @@ lingbot = [ # transformers==4.57.3 + custom Triton MoE kernels, incompatible with this # repo's torch>=2.9 / transformers>=5 (CLAUDE.md §3). Apache-2.0 code + weights. { include-group = "sidecar-wire" }, + "websockets>=15", ] locateanything = [ # Opt-in LocateAnything-3B open-vocab VLM detector. diff --git a/python/cli/src/openral_cli/deploy_sim.py b/python/cli/src/openral_cli/deploy_sim.py index 26cc2fe..e4442a0 100644 --- a/python/cli/src/openral_cli/deploy_sim.py +++ b/python/cli/src/openral_cli/deploy_sim.py @@ -297,6 +297,14 @@ class _HalSpec: default_params={}, manifest_driven=True, ), + "galaxea_a1": _HalSpec( + package="openral_hal_galaxea_a1", + executable="lifecycle_node.py", + node_name="openral_hal_galaxea_a1", + supported_robot_names=frozenset({"galaxea_a1"}), + default_params={}, + manifest_driven=True, + ), } # The lidar-less visual-SLAM twin reuses panda_mobile's HAL verbatim (same node, @@ -943,6 +951,7 @@ def resolve_launch_invocation( # noqa: PLR0912, PLR0915 # reason: a flat resol deploy_config: Path | None = None, hal_param_overrides: dict[str, object] | None = None, hal_mode: str = "sim", + enable_reasoner: bool | None = None, enable_slam: bool | None = None, enable_nav2: bool | None = None, enable_octomap: bool | None = None, @@ -1025,6 +1034,8 @@ def _scene_path(value: str | None) -> str | None: return str(cand) return value + if enable_reasoner is None: + enable_reasoner = rt.enable_reasoner enable_slam = enable_slam if enable_slam is not None else rt.enable_slam enable_nav2 = enable_nav2 if enable_nav2 is not None else rt.enable_nav2 enable_octomap = enable_octomap if enable_octomap is not None else rt.enable_octomap @@ -1057,12 +1068,20 @@ def _scene_path(value: str | None) -> str | None: slam_mono_camera = rt.slam_mono_camera slam_depth_sidecar_autostart = rt.slam_depth_sidecar_autostart # Built-in defaults for the tri-state flags nothing pinned. + if enable_reasoner is None: + enable_reasoner = True if enable_octomap_kernel_check is None: enable_octomap_kernel_check = True if enable_reward_monitor is None: enable_reward_monitor = False if enable_critic is None: enable_critic = False + if not enable_reasoner and _resolved_initial_prompt: + raise ROSConfigError( + "an initial task requires runtime.enable_reasoner=true; " + "submit a specific rSkill through /openral/execute_rskill when " + "running a direct-rSkill deployment" + ) # Established default: the Isaac ROS composable node. Do NOT auto-pick # pycuvslam just because its wheel is importable (§1.4 — explicit). if slam_visual_impl is None: @@ -1334,6 +1353,7 @@ def _scene_path(value: str | None) -> str | None: # → ``hal_mode="sim"`` (default; the scene's robosuite OSC controller # synthesises cartesian/OSC modes); ``deploy run`` → ``"real"``. f"hal_mode:={hal_mode}", + f"enable_reasoner:={'true' if enable_reasoner else 'false'}", f"enable_slam:={'true' if enable_slam else 'false'}", f"slam_backend:={slam_backend}", f"slam_visual_impl:={slam_visual_impl}", diff --git a/python/core/src/openral_core/schemas.py b/python/core/src/openral_core/schemas.py index b855948..95bc481 100644 --- a/python/core/src/openral_core/schemas.py +++ b/python/core/src/openral_core/schemas.py @@ -4309,6 +4309,7 @@ class EmbodimentExtra(BaseModel): "custom", "franka_panda", "g1", + "galaxea_a1", "google_robot", "gr1", "h1", @@ -4400,6 +4401,7 @@ class EmbodimentExtra(BaseModel): "openvla", "lingbot_vla2", "lingbot_vla", + "lingbot_va_a1", "internvla_n1", ] """VLA / policy family the skill belongs to. @@ -4428,6 +4430,13 @@ class EmbodimentExtra(BaseModel): ``lerobot==0.4.2`` venv, driven by ``tools/_lingbot_vla2_server.py --variant v1`` (adapter: ``openral_sim.policies.lingbot_vla2`` id ``lingbot_vla``). +``lingbot_va_a1`` is the Galaxea A1 EEF deployment adapter for the LingBot-VA +checkpoint. It connects to versioned local Camera Bridge and policy-gateway +services owned by the separately maintained A1 Runtime, without importing its +checkout. Runtime owns the exact model configuration, EEF transforms, cache +semantics, and IK; OpenRAL validates the gateway identity and returns its +joint/gripper proposals through the normal safety and HAL path. + ``openvla`` (OpenVLA / OpenVLA-OFT) is a transformers *custom-code* model loaded in-process (``trust_remote_code``, gated by ``OPENRAL_ALLOW_REMOTE_CODE=1``); the adapter de-normalizes the policy's @@ -5987,6 +5996,7 @@ def from_yaml(cls, path: str) -> RSkillManifest: "3d_diffuser_actor", # family diffuser_actor "lingbot_vla", # family lingbot_vla "lingbot_vla2", # family lingbot_vla2 + "lingbot_va_a1", # family lingbot_va_a1 "internvla_n1", # family internvla_n1 (InternVLA-N1 / DualVLN) # Non-VLA tool-model tokens (detector / vlm / reward). "omdet_turbo", @@ -6020,6 +6030,7 @@ def from_yaml(cls, path: str) -> RSkillManifest: "openvla": "openvla_oft", "lingbot_vla": "lingbot_vla", "lingbot_vla2": "lingbot_vla2", + "lingbot_va_a1": "lingbot_va_a1", "internvla_n1": "internvla_n1", } """VLA :data:`ModelFamily` → its canonical ```` *suggestion* token @@ -6039,6 +6050,7 @@ def from_yaml(cls, path: str) -> RSkillManifest: "openvla": frozenset({"openvla", "openvla_oft"}), "lingbot_vla": frozenset({"lingbot_vla"}), "lingbot_vla2": frozenset({"lingbot_vla2"}), + "lingbot_va_a1": frozenset({"lingbot_va_a1"}), "internvla_n1": frozenset({"internvla_n1"}), } """The documented **family → allowed ```` tokens** map: when a manifest @@ -6979,6 +6991,7 @@ class DeployRuntime(BaseModel): model_config = ConfigDict(extra="forbid") + enable_reasoner: bool | None = None enable_slam: bool | None = None enable_nav2: bool | None = None enable_octomap: bool | None = None @@ -7319,9 +7332,12 @@ def model_post_init(self, _context: object) -> None: class SensorReaderBackend(str, Enum): """Which :class:`SensorReader` implementation to instantiate. - Four backends are reserved. ``opencv_thread`` - is the default and mirrors lerobot's per-camera background-thread - pattern. ``ros2_image`` subscribes to a ROS 2 image topic published by + ``opencv_thread`` is the default and mirrors lerobot's per-camera + background-thread pattern. ``galaxea_a1_camera_bridge`` consumes the + versioned paired raw-frame service owned by an external A1 Runtime process + without importing Runtime or opening either RealSense device. Three + additional backends are reserved. + ``ros2_image`` subscribes to a ROS 2 image topic published by a vendor driver. ``gstreamer`` runs a GStreamer pipeline whose appsink delivers frames (NVMM / DMA-BUF on Jetson; CPU bytes on x86). @@ -7337,6 +7353,7 @@ class SensorReaderBackend(str, Enum): ROS2_IMAGE = "ros2_image" GSTREAMER = "gstreamer" HOLOSCAN = "holoscan" + GALAXEA_A1_CAMERA_BRIDGE = "galaxea_a1_camera_bridge" class DeadlineOverrunPolicy(str, Enum): diff --git a/python/hal/README.md b/python/hal/README.md index 671dace..456ff32 100644 --- a/python/hal/README.md +++ b/python/hal/README.md @@ -36,6 +36,7 @@ satisfies it. There is no inheritance requirement. | `ros_control.py` | `RosControlHAL` — adapter on top of `ros2_control` (no real ROS 2 import; works against `SimTransport` for unit tests and the live transport at runtime). | | `sim_transport.py` | `SimTransport` — typed in-memory `ros2_control` transport for unit tests. | | `so100_follower.py` | `SO100FollowerHAL` + `SO100_DESCRIPTION` + `so100_with_sensors` — real SO-100 follower arm via the `lerobot` SDK. | +| `galaxea_a1.py` | `GalaxeaA1HAL` + `GALAXEA_A1_DESCRIPTION` — real-only six-axis A1 and normalized gripper over a literal IPv4-loopback JSON-lines transport. The operator-provided ROS 1 SDK stays in an isolated sidecar; OpenRAL bundles no vendor code. | | `so100_sim.py` | `SO100DigitalTwin` + `SO100DigitalTwinConfig` — pure-Python in-process simulator (used by smoketests, sim tests, and as a stand-in when no hardware is connected). | | `so100_mujoco.py` | `SO100MujocoHAL` — MuJoCo digital twin for the SO-100 follower, driving the `mujoco_menagerie` MJCF with the same 6-DoF action layout as `SO100FollowerHAL`. | | `_mujoco_arm.py` | `MujocoArmHAL` + `MujocoArmHAL.from_description` (MJCF resolved via `openral_core.assets.resolve_asset`) — shared MuJoCo backend for the arms below. It self-constructs from `RobotDescription.sim` (`SimDescription`), so per-robot subclasses are 5-line wrappers and **no Python file is required to add a new MuJoCo HAL** — declare a `sim:` block in `robots//robot.yaml` and call `MujocoArmHAL.from_description(desc)`. | @@ -59,6 +60,7 @@ satisfies it. There is no inheritance requirement. | Robot | Adapter | Status | Notes | | --- | --- | --- | --- | | LeRobot SO-100 follower arm | `SO100FollowerHAL` | ✓ unit + sim | Embodiment tag: `so100_follower`. | +| Galaxea A1 (real-HW) | `GalaxeaA1HAL` | ✓ unit + offline socket · HIL fixture | Joint-position + normalized-gripper only; official ROS 1 driver and joint tracker run in a separately owned sidecar. Lab-gated HIL via `tests/hil/test_galaxea_a1.py`. | | SO-100 (pure-Python sim) | `SO100DigitalTwin` | ✓ sim | In-process; no MuJoCo, no GPU. Used by the smoketest and CI. | | SO-100 (MuJoCo digital twin) | `SO100MujocoHAL` | ✓ sim (`tests/sim/test_so100_follower_hal_mujoco.py`) | Real MuJoCo physics on the `mujoco_menagerie` MJCF; same 6-DoF action layout as `SO100FollowerHAL`. | | Franka Panda (sim) | `FrankaPandaHAL` | ✓ sim | MuJoCo-backed. | @@ -152,6 +154,7 @@ Each HAL adapter has (or will have) a thin ROS 2 lifecycle node under | ROS package | Wraps | Status | | --- | --- | --- | | `openral_hal_so100` | `SO100FollowerHAL` | ✓ working (unit + sim coverage) | +| `openral_hal_galaxea_a1` | `GalaxeaA1HAL` | ✓ real observation/hold/joint/gripper + full C++ kernel graph HIL | | `openral_hal_franka` | `FrankaPandaHAL` (planned) | skeleton | | `openral_hal_ur5e` | `UR5eHAL` (sim) / `UR5eRealHAL` (real HW) | skeleton | | `openral_hal_ur10e` | `UR10eHAL` (sim) / `UR10eRealHAL` (real HW) | skeleton | @@ -192,7 +195,8 @@ and topic names. plugin workaround baked in. - HIL: `tests/hil/test_franka_panda.py`, `tests/hil/test_sawyer.py`, `tests/hil/test_aloha.py`, - `tests/hil/test_ur5e.py`, `tests/hil/test_ur10e.py` — lab runners only + `tests/hil/test_ur5e.py`, `tests/hil/test_ur10e.py`, + `tests/hil/test_galaxea_a1.py` — lab runners only (`UR5E_HOST` / `UR10E_HOST` env vars + `[self-hosted, lab-ur*]` runner labels for the UR pair). - HIL transport bridges (lab-only): `tests/hil/_ros_control_transport.py` exposes `RosControlHILTransport` + `make_hil_transport` for the diff --git a/python/hal/src/openral_hal/__init__.py b/python/hal/src/openral_hal/__init__.py index b8e7bc7..aa5d76d 100644 --- a/python/hal/src/openral_hal/__init__.py +++ b/python/hal/src/openral_hal/__init__.py @@ -2,6 +2,10 @@ Public surface: - ``HAL``: structural Protocol every adapter must satisfy (RFC §8.2). +- ``LifecycleEStopHAL`` / ``ResettableLifecycleEStopHAL``: optional typed + lifecycle e-stop propagation and recovery contracts. +- ``HALHealthProvider`` / ``HALHealthReport``: optional cached diagnostics + contract for the generic lifecycle heartbeat. - ``RosControlHAL``: ros2_control-backed adapter. - ``SO100FollowerHAL``: lerobot SO-100 follower arm adapter. - ``SO100_DESCRIPTION`` / ``so100_with_sensors``: canonical SO-100 description @@ -57,11 +61,13 @@ fetched at a pinned SHA from ``bensonlee5/anvil-openarm-mujoco`` by ``openral_hal._anvil_openarm_v2_assets`` (``openarm:anvil_v2_bimanual``). - ``SimTransport``: typed in-memory ros2_control transport for unit tests. +- ``GalaxeaA1HAL`` / ``GALAXEA_A1_DESCRIPTION``: real Galaxea A1 through an + isolated ROS 1 Noetic sidecar (the vendor SDK remains operator-provided). -All ``*_REAL_DESCRIPTION`` constants are derived from their sim siblings via -``openral_hal._real_description.make_real_description``; they share the same -``hal`` entrypoints (``hal.sim`` / ``hal.real``) and differ only in -``sdk_kind``. +Where a sim sibling exists, ``*_REAL_DESCRIPTION`` constants are derived via +``openral_hal._real_description.make_real_description`` and share its HAL +entrypoints. Real-only platforms such as the Galaxea A1 publish one description +directly until a licensed digital-twin asset is available. """ from openral_hal.aloha import ( @@ -82,6 +88,7 @@ FrankaPandaRealHAL, ) from openral_hal.g1 import G1_DESCRIPTION, G1MujocoHAL +from openral_hal.galaxea_a1 import GALAXEA_A1_DESCRIPTION, GalaxeaA1HAL from openral_hal.h1 import H1_DESCRIPTION, H1MujocoHAL from openral_hal.openarm import OPENARM_DESCRIPTION, OpenArmMujocoHAL from openral_hal.panda_mobile import ( @@ -89,7 +96,14 @@ PANDA_MOBILE_JOINT_NAMES, PandaMobileHAL, ) -from openral_hal.protocol import HAL +from openral_hal.protocol import ( + HAL, + EStopRecovery, + HALHealthProvider, + HALHealthReport, + LifecycleEStopHAL, + ResettableLifecycleEStopHAL, +) from openral_hal.resolver import build_hal from openral_hal.ros_control import RosControlHAL from openral_hal.sawyer_real import ( @@ -127,6 +141,7 @@ "FRANKA_PANDA_DESCRIPTION", "FRANKA_PANDA_REAL_DESCRIPTION", "G1_DESCRIPTION", + "GALAXEA_A1_DESCRIPTION", "H1_DESCRIPTION", "HAL", "OPENARM_DESCRIPTION", @@ -139,12 +154,18 @@ "AlohaHAL", "AlohaMujocoHAL", "AnvilOpenArmV2MujocoHAL", + "EStopRecovery", "FrankaPandaHAL", "FrankaPandaRealHAL", "G1MujocoHAL", + "GalaxeaA1HAL", "H1MujocoHAL", + "HALHealthProvider", + "HALHealthReport", + "LifecycleEStopHAL", "OpenArmMujocoHAL", "PandaMobileHAL", + "ResettableLifecycleEStopHAL", "Rizon4MujocoHAL", "RosControlHAL", "SO100DigitalTwin", diff --git a/python/hal/src/openral_hal/galaxea_a1.py b/python/hal/src/openral_hal/galaxea_a1.py new file mode 100644 index 0000000..7bfe850 --- /dev/null +++ b/python/hal/src/openral_hal/galaxea_a1.py @@ -0,0 +1,832 @@ +"""Real-hardware HAL for the Galaxea A1 arm via an isolated ROS 1 sidecar. + +The vendor stack is ROS 1 Noetic while OpenRAL is ROS 2 / Python 3.12. This +adapter therefore owns no vendor imports: an operator-provisioned sidecar runs +the official SDK and streams typed JSON records over loopback TCP. Network I/O +lives on a worker thread so ``read_state`` and ``send_action`` never block. + +Example: + >>> from openral_hal.galaxea_a1 import GALAXEA_A1_DESCRIPTION, GalaxeaA1HAL + >>> hal = GalaxeaA1HAL() # doctest: +SKIP + >>> hal.connect() # requires the running ROS 1 sidecar # doctest: +SKIP + >>> state = hal.read_state() # doctest: +SKIP + >>> len(state.position) == len(GALAXEA_A1_DESCRIPTION.joints) == 7 # doctest: +SKIP + True + >>> hal.disconnect() # doctest: +SKIP +""" + +from __future__ import annotations + +import contextlib +import ipaddress +import json +import math +import select +import socket +import threading +import time +from dataclasses import dataclass +from typing import Protocol + +import structlog +from openral_core.exceptions import ( + ROSConfigError, + ROSEStopRequested, + ROSPerceptionStale, + ROSRuntimeError, +) +from openral_core.schemas import ( + Action, + ControlMode, + EmbodimentKind, + EndEffectorSpec, + HalEntrypoints, + IntrinsicsPinhole, + JointSpec, + JointState, + JointType, + RobotCapabilities, + RobotDescription, + SafetyEnvelope, + SensorModality, + SensorSpec, +) + +from openral_hal._base import HALBase +from openral_hal.protocol import EStopRecovery, HALHealthReport + +__all__ = ["GALAXEA_A1_DESCRIPTION", "GalaxeaA1HAL"] + +log = structlog.get_logger(__name__) + +_JOINT_NAMES = tuple(f"arm_joint{i}" for i in range(1, 7)) +_JOINT_LIMITS = ( + (-2.8798, 2.8798), + (0.0, 3.1415), + (-3.3161, 0.0), + (-2.8798, 2.8798), + (-1.6581, 1.6581), + (-2.8798, 2.8798), +) +_JOINT_ORIGINS_XYZ = ( + (-0.0011147, 0.0, 0.0892), + (0.0, -0.00004, 0.0615), + (0.34928, 0.02, 0.0), + (0.07, -0.00395, -0.00004), + (0.0, 0.0, 0.2776), + (0.0, -0.1575, -0.00023266), +) +_JOINT_ORIGINS_RPY = ( + (0.0, 0.0, 3.1416), + (1.5708, 0.0, 0.0), + (0.0, 0.0, 1.5708), + (-1.5708, 1.5708, 0.0), + (-1.5708, 0.0, 3.1416), + (1.5708, 0.0, 0.0), +) +_PROTOCOL_VERSION = 1 +_GRIPPER_STATUS_INDEX = len(_JOINT_NAMES) +_MAX_TCP_PORT = 65536 +_MAX_STATUS_MASK = 0xFFFFFFFF + + +GALAXEA_A1_DESCRIPTION = RobotDescription( + name="galaxea_a1", + embodiment_kind=EmbodimentKind.MANIPULATOR, + base_frame="base_link", + joints=[ + JointSpec( + name=name, + joint_type=JointType.REVOLUTE, + parent_link="base_link" if index == 1 else f"arm_seg{index - 1}", + child_link=f"arm_seg{index}", + axis_xyz=(0.0, 0.0, 1.0), + origin_xyz=_JOINT_ORIGINS_XYZ[index - 1], + origin_rpy=_JOINT_ORIGINS_RPY[index - 1], + position_limits=limits, + velocity_limit=velocity, + effort_limit=effort, + actuator_kind="bldc", + ) + for index, (name, limits, velocity, effort) in enumerate( + zip( + _JOINT_NAMES, + _JOINT_LIMITS, + (20.944, 20.944, 7.5398, 25.133, 25.133, 25.133), + (40.0, 40.0, 27.0, 7.0, 7.0, 7.0), + strict=True, + ), + start=1, + ) + ], + end_effectors=[ + EndEffectorSpec( + name="gripper", + kind="parallel_gripper", + n_dof=1, + ) + ], + sensors=[ + SensorSpec( + name="front", + modality=SensorModality.RGB, + frame_id="a1_front_camera", + rate_hz=30.0, + encoding="rgb8", + intrinsics=IntrinsicsPinhole( + width=480, + height=480, + fx=387.67059326171875, + fy=386.86669921875, + cx=220.1776123046875, + cy=250.0211944580078, + distortion_model="plumb_bob", + distortion_coeffs=[ + -0.05492031201720238, + 0.06242402642965317, + -0.0008095245575532317, + 0.00015016942052170634, + -0.020708303898572922, + ], + ), + vla_feature_key="observation.images.front", + vendor="Intel", + model="RealSense D455", + metadata={ + "output_width": 480, + "output_height": 480, + "crop": "x=103,y=0,width=480,height=480", + }, + ), + SensorSpec( + name="wrist", + modality=SensorModality.RGB, + frame_id="a1_wrist_camera", + parent_frame="arm_seg6", + rate_hz=30.0, + encoding="rgb8", + intrinsics=IntrinsicsPinhole( + width=640, + height=480, + fx=392.2943115234375, + fy=391.7977294921875, + cx=320.1477355957031, + cy=241.63327026367188, + distortion_model="plumb_bob", + distortion_coeffs=[ + -0.05426114425063133, + 0.05990355461835861, + 0.0004118939395993948, + 0.000653077382594347, + -0.019962556660175323, + ], + ), + vla_feature_key="observation.images.wrist", + vendor="Intel", + model="RealSense D405", + metadata={"output_width": 640, "output_height": 480}, + ), + ], + capabilities=RobotCapabilities( + can_lift_kg=5.0, + has_vision=True, + supported_control_modes=[ControlMode.JOINT_POSITION, ControlMode.GRIPPER_POSITION], + embodiment_tags=["galaxea_a1"], + ), + safety=SafetyEnvelope( + max_ee_speed_m_s=0.25, + max_joint_speed_factor=0.05, + max_force_n=40.0, + max_torque_nm=40.0, + deadman_required=True, + ), + sdk_kind="closed_with_api", + hal=HalEntrypoints( + real="openral_hal.galaxea_a1:GalaxeaA1HAL", + parameters={ + "defaults": { + "host": "127.0.0.1", + "port": 46011, + "state_timeout_s": 0.5, + "status_timeout_s": 1.0, + "feedback_limit_tolerance_rad": 0.01, + "initial_alignment_tolerance_rad": 0.05, + "tracker_alignment_timeout_s": 5.0, + "max_target_step_rad": 0.08, + "command_lease_s": 0.5, + "idle_timeout_error_mask": 64, + "gripper_ignored_error_mask": 8, + "gripper_stroke_min_mm": 0.0, + "gripper_stroke_max_mm": 104.0, + } + }, + ), +) + + +@dataclass(frozen=True) +class _Snapshot: + position: tuple[float, ...] + velocity: tuple[float, ...] + effort: tuple[float, ...] + stamp_ns: int + received_monotonic: float + status_codes: tuple[int, ...] + status_received_monotonic: float + gripper_norm: float + relay_state: str = "LOCKED" + staged_position: tuple[float, ...] = () + forwarded_position: tuple[float, ...] = () + sidecar_action_count: int = 0 + + +class _A1Transport(Protocol): + def connect(self, hello: dict[str, object], timeout_s: float) -> None: ... + + def close(self) -> None: ... + + def snapshot(self) -> _Snapshot: ... + + def send(self, message: dict[str, object]) -> None: ... + + def estop(self, timeout_s: float) -> None: ... + + +class _SocketTransport: + """Loopback JSON-lines client with one reader and one priority writer.""" + + def __init__(self, host: str, port: int) -> None: + self._host = host + self._port = port + self._sock: socket.socket | None = None + self._stop = threading.Event() + self._ready = threading.Event() + self._estop_ack = threading.Event() + self._outbound_ready = threading.Event() + self._estop_confirmed = False + self._reader_thread: threading.Thread | None = None + self._writer_thread: threading.Thread | None = None + self._lock = threading.Lock() + self._snapshot: _Snapshot | None = None + self._pending_joint: dict[str, object] | None = None + self._pending_gripper: dict[str, object] | None = None + self._estop_pending = False + self._identity_verified = False + self._error: str | None = None + + def connect(self, hello: dict[str, object], timeout_s: float) -> None: + self._stop.clear() + self._ready.clear() + self._estop_ack.clear() + self._outbound_ready.clear() + with self._lock: + self._snapshot = None + self._pending_joint = None + self._pending_gripper = None + self._estop_pending = False + self._identity_verified = False + self._estop_confirmed = False + self._error = None + try: + sock = socket.create_connection((self._host, self._port), timeout=timeout_s) + except OSError as exc: + raise ROSRuntimeError( + f"Galaxea A1 sidecar is unavailable at {self._host}:{self._port}: {exc}" + ) from exc + try: + sock.setsockopt(socket.IPPROTO_TCP, socket.TCP_NODELAY, 1) + sock.setblocking(False) + self._sock = sock + self._write(hello) + self._reader_thread = threading.Thread( + target=self._read_loop, name="openral_a1_sidecar_reader", daemon=True + ) + self._writer_thread = threading.Thread( + target=self._write_loop, name="openral_a1_sidecar_writer", daemon=True + ) + self._reader_thread.start() + self._writer_thread.start() + except Exception: + self.close() + raise + if not self._ready.wait(timeout_s): + self.close() + raise ROSRuntimeError( + f"Galaxea A1 sidecar returned no valid state within {timeout_s:.1f} s." + ) + self._raise_if_failed() + + def close(self) -> None: + self._stop.set() + self._outbound_ready.set() + sock, self._sock = self._sock, None + if sock is not None: + with contextlib.suppress(OSError): + sock.shutdown(socket.SHUT_RDWR) + sock.close() + current = threading.current_thread() + for thread in (self._reader_thread, self._writer_thread): + if thread is not None and thread is not current: + thread.join(timeout=1.0) + self._reader_thread = None + self._writer_thread = None + + def snapshot(self) -> _Snapshot: + self._raise_if_failed() + with self._lock: + snapshot = self._snapshot + if snapshot is None: + raise ROSRuntimeError("Galaxea A1 sidecar has not produced a state snapshot.") + return snapshot + + def send(self, message: dict[str, object]) -> None: + self._raise_if_failed() + with self._lock: + if "joint_targets" in message: + self._pending_joint = message + if "gripper" in message: + self._pending_gripper = message + self._outbound_ready.set() + + def estop(self, timeout_s: float) -> None: + with self._lock: + self._estop_pending = True + self._pending_joint = None + self._pending_gripper = None + self._outbound_ready.set() + self._estop_ack.wait(timeout_s) + with self._lock: + confirmed = self._estop_confirmed + self.close() + if not confirmed: + raise ROSRuntimeError( + "Galaxea A1 sidecar did not confirm that its ROS 1 stack stopped." + ) + + def _read_loop(self) -> None: + buffer = b"" + try: + while not self._stop.is_set(): + sock = self._sock + if sock is None: + return + readable, _, _ = select.select([sock], [], [], 0.02) + if not readable: + continue + chunk = sock.recv(65536) + if not chunk: + raise ConnectionError("sidecar closed the connection") + buffer += chunk + while b"\n" in buffer: + line, buffer = buffer.split(b"\n", 1) + if line: + self._handle(json.loads(line)) + except Exception as exc: + if not self._stop.is_set(): + self._fail(f"Galaxea A1 sidecar transport failed: {exc}") + + def _write_loop(self) -> None: + """Drain the latest command independently of inbound state traffic.""" + try: + while not self._stop.is_set(): + self._outbound_ready.wait() + self._outbound_ready.clear() + while not self._stop.is_set(): + message = self._take_pending_message() + if message is None: + break + self._write(message) + except Exception as exc: + if not self._stop.is_set(): + self._fail(f"Galaxea A1 sidecar transport failed: {exc}") + + def _take_pending_message(self) -> dict[str, object] | None: + """Atomically coalesce the latest arm and gripper targets. + + OpenRAL publishes per-mode action chunks back-to-back. Keeping one + mailbox per mode prevents a gripper chunk from overwriting the arm + chunk before the socket worker runs, while still bounding backlog. + """ + with self._lock: + if self._estop_pending: + message: dict[str, object] | None = {"op": "estop"} + self._estop_pending = False + else: + joint = self._pending_joint + gripper = self._pending_gripper + self._pending_joint = None + self._pending_gripper = None + if joint is None and gripper is None: + return None + stamps: list[int] = [] + for item in (joint, gripper): + stamp = None if item is None else item.get("stamp_ns") + if isinstance(stamp, int) and not isinstance(stamp, bool): + stamps.append(stamp) + message = {"op": "action", "stamp_ns": max(stamps, default=time.time_ns())} + if joint is not None: + message["joint_targets"] = joint["joint_targets"] + if gripper is not None: + message["gripper"] = gripper["gripper"] + return message + + def _write(self, message: dict[str, object]) -> None: + sock = self._sock + if sock is None: + raise ConnectionError("sidecar socket is closed") + payload = json.dumps(message, separators=(",", ":"), allow_nan=False).encode() + b"\n" + view = memoryview(payload) + while view: + _, writable, _ = select.select([], [sock], [], 0.2) + if not writable: + raise TimeoutError("sidecar send timed out") + view = view[sock.send(view) :] + + def _handle(self, raw: object) -> None: + if not isinstance(raw, dict): + raise ValueError("sidecar record must be a JSON object") + op = raw.get("op") + if op == "hello": + if raw.get("protocol") != _PROTOCOL_VERSION or raw.get("robot") != "galaxea_a1": + raise ValueError("sidecar identity does not match Galaxea A1 protocol v1") + with self._lock: + self._identity_verified = True + elif op == "state": + with self._lock: + identity_verified = self._identity_verified + if not identity_verified: + raise ValueError("sidecar sent state before its identity handshake") + snapshot = _decode_snapshot(raw) + with self._lock: + self._snapshot = snapshot + self._ready.set() + elif op == "fault": + self._fail(str(raw.get("reason", "unknown sidecar fault"))) + elif op == "estopped": + with self._lock: + self._estop_confirmed = True + self._estop_ack.set() + else: + raise ValueError(f"unsupported sidecar operation {op!r}") + + def _fail(self, reason: str) -> None: + with self._lock: + self._error = reason + self._ready.set() + self._estop_ack.set() + + def _raise_if_failed(self) -> None: + with self._lock: + error = self._error + if error is not None: + raise ROSRuntimeError(error) + + +def _decode_snapshot(raw: dict[str, object]) -> _Snapshot: + def _floats(key: str, *, required: bool = True) -> tuple[float, ...]: + value = raw.get(key) + if not isinstance(value, list): + if not required: + return () + raise ValueError(f"state.{key} must be a list") + result = tuple(float(v) for v in value) + if not all(math.isfinite(v) for v in result): + raise ValueError(f"state.{key} contains a non-finite value") + return result + + names = raw.get("name") + if names != list(_JOINT_NAMES): + raise ValueError(f"state.name must be {list(_JOINT_NAMES)!r}, got {names!r}") + position = _floats("position") + if len(position) != len(_JOINT_NAMES): + raise ValueError(f"state.position must have {len(_JOINT_NAMES)} values") + velocity = _floats("velocity", required=False) + effort = _floats("effort", required=False) + for label, values in (("velocity", velocity), ("effort", effort)): + if values and len(values) != len(_JOINT_NAMES): + raise ValueError(f"state.{label} must be empty or have {len(_JOINT_NAMES)} values") + status = raw.get("status_codes") + if not isinstance(status, list) or len(status) < len(_JOINT_NAMES) + 1: + raise ValueError("state.status_codes must include six arm motors and the gripper") + if any( + isinstance(value, bool) or not isinstance(value, int) or not 0 <= value <= _MAX_STATUS_MASK + for value in status + ): + raise ValueError("state.status_codes must contain uint32 integers") + gripper_raw = raw.get("gripper_norm") + if isinstance(gripper_raw, bool) or not isinstance(gripper_raw, (int, float)): + raise ValueError("state.gripper_norm must be numeric") + gripper = float(gripper_raw) + if not math.isfinite(gripper) or not 0.0 <= gripper <= 1.0: + raise ValueError("state.gripper_norm must be finite and within [0, 1]") + relay_state = raw.get("relay_state") + if relay_state not in {"LOCKED", "ARMING", "ACTIVE", "FAULT"}: + raise ValueError(f"state.relay_state is invalid: {relay_state!r}") + staged_position = _floats("staged_position", required=False) + forwarded_position = _floats("forwarded_position", required=False) + for label, values in ( + ("staged_position", staged_position), + ("forwarded_position", forwarded_position), + ): + if values and len(values) != len(_JOINT_NAMES): + raise ValueError(f"state.{label} must be empty or have {len(_JOINT_NAMES)} values") + now = time.monotonic() + stamp_raw = raw.get("stamp_ns") + if isinstance(stamp_raw, bool) or not isinstance(stamp_raw, int) or stamp_raw < 0: + raise ValueError("state.stamp_ns must be a non-negative integer") + sidecar_action_count = _decode_nonnegative_int(raw, "accepted_action_count", default=0) + return _Snapshot( + position=position, + velocity=velocity, + effort=effort, + stamp_ns=stamp_raw, + received_monotonic=now, + status_codes=tuple(status), + status_received_monotonic=now, + gripper_norm=gripper, + relay_state=str(relay_state), + staged_position=staged_position, + forwarded_position=forwarded_position, + sidecar_action_count=sidecar_action_count, + ) + + +def _decode_nonnegative_int(raw: dict[str, object], key: str, *, default: int | None = None) -> int: + value = raw.get(key, default) + if isinstance(value, bool) or not isinstance(value, int) or value < 0: + raise ValueError(f"state.{key} must be a non-negative integer") + return value + + +class GalaxeaA1HAL(HALBase): + """OpenRAL real HAL for the six-axis Galaxea A1 plus parallel gripper. + + Example: + >>> from openral_hal.galaxea_a1 import GalaxeaA1HAL + >>> from openral_core import Action, ControlMode + >>> hal = GalaxeaA1HAL(port=46011) # doctest: +SKIP + >>> hal.connect() # doctest: +SKIP + >>> hal.send_action( # doctest: +SKIP + ... Action(control_mode=ControlMode.GRIPPER_POSITION, gripper=[0.5], stamp_ns=0) + ... ) + >>> hal.disconnect() # doctest: +SKIP + """ + + description = GALAXEA_A1_DESCRIPTION + estop_recovery = EStopRecovery.RESTART_REQUIRED + + def __init__( + self, + *, + host: str = "127.0.0.1", + port: int = 46011, + state_timeout_s: float = 0.5, + status_timeout_s: float = 1.0, + feedback_limit_tolerance_rad: float = 0.01, + initial_alignment_tolerance_rad: float = 0.05, + tracker_alignment_timeout_s: float = 5.0, + max_target_step_rad: float = 0.08, + command_lease_s: float = 0.5, + idle_timeout_error_mask: int = 64, + gripper_ignored_error_mask: int = 8, + gripper_stroke_min_mm: float = 0.0, + gripper_stroke_max_mm: float = 104.0, + connect_timeout_s: float = 10.0, + transport: _A1Transport | None = None, + ) -> None: + """Configure the sidecar endpoint and explicit safety deadlines.""" + if not isinstance(host, str): + raise ROSConfigError("GalaxeaA1HAL requires a literal IPv4 loopback address.") + try: + sidecar_address = ipaddress.ip_address(host) + except ValueError as exc: + raise ROSConfigError("GalaxeaA1HAL requires a literal IPv4 loopback address.") from exc + if ( + not isinstance(sidecar_address, ipaddress.IPv4Address) + or not sidecar_address.is_loopback + ): + raise ROSConfigError("GalaxeaA1HAL requires a literal IPv4 loopback address.") + if isinstance(port, bool) or not isinstance(port, int) or not 0 < port < _MAX_TCP_PORT: + raise ROSConfigError("GalaxeaA1HAL requires a TCP port in [1, 65535].") + positive = { + "state_timeout_s": state_timeout_s, + "status_timeout_s": status_timeout_s, + "initial_alignment_tolerance_rad": initial_alignment_tolerance_rad, + "tracker_alignment_timeout_s": tracker_alignment_timeout_s, + "max_target_step_rad": max_target_step_rad, + "command_lease_s": command_lease_s, + "connect_timeout_s": connect_timeout_s, + } + if any(not math.isfinite(v) or v <= 0.0 for v in positive.values()): + raise ROSConfigError( + f"GalaxeaA1HAL timing and motion limits must be positive: {positive}" + ) + if not math.isfinite(feedback_limit_tolerance_rad) or feedback_limit_tolerance_rad < 0.0: + raise ROSConfigError("feedback_limit_tolerance_rad must be finite and non-negative.") + if ( + not math.isfinite(gripper_stroke_min_mm) + or not math.isfinite(gripper_stroke_max_mm) + or gripper_stroke_max_mm <= gripper_stroke_min_mm + ): + raise ROSConfigError("gripper_stroke_max_mm must exceed gripper_stroke_min_mm.") + masks = { + "idle_timeout_error_mask": idle_timeout_error_mask, + "gripper_ignored_error_mask": gripper_ignored_error_mask, + } + if any(isinstance(v, bool) or not 0 <= int(v) <= _MAX_STATUS_MASK for v in masks.values()): + raise ROSConfigError(f"GalaxeaA1HAL motor status masks must be uint32 values: {masks}") + self._host = host + self._port = port + self._state_timeout_s = state_timeout_s + self._status_timeout_s = status_timeout_s + self._feedback_limit_tolerance = feedback_limit_tolerance_rad + self._alignment = initial_alignment_tolerance_rad + self._tracker_alignment_timeout = tracker_alignment_timeout_s + self._max_step = max_target_step_rad + self._lease = command_lease_s + self._idle_mask = int(idle_timeout_error_mask) + self._gripper_mask = int(gripper_ignored_error_mask) + self._gripper_min = gripper_stroke_min_mm + self._gripper_max = gripper_stroke_max_mm + self._connect_timeout_s = connect_timeout_s + self._transport: _A1Transport = transport or _SocketTransport(host, self._port) + self._connected = False + self._first_joint_command = True + self._accepted_action_count = 0 + + def connect(self) -> None: + """Connect to the sidecar and require one complete fresh snapshot.""" + if self._connected: + raise ROSRuntimeError("GalaxeaA1HAL is already connected.") + hello: dict[str, object] = { + "op": "hello", + "protocol": _PROTOCOL_VERSION, + "robot": "galaxea_a1", + "joint_names": list(_JOINT_NAMES), + "joint_position_min": [limits[0] for limits in _JOINT_LIMITS], + "joint_position_max": [limits[1] for limits in _JOINT_LIMITS], + "initial_alignment_tolerance_rad": self._alignment, + "tracker_alignment_timeout_s": self._tracker_alignment_timeout, + "max_target_step_rad": self._max_step, + "command_lease_s": self._lease, + "state_timeout_s": self._state_timeout_s, + "status_timeout_s": self._status_timeout_s, + "feedback_limit_tolerance_rad": self._feedback_limit_tolerance, + "idle_timeout_error_mask": self._idle_mask, + "gripper_ignored_error_mask": self._gripper_mask, + "gripper_stroke_min_mm": self._gripper_min, + "gripper_stroke_max_mm": self._gripper_max, + } + try: + self._transport.connect(hello, self._connect_timeout_s) + self._fresh_snapshot(require_status=True) + except Exception: + self._transport.close() + raise + self._connected = True + self._first_joint_command = True + self._accepted_action_count = 0 + log.info("hal.connect", robot=self.description.name, sidecar=f"{self._host}:{self._port}") + + def disconnect(self) -> None: + """Close the sidecar transport idempotently.""" + self._transport.close() + self._connected = False + + def read_state(self) -> JointState: + """Return the latest non-stale six-joint feedback snapshot.""" + self._require_connected("read_state") + state = self._fresh_snapshot(require_status=True) + return JointState( + name=list(_JOINT_NAMES), + position=list(state.position), + velocity=list(state.velocity), + effort=list(state.effort), + stamp_ns=state.stamp_ns, + ) + + def send_action(self, action: Action) -> None: + """Queue one validated joint or normalized-gripper target.""" + self._require_connected("send_action") + state = self._fresh_snapshot(require_status=True) + message: dict[str, object] = {"op": "action", "stamp_ns": action.stamp_ns} + if action.control_mode == ControlMode.JOINT_POSITION: + message["joint_targets"] = list(self._validated_joint_target(action, state)) + elif action.control_mode == ControlMode.GRIPPER_POSITION: + message["gripper"] = self._validated_gripper_target(action) + else: + raise ROSConfigError( + f"GalaxeaA1HAL does not support control mode {action.control_mode.value!r}." + ) + self._transport.send(message) + self._accepted_action_count += 1 + if action.control_mode == ControlMode.JOINT_POSITION: + self._first_joint_command = False + + def _validated_joint_target(self, action: Action, state: _Snapshot) -> tuple[float, ...]: + self._validate_action_dims(action, len(_JOINT_NAMES)) + if action.gripper: + raise ROSConfigError("gripper data require GRIPPER_POSITION control mode.") + if not action.joint_targets: + raise ROSConfigError("GalaxeaA1HAL requires joint_targets in JOINT_POSITION mode.") + target = tuple(float(v) for v in action.joint_targets[0]) + if not all(math.isfinite(v) for v in target): + raise ROSConfigError("Galaxea A1 joint target contains a non-finite value.") + for name, value, (lower, upper) in zip(_JOINT_NAMES, target, _JOINT_LIMITS, strict=True): + if not lower <= value <= upper: + raise ROSConfigError( + f"Galaxea A1 joint target {name}={value:.6f} is outside " + f"[{lower:.6f}, {upper:.6f}]." + ) + bound = self._alignment if self._first_joint_command else self._max_step + if any( + abs(value - current) > bound + for value, current in zip(target, state.position, strict=True) + ): + label = "initial alignment" if self._first_joint_command else "target step" + raise ROSConfigError( + f"Galaxea A1 {label} exceeds the configured {bound:.3f} rad limit." + ) + return target + + @staticmethod + def _validated_gripper_target(action: Action) -> float: + if action.joint_targets: + raise ROSConfigError("joint_targets require JOINT_POSITION control mode.") + if not action.gripper: + raise ROSConfigError("GalaxeaA1HAL requires gripper data in GRIPPER_POSITION mode.") + gripper = float(action.gripper[0]) + if not math.isfinite(gripper) or not 0.0 <= gripper <= 1.0: + raise ROSConfigError("Galaxea A1 gripper target must be finite and within [0, 1].") + return gripper + + def health(self) -> HALHealthReport: + """Return cached hardware health for the lifecycle diagnostics heartbeat.""" + state = self._fresh_snapshot(require_status=True) + now = time.monotonic() + return HALHealthReport( + message="A1 feedback and motor status healthy", + fields={ + "sidecar": f"{self._host}:{self._port}", + "joint_state_age_s": f"{now - state.received_monotonic:.3f}", + "motor_status_age_s": f"{now - state.status_received_monotonic:.3f}", + "motor_status_codes": ",".join(str(code) for code in state.status_codes), + "command_relay_state": state.relay_state, + "hal_accepted_action_count": str(self._accepted_action_count), + "sidecar_accepted_action_count": str(state.sidecar_action_count), + "gripper_position_normalized": f"{state.gripper_norm:.6f}", + "staged_position": ",".join(f"{value:.6f}" for value in state.staged_position), + "forwarded_position": ",".join( + f"{value:.6f}" for value in state.forwarded_position + ), + }, + ) + + def estop(self) -> None: + """Stop the sidecar-owned ROS 1 stack and always raise.""" + log.critical("hal.estop", robot=self.description.name) + try: + # The sidecar acknowledges only after tracker, driver, and roscore + # have exited. Its bounded shutdown allows up to four seconds + # (3 s SIGINT grace + 1 s SIGKILL wait). + self._transport.estop(timeout_s=4.0) + finally: + self._connected = False + raise ROSEStopRequested("Emergency stop triggered on Galaxea A1; ROS 1 sidecar stopped.") + + def _fresh_snapshot(self, *, require_status: bool = False) -> _Snapshot: + state = self._transport.snapshot() + age = time.monotonic() - state.received_monotonic + if age > self._state_timeout_s: + raise ROSPerceptionStale( + f"Galaxea A1 joint state is {age:.3f} s old (limit {self._state_timeout_s:.3f} s)." + ) + if require_status: + status_age = time.monotonic() - state.status_received_monotonic + if status_age > self._status_timeout_s: + raise ROSPerceptionStale( + f"Galaxea A1 motor status is {status_age:.3f} s old " + f"(limit {self._status_timeout_s:.3f} s)." + ) + for index, code in enumerate(state.status_codes[: _GRIPPER_STATUS_INDEX + 1]): + allowed = self._idle_mask | ( + self._gripper_mask if index == _GRIPPER_STATUS_INDEX else 0 + ) + if code & ~allowed: + raise ROSRuntimeError( + f"Galaxea A1 actuator {index + 1} reports error code {code}." + ) + for name, value, (lower, upper) in zip( + _JOINT_NAMES, state.position, _JOINT_LIMITS, strict=True + ): + if not ( + lower - self._feedback_limit_tolerance + <= value + <= upper + self._feedback_limit_tolerance + ): + raise ROSRuntimeError( + f"Galaxea A1 feedback {name}={value:.6f} is outside " + f"[{lower:.6f}, {upper:.6f}] by more than the configured " + f"{self._feedback_limit_tolerance:.3f} rad tolerance." + ) + return state diff --git a/python/hal/src/openral_hal/lifecycle.py b/python/hal/src/openral_hal/lifecycle.py index 6a91ee8..09d4dd1 100644 --- a/python/hal/src/openral_hal/lifecycle.py +++ b/python/hal/src/openral_hal/lifecycle.py @@ -402,6 +402,46 @@ def _heartbeat_extra_fields(self) -> dict[str, str]: """ return {} + def _heartbeat_status(self, robot_name: str) -> tuple[int, str, dict[str, str]]: + """Return the generic status plus optional cached HAL health. + + Real hardware adapters may implement ``HALHealthProvider``. Its + ``health()`` method must use cached state only; the 1 Hz diagnostics + path must not perform device or network I/O. + """ + from openral_observability import Level + + from openral_hal.protocol import HALHealthProvider + + extras = self._heartbeat_extra_fields() + if self._estopped: + return ( + Level.ERROR, + "estop latched", + {"robot": robot_name, "estopped": "true", **extras}, + ) + if self._hal is None: + return Level.ERROR, "hal disconnected", {"robot": robot_name, **extras} + if isinstance(self._hal, HALHealthProvider): + try: + report = self._hal.health() + except Exception as exc: # reason: diagnostics must surface, not hide, HAL faults + return ( + Level.ERROR, + f"HAL health check failed: {exc}", + {"robot": robot_name, **extras}, + ) + return ( + Level.OK, + report.message, + {"robot": robot_name, "estopped": "false", **extras, **report.fields}, + ) + return ( + Level.OK, + "hal ready", + {"robot": robot_name, "estopped": "false", **extras}, + ) + def on_configure_post_hal(self) -> TransitionCallbackReturn: """Subclass extension point after the base wires HAL + heartbeat. @@ -439,7 +479,7 @@ def on_cleanup_pre_disconnect(self) -> None: def on_configure(self, state: object) -> TransitionCallbackReturn: """Construct + connect the HAL, wire the heartbeat, then run post-hook.""" from openral_core.exceptions import ROSConfigError, ROSRuntimeError - from openral_observability import DiagnosticsHeartbeat, Level + from openral_observability import DiagnosticsHeartbeat try: self._hal = self._create_hal() @@ -451,16 +491,7 @@ def on_configure(self, state: object) -> TransitionCallbackReturn: robot_name = getattr(getattr(self._hal, "description", None), "name", self._node_name) def _status() -> tuple[int, str, dict[str, str]]: - extras = self._heartbeat_extra_fields() - if self._estopped: - return Level.ERROR, "estop latched", {"robot": str(robot_name), **extras} - if self._hal is None: - return Level.ERROR, "hal disconnected", {"robot": str(robot_name), **extras} - return ( - Level.OK, - "hal ready", - {"robot": str(robot_name), "estopped": "false", **extras}, - ) + return self._heartbeat_status(str(robot_name)) self._heartbeat = DiagnosticsHeartbeat( self, @@ -745,15 +776,22 @@ def _publish_joint_state(self) -> None: """Timer callback: publish joint state. From the proprio snapshot for sim-attached HALs, else a live - ``hal.read_state``. + ``hal.read_state``. While the e-stop latch is set, snapshot-backed + HALs keep publishing the last post-step frame so operators retain + joint visibility through the latch; real HALs are skipped instead — + their transport may already be torn down (``RESTART_REQUIRED`` + stops the whole vendor stack) and polling would only spam errors. """ + if self._hal is None or self._publisher is None: + return + if self._estopped and self._proprio is None: + return + from openral_observability import producer as ral_producer from openral_observability import semconv from opentelemetry import trace from sensor_msgs.msg import JointState as RosJointState - if self._hal is None or self._publisher is None: - return tick_idx = self._read_tick_idx self._read_tick_idx += 1 tracer = trace.get_tracer("openral_hal.lifecycle") @@ -899,25 +937,75 @@ def _send_action_traced(self, action: Any, *, source: str) -> None: # noqa: ANN ) def _on_estop(self, _msg: object) -> None: - """CLAUDE.md §1.5 — latch the estop flag.""" + """Latch and invoke downstream stop for HALs that explicitly opt in.""" if self._estopped: return self._estopped = True self.get_logger().error( "openral_hal.estop_received; ignoring further commands until reset." ) + from openral_hal.protocol import LifecycleEStopHAL + + if self._hal is None or not isinstance(self._hal, LifecycleEStopHAL): + return + from openral_core.exceptions import ROSEStopRequested + + try: + self._hal.estop() + except ROSEStopRequested as exc: + self.get_logger().error(f"hardware estop completed: {exc}") + except Exception as exc: # reason: latch must survive a vendor stop-path failure + self.get_logger().fatal(f"hardware estop failed: {exc}") def _on_estop_cleared(self, _msg: object) -> None: """Clear the estop latch when the reset authority broadcasts /openral/estop_cleared. - Without this the HAL stayed latched after the kernel's estop_reset — - it dropped every command (``_on_safe_action`` returns early on - ``_estopped``) so the robot never resumed until a node restart. The - kernel's cooldown gate has already passed by the time this fires - (the dashboard publishes it only after estop_reset returns success). + Recovery is per-HAL policy. HALs without the hardware-estop opt-in + just clear the local latch — without that the node dropped every + command (``_on_safe_action`` returns early on ``_estopped``) and + never resumed until a node restart. Opted-in HALs declare + ``estop_recovery``: ``RESETTABLE`` HALs get ``reset_estop()`` and + then resume, while ``RESTART_REQUIRED`` HALs (e.g. Galaxea A1, + whose estop stops the entire ROS 1 sidecar) deliberately keep the + latch — a full lifecycle restart with fresh alignment is the only + way back. The kernel's cooldown gate has already passed by the + time this fires (the dashboard publishes it only after estop_reset + returns success). """ if not self._estopped: return + + from openral_hal.protocol import ( + EStopRecovery, + LifecycleEStopHAL, + ResettableLifecycleEStopHAL, + ) + + if isinstance(self._hal, LifecycleEStopHAL): + recovery = self._hal.estop_recovery + if recovery == EStopRecovery.RESTART_REQUIRED: + self.get_logger().error( + "openral_hal.estop_clear_rejected; this real HAL requires a full " + "lifecycle restart and fresh alignment after hardware estop." + ) + return + if recovery != EStopRecovery.RESETTABLE: + self.get_logger().fatal( + f"openral_hal.estop_clear_rejected; unsupported recovery policy " + f"{recovery!r}." + ) + return + if not isinstance(self._hal, ResettableLifecycleEStopHAL): + self.get_logger().fatal( + "openral_hal.estop_clear_rejected; resettable HAL does not implement " + "reset_estop()." + ) + return + try: + self._hal.reset_estop() + except Exception as exc: # reason: never clear local latch after reset failure + self.get_logger().fatal(f"hardware estop reset failed: {exc}") + return self._estopped = False self.get_logger().info("openral_hal.estop_cleared; resuming command execution.") diff --git a/python/hal/src/openral_hal/protocol.py b/python/hal/src/openral_hal/protocol.py index e7a5deb..caf04d8 100644 --- a/python/hal/src/openral_hal/protocol.py +++ b/python/hal/src/openral_hal/protocol.py @@ -15,11 +15,74 @@ from __future__ import annotations +from collections.abc import Mapping +from dataclasses import dataclass +from enum import StrEnum from typing import Protocol, runtime_checkable from openral_core.schemas import Action, JointState, RobotDescription -__all__ = ["HAL"] +__all__ = [ + "HAL", + "EStopRecovery", + "HALHealthProvider", + "HALHealthReport", + "LifecycleEStopHAL", + "ResettableLifecycleEStopHAL", +] + + +class EStopRecovery(StrEnum): + """Recovery policy after a lifecycle node forwards an e-stop to a HAL.""" + + RESETTABLE = "resettable" + RESTART_REQUIRED = "restart_required" + + +@dataclass(frozen=True) +class HALHealthReport: + """Cached health data exposed through the lifecycle diagnostics heartbeat. + + ``health()`` implementations must not perform device or network I/O. The + lifecycle node calls them from its low-rate diagnostics path. + """ + + message: str + fields: Mapping[str, str] + + +@runtime_checkable +class HALHealthProvider(Protocol): + """Optional HAL extension for cached diagnostics.""" + + def health(self) -> HALHealthReport: + """Return the latest cached health report without performing I/O.""" + ... + + +@runtime_checkable +class LifecycleEStopHAL(Protocol): + """Optional extension for HALs that propagate the lifecycle e-stop downstream. + + HALs that implement this protocol opt into receiving ``estop()`` when the + generic lifecycle node latches ``/openral/estop``. Existing HALs that do not + opt in retain the lifecycle node's local latch-only behavior. + """ + + estop_recovery: EStopRecovery + + def estop(self) -> None: + """Stop downstream hardware or owned processes and always raise.""" + ... + + +@runtime_checkable +class ResettableLifecycleEStopHAL(LifecycleEStopHAL, Protocol): + """Lifecycle e-stop extension for HALs that support in-process recovery.""" + + def reset_estop(self) -> None: + """Re-arm the downstream controller after its safety conditions pass.""" + ... @runtime_checkable diff --git a/python/hal/src/openral_hal/sim_bringup.py b/python/hal/src/openral_hal/sim_bringup.py index 003dfc5..023c1f2 100644 --- a/python/hal/src/openral_hal/sim_bringup.py +++ b/python/hal/src/openral_hal/sim_bringup.py @@ -229,7 +229,7 @@ def build_sim_env_from_yaml( if scene_id not in SCENES: raise ROSConfigError( f"build_sim_env_from_yaml: scene id {scene_id!r} is not registered in " - f"openral_sim.SCENES. Available: {sorted(SCENES)}." # type: ignore[call-overload] # reason: _Registry is iterable and yields comparable str keys at runtime + f"openral_sim.SCENES. Available: {sorted(SCENES)}." ) # Resolve robot_id from the YAML, the SCENES fixed_robot registry, diff --git a/python/runner/pyproject.toml b/python/runner/pyproject.toml index ae86c4d..44ab541 100644 --- a/python/runner/pyproject.toml +++ b/python/runner/pyproject.toml @@ -11,6 +11,7 @@ dependencies = [ "openral-observability", # reason: rskill_span / safety_span wired into HardwareRunner ticks "openral-rskill", # reason: HardwareRunner accepts a Skill and calls Skill.step "openral-world-state", # reason: WorldStateAggregator is the runner's WorldState producer + "msgpack>=1.1", # reason: native A1 Runtime local service framing "structlog>=24.1", # reason: structured logging per CLAUDE.md §5.1 ] diff --git a/python/runner/src/openral_runner/backends/galaxea_a1_camera_bridge.py b/python/runner/src/openral_runner/backends/galaxea_a1_camera_bridge.py new file mode 100644 index 0000000..b41b3ae --- /dev/null +++ b/python/runner/src/openral_runner/backends/galaxea_a1_camera_bridge.py @@ -0,0 +1,256 @@ +"""Native SensorReader for the A1 Runtime paired Camera Bridge.""" + +from __future__ import annotations + +import time +from dataclasses import dataclass +from threading import RLock +from typing import Literal, cast + +import numpy as np +from numpy.typing import NDArray +from openral_core import FrameEncoding, SensorFrame +from openral_core.exceptions import ROSConfigError, ROSPerceptionStale, ROSRuntimeError + +from openral_runner.backends.galaxea_a1_ipc import ( + RuntimeLocalClient, + decode_array, +) + +__all__ = ["GalaxeaA1CameraBridgeReader"] + +_PROTOCOL_VERSION = 2 +_SOCKET_NAME = "a1-camera-bridge.sock" +_MAX_RESPONSE_BYTES = 128 * 1024 * 1024 +_SHA256_HEX_LENGTH = 64 +_COLOR_NDIM = 3 +_COLOR_CHANNELS = 3 + + +@dataclass(frozen=True) +class _CachedCameraFrame: + rgb: NDArray[np.uint8] + stamp_monotonic_ns: int + stamp_wall_ns: int + + +class _GalaxeaA1CameraBridgeSession: + """Reference-counted native client for one atomic paired-frame stream.""" + + def __init__(self) -> None: + self._lock = RLock() + self._owners = 0 + self._client: RuntimeLocalClient | None = None + self._contract_digest: str | None = None + self._shapes: dict[str, tuple[int, int, int]] = {} + self._after = {"front": -1, "wrist": -1} + self._pair: dict[str, _CachedCameraFrame] | None = None + self._consumed: set[str] = set() + + def open(self) -> None: + with self._lock: + if self._owners: + self._owners += 1 + return + client = RuntimeLocalClient( + _SOCKET_NAME, + label="Galaxea A1 Camera Bridge", + max_response_bytes=_MAX_RESPONSE_BYTES, + ) + client.connect(timeout_s=5.0) + try: + response = client.call( + {"version": _PROTOCOL_VERSION, "op": "describe"}, + timeout_s=5.0, + ) + self._install_description(response.get("metadata")) + except BaseException: + client.close() + raise + self._client = client + self._owners = 1 + + def close(self) -> None: + with self._lock: + if not self._owners: + return + self._owners -= 1 + if self._owners: + return + client, self._client = self._client, None + self._pair = None + self._consumed.clear() + if client is not None: + client.close() + + def read( + self, + camera: Literal["front", "wrist"], + *, + max_age_ms: int, + ) -> _CachedCameraFrame: + if max_age_ms <= 0: + raise ROSConfigError("max_age_ms must be positive") + with self._lock: + if self._client is None: + raise ROSRuntimeError("Galaxea A1 Camera Bridge session is closed") + if self._pair is None or camera in self._consumed: + self._pair = self._read_pair(timeout_s=max_age_ms / 1000.0) + self._consumed.clear() + frame = self._pair[camera] + age_ms = (time.monotonic_ns() - frame.stamp_monotonic_ns) / 1e6 + if age_ms > max_age_ms: + raise ROSPerceptionStale( + f"Galaxea A1 {camera} frame is {age_ms:.1f} ms old (budget {max_age_ms} ms)" + ) + self._consumed.add(camera) + return frame + + def _install_description(self, value: object) -> None: + if not isinstance(value, dict): + raise ROSRuntimeError("A1 Camera Bridge metadata is missing") + metadata = cast(dict[str, object], value) + digest = metadata.get("contract_digest") + if not isinstance(digest, str) or len(digest) != _SHA256_HEX_LENGTH: + raise ROSRuntimeError("A1 Camera Bridge contract digest is invalid") + self._contract_digest = digest + self._shapes = { + camera: _shape3(metadata.get(f"{camera}_color_shape"), camera=camera) + for camera in ("front", "wrist") + } + + def _read_pair(self, *, timeout_s: float) -> dict[str, _CachedCameraFrame]: + assert self._client is not None + assert self._contract_digest is not None + response = self._client.call( + { + "version": _PROTOCOL_VERSION, + "op": "next_pair", + "contract_digest": self._contract_digest, + "after": dict(self._after), + }, + timeout_s=max(1.0, timeout_s), + ) + if response.get("changed") is not True: + raise ROSPerceptionStale("no fresh synchronized Galaxea A1 camera pair") + metadata = response.get("metadata") + if ( + not isinstance(metadata, dict) + or metadata.get("contract_digest") != self._contract_digest + ): + raise ROSRuntimeError("A1 Camera Bridge contract changed while connected") + wall_ns = time.time_ns() + pair = { + camera: self._decode_frame( + response.get(camera), + camera=camera, + wall_ns=wall_ns, + ) + for camera in ("front", "wrist") + } + self._after = { + camera: _plain_int(response[camera].get("seq"), label=f"{camera}.seq") + for camera in ("front", "wrist") + } + return pair + + def _decode_frame( + self, + value: object, + *, + camera: str, + wall_ns: int, + ) -> _CachedCameraFrame: + if not isinstance(value, dict): + raise ROSRuntimeError(f"A1 Camera Bridge response is missing {camera}") + payload = cast(dict[str, object], value) + monotonic_s = payload.get("monotonic_s") + if ( + isinstance(monotonic_s, bool) + or not isinstance(monotonic_s, (int, float)) + or not np.isfinite(monotonic_s) + ): + raise ROSRuntimeError(f"A1 Camera Bridge {camera} timestamp is invalid") + bgr = decode_array( + payload.get("color_bgr"), + shape=self._shapes[camera], + dtype=np.dtype(np.uint8), + label=f"A1 Camera Bridge {camera} color", + ) + return _CachedCameraFrame( + rgb=np.ascontiguousarray(bgr[..., ::-1]), + stamp_monotonic_ns=int(float(monotonic_s) * 1e9), + stamp_wall_ns=wall_ns, + ) + + +class GalaxeaA1CameraBridgeReader: + """Expose one synchronized A1 Runtime camera view as RGB8.""" + + def __init__( + self, + *, + sensor_id: str, + camera: Literal["front", "wrist"], + session: _GalaxeaA1CameraBridgeSession, + default_max_age_ms: int, + ) -> None: + """Store one camera view over an explicitly shared native session.""" + if default_max_age_ms <= 0: + raise ROSConfigError("default_max_age_ms must be positive") + self.sensor_id = sensor_id + self.is_open = False + self._camera = camera + self._session = session + self._default_max_age_ms = default_max_age_ms + + def open(self) -> None: + """Acquire the shared Runtime camera connection.""" + if self.is_open: + return + self._session.open() + self.is_open = True + + def close(self) -> None: + """Release this view and close the connection after the last owner.""" + if not self.is_open: + return + try: + self._session.close() + finally: + self.is_open = False + + def read_latest(self, max_age_ms: int | None = None) -> SensorFrame: + """Return the latest source-timestamped RGB frame.""" + if not self.is_open: + raise ROSRuntimeError(f"GalaxeaA1CameraBridgeReader({self.sensor_id!r}) is closed") + budget_ms = self._default_max_age_ms if max_age_ms is None else max_age_ms + frame = self._session.read(self._camera, max_age_ms=budget_ms) + height, width, channels = frame.rgb.shape + return SensorFrame( + sensor_id=self.sensor_id, + stamp_monotonic_ns=frame.stamp_monotonic_ns, + stamp_wall_ns=frame.stamp_wall_ns, + encoding=FrameEncoding.RGB8, + width=int(width), + height=int(height), + channels=int(channels), + data=frame.rgb.tobytes(), + ) + + +def _shape3(value: object, *, camera: str) -> tuple[int, int, int]: + if ( + not isinstance(value, (list, tuple)) + or len(value) != _COLOR_NDIM + or value[2] != _COLOR_CHANNELS + or any(isinstance(item, bool) or not isinstance(item, int) or item <= 0 for item in value) + ): + raise ROSRuntimeError(f"A1 Camera Bridge {camera} shape is invalid") + return cast(tuple[int, int, int], tuple(value)) + + +def _plain_int(value: object, *, label: str) -> int: + if isinstance(value, bool) or not isinstance(value, int): + raise ROSRuntimeError(f"A1 Camera Bridge {label} must be an integer") + return value diff --git a/python/runner/src/openral_runner/backends/galaxea_a1_ipc.py b/python/runner/src/openral_runner/backends/galaxea_a1_ipc.py new file mode 100644 index 0000000..05184ac --- /dev/null +++ b/python/runner/src/openral_runner/backends/galaxea_a1_ipc.py @@ -0,0 +1,169 @@ +"""Native client for Galaxea A1 Runtime local MessagePack services.""" + +from __future__ import annotations + +import os +import socket +import struct +from pathlib import Path +from typing import Any + +import msgpack +import numpy as np +from numpy.typing import NDArray +from openral_core.exceptions import ROSConfigError, ROSRuntimeError + +_PACKET_LENGTH = struct.Struct("!I") + + +def runtime_socket_path(name: str) -> Path: + """Resolve one public Runtime endpoint without locating its source checkout.""" + if not name or Path(name).name != name: + raise ROSConfigError("A1 Runtime socket name must be one path component") + configured = os.environ.get("A1_PROCESS_STATE_ROOT", "").strip() + if configured: + state_root = Path(configured) + else: + runtime_root = Path(os.environ.get("XDG_RUNTIME_DIR", "/tmp")) + state_root = runtime_root / f"galaxea-a1-runtime-{os.getuid()}" + return state_root.expanduser().resolve() / name + + +class RuntimeLocalClient: + """One synchronous connection to a private A1 Runtime service.""" + + def __init__( + self, + socket_name: str, + *, + label: str, + max_response_bytes: int, + ) -> None: + """Store one endpoint contract without opening the socket.""" + if max_response_bytes <= 0: + raise ROSConfigError("A1 Runtime max_response_bytes must be positive") + self.path = runtime_socket_path(socket_name) + self.label = label + self.max_response_bytes = max_response_bytes + self._socket: socket.socket | None = None + + def connect(self, *, timeout_s: float) -> None: + """Connect to the Runtime-owned Unix socket.""" + if timeout_s <= 0: + raise ROSConfigError(f"{self.label} connect timeout must be positive") + if self._socket is not None: + return + active_socket = socket.socket(socket.AF_UNIX, socket.SOCK_STREAM) + active_socket.settimeout(timeout_s) + try: + active_socket.connect(str(self.path)) + except OSError as exc: + active_socket.close() + raise ROSConfigError( + f"{self.label} is unavailable at {self.path}; start the " + "corresponding A1 Runtime service" + ) from exc + self._socket = active_socket + + def call( + self, + request: dict[str, Any], + *, + timeout_s: float, + ) -> dict[str, Any]: + """Perform one bounded request and require an explicit success reply.""" + active_socket = self._socket + if active_socket is None: + raise ROSRuntimeError(f"{self.label} client is closed") + if timeout_s <= 0: + raise ROSConfigError(f"{self.label} request timeout must be positive") + active_socket.settimeout(timeout_s) + try: + _send_packet(active_socket, request) + response = _receive_packet( + active_socket, + max_bytes=self.max_response_bytes, + ) + except (OSError, ValueError) as exc: + self.close() + raise ROSRuntimeError(f"{self.label} transport failed: {exc}") from exc + if not isinstance(response, dict): + self.close() + raise ROSRuntimeError(f"{self.label} returned a non-map response") + if response.get("ok") is not True: + raise ROSRuntimeError( + f"{self.label} rejected the request: {response.get('error', 'unknown error')}" + ) + return response + + def close(self) -> None: + """Close the local connection idempotently.""" + active_socket, self._socket = self._socket, None + if active_socket is not None: + active_socket.close() + + +def encode_array(value: NDArray[Any]) -> dict[str, Any]: + """Encode one contiguous ndarray with an explicit shape and dtype.""" + array = np.ascontiguousarray(value) + return { + "shape": list(array.shape), + "dtype": array.dtype.str, + "data": array.tobytes(), + } + + +def decode_array( + value: object, + *, + shape: tuple[int, ...], + dtype: np.dtype[Any], + label: str, +) -> NDArray[Any]: + """Decode an exact ndarray payload and reject protocol drift.""" + if not isinstance(value, dict) or set(value) != {"shape", "dtype", "data"}: + raise ROSRuntimeError(f"{label} array payload is invalid") + if value["shape"] != list(shape) or value["dtype"] != dtype.str: + raise ROSRuntimeError(f"{label} array contract mismatch: expected {shape} {dtype}") + data = value["data"] + size = int(np.prod(shape)) * dtype.itemsize + if not isinstance(data, bytes) or len(data) != size: + raise ROSRuntimeError(f"{label} array byte length mismatch") + return np.frombuffer(data, dtype=dtype).reshape(shape).copy() + + +def _send_packet(active_socket: socket.socket, value: object) -> None: + payload = msgpack.packb(value, use_bin_type=True) + active_socket.sendall(_PACKET_LENGTH.pack(len(payload)) + payload) + + +def _receive_packet( + active_socket: socket.socket, + *, + max_bytes: int, +) -> object | None: + header = _receive_exact(active_socket, _PACKET_LENGTH.size) + if header is None: + return None + (size,) = _PACKET_LENGTH.unpack(header) + if size <= 0 or size > max_bytes: + raise ValueError(f"invalid A1 Runtime packet size: {size}") + payload = _receive_exact(active_socket, size) + if payload is None: + raise ConnectionError("A1 Runtime packet ended early") + decoded: object = msgpack.unpackb(payload, raw=False, strict_map_key=False) + return decoded + + +def _receive_exact(active_socket: socket.socket, size: int) -> bytes | None: + chunks: list[bytes] = [] + remaining = size + while remaining: + chunk = active_socket.recv(remaining) + if not chunk: + if remaining == size: + return None + raise ConnectionError("A1 Runtime connection ended mid-packet") + chunks.append(chunk) + remaining -= len(chunk) + return b"".join(chunks) diff --git a/python/runner/src/openral_runner/factory.py b/python/runner/src/openral_runner/factory.py index 6d0ca86..f2e347d 100644 --- a/python/runner/src/openral_runner/factory.py +++ b/python/runner/src/openral_runner/factory.py @@ -6,7 +6,8 @@ * ``SKILL_REGISTRY`` — :attr:`VLASpec.id` → :class:`Skill` factory. Today only ``gpu_passthrough`` (a no-op rSkill for plumbing verification). * ``SENSOR_BACKEND_REGISTRY`` — :attr:`SensorReaderConfig.backend` → - :class:`SensorReader` factory (``opencv_thread`` / ``gstreamer``). + :class:`SensorReader` factory (``opencv_thread`` / ``gstreamer`` / + ``galaxea_a1_camera_bridge``). Adding skills / backends is additive — append to the dict. The rSkill that drives a real deployment is selected at runtime by the reasoner from the @@ -15,7 +16,8 @@ from __future__ import annotations -from collections.abc import Callable +from collections.abc import Callable, Sequence +from typing import TYPE_CHECKING, Literal, cast import structlog from openral_core import ( @@ -29,9 +31,15 @@ from openral_runner.backends.gstreamer.pipeline import PipelineSpec, Source from openral_runner.sensor_reader import SensorReader +if TYPE_CHECKING: + from openral_runner.backends.galaxea_a1_camera_bridge import ( + _GalaxeaA1CameraBridgeSession, + ) + __all__ = [ "SENSOR_BACKEND_REGISTRY", "SKILL_REGISTRY", + "make_sensor_readers", ] log = structlog.get_logger(__name__) @@ -186,6 +194,77 @@ def _make_gstreamer_reader(cfg: SensorReaderConfig) -> SensorReader: ) +def _galaxea_a1_camera_bridge_params( + cfg: SensorReaderConfig, +) -> Literal["front", "wrist"]: + """Validate and return the explicit A1 camera bridge scene parameters.""" + params = cfg.backend_params + allowed = {"camera"} + unknown = set(params) - allowed + if unknown: + raise ROSConfigError( + f"SensorReaderConfig({cfg.sensor_id!r}, " + f"backend=galaxea_a1_camera_bridge) has unknown params {sorted(unknown)!r}" + ) + camera = params.get("camera") + if camera not in {"front", "wrist"}: + raise ROSConfigError( + f"SensorReaderConfig({cfg.sensor_id!r}, " + "backend=galaxea_a1_camera_bridge) requires " + "backend_params.camera to be 'front' or 'wrist'" + ) + return cast(Literal["front", "wrist"], camera) + + +def _make_galaxea_a1_camera_bridge_reader( + cfg: SensorReaderConfig, + *, + session: _GalaxeaA1CameraBridgeSession | None = None, +) -> SensorReader: + """Build one view over an explicitly shareable A1 paired-camera session.""" + from openral_runner.backends.galaxea_a1_camera_bridge import ( + GalaxeaA1CameraBridgeReader, + _GalaxeaA1CameraBridgeSession, + ) + + camera = _galaxea_a1_camera_bridge_params(cfg) + if session is None: + paired_session = _GalaxeaA1CameraBridgeSession() + elif isinstance(session, _GalaxeaA1CameraBridgeSession): + paired_session = session + else: + raise TypeError("session must be an A1 camera bridge session") + return GalaxeaA1CameraBridgeReader( + sensor_id=cfg.sensor_id, + camera=camera, + session=paired_session, + default_max_age_ms=cfg.max_age_ms, + ) + + +def make_sensor_readers(configs: Sequence[SensorReaderConfig]) -> list[SensorReader]: + """Build readers in order, sharing resources within one deployment batch.""" + a1_backend = "galaxea_a1_camera_bridge" + a1_session: _GalaxeaA1CameraBridgeSession | None = None + readers: list[SensorReader] = [] + for cfg in configs: + if cfg.backend.value == a1_backend: + from openral_runner.backends.galaxea_a1_camera_bridge import ( + _GalaxeaA1CameraBridgeSession, + ) + + _galaxea_a1_camera_bridge_params(cfg) + if a1_session is None: + a1_session = _GalaxeaA1CameraBridgeSession() + readers.append(_make_galaxea_a1_camera_bridge_reader(cfg, session=a1_session)) + continue + factory = SENSOR_BACKEND_REGISTRY.get(cfg.backend.value) + if factory is None: + raise ROSConfigError(f"unknown sensor reader backend {cfg.backend.value!r}") + readers.append(factory(cfg)) + return readers + + def _gstreamer_spec_from_params( cfg: SensorReaderConfig, source_param: object, @@ -246,5 +325,6 @@ def _copy_bool_if_present(dst: dict[str, object], src: dict[str, object], key: s SENSOR_BACKEND_REGISTRY: dict[str, Callable[[SensorReaderConfig], SensorReader]] = { "opencv_thread": _make_opencv_thread_reader, "gstreamer": _make_gstreamer_reader, + "galaxea_a1_camera_bridge": _make_galaxea_a1_camera_bridge_reader, } """Registry of SensorReader factories. Keyed by :attr:`SensorReaderConfig.backend`.""" diff --git a/python/sensors/src/openral_sensors/ros_publisher.py b/python/sensors/src/openral_sensors/ros_publisher.py index 2348887..5db4331 100644 --- a/python/sensors/src/openral_sensors/ros_publisher.py +++ b/python/sensors/src/openral_sensors/ros_publisher.py @@ -129,6 +129,7 @@ def __init__( qos_depth: int = _DEFAULT_QOS_DEPTH, camera_info: IntrinsicsPinhole | None = None, info_topic: str | None = None, + node: Node | None = None, ) -> None: """Stash configuration; no ROS I/O until :meth:`start`.""" if not topic.startswith("/"): @@ -149,8 +150,10 @@ def __init__( self._qos_depth = qos_depth self._camera_info_spec = camera_info - # Populated by start(). - self._node: Node | None = None + # A composed runtime injects its existing node. Standalone callers + # retain the original private-node behavior. + self._node: Node | None = node + self._owns_node = node is None self._image_publisher: Publisher | None = None self._info_publisher: Publisher | None = None self._we_initialised_rclpy = False @@ -187,15 +190,19 @@ def info_topic(self) -> str: """The configured ``CameraInfo`` companion topic (read-only).""" return self._info_topic - def start(self) -> None: - """Init rclpy (if needed), create publishers, start the pump thread. + def prepare(self) -> None: + """Create ROS resources without starting the frame-pump thread. + + Multi-camera callers prepare every publisher before starting any + background pump. This avoids concurrent rclpy operations while another + camera's node is still being constructed. Raises: RuntimeError: When ``rclpy`` / ``sensor_msgs`` are not importable. Source a ROS 2 install before instantiating the lifecycle node that owns this publisher. """ - if self._is_started: + if self._image_publisher is not None: return try: import rclpy @@ -217,9 +224,10 @@ def start(self) -> None: rclpy.init() self._we_initialised_rclpy = True - from rclpy.node import Node + if self._node is None: + from rclpy.node import Node - self._node = Node(self._node_name) + self._node = Node(self._node_name) # Image stream QoS — sensor_data-style per CLAUDE.md §5.3. image_qos = QoSProfile( history=QoSHistoryPolicy.KEEP_LAST, @@ -240,6 +248,11 @@ def start(self) -> None: CameraInfo, self._info_topic, info_qos ) + def start(self) -> None: + """Prepare ROS resources and start the frame-pump thread.""" + if self._is_started: + return + self.prepare() self._stop_event.clear() self._thread = threading.Thread( target=self._pump_loop, @@ -263,7 +276,7 @@ def stop(self) -> None: to finish its current publish; a stuck thread is logged but not awaited indefinitely. """ - if not self._is_started: + if not self._is_started and self._image_publisher is None: return self._stop_event.set() if self._thread is not None: @@ -281,10 +294,10 @@ def stop(self) -> None: if self._info_publisher is not None and self._node is not None: self._node.destroy_publisher(self._info_publisher) self._info_publisher = None - if self._node is not None: + if self._node is not None and self._owns_node: self._node.destroy_node() - self._node = None - if self._we_initialised_rclpy: + self._node = None + if self._we_initialised_rclpy and self._owns_node: import rclpy if rclpy.ok(): diff --git a/python/sim/src/openral_sim/policies/__init__.py b/python/sim/src/openral_sim/policies/__init__.py index c291103..626f862 100644 --- a/python/sim/src/openral_sim/policies/__init__.py +++ b/python/sim/src/openral_sim/policies/__init__.py @@ -20,6 +20,7 @@ def _register_policies() -> None: diffusion, gr00t, internvla_n1, + lingbot_va_a1, lingbot_vla2, mock, molmoact2, diff --git a/python/sim/src/openral_sim/policies/lingbot_va_a1.py b/python/sim/src/openral_sim/policies/lingbot_va_a1.py new file mode 100644 index 0000000..7e76b47 --- /dev/null +++ b/python/sim/src/openral_sim/policies/lingbot_va_a1.py @@ -0,0 +1,254 @@ +"""LingBot-VA adapter for the Runtime-owned Galaxea A1 policy gateway.""" + +from __future__ import annotations + +from typing import Any + +import numpy as np +from numpy.typing import NDArray +from openral_core import VLASpec +from openral_core.exceptions import ROSConfigError, ROSRuntimeError +from openral_rskill.loader import load_rskill_manifest +from openral_runner.backends.galaxea_a1_ipc import ( + RuntimeLocalClient, + decode_array, + encode_array, +) + +from openral_sim.registry import POLICIES + +_PROTOCOL_VERSION = "galaxea_a1_openral_policy_v1" +_SOCKET_NAME = "a1-openral-policy.sock" +_MAX_RESPONSE_BYTES = 2 * 1024 * 1024 +_ACTION_DIM = 7 +_COLOR_NDIM = 3 +_COLOR_CHANNELS = 3 + + +def _joint_contract( + spec: VLASpec, + robot_description: Any, +) -> tuple[tuple[str, ...], NDArray[np.float64], NDArray[np.float64], float]: + """Validate the rSkill replay bound against the active A1 HAL contract.""" + if robot_description is None or robot_description.name != "galaxea_a1": + raise ROSConfigError("lingbot_va_a1 requires the Galaxea A1 RobotDescription") + joints = tuple(robot_description.joints) + names = tuple(joint.name for joint in joints) + lower: list[float] = [] + upper: list[float] = [] + for joint in joints: + limits = joint.position_limits + if limits is None: + raise ROSConfigError(f"OpenRAL joint {joint.name!r} requires finite position limits") + lo, hi = (float(item) for item in limits) + if not np.isfinite((lo, hi)).all() or lo >= hi: + raise ROSConfigError(f"OpenRAL joint {joint.name!r} requires finite position limits") + lower.append(lo) + upper.append(hi) + raw_step = spec.extra.get("max_joint_substep_rad") + if isinstance(raw_step, bool) or not isinstance(raw_step, (int, float)): + raise ROSConfigError("lingbot_va_a1 requires numeric policy_extras.max_joint_substep_rad") + step = float(raw_step) + try: + defaults = robot_description.hal.parameters.defaults + phase_limits = ( + float(defaults["max_target_step_rad"]), + float(defaults["initial_alignment_tolerance_rad"]), + ) + except (AttributeError, KeyError, TypeError, ValueError) as exc: + raise ROSConfigError( + "Galaxea A1 HAL requires target-step and initial-alignment limits" + ) from exc + if ( + not np.isfinite(step) + or step <= 0 + or any(not np.isfinite(limit) or limit <= 0 for limit in phase_limits) + or step > min(phase_limits) + ): + raise ROSConfigError( + "policy max_joint_substep_rad must be positive and no greater than " + "the A1 HAL target-step and initial-alignment limits" + ) + return ( + names, + np.asarray(lower, dtype=np.float64), + np.asarray(upper, dtype=np.float64), + step, + ) + + +def _model_identity(spec: VLASpec) -> tuple[str, str]: + """Return the immutable model repo and revision from the loaded rSkill.""" + manifest = load_rskill_manifest(spec.weights_uri) + uri = manifest.weights_uri + if not isinstance(uri, str) or not uri.startswith("hf://") or "@" not in uri: + raise ROSConfigError( + "lingbot_va_a1 rSkill weights_uri must pin an hf:// repository revision" + ) + repo_id, revision = uri.removeprefix("hf://").rsplit("@", 1) + if not repo_id or not revision: + raise ROSConfigError("lingbot_va_a1 rSkill model identity is incomplete") + return repo_id, revision + + +class _LingBotVaA1Adapter: + """Proxy A1 observations to Runtime and return its typed action proposal.""" + + device = "external:a1-runtime" + + def __init__(self, spec: VLASpec, *, robot_description: Any) -> None: + self.spec = spec + names, lower, upper, max_step = _joint_contract(spec, robot_description) + latency_budget_ms = spec.extra.get("latency_budget_ms") + if isinstance(latency_budget_ms, bool) or not isinstance( + latency_budget_ms, + (int, float), + ): + raise ROSConfigError( + "lingbot_va_a1 requires its manifest latency budget in VLASpec.extra" + ) + self._timeout_s = float(latency_budget_ms) / 1000.0 + if not np.isfinite(self._timeout_s) or self._timeout_s <= 0: + raise ROSConfigError("lingbot_va_a1 latency budget must be positive") + self._client = RuntimeLocalClient( + _SOCKET_NAME, + label="Galaxea A1 OpenRAL policy gateway", + max_response_bytes=_MAX_RESPONSE_BYTES, + ) + self._instruction: str | None = None + try: + self._client.connect(timeout_s=5.0) + description = self._client.call( + {"protocol": _PROTOCOL_VERSION, "op": "describe"}, + timeout_s=5.0, + ) + self._validate_description( + description.get("metadata"), + joint_names=names, + expected_model=_model_identity(spec), + ) + self._client.call( + { + "protocol": _PROTOCOL_VERSION, + "op": "configure", + "contract": { + "joint_names": list(names), + "lower_limits": lower.tolist(), + "upper_limits": upper.tolist(), + "max_joint_step_rad": max_step, + }, + }, + timeout_s=5.0, + ) + except BaseException: + self._client.close() + raise + + def reset(self) -> None: + """Clear the local instruction identity; Runtime resets on the next step.""" + self._instruction = None + + def close(self) -> None: + """Close only this client; the Runtime-owned gateway remains alive.""" + self._client.close() + + def step( + self, + observation: dict[str, Any], + instruction: str, + ) -> NDArray[np.float32]: + """Return one absolute A1 joint/gripper action.""" + if not instruction.strip(): + raise ROSRuntimeError("lingbot_va_a1 instruction must be non-empty") + if self._instruction != instruction: + self._client.call( + { + "protocol": _PROTOCOL_VERSION, + "op": "reset", + "instruction": instruction, + }, + timeout_s=self._timeout_s, + ) + self._instruction = instruction + joints = np.asarray(observation.get("state"), dtype=np.float64) + if joints.shape != (6,) or not np.isfinite(joints).all(): + raise ROSRuntimeError( + f"lingbot_va_a1 requires six finite A1 joint positions, got {joints.shape}" + ) + images = observation.get("images") + if not isinstance(images, dict): + raise ROSRuntimeError("lingbot_va_a1 requires OpenRAL image observations") + front = _rgb_image(images.get("front"), name="front") + wrist = _rgb_image(images.get("wrist"), name="wrist") + response = self._client.call( + { + "protocol": _PROTOCOL_VERSION, + "op": "step", + "instruction": instruction, + "timeout_s": self._timeout_s, + "joints": encode_array(joints), + "front_rgb": encode_array(front), + "wrist_rgb": encode_array(wrist), + }, + timeout_s=self._timeout_s, + ) + action = decode_array( + response.get("action"), + shape=(7,), + dtype=np.dtype(np.float32), + label="Galaxea A1 OpenRAL policy action", + ) + if not np.isfinite(action).all() or not 0.0 <= float(action[6]) <= 1.0: + raise ROSRuntimeError( + "Galaxea A1 OpenRAL policy returned an invalid joint/gripper action" + ) + return action + + @staticmethod + def _validate_description( + value: Any, + *, + joint_names: tuple[str, ...], + expected_model: tuple[str, str], + ) -> None: + if not isinstance(value, dict): + raise ROSConfigError("Galaxea A1 OpenRAL policy metadata is missing") + if value.get("protocol") != _PROTOCOL_VERSION: + raise ROSConfigError("Galaxea A1 OpenRAL policy protocol mismatch") + if value.get("joint_names") != list(joint_names): + raise ROSConfigError( + "Galaxea A1 OpenRAL policy joint contract does not match the robot" + ) + if value.get("action_dim") != _ACTION_DIM or value.get("gripper_range") != [ + 0.0, + 1.0, + ]: + raise ROSConfigError("Galaxea A1 OpenRAL policy action contract mismatch") + actual_model = ( + value.get("model_repo_id"), + value.get("model_revision"), + ) + if actual_model != expected_model: + raise ROSConfigError( + "Galaxea A1 OpenRAL policy serves a different model: " + f"expected {expected_model[0]}@{expected_model[1]}, " + f"got {actual_model[0]}@{actual_model[1]}" + ) + + +def _rgb_image(value: Any, *, name: str) -> NDArray[np.uint8]: + image = np.asarray(value) + if image.ndim != _COLOR_NDIM or image.shape[2] != _COLOR_CHANNELS or image.dtype != np.uint8: + raise ROSRuntimeError( + f"lingbot_va_a1 image {name!r} must be HWC uint8 RGB, " + f"got shape={image.shape} dtype={image.dtype}" + ) + return image + + +@POLICIES.register("lingbot_va_a1") +def _build_lingbot_va_a1(env_cfg: Any) -> _LingBotVaA1Adapter: + return _LingBotVaA1Adapter( + env_cfg.vla, + robot_description=getattr(env_cfg, "robot_description", None), + ) diff --git a/python/sim/src/openral_sim/policy_deps.py b/python/sim/src/openral_sim/policy_deps.py index d71366b..a0f5a3c 100644 --- a/python/sim/src/openral_sim/policy_deps.py +++ b/python/sim/src/openral_sim/policy_deps.py @@ -96,6 +96,11 @@ "runs in tools/lingbot_vla2_sidecar.py's own auto-provisioned Python 3.12 " "+ torch-2.8 venv." ), + "lingbot_va_a1": ( + "Install the LingBot wire dependencies with `just sync --all-packages " + "--group lingbot`, then start the A1 Runtime camera bridge, " + "contract-checked LingBot server, and OpenRAL policy gateway." + ), "internvla_n1": ( "Install the rldx extras: `just sync --all-packages --group rldx` " "(adds pyzmq + msgpack for the InternVLA-N1 sidecar client). The " @@ -123,6 +128,7 @@ "gr00t": ("sim", "gr00t"), "diffuser_actor": ("rlbench",), "lingbot_vla2": ("lingbot",), + "lingbot_va_a1": ("lingbot",), "internvla_n1": ("rldx",), "mock": (), } @@ -158,6 +164,7 @@ # sidecar's own auto-provisioned py3.12 + torch-2.8 venv). See # openral_sim.policies.lingbot_vla2. "lingbot_vla2": ("zmq", "msgpack"), + "lingbot_va_a1": ("websockets", "msgpack"), # InternVLA-N1 shares the sidecar contract — the openral side only # needs the ZMQ + msgpack wire; the transformers-4.51 stack lives in # the sidecar's auto-provisioned py3.11 venv. diff --git a/python/sim/src/openral_sim/registry.py b/python/sim/src/openral_sim/registry.py index c38418f..5318e4b 100644 --- a/python/sim/src/openral_sim/registry.py +++ b/python/sim/src/openral_sim/registry.py @@ -24,7 +24,7 @@ def _build(env_cfg): from __future__ import annotations -from collections.abc import Callable +from collections.abc import Callable, Iterator from typing import TYPE_CHECKING, Generic, TypeVar from openral_core.exceptions import ROSConfigError @@ -129,6 +129,10 @@ def names(self) -> list[str]: def __contains__(self, name: object) -> bool: return isinstance(name, str) and name in self._items + def __iter__(self) -> Iterator[str]: + """Iterate over registered IDs, so ``sorted(REGISTRY)`` works in error messages.""" + return iter(self._items) + SCENES: _Registry[SimRollout] = _Registry("scene") POLICIES: _Registry[PolicyAdapter] = _Registry("policy") diff --git a/robots/galaxea_a1/robot.yaml b/robots/galaxea_a1/robot.yaml new file mode 100644 index 0000000..e7095fe --- /dev/null +++ b/robots/galaxea_a1/robot.yaml @@ -0,0 +1,182 @@ +# Galaxea A1 real-hardware manifest. +# +# Kinematic limits and link names are transcribed from the official A1 SDK +# A1_URDF_0607_0028.urdf. The SDK itself remains operator-provided and is not +# redistributed by OpenRAL. The HAL remains intentionally joint/gripper-space; +# the LingBot rSkill lowers its EEF output before the safety boundary. URDF +# asset publication and lowered collision geometry follow after the vendor +# confirms a redistributable asset source. +name: galaxea_a1 +embodiment_kind: manipulator +base_frame: base_link + +joints: + - name: arm_joint1 + joint_type: revolute + parent_link: base_link + child_link: arm_seg1 + axis_xyz: [0, 0, 1] + origin_xyz: [-0.0011147, 0, 0.0892] + origin_rpy: [0, 0, 3.1416] + position_limits: [-2.8798, 2.8798] + velocity_limit: 20.944 + effort_limit: 40.0 + actuator_kind: bldc + - name: arm_joint2 + joint_type: revolute + parent_link: arm_seg1 + child_link: arm_seg2 + axis_xyz: [0, 0, 1] + origin_xyz: [0, -0.00004, 0.0615] + origin_rpy: [1.5708, 0, 0] + position_limits: [0.0, 3.1415] + velocity_limit: 20.944 + effort_limit: 40.0 + actuator_kind: bldc + - name: arm_joint3 + joint_type: revolute + parent_link: arm_seg2 + child_link: arm_seg3 + axis_xyz: [0, 0, 1] + origin_xyz: [0.34928, 0.02, 0] + origin_rpy: [0, 0, 1.5708] + position_limits: [-3.3161, 0.0] + velocity_limit: 7.5398 + effort_limit: 27.0 + actuator_kind: bldc + - name: arm_joint4 + joint_type: revolute + parent_link: arm_seg3 + child_link: arm_seg4 + axis_xyz: [0, 0, 1] + origin_xyz: [0.07, -0.00395, -0.00004] + origin_rpy: [-1.5708, 1.5708, 0] + position_limits: [-2.8798, 2.8798] + velocity_limit: 25.133 + effort_limit: 7.0 + actuator_kind: bldc + - name: arm_joint5 + joint_type: revolute + parent_link: arm_seg4 + child_link: arm_seg5 + axis_xyz: [0, 0, 1] + origin_xyz: [0, 0, 0.2776] + origin_rpy: [-1.5708, 0, 3.1416] + position_limits: [-1.6581, 1.6581] + velocity_limit: 25.133 + effort_limit: 7.0 + actuator_kind: bldc + - name: arm_joint6 + joint_type: revolute + parent_link: arm_seg5 + child_link: arm_seg6 + axis_xyz: [0, 0, 1] + origin_xyz: [0, -0.1575, -0.00023266] + origin_rpy: [1.5708, 0, 0] + position_limits: [-2.8798, 2.8798] + velocity_limit: 25.133 + effort_limit: 7.0 + actuator_kind: bldc + +end_effectors: + - name: gripper + kind: parallel_gripper + n_dof: 1 + +sensors: + - name: front + modality: rgb + frame_id: a1_front_camera + rate_hz: 30.0 + encoding: rgb8 + intrinsics: + width: 480 + height: 480 + fx: 387.67059326171875 + fy: 386.86669921875 + cx: 220.1776123046875 + cy: 250.0211944580078 + distortion_model: plumb_bob + distortion_coeffs: [-0.05492031201720238, 0.06242402642965317, -0.0008095245575532317, 0.00015016942052170634, -0.020708303898572922] + vla_feature_key: observation.images.front + vendor: Intel + model: RealSense D455 + metadata: + output_width: 480 + output_height: 480 + crop: "x=103,y=0,width=480,height=480" + - name: wrist + modality: rgb + frame_id: a1_wrist_camera + parent_frame: arm_seg6 + rate_hz: 30.0 + encoding: rgb8 + intrinsics: + width: 640 + height: 480 + fx: 392.2943115234375 + fy: 391.7977294921875 + cx: 320.1477355957031 + cy: 241.63327026367188 + distortion_model: plumb_bob + distortion_coeffs: [-0.05426114425063133, 0.05990355461835861, 0.0004118939395993948, 0.000653077382594347, -0.019962556660175323] + vla_feature_key: observation.images.wrist + vendor: Intel + model: RealSense D405 + metadata: + output_width: 640 + output_height: 480 + +capabilities: + can_lift_kg: 5.0 + has_vision: true + supported_control_modes: [joint_position, gripper_position] + embodiment_tags: [galaxea_a1] + +safety: + max_ee_speed_m_s: 0.25 + max_joint_speed_factor: 0.05 + max_force_n: 40.0 + max_torque_nm: 40.0 + deadman_required: true + +sdk_kind: closed_with_api +hal: + sim: null + real: openral_hal.galaxea_a1:GalaxeaA1HAL + parameters: + defaults: + host: 127.0.0.1 + port: 46011 + state_timeout_s: 0.5 + status_timeout_s: 1.0 + # Feedback may sit just beyond a nominal URDF endpoint because of encoder + # zero/quantization. Commands remain strictly inside the URDF limits. + feedback_limit_tolerance_rad: 0.01 + initial_alignment_tolerance_rad: 0.05 + # The official tracker starts from a compiled task.info pose. Its output + # remains staged until it converges to the measured initial hold. + tracker_alignment_timeout_s: 5.0 + # This remains far below the A1 Runtime's independently validated 1.70 + # rad IK-solution bound, while allowing the official tracker to cross its + # observed 0.03..0.038 rad steady-state residual. + max_target_step_rad: 0.08 + command_lease_s: 0.5 + # The vendor documents bit 6 as ECU->ACU timeout. It is observed while + # idle and is the only accepted arm bit. The gripper additionally ignores + # bit 3 (position jump) for compatibility with the physical G2 gripper. + idle_timeout_error_mask: 64 + gripper_ignored_error_mask: 8 + gripper_stroke_min_mm: 0.0 + gripper_stroke_max_mm: 104.0 + +onboard_compute: + driver_repo: https://github.com/userguide-galaxea/A1_SDK + driver_runtime: ROS 1 Noetic sidecar + command_topic: /arm_joint_target_position + joint_state_topic: /joint_states_host + motor_status_topic: /arm_status_host + gripper_command_topic: /gripper_position_control_host + gripper_feedback_topic: /gripper_stroke_host + +schema_version: "0.1" diff --git a/rskills/lingbot-va-galaxea-a1-fruit-placement/README.md b/rskills/lingbot-va-galaxea-a1-fruit-placement/README.md new file mode 100644 index 0000000..c1e8c9c --- /dev/null +++ b/rskills/lingbot-va-galaxea-a1-fruit-placement/README.md @@ -0,0 +1,155 @@ +--- +language: +- en +license: apache-2.0 +pipeline_tag: robotics +tags: +- OpenRAL +- rskill +- lingbot_va_a1 +- vision-language-action +- galaxea_a1 +base_model: +- robbyant/lingbot-va-base +base_model_relation: finetune +datasets: +- pengyue-polaron/nyush-galaxea-a1-fruit-placement-eef-v21 +inference: false +--- + +# rskill-lingbot_va_a1-galaxea_a1-fruit_placement-bf16 + +> **OpenRAL rSkill** — a LingBot-VA fruit-placement policy for the Galaxea A1, +> deployed through OpenRAL's observation, typed-action, safety-kernel, and HAL +> contracts. + +This package points to the public checkpoint at +[`pengyue-polaron/lingbot-va-galaxea-a1-fruit-placement-eef`](https://huggingface.co/pengyue-polaron/lingbot-va-galaxea-a1-fruit-placement-eef) +and does not copy model weights into the OpenRAL repository. + +## Preview + +![Galaxea A1 fruit-placement scene](https://huggingface.co/pengyue-polaron/lingbot-va-galaxea-a1-fruit-placement-eef/resolve/90e017bdbc6afac2e441b4634c9192776bbcb8b7/assets/fruit_placement_agent_view_labeled.png) + +## What this skill does + +The policy picks fruit, including a mango, from a tabletop and places it into a +bowl or plate. It consumes synchronized front and wrist RGB observations and +predicts episode-relative end-effector pose plus a continuous normalized gripper +command. + +| Field | Value | +| --- | --- | +| Actions | `pick`, `place` | +| Objects | `fruit`, `mango`, `bowl`, `plate` | +| Scene | `tabletop` | +| Embodiment | `galaxea_a1` | + +## Upstream model and training + +The checkpoint is a full-parameter fine-tune of +[`robbyant/lingbot-va-base`](https://huggingface.co/robbyant/lingbot-va-base). +It jointly predicts video latents and robot-action channels. Training used 130 +episodes and 44,824 frames at 30 FPS from the revision-pinned +[`nyush-galaxea-a1-fruit-placement-eef-v21`](https://huggingface.co/datasets/pengyue-polaron/nyush-galaxea-a1-fruit-placement-eef-v21) +dataset. The run used 1,000 optimizer steps, two NVIDIA H100 80 GB GPUs, +full-parameter FSDP, bfloat16, and an effective global batch size of 16. + +The model emits 16 EEF/gripper steps per chunk. Quantile normalization and the +action-channel map `[0, 1, 2, 3, 4, 5, 6, 28]` are applied in the external +LingBot server from the checkpoint's `configs/va_a1_cfg.py`; this is not a +LeRobot `PolicyProcessorPipeline`. + +The A1 Runtime policy gateway validates each physical EEF target, solves IK, +and emits six absolute joint targets plus one normalized gripper target. If an +IK solution is farther than the rSkill's feedback-relative joint-step bound, +the gateway advances toward that same solution on subsequent 30 Hz ticks and +does not consume the next model action until the full solved target can be +dispatched. It then writes the dispatched target's FK result into the LingBot +KV cache. OpenRAL's thin adapter validates the gateway model and robot contract, +then routes the typed proposal through the normal candidate-action, C++ safety +kernel, safe-action, and Galaxea A1 HAL path. Runtime's IK is constructed with +the active OpenRAL robot manifest's ordered joint limits, so Runtime calibration +margins cannot widen the official command envelope. + +## Sensors and observation contract + +| Direction | Key | Shape | Notes | +| --- | --- | --- | --- | +| in | `observation.images.front` | `(480, 480, 3)` RGB uint8 | Cropped D455 front view from the A1 Runtime Camera Bridge | +| in | `observation.images.wrist` | `(480, 640, 3)` RGB uint8 | D405 wrist view from the same paired Camera Bridge | +| in | `observation.state` | `(6,)` float32 | Six A1 arm joints in radians | +| out | action | `(7,)` float32 | Six absolute joint targets in radians and one normalized gripper target | + +The A1 Runtime remains the sole camera-device owner. OpenRAL connects to its +versioned paired Camera Bridge and policy gateway over private per-user Unix +sockets; it neither imports the Runtime checkout nor opens either RealSense +device. + +## Supported robots + +| Robot | Embodiment tag | Status | Notes | +| --- | --- | --- | --- | +| Galaxea A1, original arm | `galaxea_a1` | Hardware-in-the-loop integration | Joint/gripper round trips and one visually verified model-driven lemon pick-and-place have passed through OpenRAL; automatic task adjudication remains pending | + +This rSkill is specific to the six-joint Galaxea A1 contract in +`robots/galaxea_a1/robot.yaml`. It is not a generic Cartesian HAL and does not +enable the vendor AnyGrasp/AnyEffector path. + +## Manifest summary + +| Field | Value | +| --- | --- | +| `name` | `OpenRAL/rskill-lingbot_va_a1-galaxea_a1-fruit_placement-bf16` | +| `version` | `0.1.0` | +| `license` | `apache-2.0` | +| `model_family` | `lingbot_va_a1` | +| `embodiment_tags` | `galaxea_a1` | +| `runtime` / precision | `pytorch` / `bf16` | +| `weights_uri` | revision-pinned public LingBot-VA A1 checkpoint | +| `state_contract.dim` / `action_contract.dim` | `6` / `7` | +| `chunk_size` / `n_action_steps` | `16` / `8` | +| `latency_budget.per_chunk_ms` | `6000` | +| `commercial_use_allowed` | `true` | + +Full schema: [`openral_core.schemas.RSkillManifest`](../../python/core/src/openral_core/schemas.py). + +## Quick start + +The software-only compatibility check does not initialize ROS or hardware: + +```bash +uv run --group lingbot openral rskill check \ + rskills/lingbot-va-galaxea-a1-fruit-placement/rskill.yaml \ + --robot robots/galaxea_a1/robot.yaml +``` + +For real deployment, follow the owner-separated startup sequence in +[`docs/methods/01-hal.md`](../../docs/methods/01-hal.md): start the A1 Runtime +camera owner, LingBot server, and policy gateway; start the isolated OpenRAL +ROS1 sidecar; then run the OpenRAL deployment scene. Do not start the A1 +Runtime joint execution bridge at the same time. + +## Evaluation + +No formal automatic task-success result is shipped yet. Validation has +exercised the real paired cameras, real model server, manifest-to-policy +construction, EEF validation and IK, joint-step subdivision, and OpenRAL typed +joint/gripper dispatch. Separate real-hardware joint/gripper round trips and a +visually verified lemon pick-and-place have traversed candidate action, the C++ +safety kernel, safe action, HAL, ROS 1 relay, and the official driver. The +non-terminating VLA continued after the visible placement and was stopped by +the unchanged joint-solution jump guard; a success detector or bounded episode +termination is still needed for a formal task-success result. + +## License + +This rSkill package and the revision-pinned model weights are Apache-2.0. The +model repository contains the authoritative `LICENSE.txt`; the OpenRAL package +references the weights and does not redistribute them. + +## See also + +- [`robots/galaxea_a1/robot.yaml`](../../robots/galaxea_a1/robot.yaml) +- [`scenes/deploy/galaxea_a1_bench.yaml`](../../scenes/deploy/galaxea_a1_bench.yaml) +- [`docs/methods/01-hal.md`](../../docs/methods/01-hal.md) diff --git a/rskills/lingbot-va-galaxea-a1-fruit-placement/SKILL.md b/rskills/lingbot-va-galaxea-a1-fruit-placement/SKILL.md new file mode 100644 index 0000000..a70a9b0 --- /dev/null +++ b/rskills/lingbot-va-galaxea-a1-fruit-placement/SKILL.md @@ -0,0 +1,73 @@ +--- +name: lingbot-va-galaxea-a1-fruit-placement +description: >- + S1 Vision-Language-Action policy. Capabilities: pick, place on fruit, mango, bowl, plate. LingBot-VA fruit-placement policy for the Galaxea A1. It consumes the synchronized front and wrist RGB views, predicts episode-relative EEF pose and continuous gripper chunks, and uses the tracked A1 Runtime IK contract before OpenRAL validates and executes the resulting joint/gripper actions. Discovery view of an OpenRAL rSkill — NOT directly runnable by an agent harness; it runs via rSkill.from_pretrained + the robot HAL. +metadata: + openral_rskill: true # generated discovery view of an rSkill + schema_version: 0.1 + rskill_id: OpenRAL/rskill-lingbot_va_a1-galaxea_a1-fruit_placement-bf16 + manifest: ./rskill.yaml + role: s1 + kind: vla + model_family: lingbot_va_a1 + embodiment_tags: [galaxea_a1] + actions: [pick, place] + objects: [fruit, mango, bowl, plate] + scenes: [tabletop] + sensors_required: ['rgb:observation.images.front', 'rgb:observation.images.wrist'] + state_dim: 6 + action_dim: 7 + runtime: pytorch + quantization: bf16/pytorch + chunk_size: 16 + n_action_steps: 8 + latency_budget: {per_chunk_ms: 6000.0, max_execution_s: 420.0} + license_code: Apache-2.0 + license_weights: apache-2.0 + weights_uri: hf://pengyue-polaron/lingbot-va-galaxea-a1-fruit-placement-eef@90e017bdbc6afac2e441b4634c9192776bbcb8b7 + source_repo: hf://robbyant/lingbot-va-base +--- + +# lingbot-va-galaxea-a1-fruit-placement — rSkill discovery view + +> **Generated view, not a hand-written skill.** This `SKILL.md` is a discovery-only +> mirror of [`rskill.yaml`](./rskill.yaml), produced by `tools/generate_rskill_skillmd.py`. +> It lets tools that read the standard agent-skill format find and reason about this +> OpenRAL rSkill. The `rskill.yaml` manifest is the single source of truth +> (CLAUDE.md §1.3). Do not edit by hand — edit the manifest and regenerate. + +## What it is + +An OpenRAL **Vision-Language-Action policy** (`role: s1`, `kind: vla`). LingBot-VA fruit-placement policy for the Galaxea A1. It consumes the synchronized front and wrist RGB views, predicts episode-relative EEF pose and continuous gripper chunks, and uses the tracked A1 Runtime IK contract before OpenRAL validates and executes the resulting joint/gripper actions. + +## Capabilities + +- **Verbs:** pick · place +- **Objects:** fruit · mango · bowl · plate +- **Scenes:** tabletop +- **Embodiments:** galaxea_a1 + +## Why this is discovery-only + +An agent skill is natural-language instructions loaded into an LLM's context. An rSkill +is an executable artifact: it carries a typed capability/embodiment contract, model weights, +a runtime, and a license/provenance gate — none of which fit in freeform markdown. So an +agent can use this view to *select* the right skill, but cannot *execute* it by loading +this file. Execution always goes through the OpenRAL loader and the robot HAL. + +## License + +- **Code:** Apache-2.0. +- **Weights:** `apache-2.0` — permissive / commercial-use OK + +## How to actually run it (not via an agent harness) + +```python +from openral_rskill import rSkill + +skill = rSkill.from_pretrained("OpenRAL/rskill-lingbot_va_a1-galaxea_a1-fruit_placement-bf16") +# the loader validates embodiment / sensors / runtime / quantization against the target +# RobotDescription and enforces the weight-license gate before any weights load. +``` + +See [`rskill.yaml`](./rskill.yaml) for the authoritative, validated manifest. diff --git a/rskills/lingbot-va-galaxea-a1-fruit-placement/rskill.yaml b/rskills/lingbot-va-galaxea-a1-fruit-placement/rskill.yaml new file mode 100644 index 0000000..86ffae2 --- /dev/null +++ b/rskills/lingbot-va-galaxea-a1-fruit-placement/rskill.yaml @@ -0,0 +1,106 @@ +# A1 Runtime LingBot-VA checkpoint deployed through OpenRAL. +# +# The model predicts episode-relative EEF pose + normalized gripper chunks. +# The Runtime-owned policy gateway validates those targets and lowers each step +# to six absolute A1 joint positions plus one gripper value. +# Both typed actions then traverse OpenRAL's safety kernel and A1 HAL. +schema_version: "0.1" +name: "OpenRAL/rskill-lingbot_va_a1-galaxea_a1-fruit_placement-bf16" +version: "0.1.0" +license: "apache-2.0" +role: "s1" +kind: "vla" +model_family: "lingbot_va_a1" + +embodiment_tags: + - "galaxea_a1" + +sensors_required: + - modality: "rgb" + vla_feature_key: "observation.images.front" + min_width: 480 + min_height: 480 + - modality: "rgb" + vla_feature_key: "observation.images.wrist" + min_width: 640 + min_height: 480 + +actuators_required: + - kind: "joint_position" + control_mode_semantics: + mode: "absolute" + joint_order: + - arm_joint1 + - arm_joint2 + - arm_joint3 + - arm_joint4 + - arm_joint5 + - arm_joint6 + - kind: "gripper_position" + control_mode_semantics: + mode: "absolute" + gripper_convention: "normalized_open_unit" + +runtime: "pytorch" +quantization: + dtype: "bf16" + backend: "pytorch" +weights_uri: "hf://pengyue-polaron/lingbot-va-galaxea-a1-fruit-placement-eef@90e017bdbc6afac2e441b4634c9192776bbcb8b7" + +# No `processors` block: this checkpoint is not a lerobot +# PolicyProcessorPipeline. Quantile normalization and the action-channel map +# ship in configs/va_a1_cfg.py and are applied by the external LingBot server +# before OpenRAL receives the physical EEF target. +state_contract: + dim: 6 + +chunk_size: 16 +n_action_steps: 8 +latency_budget: + # Measured through the full OpenRAL camera + websocket + EEF/IK adapter: + # ~4.05 s for the first model call on this A1 host; replay ticks are cheap. + per_chunk_ms: 6000.0 + max_execution_s: 420.0 + +dataset_uri: "hf://pengyue-polaron/nyush-galaxea-a1-fruit-placement-eef-v21@1bc2c4035e7dc638f7dd9fa5ec7987bec66d0933" +source_repo: "hf://robbyant/lingbot-va-base" +description: > + LingBot-VA fruit-placement policy for the Galaxea A1. It consumes the + synchronized front and wrist RGB views, predicts episode-relative EEF pose + and continuous gripper chunks, and uses the tracked A1 Runtime IK contract + before OpenRAL validates and executes the resulting joint/gripper actions. + +# The LingBot EEF solution may be farther from current feedback than the A1 +# joint tracker accepts in one update. This policy-owned per-tick lowering +# bound remains below both the HAL's 0.08 rad live target-step ceiling and its +# 0.05 rad initial-alignment ceiling. +policy_extras: + max_joint_substep_rad: 0.045 + +actions: + - "pick" + - "place" +objects: + - "fruit" + - "mango" + - "bowl" + - "plate" +scenes: + - "tabletop" + +action_contract: + dim: 7 + joint_units: "radians" + slots: + - range: [0, 5] + control_mode: "joint_position" + joint_names: + - arm_joint1 + - arm_joint2 + - arm_joint3 + - arm_joint4 + - arm_joint5 + - arm_joint6 + - range: [6, 6] + control_mode: "gripper_position" + ee: "gripper" diff --git a/scenes/deploy/galaxea_a1_bench.yaml b/scenes/deploy/galaxea_a1_bench.yaml new file mode 100644 index 0000000..c1a67b8 --- /dev/null +++ b/scenes/deploy/galaxea_a1_bench.yaml @@ -0,0 +1,68 @@ +# Galaxea A1 hardware deployment. Start the ROS 1 sidecar, A1 Camera Bridge, +# LingBot policy server, and A1 Runtime OpenRAL policy gateway first, then run: +# openral deploy run --config scenes/deploy/galaxea_a1_bench.yaml +scene: + id: galaxea_a1_bench +robot_id: galaxea_a1 + +sensors: + # These readers consume the A1 Runtime's public paired-frame bridge. They do + # not open either RealSense device; `just camera-web` remains the sole owner. + - name: front + modality: rgb + frame_id: a1_front_camera + rate_hz: 30.0 + encoding: rgb8 + intrinsics: + width: 480 + height: 480 + fx: 387.67059326171875 + fy: 386.86669921875 + cx: 220.1776123046875 + cy: 250.0211944580078 + distortion_model: plumb_bob + distortion_coeffs: [-0.05492031201720238, 0.06242402642965317, -0.0008095245575532317, 0.00015016942052170634, -0.020708303898572922] + vla_feature_key: observation.images.front + deploy_binding: + backend: galaxea_a1_camera_bridge + backend_params: + camera: front + max_age_ms: 500 + - name: wrist + modality: rgb + frame_id: a1_wrist_camera + parent_frame: arm_seg6 + rate_hz: 30.0 + encoding: rgb8 + intrinsics: + width: 640 + height: 480 + fx: 392.2943115234375 + fy: 391.7977294921875 + cx: 320.1477355957031 + cy: 241.63327026367188 + distortion_model: plumb_bob + distortion_coeffs: [-0.05426114425063133, 0.05990355461835861, 0.0004118939395993948, 0.000653077382594347, -0.019962556660175323] + vla_feature_key: observation.images.wrist + deploy_binding: + backend: galaxea_a1_camera_bridge + backend_params: + camera: wrist + max_age_ms: 500 +runtime: + enable_reasoner: false + enable_slam: false + enable_nav2: false + enable_octomap: false + enable_octomap_kernel_check: true + enable_object_detector: false + object_detector_onnx: null + object_detector_manifest: null + object_detector_query: null + object_detector_locators: null + enable_reward_monitor: false + reward_monitor_manifest: null + reward_monitor_task: null + enable_critic: false + spatial_memory_ingest: false + approach_skill_id: null diff --git a/tests/hil/test_galaxea_a1.py b/tests/hil/test_galaxea_a1.py new file mode 100644 index 0000000..afffdf3 --- /dev/null +++ b/tests/hil/test_galaxea_a1.py @@ -0,0 +1,268 @@ +"""Hardware-in-loop bring-up test for the Galaxea A1 ROS 1 sidecar. + +The operator starts ``tools/run_galaxea_a1_sidecar.sh`` first. This test then +uses the real OpenRAL HAL and the real loopback protocol; it never imports or +bundles the vendor SDK. + +Environment: + GALAXEA_A1_HIL: Must be ``"1"`` to connect. This explicit gate prevents a + developer laptop from claiming a nearby sidecar accidentally. + GALAXEA_A1_HOST / GALAXEA_A1_PORT: Sidecar endpoint (defaults to + ``127.0.0.1:46011``). + GALAXEA_A1_ALLOW_HOLD: When ``"1"``, repeatedly sends the measured current + pose for two seconds and verifies that no joint moves by 1 degree or + more. This duration also proves that the sidecar command lease is + renewed after the relay becomes active. + Feedback within the tracked endpoint tolerance is projected to the + exact command limit; larger projections fail before publication. The + default is observation-only and sends no action. + GALAXEA_A1_ALLOW_NUDGE: When ``"1"``, implies the hold gate, then moves + ``arm_joint1`` by +0.01 rad and returns to the measured start. Both + legs must settle within 0.008 rad while every joint remains inside a + bounded excursion from the initial feedback. + GALAXEA_A1_ALLOW_GRIPPER: When ``"1"``, implies the hold gate, moves the + G2 gripper by the vendor example's 10 mm step away from the nearest + endpoint, verifies feedback within the measured 2.5 mm steady-state + tolerance, and returns to the measured opening. + +The single test owns one sidecar session. It validates feedback, cached health, +and downstream e-stop in one bounded sequence because the sidecar deliberately +stops its ROS 1 driver/tracker stack when its sole client disconnects. +""" + +from __future__ import annotations + +import math +import os +import time + +import pytest +from openral_core import Action, ControlMode +from openral_core.exceptions import ROSEStopRequested +from openral_hal.galaxea_a1 import GALAXEA_A1_DESCRIPTION, GalaxeaA1HAL + +_ENABLED = os.environ.get("GALAXEA_A1_HIL", "0") == "1" +_ALLOW_HOLD = os.environ.get("GALAXEA_A1_ALLOW_HOLD", "0") == "1" +_ALLOW_NUDGE = os.environ.get("GALAXEA_A1_ALLOW_NUDGE", "0") == "1" +_ALLOW_GRIPPER = os.environ.get("GALAXEA_A1_ALLOW_GRIPPER", "0") == "1" +_HOST = os.environ.get("GALAXEA_A1_HOST", "127.0.0.1") +_PORT = int(os.environ.get("GALAXEA_A1_PORT", "46011")) +_NUDGE_RAD = 0.01 +_SETTLE_TOLERANCE_RAD = 0.008 +_MOTION_TIMEOUT_S = 3.0 +_MAX_EXCURSION_RAD = 0.025 +_GRIPPER_NUDGE_MM = 10.0 +# This G2's measured mid-stroke steady-state error was 2.08-2.21 mm across +# repeated 10 mm commands. Keep modest headroom while still requiring at least +# 7.5 mm of the commanded excursion and the same bound on the return leg. +_GRIPPER_TOLERANCE_MM = 2.5 +_HOLD_DURATION_S = 2.0 + +pytestmark = pytest.mark.skipif( + not _ENABLED, + reason="GALAXEA_A1_HIL=1 is required for the live A1 sidecar test.", +) + + +def test_galaxea_a1_observe_optional_hold_and_estop() -> None: + """Validate one complete, bounded hardware session and stop it downstream.""" + hal = GalaxeaA1HAL(host=_HOST, port=_PORT, connect_timeout_s=20.0) + connected = False + try: + hal.connect() + connected = True + + start = time.monotonic() + samples = [hal.read_state()] + for _ in range(2): + time.sleep(0.05) + samples.append(hal.read_state()) + elapsed = time.monotonic() - start + + expected_names = [joint.name for joint in GALAXEA_A1_DESCRIPTION.joints] + defaults = GALAXEA_A1_DESCRIPTION.hal.parameters.defaults + feedback_tolerance = float(defaults["feedback_limit_tolerance_rad"]) + assert elapsed < 1.0, f"three cached A1 reads took {elapsed:.3f} s" + for state in samples: + assert state.name == expected_names + assert len(state.position) == 6 + assert all(math.isfinite(value) for value in state.position) + for value, joint in zip(state.position, GALAXEA_A1_DESCRIPTION.joints, strict=True): + lower, upper = joint.position_limits + assert lower - feedback_tolerance <= value <= upper + feedback_tolerance + + report = hal.health() + assert report.message == "A1 feedback and motor status healthy" + assert len(report.fields["motor_status_codes"].split(",")) >= 7 + + if _ALLOW_HOLD or _ALLOW_NUDGE or _ALLOW_GRIPPER: + before = samples[-1] + hold_target: list[float] = [] + for value, joint in zip(before.position, GALAXEA_A1_DESCRIPTION.joints, strict=True): + lower, upper = joint.position_limits + projected = min(upper, max(lower, value)) + assert abs(projected - value) <= feedback_tolerance + hold_target.append(projected) + relay_deadline = time.monotonic() + float(defaults["tracker_alignment_timeout_s"]) + while time.monotonic() < relay_deadline: + hal.send_action( + Action( + control_mode=ControlMode.JOINT_POSITION, + horizon=1, + joint_targets=[hold_target], + stamp_ns=time.time_ns(), + ) + ) + time.sleep(0.05) + if hal.health().fields["command_relay_state"] == "ACTIVE": + break + assert hal.health().fields["command_relay_state"] == "ACTIVE", ( + "A1 staged command relay did not become ACTIVE" + ) + hold_deadline = time.monotonic() + _HOLD_DURATION_S + while time.monotonic() < hold_deadline: + hal.send_action( + Action( + control_mode=ControlMode.JOINT_POSITION, + horizon=1, + joint_targets=[hold_target], + stamp_ns=time.time_ns(), + ) + ) + time.sleep(0.05) + current = hal.read_state() + for previous, actual in zip(before.position, current.position, strict=True): + assert abs(actual - previous) < 0.0175, ( + f"A1 joint moved {abs(actual - previous):.4f} rad during hold" + ) + + if _ALLOW_NUDGE: + nudge_target = list(hold_target) + joint1_upper = GALAXEA_A1_DESCRIPTION.joints[0].position_limits[1] + nudge_target[0] += _NUDGE_RAD + assert nudge_target[0] <= joint1_upper + + nudge_samples = [] + last_report = hal.health() + deadline = time.monotonic() + _MOTION_TIMEOUT_S + while time.monotonic() < deadline: + hal.send_action( + Action( + control_mode=ControlMode.JOINT_POSITION, + horizon=1, + joint_targets=[nudge_target], + stamp_ns=time.time_ns(), + ) + ) + time.sleep(0.05) + state = hal.read_state() + nudge_samples.append(state) + last_report = hal.health() + if abs(state.position[0] - nudge_target[0]) <= _SETTLE_TOLERANCE_RAD: + break + assert nudge_samples + assert abs(nudge_samples[-1].position[0] - nudge_target[0]) <= ( + _SETTLE_TOLERANCE_RAD + ), ( + f"joint1 target={nudge_target[0]:.6f}, " + f"feedback={nudge_samples[-1].position[0]:.6f}, " + f"staged={last_report.fields['staged_position']}, " + f"forwarded={last_report.fields['forwarded_position']}" + ) + for state in nudge_samples: + for index, (initial, current) in enumerate( + zip(before.position, state.position, strict=True) + ): + assert abs(current - initial) < _MAX_EXCURSION_RAD, ( + f"A1 joint {index + 1} excursion " + f"{abs(current - initial):.4f} rad exceeded " + f"{_MAX_EXCURSION_RAD:.4f} rad" + ) + + return_samples = [] + deadline = time.monotonic() + _MOTION_TIMEOUT_S + while time.monotonic() < deadline: + hal.send_action( + Action( + control_mode=ControlMode.JOINT_POSITION, + horizon=1, + joint_targets=[hold_target], + stamp_ns=time.time_ns(), + ) + ) + time.sleep(0.05) + state = hal.read_state() + return_samples.append(state) + if ( + max( + abs(current - initial) + for current, initial in zip( + state.position, before.position, strict=True + ) + ) + <= _SETTLE_TOLERANCE_RAD + ): + break + assert return_samples + final_error = max( + abs(current - initial) + for current, initial in zip( + return_samples[-1].position, before.position, strict=True + ) + ) + assert final_error <= _SETTLE_TOLERANCE_RAD + + if _ALLOW_GRIPPER: + initial_gripper = float(hal.health().fields["gripper_position_normalized"]) + gripper_stroke = float(defaults["gripper_stroke_max_mm"]) - float( + defaults["gripper_stroke_min_mm"] + ) + gripper_nudge = _GRIPPER_NUDGE_MM / gripper_stroke + gripper_tolerance = _GRIPPER_TOLERANCE_MM / gripper_stroke + direction = 1.0 if initial_gripper <= 0.5 else -1.0 + gripper_target = initial_gripper + direction * gripper_nudge + assert 0.0 <= gripper_target <= 1.0 + + gripper_feedback = initial_gripper + returned_gripper = initial_gripper + try: + deadline = time.monotonic() + _MOTION_TIMEOUT_S + while time.monotonic() < deadline: + hal.send_action( + Action( + control_mode=ControlMode.GRIPPER_POSITION, + gripper=[gripper_target], + stamp_ns=time.time_ns(), + ) + ) + time.sleep(0.05) + gripper_feedback = float(hal.health().fields["gripper_position_normalized"]) + if abs(gripper_feedback - gripper_target) <= gripper_tolerance: + break + finally: + deadline = time.monotonic() + _MOTION_TIMEOUT_S + while time.monotonic() < deadline: + hal.send_action( + Action( + control_mode=ControlMode.GRIPPER_POSITION, + gripper=[initial_gripper], + stamp_ns=time.time_ns(), + ) + ) + time.sleep(0.05) + returned_gripper = float(hal.health().fields["gripper_position_normalized"]) + if abs(returned_gripper - initial_gripper) <= gripper_tolerance: + break + assert abs(returned_gripper - initial_gripper) <= gripper_tolerance, ( + f"G2 return target={initial_gripper:.6f}, feedback={returned_gripper:.6f}" + ) + assert abs(gripper_feedback - gripper_target) <= gripper_tolerance, ( + f"G2 target={gripper_target:.6f}, feedback={gripper_feedback:.6f}" + ) + + with pytest.raises(ROSEStopRequested): + hal.estop() + connected = False + finally: + if connected: + hal.disconnect() diff --git a/tests/hil/test_galaxea_a1_deploy.py b/tests/hil/test_galaxea_a1_deploy.py new file mode 100644 index 0000000..709242d --- /dev/null +++ b/tests/hil/test_galaxea_a1_deploy.py @@ -0,0 +1,397 @@ +"""Full-graph HIL gate for the Galaxea A1 deployment path. + +Run this test *inside* an already-running ``openral deploy run`` container. +The ROS 1 sidecar must be running separately. Unlike the HAL-only A1 HIL +fixture, this gate proves the standard OpenRAL control path: + +``candidate_action -> C++ safety kernel -> safe_action -> HAL -> ROS 1 relay``. + +Environment: + GALAXEA_A1_DEPLOY_HIL: Must be ``"1"`` to connect to the live ROS 2 graph. + GALAXEA_A1_ALLOW_HOLD: Must also be ``"1"``. The only candidate action is + the freshly measured current joint pose. + GALAXEA_A1_ALLOW_NUDGE: When ``"1"``, moves ``arm_joint1`` by +0.01 rad + through the same candidate-action path, bounds every joint's excursion, + and returns to the measured start before e-stop. + +The test is bounded and always publishes ``/openral/estop`` during teardown. +It requires the relay to begin LOCKED, the kernel and HAL diagnostics to be +healthy, the safe action to equal the candidate exactly, and every joint to +remain within one degree of its measured start. +""" + +from __future__ import annotations + +import importlib.util +import math +import os +import time +from collections.abc import Callable +from dataclasses import dataclass, field +from typing import TYPE_CHECKING + +import pytest +from openral_hal.galaxea_a1 import GALAXEA_A1_DESCRIPTION + +if TYPE_CHECKING: + from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus + from openral_msgs.msg import ActionChunk, FailureTrigger + from rclpy.node import Node + from sensor_msgs.msg import JointState + +_ENABLED = os.environ.get("GALAXEA_A1_DEPLOY_HIL", "0") == "1" +_ALLOW_HOLD = os.environ.get("GALAXEA_A1_ALLOW_HOLD", "0") == "1" +_ALLOW_NUDGE = os.environ.get("GALAXEA_A1_ALLOW_NUDGE", "0") == "1" +_ROS2_AVAILABLE = importlib.util.find_spec("rclpy") is not None +_EXPECTED_NAMES = tuple(joint.name for joint in GALAXEA_A1_DESCRIPTION.joints) +_FEEDBACK_TOLERANCE_RAD = float( + GALAXEA_A1_DESCRIPTION.hal.parameters.defaults["feedback_limit_tolerance_rad"] +) +_MAX_HOLD_DRIFT_RAD = 0.0175 +_GRAPH_TIMEOUT_S = 10.0 +_HOLD_TIMEOUT_S = 10.0 +_ESTOP_TIMEOUT_S = 5.0 +_COMMAND_PERIOD_S = 0.05 +_NUDGE_RAD = 0.01 +_SETTLE_TOLERANCE_RAD = 0.008 +_MOTION_TIMEOUT_S = 3.0 +_MAX_EXCURSION_RAD = 0.025 + +pytestmark = [ + pytest.mark.skipif( + not _ENABLED, + reason="GALAXEA_A1_DEPLOY_HIL=1 is required for the live deploy-graph test.", + ), + pytest.mark.skipif( + not _ALLOW_HOLD, + reason="GALAXEA_A1_ALLOW_HOLD=1 is required for the current-pose hold.", + ), + pytest.mark.skipif( + not _ROS2_AVAILABLE, + reason="rclpy is unavailable; run this test inside the OpenRAL deploy image.", + ), +] + + +@dataclass +class _Observed: + """Latest real graph evidence collected by one single-threaded ROS node.""" + + joint_state: JointState | None = None + diagnostics: dict[str, DiagnosticStatus] = field(default_factory=dict) + diagnostic_history: dict[str, list[DiagnosticStatus]] = field(default_factory=dict) + safe_actions: list[ActionChunk] = field(default_factory=list) + failures: list[FailureTrigger] = field(default_factory=list) + + def on_joint_state(self, message: JointState) -> None: + self.joint_state = message + + def on_diagnostics(self, message: DiagnosticArray) -> None: + for status in message.status: + self.diagnostics[status.name] = status + self.diagnostic_history.setdefault(status.name, []).append(status) + + def on_safe_action(self, message: ActionChunk) -> None: + self.safe_actions.append(message) + + def on_failure(self, message: FailureTrigger) -> None: + self.failures.append(message) + + +def _diagnostic_values(status: DiagnosticStatus) -> dict[str, str]: + return {item.key: item.value for item in status.values} + + +def _spin_until(node: Node, predicate: Callable[[], bool], timeout_s: float) -> bool: + import rclpy + + deadline = time.monotonic() + timeout_s + while time.monotonic() < deadline: + rclpy.spin_once(node, timeout_sec=0.05) + if predicate(): + return True + return False + + +def _ordered_positions(message: JointState) -> tuple[float, ...]: + assert len(message.name) == len(set(message.name)), "duplicate joint name in /joint_states" + by_name = dict(zip(message.name, message.position, strict=True)) + assert set(_EXPECTED_NAMES).issubset(by_name), ( + f"/joint_states missing A1 joints: {sorted(set(_EXPECTED_NAMES) - set(by_name))}" + ) + values = tuple(float(by_name[name]) for name in _EXPECTED_NAMES) + assert all(math.isfinite(value) for value in values) + return values + + +def _project_feedback_to_command_limits(position: tuple[float, ...]) -> tuple[float, ...]: + projected: list[float] = [] + for value, joint in zip(position, GALAXEA_A1_DESCRIPTION.joints, strict=True): + lower, upper = joint.position_limits + command = min(upper, max(lower, value)) + assert abs(command - value) <= _FEEDBACK_TOLERANCE_RAD, ( + f"{joint.name} feedback {value:.6f} is too far outside its command limits" + ) + projected.append(command) + return tuple(projected) + + +def test_galaxea_a1_full_deploy_current_hold_and_estop() -> None: + """Prove a measured hold traverses the real kernel and relay unchanged.""" + import rclpy + from diagnostic_msgs.msg import DiagnosticArray + from openral_msgs.msg import ActionChunk, FailureTrigger + from rclpy.qos import ( + DurabilityPolicy, + HistoryPolicy, + QoSProfile, + ReliabilityPolicy, + qos_profile_sensor_data, + ) + from sensor_msgs.msg import JointState + from std_msgs.msg import Empty + + rclpy.init() + node = rclpy.create_node("openral_hil_galaxea_a1_deploy") + observed = _Observed() + control_qos = QoSProfile( + history=HistoryPolicy.KEEP_LAST, + depth=1, + reliability=ReliabilityPolicy.RELIABLE, + durability=DurabilityPolicy.VOLATILE, + ) + safety_qos = QoSProfile( + history=HistoryPolicy.KEEP_LAST, + depth=10, + reliability=ReliabilityPolicy.RELIABLE, + durability=DurabilityPolicy.VOLATILE, + ) + candidate_pub = node.create_publisher(ActionChunk, "/openral/candidate_action", control_qos) + estop_pub = node.create_publisher(Empty, "/openral/estop", safety_qos) + candidate: ActionChunk | None = None + subscriptions = [ + node.create_subscription( + JointState, + "/joint_states", + observed.on_joint_state, + qos_profile_sensor_data, + ), + node.create_subscription( + DiagnosticArray, + "/diagnostics", + observed.on_diagnostics, + 10, + ), + node.create_subscription( + ActionChunk, + "/openral/safe_action", + observed.on_safe_action, + control_qos, + ), + node.create_subscription( + FailureTrigger, + "/openral/failure/safety", + observed.on_failure, + safety_qos, + ), + ] + try: + assert _spin_until( + node, + lambda: ( + observed.joint_state is not None + and "openral_safety_kernel" in observed.diagnostics + and "openral_hal_galaxea_a1" in observed.diagnostics + and node.count_subscribers("/openral/candidate_action") >= 1 + and node.count_subscribers("/openral/estop") >= 1 + and node.count_publishers("/openral/safe_action") >= 1 + ), + _GRAPH_TIMEOUT_S, + ), "full OpenRAL graph did not become observable within 10 s" + + kernel = observed.diagnostics["openral_safety_kernel"] + kernel_values = _diagnostic_values(kernel) + assert kernel.message == "passthrough active" + assert kernel_values["envelope_loaded"] == "true" + assert kernel_values["n_dof"] == "6" + + hal = observed.diagnostics["openral_hal_galaxea_a1"] + hal_values = _diagnostic_values(hal) + assert hal.message == "A1 feedback and motor status healthy" + assert hal_values["estopped"] == "false" + assert hal_values["command_relay_state"] == "LOCKED" + + assert observed.joint_state is not None + initial = _ordered_positions(observed.joint_state) + target = _project_feedback_to_command_limits(initial) + trace_id = f"galaxea-a1-deploy-hil-{time.time_ns()}" + candidate = ActionChunk() + candidate.header.stamp = node.get_clock().now().to_msg() + candidate.control_mode = 0 + candidate.horizon = 1 + candidate.flat = list(target) + candidate.n_dof = len(target) + candidate.confidence = 1.0 + candidate.rskill_id = "galaxea_a1_current_hold_hil" + candidate.rskill_revision = "local" + candidate.trace_id = trace_id + candidate.tick_index = 1 + + deadline = time.monotonic() + _HOLD_TIMEOUT_S + while time.monotonic() < deadline: + cycle_started = time.monotonic() + candidate.header.stamp = node.get_clock().now().to_msg() + candidate_pub.publish(candidate) + rclpy.spin_once(node, timeout_sec=0.01) + if observed.joint_state is not None: + current = _ordered_positions(observed.joint_state) + assert ( + max(abs(now - start) for now, start in zip(current, initial, strict=True)) + < _MAX_HOLD_DRIFT_RAD + ) + hal_status = observed.diagnostics.get("openral_hal_galaxea_a1") + matching_safe = [ + action for action in observed.safe_actions if action.trace_id == trace_id + ] + if ( + hal_status is not None + and _diagnostic_values(hal_status).get("command_relay_state") == "ACTIVE" + and matching_safe + ): + break + time.sleep(max(0.0, _COMMAND_PERIOD_S - (time.monotonic() - cycle_started))) + else: + hal_status = observed.diagnostics.get("openral_hal_galaxea_a1") + hal_debug = ( + _diagnostic_values(hal_status) if hal_status is not None else {"status": "missing"} + ) + hal_history = [ + _diagnostic_values(status) + for status in observed.diagnostic_history.get("openral_hal_galaxea_a1", []) + ] + kernel_status = observed.diagnostics.get("openral_safety_kernel") + kernel_debug = ( + _diagnostic_values(kernel_status) + if kernel_status is not None + else {"status": "missing"} + ) + pytest.fail( + "current hold did not traverse the kernel and activate the relay; " + f"matching_safe={len(matching_safe)}, hal={hal_debug}, " + f"hal_history={hal_history}, kernel={kernel_debug}" + ) + + matching_safe = [action for action in observed.safe_actions if action.trace_id == trace_id] + assert matching_safe + assert tuple(matching_safe[-1].flat) == target + assert matching_safe[-1].n_dof == len(target) + assert matching_safe[-1].control_mode == 0 + assert not [failure for failure in observed.failures if failure.trace_id == trace_id] + + hal_values = _diagnostic_values(observed.diagnostics["openral_hal_galaxea_a1"]) + expected_csv = ",".join(f"{value:.6f}" for value in target) + assert hal_values["command_relay_state"] == "ACTIVE" + assert hal_values["staged_position"] == expected_csv + assert hal_values["forwarded_position"] == expected_csv + + if _ALLOW_NUDGE: + nudge_target = list(target) + nudge_target[0] += _NUDGE_RAD + assert nudge_target[0] <= GALAXEA_A1_DESCRIPTION.joints[0].position_limits[1] + nudge_trace_id = f"galaxea-a1-deploy-hil-nudge-{time.time_ns()}" + candidate.flat = nudge_target + candidate.trace_id = nudge_trace_id + candidate.rskill_id = "galaxea_a1_joint1_nudge_hil" + candidate.tick_index = 2 + + deadline = time.monotonic() + _MOTION_TIMEOUT_S + settled = False + while time.monotonic() < deadline: + cycle_started = time.monotonic() + candidate.header.stamp = node.get_clock().now().to_msg() + candidate_pub.publish(candidate) + rclpy.spin_once(node, timeout_sec=0.01) + if observed.joint_state is not None: + current = _ordered_positions(observed.joint_state) + for index, (start, actual) in enumerate(zip(initial, current, strict=True)): + assert abs(actual - start) < _MAX_EXCURSION_RAD, ( + f"A1 joint {index + 1} excursion " + f"{abs(actual - start):.4f} rad exceeded " + f"{_MAX_EXCURSION_RAD:.4f} rad" + ) + settled = abs(current[0] - nudge_target[0]) <= _SETTLE_TOLERANCE_RAD and any( + action.trace_id == nudge_trace_id for action in observed.safe_actions + ) + if settled: + break + time.sleep(max(0.0, _COMMAND_PERIOD_S - (time.monotonic() - cycle_started))) + assert settled, "joint1 did not settle at the full-graph +0.01 rad target" + nudge_safe = [ + action for action in observed.safe_actions if action.trace_id == nudge_trace_id + ] + assert nudge_safe + assert tuple(nudge_safe[-1].flat) == tuple(nudge_target) + assert not [ + failure for failure in observed.failures if failure.trace_id == nudge_trace_id + ] + + return_trace_id = f"galaxea-a1-deploy-hil-return-{time.time_ns()}" + candidate.flat = list(target) + candidate.trace_id = return_trace_id + candidate.rskill_id = "galaxea_a1_joint1_return_hil" + candidate.tick_index = 3 + deadline = time.monotonic() + _MOTION_TIMEOUT_S + returned = False + while time.monotonic() < deadline: + cycle_started = time.monotonic() + candidate.header.stamp = node.get_clock().now().to_msg() + candidate_pub.publish(candidate) + rclpy.spin_once(node, timeout_sec=0.01) + if observed.joint_state is not None: + current = _ordered_positions(observed.joint_state) + returned = max( + abs(actual - start) for actual, start in zip(current, initial, strict=True) + ) <= _SETTLE_TOLERANCE_RAD and any( + action.trace_id == return_trace_id for action in observed.safe_actions + ) + if returned: + break + time.sleep(max(0.0, _COMMAND_PERIOD_S - (time.monotonic() - cycle_started))) + assert returned, "joint1 did not return to the measured full-graph start" + return_safe = [ + action for action in observed.safe_actions if action.trace_id == return_trace_id + ] + assert return_safe + assert tuple(return_safe[-1].flat) == target + assert not [ + failure for failure in observed.failures if failure.trace_id == return_trace_id + ] + finally: + # The real A1 HAL requires a fresh sidecar and lifecycle after estop. + # Keep the already-validated current hold alive until the e-stop is + # observed. The sidecar's short command lease must not race DDS + # delivery of the stop request and turn a clean e-stop into an + # unacknowledged transport fault. + estop_confirmed = False + try: + deadline = time.monotonic() + _ESTOP_TIMEOUT_S + while time.monotonic() < deadline: + cycle_started = time.monotonic() + if candidate is not None: + candidate.header.stamp = node.get_clock().now().to_msg() + candidate_pub.publish(candidate) + estop_pub.publish(Empty()) + rclpy.spin_once(node, timeout_sec=0.01) + status = observed.diagnostics.get("openral_hal_galaxea_a1") + if status is not None and _diagnostic_values(status).get("estopped") == "true": + estop_confirmed = True + break + time.sleep(max(0.0, _COMMAND_PERIOD_S - (time.monotonic() - cycle_started))) + finally: + for subscription in subscriptions: + node.destroy_subscription(subscription) + node.destroy_publisher(candidate_pub) + node.destroy_publisher(estop_pub) + node.destroy_node() + rclpy.try_shutdown() + assert estop_confirmed, "HAL diagnostics did not confirm the downstream e-stop" diff --git a/tests/unit/test_all_manifests_validate.py b/tests/unit/test_all_manifests_validate.py index a505c64..05f7002 100644 --- a/tests/unit/test_all_manifests_validate.py +++ b/tests/unit/test_all_manifests_validate.py @@ -46,6 +46,7 @@ "anvil_openarm_v2", "franka_panda", "g1", + "galaxea_a1", "google_robot", "gr1", "h1", @@ -106,6 +107,7 @@ _DEPLOY_STEMS: list[str] = [ "g1_vln", + "galaxea_a1_bench", "isaac_franka", "isaac_franka_bowl", "isaac_franka_urdf", diff --git a/tests/unit/test_deploy_run_real_resolution.py b/tests/unit/test_deploy_run_real_resolution.py index bb2bce0..930796c 100644 --- a/tests/unit/test_deploy_run_real_resolution.py +++ b/tests/unit/test_deploy_run_real_resolution.py @@ -11,9 +11,11 @@ from __future__ import annotations +from pathlib import Path + import pytest from openral_cli.deploy_sim import resolve_launch_invocation -from openral_core.exceptions import ROSCapabilityMismatch +from openral_core.exceptions import ROSCapabilityMismatch, ROSConfigError def _resolve(robot_id: str, mode: str) -> object: @@ -27,6 +29,32 @@ def _resolve(robot_id: str, mode: str) -> object: class TestRealModeResolution: + def test_galaxea_a1_scene_resolves_to_real_lifecycle_package(self) -> None: + config = Path("scenes/deploy/galaxea_a1_bench.yaml") + inv = resolve_launch_invocation( + config=config, + robot_override=None, + dashboard_port=4318, + reset_to_pose_service=None, + hal_mode="real", + ) + assert inv.hal.package == "openral_hal_galaxea_a1" + assert inv.hal.executable == "lifecycle_node.py" + assert inv.hal_params["hal_mode"] == "real" + assert str(inv.hal_params["robot_yaml"]).endswith("robots/galaxea_a1/robot.yaml") + assert "enable_reasoner:=false" in inv.argv_template + + def test_direct_rskill_scene_rejects_initial_task(self) -> None: + with pytest.raises(ROSConfigError, match=r"runtime\.enable_reasoner=true"): + resolve_launch_invocation( + config=Path("scenes/deploy/galaxea_a1_bench.yaml"), + robot_override=None, + dashboard_port=4318, + reset_to_pose_service=None, + hal_mode="real", + initial_task_prompt="pick the mango", + ) + def test_manifest_robot_real_forwards_hal_mode(self) -> None: inv = _resolve("franka_panda", "real") assert inv.hal_params["hal_mode"] == "real" @@ -46,8 +74,6 @@ def test_sim_only_robot_real_fails_fast(self) -> None: _resolve("g1", "real") def test_no_robot_and_no_config_raises(self) -> None: - from openral_core.exceptions import ROSConfigError - with pytest.raises(ROSConfigError, match="robot_id is undefined"): resolve_launch_invocation( config=None, diff --git a/tests/unit/test_eval_registry_and_runner.py b/tests/unit/test_eval_registry_and_runner.py index 70aca34..66f5ecc 100644 --- a/tests/unit/test_eval_registry_and_runner.py +++ b/tests/unit/test_eval_registry_and_runner.py @@ -72,6 +72,7 @@ def test_builtin_registries_populated() -> None: assert "zero" in POLICIES assert "random" in POLICIES assert "smolvla" in POLICIES + assert "lingbot_va_a1" in POLICIES def test_registry_unknown_id_raises_with_known_list() -> None: @@ -87,6 +88,23 @@ def _factory_a() -> int: assert "widget" in msg and "typo" in msg and "['a']" in msg +def test_registry_iterates_ids_for_error_messages() -> None: + """``sorted(REGISTRY)`` is used in error paths (e.g. sim_bringup's unknown-scene + message); before ``__iter__`` existed that crashed with a TypeError, masking the + typed ROSConfigError it was formatting.""" + reg: _Registry[int] = _Registry("widget") + + @reg.register("b") + def _factory_b() -> int: + return 2 + + @reg.register("a") + def _factory_a() -> int: + return 1 + + assert sorted(reg) == ["a", "b"] + + def test_registry_duplicate_registration_rejected() -> None: reg: _Registry[int] = _Registry("widget") reg.register("a")(lambda: 1) diff --git a/tests/unit/test_galaxea_a1.py b/tests/unit/test_galaxea_a1.py new file mode 100644 index 0000000..8fe6644 --- /dev/null +++ b/tests/unit/test_galaxea_a1.py @@ -0,0 +1,860 @@ +"""Galaxea A1 HAL contract tests against a process-boundary fake transport.""" + +from __future__ import annotations + +import json +import runpy +import signal +import socket +import sys +import threading +import time +from pathlib import Path +from types import SimpleNamespace +from typing import Any + +import pytest +from openral_core import ( + Action, + ControlMode, + RobotDescription, + ROSConfigError, + ROSEStopRequested, + ROSRuntimeError, + RSkillManifest, + SensorReaderBackend, + SensorReaderConfig, + VLASpec, +) +from openral_hal.galaxea_a1 import ( + GALAXEA_A1_DESCRIPTION, + GalaxeaA1HAL, + _Snapshot, + _SocketTransport, +) +from openral_hal.protocol import HAL, HALHealthProvider, LifecycleEStopHAL +from openral_runner.backends.galaxea_a1_ipc import RuntimeLocalClient, runtime_socket_path +from openral_runner.factory import SENSOR_BACKEND_REGISTRY, make_sensor_readers +from openral_sim.policies.lingbot_va_a1 import ( + _joint_contract, + _model_identity, +) + +_SIDECAR = runpy.run_path( + str(Path(__file__).resolve().parents[2] / "tools" / "galaxea_a1_ros1_sidecar.py") +) + + +class FakeA1Transport: + def __init__(self, *, status_codes: tuple[int, ...] = (0, 0, 0, 0, 0, 0, 0)) -> None: + self.connected = False + self.hello: dict[str, object] | None = None + self.messages: list[dict[str, object]] = [] + self.estopped = False + now = time.monotonic() + self.current = _Snapshot( + position=(0.0, 1.0, -1.0, 0.0, 0.0, 0.0), + velocity=(0.0,) * 6, + effort=(0.0,) * 6, + stamp_ns=time.time_ns(), + received_monotonic=now, + status_codes=status_codes, + status_received_monotonic=now, + gripper_norm=0.5, + ) + + def connect(self, hello: dict[str, object], timeout_s: float) -> None: + del timeout_s + self.connected = True + self.hello = hello + + def close(self) -> None: + self.connected = False + + def snapshot(self) -> _Snapshot: + return self.current + + def send(self, message: dict[str, object]) -> None: + self.messages.append(message) + + def estop(self, timeout_s: float) -> None: + del timeout_s + self.estopped = True + self.connected = False + + +def _joint_action(delta: float = 0.0) -> Action: + return Action( + control_mode=ControlMode.JOINT_POSITION, + joint_targets=[[delta, 1.0, -1.0, 0.0, 0.0, 0.0]], + stamp_ns=time.time_ns(), + ) + + +def test_description_and_protocol_surface() -> None: + transport = FakeA1Transport() + hal = GalaxeaA1HAL(transport=transport) + assert isinstance(hal, HAL) + assert isinstance(hal, HALHealthProvider) + assert isinstance(hal, LifecycleEStopHAL) + assert hal.description is GALAXEA_A1_DESCRIPTION + assert hal.description.sdk_kind == "closed_with_api" + assert hal.description.hal.sim is None + assert hal.description.capabilities.has_vision + assert [sensor.name for sensor in hal.description.sensors] == ["front", "wrist"] + assert "policy_substep_rad" not in hal.description.hal.parameters.defaults + + +def test_lingbot_manifest_owns_policy_joint_substep() -> None: + manifest_path = ( + Path(__file__).resolve().parents[2] + / "rskills" + / "lingbot-va-galaxea-a1-fruit-placement" + / "rskill.yaml" + ) + manifest = RSkillManifest.from_yaml(str(manifest_path)) + + assert manifest.policy_extras["max_joint_substep_rad"] == 0.045 + assert "runtime_config" not in manifest.policy_extras + + +def test_lingbot_rskill_pins_the_gateway_model_identity() -> None: + spec = VLASpec( + id="lingbot_va_a1", + weights_uri="rskills/lingbot-va-galaxea-a1-fruit-placement", + ) + + repo_id, revision = _model_identity(spec) + + assert repo_id == "pengyue-polaron/lingbot-va-galaxea-a1-fruit-placement-eef" + assert revision == "90e017bdbc6afac2e441b4634c9192776bbcb8b7" + + +def test_lingbot_joint_contract_uses_manifest_limits_and_policy_bound() -> None: + spec = VLASpec( + id="lingbot_va_a1", + weights_uri="rskills/lingbot-va-galaxea-a1-fruit-placement", + extra={"max_joint_substep_rad": 0.045}, + ) + + names, lower, upper, step = _joint_contract( + spec, + GALAXEA_A1_DESCRIPTION, + ) + + assert names == tuple(f"arm_joint{index}" for index in range(1, 7)) + assert lower == pytest.approx([-2.8798, 0.0, -3.3161, -2.8798, -1.6581, -2.8798]) + assert upper == pytest.approx([2.8798, 3.1415, 0.0, 2.8798, 1.6581, 2.8798]) + assert step == 0.045 + + +def test_lingbot_joint_contract_rejects_bound_above_hal_phase_limit() -> None: + spec = VLASpec( + id="lingbot_va_a1", + weights_uri="rskills/lingbot-va-galaxea-a1-fruit-placement", + extra={"max_joint_substep_rad": 0.06}, + ) + + with pytest.raises(ROSConfigError, match="no greater than"): + _joint_contract(spec, GALAXEA_A1_DESCRIPTION) + + +def test_manifest_and_hal_pin_official_a1_joint_transforms() -> None: + manifest_path = Path(__file__).resolve().parents[2] / "robots" / "galaxea_a1" / "robot.yaml" + manifest = RobotDescription.from_yaml(str(manifest_path)) + expected = [ + ((-0.0011147, 0.0, 0.0892), (0.0, 0.0, 3.1416), (0.0, 0.0, 1.0)), + ((0.0, -0.00004, 0.0615), (1.5708, 0.0, 0.0), (0.0, 0.0, 1.0)), + ((0.34928, 0.02, 0.0), (0.0, 0.0, 1.5708), (0.0, 0.0, 1.0)), + ((0.07, -0.00395, -0.00004), (-1.5708, 1.5708, 0.0), (0.0, 0.0, 1.0)), + ((0.0, 0.0, 0.2776), (-1.5708, 0.0, 3.1416), (0.0, 0.0, 1.0)), + ((0.0, -0.1575, -0.00023266), (1.5708, 0.0, 0.0), (0.0, 0.0, 1.0)), + ] + + for description in (manifest, GALAXEA_A1_DESCRIPTION): + assert [ + (joint.origin_xyz, joint.origin_rpy, joint.axis_xyz) for joint in description.joints + ] == expected + + +@pytest.mark.parametrize("host", ["0.0.0.0", "192.168.1.20", "sidecar.example", "::1"]) +def test_hal_rejects_non_loopback_sidecar_hosts(host: str) -> None: + with pytest.raises(ROSConfigError, match="IPv4 loopback"): + GalaxeaA1HAL(host=host) + + +@pytest.mark.parametrize("port", [True, "46011", 46011.5, 0, 65536]) +def test_hal_rejects_invalid_sidecar_ports(port: object) -> None: + with pytest.raises(ROSConfigError, match="TCP port"): + GalaxeaA1HAL(port=port) # type: ignore[arg-type] # reason: runtime validation contract + + +def test_camera_bridge_backend_uses_runtime_process_endpoint( + monkeypatch: pytest.MonkeyPatch, + tmp_path: Path, +) -> None: + monkeypatch.setenv("A1_PROCESS_STATE_ROOT", str(tmp_path)) + cfg = SensorReaderConfig( + sensor_id="front", + backend=SensorReaderBackend.GALAXEA_A1_CAMERA_BRIDGE, + backend_params={"camera": "front"}, + max_age_ms=500, + ) + reader = SENSOR_BACKEND_REGISTRY[cfg.backend.value](cfg) + assert reader.sensor_id == "front" + assert not reader.is_open + assert runtime_socket_path("a1-camera-bridge.sock") == (tmp_path / "a1-camera-bridge.sock") + with pytest.raises(ROSConfigError, match="corresponding A1 Runtime service"): + reader.open() + + +def test_camera_bridge_backend_rejects_checkout_parameters() -> None: + cfg = SensorReaderConfig( + sensor_id="front", + backend=SensorReaderBackend.GALAXEA_A1_CAMERA_BRIDGE, + backend_params={ + "camera": "front", + "runtime_root_env": "OPENRAL_GALAXEA_A1_RUNTIME_ROOT", + }, + ) + + with pytest.raises(ROSConfigError, match="unknown params"): + SENSOR_BACKEND_REGISTRY[cfg.backend.value](cfg) + + +def test_camera_bridge_backend_rejects_unknown_camera() -> None: + cfg = SensorReaderConfig( + sensor_id="front", + backend=SensorReaderBackend.GALAXEA_A1_CAMERA_BRIDGE, + backend_params={"camera": "side"}, + ) + with pytest.raises(ROSConfigError, match="'front' or 'wrist'"): + SENSOR_BACKEND_REGISTRY[cfg.backend.value](cfg) + + +def test_runtime_local_client_closes_a_broken_transport() -> None: + client_socket, peer_socket = socket.socketpair() + client = RuntimeLocalClient( + "test.sock", + label="test Runtime service", + max_response_bytes=1024, + ) + client._socket = client_socket + peer_socket.close() + + with pytest.raises(ROSRuntimeError, match="transport failed"): + client.call({"op": "describe"}, timeout_s=0.1) + + assert client._socket is None + + +def test_camera_bridge_batch_shares_one_paired_session() -> None: + configs = [ + SensorReaderConfig( + sensor_id=camera, + backend=SensorReaderBackend.GALAXEA_A1_CAMERA_BRIDGE, + backend_params={"camera": camera}, + max_age_ms=500, + ) + for camera in ("front", "wrist") + ] + + front, wrist = make_sensor_readers(configs) + + assert front._session is wrist._session + + +def test_connect_reads_state_and_sends_aligned_joint_target() -> None: + transport = FakeA1Transport() + hal = GalaxeaA1HAL(transport=transport) + hal.connect() + assert hal.read_state().position == [0.0, 1.0, -1.0, 0.0, 0.0, 0.0] + hal.send_action(_joint_action(0.01)) + assert transport.messages[-1]["joint_targets"] == [0.01, 1.0, -1.0, 0.0, 0.0, 0.0] + assert transport.hello is not None + assert transport.hello["robot"] == "galaxea_a1" + assert transport.hello["command_lease_s"] == 0.5 + + +def test_sidecar_cross_checks_identity_limits_and_mask_types() -> None: + hello = { + "op": "hello", + "protocol": 1, + "robot": "galaxea_a1", + "joint_names": [f"arm_joint{i}" for i in range(1, 7)], + "joint_position_min": [-2.8798, 0.0, -3.3161, -2.8798, -1.6581, -2.8798], + "joint_position_max": [2.8798, 3.1415, 0.0, 2.8798, 1.6581, 2.8798], + "initial_alignment_tolerance_rad": 0.05, + "tracker_alignment_timeout_s": 5.0, + "max_target_step_rad": 0.08, + "command_lease_s": 0.5, + "state_timeout_s": 0.5, + "status_timeout_s": 1.0, + "feedback_limit_tolerance_rad": 0.01, + "idle_timeout_error_mask": 64, + "gripper_ignored_error_mask": 8, + "gripper_stroke_min_mm": 0.0, + "gripper_stroke_max_mm": 104.0, + } + decode_hello = _SIDECAR["_hello_config"] + + assert decode_hello(hello)["joint_position_min"][1] == 0.0 + with pytest.raises(ValueError, match="official A1 limits"): + decode_hello( + {**hello, "joint_position_min": [-3.0, 0.0, -3.3161, -2.8798, -1.6581, -2.8798]} + ) + with pytest.raises(ValueError, match="integers"): + decode_hello({**hello, "idle_timeout_error_mask": True}) + + +def test_sidecar_pins_every_vendor_topic_in_launch_commands() -> None: + driver = _SIDECAR["_driver_argv"]("/dev/a1") + tracker = _SIDECAR["_tracker_argv"]() + tracker_parameters = _SIDECAR["_tracker_parameters"]("/opt/galaxea/A1_SDK") + + assert "single_arm_serial_port_path:=/dev/a1" in driver + assert "single_arm_joint_states_topic:=/joint_states_host" in driver + assert "single_arm_control_topic:=/arm_joint_command_host" in driver + assert "single_arm_gripper_position_control_topic:=/gripper_position_control_host" in driver + assert tracker == [ + "rosrun", + "mobiman", + "jointTracker_demo_node", + "__name:=openral_a1_joint_tracker", + ] + assert ( + tracker_parameters["/openral_a1_joint_tracker/joint_states_sub_topic"] + == "/joint_states_host" + ) + assert ( + tracker_parameters["/openral_a1_joint_tracker/arm_joint_command_topic"] + == "/openral/arm_joint_command_staged" + ) + assert ( + tracker_parameters["/openral_a1_joint_tracker/arm_joint_target_position"] + == "/arm_joint_target_position" + ) + assert not any("eePose" in key for key in tracker_parameters) + + +class _FakeSidecarPublisher: + def __init__(self, topic: str) -> None: + self.topic = topic + self.messages: list[object] = [] + + def publish(self, message: object) -> None: + self.messages.append(message) + + def get_num_connections(self) -> int: + return 1 + + +class _FakeSidecarSubscriber: + def get_num_connections(self) -> int: + return 1 + + +class _FakeSidecarRospy: + class Time: + @staticmethod + def now() -> int: + return 123 + + def __init__(self) -> None: + self.publishers: dict[str, _FakeSidecarPublisher] = {} + + def Publisher( # noqa: N802 - mirrors rospy's public API + self, topic: str, _message_type: object, *, queue_size: int + ) -> _FakeSidecarPublisher: + assert queue_size == 1 + publisher = _FakeSidecarPublisher(topic) + self.publishers[topic] = publisher + return publisher + + def Subscriber( # noqa: N802 - mirrors rospy's public API + self, + _topic: str, + _message_type: object, + _callback: object, + *, + queue_size: int, + ) -> _FakeSidecarSubscriber: + assert queue_size == 1 + return _FakeSidecarSubscriber() + + def logerr(self, *_args: object) -> None: + pass + + def logwarn(self, *_args: object) -> None: + pass + + +def _sidecar_joint_state() -> SimpleNamespace: + return SimpleNamespace( + header=SimpleNamespace( + stamp=SimpleNamespace(to_nsec=time.time_ns), + frame_id="", + ), + name=[], + position=[], + velocity=[], + effort=[], + ) + + +def _sidecar_arm_command(position: tuple[float, ...]) -> SimpleNamespace: + return SimpleNamespace( + header=SimpleNamespace(stamp=0), + p_des=list(position), + v_des=[0.0] * 6, + kp=[1.0] * 6, + kd=[1.0] * 6, + t_ff=[0.0] * 6, + mode=0, + ) + + +def _sidecar_gripper_command() -> SimpleNamespace: + return SimpleNamespace(header=SimpleNamespace(stamp=0), gripper_stroke=0.0) + + +_SIDECAR_HELLO: dict[str, object] = { + "op": "hello", + "protocol": 1, + "robot": "galaxea_a1", + "joint_names": [f"arm_joint{i}" for i in range(1, 7)], + "joint_position_min": [-2.8798, 0.0, -3.3161, -2.8798, -1.6581, -2.8798], + "joint_position_max": [2.8798, 3.1415, 0.0, 2.8798, 1.6581, 2.8798], + "initial_alignment_tolerance_rad": 0.05, + "tracker_alignment_timeout_s": 5.0, + "max_target_step_rad": 0.08, + "command_lease_s": 0.5, + "state_timeout_s": 0.5, + "status_timeout_s": 1.0, + "feedback_limit_tolerance_rad": 0.01, + "idle_timeout_error_mask": 64, + "gripper_ignored_error_mask": 8, + "gripper_stroke_min_mm": 0.0, + "gripper_stroke_max_mm": 104.0, +} + + +def _primed_sidecar_bridge( + current: tuple[float, ...], +) -> tuple[_FakeSidecarRospy, Any, dict[str, object]]: + """Build a configured Bridge with joint/gripper/status feedback already latched.""" + rospy = _FakeSidecarRospy() + bridge = _SIDECAR["Bridge"]( + rospy, + _sidecar_joint_state, + object, + _sidecar_gripper_command, + SimpleNamespace, + ) + config = _SIDECAR["_hello_config"](dict(_SIDECAR_HELLO)) + joint_feedback = _sidecar_joint_state() + joint_feedback.name = [f"arm_joint{i}" for i in range(1, 7)] + joint_feedback.position = list(current) + joint_feedback.velocity = [0.0] * 6 + joint_feedback.effort = [0.0] * 6 + bridge._on_joint(joint_feedback) + bridge._on_gripper(SimpleNamespace(position=[52.0])) + bridge._on_status( + SimpleNamespace( + data=SimpleNamespace(motor_errors=[SimpleNamespace(error_code=64) for _ in range(7)]) + ) + ) + bridge.configure(config) + return rospy, bridge, config + + +def test_sidecar_relay_never_forwards_unaligned_tracker_startup() -> None: + arm_command = _sidecar_arm_command + # Encoder zero/quantization can leave feedback just beyond a nominal + # endpoint. The requested hold is projected to the exact legal endpoint. + current = (0.0, -0.001, -1.0, 0.0, 0.0, 0.0) + hold_target = (0.0, 0.0, -1.0, 0.0, 0.0, 0.0) + rospy, bridge, config = _primed_sidecar_bridge(current) + + assert not bridge.apply({"joint_targets": list(hold_target)}, config, True) + target_publisher = rospy.publishers["/arm_joint_target_position"] + assert len(target_publisher.messages) == 1 + host = rospy.publishers["/arm_joint_command_host"] + bridge._on_staged(arm_command((0.105, 0.889, -0.623, 1.919, 0.312, 1.245))) + assert bridge.relay_state() == "ARMING" + assert host.messages == [] + updated_target = (0.01, 0.0, -1.0, 0.0, 0.0, 0.0) + assert not bridge.apply({"joint_targets": list(updated_target)}, config, False) + assert len(target_publisher.messages) == 2 + assert host.messages == [] + + # The tracker begins at measured feedback. This near-endpoint sample is + # tolerated only while staged and must never reach the host command topic. + bridge._on_staged(arm_command(current)) + assert bridge.relay_state() == "ARMING" + assert host.messages == [] + + bridge._on_staged(arm_command(updated_target)) + assert bridge.relay_state() == "ACTIVE" + assert len(host.messages) == 1 + + gripper_publisher = rospy.publishers["/gripper_position_control_host"] + assert not bridge.apply({"gripper": 0.75}, config, False) + assert not bridge.apply({"gripper": 0.75}, config, False) + assert len(gripper_publisher.messages) == 1 + assert gripper_publisher.messages[0].gripper_stroke == 78.0 + + +def test_sidecar_gripper_requires_active_relay() -> None: + """Gripper actuation is gated by the same relay machine as arm joints. + + The gripper bypasses the tracker's staged-hold interpolation, so a + setpoint arriving while the relay is still ``LOCKED`` or ``ARMING`` must + fail closed instead of moving the physical gripper before alignment. + """ + current = (0.0, -0.001, -1.0, 0.0, 0.0, 0.0) + hold_target = (0.0, 0.0, -1.0, 0.0, 0.0, 0.0) + rospy, bridge, config = _primed_sidecar_bridge(current) + gripper_publisher = rospy.publishers["/gripper_position_control_host"] + + # LOCKED: a gripper-only action is refused and nothing is published. + with pytest.raises(RuntimeError, match="not ACTIVE: LOCKED"): + bridge.apply({"gripper": 0.5}, config, True) + assert gripper_publisher.messages == [] + + # First joint command stages the relay (ARMING); gripper stays refused, + # even when it rides in the same packet as a valid joint target. + assert not bridge.apply({"joint_targets": list(hold_target)}, config, True) + assert bridge.relay_state() == "ARMING" + with pytest.raises(RuntimeError, match="not ACTIVE: ARMING"): + bridge.apply({"gripper": 0.5, "joint_targets": list(hold_target)}, config, False) + assert gripper_publisher.messages == [] + + # Tracker aligns with the staged hold -> ACTIVE: the setpoint now forwards. + bridge._on_staged(_sidecar_arm_command(hold_target)) + assert bridge.relay_state() == "ACTIVE" + assert not bridge.apply({"gripper": 0.5}, config, False) + assert len(gripper_publisher.messages) == 1 + assert gripper_publisher.messages[0].gripper_stroke == 52.0 + + +def test_deploy_sim_of_real_only_robot_raises_typed_errors() -> None: + """`deploy sim` of the hardware-only A1 fails with typed errors, never a TypeError. + + The bare-twin path reports the robot as real-hardware-only; attaching the + deploy bench scene (which registers no sim scene id) reports the unknown + scene id — the latter used to crash with ``TypeError: '_Registry' object + is not iterable`` while formatting its own error message. + """ + from openral_core.exceptions import ROSCapabilityMismatch + from openral_hal.resolver import build_hal + + repo_root = Path(__file__).resolve().parents[2] + robot_yaml = repo_root / "robots" / "galaxea_a1" / "robot.yaml" + description = RobotDescription.from_yaml(str(robot_yaml)) + with pytest.raises(ROSCapabilityMismatch, match="real-hardware-only"): + build_hal(description, mode="sim") + with pytest.raises(ROSConfigError, match="not registered"): + build_hal( + description, + mode="sim", + sim_env_yaml=str(repo_root / "scenes" / "deploy" / "galaxea_a1_bench.yaml"), + ) + + +def test_sidecar_stop_waits_for_sigkill_completion() -> None: + stack = _SIDECAR["Stack"](shutdown_grace_s=0.05, kill_wait_s=1.0) + child = stack.start( + [ + sys.executable, + "-c", + ("import signal,time;signal.signal(signal.SIGINT, signal.SIG_IGN);time.sleep(30)"), + ] + ) + time.sleep(0.1) + + stack.stop() + + assert child.poll() is not None + assert child.returncode == -signal.SIGKILL + + +def test_sidecar_drains_buffered_action_before_active_lease_check() -> None: + """A tracker callback stall must not hide a fresh buffered hold.""" + + class FakeRospy: + def __init__(self, bridge: FakeBridge) -> None: + self._bridge = bridge + + def loginfo(self, *_args: object) -> None: + pass + + def is_shutdown(self) -> bool: + return self._bridge.apply_count >= 2 + + class FakeBridge: + def __init__(self) -> None: + self.apply_count = 0 + self.active = False + self.state_entered = threading.Event() + self.release_state = threading.Event() + + def configure(self, _config: dict[str, object]) -> None: + pass + + def apply( + self, + _packet: dict[str, object], + _config: dict[str, object], + _first_joint_command: bool, + ) -> bool: + self.apply_count += 1 + return False + + def relay_state(self) -> str: + return "ACTIVE" if self.active else "ARMING" + + def state(self, _config: dict[str, object]) -> dict[str, object]: + if self.apply_count == 1: + self.state_entered.set() + assert self.release_state.wait(1.0) + return {"op": "state"} + + listener = socket.socket() + listener.bind(("127.0.0.1", 0)) + listener.listen(1) + client = socket.create_connection(listener.getsockname(), timeout=1.0) + bridge = FakeBridge() + failures: list[BaseException] = [] + + def serve() -> None: + try: + _SIDECAR["_serve"](listener, bridge, FakeRospy(bridge), lambda: None) + except BaseException as exc: + failures.append(exc) + + server = threading.Thread(target=serve, daemon=True) + server.start() + hello = { + "op": "hello", + "protocol": 1, + "robot": "galaxea_a1", + "joint_names": [f"arm_joint{i}" for i in range(1, 7)], + "joint_position_min": [-2.8798, 0.0, -3.3161, -2.8798, -1.6581, -2.8798], + "joint_position_max": [2.8798, 3.1415, 0.0, 2.8798, 1.6581, 2.8798], + "initial_alignment_tolerance_rad": 0.05, + "tracker_alignment_timeout_s": 5.0, + "max_target_step_rad": 0.08, + "command_lease_s": 0.05, + "state_timeout_s": 0.5, + "status_timeout_s": 1.0, + "feedback_limit_tolerance_rad": 0.01, + "idle_timeout_error_mask": 64, + "gripper_ignored_error_mask": 8, + "gripper_stroke_min_mm": 0.0, + "gripper_stroke_max_mm": 104.0, + } + client.sendall(json.dumps(hello).encode() + b"\n") + client.sendall(json.dumps({"op": "action", "joint_targets": [0.0] * 6}).encode() + b"\n") + assert bridge.state_entered.wait(1.0) + + # Queue a fresh hold while the server is starved in bridge.state(), then + # let the relay become ACTIVE after the old command is already expired. + client.sendall(json.dumps({"op": "action", "joint_targets": [0.0] * 6}).encode() + b"\n") + time.sleep(0.08) + bridge.active = True + bridge.release_state.set() + server.join(timeout=1.0) + client.close() + listener.close() + + assert not failures + assert bridge.apply_count == 2 + + +def test_real_socket_transport_round_trip() -> None: + listener = socket.socket() + listener.bind(("127.0.0.1", 0)) + listener.listen(1) + port = listener.getsockname()[1] + action_received = threading.Event() + records: list[dict[str, object]] = [] + + def _server() -> None: + with listener: + conn, _ = listener.accept() + with conn, conn.makefile("rb") as stream: + records.append(json.loads(stream.readline())) + conn.sendall( + json.dumps({"op": "hello", "protocol": 1, "robot": "galaxea_a1"}).encode() + + b"\n" + ) + state = { + "op": "state", + "name": [f"arm_joint{i}" for i in range(1, 7)], + "position": [0.0, 1.0, -1.0, 0.0, 0.0, 0.0], + "velocity": [0.0] * 6, + "effort": [0.0] * 6, + "stamp_ns": time.time_ns(), + "status_codes": [0] * 7, + "gripper_norm": 0.5, + "relay_state": "LOCKED", + } + conn.sendall(json.dumps(state).encode() + b"\n") + records.append(json.loads(stream.readline())) + action_received.set() + + server = threading.Thread(target=_server, daemon=True) + server.start() + hal = GalaxeaA1HAL(host="127.0.0.1", port=port, connect_timeout_s=2.0) + hal.connect() + hal.send_action(_joint_action(0.01)) + assert action_received.wait(2.0) + hal.disconnect() + server.join(timeout=2.0) + assert records[0]["op"] == "hello" + assert records[1]["joint_targets"] == [0.01, 1.0, -1.0, 0.0, 0.0, 0.0] + + +def test_socket_transport_coalesces_adjacent_arm_and_gripper_modes() -> None: + transport = _SocketTransport("127.0.0.1", 46011) + transport.send({"op": "action", "stamp_ns": 10, "joint_targets": [0.0] * 6}) + transport.send({"op": "action", "stamp_ns": 11, "gripper": 0.75}) + + message = transport._take_pending_message() + + assert message == { + "op": "action", + "stamp_ns": 11, + "joint_targets": [0.0] * 6, + "gripper": 0.75, + } + + +def test_socket_transport_writer_is_not_blocked_by_state_reader( + monkeypatch: pytest.MonkeyPatch, +) -> None: + transport = _SocketTransport("127.0.0.1", 46011) + client, server = socket.socketpair() + reader_entered = threading.Event() + release_reader = threading.Event() + + def blocked_handle(_raw: object) -> None: + reader_entered.set() + assert release_reader.wait(1.0) + + monkeypatch.setattr(transport, "_handle", blocked_handle) + transport._sock = client + transport._reader_thread = threading.Thread(target=transport._read_loop, daemon=True) + transport._writer_thread = threading.Thread(target=transport._write_loop, daemon=True) + transport._reader_thread.start() + transport._writer_thread.start() + try: + server.sendall(b"{}\n") + assert reader_entered.wait(1.0) + transport.send({"op": "action", "stamp_ns": 10, "joint_targets": [0.0] * 6}) + server.settimeout(1.0) + assert json.loads(server.recv(4096))["joint_targets"] == [0.0] * 6 + finally: + release_reader.set() + transport.close() + server.close() + + +def test_first_joint_target_must_align_with_feedback() -> None: + hal = GalaxeaA1HAL(transport=FakeA1Transport()) + hal.connect() + with pytest.raises(ROSConfigError, match="initial alignment"): + hal.send_action(_joint_action(0.06)) + + +def test_small_feedback_endpoint_error_is_tolerated_but_larger_error_fails() -> None: + transport = FakeA1Transport() + transport.current = _Snapshot( + position=(0.0, -0.003, -1.0, 0.0, 0.0, 0.0), + velocity=(0.0,) * 6, + effort=(0.0,) * 6, + stamp_ns=time.time_ns(), + received_monotonic=time.monotonic(), + status_codes=(0,) * 7, + status_received_monotonic=time.monotonic(), + gripper_norm=0.5, + ) + GalaxeaA1HAL(transport=transport).connect() + + transport.current = _Snapshot( + position=(0.0, -0.02, -1.0, 0.0, 0.0, 0.0), + velocity=(0.0,) * 6, + effort=(0.0,) * 6, + stamp_ns=time.time_ns(), + received_monotonic=time.monotonic(), + status_codes=(0,) * 7, + status_received_monotonic=time.monotonic(), + gripper_norm=0.5, + ) + with pytest.raises(ROSRuntimeError, match="feedback arm_joint2"): + GalaxeaA1HAL(transport=transport).connect() + + +def test_motor_fault_blocks_command_but_idle_timeout_is_allowed() -> None: + bad_transport = FakeA1Transport(status_codes=(1, 0, 0, 0, 0, 0, 0)) + bad = GalaxeaA1HAL(transport=bad_transport) + with pytest.raises(ROSRuntimeError, match="error code 1"): + bad.connect() + assert not bad_transport.connected + + idle = FakeA1Transport(status_codes=(64, 64, 64, 64, 64, 64, 72)) + hal = GalaxeaA1HAL(transport=idle) + hal.connect() + hal.send_action(_joint_action()) + assert len(idle.messages) == 1 + + +def test_absolute_joint_limit_is_enforced_inside_hal() -> None: + transport = FakeA1Transport() + transport.current = _Snapshot( + position=(2.87, 1.0, -1.0, 0.0, 0.0, 0.0), + velocity=(0.0,) * 6, + effort=(0.0,) * 6, + stamp_ns=time.time_ns(), + received_monotonic=time.monotonic(), + status_codes=(0,) * 7, + status_received_monotonic=time.monotonic(), + gripper_norm=0.5, + ) + hal = GalaxeaA1HAL(transport=transport) + hal.connect() + with pytest.raises(ROSConfigError, match="outside"): + hal.send_action(_joint_action(2.89)) + + +def test_health_report_exposes_motor_codes_without_io() -> None: + hal = GalaxeaA1HAL(transport=FakeA1Transport(status_codes=(64,) * 7)) + hal.connect() + + report = hal.health() + + assert report.message == "A1 feedback and motor status healthy" + assert report.fields["motor_status_codes"] == "64,64,64,64,64,64,64" + assert report.fields["gripper_position_normalized"] == "0.500000" + + +def test_gripper_action_uses_normalized_contract() -> None: + transport = FakeA1Transport() + hal = GalaxeaA1HAL(transport=transport) + hal.connect() + hal.send_action( + Action(control_mode=ControlMode.GRIPPER_POSITION, gripper=[0.75], stamp_ns=time.time_ns()) + ) + assert transport.messages[-1]["gripper"] == 0.75 + + +def test_estop_stops_transport_and_always_raises() -> None: + transport = FakeA1Transport() + hal = GalaxeaA1HAL(transport=transport) + hal.connect() + with pytest.raises(ROSEStopRequested): + hal.estop() + assert transport.estopped + with pytest.raises(ROSRuntimeError): + hal.read_state() diff --git a/tests/unit/test_intree_manifests_have_processors.py b/tests/unit/test_intree_manifests_have_processors.py index cd43794..d54016e 100644 --- a/tests/unit/test_intree_manifests_have_processors.py +++ b/tests/unit/test_intree_manifests_have_processors.py @@ -50,6 +50,12 @@ # applied server-side by the model's FeatureTransform, not a lerobot # PolicyProcessorPipeline, so `processors is None` is correct. "lingbot-vla-4b-robotwin", + # LingBot-VA on Galaxea A1: the checkpoint's quantile normalization + # and action-channel mapping ship in configs/va_a1_cfg.py and are + # applied by the contract-checked A1 Runtime model server. OpenRAL + # consumes the server's physical EEF output; there is no lerobot + # PolicyProcessorPipeline or processor JSON pair to materialize. + "lingbot-va-galaxea-a1-fruit-placement", # InternVLA-N1: runs its own AutoProcessor + NavDP pipeline inside the # py3.11 sidecar; no lerobot PolicyProcessorPipeline. `internvla_n1` is # excluded from `_MODERN_PROCESSOR_FAMILIES` for the same reason. diff --git a/tests/unit/test_robot_manifests_match_hal_constants.py b/tests/unit/test_robot_manifests_match_hal_constants.py index fb3be92..ed7584e 100644 --- a/tests/unit/test_robot_manifests_match_hal_constants.py +++ b/tests/unit/test_robot_manifests_match_hal_constants.py @@ -19,6 +19,7 @@ - ``robots/rizon4/robot.yaml`` ↔ ``RIZON4_DESCRIPTION`` (HAL `flexiv_rizon4.py`) - ``robots/openarm/robot.yaml`` ↔ ``OPENARM_DESCRIPTION`` (HAL `openarm.py`) - ``robots/anvil_openarm_v2/robot.yaml`` ↔ ``ANVIL_OPENARM_V2_DESCRIPTION`` +- ``robots/galaxea_a1/robot.yaml`` ↔ ``GALAXEA_A1_DESCRIPTION`` Most covered YAMLs pin to the **real-hardware** ``*_REAL_DESCRIPTION`` constant because those YAMLs are the production-deployment manifests. @@ -87,6 +88,7 @@ # wrapper around Anvil's driver stack, # github.com/anvil-robotics/openarm, is a tracked follow-up). ("robots/anvil_openarm_v2/robot.yaml", "ANVIL_OPENARM_V2_DESCRIPTION"), + ("robots/galaxea_a1/robot.yaml", "GALAXEA_A1_DESCRIPTION"), ], ) def test_robot_yaml_matches_hal_description(manifest_path: str, hal_constant_attr: str) -> None: diff --git a/tests/unit/test_sensor_leg.py b/tests/unit/test_sensor_leg.py index 9abd91f..a25b385 100644 --- a/tests/unit/test_sensor_leg.py +++ b/tests/unit/test_sensor_leg.py @@ -14,7 +14,12 @@ from pathlib import Path import pytest -from openral_core import SensorDeployBinding, SensorSpec +from openral_core import ( + SensorDeployBinding, + SensorReaderBackend, + SensorReaderConfig, + SensorSpec, +) from openral_rskill_ros.sensor_leg import ( SensorLeg, _publish_rate_hz, @@ -82,6 +87,70 @@ def test_empty_leg_close_is_idempotent() -> None: assert leg.publishers == [] +def test_multi_camera_ros_publishers_start_only_after_explicit_lifecycle_gate( + monkeypatch: pytest.MonkeyPatch, +) -> None: + """All ROS entities are prepared before background publishing is allowed.""" + from openral_runner.factory import SENSOR_BACKEND_REGISTRY + from openral_sensors.ros_publisher import SensorRosPublisher + + events: list[tuple[str, str]] = [] + + class _Reader: + def __init__(self, sensor_id: str) -> None: + self.sensor_id = sensor_id + + def open(self) -> None: + events.append(("open", self.sensor_id)) + + def close(self) -> None: + events.append(("close", self.sensor_id)) + + def build_reader(config: SensorReaderConfig) -> _Reader: + return _Reader(config.sensor_id) + + backend = SensorReaderBackend.OPENCV_THREAD + monkeypatch.setitem(SENSOR_BACKEND_REGISTRY, backend.value, build_reader) + monkeypatch.setattr( + SensorRosPublisher, + "prepare", + lambda self: events.append(("prepare", self._reader.sensor_id)), + ) + monkeypatch.setattr( + SensorRosPublisher, + "start", + lambda self: events.append(("start", self._reader.sensor_id)), + ) + monkeypatch.setattr(SensorRosPublisher, "stop", lambda self: None) + + binding = SensorDeployBinding(backend=backend) + leg = open_deploy_sensor_readers( + [ + _spec("front", binding=binding), + _spec("wrist", binding=binding), + ], + ros_node=object(), + ) + try: + assert events == [ + ("open", "front"), + ("open", "wrist"), + ("prepare", "front"), + ("prepare", "wrist"), + ] + leg.start() + assert events == [ + ("open", "front"), + ("open", "wrist"), + ("prepare", "front"), + ("prepare", "wrist"), + ("start", "front"), + ("start", "wrist"), + ] + finally: + leg.close() + + def test_gstreamer_testsrc_leg_publishes_frames(tmp_path: Path) -> None: """Full leg over a real videotestsrc pipeline: open → ROS tee → frame → close. @@ -143,6 +212,7 @@ def update_image_frame(self, sensor_name, frame): aggregator = _CountingAggregator(description) leg = open_deploy_sensor_readers([spec], aggregator=aggregator) try: + leg.start() assert len(leg.readers) == 1, leg.readers # The direct-aggregator pump is registered as a publisher-shaped pump; # the sensor is recorded for WorldState's direct_image_frame_sensors. diff --git a/tests/unit/test_sensor_ros_publisher.py b/tests/unit/test_sensor_ros_publisher.py index c5c7f3f..73618fd 100644 --- a/tests/unit/test_sensor_ros_publisher.py +++ b/tests/unit/test_sensor_ros_publisher.py @@ -115,6 +115,23 @@ def test_publisher_construction_does_not_touch_ros() -> None: assert pub.info_topic == "/cam/image_raw/camera_info" +def test_publisher_accepts_external_node_without_owning_its_lifecycle() -> None: + """Composed runtimes retain node ownership across publisher stop.""" + from openral_sensors.ros_publisher import SensorRosPublisher + + reader = _FakeReader(sensor_id="wrist_rgb", frame=_make_frame()) + node = object() + pub = SensorRosPublisher( + reader=reader, + topic="/cam/image_raw", + rate_hz=30.0, + node=node, # type: ignore[arg-type] # reason: constructor is ROS-I/O-free + ) + + assert pub._node is node + assert pub._owns_node is False + + def test_publisher_start_without_rclpy_raises_runtime_error() -> None: """When rclpy is genuinely absent, start() raises RuntimeError with install hint. diff --git a/tests/unit/test_skill_runner_camera_slots.py b/tests/unit/test_skill_runner_camera_slots.py index 2d06646..52b2d47 100644 --- a/tests/unit/test_skill_runner_camera_slots.py +++ b/tests/unit/test_skill_runner_camera_slots.py @@ -19,30 +19,42 @@ from __future__ import annotations +import importlib.util from pathlib import Path +from types import ModuleType import numpy as np import pytest - -# The module under test bundles the full lifecycle node (rclpy / IDL). -# Skip cleanly when those aren't sourced — the helpers are still defined -# at module top-level, just unreachable. -pytest.importorskip("rclpy") -pytest.importorskip("openral_msgs") - -from openral_core import RobotDescription +from openral_core import RobotDescription, RSkillManifest from openral_core.exceptions import ROSRuntimeError from openral_core.schemas import FrameEncoding, SensorFrame -from openral_rskill_ros.rskill_runner_node import ( - _build_runtime_skill_from_manifest, - _decode_image_frames, - _sensor_name_to_vla_slot, - _vla_camera_slots, -) _REPO_ROOT = Path(__file__).resolve().parents[2] +def _load_skill_runner_module() -> ModuleType: + """Load the runner helpers without requiring a sourced ROS 2 workspace.""" + source = ( + _REPO_ROOT + / "packages" + / "openral_rskill_ros" + / "openral_rskill_ros" + / "rskill_runner_node.py" + ) + spec = importlib.util.spec_from_file_location("_test_camera_skill_runner_node", source) + assert spec is not None and spec.loader is not None + module = importlib.util.module_from_spec(spec) + spec.loader.exec_module(module) + return module + + +_RUNNER = _load_skill_runner_module() +_build_runtime_skill_from_manifest = _RUNNER._build_runtime_skill_from_manifest +_decode_image_frames = _RUNNER._decode_image_frames +_sensor_name_to_vla_slot = _RUNNER._sensor_name_to_vla_slot +_vla_camera_slots = _RUNNER._vla_camera_slots + + def _franka() -> RobotDescription: return RobotDescription.from_yaml(str(_REPO_ROOT / "robots" / "franka_panda" / "robot.yaml")) @@ -110,6 +122,38 @@ def test_frames_without_data_are_skipped(self) -> None: class TestBuildRuntimeSkillSceneCameras: + def test_threads_manifest_policy_extras_into_runtime_vla( + self, monkeypatch: pytest.MonkeyPatch + ) -> None: + """The deploy runner preserves adapter-owned rSkill configuration.""" + import openral_sim.factory as _sim_factory + + yaml_path = _REPO_ROOT / "rskills" / "lingbot-va-galaxea-a1-fruit-placement" / "rskill.yaml" + description = RobotDescription.from_yaml( + str(_REPO_ROOT / "robots" / "galaxea_a1" / "robot.yaml") + ) + captured: dict[str, object] = {} + + def _capture(env_cfg: object) -> object: + captured["extra"] = dict(env_cfg.vla.extra) # type: ignore[attr-defined] + raise ImportError("stop before A1 Runtime import") + + monkeypatch.setattr(_sim_factory, "make_policy", _capture) + + with pytest.raises(ROSRuntimeError): + _build_runtime_skill_from_manifest( + yaml_path=yaml_path, + prompt="put the mango into the blue bowl", + scene_cameras=("front", "wrist"), + description=description, + ) + + manifest = RSkillManifest.from_yaml(str(yaml_path)) + assert captured["extra"] == { + **manifest.policy_extras, + "latency_budget_ms": manifest.latency_budget.per_chunk_ms, + } + def test_overrides_sensor_name_scene_cameras_with_vla_slots( self, monkeypatch: pytest.MonkeyPatch ) -> None: diff --git a/tools/galaxea_a1_ros1_sidecar.py b/tools/galaxea_a1_ros1_sidecar.py new file mode 100755 index 0000000..03f3506 --- /dev/null +++ b/tools/galaxea_a1_ros1_sidecar.py @@ -0,0 +1,785 @@ +#!/usr/bin/env python3 +"""Galaxea A1 ROS 1 sidecar for :class:`openral_hal.galaxea_a1.GalaxeaA1HAL`. + +Run this file inside a ROS Noetic environment with the operator-provided A1 +SDK sourced. It owns roscore, the official serial driver, and the official +joint tracker; every exit path stops all three. No vendor code is bundled in +OpenRAL. +""" + +from __future__ import annotations + +import argparse +import contextlib +import copy +import json +import math +import os +import select +import signal +import socket +import subprocess +import sys +import threading +import time +from collections.abc import Callable, Sequence +from typing import Any + +JOINT_NAMES = tuple(f"arm_joint{i}" for i in range(1, 7)) +JOINT_LIMITS = ( + (-2.8798, 2.8798), + (0.0, 3.1415), + (-3.3161, 0.0), + (-2.8798, 2.8798), + (-1.6581, 1.6581), + (-2.8798, 2.8798), +) +PROTOCOL_VERSION = 1 +GRIPPER_STATUS_INDEX = len(JOINT_NAMES) +MAX_STATUS_MASK = 0xFFFFFFFF +TRACKER_NODE = "openral_a1_joint_tracker" +STAGED_COMMAND_TOPIC = "/openral/arm_joint_command_staged" +HOST_COMMAND_TOPIC = "/arm_joint_command_host" +TRACKER_ALLOWED_MODES = (0,) + + +def _driver_argv(serial: str) -> list[str]: + """Build the official driver launch command with every live topic explicit.""" + return [ + "roslaunch", + "signal_arm", + "single_arm_node.launch", + f"single_arm_serial_port_path:={serial}", + "single_arm_joint_states_topic:=/joint_states_host", + "single_arm_gripper_feedback_topic:=/gripper_stroke_host", + "single_arm_control_topic:=/arm_joint_command_host", + "single_arm_arm_status_topic:=/arm_status_host", + "single_arm_gripper_position_control_topic:=/gripper_position_control_host", + "single_arm_gripper_version:=G2", + ] + + +def _tracker_argv() -> list[str]: + """Run only the official joint tracker node under an OpenRAL-owned name.""" + return [ + "rosrun", + "mobiman", + "jointTracker_demo_node", + f"__name:={TRACKER_NODE}", + ] + + +def _tracker_parameters(sdk_root: str) -> dict[str, str]: + """Return the official tracker parameters with every live topic explicit. + + The current vendor ``jointTrackerdemo.launch`` declares its arguments with + ``value=`` rather than ``default=``, so ROS 1 rejects command-line + overrides. Starting the same official binary directly avoids that + SDK-version trap and also omits the unrelated EE-pose publisher. + """ + share = os.path.join(sdk_root, "install", "share", "mobiman") + private = f"/{TRACKER_NODE}" + return { + f"{private}/joint_states_sub_topic": "/joint_states_host", + f"{private}/arm_joint_command_topic": STAGED_COMMAND_TOPIC, + f"{private}/arm_joint_target_position": "/arm_joint_target_position", + f"{private}/taskFile": os.path.join(share, "config", "task.info"), + f"{private}/urdfFile": os.path.join(share, "urdf", "A1", "urdf", "A1_URDF_0607_0028.urdf"), + f"{private}/libFolder": os.path.join(share, "auto_generated", "x1_robot"), + } + + +class Stack: + """Own and tear down the ROS 1 process group in reverse start order.""" + + def __init__(self, shutdown_grace_s: float = 3.0, kill_wait_s: float = 1.0) -> None: + self._children: list[Any] = [] + self._shutdown_grace_s = shutdown_grace_s + self._kill_wait_s = kill_wait_s + + def start(self, argv: Sequence[str]) -> Any: + child = subprocess.Popen(argv, start_new_session=True) + self._children.append(child) + return child + + def stop(self) -> None: + for child in reversed(self._children): + if child.poll() is None: + with contextlib.suppress(ProcessLookupError): + os.killpg(child.pid, signal.SIGINT) + deadline = time.monotonic() + self._shutdown_grace_s + for child in reversed(self._children): + remaining = max(0.0, deadline - time.monotonic()) + try: + child.wait(timeout=remaining) + except subprocess.TimeoutExpired: + with contextlib.suppress(ProcessLookupError): + os.killpg(child.pid, signal.SIGKILL) + # An e-stop acknowledgement is meaningful only after the + # tracker/driver process has actually exited, not merely after + # SIGKILL was requested. + child.wait(timeout=self._kill_wait_s) + self._children = [] + + +class Bridge: + def __init__( + self, + rospy: Any, + joint_state_type: Callable[[], Any], + status_type: Any, + gripper_type: Callable[[], Any], + arm_control_type: Callable[[], Any], + ) -> None: + self._rospy = rospy + self._lock = threading.Lock() + self._joint: tuple[tuple[float, ...], tuple[float, ...], tuple[float, ...], int] | None = ( + None + ) + self._gripper_mm: float | None = None + self._status: tuple[int, ...] | None = None + self._joint_updated = 0.0 + self._gripper_updated = 0.0 + self._status_updated = 0.0 + self._relay_config: dict[str, Any] | None = None + self._relay_state = "LOCKED" + self._relay_fault = "" + self._activation_target: tuple[float, ...] | None = None + self._activation_deadline = 0.0 + self._published_target: tuple[float, ...] | None = None + self._published_gripper_mm: float | None = None + self._staged_position: tuple[float, ...] | None = None + self._forwarded_position: tuple[float, ...] | None = None + self._accepted_action_count = 0 + self._joint_pub = rospy.Publisher( + "/arm_joint_target_position", joint_state_type, queue_size=1 + ) + self._arm_command_pub = rospy.Publisher(HOST_COMMAND_TOPIC, arm_control_type, queue_size=1) + self._gripper_pub = rospy.Publisher( + "/gripper_position_control_host", gripper_type, queue_size=1 + ) + rospy.Subscriber("/joint_states_host", joint_state_type, self._on_joint, queue_size=1) + rospy.Subscriber("/gripper_stroke_host", joint_state_type, self._on_gripper, queue_size=1) + rospy.Subscriber("/arm_status_host", status_type, self._on_status, queue_size=1) + self._staged_sub = rospy.Subscriber( + STAGED_COMMAND_TOPIC, arm_control_type, self._on_staged, queue_size=1 + ) + self._joint_state_type = joint_state_type + self._gripper_type = gripper_type + + def configure(self, config: dict[str, Any]) -> None: + """Install the validated client contract while keeping motion locked.""" + with self._lock: + self._relay_config = config + self._relay_state = "LOCKED" + self._relay_fault = "" + self._activation_target = None + self._activation_deadline = 0.0 + self._published_target = None + self._published_gripper_mm = None + self._staged_position = None + self._forwarded_position = None + self._accepted_action_count = 0 + + def _on_joint(self, msg: Any) -> None: + try: + position = self._ordered(msg.name, msg.position, "position") + velocity = self._ordered_optional(msg.name, msg.velocity) + effort = self._ordered_optional(msg.name, msg.effort) + except ValueError as exc: + self._rospy.logerr("invalid /joint_states_host: %s", exc) + return + with self._lock: + self._joint = (position, velocity, effort, msg.header.stamp.to_nsec()) + self._joint_updated = time.monotonic() + + def _on_gripper(self, msg: Any) -> None: + if not msg.position or not math.isfinite(float(msg.position[0])): + return + with self._lock: + self._gripper_mm = float(msg.position[0]) + self._gripper_updated = time.monotonic() + + def _on_status(self, msg: Any) -> None: + codes = tuple(int(item.error_code) for item in msg.data.motor_errors) + if len(codes) < 7: + self._rospy.logerr("invalid /arm_status_host: expected 7 motors, got %d", len(codes)) + return + if any(code < 0 or code > MAX_STATUS_MASK for code in codes): + self._rospy.logerr("invalid /arm_status_host: motor code outside uint32 range") + return + with self._lock: + self._status = codes + self._status_updated = time.monotonic() + + def _on_staged(self, msg: Any) -> None: + try: + position = self._decode_tracker_command(msg) + except (AttributeError, OverflowError, TypeError, ValueError) as exc: + self._latch_relay_fault(str(exc)) + return + + now = time.monotonic() + output = None + with self._lock: + self._staged_position = position + config = self._relay_config + target = self._activation_target + joint = self._joint + status = self._status + feedback_age = now - self._joint_updated + status_age = now - self._status_updated + position_in_command_limits = all( + lower <= value <= upper for value, (lower, upper) in zip(position, JOINT_LIMITS) + ) + if not position_in_command_limits: + tolerance = config["feedback_limit_tolerance"] if config is not None else 0.0 + position_in_feedback_limits = all( + lower - tolerance <= value <= upper + tolerance + for value, (lower, upper) in zip(position, JOINT_LIMITS) + ) + # The official tracker starts its interpolation at measured + # feedback. Encoder zero/quantization can put that sample just + # beyond a nominal endpoint even though the requested target + # has already been projected to the exact command limit. + # Keep such samples staged while ARMING; they are never + # forwarded to the host command topic. Any larger excursion, + # or any out-of-range command after activation, remains a + # fail-closed relay fault. + if self._relay_state == "ARMING" and position_in_feedback_limits: + return + for name, value, (lower, upper) in zip(JOINT_NAMES, position, JOINT_LIMITS): + if not lower <= value <= upper: + self._latch_relay_fault_locked( + f"tracker command {name}={value:.6f} is outside " + f"[{lower:.6f}, {upper:.6f}]" + ) + return + if ( + config is not None + and target is not None + and joint is not None + and status is not None + and feedback_age <= config["state_timeout"] + and status_age <= config["status_timeout"] + ): + try: + self._check_status(status, config) + except RuntimeError as exc: + self._latch_relay_fault_locked(str(exc)) + else: + current = joint[0] + for name, value, lower, upper in zip( + JOINT_NAMES, + current, + config["joint_position_min"], + config["joint_position_max"], + ): + tolerance = config["feedback_limit_tolerance"] + if not lower - tolerance <= value <= upper + tolerance: + self._latch_relay_fault_locked( + f"{name} feedback {value:.6f} is outside " + f"[{lower:.6f}, {upper:.6f}] by more than " + f"{tolerance:.3f} rad" + ) + return + target_error = max(abs(a - b) for a, b in zip(position, target)) + feedback_error = max(abs(a - b) for a, b in zip(position, current)) + if ( + self._relay_state == "ARMING" + and target_error <= config["alignment"] + and feedback_error <= config["alignment"] + ): + self._relay_state = "ACTIVE" + self._rospy.logwarn( + "OpenRAL A1 relay ACTIVE after staged hold alignment " + "(target %.6f rad, feedback %.6f rad)", + target_error, + feedback_error, + ) + if self._relay_state == "ACTIVE": + output = copy.deepcopy(msg) + self._forwarded_position = position + if output is not None: + output.header.stamp = self._rospy.Time.now() + self._arm_command_pub.publish(output) + + @staticmethod + def _decode_tracker_command(msg: Any) -> tuple[float, ...]: + vectors = { + "p_des": msg.p_des, + "v_des": msg.v_des, + "kp": msg.kp, + "kd": msg.kd, + "t_ff": msg.t_ff, + } + decoded: dict[str, tuple[float, ...]] = {} + for label, values in vectors.items(): + if len(values) != len(JOINT_NAMES): + raise ValueError( + f"tracker command {label} has {len(values)} values, " + f"need exactly {len(JOINT_NAMES)}" + ) + decoded[label] = tuple(float(value) for value in values) + if not all(math.isfinite(value) for value in decoded[label]): + raise ValueError(f"tracker command {label} contains non-finite values") + if any(value < 0.0 for value in decoded["kp"]): + raise ValueError("tracker command kp contains negative gains") + if any(value < 0.0 for value in decoded["kd"]): + raise ValueError("tracker command kd contains negative gains") + if isinstance(msg.mode, bool) or not isinstance(msg.mode, int): + raise ValueError(f"tracker command mode is not an integer: {msg.mode!r}") + if msg.mode not in TRACKER_ALLOWED_MODES: + raise ValueError( + f"tracker command mode {msg.mode} is not allowed; " + f"expected one of {list(TRACKER_ALLOWED_MODES)}" + ) + return decoded["p_des"] + + def _latch_relay_fault(self, reason: str) -> None: + with self._lock: + self._latch_relay_fault_locked(reason) + + def _latch_relay_fault_locked(self, reason: str) -> None: + if not self._relay_fault: + self._relay_fault = reason + self._relay_state = "FAULT" + self._rospy.logerr("OpenRAL A1 relay FAULT: %s", reason) + + @staticmethod + def _ordered(names: Sequence[str], values: Sequence[float], label: str) -> tuple[float, ...]: + if len(values) < len(JOINT_NAMES): + raise ValueError(f"{label} has {len(values)} values") + if names: + if len(names) != len(set(names)): + raise ValueError("duplicate joint names") + by_name = dict(zip(names, values)) + missing = [name for name in JOINT_NAMES if name not in by_name] + if missing: + raise ValueError(f"missing joints {missing!r}") + result = tuple(float(by_name[name]) for name in JOINT_NAMES) + else: + result = tuple(float(value) for value in values[: len(JOINT_NAMES)]) + if not all(math.isfinite(value) for value in result): + raise ValueError(f"{label} contains non-finite values") + return result + + @classmethod + def _ordered_optional(cls, names: Sequence[str], values: Sequence[float]) -> tuple[float, ...]: + if not values: + return () + return cls._ordered(names, values, "optional vector") + + def ready(self) -> bool: + with self._lock: + return ( + self._joint is not None + and self._gripper_mm is not None + and self._status is not None + ) + + def command_paths_ready(self) -> bool: + """Return whether staged and host command consumers are connected.""" + return bool( + self._joint_pub.get_num_connections() > 0 + and self._staged_sub.get_num_connections() > 0 + and self._arm_command_pub.get_num_connections() > 0 + and self._gripper_pub.get_num_connections() > 0 + ) + + def relay_state(self) -> str: + with self._lock: + return self._relay_state + + def state(self, config: dict[str, Any]) -> dict[str, Any]: + with self._lock: + joint = self._joint + gripper_mm = self._gripper_mm + status = self._status + updated = (self._joint_updated, self._gripper_updated, self._status_updated) + relay_state = self._relay_state + relay_fault = self._relay_fault + activation_deadline = self._activation_deadline + staged_position = self._staged_position + forwarded_position = self._forwarded_position + accepted_action_count = self._accepted_action_count + if joint is None or gripper_mm is None or status is None: + raise RuntimeError("hardware feedback is incomplete") + now = time.monotonic() + if relay_fault: + raise RuntimeError(f"command relay fault: {relay_fault}") + if relay_state == "ARMING" and now > activation_deadline: + self._latch_relay_fault( + f"tracker did not align its staged hold within " + f"{config['tracker_alignment_timeout']:.3f} s" + ) + raise RuntimeError("command relay fault: tracker staged-hold alignment timed out") + if now - updated[0] > config["state_timeout"]: + raise RuntimeError("joint feedback is stale") + if now - updated[1] > config["state_timeout"]: + raise RuntimeError("gripper feedback is stale") + if now - updated[2] > config["status_timeout"]: + raise RuntimeError("motor status is stale") + position, velocity, effort, stamp_ns = joint + for name, value, lower, upper in zip( + JOINT_NAMES, + position, + config["joint_position_min"], + config["joint_position_max"], + ): + tolerance = config["feedback_limit_tolerance"] + if not lower - tolerance <= value <= upper + tolerance: + raise RuntimeError( + f"{name} feedback {value:.6f} is outside [{lower:.6f}, {upper:.6f}] " + f"by more than {tolerance:.3f} rad" + ) + self._check_status(status, config) + gripper_norm = (gripper_mm - config["gripper_min"]) / ( + config["gripper_max"] - config["gripper_min"] + ) + if not 0.0 <= gripper_norm <= 1.0: + raise RuntimeError(f"gripper feedback {gripper_mm:.3f} mm is outside configured range") + return { + "op": "state", + "name": list(JOINT_NAMES), + "position": list(position), + "velocity": list(velocity), + "effort": list(effort), + "stamp_ns": int(stamp_ns), + "status_codes": list(status), + "gripper_norm": gripper_norm, + "relay_state": relay_state, + "staged_position": list(staged_position or ()), + "forwarded_position": list(forwarded_position or ()), + "accepted_action_count": accepted_action_count, + } + + def apply( + self, packet: dict[str, Any], config: dict[str, Any], first_joint_command: bool + ) -> bool: + state = self.state(config) + self._check_status(state["status_codes"], config) + target = packet.get("joint_targets") + if target is not None: + if not isinstance(target, list) or len(target) != len(JOINT_NAMES): + raise ValueError("joint_targets must contain six values") + target = tuple(float(value) for value in target) + if not all(math.isfinite(value) for value in target): + raise ValueError("joint target contains a non-finite value") + for name, value, lower, upper in zip( + JOINT_NAMES, + target, + config["joint_position_min"], + config["joint_position_max"], + ): + if not lower <= value <= upper: + raise ValueError( + f"{name} target {value:.6f} is outside [{lower:.6f}, {upper:.6f}]" + ) + with self._lock: + relay_state = self._relay_state + bound = ( + config["alignment"] + if first_joint_command or relay_state == "ARMING" + else config["max_step"] + ) + if any(abs(target[i] - state["position"][i]) > bound for i in range(len(JOINT_NAMES))): + raise ValueError(f"joint target exceeds {bound:.3f} rad feedback-relative bound") + if first_joint_command: + with self._lock: + self._relay_state = "ARMING" + self._activation_target = target + self._activation_deadline = ( + time.monotonic() + config["tracker_alignment_timeout"] + ) + elif relay_state == "ARMING": + # Closed-loop IK recomputes a feedback-relative substep on + # every tick, so encoder noise can move the target slightly + # while the official tracker is still converging. Keep the + # original bounded deadline, but align against the latest + # target. Every ARMING target remains subject to the tighter + # initial-alignment bound above. + with self._lock: + self._activation_target = target + elif relay_state != "ACTIVE": + raise RuntimeError(f"command relay is not ACTIVE: {relay_state}") + with self._lock: + publish_target = self._published_target != target + if publish_target: + self._published_target = target + if publish_target: + msg = self._joint_state_type() + msg.header.stamp = self._rospy.Time.now() + msg.header.frame_id = "base_link" + msg.name = list(JOINT_NAMES) + msg.position = list(target) + self._joint_pub.publish(msg) + first_joint_command = False + gripper = packet.get("gripper") + if gripper is not None: + gripper = float(gripper) + if not math.isfinite(gripper) or not 0.0 <= gripper <= 1.0: + raise ValueError("gripper must be within [0, 1]") + with self._lock: + gripper_relay_state = self._relay_state + if gripper_relay_state != "ACTIVE": + # The gripper bypasses the tracker's staged-hold interpolation, + # so it must not actuate before the arm relay has finished + # alignment. Same fail-closed contract as the joint path above; + # the deploy flow always streams the joint hold to ACTIVE + # before the first gripper setpoint. + raise RuntimeError( + f"gripper command refused: command relay is not ACTIVE: {gripper_relay_state}" + ) + gripper_mm = config["gripper_min"] + gripper * ( + config["gripper_max"] - config["gripper_min"] + ) + with self._lock: + publish_gripper = self._published_gripper_mm != gripper_mm + if publish_gripper: + self._published_gripper_mm = gripper_mm + if publish_gripper: + msg = self._gripper_type() + msg.header.stamp = self._rospy.Time.now() + msg.gripper_stroke = gripper_mm + self._gripper_pub.publish(msg) + if target is None and gripper is None: + raise ValueError("action has no joint_targets or gripper") + with self._lock: + self._accepted_action_count += 1 + return first_joint_command + + @staticmethod + def _check_status(codes: Sequence[int], config: dict[str, Any]) -> None: + for index, code in enumerate(codes[: GRIPPER_STATUS_INDEX + 1]): + allowed = config["idle_mask"] | ( + config["gripper_mask"] if index == GRIPPER_STATUS_INDEX else 0 + ) + if int(code) & ~allowed: + raise RuntimeError(f"actuator {index + 1} reports error code {code}") + + +def _send(conn: socket.socket, message: dict[str, Any]) -> None: + conn.sendall(json.dumps(message, separators=(",", ":"), allow_nan=False).encode() + b"\n") + + +def _hello_config(packet: dict[str, Any]) -> dict[str, Any]: + if packet.get("op") != "hello" or packet.get("protocol") != PROTOCOL_VERSION: + raise ValueError("expected OpenRAL Galaxea A1 protocol hello v1") + if packet.get("robot") != "galaxea_a1": + raise ValueError("robot identity mismatch") + if packet.get("joint_names") != list(JOINT_NAMES): + raise ValueError("joint_names mismatch") + joint_min = tuple(float(value) for value in packet["joint_position_min"]) + joint_max = tuple(float(value) for value in packet["joint_position_max"]) + if len(joint_min) != len(JOINT_NAMES) or len(joint_max) != len(JOINT_NAMES): + raise ValueError("joint position limits must contain six lower and upper values") + if any( + not math.isfinite(lower) or not math.isfinite(upper) or lower >= upper + for lower, upper in zip(joint_min, joint_max) + ): + raise ValueError("joint position limits must be finite ordered pairs") + if tuple(zip(joint_min, joint_max)) != JOINT_LIMITS: + raise ValueError("joint position limits do not match the sidecar's official A1 limits") + idle_mask = packet["idle_timeout_error_mask"] + gripper_mask = packet["gripper_ignored_error_mask"] + if any( + isinstance(value, bool) or not isinstance(value, int) for value in (idle_mask, gripper_mask) + ): + raise ValueError("motor status masks must be integers") + config: dict[str, Any] = { + "alignment": float(packet["initial_alignment_tolerance_rad"]), + "max_step": float(packet["max_target_step_rad"]), + "lease": float(packet["command_lease_s"]), + "state_timeout": float(packet["state_timeout_s"]), + "status_timeout": float(packet["status_timeout_s"]), + "feedback_limit_tolerance": float(packet["feedback_limit_tolerance_rad"]), + "tracker_alignment_timeout": float(packet["tracker_alignment_timeout_s"]), + "idle_mask": idle_mask, + "gripper_mask": gripper_mask, + "gripper_min": float(packet["gripper_stroke_min_mm"]), + "gripper_max": float(packet["gripper_stroke_max_mm"]), + "joint_position_min": joint_min, + "joint_position_max": joint_max, + } + positive = ( + "alignment", + "max_step", + "lease", + "state_timeout", + "status_timeout", + "tracker_alignment_timeout", + ) + if any(not math.isfinite(config[key]) or config[key] <= 0.0 for key in positive): + raise ValueError("motion limits and command lease must be positive") + if ( + not math.isfinite(config["feedback_limit_tolerance"]) + or config["feedback_limit_tolerance"] < 0.0 + ): + raise ValueError("feedback limit tolerance must be finite and non-negative") + if ( + not math.isfinite(config["gripper_min"]) + or not math.isfinite(config["gripper_max"]) + or config["gripper_max"] <= config["gripper_min"] + ): + raise ValueError("invalid gripper range") + if any( + isinstance(config[key], bool) or not 0 <= config[key] <= MAX_STATUS_MASK + for key in ("idle_mask", "gripper_mask") + ): + raise ValueError("motor status masks must be uint32 values") + return config + + +def _serve( + listener: socket.socket, + bridge: Bridge, + rospy: Any, + stop_stack: Callable[[], None], +) -> None: + conn, address = listener.accept() + rospy.loginfo("OpenRAL client connected from %s:%d", *address) + conn.setblocking(False) + buffer = b"" + config = None + last_command = None + first_joint_command = True + last_state_send = 0.0 + while not rospy.is_shutdown(): + readable, _, _ = select.select([conn], [], [], 0.01) + if readable: + chunk = conn.recv(65536) + if not chunk: + return + buffer += chunk + while b"\n" in buffer: + line, buffer = buffer.split(b"\n", 1) + if not line: + continue + packet = json.loads(line) + if config is None: + config = _hello_config(packet) + bridge.configure(config) + _send( + conn, + {"op": "hello", "protocol": PROTOCOL_VERSION, "robot": "galaxea_a1"}, + ) + elif packet.get("op") == "action": + first_joint_command = bridge.apply(packet, config, first_joint_command) + last_command = time.monotonic() + elif packet.get("op") == "estop": + stop_stack() + _send(conn, {"op": "estopped"}) + return + else: + raise ValueError(f"unsupported operation {packet.get('op')!r}") + # Drain a readable packet before evaluating the lease. The ROS tracker + # callback may briefly starve this thread while the relay transitions + # ARMING -> ACTIVE; current actions or an e-stop can already be buffered + # on the socket at that point. Checking first would fault on an old + # timestamp without consuming the fresh safety input. + now = time.monotonic() + if ( + config is not None + and last_command is not None + and bridge.relay_state() == "ACTIVE" + and now - last_command > config["lease"] + ): + raise RuntimeError("command lease expired after %.3f s" % (now - last_command)) + if config is not None and now - last_state_send >= 0.02: + _send(conn, bridge.state(config)) + last_state_send = now + + +def _wait_for(predicate: Callable[[], bool], timeout_s: float, label: str) -> None: + deadline = time.monotonic() + timeout_s + while time.monotonic() < deadline: + if predicate(): + return + time.sleep(0.05) + raise RuntimeError(f"timed out waiting for {label}") + + +def _port_is_open(host: str, port: int) -> bool: + with socket.socket() as probe: + probe.settimeout(0.1) + return probe.connect_ex((host, port)) == 0 + + +def main() -> int: + parser = argparse.ArgumentParser() + parser.add_argument("--bind", default="127.0.0.1") + parser.add_argument("--port", type=int, default=46011) + parser.add_argument("--serial", default="/dev/a1") + parser.add_argument("--startup-timeout-s", type=float, default=20.0) + args = parser.parse_args() + + stack = Stack() + listener = None + try: + if _port_is_open("127.0.0.1", 11311): + raise RuntimeError("ROS master port 11311 is already occupied inside the sidecar") + roscore = stack.start(["roscore"]) + _wait_for( + lambda: roscore.poll() is None and _port_is_open("127.0.0.1", 11311), + args.startup_timeout_s, + "ROS master", + ) + import rospy # type: ignore[import-not-found] # reason: provided by ROS 1 Noetic sidecar + from sensor_msgs.msg import JointState + from signal_arm.msg import ( # type: ignore[import-not-found] # reason: operator-provided vendor SDK + arm_control, + gripper_position_control, + status_stamped, + ) + + rospy.init_node("openral_galaxea_a1_sidecar", disable_signals=True) + bridge = Bridge( + rospy, + JointState, + status_stamped, + gripper_position_control, + arm_control, + ) + driver = stack.start(_driver_argv(args.serial)) + _wait_for( + lambda: driver.poll() is None and bridge.ready(), + args.startup_timeout_s, + "fresh A1 feedback and motor status", + ) + tracker_parameters = _tracker_parameters(os.environ["A1_SDK_ROOT"]) + missing_tracker_assets = [ + value + for key, value in tracker_parameters.items() + if key.endswith(("taskFile", "urdfFile", "libFolder")) and not os.path.exists(value) + ] + if missing_tracker_assets: + raise RuntimeError(f"A1 SDK tracker assets are missing: {missing_tracker_assets!r}") + for name, value in tracker_parameters.items(): + rospy.set_param(name, value) + tracker = stack.start(_tracker_argv()) + _wait_for( + lambda: tracker.poll() is None and bridge.command_paths_ready(), + args.startup_timeout_s, + "A1 joint tracker and gripper command subscribers", + ) + listener = socket.socket(socket.AF_INET, socket.SOCK_STREAM) + listener.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) + listener.bind((args.bind, args.port)) + listener.listen(1) + print(f"OpenRAL Galaxea A1 sidecar ready on {args.bind}:{args.port}", flush=True) + _serve(listener, bridge, rospy, stack.stop) + return 0 + except KeyboardInterrupt: + return 130 + except Exception as exc: + print(f"Galaxea A1 sidecar fault: {exc}", file=sys.stderr, flush=True) + return 1 + finally: + if listener is not None: + listener.close() + stack.stop() + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/tools/run_galaxea_a1_sidecar.sh b/tools/run_galaxea_a1_sidecar.sh new file mode 100755 index 0000000..70a8ada --- /dev/null +++ b/tools/run_galaxea_a1_sidecar.sh @@ -0,0 +1,134 @@ +#!/usr/bin/env bash +set -euo pipefail + +usage() { + echo "usage: $0 --image IMAGE --sdk-root ABS_PATH [--serial /dev/a1] [--port 46011] [--check-only]" >&2 +} + +image="" +sdk_root="" +serial="/dev/a1" +port="46011" +check_only="false" +while (( $# > 0 )); do + case "$1" in + --image) image="${2:-}"; shift 2 ;; + --sdk-root) sdk_root="${2:-}"; shift 2 ;; + --serial) serial="${2:-}"; shift 2 ;; + --port) port="${2:-}"; shift 2 ;; + --check-only) check_only="true"; shift ;; + *) usage; exit 2 ;; + esac +done + +if [[ -z "${image}" || "${sdk_root}" != /* || ! -f "${sdk_root}/install/setup.bash" ]]; then + usage + exit 2 +fi +if [[ ! -e "${serial}" ]]; then + echo "serial device does not exist: ${serial}" >&2 + exit 2 +fi +if [[ ! "${port}" =~ ^[0-9]+$ ]] || (( port < 1 || port > 65535 )); then + echo "sidecar port must be an integer in [1, 65535]: ${port}" >&2 + exit 2 +fi +if ! command -v docker >/dev/null 2>&1; then + echo "docker is not installed or not on PATH" >&2 + exit 1 +fi +if ! docker image inspect "${image}" >/dev/null 2>&1; then + echo "sidecar image is not available locally: ${image}" >&2 + exit 1 +fi +if [[ ! -f "${sdk_root}/install/share/signal_arm/launch/single_arm_node.launch" ]]; then + echo "A1 SDK is missing signal_arm/single_arm_node.launch: ${sdk_root}" >&2 + exit 1 +fi +tracker_binary="${sdk_root}/install/lib/mobiman/jointTracker_demo_node" +tracker_share="${sdk_root}/install/share/mobiman" +tracker_source="${tracker_share}/auto_generated/x1_robot" +for required in \ + "${tracker_binary}" \ + "${tracker_share}/config/task.info" \ + "${tracker_share}/urdf/A1/urdf/A1_URDF_0607_0028.urdf" \ + "${tracker_source}"; do + if [[ ! -e "${required}" ]]; then + echo "A1 SDK tracker dependency is missing: ${required}" >&2 + exit 1 + fi +done +if command -v fuser >/dev/null 2>&1 && fuser "${serial}" >/dev/null 2>&1; then + echo "serial device is already owned by another process: ${serial}" >&2 + exit 1 +fi +if command -v ss >/dev/null 2>&1 && ss -H -ltn "sport = :${port}" | grep -q .; then + echo "sidecar TCP port is already listening on the host: ${port}" >&2 + exit 1 +fi +if docker container inspect openral-galaxea-a1-sidecar >/dev/null 2>&1; then + echo "container name is already in use: openral-galaxea-a1-sidecar" >&2 + exit 1 +fi + +lock_name="${serial//\//_}" +lock_path="/tmp/openral-galaxea-a1-${lock_name}.lock" +exec 9>"${lock_path}" +if command -v flock >/dev/null 2>&1 && ! flock -n 9; then + echo "another OpenRAL A1 sidecar launcher holds ${lock_path}" >&2 + exit 1 +fi + +cache_base="${XDG_CACHE_HOME:-${HOME}/.cache}" +if [[ "${cache_base}" != /* ]]; then + echo "XDG_CACHE_HOME must be absolute when set: ${cache_base}" >&2 + exit 2 +fi +tracker_cache_parent="${cache_base}/openral/galaxea-a1" +tracker_cache="${tracker_cache_parent}/x1_robot" +cache_probe="${tracker_cache_parent}" +while [[ ! -e "${cache_probe}" ]]; do + cache_probe="$(dirname "${cache_probe}")" +done +if [[ ! -d "${cache_probe}" || ! -w "${cache_probe}" ]]; then + echo "tracker cache parent is not writable: ${cache_probe}" >&2 + exit 1 +fi + +if [[ "${check_only}" == "true" ]]; then + echo "Galaxea A1 sidecar preflight passed" + echo " image: ${image}" + echo " SDK: ${sdk_root}" + echo " tracker cache: ${tracker_cache}" + echo " serial: ${serial}" + echo " TCP: 127.0.0.1:${port}" + exit 0 +fi + +if [[ ! -d "${tracker_cache}" ]]; then + mkdir -p "${tracker_cache_parent}" + tracker_cache_seed="$(mktemp -d "${tracker_cache_parent}/.x1_robot.seed.XXXXXX")" + cp -a "${tracker_source}/." "${tracker_cache_seed}/" + mv "${tracker_cache_seed}" "${tracker_cache}" +fi +if [[ ! -w "${tracker_cache}" ]]; then + echo "tracker cache is not writable: ${tracker_cache}" >&2 + exit 1 +fi + +repo_root="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)" +exec docker run --rm --name openral-galaxea-a1-sidecar \ + --network bridge \ + --publish "127.0.0.1:${port}:${port}" \ + --device "${serial}:${serial}" \ + -e A1_SDK_ROOT=/opt/galaxea/A1_SDK \ + -e ROS_MASTER_URI=http://127.0.0.1:11311 \ + -e ROS_HOSTNAME=127.0.0.1 \ + -v "${sdk_root}:/opt/galaxea/A1_SDK:ro" \ + -v "${tracker_cache}:/opt/galaxea/A1_SDK/install/share/mobiman/auto_generated/x1_robot:rw" \ + -v "${repo_root}/tools/galaxea_a1_ros1_sidecar.py:/opt/openral/galaxea_a1_ros1_sidecar.py:ro" \ + "${image}" \ + python3 /opt/openral/galaxea_a1_ros1_sidecar.py \ + --bind 0.0.0.0 \ + --serial "${serial}" \ + --port "${port}" diff --git a/uv.lock b/uv.lock index 9c979df..aaa6780 100644 --- a/uv.lock +++ b/uv.lock @@ -2878,6 +2878,7 @@ name = "openral-runner" version = "0.1.0" source = { editable = "python/runner" } dependencies = [ + { name = "msgpack" }, { name = "openral-core" }, { name = "openral-hal" }, { name = "openral-observability" }, @@ -2896,6 +2897,7 @@ opencv = [ [package.metadata] requires-dist = [ + { name = "msgpack", specifier = ">=1.1" }, { name = "opencv-python", marker = "extra == 'opencv'", specifier = ">=4.8" }, { name = "openral-core", editable = "python/core" }, { name = "openral-hal", editable = "python/hal" }, @@ -3040,6 +3042,7 @@ libero = [ lingbot = [ { name = "msgpack" }, { name = "pyzmq" }, + { name = "websockets" }, ] locateanything = [ { name = "msgpack" }, @@ -3181,6 +3184,7 @@ libero = [ lingbot = [ { name = "msgpack", specifier = ">=1" }, { name = "pyzmq", specifier = ">=25" }, + { name = "websockets", specifier = ">=15" }, ] locateanything = [ { name = "msgpack", specifier = ">=1" },