diff --git a/.github/docker/message-codegen-reference.Dockerfile b/.github/docker/message-codegen-reference.Dockerfile index 69770075d64..aecca8405d1 100644 --- a/.github/docker/message-codegen-reference.Dockerfile +++ b/.github/docker/message-codegen-reference.Dockerfile @@ -1,7 +1,7 @@ FROM ros:jazzy-ros-base RUN apt-get update && apt-get install -y --no-install-recommends \ - build-essential cmake python3-colcon-common-extensions \ + build-essential cmake python3-colcon-common-extensions python3-pytest python3-typing-extensions \ ros-jazzy-common-interfaces ros-jazzy-rosidl-default-generators \ && rm -rf /var/lib/apt/lists/* diff --git a/.github/scripts/message_wheel_smoke.py b/.github/scripts/message_wheel_smoke.py new file mode 100644 index 00000000000..2b8199e3ac7 --- /dev/null +++ b/.github/scripts/message_wheel_smoke.py @@ -0,0 +1,33 @@ +#!/usr/bin/env python3 +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Verify the standalone built-in wheel without installing the robot runtime.""" + +import json +from pathlib import Path + +import dimos_generated as messages +from dimos_generated_schemas.provider import package_root + +assert messages.__dimos_version__ == "0.1.0" +header = messages.std_msgs.msg.Header() +assert type(header).decode(header.encode()).encode() == header.encode() +root = Path(package_root()) +metadata = json.loads((root / "message-package.json").read_text()) +assert metadata["shared"] +assert "std_msgs/msg/Header" in metadata["owned"] +assert (root / "schemas/std_msgs/msg/Header.msg").is_file() +assert (root / "lib/cmake/dimos_generated/dimos_generatedConfig.cmake").is_file() +assert (root / "rust/Cargo.toml").is_file() diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 54b683c5137..c52093aa0fb 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -277,9 +277,17 @@ jobs: with: enable-cache: true prune-cache: true + - name: Build standalone CDR dependency + run: bash scripts/setup_message_codegen.sh - name: Install lint dependencies run: uv sync --only-group lint --frozen + - name: Install native build typing dependencies + run: uv pip install --no-deps pybind11==3.0.1 types-setuptools==77.0.2.20250328 + - name: Generate CDR typing package + run: uv run python -m dimos.message_codegen.generate --output build/messages - name: Mypy + env: + MYPYPATH: build/messages/typing run: uv run mypy # The deno-fmt pre-commit hook needs deno on PATH. @@ -333,6 +341,8 @@ jobs: run: | sudo apt-get update sudo apt-get install -y portaudio19-dev + - name: Build standalone CDR dependency + run: bash scripts/setup_message_codegen.sh - name: Build and test PyO3 bindings # PyO3 extension modules (cdylib + `extension-module`) are the one case # that still needs a plain build: pytest imports the compiled .so. @@ -374,7 +384,7 @@ jobs: sudo apt-get update # lcm.pc carries `Requires: glib-2.0`, so pkg-config cannot resolve # lcm at all without glib's own .pc file - sudo apt-get install -y liblcm-dev libglib2.0-dev nlohmann-json3-dev + sudo apt-get install -y liblcm-dev libglib2.0-dev nlohmann-json3-dev portaudio19-dev libturbojpeg base=https://github.com/eclipse-zenoh curl -fsSL -o /tmp/zenoh-c.zip \ "$base/zenoh-c/releases/download/$ZENOH_VERSION/zenoh-c-$ZENOH_VERSION-x86_64-unknown-linux-gnu-standalone.zip" @@ -390,6 +400,11 @@ jobs: sudo unzip -q /tmp/zenoh-c.zip -d /usr/local sudo unzip -q /tmp/zenoh-cpp.zip -d /usr/local sudo ldconfig + - name: Generate native CDR message package + run: | + bash scripts/setup_message_codegen.sh + bash scripts/install_native_messages.sh + echo "CMAKE_PREFIX_PATH=$PWD/build/message-codegen/install" >> "$GITHUB_ENV" - name: Configure run: cmake -S native/cpp -B build/native-cpp -DDIMOS_NATIVE_BUILD_TESTS=ON - name: Build @@ -397,6 +412,27 @@ jobs: - name: Test run: ctest --test-dir build/native-cpp --output-on-failure + - uses: astral-sh/setup-uv@v10.0.1 + - name: Verify the documented external three-language project + run: | + uv sync --group tests --frozen + cmake --install build/native-cpp --prefix "$PWD/build/native-sdk" + uv run python scripts/package_native_sdk.py --output examples/message-project/sdk + export CMAKE_PREFIX_PATH="$PWD/build/native-sdk:$CMAKE_PREFIX_PATH" + uv run dimos build --project examples/message-project + uv pip install --no-deps examples/message-project/dist/story_messages-*.whl + (cd examples/message-project/cpp && cmake --preset dimos && cmake --build --preset dimos) + cargo build --locked --manifest-path examples/message-project/rust/Cargo.toml + uv run python examples/message-project/demo_blueprint.py --transport zenoh + - name: Retain custom message and native SDK review artifacts + uses: actions/upload-artifact@v7 + with: + name: external-message-project + path: | + examples/message-project/dist/ + examples/message-project/sdk/ + build/native-sdk/ + - name: Find C++ native modules id: find-modules run: | @@ -481,6 +517,8 @@ jobs: - uses: actions/setup-node@v7 with: node-version: 'lts/*' + - name: Build standalone CDR dependency + run: bash scripts/setup_message_codegen.sh - name: Install Python dependencies run: uv sync --group tests --frozen - name: Restore Hugging Face model cache @@ -603,8 +641,6 @@ jobs: run: | sudo apt-get update sudo apt-get install -y portaudio19-dev libturbojpeg ffmpeg - - name: Fetch the go2_short replay dataset - run: git lfs pull --include="data/.lfs/go2_short.db.tar.gz" --exclude="" - name: Build standalone CDR dependency run: bash scripts/setup_message_codegen.sh - name: Install dependencies @@ -632,7 +668,8 @@ jobs: contains(fromJSON('["success", "skipped"]'), needs.cachix-build-check.result) && contains(fromJSON('["success", "skipped"]'), needs.cachix-build.result) needs: [cachix-build-check, cachix-build] - timeout-minutes: 20 + # Cold native/Python setup can consume nine minutes before pytest starts. + timeout-minutes: 30 strategy: matrix: pyver: ['3.10', '3.11', '3.12'] @@ -715,8 +752,17 @@ jobs: run: bash scripts/setup_message_codegen.sh - name: Install dependencies run: uv sync --group tests --frozen + - name: Install CDR conformance test dependencies + run: uv pip install mcap==1.4.0 rosbags==0.11.0 mypy==1.19.1 pybind11==3.0.1 + - name: Install Rust for offline planner bindings + uses: dtolnay/rust-toolchain@stable + - name: Build offline planner Python bindings + run: | + source .venv/bin/activate + maturin develop -m dimos/mapping/ray_tracing/rust/py/Cargo.toml + maturin develop -m dimos/navigation/nav_3d/mls_planner/rust/py/Cargo.toml - name: Run tests - run: uv run pytest --numprocesses=logical --cov=dimos/ --junitxml=junit.xml -m 'not (self_hosted or mujoco or self_hosted_large or web_browser or bake_e2e or native_e2e)' + run: uv run pytest --numprocesses=logical --cov=dimos/ --junitxml=junit.xml -o faulthandler_timeout=120 -m 'not (self_hosted or mujoco or self_hosted_large or web_browser or bake_e2e or native_e2e)' - name: Re-run the failing tests with maximum verbosity if: failure() env: diff --git a/.github/workflows/message-codegen.yml b/.github/workflows/message-codegen.yml index ef68bc1f6ed..3ce4e45326f 100644 --- a/.github/workflows/message-codegen.yml +++ b/.github/workflows/message-codegen.yml @@ -5,12 +5,17 @@ on: paths: - dimos/message_codegen/** - examples/message-codegen/** + - examples/message-project/** + - packages/dimos-generated/** + - docs/development/messages.md - native/rust/dimos-lcm-transport/** - scripts/*message*codegen* - scripts/test_message_packages.sh + - scripts/package_messages.sh - scripts/test_message_mcap.sh - dimos/protocol/cdr_mcap.py - dimos/protocol/test_cdr_mcap.py + - dimos/protocol/pubsub/impl/*ros*.py - scripts/vendor_message_definitions.py - .github/workflows/message-codegen.yml - .github/docker/message-codegen-reference.Dockerfile @@ -35,7 +40,7 @@ jobs: run: | test ! -d /opt/ros uv venv .venv --python 3.12 - uv pip install --python .venv/bin/python pybind11==3.0.1 rosbags==0.11.0 numpy pytest pytest-asyncio pytest-env lcm-dimos-fork setuptools wheel mcap==1.4.0 rerun-sdk==0.32.0 pillow + uv pip install --python .venv/bin/python pybind11==3.0.1 rosbags==0.11.0 numpy PyYAML==6.0.3 pytest pytest-asyncio pytest-env lcm-dimos-fork setuptools wheel mcap==1.4.0 rerun-sdk==0.32.0 pillow mypy==1.19.0 .venv/bin/python -c "import importlib.util; assert importlib.util.find_spec('rclpy') is None" - name: Build the pinned standalone CDR library run: bash scripts/setup_message_codegen.sh @@ -43,6 +48,37 @@ jobs: run: bash scripts/test_message_codegen.sh - name: Build and consume wheel, sdist, CMake, and Cargo packages run: bash scripts/test_message_packages.sh + - name: Build standalone message authoring and built-in packages + run: | + uv build dimos/message_codegen --out-dir build/message-codegen/ux-wheelhouse + CMAKE_PREFIX_PATH="$PWD/build/message-codegen/install" uv build packages/dimos-generated --out-dir build/message-codegen/ux-wheelhouse + uv venv --seed build/message-codegen/download-env + build/message-codegen/download-env/bin/python -m pip download --only-binary=:all: --dest build/message-codegen/ux-wheelhouse setuptools wheel packaging pybind11==3.0.1 cmake + - name: Verify custom package ownership, rebuilds, and isolated pip installs + env: + DIMOS_FASTCDR_PREFIX: ${{ github.workspace }}/build/message-codegen/install + DIMOS_MESSAGE_WHEELHOUSE: ${{ github.workspace }}/build/message-codegen/ux-wheelhouse + run: | + .venv/bin/python -m pytest -o addopts="" --noconftest -q \ + dimos/message_codegen/test_project.py \ + dimos/message_codegen/test_documented_project.py \ + dimos/message_codegen/test_ownership.py \ + dimos/message_codegen/test_ownership_native.py \ + dimos/message_codegen/test_project_install.py + - name: Package matching built-in CMake and Rust artifacts + run: bash scripts/package_messages.sh + - name: Consume relocated built-in native packages + env: + DIMOS_BUILTIN_PACKAGE: ${{ github.workspace }}/build/message-codegen/release + DIMOS_FASTCDR_PREFIX: ${{ github.workspace }}/build/message-codegen/install + run: .venv/bin/python -m pytest -o addopts="" --noconftest -q dimos/message_codegen/test_builtin_artifacts.py + - name: Retain standalone message build artifacts + uses: actions/upload-artifact@v7 + with: + name: message-authoring-packages + path: | + build/message-codegen/ux-wheelhouse/dimos* + build/message-codegen/release/dist/* - name: Record generated CDR and verify both viewer decoders run: bash scripts/test_message_mcap.sh - name: Retain self-contained recording and viewer evidence @@ -61,6 +97,12 @@ jobs: build/message-codegen/external-app/evidence/ build/message-codegen/external-app/dist/ build/message-codegen/external-app/generated/rust/target/package/*.crate + - name: Retain the standalone Python binding for ROS bridge verification + uses: actions/upload-artifact@v7 + with: + name: message-codegen-python-binding + path: build/message-codegen/demo/cpp/build/dimos_generated*.so + if-no-files-found: error - name: Retain executable demo evidence if: always() uses: actions/upload-artifact@v7 @@ -78,6 +120,10 @@ jobs: with: name: message-codegen-evidence path: build/message-codegen/demo/evidence/ + - uses: actions/download-artifact@v8 + with: + name: message-codegen-python-binding + path: build/message-codegen/demo/cpp/build/ - name: Build independent ROS2 reference environment run: docker build -f .github/docker/message-codegen-reference.Dockerfile -t dimos-cdr-jazzy-reference .github/docker - name: Compare native payloads with independently generated ROS2 types @@ -95,6 +141,18 @@ jobs: source install/setup.bash python3 /source/examples/message-codegen/demo_ros_reference.py --build /demo ' | tee build/message-codegen/demo/evidence/jazzy-reference.txt + - name: Verify generated messages through the optional ROS bridge + run: | + docker run --rm -v "$PWD:/source:ro" dimos-cdr-jazzy-reference ' + set -e + source /opt/ros/jazzy/setup.bash + export PYTHONPATH=/source:/source/build/message-codegen/demo/cpp/build:$PYTHONPATH + cd /source + python3 -m pytest -o addopts="" --noconftest --import-mode=importlib -p no:cacheprovider \ + dimos/protocol/pubsub/impl/test_rospubsub_conversion.py \ + dimos/protocol/pubsub/impl/test_rospubsub.py -q + python3 examples/message-codegen/demo_ros_bridge.py + ' | tee build/message-codegen/demo/evidence/ros-bridge.txt - uses: actions/upload-artifact@v7 with: name: message-codegen-jazzy-reference diff --git a/.github/workflows/release-build-check.yml b/.github/workflows/release-build-check.yml index a77e6f37757..4d571133db5 100644 --- a/.github/workflows/release-build-check.yml +++ b/.github/workflows/release-build-check.yml @@ -4,6 +4,12 @@ name: release-build-check on: + workflow_dispatch: + inputs: + full-matrix: + type: boolean + default: false + description: Build and test all supported release wheels without publishing pull_request: paths: - .github/workflows/release.yml @@ -13,6 +19,8 @@ on: - .github/scripts/wheel_smoke.py - pyproject.toml - setup.py + - packages/dimos-generated/** + - .github/scripts/message_wheel_smoke.py - dimos/message_codegen/** - scripts/setup_message_codegen.sh - MANIFEST.in @@ -27,6 +35,7 @@ permissions: {} jobs: build-one-wheel: + if: github.event_name != 'workflow_dispatch' || !inputs.full-matrix runs-on: ubuntu-latest permissions: contents: read @@ -34,17 +43,81 @@ jobs: # Keep in sync with the build-wheels env in release.yml CIBW_MANYLINUX_X86_64_IMAGE: manylinux_2_34 CIBW_BEFORE_BUILD: "bash {project}/scripts/setup_message_codegen.sh" + CIBW_BEFORE_TEST: "pip install --no-index --find-links {project}/wheelhouse --find-links {project}/build/message-test-dependencies dimos-generated==0.1.0 dimos-message-build==0.1.0" CIBW_TEST_COMMAND: "python {project}/.github/scripts/wheel_smoke.py" steps: - name: Checkout uses: actions/checkout@v7 - name: Build cockpit dist uses: ./.github/actions/build-cockpit + - uses: astral-sh/setup-uv@v10.0.1 + - name: Build message backend wheel and prepare its test dependencies + run: | + uv build dimos/message_codegen --wheel --out-dir wheelhouse + uv venv --seed build/message-wheel-download + build/message-wheel-download/bin/python -m pip download --only-binary=:all: --platform any --implementation py --python-version 3.10 --dest build/message-test-dependencies setuptools==77.0.3 wheel==0.48.0 packaging==25.0 pybind11==3.0.1 tomli==2.4.0 + - name: Build built-in message wheels + uses: pypa/cibuildwheel@v4.2.0 + with: + package-dir: packages/dimos-generated + only: cp312-manylinux_x86_64 + env: + CIBW_BEFORE_TEST: "" + CIBW_TEST_COMMAND: "python {project}/.github/scripts/message_wheel_smoke.py" - name: Build one wheel uses: pypa/cibuildwheel@v4.2.0 with: only: cp312-manylinux_x86_64 + build-wheel-matrix: + if: github.event_name == 'workflow_dispatch' && inputs.full-matrix + strategy: + fail-fast: false + matrix: + include: + - { os: ubuntu-latest, arch: x86_64 } + - { os: ubuntu-24.04-arm, arch: aarch64 } + - { os: macos-14, arch: arm64 } + runs-on: ${{ matrix.os }} + permissions: + contents: read + env: + # Same build and installed-wheel smoke test as release.yml, without publish jobs. + CIBW_ARCHS: ${{ matrix.arch }} + CIBW_BUILD: "cp310-* cp311-* cp312-*" + CIBW_SKIP: "*-musllinux_*" + CIBW_MANYLINUX_X86_64_IMAGE: manylinux_2_34 + CIBW_MANYLINUX_AARCH64_IMAGE: manylinux_2_34 + CIBW_BEFORE_BUILD: "bash {project}/scripts/setup_message_codegen.sh" + CIBW_BEFORE_TEST: "pip install --no-index --find-links {project}/wheelhouse --find-links {project}/build/message-test-dependencies dimos-generated==0.1.0 dimos-message-build==0.1.0" + CIBW_TEST_COMMAND: "python {project}/.github/scripts/wheel_smoke.py" + MACOSX_DEPLOYMENT_TARGET: "11.0" + steps: + - name: Checkout + uses: actions/checkout@v7 + - name: Build cockpit dist + uses: ./.github/actions/build-cockpit + - uses: astral-sh/setup-uv@v10.0.1 + - name: Build message backend wheel and prepare its test dependencies + run: | + uv build dimos/message_codegen --wheel --out-dir wheelhouse + uv venv --seed build/message-wheel-download + build/message-wheel-download/bin/python -m pip download --only-binary=:all: --platform any --implementation py --python-version 3.10 --dest build/message-test-dependencies setuptools==77.0.3 wheel==0.48.0 packaging==25.0 pybind11==3.0.1 tomli==2.4.0 + - name: Build built-in message wheels + uses: pypa/cibuildwheel@v4.2.0 + with: + package-dir: packages/dimos-generated + env: + CIBW_BEFORE_TEST: "" + CIBW_TEST_COMMAND: "python {project}/.github/scripts/message_wheel_smoke.py" + - name: Build and test supported wheels + uses: pypa/cibuildwheel@v4.2.0 + - name: Upload review wheels + uses: actions/upload-artifact@v7 + with: + name: checked-wheels-${{ matrix.os }}-${{ matrix.arch }} + path: wheelhouse/*.whl + build-sdist: runs-on: ubuntu-latest permissions: @@ -61,5 +134,12 @@ jobs: prune-cache: true - name: Build sdist run: uv build --sdist + - name: Build lightweight backend artifacts + run: uv build dimos/message_codegen --out-dir message-dist + - name: Upload build backend for review + uses: actions/upload-artifact@v7 + with: + name: checked-message-backend + path: message-dist/* - name: Check sdist contents run: python3 .github/scripts/check_sdist.py dist/*.tar.gz diff --git a/.github/workflows/release.yml b/.github/workflows/release.yml index c16b43fbb41..3f79a4900ff 100644 --- a/.github/workflows/release.yml +++ b/.github/workflows/release.yml @@ -123,6 +123,7 @@ jobs: CIBW_MANYLINUX_AARCH64_IMAGE: manylinux_2_34 # open3d>=0.19 requires glibc 2.31+ # Asserts the packaged relay + Cockpit and serves them from the installed wheel. CIBW_BEFORE_BUILD: "bash {project}/scripts/setup_message_codegen.sh" + CIBW_BEFORE_TEST: "pip install --no-index --find-links {project}/wheelhouse --find-links {project}/build/message-test-dependencies dimos-generated==0.1.0 dimos-message-build==0.1.0" CIBW_TEST_COMMAND: "python {project}/.github/scripts/wheel_smoke.py" MACOSX_DEPLOYMENT_TARGET: "11.0" steps: @@ -130,6 +131,19 @@ jobs: uses: actions/checkout@v7 - name: Build cockpit dist # setup.py copies it into the wheel's _relay_dist uses: ./.github/actions/build-cockpit + - uses: astral-sh/setup-uv@v10.0.1 + - name: Build message backend wheel and prepare its test dependencies + run: | + uv build dimos/message_codegen --wheel --out-dir wheelhouse + uv venv --seed build/message-wheel-download + build/message-wheel-download/bin/python -m pip download --only-binary=:all: --platform any --implementation py --python-version 3.10 --dest build/message-test-dependencies setuptools==77.0.3 wheel==0.48.0 packaging==25.0 pybind11==3.0.1 tomli==2.4.0 + - name: Build built-in message wheels + uses: pypa/cibuildwheel@v4.2.0 + with: + package-dir: packages/dimos-generated + env: + CIBW_BEFORE_TEST: "" + CIBW_TEST_COMMAND: "python {project}/.github/scripts/message_wheel_smoke.py" - name: Build wheels uses: pypa/cibuildwheel@v4.2.0 - name: Attestation @@ -154,9 +168,7 @@ jobs: - run: uv venv .venv --python 3.12 - run: bash scripts/setup_message_codegen.sh - name: Package CMake headers, schemas, and native Rust sources - env: - MESSAGE_VERSION: ${{ needs.validate.outputs.version }} - run: bash scripts/package_messages.sh "$MESSAGE_VERSION" + run: bash scripts/package_messages.sh - uses: actions/upload-artifact@v7 with: name: native-message-packages diff --git a/.gitignore b/.gitignore index fbe1880699d..982e218719b 100644 --- a/.gitignore +++ b/.gitignore @@ -104,3 +104,6 @@ site/ # Optuna study store, written by the relocalization eval (see lidar/eval.md) optuna.db + +# Generated typing interfaces from editable native builds. +/dimos_generated-stubs/ diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index ae979f2876e..1646062f45b 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -140,6 +140,12 @@ repos: bash -c ' set -euo pipefail git ls-files "*Cargo.toml" | while read -r m; do + # This consumer intentionally references generated packages absent in a clean checkout. + # Format its checked-in source directly; native CI builds and runs the full project. + if [ "$m" = examples/message-project/rust/Cargo.toml ]; then + rustfmt --edition 2024 examples/message-project/rust/src/main.rs + continue + fi cargo locate-project --manifest-path "$m" --workspace --message-format plain done | sort -u | while read -r root; do cargo fmt --manifest-path "$root" --all diff --git a/AGENTS.md b/AGENTS.md index dfa7fe021ac..ebd3396b84c 100644 --- a/AGENTS.md +++ b/AGENTS.md @@ -157,7 +157,7 @@ Autonomous subsystems. Communicate via `In[T]`/`Out[T]` typed streams. Run in fo from dimos.core.module import Module from dimos.core.stream import In, Out from dimos.core.core import rpc -from dimos.msgs.sensor_msgs import Image +from dimos_generated.sensor_msgs.msg import Image class MyModule(Module): color_image: In[Image] diff --git a/Cargo.lock b/Cargo.lock index 4f0060bd78e..e849de67051 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -114,7 +114,7 @@ dependencies = [ "nom 7.1.3", "num-traits", "rusticata-macros 4.1.0", - "thiserror 2.0.20", + "thiserror", "time", ] @@ -262,6 +262,20 @@ name = "bytemuck" version = "1.25.2" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "95832e849adfb21180ccb6826a99da14e5d266ae5c2e668e1602cf234f153797" +dependencies = [ + "bytemuck_derive", +] + +[[package]] +name = "bytemuck_derive" +version = "1.12.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "6a1f896587b6f2c069c73d2f0913e2d590c3990285cd2f0b6aa02b786b4c679c" +dependencies = [ + "proc-macro2", + "quote", + "syn 3.0.4", +] [[package]] name = "byteorder" @@ -344,15 +358,6 @@ version = "0.3.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "b0fc239e0f6cb375d2402d48afb92f76f5404fd1df208a41930ec81eda078bea" -[[package]] -name = "cmake" -version = "0.1.58" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "c0f78a02292a74a88ac736019ab962ece0bc380e3f977bf72e376c5d78ff0678" -dependencies = [ - "cc", -] - [[package]] name = "combine" version = "4.6.8" @@ -610,7 +615,7 @@ version = "1.0.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "10d60334b3b2e7c9d91ef8150abfb6fa4c1c39ebbcf4a81c2e346aad939fee3e" dependencies = [ - "thiserror 2.0.20", + "thiserror", ] [[package]] @@ -648,13 +653,12 @@ dependencies = [ ] [[package]] -name = "dimos-lcm" +name = "dimos-generated-messages" version = "0.1.0" -source = "git+https://github.com/dimensionalOS/dimos-lcm.git?branch=rust-codegen#04d78e8622500244123ba9cefa4c51b4cb454549" dependencies = [ - "byteorder", - "socket2 0.5.10", - "tokio", + "re_cdr", + "serde", + "serde-big-array", ] [[package]] @@ -671,9 +675,9 @@ name = "dimos-livox" version = "0.1.0" dependencies = [ "crc", + "dimos-generated-messages", "dimos-module", "etherparse", - "lcm-msgs", "pcap-parser", "serde", "serde_json", @@ -691,8 +695,8 @@ dependencies = [ "anyhow", "crossbeam-channel", "crossbeam-utils", + "dimos-generated-messages", "dimos-module", - "lcm-msgs", "lz4_flex 0.14.0", "mcap", "rayon", @@ -702,7 +706,6 @@ dependencies = [ "tempfile", "tokio", "tracing", - "turbojpeg", "validator", ] @@ -711,8 +714,8 @@ name = "dimos-mls-planner" version = "0.1.0" dependencies = [ "ahash", + "dimos-generated-messages", "dimos-module", - "lcm-msgs", "rayon", "serde", "tokio", @@ -736,9 +739,9 @@ dependencies = [ name = "dimos-module" version = "0.1.0" dependencies = [ - "dimos-lcm", + "dimos-generated-messages", + "dimos-lcm-transport", "dimos-module-macros", - "lcm-msgs", "nalgebra", "rayon", "serde", @@ -766,8 +769,8 @@ dependencies = [ name = "dimos-native-module-examples" version = "0.1.0" dependencies = [ + "dimos-generated-messages", "dimos-module", - "lcm-msgs", "serde", "tokio", "tracing", @@ -778,9 +781,9 @@ dependencies = [ name = "dimos-pointlio" version = "0.1.0" dependencies = [ + "dimos-generated-messages", "dimos-livox", "dimos-module", - "lcm-msgs", "pointlio-core", "serde", "serde_json", @@ -808,8 +811,8 @@ version = "0.1.0" dependencies = [ "ahash", "arrayvec", + "dimos-generated-messages", "dimos-module", - "lcm-msgs", "nalgebra", "rayon", "serde", @@ -1114,12 +1117,6 @@ dependencies = [ "slab", ] -[[package]] -name = "gcd" -version = "2.3.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "1d758ba1b47b00caf47f24925c0074ecb20d6dfcffe7f6d53395c0465674841a" - [[package]] name = "generic-array" version = "0.14.7" @@ -1573,7 +1570,7 @@ dependencies = [ "jni-sys", "log", "simd_cesu8", - "thiserror 2.0.20", + "thiserror", "walkdir", "windows-link", ] @@ -1681,14 +1678,6 @@ version = "1.5.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "bbd2bcb4c963f2ddae06a2efc7e9f3591312473c50c6685e1f298068316e66fe" -[[package]] -name = "lcm-msgs" -version = "0.1.0" -source = "git+https://github.com/dimensionalOS/dimos-lcm.git?branch=rust-codegen#04d78e8622500244123ba9cefa4c51b4cb454549" -dependencies = [ - "byteorder", -] - [[package]] name = "libc" version = "0.2.189" @@ -1824,7 +1813,7 @@ dependencies = [ "num_cpus", "paste", "static_assertions", - "thiserror 2.0.20", + "thiserror", "zstd", ] @@ -2489,7 +2478,7 @@ dependencies = [ "rustc-hash", "rustls", "socket2 0.6.5", - "thiserror 2.0.20", + "thiserror", "tokio", "tracing", "web-time", @@ -2513,7 +2502,7 @@ dependencies = [ "rustls-pki-types", "rustls-platform-verifier", "slab", - "thiserror 2.0.20", + "thiserror", "tinyvec", "tracing", "web-time", @@ -2650,6 +2639,18 @@ dependencies = [ "yasna", ] +[[package]] +name = "re_cdr" +version = "0.1.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "bba10c21433b597481f366b978cdf2617e56785b38cb0dcc3833c6b8ad7e061a" +dependencies = [ + "bytemuck", + "byteorder", + "serde", + "thiserror", +] + [[package]] name = "redox_syscall" version = "0.5.18" @@ -2667,7 +2668,7 @@ checksum = "a4e608c6638b9c18977b00b475ac1f28d14e84b27d8d42f70e0bf1e3dec127ac" dependencies = [ "getrandom 0.2.17", "libredox", - "thiserror 2.0.20", + "thiserror", ] [[package]] @@ -2773,7 +2774,7 @@ source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "c51c9ae4df8a7fba42103df5c621fa3c37eccf3a3c650879e90fc48b11cc192c" dependencies = [ "hashbrown 0.16.1", - "thiserror 2.0.20", + "thiserror", ] [[package]] @@ -3053,6 +3054,15 @@ dependencies = [ "serde_derive", ] +[[package]] +name = "serde-big-array" +version = "0.5.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "11fc7cc2c76d73e0f27ee52abbd64eec84d46f370c88371120433196934e4b7f" +dependencies = [ + "serde", +] + [[package]] name = "serde_core" version = "1.0.229" @@ -3457,33 +3467,13 @@ dependencies = [ "windows-sys 0.61.2", ] -[[package]] -name = "thiserror" -version = "1.0.69" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "b6aaf5339b578ea85b50e080feb250a3e8ae8cfcdff9a461c9ec2904bc923f52" -dependencies = [ - "thiserror-impl 1.0.69", -] - [[package]] name = "thiserror" version = "2.0.20" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "ec86235f5fcc2a73650310756d2ac5b138a5780bbbdfae3eeccec992c435ba4f" dependencies = [ - "thiserror-impl 2.0.20", -] - -[[package]] -name = "thiserror-impl" -version = "1.0.69" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "4fee6c4efc90059e10f81e6d42c60a18f76588c3d74cb83a0b242a2b6c7504c1" -dependencies = [ - "proc-macro2", - "quote", - "syn 2.0.119", + "thiserror-impl", ] [[package]] @@ -3798,29 +3788,6 @@ dependencies = [ "syn 2.0.119", ] -[[package]] -name = "turbojpeg" -version = "1.5.1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "81313fcd64aaa30ddfd59eaa122968136b2dd9cd41d9619b126cf71e67a01702" -dependencies = [ - "gcd", - "libc", - "thiserror 1.0.69", - "turbojpeg-sys", -] - -[[package]] -name = "turbojpeg-sys" -version = "1.2.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "49b5a974bc59382d35927e3a07eef2ba9a24fe7227cefe52ec44b508e1b90f86" -dependencies = [ - "anyhow", - "cmake", - "libc", -] - [[package]] name = "twox-hash" version = "1.6.3" @@ -4377,7 +4344,7 @@ dependencies = [ "oid-registry", "ring", "rusticata-macros 4.1.0", - "thiserror 2.0.20", + "thiserror", "time", ] diff --git a/Cargo.toml b/Cargo.toml index 817402879a5..17cd8c093a9 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -1,6 +1,7 @@ [workspace] resolver = "2" members = [ + "dimos/message_codegen", "dimos/experimental/memory/rust", "native/rust/dimos-module", "native/rust/dimos-module-macros", diff --git a/MANIFEST.in b/MANIFEST.in index 23c70f4be9c..d4b1ce2c77d 100644 --- a/MANIFEST.in +++ b/MANIFEST.in @@ -13,6 +13,8 @@ graft dimos/message_codegen/schemas graft dimos/message_codegen/templates include dimos/message_codegen/_vendor/*.pyi include scripts/setup_message_codegen.sh +graft packages/dimos-generated +prune packages/dimos-generated/build # --- Exclusions (must come after the includes above so they win) --- # Test fixtures must never ship. diff --git a/README.md b/README.md index 7c5699508a4..a33516a2cea 100644 --- a/README.md +++ b/README.md @@ -250,8 +250,10 @@ from dimos.core.coordination.blueprints import autoconnect from dimos.core.core import rpc from dimos.core.module import Module from dimos.core.stream import In, Out -from dimos.msgs.geometry_msgs import Twist -from dimos.msgs.sensor_msgs import Image, ImageFormat +from dimos_generated.geometry_msgs.msg import Twist +from dimos_generated.sensor_msgs.msg import Image +from dimos.msgs.image import image_from_array +from dimos.msgs.time import header_now class RobotConnection(Module): cmd_vel: In[Twist] @@ -263,10 +265,10 @@ class RobotConnection(Module): def _image_loop(self): while True: - img = Image.from_numpy( + img = image_from_array( np.zeros((120, 160, 3), np.uint8), - format=ImageFormat.RGB, - frame_id="camera_optical", + encoding="rgb8", + header=header_now(frame_id="camera_optical"), ) self.color_image.publish(img) time.sleep(0.2) @@ -296,7 +298,7 @@ A blueprint example that connects the image stream from a robot to an MCP-backed ```py skip from dimos.core.coordination.blueprints import autoconnect from dimos.core.transport import LCMTransport -from dimos.msgs.sensor_msgs import Image +from dimos_generated.sensor_msgs.msg import Image from dimos.robot.unitree.go2.connection import go2_connection from dimos.agents.mcp.mcp_client import McpClient from dimos.agents.mcp.mcp_server import McpServer @@ -344,9 +346,9 @@ uv run pytest --numprocesses=auto dimos ## Multi Language Support -Python is our glue and prototyping language, but we support many languages via LCM interop. +Python, C++ and Rust share generated `.msg` value types and CDR codecs over raw LCM or Zenoh. -Check our language interop examples: -- [C++](examples/language-interop/cpp/) -- [Lua](examples/language-interop/lua/) -- [TypeScript](examples/language-interop/ts/) +- [Add and use a message](docs/development/messages.md) +- [Three-language relay and recording demos](examples/message-codegen/README.md) +- [C++ virtual robot controller](examples/language-interop/cpp/README.md) +- [Browser CDR SDK](docs/web/web_sdk.md) diff --git a/data/.lfs/astar_corner_min_cost.png.tar.gz b/data/.lfs/astar_corner_min_cost.png.tar.gz index 35f3ffe0b68..a6c2a84d750 100644 --- a/data/.lfs/astar_corner_min_cost.png.tar.gz +++ b/data/.lfs/astar_corner_min_cost.png.tar.gz @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:42517c5f67a9f06949cb2015a345f9d6b43d22cafd50e1fefb9b5d24d8b72509 -size 5671 +oid sha256:bb08a2ae88cdd133924d3e48f796bc80d1c78c21a42a819ba9a486e1363ed0cb +size 5640 diff --git a/data/.lfs/astar_min_cost.png.tar.gz b/data/.lfs/astar_min_cost.png.tar.gz index 752a7782951..820eaab9933 100644 --- a/data/.lfs/astar_min_cost.png.tar.gz +++ b/data/.lfs/astar_min_cost.png.tar.gz @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:06b67aa0d18c291c3525e67ca3a2a9ab2530f6fe782a850872ba4c343353a20a -size 12018 +oid sha256:861d4613729fba6b43855661497dbd3c69f9f7597ea31d0c1aee9ad82666a0ab +size 11961 diff --git a/data/.lfs/gradient_simple.png.tar.gz b/data/.lfs/gradient_simple.png.tar.gz index 7232282ce42..f0833e747bf 100644 --- a/data/.lfs/gradient_simple.png.tar.gz +++ b/data/.lfs/gradient_simple.png.tar.gz @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:e418f2a6858c757cb72bd25772749a1664c97a407682d88ad2b51c4bbdcb8006 -size 11568 +oid sha256:b696ca8d86efc625fddaf7dd81fecd16cd0797e008c89b74af69fe9d7ec1e1ac +size 11503 diff --git a/data/.lfs/make_path_mask_full.png.tar.gz b/data/.lfs/make_path_mask_full.png.tar.gz index 0e9336aaea5..c28598c2644 100644 --- a/data/.lfs/make_path_mask_full.png.tar.gz +++ b/data/.lfs/make_path_mask_full.png.tar.gz @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:b772d266dffa82ccf14f13c7d8cc2443210202836883c80f016a56d4cfe2b52a -size 11213 +oid sha256:804c17a3f28ba5525d253ba89b08a7ac38bf0f62cb89b799f85b27fe431192a2 +size 11320 diff --git a/data/.lfs/make_path_mask_two_meters.png.tar.gz b/data/.lfs/make_path_mask_two_meters.png.tar.gz index 7fa9e767b8f..766886698cd 100644 --- a/data/.lfs/make_path_mask_two_meters.png.tar.gz +++ b/data/.lfs/make_path_mask_two_meters.png.tar.gz @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:da608d410f4a1afee0965abfac814bc05267bdde31b0d3a9622c39515ee4f813 -size 11395 +oid sha256:c3f8a5f9371b17229634aa51bf0fe22999d57990676dea9bc866ce3945062e92 +size 11363 diff --git a/data/.lfs/resample_path_simple.png.tar.gz b/data/.lfs/resample_path_simple.png.tar.gz index 1a8c1118d60..ff26518b329 100644 --- a/data/.lfs/resample_path_simple.png.tar.gz +++ b/data/.lfs/resample_path_simple.png.tar.gz @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:0b5c454ed6cc66cf4446ce4a246464aec27368da4902651b4ad9ed29b3ba56ec -size 118319 +oid sha256:4af4f0623cae679abf46246239558b77cead04ec024bd539d31c375c29981a06 +size 117658 diff --git a/data/.lfs/resample_path_smooth.png.tar.gz b/data/.lfs/resample_path_smooth.png.tar.gz index 80af3d3805d..af21c35e372 100644 --- a/data/.lfs/resample_path_smooth.png.tar.gz +++ b/data/.lfs/resample_path_smooth.png.tar.gz @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:6cc0dfd80bada94f2ab1bb577e2ec1734dad6894113f2fe77964bd80d886c3d3 -size 109699 +oid sha256:243ffb720247f543793243305b346e44febaff5398572a7979ed770751587302 +size 109328 diff --git a/data/.lfs/unitree_go2_detection_cdr.tar.gz b/data/.lfs/unitree_go2_detection_cdr.tar.gz new file mode 100644 index 00000000000..cfb22141ba5 --- /dev/null +++ b/data/.lfs/unitree_go2_detection_cdr.tar.gz @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:966a7b4d542a58a7891b7c0fc4951e3ea3975c56f52cd99b0a5f0541198d2d40 +size 5823304 diff --git a/data/.lfs/visualize_occupancy_rainbow.png.tar.gz b/data/.lfs/visualize_occupancy_rainbow.png.tar.gz index 9bbd2e6ea1f..18c4da65865 100644 --- a/data/.lfs/visualize_occupancy_rainbow.png.tar.gz +++ b/data/.lfs/visualize_occupancy_rainbow.png.tar.gz @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:3dc1e3b6519f7d7ff25b16c3124ee447f02857eeb3eb20930cdab95464b1f0a3 -size 11582 +oid sha256:3513b154f81dc5c1b34a74ea517e5789389fbae05f989136393794ee8bba7047 +size 11516 diff --git a/data/.lfs/visualize_occupancy_turbo.png.tar.gz b/data/.lfs/visualize_occupancy_turbo.png.tar.gz index e2863cdae6d..249eb9f97e3 100644 --- a/data/.lfs/visualize_occupancy_turbo.png.tar.gz +++ b/data/.lfs/visualize_occupancy_turbo.png.tar.gz @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:c21874bab6ec7cd9692d2b1e67498ddfff3c832ec992e9552fee17093759b270 -size 18593 +oid sha256:7e28df6a9193dd56081b51a8af3896be6cd4c5dda1deeff44b383e05e462e10b +size 18229 diff --git a/dimos/agents/mcp/mcp_client.py b/dimos/agents/mcp/mcp_client.py index 5c98a1332a9..996913fa2a5 100644 --- a/dimos/agents/mcp/mcp_client.py +++ b/dimos/agents/mcp/mcp_client.py @@ -399,6 +399,11 @@ def _process_message( def _append_image_to_history( mcp_client: McpClient, func_name: str, uuid_: str, result: Any ) -> None: + if isinstance(result, dict) and result.get("type") == "image": + result = { + "type": "image_url", + "image_url": {"url": f"data:{result['mimeType']};base64,{result['data']}"}, + } mcp_client.add_message( HumanMessage( content=[ diff --git a/dimos/agents/mcp/mcp_server.py b/dimos/agents/mcp/mcp_server.py index 61d7572af8c..17ae8d33c50 100644 --- a/dimos/agents/mcp/mcp_server.py +++ b/dimos/agents/mcp/mcp_server.py @@ -14,6 +14,7 @@ from __future__ import annotations import asyncio +import base64 from collections.abc import AsyncGenerator, Callable import concurrent.futures import json @@ -22,6 +23,7 @@ from typing import TYPE_CHECKING, Any import uuid +from dimos_generated.sensor_msgs.msg import Image from fastapi import FastAPI from fastapi.middleware.cors import CORSMiddleware from fastapi.responses import JSONResponse @@ -36,6 +38,7 @@ from dimos.core.module import Module from dimos.core.rpc_client import RpcCall, RPCClient from dimos.core.transport_factory import make_transport +from dimos.msgs.image import image_to_jpeg from dimos.utils.logging_config import setup_logger if TYPE_CHECKING: @@ -213,6 +216,15 @@ def _can_wait(holder: str) -> bool: duration = f"{time.monotonic() - t0:.3f}s" response = str(result)[:200] + if isinstance(result, Image): + content = { + "type": "image", + "mimeType": "image/jpeg", + "data": base64.b64encode(image_to_jpeg(result)).decode("ascii"), + } + logger.info("MCP tool done", tool=name, duration=duration, response=response) + return _jsonrpc_result(req_id, {"content": [content]}) + if hasattr(result, "agent_encode"): logger.info("MCP tool done", tool=name, duration=duration, response=response) return _jsonrpc_result(req_id, {"content": result.agent_encode()}) diff --git a/dimos/agents/mcp/test_generated_image.py b/dimos/agents/mcp/test_generated_image.py new file mode 100644 index 00000000000..3fcc5442df5 --- /dev/null +++ b/dimos/agents/mcp/test_generated_image.py @@ -0,0 +1,56 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""A generated CDR image survives MCP serialization and agent history conversion.""" + +import asyncio +import base64 + +import cv2 +from dimos_generated.sensor_msgs.msg import Image +import numpy as np + +from dimos.agents.mcp.mcp_client import _append_image_to_history +from dimos.agents.mcp.mcp_server import handle_request +from dimos.msgs.image import image_from_array + + +def test_generated_image_mcp_result_reaches_model_history(mocker): + pixels = np.zeros((12, 16, 3), dtype=np.uint8) + pixels[..., 0] = 255 + original = image_from_array(pixels, encoding="rgb8") + response = asyncio.run( + handle_request( + {"method": "tools/call", "id": 1, "params": {"name": "observe"}}, + [], + {"observe": lambda: Image.decode(original.encode())}, + ) + ) + assert response is not None + content = response["result"]["content"] + assert len(content) == 1 + image = content[0] + assert image["type"] == "image" + assert image["mimeType"] == "image/jpeg" + jpeg = base64.b64decode(image["data"]) + decoded = cv2.imdecode(np.frombuffer(jpeg, dtype=np.uint8), cv2.IMREAD_COLOR) + assert decoded.shape == (12, 16, 3) + assert decoded[0, 0].tolist() == [0, 0, 254] + client = mocker.Mock() + _append_image_to_history(client, "observe", "test-image", image) + message = client.add_message.call_args.args[0] + assert message.content[1] == { + "type": "image_url", + "image_url": {"url": "data:image/jpeg;base64," + image["data"]}, + } diff --git a/dimos/agents/mcp/test_mcp_client.py b/dimos/agents/mcp/test_mcp_client.py index b31637630e6..2be2fcab2e9 100644 --- a/dimos/agents/mcp/test_mcp_client.py +++ b/dimos/agents/mcp/test_mcp_client.py @@ -14,11 +14,12 @@ from typing import Any +from dimos_generated.sensor_msgs.msg import Image from langchain_core.messages import HumanMessage from dimos.agents.annotation import skill from dimos.core.module import Module -from dimos.msgs.sensor_msgs.Image import Image +from dimos.msgs.image import image_from_file from dimos.utils.data import get_data @@ -177,7 +178,7 @@ class Visualizer(Module): @skill def take_a_picture(self) -> Image: """Takes a picture.""" - return Image.from_file(get_data("cafe-smol.jpg")).to_rgb() + return image_from_file(get_data("cafe-smol.jpg")) def test_image(agent_setup): diff --git a/dimos/agents/skills/navigation.py b/dimos/agents/skills/navigation.py index 53748f82019..10579d16d80 100644 --- a/dimos/agents/skills/navigation.py +++ b/dimos/agents/skills/navigation.py @@ -15,6 +15,8 @@ import time from typing import Any +from dimos_generated.geometry_msgs.msg import Point, Pose, PoseStamped +from dimos_generated.sensor_msgs.msg import Image from reactivex.disposable import Disposable from dimos.agents.annotation import skill @@ -23,10 +25,8 @@ from dimos.core.module import Module from dimos.core.stream import In from dimos.models.qwen.bbox import BBox -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Vector3 import Vector3, make_vector3 -from dimos.msgs.sensor_msgs.Image import Image +from dimos.msgs.geometry import quaternion_euler, quaternion_from_euler +from dimos.msgs.time import header_now from dimos.navigation.base import NavigationState from dimos.navigation.navigation_spec import NavigationInterfaceSpec from dimos.navigation.visual.query import get_object_bbox_from_image @@ -96,13 +96,13 @@ def tag_location(self, location_name: str) -> str: if not self._latest_odom: return "No odometry data received yet, cannot tag location." - position = self._latest_odom.position - rotation = self._latest_odom.orientation + position = self._latest_odom.pose.position + rotation = self._latest_odom.pose.orientation location = RobotLocation( name=location_name, position=(position.x, position.y, position.z), - rotation=(rotation.x, rotation.y, rotation.z), + rotation=quaternion_euler(rotation), ) if not self._spatial_memory.tag_location(location): @@ -158,16 +158,22 @@ def _navigate_by_tagged_location(self, query: str) -> str | None: logger.info("Found tagged location", location=robot_location) goal_pose = PoseStamped( - position=make_vector3(*robot_location.position), - orientation=Quaternion.from_euler(Vector3(*robot_location.rotation)), - frame_id="map", + header=header_now("map"), + pose=Pose( + position=Point( + x=robot_location.position[0], + y=robot_location.position[1], + z=robot_location.position[2], + ), + orientation=quaternion_from_euler(*robot_location.rotation), + ), ) return self._navigate_to(goal_pose, f"Found a tagged location called '{query}'.") def _navigate_to(self, pose: PoseStamped, message: str) -> str: logger.info( - f"Navigating to pose: ({pose.position.x:.2f}, {pose.position.y:.2f}, {pose.position.z:.2f})" + f"Navigating to pose: ({pose.pose.position.x:.2f}, {pose.pose.position.y:.2f}, {pose.pose.position.z:.2f})" ) self._navigation.set_goal(pose) @@ -281,7 +287,8 @@ def _get_goal_pose_from_result(self, result: dict[str, Any]) -> PoseStamped | No theta = first.get("rot_z", 0) return PoseStamped( - position=make_vector3(pos_x, pos_y, 0), - orientation=Quaternion.from_euler(make_vector3(0, 0, theta)), - frame_id="map", + header=header_now("map"), + pose=Pose( + position=Point(x=pos_x, y=pos_y), orientation=quaternion_from_euler(0, 0, theta) + ), ) diff --git a/dimos/agents/skills/observe_skill.py b/dimos/agents/skills/observe_skill.py index 986856ec499..a37a3a19a8d 100644 --- a/dimos/agents/skills/observe_skill.py +++ b/dimos/agents/skills/observe_skill.py @@ -12,11 +12,12 @@ # See the License for the specific language governing permissions and # limitations under the License. +from dimos_generated.sensor_msgs.msg import Image + from dimos.agents.annotation import skill from dimos.agents.skill_result import SkillResult from dimos.core.module import Module from dimos.core.stream import In -from dimos.msgs.sensor_msgs.Image import Image class ObserveSkill(Module): diff --git a/dimos/agents/skills/person_follow.py b/dimos/agents/skills/person_follow.py index e9dc39802ac..c8579f90731 100644 --- a/dimos/agents/skills/person_follow.py +++ b/dimos/agents/skills/person_follow.py @@ -17,6 +17,9 @@ import time from typing import Any +from dimos_generated.geometry_msgs.msg import Twist +from dimos_generated.sensor_msgs.msg import CameraInfo, Image, PointCloud2 +from dimos_generated.tf2_msgs.msg import TFMessage import numpy as np from reactivex.disposable import Disposable @@ -30,11 +33,7 @@ from dimos.models.segmentation.edge_tam import EdgeTAMProcessor from dimos.models.vl.base import VlModel from dimos.models.vl.create import create -from dimos.msgs.geometry_msgs.Twist import Twist -from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo -from dimos.msgs.sensor_msgs.Image import Image, ImageFormat -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 -from dimos.msgs.tf2_msgs.TFMessage import TFMessage +from dimos.msgs.image import image_from_array from dimos.navigation.visual.query import get_object_bbox_from_image from dimos.navigation.visual_servoing.detection_navigation import DetectionNavigation from dimos.navigation.visual_servoing.visual_servoing_2d import VisualServoing2D @@ -204,7 +203,7 @@ def stop_following(self) -> str: """ self._stop_following() - self.cmd_vel.publish(Twist.zero()) + self.cmd_vel.publish(Twist()) if self._thread is not None: self._thread.join(timeout=DEFAULT_THREAD_JOIN_TIMEOUT) @@ -247,7 +246,7 @@ def _follow_person( ) if len(initial_detections) == 0: - self.cmd_vel.publish(Twist.zero()) + self.cmd_vel.publish(Twist()) return f"EdgeTAM failed to segment '{query}'." logger.info(f"EdgeTAM initialized with {len(initial_detections)} detections") @@ -279,7 +278,7 @@ def _follow_loop(self, tracker: "EdgeTAMProcessor", query: str) -> None: detections = tracker.process_image(latest_image) if len(detections) == 0: - self.cmd_vel.publish(Twist.zero()) + self.cmd_vel.publish(Twist()) lost_count += 1 if lost_count > self._max_lost_frames: @@ -321,7 +320,7 @@ def _stop_following(self) -> None: self._should_stop.set() def _send_stop_reason(self, query: str, reason: str) -> None: - self.cmd_vel.publish(Twist.zero()) + self.cmd_vel.publish(Twist()) self.tool_update( "follow_person", f"Person follow stopped for '{query}'. Reason: {reason}.", @@ -332,4 +331,4 @@ def _send_stop_reason(self, query: str, reason: str) -> None: def _decode_base64_image(b64: str) -> Image: bgr_array = get_turbojpeg().decode(base64.b64decode(b64)) - return Image(data=bgr_array, format=ImageFormat.BGR) + return image_from_array(bgr_array, encoding="bgr8") diff --git a/dimos/agents/skills/test_generated_navigation.py b/dimos/agents/skills/test_generated_navigation.py new file mode 100644 index 00000000000..49551742803 --- /dev/null +++ b/dimos/agents/skills/test_generated_navigation.py @@ -0,0 +1,70 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Generated navigation goals and named-location orientation round trips.""" + +import math + +from dimos_generated.geometry_msgs.msg import Point, Pose, PoseStamped +import pytest + +from dimos.agents.skills.navigation import NavigationSkillContainer +from dimos.msgs.geometry import quaternion_euler, quaternion_from_euler + + +@pytest.fixture +def skill(mocker): + value = NavigationSkillContainer() + memory = mocker.Mock() + navigator = mocker.Mock() + mocker.patch.object(value, "_spatial_memory", memory, create=True) + mocker.patch.object(value, "_navigation", navigator, create=True) + value._skill_started = True + try: + yield value, memory, navigator + finally: + value.stop() + + +def test_tagged_location_preserves_euler_orientation_through_cdr_goal(skill): + value, memory, navigator = skill + angles = (0.2, -0.3, math.pi / 2) + pose = PoseStamped( + pose=Pose(position=Point(x=1, y=2, z=3), orientation=quaternion_from_euler(*angles)) + ) + value._on_odom(PoseStamped.decode(pose.encode())) + assert "Tagged 'desk'" in value.tag_location("desk") + tagged = memory.tag_location.call_args.args[0] + assert tagged.position == (1, 2, 3) + assert tagged.rotation == pytest.approx(angles) + memory.query_tagged_location.return_value = tagged + assert "Found a tagged location" in value._navigate_by_tagged_location("desk") + goal = navigator.set_goal.call_args.args[0] + decoded = PoseStamped.decode(goal.encode()) + assert decoded.header.frame_id == "map" + assert decoded.pose.position == pose.pose.position + assert quaternion_euler(decoded.pose.orientation) == pytest.approx(angles) + + +def test_semantic_result_builds_generated_map_goal(skill): + value, _, navigator = skill + result = {"distance": 0.1, "metadata": [{"pos_x": 4, "pos_y": -2, "rot_z": math.pi / 2}]} + goal = value._get_goal_pose_from_result(result) + assert goal is not None + value._navigate_to(goal, "Found desk") + decoded = PoseStamped.decode(navigator.set_goal.call_args.args[0].encode()) + assert decoded.header.frame_id == "map" + assert decoded.pose.position == Point(x=4, y=-2) + assert quaternion_euler(decoded.pose.orientation) == pytest.approx((0, 0, math.pi / 2)) + assert value._get_goal_pose_from_result({"distance": 0.9}) is None diff --git a/dimos/agents/skills/test_generated_person_follow.py b/dimos/agents/skills/test_generated_person_follow.py new file mode 100644 index 00000000000..8122ba5ea19 --- /dev/null +++ b/dimos/agents/skills/test_generated_person_follow.py @@ -0,0 +1,81 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from types import SimpleNamespace +from unittest.mock import MagicMock + +from dimos_generated.geometry_msgs.msg import Twist +from dimos_generated.sensor_msgs.msg import CameraInfo, Image +import numpy as np +import pytest + +from dimos.agents.skills import person_follow +from dimos.core.global_config import GlobalConfig +from dimos.msgs.image import image_from_array +from dimos.perception.detection.type.detection2d.bbox import Detection2DBBox +from dimos.perception.detection.type.detection2d.imageDetections2D import ImageDetections2D + + +@pytest.mark.parametrize("lost", [False, True]) +def test_generated_person_follow_loop_stops_cleanly(monkeypatch, lost): + monkeypatch.setattr(person_follow, "create", lambda _: MagicMock()) + camera = CameraInfo(width=640, height=480, k=[500, 0, 320, 0, 500, 240, 0, 0, 1]) + module = person_follow.PersonFollowSkillContainer( + camera_info=camera, g=GlobalConfig(simulation="") + ) + image = image_from_array(np.zeros((480, 640, 3), dtype=np.uint8), encoding="rgb8") + image = Image.decode(image.encode()) + detection = Detection2DBBox( + bbox=(270, 100, 370, 300), + track_id=1, + class_id=0, + confidence=1, + name="person", + ts=0, + image=image, + ) + tracker = SimpleNamespace( + init_track=lambda **kwargs: ImageDetections2D(image, [detection]), + stop=lambda: None, + process_image=lambda frame: ImageDetections2D(frame, [] if lost else [detection]), + ) + outputs = [] + + def receive(value): + outputs.append(Twist.decode(value.encode())) + if not lost: + module._should_stop.set() + + unsubscribe = module.cmd_vel.subscribe(receive) + module._max_lost_frames = 0 + module._frequency = 1000 + try: + module._on_color_image(image) + module._tracker = tracker + status = module._follow_person("person", (270, 100, 370, 300)) + assert "Starting to follow" in status + thread = module._thread + assert thread is not None + thread.join(timeout=2) + assert not thread.is_alive() + assert outputs + assert outputs[-1] == Twist() + if lost: + assert all(value == Twist() for value in outputs) + else: + assert outputs[0].linear.x == 0.5 + assert outputs[0].angular.z == 0 + finally: + unsubscribe() + module.stop() diff --git a/dimos/agents/skills/test_navigation.py b/dimos/agents/skills/test_navigation.py index f206d63ba04..08206c2c6dc 100644 --- a/dimos/agents/skills/test_navigation.py +++ b/dimos/agents/skills/test_navigation.py @@ -14,14 +14,14 @@ from typing import Any +from dimos_generated.geometry_msgs.msg import PoseStamped +from dimos_generated.sensor_msgs.msg import Image from langchain_core.messages import HumanMessage from dimos.agents.skills.navigation import NavigationSkillContainer from dimos.core.core import rpc from dimos.core.module import Module from dimos.core.stream import Out -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.sensor_msgs.Image import Image from dimos.navigation.base import NavigationState from dimos.types.robot_location import RobotLocation diff --git a/dimos/agents/skills/test_observe_skill.py b/dimos/agents/skills/test_observe_skill.py index 616f58c77d2..018f03aefd5 100644 --- a/dimos/agents/skills/test_observe_skill.py +++ b/dimos/agents/skills/test_observe_skill.py @@ -14,8 +14,8 @@ from collections.abc import Iterator import threading -import time +from dimos_generated.sensor_msgs.msg import Image import numpy as np import pytest from reactivex.scheduler import ThreadPoolScheduler @@ -23,7 +23,7 @@ from dimos.agents.skill_result import SkillResult from dimos.agents.skills.observe_skill import ObserveSkill from dimos.core.transport import LCMTransport -from dimos.msgs.sensor_msgs.Image import Image, ImageFormat +from dimos.msgs.image import image_from_array, image_view @pytest.fixture @@ -35,20 +35,22 @@ def module(monkeypatch: pytest.MonkeyPatch) -> Iterator[ObserveSkill]: monkeypatch.setattr("dimos.utils.reactive.get_scheduler", lambda: scheduler) m = ObserveSkill() m.color_image.transport = LCMTransport("/test_observe/color_image", Image) - yield m - m.stop() - scheduler.executor.shutdown(wait=True) + try: + yield m + finally: + m.stop() + scheduler.executor.shutdown(wait=True) def test_observe_returns_published_frame(module: ObserveSkill) -> None: - frame = Image.from_numpy(np.zeros((8, 8, 3), dtype=np.uint8), format=ImageFormat.RGB, ts=1.0) + frame = image_from_array(np.zeros((8, 8, 3), dtype=np.uint8), encoding="rgb8") stop = threading.Event() # get_next subscribes lazily, so keep publishing until observe picks a frame up. def pump() -> None: while not stop.is_set(): module.color_image.transport.publish(frame) - time.sleep(0.05) + stop.wait(0.05) thread = threading.Thread(target=pump, daemon=True) thread.start() @@ -59,7 +61,7 @@ def pump() -> None: thread.join() assert isinstance(result, Image) - assert result.data.shape[:2] == (8, 8) + assert image_view(result).shape[:2] == (8, 8) def test_observe_without_frames_fails( diff --git a/dimos/agents/skills/test_unitree_skill_container.py b/dimos/agents/skills/test_unitree_skill_container.py index d3d2e72f4ae..278c71eb370 100644 --- a/dimos/agents/skills/test_unitree_skill_container.py +++ b/dimos/agents/skills/test_unitree_skill_container.py @@ -16,14 +16,13 @@ import math from typing import Any +from dimos_generated.geometry_msgs.msg import Point, Pose, PoseStamped from langchain_core.messages import HumanMessage import pytest from dimos.core.core import rpc from dimos.core.module import Module -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Vector3 import Vector3 +from dimos.msgs.geometry import quaternion_euler, quaternion_from_euler from dimos.navigation.base import NavigationState from dimos.robot.unitree.unitree_skill_container import ( _UNITREE_COMMANDS, @@ -82,23 +81,25 @@ def test_did_you_mean() -> None: def _pose(x: float, y: float, yaw_deg: float) -> PoseStamped: return PoseStamped( - position=Vector3(x, y, 0.3), - orientation=Quaternion.from_euler(Vector3(0, 0, math.radians(yaw_deg))), + pose=Pose( + position=Point(x=x, y=y, z=0.3), + orientation=quaternion_from_euler(0, 0, math.radians(yaw_deg)), + ), ) def _xy(pose: PoseStamped) -> tuple[float, float]: - return pose.position.x, pose.position.y + return pose.pose.position.x, pose.pose.position.y def _yaw_deg(pose: PoseStamped) -> float: - return math.degrees(pose.orientation.to_euler().yaw) + return math.degrees(quaternion_euler(pose.pose.orientation)[2]) def test_move_to_world_keeps_heading() -> None: goal = _goal_pose(_pose(1, 1, 90), 4, 1, None, relative=False) assert _xy(goal) == pytest.approx((4, 1)) - assert goal.position.z == pytest.approx(0.3) + assert goal.pose.position.z == pytest.approx(0.3) assert _yaw_deg(goal) == pytest.approx(90) diff --git a/dimos/agents/test_generated_vlm.py b/dimos/agents/test_generated_vlm.py new file mode 100644 index 00000000000..2a456afca18 --- /dev/null +++ b/dimos/agents/test_generated_vlm.py @@ -0,0 +1,80 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Verify generated camera messages reach VLM requests through stream and RPC.""" + +import base64 + +import cv2 +from dimos_generated.sensor_msgs.msg import Image +from langchain_core.messages import AIMessage, HumanMessage +import numpy as np +import pytest + +from dimos.agents.vlm_agent import VLMAgent +from dimos.msgs.image import image_from_array + + +@pytest.fixture +def agent(mocker): + model = mocker.Mock() + model.invoke.return_value = AIMessage(content="red image") + mocker.patch("langchain.chat_models.init_chat_model", return_value=model) + value = VLMAgent() + try: + yield value, model + finally: + value.stop() + + +@pytest.mark.parametrize("use_stream", [False, True]) +def test_generated_image_reaches_model_as_jpeg(agent, use_stream): + value, model = agent + pixels = np.zeros((12, 16, 3), dtype=np.uint8) + pixels[..., 0] = 255 + original = image_from_array(pixels, encoding="rgb8") + image = Image.decode(original.encode()) + answers = [] + unsubscribe = value.answer_stream.subscribe(answers.append) + try: + if use_stream: + value._on_image(image) + value._on_query(HumanMessage(content="What color?")) + assert answers[0].content == "red image" + else: + assert value.query_image(image, "What color?") == "red image" + request = model.invoke.call_args.args[0][-1] + assert request.content[0] == {"type": "text", "text": "What color?"} + prefix, encoded = request.content[1]["image_url"]["url"].split(",", 1) + assert prefix == "data:image/jpeg;base64" + decoded = cv2.imdecode( + np.frombuffer(base64.b64decode(encoded), dtype=np.uint8), cv2.IMREAD_COLOR + ) + assert decoded.shape == (12, 16, 3) + assert decoded[0, 0].tolist() == [0, 0, 254] + assert image.encode() == original.encode() + finally: + unsubscribe() + + +def test_query_before_image_does_not_invoke_model(agent): + value, model = agent + answers = [] + unsubscribe = value.answer_stream.subscribe(answers.append) + try: + value._on_query(HumanMessage(content="What color?")) + assert answers[0].content == "No image available yet." + model.invoke.assert_not_called() + finally: + unsubscribe() diff --git a/dimos/agents/testing/vlm_stream_tester.py b/dimos/agents/testing/vlm_stream_tester.py index ffa9979b878..c3cacf04259 100644 --- a/dimos/agents/testing/vlm_stream_tester.py +++ b/dimos/agents/testing/vlm_stream_tester.py @@ -15,6 +15,7 @@ import threading import time +from dimos_generated.sensor_msgs.msg import Image from langchain_core.messages import AIMessage, HumanMessage from reactivex.disposable import Disposable @@ -23,7 +24,6 @@ from dimos.core.core import rpc from dimos.core.module import Module from dimos.core.stream import In, Out -from dimos.msgs.sensor_msgs.Image import Image from dimos.utils.logging_config import setup_logger logger = setup_logger() diff --git a/dimos/agents/vlm_agent.py b/dimos/agents/vlm_agent.py index ceef36d93cf..30c0cffd291 100644 --- a/dimos/agents/vlm_agent.py +++ b/dimos/agents/vlm_agent.py @@ -12,8 +12,10 @@ # See the License for the specific language governing permissions and # limitations under the License. +import base64 from typing import TYPE_CHECKING, Any +from dimos_generated.sensor_msgs.msg import Image from langchain_core.messages import AIMessage, HumanMessage, SystemMessage from reactivex.disposable import Disposable @@ -21,7 +23,7 @@ from dimos.core.core import rpc from dimos.core.module import Module, ModuleConfig from dimos.core.stream import In, Out -from dimos.msgs.sensor_msgs.Image import Image +from dimos.msgs.image import image_to_jpeg from dimos.utils.logging_config import setup_logger if TYPE_CHECKING: @@ -102,7 +104,11 @@ def _invoke(self, msg: HumanMessage, **kwargs: Any) -> AIMessage: def _invoke_image( self, image: Image, query: str, response_format: dict[str, Any] | None = None ) -> AIMessage: - content = [{"type": "text", "text": query}, *image.agent_encode()] + encoded = base64.b64encode(image_to_jpeg(image)).decode("ascii") + content: list[str | dict[str, Any]] = [ + {"type": "text", "text": query}, + {"type": "image_url", "image_url": {"url": f"data:image/jpeg;base64,{encoded}"}}, + ] kwargs: dict[str, Any] = {} if response_format: kwargs["response_format"] = response_format diff --git a/dimos/agents/vlm_agent_spec.py b/dimos/agents/vlm_agent_spec.py index a1bebbfe20a..5bca2b7ddef 100644 --- a/dimos/agents/vlm_agent_spec.py +++ b/dimos/agents/vlm_agent_spec.py @@ -14,7 +14,8 @@ from typing import Any, Protocol -from dimos.msgs.sensor_msgs.Image import Image +from dimos_generated.sensor_msgs.msg import Image + from dimos.spec.utils import Spec diff --git a/dimos/cli/bake/test_bake_e2e.py b/dimos/cli/bake/test_bake_e2e.py index 7da84bd4e99..8c7c5ace192 100644 --- a/dimos/cli/bake/test_bake_e2e.py +++ b/dimos/cli/bake/test_bake_e2e.py @@ -27,14 +27,17 @@ import subprocess import time +from dimos_generated.geometry_msgs.msg import Quaternion, Transform, TransformStamped, Vector3 +from dimos_generated.sensor_msgs.msg import PointCloud2 +from dimos_generated.std_msgs.msg import Header +from dimos_generated.tf2_msgs.msg import TFMessage import numpy as np import pytest from dimos.core.global_config import global_config from dimos.core.transport_factory import make_transport -from dimos.msgs.geometry_msgs.Transform import Transform -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 +from dimos.msgs.pointcloud import pointcloud_from_xyz +from dimos.msgs.time import time_from_nanoseconds pytestmark = pytest.mark.bake_e2e @@ -110,14 +113,16 @@ def await_listener(proc: subprocess.Popen[bytes], port: int, timeout: float = 20 raise AssertionError(f"the host never listened on {port}") -def ground_patch(ts: float) -> PointCloud2: +def ground_patch(stamp_ns: int) -> PointCloud2: """A flat floor patch below the sensor, enough for the planner to surface.""" grid = np.arange(-10, 11) * 0.1 xs, ys = np.meshgrid(grid, grid) points = np.stack([xs.ravel(), ys.ravel(), np.full(xs.size, -SENSOR_Z)], axis=1).astype( np.float32 ) - return PointCloud2.from_numpy(points, frame_id="lidar", timestamp=ts) + return pointcloud_from_xyz( + points, header=Header(stamp=time_from_nanoseconds(stamp_ns), frame_id="lidar") + ) @pytest.fixture @@ -155,7 +160,7 @@ def test_the_host_publishes_its_outputs_and_hides_the_suppressed_hop(baked, zeno transports.append(transport) lidar = make_transport("lidar", PointCloud2) - tf = make_transport("tf", Transform) + tf = make_transport("tf", TFMessage) transports += [lidar, tf] lidar.start() tf.start() @@ -163,18 +168,23 @@ def test_the_host_publishes_its_outputs_and_hides_the_suppressed_hop(baked, zeno deadline = time.monotonic() + 60 while time.monotonic() < deadline and "surface_map" not in seen: # Same stamp on both: the mapper drops a cloud it has no transform for. - ts = time.time() + stamp_ns = time.time_ns() tf.broadcast( None, - Transform( - translation=Vector3(0.0, 0.0, SENSOR_Z), - frame_id="odom", - child_frame_id="lidar", - ts=ts, + TFMessage( + transforms=[ + TransformStamped( + header=Header(stamp=time_from_nanoseconds(stamp_ns), frame_id="odom"), + child_frame_id="lidar", + transform=Transform( + translation=Vector3(z=SENSOR_Z), rotation=Quaternion(w=1.0) + ), + ) + ] ), ) time.sleep(0.05) - lidar.broadcast(None, ground_patch(ts)) + lidar.broadcast(None, ground_patch(stamp_ns)) time.sleep(0.25) assert proc.poll() is None, "the host exited before publishing anything" diff --git a/dimos/cli/bake/test_cli.py b/dimos/cli/bake/test_cli.py index 438cc02e409..2de7afd7c05 100644 --- a/dimos/cli/bake/test_cli.py +++ b/dimos/cli/bake/test_cli.py @@ -113,7 +113,7 @@ def test_emit_config_is_a_complete_stdin_blob() -> None: for section in blob["modules"].values(): assert isinstance(section["config"], dict) and section["config"] assert blob["modules"]["ray_tracing"]["topics"]["lidar"] == ( - "dimos/lidar/sensor_msgs.PointCloud2" + "dimos/lidar/sensor_msgs/msg/PointCloud2" ) assert blob["suppress"] == list(graph.suppressed_topics()) assert blob["qos"] == graph.qos() diff --git a/dimos/cli/bake/test_codegen.py b/dimos/cli/bake/test_codegen.py index ff7ed602dfe..c631fbe6031 100644 --- a/dimos/cli/bake/test_codegen.py +++ b/dimos/cli/bake/test_codegen.py @@ -64,7 +64,7 @@ def test_main_rs_lists_entries_with_their_thread_counts() -> None: main = render_main_rs("go2-nav", [MAPPER, threaded], GRAPH) assert ' ModuleEntry::new::("mapper"),' in main assert ' ModuleEntry::new::("planner").threads(2),' in main - assert ' "dimos/local_map/sensor_msgs.PointCloud2",' in main + assert ' "dimos/local_map/sensor_msgs/msg/PointCloud2",' in main assert main.startswith("// GENERATED BY dimos bake") assert 'name: "go2-nav",' in main assert 'include_str!("graph.json")' in main @@ -87,11 +87,11 @@ def test_generate_crate_writes_the_blobs_the_host_includes(tmp_path: Path) -> No assert crate == tmp_path / "build" / "dimos-bake" / "go2-nav" topics = json.loads((crate / "src" / "default_topics.json").read_text()) - assert topics["mapper"]["lidar"] == "dimos/lidar/sensor_msgs.PointCloud2" - assert topics["planner"]["local_map"] == "dimos/local_map/sensor_msgs.PointCloud2" + assert topics["mapper"]["lidar"] == "dimos/lidar/sensor_msgs/msg/PointCloud2" + assert topics["planner"]["local_map"] == "dimos/local_map/sensor_msgs/msg/PointCloud2" qos = json.loads((crate / "src" / "default_qos.json").read_text()) - assert qos["dimos/local_map/sensor_msgs.PointCloud2"]["congestion_control"] == "drop" + assert qos["dimos/local_map/sensor_msgs/msg/PointCloud2"]["congestion_control"] == "drop" graph = json.loads((crate / "src" / "graph.json").read_text()) assert graph["host"] == "go2-nav" diff --git a/dimos/cli/bake/test_discovery.py b/dimos/cli/bake/test_discovery.py index b3cb497e491..905deee3bbf 100644 --- a/dimos/cli/bake/test_discovery.py +++ b/dimos/cli/bake/test_discovery.py @@ -41,10 +41,10 @@ threads = 2 [package.metadata.dimos.module.demo.inputs] -lidar = "sensor_msgs.PointCloud2" +lidar = "sensor_msgs/msg/PointCloud2" [package.metadata.dimos.module.demo.outputs] -global_map = "sensor_msgs.PointCloud2" +global_map = "sensor_msgs/msg/PointCloud2" """ @@ -62,8 +62,8 @@ def test_parses_a_registered_module(tmp_path: Path) -> None: assert info.rust_path == "demo::module::Demo" assert info.python_ref == "dimos.demo.module:Demo" assert info.threads == 2 - assert info.inputs == {"lidar": "sensor_msgs.PointCloud2"} - assert info.outputs == {"global_map": "sensor_msgs.PointCloud2"} + assert info.inputs == {"lidar": "sensor_msgs/msg/PointCloud2"} + assert info.outputs == {"global_map": "sensor_msgs/msg/PointCloud2"} def test_a_crate_without_the_metadata_table_is_not_a_module(tmp_path: Path) -> None: @@ -84,7 +84,7 @@ def test_a_missing_required_key_is_an_error(tmp_path: Path) -> None: def test_a_non_string_msg_type_is_an_error(tmp_path: Path) -> None: - manifest = MANIFEST.replace('lidar = "sensor_msgs.PointCloud2"', "lidar = 3") + manifest = MANIFEST.replace('lidar = "sensor_msgs/msg/PointCloud2"', "lidar = 3") with pytest.raises(BakeError, match="lidar"): parse_manifest(write_crate(tmp_path, "dimos/demo/rust", manifest)) diff --git a/dimos/cli/bake/test_graph.py b/dimos/cli/bake/test_graph.py index 1641770470d..a3a9aa29464 100644 --- a/dimos/cli/bake/test_graph.py +++ b/dimos/cli/bake/test_graph.py @@ -22,8 +22,8 @@ from dimos.cli.bake.errors import BakeError from dimos.cli.bake.graph import Connection, Graph, build_graph, parse_remap, render -PC2 = "sensor_msgs.PointCloud2" -POSE = "geometry_msgs.PoseStamped" +PC2 = "sensor_msgs/msg/PointCloud2" +POSE = "geometry_msgs/msg/PoseStamped" def module( @@ -155,20 +155,20 @@ def test_rendered_graph_is_grouped_by_direction() -> None: "Host `host`: mapper, planner", "", "Internal connections:", - " dimos/local_map/sensor_msgs.PointCloud2 sensor_msgs.PointCloud2 [SUPPRESSED]", + " dimos/local_map/sensor_msgs/msg/PointCloud2 sensor_msgs/msg/PointCloud2 [SUPPRESSED]", " out mapper.local_map", " in planner.local_map", "", "External inputs (subscribed):", - " dimos/goal/geometry_msgs.PoseStamped geometry_msgs.PoseStamped", + " dimos/goal/geometry_msgs/msg/PoseStamped geometry_msgs/msg/PoseStamped", " in planner.goal", - " dimos/lidar/sensor_msgs.PointCloud2 sensor_msgs.PointCloud2", + " dimos/lidar/sensor_msgs/msg/PointCloud2 sensor_msgs/msg/PointCloud2", " in mapper.lidar", "", "External outputs (published):", - " dimos/path/geometry_msgs.PoseStamped geometry_msgs.PoseStamped", + " dimos/path/geometry_msgs/msg/PoseStamped geometry_msgs/msg/PoseStamped", " out planner.path", - " dimos/region_bounds/geometry_msgs.PoseStamped geometry_msgs.PoseStamped", + " dimos/region_bounds/geometry_msgs/msg/PoseStamped geometry_msgs/msg/PoseStamped", " out mapper.region_bounds", ] diff --git a/dimos/cli/cameracalibrate/cameracalibrate.py b/dimos/cli/cameracalibrate/cameracalibrate.py index 5528a60f08f..2445922e9eb 100644 --- a/dimos/cli/cameracalibrate/cameracalibrate.py +++ b/dimos/cli/cameracalibrate/cameracalibrate.py @@ -30,10 +30,13 @@ # Use setdefault so an explicit OPENCV_OPENCL_RUNTIME from the environment still wins. os.environ.setdefault("OPENCV_OPENCL_RUNTIME", "disabled") +from dimos_generated.sensor_msgs.msg import Image import numpy as np import typer import yaml +from dimos.msgs.image import image_to_bgr + _IMAGE_EXTS = frozenset({".png", ".jpg", ".jpeg"}) @@ -102,7 +105,7 @@ def write_camera_info_yaml( ) -> None: """Write ROS-style CameraInfo YAML loadable by dimos CameraInfo helpers. - The emitted schema is accepted by ``CameraInfo.from_yaml``. + The emitted schema is accepted by ``dimos.msgs.camera_info.camera_info_from_yaml``. """ k = np.asarray(K, dtype=np.float64).reshape(3, 3) d = np.asarray(D, dtype=np.float64).ravel() @@ -544,11 +547,10 @@ def _capture_frames_from_topic( ``topic_uri`` follows the pubsub registry format ``":"``, e.g. ``"jpeg_lcm:/color_image"`` or ``"pshm:color_image"``. The publisher must - emit ``sensor_msgs.Image`` messages; ``Image.to_opencv()`` normalizes the + emit ``sensor_msgs.Image`` messages; ``image_to_bgr()`` normalizes the payload to BGR before detection. Raises ``RuntimeError`` if no frames arrive within ``timeout_sec``. """ - from dimos.msgs.sensor_msgs.Image import Image from dimos.protocol.pubsub.registry import subscribe_pubsub_uri if target_count < 1: @@ -560,7 +562,7 @@ def _capture_frames_from_topic( def _on_image(msg: Any) -> None: try: - arr = msg.to_opencv() + arr = image_to_bgr(msg) except (AttributeError, ValueError): return with lock: diff --git a/dimos/cli/cameracalibrate/test_cameracalibrate.py b/dimos/cli/cameracalibrate/test_cameracalibrate.py index e639c52dbda..638ce43ab04 100644 --- a/dimos/cli/cameracalibrate/test_cameracalibrate.py +++ b/dimos/cli/cameracalibrate/test_cameracalibrate.py @@ -17,6 +17,8 @@ from unittest.mock import MagicMock import cv2 +from dimos_generated.sensor_msgs.msg import Image +from dimos_generated.std_msgs.msg import Header import numpy as np import pytest from typer.testing import CliRunner @@ -31,7 +33,8 @@ load_frames_from_folder, write_camera_info_yaml, ) -from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo as DimosCameraInfo +from dimos.msgs.camera_info import camera_info_from_yaml, intrinsic_matrix +from dimos.msgs.image import image_from_array def _synthetic_chessboard_gray( @@ -157,15 +160,15 @@ def test_cli_folder_with_synthetic_images_writes_yaml_preview_and_camera_info( assert preview_image is not None assert preview_image.shape == (480, 640, 3) - dimos_info = DimosCameraInfo.from_yaml(str(out)) + dimos_info = camera_info_from_yaml(out, header=Header(frame_id="camera_optical")) assert dimos_info.width == 640 assert dimos_info.height == 480 assert dimos_info.distortion_model == "plumb_bob" - assert dimos_info.frame_id == "camera_optical" - assert dimos_info.get_K_matrix().shape == (3, 3) - assert dimos_info.get_D_coeffs().shape == (5,) - assert dimos_info.get_R_matrix().shape == (3, 3) - assert dimos_info.get_P_matrix().shape == (3, 4) + assert dimos_info.header.frame_id == "camera_optical" + assert intrinsic_matrix(dimos_info).shape == (3, 3) + assert np.asarray(dimos_info.d).shape == (5,) + assert np.asarray(dimos_info.r).reshape(3, 3).shape == (3, 3) + assert np.asarray(dimos_info.p).reshape(3, 4).shape == (3, 4) def test_cli_help_lists_cameracalibrate_flags() -> None: @@ -227,7 +230,7 @@ def test_cli_folder_writes_only_explicit_yaml_and_prints_rms(tmp_path: Path) -> assert out.exists() preview = tmp_path / "camera_info.preview.png" assert not preview.exists() - dimos_info = DimosCameraInfo.from_yaml(str(out)) + dimos_info = camera_info_from_yaml(out, header=Header(frame_id="camera_optical")) assert dimos_info.width == 640 assert dimos_info.height == 480 assert dimos_info.distortion_model == "plumb_bob" @@ -458,12 +461,11 @@ def test_capture_frames_from_topic_mocked_space_fills_target( monkeypatch: pytest.MonkeyPatch, ) -> None: """SPACE accepts frames delivered over a fake pubsub subscription.""" - from dimos.msgs.sensor_msgs.Image import Image cols, rows = 9, 6 gray = _synthetic_chessboard_gray(640, 480, cols, rows, square_px=40) bgr = cv2.cvtColor(gray, cv2.COLOR_GRAY2BGR) - image_msg = Image.from_opencv(bgr) + image_msg = image_from_array(bgr, encoding="bgr8") record: dict[str, object] = {} monkeypatch.setattr( @@ -521,12 +523,11 @@ def test_cli_topic_source_uri_passes_through( tmp_path: Path, ) -> None: """``--source topic --topic `` forwards the URI verbatim to the registry.""" - from dimos.msgs.sensor_msgs.Image import Image cols, rows = 9, 6 gray = _synthetic_chessboard_gray(640, 480, cols, rows, square_px=40) bgr = cv2.cvtColor(gray, cv2.COLOR_GRAY2BGR) - image_msg = Image.from_opencv(bgr) + image_msg = image_from_array(bgr, encoding="bgr8") record: dict[str, object] = {} monkeypatch.setattr( @@ -802,16 +803,16 @@ def test_write_camera_info_yaml_round_trip_matches_k_d_size_and_model(tmp_path: D=D, distortion_model="plumb_bob", ) - dimos_info = DimosCameraInfo.from_yaml(path) + dimos_info = camera_info_from_yaml(path, header=Header(frame_id="camera_optical")) assert dimos_info.width == 640 assert dimos_info.height == 480 assert dimos_info.distortion_model == "plumb_bob" - assert np.allclose(dimos_info.get_K_matrix(), K) - assert np.allclose(dimos_info.get_D_coeffs(), D) + assert np.allclose(intrinsic_matrix(dimos_info), K) + assert np.allclose(np.asarray(dimos_info.d), D) def test_write_camera_info_yaml_round_trip(tmp_path: Path) -> None: - """YAML written by ``write_camera_info_yaml`` round-trips through ``CameraInfo.from_yaml``.""" + """YAML written by ``write_camera_info_yaml`` round-trips through ``camera_info_from_yaml``.""" K = np.array([[600.0, 0.5, 400.0], [0.0, 605.0, 300.5], [0.0, 0.0, 1.0]], dtype=np.float64) D = np.array([-0.12, 0.08, 0.002, -0.001, 0.0], dtype=np.float64) R = np.array([[0.999, -0.01, 0.0], [0.01, 0.999, 0.0], [0.0, 0.0, 1.0]], dtype=np.float64) @@ -831,15 +832,15 @@ def test_write_camera_info_yaml_round_trip(tmp_path: Path) -> None: P=P, distortion_model="plumb_bob", ) - dimos_info = DimosCameraInfo.from_yaml(path) + dimos_info = camera_info_from_yaml(path, header=Header(frame_id="camera_optical")) assert dimos_info.width == 800 assert dimos_info.height == 600 assert dimos_info.distortion_model == "plumb_bob" - assert dimos_info.frame_id == "camera_optical" - assert np.allclose(dimos_info.get_K_matrix(), K) - assert np.allclose(dimos_info.get_D_coeffs(), D) - assert np.allclose(dimos_info.get_R_matrix(), R) - assert np.allclose(dimos_info.get_P_matrix(), P) + assert dimos_info.header.frame_id == "camera_optical" + assert np.allclose(intrinsic_matrix(dimos_info), K) + assert np.allclose(np.asarray(dimos_info.d), D) + assert np.allclose(np.asarray(dimos_info.r).reshape(3, 3), R) + assert np.allclose(np.asarray(dimos_info.p).reshape(3, 4), P) def test_write_camera_info_yaml_custom_r_p_and_distortion_model(tmp_path: Path) -> None: @@ -859,11 +860,11 @@ def test_write_camera_info_yaml_custom_r_p_and_distortion_model(tmp_path: Path) P=P, distortion_model="rational_polynomial", ) - dimos_info = DimosCameraInfo.from_yaml(path) + dimos_info = camera_info_from_yaml(path, header=Header(frame_id="camera_optical")) assert dimos_info.width == 320 assert dimos_info.height == 240 assert dimos_info.distortion_model == "rational_polynomial" - assert np.allclose(dimos_info.get_K_matrix(), K) - assert np.allclose(dimos_info.get_D_coeffs(), D) - assert np.allclose(dimos_info.get_R_matrix(), R) - assert np.allclose(dimos_info.get_P_matrix(), P) + assert np.allclose(intrinsic_matrix(dimos_info), K) + assert np.allclose(np.asarray(dimos_info.d), D) + assert np.allclose(np.asarray(dimos_info.r).reshape(3, 3), R) + assert np.allclose(np.asarray(dimos_info.p).reshape(3, 4), P) diff --git a/dimos/cli/commands/build.py b/dimos/cli/commands/build.py new file mode 100644 index 00000000000..410970f4459 --- /dev/null +++ b/dimos/cli/commands/build.py @@ -0,0 +1,45 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Build external message packages without rebuilding the DimOS runtime.""" + +from importlib import import_module +from pathlib import Path +from subprocess import CalledProcessError + +import typer + + +def build( + project: Path = typer.Option(Path("."), "--project", help="Message project directory."), + language: list[str] = typer.Option( + [], "--language", help="Build only this language; repeatable." + ), + install: bool = typer.Option( + False, "--install", help="Also install the wheel into the active virtualenv." + ), + offline: bool = typer.Option(False, "--offline", help="Require cached Cargo dependencies."), +) -> None: + """Discover, generate and build this project's custom message packages.""" + try: + core = import_module("dimos_message_build.build") + config = core.Project.load(project) + artifacts = core.build_project( + config, tuple(language) or None, install=install, offline=offline + ) + for kind, path in artifacts.items(): + typer.echo(f"{kind}: {path}") + except (ValueError, FileNotFoundError, CalledProcessError) as error: + typer.echo(f"build: {error}", err=True) + raise typer.Exit(1) from error diff --git a/dimos/cli/dimos.py b/dimos/cli/dimos.py index 06899c059bf..73b9d5d6557 100644 --- a/dimos/cli/dimos.py +++ b/dimos/cli/dimos.py @@ -51,6 +51,7 @@ from dimos.cli.cloud import login as cloud_login, logout as cloud_logout, whoami as cloud_whoami from dimos.cli.commands.apriltag import apriltag from dimos.cli.commands.bake import bake +from dimos.cli.commands.build import build from dimos.cli.commands.cameracalibrate import cameracalibrate from dimos.cli.commands.data import data_app from dimos.cli.commands.dataprep import dataprep_app @@ -108,6 +109,7 @@ def cli_main() -> None: main.add_typer(hardware_app, name="hardware") main.add_typer(data_app, name="data") main.add_typer(go2tool_app, name="go2tool") +main.command()(build) main.command()(shell) main.add_typer(cache_app, name="cache") main.command("login")(cloud_login) diff --git a/dimos/cli/hardware/g1.py b/dimos/cli/hardware/g1.py index 636c60c41e1..b41ab602078 100644 --- a/dimos/cli/hardware/g1.py +++ b/dimos/cli/hardware/g1.py @@ -19,10 +19,10 @@ import time from typing import Any, NoReturn, Protocol, TypeGuard +from dimos_generated.sensor_msgs.msg import JointState import typer from dimos.control.tasks.trajectory_task.trajectory_task import JOINT_TRAJECTORY_TASK_NAME -from dimos.msgs.sensor_msgs.JointState import JointState from dimos.porcelain.dimos import Dimos from dimos.porcelain.module_handle import ModuleHandle from dimos.robot.unitree.g1.ready_pose import G1_READY_JOINTS, G1_READY_SPEED_SCALE diff --git a/dimos/cli/spy/test_spy.py b/dimos/cli/spy/test_spy.py index 130c393fca3..11fe0d18dea 100644 --- a/dimos/cli/spy/test_spy.py +++ b/dimos/cli/spy/test_spy.py @@ -26,6 +26,7 @@ import time from typing import Any +from dimos_generated.geometry_msgs.msg import Vector3 import pytest from dimos.cli.spy.core import ( @@ -39,13 +40,12 @@ default_sources, split_type_suffix, ) -from dimos.msgs.geometry_msgs.Vector3 import Vector3 from dimos.protocol.pubsub.impl.lcmpubsub import LCMPubSubBase, Topic from dimos.protocol.pubsub.impl.zenohpubsub import ZenohPubSubBase from dimos.protocol.service.zenohservice import ZenohSessionPool -VEC = Vector3(1.0, 2.0, 3.0) -VEC_BYTES = VEC.lcm_encode() +VEC = Vector3(x=1.0, y=2.0, z=3.0) +VEC_BYTES = VEC.encode() # TopicStats: pure, deterministic (injected timestamps, no sleeps) @@ -106,10 +106,13 @@ def test_topic_stats_history_eviction_keeps_totals(): def test_split_type_suffix(): - assert split_type_suffix("/cmd_vel#geometry_msgs.Twist") == ("/cmd_vel", "geometry_msgs.Twist") - assert split_type_suffix("dimos/cmd_vel#geometry_msgs.Twist") == ( + assert split_type_suffix("/cmd_vel#geometry_msgs/msg/Twist") == ( + "/cmd_vel", + "geometry_msgs/msg/Twist", + ) + assert split_type_suffix("dimos/cmd_vel#geometry_msgs/msg/Twist") == ( "dimos/cmd_vel", - "geometry_msgs.Twist", + "geometry_msgs/msg/Twist", ) assert split_type_suffix("/plain") == ("/plain", None) @@ -198,7 +201,7 @@ def _assert_no_decode(monkeypatch): def boom(*a, **k): raise AssertionError("spy decoded a payload on the hot path") - monkeypatch.setattr(Vector3, "lcm_decode", staticmethod(boom)) + monkeypatch.setattr(Vector3, "decode", staticmethod(boom)) @pytest.fixture @@ -324,15 +327,15 @@ def test_transport_spy_merges_sources_and_totals(): spy.start() assert a.started and b.started - a.emit("/t#geometry_msgs.Twist", 10) - a.emit("/t#geometry_msgs.Twist", 10) - b.emit("dimos/t#geometry_msgs.Twist", 20) + a.emit("/t#geometry_msgs/msg/Twist", 10) + a.emit("/t#geometry_msgs/msg/Twist", 10) + b.emit("dimos/t#geometry_msgs/msg/Twist", 20) a.emit("/u", 5) snap = spy.snapshot() - assert snap[SpyKey("lcm", "/t#geometry_msgs.Twist")].total_msgs == 2 - assert snap[SpyKey("lcm", "/t#geometry_msgs.Twist")].total_bytes == 20 - assert snap[SpyKey("zenoh", "dimos/t#geometry_msgs.Twist")].total_msgs == 1 + assert snap[SpyKey("lcm", "/t#geometry_msgs/msg/Twist")].total_msgs == 2 + assert snap[SpyKey("lcm", "/t#geometry_msgs/msg/Twist")].total_bytes == 20 + assert snap[SpyKey("zenoh", "dimos/t#geometry_msgs/msg/Twist")].total_msgs == 1 assert snap[SpyKey("lcm", "/u")].total_bytes == 5 assert spy.totals.total_msgs == 4 assert spy.totals.total_bytes == 45 @@ -448,4 +451,4 @@ def test_fake_source_satisfies_protocol(): def test_subscribe_decoded_is_not_implemented_in_v1(): src = LCMSpySource() with pytest.raises(NotImplementedError): - src.subscribe_decoded("/x#geometry_msgs.Vector3", lambda m: None) + src.subscribe_decoded("/x#geometry_msgs/msg/Vector3", lambda m: None) diff --git a/dimos/cli/test_topic.py b/dimos/cli/test_topic.py index 1e35815ef9d..ed7340bb1da 100644 --- a/dimos/cli/test_topic.py +++ b/dimos/cli/test_topic.py @@ -12,15 +12,15 @@ # See the License for the specific language governing permissions and # limitations under the License. +from dimos_generated.geometry_msgs.msg import Twist, Vector3 +from dimos_generated.sensor_msgs.msg import CameraInfo +from dimos_generated.std_msgs.msg import Header import pytest from dimos.cli.topic import _build_eval_context, _decode_typed_lcm_message, topic_send from dimos.core.global_config import global_config from dimos.core.transport import PubSubTransport from dimos.core.transport_factory import make_transport -from dimos.msgs.geometry_msgs.Twist import Twist -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo from dimos.protocol.pubsub.impl.lcmpubsub import LCM, Topic from dimos.utils.testing.collector import CallbackCollector @@ -30,18 +30,18 @@ def test_decode_typed_lcm_message_resolves_message_submodule() -> None: width=1920, height=1080, distortion_model="plumb_bob", - frame_id="camera_optical", + header=Header(frame_id="camera_optical"), ) decoded = _decode_typed_lcm_message( - "/camera_info#sensor_msgs.CameraInfo", - msg.lcm_encode(), + "/camera_info#sensor_msgs/msg/CameraInfo", + msg.encode(), ) assert isinstance(decoded, CameraInfo) assert decoded.width == 1920 assert decoded.height == 1080 - assert decoded.frame_id == "camera_optical" + assert decoded.header.frame_id == "camera_optical" assert decoded.distortion_model == "plumb_bob" @@ -71,14 +71,14 @@ def capturing_make_transport(name: str, msg_type: type) -> PubSubTransport[objec bus = LCM(url=lcm_url) bus.start() collector = CallbackCollector(1) - bus.subscribe(Topic(topic="/test_topic_send", lcm_type=Twist), collector) + bus.subscribe(Topic(topic="/test_topic_send", msg_type=Twist), collector) try: - topic_send("/test_topic_send", "Twist(Vector3(0.5, 0, 0), Vector3(0, 0, 0))") + topic_send("/test_topic_send", "Twist(linear=Vector3(x=0.5), angular=Vector3())") collector.wait() finally: for transport in transports: transport.stop() bus.stop() - assert collector.results[0][0] == Twist(Vector3(0.5, 0, 0), Vector3(0, 0, 0)) + assert collector.results[0][0] == Twist(linear=Vector3(x=0.5), angular=Vector3()) diff --git a/dimos/cli/topic.py b/dimos/cli/topic.py index 1e5dd05dbd7..d9896ad58b2 100644 --- a/dimos/cli/topic.py +++ b/dimos/cli/topic.py @@ -15,49 +15,40 @@ from __future__ import annotations from collections.abc import Callable -import importlib -import pkgutil import re import time +from types import SimpleNamespace +from typing import cast import typer from dimos.core.global_config import global_config from dimos.core.transport import PubSubTransport from dimos.core.transport_factory import make_transport, transport_topic +from dimos.message_codegen.registry import message_types +from dimos.msgs.helpers import resolve_msg_type from dimos.protocol.pubsub.impl.lcmpubsub import LCMPubSubBase, Topic from dimos.protocol.pubsub.impl.zenohpubsub import Zenoh -_modules_to_try = [ - "dimos.msgs.geometry_msgs", - "dimos.msgs.nav_msgs", - "dimos.msgs.sensor_msgs", - "dimos.msgs.std_msgs", - "dimos.msgs.vision_msgs", - "dimos.msgs.tf2_msgs", -] - def _resolve_type(type_name: str) -> type: - for module_name in _modules_to_try: - try: - module = importlib.import_module(f"{module_name}.{type_name}") - except ImportError: - continue - if hasattr(module, type_name): - return getattr(module, type_name) # type: ignore[no-any-return] - - raise ValueError(f"Could not find type '{type_name}' in any known message modules") + types = message_types() + if type_name in types: + return cast("type", types[type_name]) + matches = [value for name, value in types.items() if name.rsplit("/", 1)[-1] == type_name] + if len(matches) == 1: + return cast("type", matches[0]) + if matches: + raise ValueError(f"Ambiguous message type {type_name!r}; use package/msg/Type") + raise ValueError(f"Unknown installed message type {type_name!r}") def _decode_typed_lcm_message(channel: str, data: bytes) -> object: - from dimos.msgs.helpers import resolve_msg_type - - _, msg_name = channel.split("#", 1) # e.g. "nav_msgs.Odometry" + _, msg_name = channel.split("#", 1) # e.g. "nav_msgs/msg/Odometry" cls = resolve_msg_type(msg_name) if cls is None: raise ValueError(f"Could not resolve message type from channel: {channel}") - return cls.lcm_decode(data) + return cls.decode(data) def _listen_forever(listening_msg: str, on_stop: Callable[[], None] = lambda: None) -> None: @@ -72,7 +63,7 @@ def _listen_forever(listening_msg: str, on_stop: Callable[[], None] = lambda: No def topic_echo(topic: str, type_name: str | None) -> None: - # Explicit mode (legacy): backend chosen by make_transport from global_config. + # Explicit message type: backend chosen by make_transport from global_config. if type_name is not None: msg_type = _resolve_type(type_name) transport: PubSubTransport[object] = make_transport(topic, msg_type) @@ -94,7 +85,7 @@ def _topic_echo_inferred_lcm(topic: str) -> None: autoconf(check_only=True) - # Listen on /topic#pkg.Msg and decode from the msg_name suffix. + # Listen on /topic#pkg/msg/Type and decode from the msg_name suffix. bus = LCMPubSubBase() bus.start() # starts threaded handle loop @@ -107,7 +98,7 @@ def on_msg(channel: str, data: bytes) -> None: bus.l.subscribe(typed_pattern, on_msg) _listen_forever( - f"Listening on {topic} (inferring from typed LCM channels like '{topic}#pkg.Msg')... " + f"Listening on {topic} (inferring from typed LCM channels like '{topic}#pkg/msg/Type')... " "(Ctrl+C to stop)", bus.stop, ) @@ -118,37 +109,32 @@ def _topic_echo_inferred_zenoh(topic: str) -> None: bus = Zenoh() bus.start() - # Typed Zenoh keys embed the type as a trailing segment ("dimos/topic/pkg.Msg"); + # Typed Zenoh keys embed the type as trailing segments ("dimos/topic/pkg/msg/Type"); # a wildcard subscription decodes each message from that suffix. Untyped keys # don't resolve to a type and are skipped by the encoder. The ignore reflects the # pattern Topic vs the encoder's concrete-topic protocol (see lcmpubsub.py). bus.subscribe(Topic(f"{key}/**"), lambda msg, _topic: print(msg)) # type: ignore[arg-type] _listen_forever( - f"Listening on {topic} (inferring from typed Zenoh keys like '{key}/pkg.Msg')... " + f"Listening on {topic} (inferring from typed Zenoh keys like '{key}/pkg/msg/Type')... " "(Ctrl+C to stop)", bus.stop, ) def _build_eval_context() -> dict[str, object]: - # The msgs packages are namespace packages (no __init__.py), so walk their - # submodules; each message file defines a class of the same name. - eval_context: dict[str, object] = {} - for package_name in _modules_to_try: - package = importlib.import_module(package_name) - for module_info in pkgutil.iter_modules(package.__path__): - name = module_info.name - if name.startswith("test_"): - continue - try: - submodule = importlib.import_module(f"{package_name}.{name}") - except ImportError: - continue - obj = getattr(submodule, name, None) - if obj is not None: - eval_context[name] = obj - return eval_context + types = message_types() + context: dict[str, object] = {} + packages: dict[str, SimpleNamespace] = {} + short_names: dict[str, list[type]] = {} + for name, cls in types.items(): + package, _, short = name.split("/") + namespace = packages.setdefault(package, SimpleNamespace(msg=SimpleNamespace())) + setattr(namespace.msg, short, cls) + short_names.setdefault(short, []).append(cls) + context.update(packages) + context.update({name: classes[0] for name, classes in short_names.items() if len(classes) == 1}) + return context def topic_send(topic: str, message_expr: str) -> None: diff --git a/dimos/conftest.py b/dimos/conftest.py index 7e185b36528..a5ed193ce0d 100644 --- a/dimos/conftest.py +++ b/dimos/conftest.py @@ -171,6 +171,25 @@ def pytest_sessionstart(session): _arm_crash_dumps() +@pytest.hookimpl(optionalhook=True) +def pytest_testnodedown(node, error): + """Expose a crashed worker's dump before a later job timeout loses it.""" + if error is None: + return + base = pathlib.Path( + os.environ.get("DIMOS_CRASH_DIR") + or os.path.join(os.environ.get("RUNNER_TEMP") or tempfile.gettempdir(), "pytest-crash") + ) + for path in sorted(base.glob(f"crash-{node.gateway.id}-*.log")): + try: + dump = path.read_text(errors="replace") + except OSError as exc: + print(f"Cannot read worker crash diagnostic {path.name}: {exc}", flush=True) + continue + if dump: + print(f"\nWorker crash diagnostic {path.name}:\n{dump}", flush=True) + + def pytest_ignore_collect(collection_path: pathlib.Path) -> bool | None: # Nested Python projects own their dependencies and test invocation. if collection_path.is_dir() and (collection_path / "pyproject.toml").is_file(): @@ -196,10 +215,6 @@ def pytest_configure(config): "markers", "bake_e2e: dimos bake e2e (builds rust); runs in the CI rust job", ) - config.addinivalue_line( - "markers", - "native_e2e: native module e2e (builds rust); runs in the CI rust job", - ) config.addinivalue_line("markers", "skipif_in_ci: skip when CI env var is set") config.addinivalue_line("markers", "skipif_no_openai: skip when OPENAI_API_KEY is not set") config.addinivalue_line("markers", "skipif_no_alibaba: skip when ALIBABA_API_KEY is not set") diff --git a/dimos/control/_control_test_helpers.py b/dimos/control/_control_test_helpers.py index 0b925b41392..b9460c95802 100644 --- a/dimos/control/_control_test_helpers.py +++ b/dimos/control/_control_test_helpers.py @@ -23,14 +23,14 @@ from typing import Any +from dimos_generated.geometry_msgs.msg import Pose, PoseStamped + from dimos.control.task import ( BaseControlTask, CoordinatorState, JointCommandOutput, ResourceClaim, ) -from dimos.msgs.geometry_msgs.Pose import Pose -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped class RecordingTask(BaseControlTask): diff --git a/dimos/control/benchmarking/benchmark.py b/dimos/control/benchmarking/benchmark.py index fd22f507760..9de58d99463 100644 --- a/dimos/control/benchmarking/benchmark.py +++ b/dimos/control/benchmarking/benchmark.py @@ -51,6 +51,9 @@ import time from typing import Any, Literal +from dimos_generated.geometry_msgs.msg import Point, Pose, PoseStamped, Twist +from dimos_generated.nav_msgs.msg import Path as NavPath +from dimos_generated.std_msgs.msg import Float32, Header, Int8 import numpy as np from reactivex.disposable import Disposable @@ -68,13 +71,7 @@ from dimos.core.core import rpc from dimos.core.module import Module, ModuleConfig from dimos.core.stream import In, Out -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Twist import Twist -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.msgs.nav_msgs.Path import Path as NavPath -from dimos.msgs.std_msgs.Float32 import Float32 -from dimos.msgs.std_msgs.Int8 import Int8 +from dimos.msgs.geometry import quaternion_euler, quaternion_from_euler from dimos.utils.logging_config import setup_logger logger = setup_logger() @@ -119,18 +116,23 @@ def shift_path_to_start_at_pose(path: NavPath, start_pose: PoseStamped) -> NavPa at the robot's current pose, so the operator only has to roughly aim the robot. Scoring is then in the executed frame regardless of where the plant starts.""" - px0, py0 = path.poses[0].position.x, path.poses[0].position.y - pyaw0 = path.poses[0].orientation.euler[2] - sx, sy = start_pose.position.x, start_pose.position.y - dyaw = start_pose.orientation.euler[2] - pyaw0 + px0, py0 = path.poses[0].pose.position.x, path.poses[0].pose.position.y + pyaw0 = quaternion_euler(path.poses[0].pose.orientation)[2] + sx, sy = start_pose.pose.position.x, start_pose.pose.position.y + dyaw = quaternion_euler(start_pose.pose.orientation)[2] - pyaw0 cd, sd = math.cos(dyaw), math.sin(dyaw) new = [] for p in path.poses: - rx, ry = p.position.x - px0, p.position.y - py0 + rx, ry = p.pose.position.x - px0, p.pose.position.y - py0 new.append( PoseStamped( - position=Vector3(sx + rx * cd - ry * sd, sy + rx * sd + ry * cd, 0.0), - orientation=Quaternion.from_euler(Vector3(0.0, 0.0, p.orientation.euler[2] + dyaw)), + header=Header(frame_id=""), + pose=Pose( + position=Point(x=sx + rx * cd - ry * sd, y=sy + rx * sd + ry * cd, z=0.0), + orientation=quaternion_from_euler( + 0.0, 0.0, quaternion_euler(p.pose.orientation)[2] + dyaw + ), + ), ) ) return NavPath(poses=new) @@ -172,7 +174,10 @@ def from_path( velocity_threshold: float, timeout: float, ) -> RunRecording: - ref = [[p.position.x, p.position.y, p.orientation.euler[2]] for p in reference.poses] + ref = [ + [p.pose.position.x, p.pose.position.y, quaternion_euler(p.pose.orientation)[2]] + for p in reference.poses + ] return cls( robot=robot, path=path_name, @@ -197,8 +202,11 @@ def reference_path(self) -> NavPath: return NavPath( poses=[ PoseStamped( - position=Vector3(x, y, 0.0), - orientation=Quaternion.from_euler(Vector3(0.0, 0.0, yaw)), + header=Header(frame_id=""), + pose=Pose( + position=Point(x=x, y=y, z=0.0), + orientation=quaternion_from_euler(0.0, 0.0, yaw), + ), ) for x, y, yaw in self.reference ] @@ -246,12 +254,12 @@ def on_odom(self, pose: PoseStamped, now: float | None = None) -> None: else: dt = now - self._prev_t if dt > 0: - dx = pose.position.x - self._prev_pose.position.x - dy = pose.position.y - self._prev_pose.position.y - y1 = pose.orientation.euler[2] - dyaw = (y1 - self._prev_pose.orientation.euler[2] + math.pi) % ( - 2 * math.pi - ) - math.pi + dx = pose.pose.position.x - self._prev_pose.pose.position.x + dy = pose.pose.position.y - self._prev_pose.pose.position.y + y1 = quaternion_euler(pose.pose.orientation)[2] + dyaw = ( + y1 - quaternion_euler(self._prev_pose.pose.orientation)[2] + math.pi + ) % (2 * math.pi) - math.pi c, s = math.cos(y1), math.sin(y1) bx = (dx / dt) * c + (dy / dt) * s by = -(dx / dt) * s + (dy / dt) * c @@ -263,9 +271,9 @@ def on_odom(self, pose: PoseStamped, now: float | None = None) -> None: self._ticks.append( [ t_rel, - pose.position.x, - pose.position.y, - pose.orientation.euler[2], + pose.pose.position.x, + pose.pose.position.y, + quaternion_euler(pose.pose.orientation)[2], self._cmd_vx, self._cmd_vy, self._cmd_wz, @@ -326,7 +334,9 @@ def __init__( progress_frac: float = 0.7, window: int = 20, ) -> None: - self._xy = np.array([[p.position.x, p.position.y] for p in reference.poses], dtype=float) + self._xy = np.array( + [[p.pose.position.x, p.pose.position.y] for p in reference.poses], dtype=float + ) self._goal = self._xy[-1] self._n = len(self._xy) self._goal_tol = goal_tolerance @@ -544,7 +554,9 @@ def _await_completion(self, monitor: CompletionMonitor, timeout_s: float) -> tup pose = self._recorder.latest_pose() if pose is not None: lin, ang = self._recorder.body_speed() - if monitor.update(pose.position.x, pose.position.y, lin, ang, time.perf_counter()): + if monitor.update( + pose.pose.position.x, pose.pose.position.y, lin, ang, time.perf_counter() + ): return True, "goal+stop" time.sleep(0.05) return False, "timeout" diff --git a/dimos/control/benchmarking/paths.py b/dimos/control/benchmarking/paths.py index 94c688d5d27..0138ec9248e 100644 --- a/dimos/control/benchmarking/paths.py +++ b/dimos/control/benchmarking/paths.py @@ -22,13 +22,17 @@ import math +from dimos_generated.geometry_msgs.msg import ( + Point as GeoPoint, + Pose, + PoseStamped, +) +from dimos_generated.nav_msgs.msg import Path +from dimos_generated.std_msgs.msg import Header + from dimos.memory.vis.space.elements import Point, Polyline, Text from dimos.memory.vis.space.space import Space -from dimos.msgs.geometry_msgs.Point import Point as GeoPoint -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.msgs.nav_msgs.Path import Path +from dimos.msgs.geometry import quaternion_from_euler # Plot styling constants for the trajectory renderers below. _REF_COLOR = "#cccccc" # reference path = light gray @@ -44,8 +48,10 @@ def _xy_to_path(executed_xy: list[tuple[float, float]]) -> Path: """Wrap (x, y) tuples in a nav_msgs.Path so memory Polyline can render them.""" poses = [ PoseStamped( - position=Vector3(x, y, 0.0), - orientation=Quaternion.from_euler(Vector3(0.0, 0.0, 0.0)), + header=Header(frame_id=""), + pose=Pose( + position=GeoPoint(x=x, y=y, z=0.0), orientation=quaternion_from_euler(0.0, 0.0, 0.0) + ), ) for x, y in executed_xy ] @@ -54,8 +60,10 @@ def _xy_to_path(executed_xy: list[tuple[float, float]]) -> Path: def _pose(x: float, y: float, yaw: float) -> PoseStamped: return PoseStamped( - position=Vector3(x, y, 0.0), - orientation=Quaternion.from_euler(Vector3(0.0, 0.0, yaw)), + header=Header(frame_id=""), + pose=Pose( + position=GeoPoint(x=x, y=y, z=0.0), orientation=quaternion_from_euler(0.0, 0.0, yaw) + ), ) @@ -238,9 +246,8 @@ def hold_heading(path: Path, yaw: float = 0.0) -> Path: of facing the travel direction. """ return Path( - ts=path.ts, - frame_id=path.frame_id, - poses=[_pose(p.position.x, p.position.y, yaw) for p in path.poses], + header=path.header, + poses=[_pose(p.pose.position.x, p.pose.position.y, yaw) for p in path.poses], ) @@ -438,8 +445,8 @@ def path_to_svg(path: Path, size_px: int = 400, margin_px: int = 20) -> str: sp = Space() sp.add(Polyline(msg=path, color="#000000", width=_REF_WIDTH)) - sp.add(Point(msg=path.poses[0], color=_START_COLOR, radius=_MARKER_RADIUS)) - sp.add(Point(msg=path.poses[-1], color=_END_COLOR, radius=_MARKER_RADIUS)) + sp.add(Point(msg=path.poses[0].pose.position, color=_START_COLOR, radius=_MARKER_RADIUS)) + sp.add(Point(msg=path.poses[-1].pose.position, color=_END_COLOR, radius=_MARKER_RADIUS)) return sp.to_svg() @@ -458,8 +465,8 @@ def trajectory_to_svg( sp.add(Polyline(msg=_xy_to_path(executed_xy), color=_EXE_COLOR, width=_EXE_WIDTH)) sx, sy = executed_xy[0] ex, ey = executed_xy[-1] - sp.add(Point(msg=GeoPoint(sx, sy, 0.0), color=_START_COLOR, radius=_MARKER_RADIUS)) - sp.add(Point(msg=GeoPoint(ex, ey, 0.0), color=_END_COLOR, radius=_MARKER_RADIUS)) + sp.add(Point(msg=GeoPoint(x=sx, y=sy), color=_START_COLOR, radius=_MARKER_RADIUS)) + sp.add(Point(msg=GeoPoint(x=ex, y=ey), color=_END_COLOR, radius=_MARKER_RADIUS)) return sp.to_svg() @@ -487,8 +494,8 @@ def multi_trajectory_to_svg( sp.add(Polyline(msg=reference, color=_REF_COLOR, width=_REF_WIDTH * 1.4)) # Establish bounds for legend placement (below the path) and title (above). - all_ys = [p.position.y for p in reference.poses] - all_xs = [p.position.x for p in reference.poses] + all_ys = [p.pose.position.y for p in reference.poses] + all_xs = [p.pose.position.x for p in reference.poses] for xy in cohorts.values(): all_ys.extend(y for _, y in xy) all_xs.extend(x for x, _ in xy) @@ -505,7 +512,7 @@ def multi_trajectory_to_svg( if xy: sp.add(Polyline(msg=_xy_to_path(xy), color=color, width=_EXE_WIDTH)) sx, sy = xy[0] - sp.add(Point(msg=GeoPoint(sx, sy, 0.0), color=color, radius=_MARKER_RADIUS * 0.7)) + sp.add(Point(msg=GeoPoint(x=sx, y=sy), color=color, radius=_MARKER_RADIUS * 0.7)) # Legend row (world coords below the plot). ly = y_min - 0.4 - i * 0.25 sp.add( @@ -513,12 +520,18 @@ def multi_trajectory_to_svg( msg=Path( poses=[ PoseStamped( - position=Vector3(x_min, ly, 0.0), - orientation=Quaternion.from_euler(Vector3(0, 0, 0)), + header=Header(frame_id=""), + pose=Pose( + position=GeoPoint(x=x_min, y=ly, z=0.0), + orientation=quaternion_from_euler(0, 0, 0), + ), ), PoseStamped( - position=Vector3(x_min + 0.4, ly, 0.0), - orientation=Quaternion.from_euler(Vector3(0, 0, 0)), + header=Header(frame_id=""), + pose=Pose( + position=GeoPoint(x=x_min + 0.4, y=ly, z=0.0), + orientation=quaternion_from_euler(0, 0, 0), + ), ), ] ), diff --git a/dimos/control/benchmarking/score.py b/dimos/control/benchmarking/score.py index 810fd9c587d..d17ee0036be 100644 --- a/dimos/control/benchmarking/score.py +++ b/dimos/control/benchmarking/score.py @@ -31,13 +31,14 @@ from pathlib import Path from typing import Any +from dimos_generated.geometry_msgs.msg import Point, Pose, PoseStamped, Twist, Vector3 +from dimos_generated.std_msgs.msg import Header + from dimos.control.benchmarking.benchmark import RunRecording from dimos.control.benchmarking.scoring import ExecutedTrajectory, TrajectoryTick, score_run from dimos.control.benchmarking.tuning import OperatingPoint, OperatingPointMap, invert_tolerance -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Twist import Twist -from dimos.msgs.geometry_msgs.Vector3 import Vector3 +from dimos.msgs.geometry import quaternion_from_euler +from dimos.msgs.time import time_from_seconds from dimos.utils.logging_config import setup_logger logger = setup_logger() @@ -50,11 +51,15 @@ def _executed_from_recording(rec: RunRecording) -> ExecutedTrajectory: TrajectoryTick( t=t, pose=PoseStamped( - ts=t, - position=Vector3(x, y, 0.0), - orientation=Quaternion.from_euler(Vector3(0.0, 0.0, yaw)), + header=Header(frame_id="", stamp=time_from_seconds(t)), + pose=Pose( + position=Point(x=x, y=y, z=0.0), + orientation=quaternion_from_euler(0.0, 0.0, yaw), + ), + ), + cmd_twist=Twist( + linear=Vector3(x=cvx, y=cvy, z=0.0), angular=Vector3(x=0.0, y=0.0, z=cwz) ), - cmd_twist=Twist(linear=Vector3(cvx, cvy, 0.0), angular=Vector3(0.0, 0.0, cwz)), actual_twist=Twist(), ) ) diff --git a/dimos/control/benchmarking/scoring.py b/dimos/control/benchmarking/scoring.py index a6c1b8a1ab8..5b434bff2a6 100644 --- a/dimos/control/benchmarking/scoring.py +++ b/dimos/control/benchmarking/scoring.py @@ -31,12 +31,12 @@ from dataclasses import dataclass, field import math +from dimos_generated.geometry_msgs.msg import PoseStamped, Twist +from dimos_generated.nav_msgs.msg import Path import numpy as np from numpy.typing import NDArray -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.geometry_msgs.Twist import Twist -from dimos.msgs.nav_msgs.Path import Path +from dimos.msgs.geometry import quaternion_euler from dimos.utils.trigonometry import angle_diff @@ -75,7 +75,7 @@ class ScoreResult: def _path_xy(path: Path) -> NDArray[np.float64]: - return np.array([[p.position.x, p.position.y] for p in path.poses], dtype=np.float64) + return np.array([[p.pose.position.x, p.pose.position.y] for p in path.poses], dtype=np.float64) def nearest_segment( @@ -146,7 +146,10 @@ def score_run(reference_path: Path, executed: ExecutedTrajectory) -> ScoreResult if len(path_xy) == 0: return ScoreResult(arrived=executed.arrived, n_ticks=len(executed.ticks)) path_yaws = np.unwrap( - np.array([p.orientation.euler[2] for p in reference_path.poses], dtype=np.float64) + np.array( + [quaternion_euler(p.pose.orientation)[2] for p in reference_path.poses], + dtype=np.float64, + ) ).astype(np.float64) cte_sq: list[float] = [] @@ -157,13 +160,13 @@ def score_run(reference_path: Path, executed: ExecutedTrajectory) -> ScoreResult ang_sq: list[float] = [] for tick in executed.ticks: - pt = np.array([tick.pose.position.x, tick.pose.position.y], dtype=np.float64) + pt = np.array([tick.pose.pose.position.x, tick.pose.pose.position.y], dtype=np.float64) seg_idx, d, t_along = nearest_segment(pt, path_xy) cte_abs.append(d) cte_sq.append(d * d) path_yaw = _reference_yaw(path_yaws, seg_idx, t_along) - he = abs(angle_diff(tick.pose.orientation.euler[2], path_yaw)) + he = abs(angle_diff(quaternion_euler(tick.pose.pose.orientation)[2], path_yaw)) he_abs.append(he) he_sq.append(he * he) diff --git a/dimos/control/benchmarking/test_benchmark.py b/dimos/control/benchmarking/test_benchmark.py index 12b9c1e2729..958d3537d90 100644 --- a/dimos/control/benchmarking/test_benchmark.py +++ b/dimos/control/benchmarking/test_benchmark.py @@ -21,6 +21,8 @@ from types import SimpleNamespace +from dimos_generated.geometry_msgs.msg import Point, Pose, PoseStamped, Twist, Vector3 +from dimos_generated.std_msgs.msg import Header import pytest from dimos.control.benchmarking.benchmark import ( @@ -44,17 +46,17 @@ DEFAULT_ARTIFACT_PATH, create_task, ) -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Twist import Twist -from dimos.msgs.geometry_msgs.Vector3 import Vector3 +from dimos.msgs.geometry import quaternion_euler, quaternion_from_euler _JOINTS = ["go2/vx", "go2/vy", "go2/wz"] def _pose(x=0.0, y=0.0, yaw=0.0) -> PoseStamped: return PoseStamped( - position=Vector3(x, y, 0.0), orientation=Quaternion.from_euler(Vector3(0.0, 0.0, yaw)) + header=Header(frame_id=""), + pose=Pose( + position=Point(x=x, y=y, z=0.0), orientation=quaternion_from_euler(0.0, 0.0, yaw) + ), ) @@ -174,12 +176,12 @@ def test_battery_registry_selects_fullpose_paths(): } # The point of the battery: commanded yaw decoupled from the tangent. strafe = fullpose["strafe_left_2m"] - assert all(abs(p.orientation.euler[2]) < 1e-9 for p in strafe.poses) - assert strafe.poses[-1].position.y > 1.0 # travel is +y while yaw is 0 + assert all(abs(quaternion_euler(p.pose.orientation)[2]) < 1e-9 for p in strafe.poses) + assert strafe.poses[-1].pose.position.y > 1.0 # travel is +y while yaw is 0 # square_crab: square geometry, one held heading through all corners. crab = fullpose["square_crab"] - assert all(abs(p.orientation.euler[2]) < 1e-9 for p in crab.poses) - assert max(p.position.y for p in crab.poses) > 1.0 + assert all(abs(quaternion_euler(p.pose.orientation)[2]) < 1e-9 for p in crab.poses) + assert max(p.pose.position.y for p in crab.poses) > 1.0 # "all" = tangent-heading battery + full-pose battery. assert set(BATTERIES["all"]()) == set(path_set()) | set(fullpose) @@ -187,7 +189,7 @@ def test_battery_registry_selects_fullpose_paths(): def test_anchor_shifts_path_to_pose(): ref = straight_line(length=2.0) anchored = shift_path_to_start_at_pose(ref, _pose(5.0, 3.0, 0.0)) - p0 = anchored.poses[0].position + p0 = anchored.poses[0].pose.position assert p0.x == 5.0 and p0.y == 3.0 @@ -256,7 +258,9 @@ def test_completion_does_not_fire_prematurely_on_closed_path(): # path yet -> not complete. done = False for i in range(10): - done = mon.update(ref.poses[0].position.x, ref.poses[0].position.y, 0.0, 0.0, i * 0.1) + done = mon.update( + ref.poses[0].pose.position.x, ref.poses[0].pose.position.y, 0.0, 0.0, i * 0.1 + ) assert not done @@ -316,7 +320,9 @@ def test_end_to_end_controller_benchmark_scoring(tmp_path): out = task.compute(_state(plant.x, plant.y, plant.yaw, t=k * 0.1)) cvx, cvy, cwz = out.velocities if out is not None else (0.0, 0.0, 0.0) # Benchmark records what it sees over the transport: cmd_vel + odom. - recorder.on_cmd_vel(Twist(linear=Vector3(cvx, cvy, 0.0), angular=Vector3(0.0, 0.0, cwz))) + recorder.on_cmd_vel( + Twist(linear=Vector3(x=cvx, y=cvy, z=0.0), angular=Vector3(x=0.0, y=0.0, z=cwz)) + ) recorder.on_odom(_pose(plant.x, plant.y, plant.yaw), now=k * 0.1) lin, ang = recorder.body_speed() if monitor.update(plant.x, plant.y, lin, ang, k * 0.1): @@ -386,7 +392,9 @@ def test_end_to_end_fullpose_benchmark_scoring(tmp_path): for k in range(800): out = task.compute(_state(plant.x, plant.y, plant.yaw, t=k * 0.1)) cvx, cvy, cwz = out.velocities if out is not None else (0.0, 0.0, 0.0) - recorder.on_cmd_vel(Twist(linear=Vector3(cvx, cvy, 0.0), angular=Vector3(0.0, 0.0, cwz))) + recorder.on_cmd_vel( + Twist(linear=Vector3(x=cvx, y=cvy, z=0.0), angular=Vector3(x=0.0, y=0.0, z=cwz)) + ) recorder.on_odom(_pose(plant.x, plant.y, plant.yaw), now=k * 0.1) lin, ang = recorder.body_speed() if monitor.update(plant.x, plant.y, lin, ang, k * 0.1): diff --git a/dimos/control/benchmarking/velocity_profile.py b/dimos/control/benchmarking/velocity_profile.py index e1eec131b7b..cfa45f510b2 100644 --- a/dimos/control/benchmarking/velocity_profile.py +++ b/dimos/control/benchmarking/velocity_profile.py @@ -40,10 +40,10 @@ from dataclasses import dataclass from typing import Protocol +from dimos_generated.nav_msgs.msg import Path import numpy as np from dimos.control.tasks.velocity_profiler import VelocityProfiler -from dimos.msgs.nav_msgs.Path import Path # Go2 Rung-1 saturation envelope (mirrors runner.VX_MAX / WZ_MAX). GO2_VX_MAX = 1.0 # m/s @@ -108,7 +108,9 @@ def __init__(self, cfg: VelocityProfileConfig | None = None) -> None: def for_path(self, path: Path) -> None: """(Re)compute the speed profile for ``path``. Call on path start.""" self._profile = np.asarray(self._profiler.compute_profile(path), dtype=float) - self._pts = np.array([[p.position.x, p.position.y] for p in path.poses], dtype=float) + self._pts = np.array( + [[p.pose.position.x, p.pose.position.y] for p in path.poses], dtype=float + ) def speed_limit_at(self, x: float, y: float) -> float: """Profile speed at the nearest path index, min over the lookahead diff --git a/dimos/control/coordinator.py b/dimos/control/coordinator.py index c784576c24d..3f4271845f9 100644 --- a/dimos/control/coordinator.py +++ b/dimos/control/coordinator.py @@ -32,6 +32,11 @@ import time from typing import TYPE_CHECKING, Any +from dimos_generated.geometry_msgs.msg import Twist +from dimos_generated.sensor_msgs.msg import JointState +from dimos_generated.std_msgs.msg import Float32 +from dimos_generated.trajectory_msgs.msg import JointTrajectory + from dimos.control.components import ( TWIST_SUFFIX_MAP, HardwareComponent, @@ -64,10 +69,6 @@ ) from dimos.hardware.manipulators.spec import ManipulatorAdapter from dimos.hardware.whole_body.spec import WholeBodyAdapter -from dimos.msgs.geometry_msgs.Twist import Twist -from dimos.msgs.sensor_msgs.JointState import JointState -from dimos.msgs.std_msgs.Float32 import Float32 -from dimos.msgs.trajectory_msgs.JointTrajectory import JointTrajectory from dimos.utils.logging_config import setup_logger if TYPE_CHECKING: diff --git a/dimos/control/examples/cartesian_ik_jogger.py b/dimos/control/examples/cartesian_ik_jogger.py index 7e438f02391..88177241d79 100644 --- a/dimos/control/examples/cartesian_ik_jogger.py +++ b/dimos/control/examples/cartesian_ik_jogger.py @@ -37,8 +37,13 @@ import time from typing import Any +from dimos_generated.geometry_msgs.msg import Point, Pose +from dimos_generated.std_msgs.msg import Header import numpy as np +from dimos.msgs.geometry import quaternion_from_euler +from dimos.msgs.time import time_from_seconds + try: import pygame except ImportError: @@ -125,17 +130,16 @@ def from_fk( def to_pose_stamped(self) -> Any: """Convert to PoseStamped for LCM publishing.""" - from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped - from dimos.msgs.geometry_msgs.Quaternion import Quaternion - from dimos.msgs.geometry_msgs.Vector3 import Vector3 + from dimos_generated.geometry_msgs.msg import PoseStamped, Vector3 - position = Vector3(self.x, self.y, self.z) - orientation = Quaternion.from_euler(Vector3(self.roll, self.pitch, self.yaw)) + position = Vector3(x=self.x, y=self.y, z=self.z) + orientation = quaternion_from_euler(self.roll, self.pitch, self.yaw) return PoseStamped( - ts=time.time(), - position=position, - orientation=orientation, + header=Header(frame_id="", stamp=time_from_seconds(time.time())), + pose=Pose( + position=Point(x=position.x, y=position.y, z=position.z), orientation=orientation + ), ) @@ -171,8 +175,9 @@ def run_jogger_ui(model_path: str | None = None, ee_joint_id: int = 6) -> None: If None, uses Piper model. ee_joint_id: End-effector joint ID in the model """ + from dimos_generated.geometry_msgs.msg import PoseStamped + from dimos.core.transport import LCMTransport - from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped # Use Piper model if not specified if model_path is None: diff --git a/dimos/control/path_following_coordinator.py b/dimos/control/path_following_coordinator.py index 09772ca3b02..4b410a248c6 100644 --- a/dimos/control/path_following_coordinator.py +++ b/dimos/control/path_following_coordinator.py @@ -16,10 +16,11 @@ from __future__ import annotations +from dimos_generated.nav_msgs.msg import Path +from dimos_generated.std_msgs.msg import Float32 + from dimos.control.coordinator import ControlCoordinator from dimos.core.stream import In -from dimos.msgs.nav_msgs.Path import Path -from dimos.msgs.std_msgs.Float32 import Float32 class PathFollowingCoordinator(ControlCoordinator): diff --git a/dimos/control/task.py b/dimos/control/task.py index b0472c37fbb..73c30c455af 100644 --- a/dimos/control/task.py +++ b/dimos/control/task.py @@ -35,9 +35,8 @@ from dimos.hardware.whole_body.spec import IMUState if TYPE_CHECKING: - from dimos.msgs.geometry_msgs.Pose import Pose - from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped - from dimos.msgs.geometry_msgs.TwistStamped import TwistStamped + from dimos_generated.geometry_msgs.msg import Pose, PoseStamped, TwistStamped + from dimos.teleop.webxr.controller_types import Buttons @@ -187,6 +186,7 @@ class ControlTask(Protocol): Use state.t_now passed to compute() for all timing. Example: + >>> from dimos.msgs.trajectory import sample_trajectory >>> class MyTask: ... @property ... def name(self) -> str: @@ -204,7 +204,7 @@ class ControlTask(Protocol): ... def compute(self, state: CoordinatorState) -> JointCommandOutput | None: ... # Use state.t_now, NOT time.time()! ... t_elapsed = state.t_now - self._start_time - ... positions = self._trajectory.sample(t_elapsed) + ... positions, _ = sample_trajectory(self._trajectory, t_elapsed) ... return JointCommandOutput( ... joint_names=["left/joint1", "left/joint2"], ... positions=positions, diff --git a/dimos/control/tasks/cartesian_ik_task/cartesian_ik_task.py b/dimos/control/tasks/cartesian_ik_task/cartesian_ik_task.py index 27bc3e717ff..33784e372dc 100644 --- a/dimos/control/tasks/cartesian_ik_task/cartesian_ik_task.py +++ b/dimos/control/tasks/cartesian_ik_task/cartesian_ik_task.py @@ -17,10 +17,12 @@ from __future__ import annotations from collections.abc import Mapping +import copy import threading from typing import TYPE_CHECKING import attrs +from dimos_generated.geometry_msgs.msg import Pose, PoseStamped from dimos.control.task import CoordinatorState from dimos.control.tasks.pose_target_ik import ( @@ -31,8 +33,6 @@ PoseTargetIKTaskParams, string_tuple_converter, ) -from dimos.msgs.geometry_msgs.Pose import Pose -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped if TYPE_CHECKING: from dimos.control.coordinator import TaskConfig @@ -72,12 +72,7 @@ def is_active(self) -> bool: def on_cartesian_command(self, pose: Pose | PoseStamped, t_now: float) -> bool: """Accept an absolute target pose and activate tracking.""" - target = PoseStamped( - ts=pose.ts if isinstance(pose, PoseStamped) else 0.0, - frame_id=pose.frame_id if isinstance(pose, PoseStamped) else "", - position=pose.position, - orientation=pose.orientation, - ) + target = copy.deepcopy(pose) if isinstance(pose, PoseStamped) else PoseStamped(pose=pose) with self._lock: self._target_pose = target self._last_update_time = t_now diff --git a/dimos/control/tasks/cartesian_ik_task/test_cartesian_ik_task.py b/dimos/control/tasks/cartesian_ik_task/test_cartesian_ik_task.py index 86b9f6092fe..fdf21be6b34 100644 --- a/dimos/control/tasks/cartesian_ik_task/test_cartesian_ik_task.py +++ b/dimos/control/tasks/cartesian_ik_task/test_cartesian_ik_task.py @@ -16,6 +16,9 @@ from pathlib import Path +from dimos_generated.geometry_msgs.msg import Point, Pose, PoseStamped +from dimos_generated.sensor_msgs.msg import JointState +from dimos_generated.std_msgs.msg import Header import pytest from pytest_mock import MockerFixture @@ -26,9 +29,6 @@ ) from dimos.control.tasks.pose_target_ik import PinkPoseTargetSolver from dimos.manipulation.planning.spec.config import RobotModelConfig -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.msgs.sensor_msgs.JointState import JointState from dimos.robot.assets.model import RobotModel @@ -57,7 +57,9 @@ def test_cartesian_leaf_maps_absolute_pose_to_configured_frame( solver = mocker.Mock(spec=PinkPoseTargetSolver) solver.step.return_value = JointState(name=["arm/joint"], position=[0.01]) task = CartesianIKTask("cartesian", _config(), solver=solver) - target = PoseStamped(position=Vector3(0.4, 0.2, 0.1), frame_id="world") + target = PoseStamped( + header=Header(frame_id="world"), pose=Pose(position=Point(x=0.4, y=0.2, z=0.1)) + ) task.on_cartesian_command(target, t_now=2.0) output = task.compute( @@ -75,7 +77,7 @@ def test_cartesian_leaf_maps_absolute_pose_to_configured_frame( def test_cartesian_leaf_clears_after_timeout(mocker: MockerFixture) -> None: solver = mocker.Mock(spec=PinkPoseTargetSolver) task = CartesianIKTask("cartesian", _config(), solver=solver) - task.on_cartesian_command(PoseStamped(), t_now=1.0) + task.on_cartesian_command(PoseStamped(header=Header(frame_id=""), pose=Pose()), t_now=1.0) output = task.compute( CoordinatorState( @@ -99,11 +101,11 @@ def test_cartesian_clear_reseeds_command_from_feedback(mocker: MockerFixture) -> t_now=1.0, dt=0.01, ) - task.on_cartesian_command(PoseStamped(), t_now=1.0) + task.on_cartesian_command(PoseStamped(header=Header(frame_id=""), pose=Pose()), t_now=1.0) assert task.compute(state) is not None task.clear() - task.on_cartesian_command(PoseStamped(), t_now=1.1) + task.on_cartesian_command(PoseStamped(header=Header(frame_id=""), pose=Pose()), t_now=1.1) solver.step.return_value = JointState(name=["arm/joint"], position=[0.01]) assert task.compute(state) is not None diff --git a/dimos/control/tasks/eef_twist_task/eef_twist_task.py b/dimos/control/tasks/eef_twist_task/eef_twist_task.py index b62ee7f0d5a..ac9d6ff889d 100644 --- a/dimos/control/tasks/eef_twist_task/eef_twist_task.py +++ b/dimos/control/tasks/eef_twist_task/eef_twist_task.py @@ -21,6 +21,7 @@ from typing import TYPE_CHECKING import attrs +from dimos_generated.geometry_msgs.msg import PoseStamped import numpy as np import pinocchio @@ -33,14 +34,15 @@ PoseTargetIKTaskParams, string_tuple_converter, ) -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped +from dimos.msgs.geometry import pose_from_matrix, pose_matrix from dimos.utils.logging_config import setup_logger -from dimos.utils.transform_utils import matrix_to_pose, pose_to_matrix, twist_to_numpy +from dimos.utils.transform_utils import twist_to_numpy if TYPE_CHECKING: + from dimos_generated.geometry_msgs.msg import TwistStamped + from dimos.control.coordinator import TaskConfig from dimos.control.hardware_interface import ConnectedHardware, ConnectedWholeBody - from dimos.msgs.geometry_msgs.TwistStamped import TwistStamped logger = setup_logger() @@ -89,7 +91,7 @@ def on_cartesian_command(self, pose: object, t_now: float) -> bool: return False def on_ee_twist_command(self, twist: TwistStamped, t_now: float) -> bool: - values = twist_to_numpy(twist) + values = twist_to_numpy(twist.twist) if values.shape != (6,) or not np.all(np.isfinite(values)): logger.warning("EEFTwistTask rejecting invalid twist", task=self._name) return False @@ -193,16 +195,11 @@ def create_task( def _integrate_twist(pose: PoseStamped, twist: TwistStamped, dt: float) -> PoseStamped: - matrix = np.asarray(pose_to_matrix(pose), dtype=np.float64) - values = twist_to_numpy(twist) + matrix = np.asarray(pose_matrix(pose.pose), dtype=np.float64) + values = twist_to_numpy(twist.twist) matrix[:3, 3] += values[:3] * dt angular_step = values[3:] * dt if np.linalg.norm(angular_step) > 0.0: matrix[:3, :3] = pinocchio.exp3(angular_step) @ matrix[:3, :3] - integrated = matrix_to_pose(matrix) - return PoseStamped( - ts=pose.ts, - frame_id=pose.frame_id, - position=integrated.position, - orientation=integrated.orientation, - ) + integrated = pose_from_matrix(matrix) + return PoseStamped(header=pose.header, pose=integrated) diff --git a/dimos/control/tasks/eef_twist_task/test_eef_twist_task.py b/dimos/control/tasks/eef_twist_task/test_eef_twist_task.py index 864267901b9..7e69037f904 100644 --- a/dimos/control/tasks/eef_twist_task/test_eef_twist_task.py +++ b/dimos/control/tasks/eef_twist_task/test_eef_twist_task.py @@ -14,6 +14,9 @@ from pathlib import Path +from dimos_generated.geometry_msgs.msg import Point, Pose, PoseStamped, Twist, TwistStamped, Vector3 +from dimos_generated.sensor_msgs.msg import JointState +from dimos_generated.std_msgs.msg import Header import numpy as np import pytest from pytest_mock import MockerFixture @@ -25,9 +28,6 @@ ) from dimos.control.tasks.pose_target_ik import PinkPoseTargetSolver from dimos.manipulation.planning.spec.config import RobotModelConfig -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.geometry_msgs.TwistStamped import TwistStamped -from dimos.msgs.sensor_msgs.JointState import JointState from dimos.robot.assets.model import RobotModel @@ -41,7 +41,9 @@ def _robot_model() -> RobotModelConfig: def _solver(mocker: MockerFixture) -> PinkPoseTargetSolver: solver = mocker.Mock(spec=PinkPoseTargetSolver) solver.frame_poses.return_value = { - "tool": PoseStamped(frame_id="world", position=[0.0, 0.0, 0.0]) + "tool": PoseStamped( + header=Header(frame_id="world"), pose=Pose(position=Point(x=0.0, y=0.0, z=0.0)) + ) } solver.step.return_value = JointState( name=["arm/joint1", "arm/joint2"], @@ -92,7 +94,7 @@ def _state(t_now: float = 1.0, *, dt: float = 0.01) -> CoordinatorState: def _twist(x: float = 0.1) -> TwistStamped: - return TwistStamped(frame_id="tool", linear=[x, 0.0, 0.0], angular=[0.0, 0.0, 0.0]) + return TwistStamped(header=Header(frame_id="tool"), twist=Twist(linear=Vector3(x=x))) def test_twist_integrates_target_and_uses_shared_persistent_command( @@ -108,8 +110,8 @@ def test_twist_integrates_target_and_uses_shared_persistent_command( assert second is not None first_target = solver.step.call_args_list[0].args[0]["tool"] second_target = solver.step.call_args_list[1].args[0]["tool"] - assert first_target.position.x == pytest.approx(0.001) - assert second_target.position.x == pytest.approx(0.002) + assert first_target.pose.position.x == pytest.approx(0.001) + assert second_target.pose.position.x == pytest.approx(0.002) def test_zero_twist_holds_the_integrated_target(mocker: MockerFixture) -> None: @@ -121,7 +123,7 @@ def test_zero_twist_holds_the_integrated_target(mocker: MockerFixture) -> None: task.compute(_state(1.03)) target = solver.step.call_args.args[0]["tool"] - assert target.position.x == pytest.approx(0.001) + assert target.pose.position.x == pytest.approx(0.001) def test_stale_twist_stops_motion_without_dropping_hold(mocker: MockerFixture) -> None: @@ -133,7 +135,7 @@ def test_stale_twist_stops_motion_without_dropping_hold(mocker: MockerFixture) - assert output is not None target = solver.step.call_args.args[0]["tool"] - assert target.position.x == pytest.approx(0.001) + assert target.pose.position.x == pytest.approx(0.001) def test_estop_rejects_input_and_reanchors_after_clear(mocker: MockerFixture) -> None: @@ -165,6 +167,6 @@ def test_preemption_discards_twist_target_and_command_state(mocker: MockerFixtur def test_invalid_twist_is_rejected(mocker: MockerFixture) -> None: task, _ = _task(mocker) invalid = _twist() - invalid.linear.x = np.nan + invalid.twist.linear.x = np.nan assert not task.on_ee_twist_command(invalid, t_now=1.0) diff --git a/dimos/control/tasks/g1_groot_wbc_task/g1_groot_wbc_task.py b/dimos/control/tasks/g1_groot_wbc_task/g1_groot_wbc_task.py index deed610f770..9f925a729cd 100644 --- a/dimos/control/tasks/g1_groot_wbc_task/g1_groot_wbc_task.py +++ b/dimos/control/tasks/g1_groot_wbc_task/g1_groot_wbc_task.py @@ -48,8 +48,9 @@ from dimos.utils.logging_config import setup_logger if TYPE_CHECKING: + from dimos_generated.geometry_msgs.msg import Twist + from dimos.hardware.whole_body.spec import WholeBodyAdapter - from dimos.msgs.geometry_msgs.Twist import Twist logger = setup_logger() diff --git a/dimos/control/tasks/g1_groot_wbc_task/test_g1_groot_wbc_task.py b/dimos/control/tasks/g1_groot_wbc_task/test_g1_groot_wbc_task.py index 95304b2a10c..6a13203b4f9 100644 --- a/dimos/control/tasks/g1_groot_wbc_task/test_g1_groot_wbc_task.py +++ b/dimos/control/tasks/g1_groot_wbc_task/test_g1_groot_wbc_task.py @@ -26,6 +26,7 @@ from typing import Any from unittest.mock import MagicMock +from dimos_generated.geometry_msgs.msg import Twist, Vector3 import numpy as np import pytest @@ -37,7 +38,6 @@ G1GrootWBCTaskConfig, ) from dimos.hardware.whole_body.spec import IMUState -from dimos.msgs.geometry_msgs.Twist import Twist class _StubSession: @@ -186,7 +186,10 @@ def test_nonzero_cmd_uses_walk_until_timeout( def test_on_twist_command_sets_velocity_command(task: G1GrootWBCTask) -> None: - task.on_twist_command(Twist(linear=[0.5, 0.25, 0.0], angular=[0.0, 0.0, 0.3]), t_now=100.0) + task.on_twist_command( + Twist(linear=Vector3(x=0.5, y=0.25, z=0.0), angular=Vector3(x=0.0, y=0.0, z=0.3)), + t_now=100.0, + ) assert list(task._cmd) == pytest.approx([0.5, 0.25, 0.3]) assert task._last_cmd_time == 100.0 diff --git a/dimos/control/tasks/gripper_task/gripper_task.py b/dimos/control/tasks/gripper_task/gripper_task.py index 2945f69d877..d41ac8acaac 100644 --- a/dimos/control/tasks/gripper_task/gripper_task.py +++ b/dimos/control/tasks/gripper_task/gripper_task.py @@ -32,7 +32,7 @@ from dimos.utils.logging_config import setup_logger if TYPE_CHECKING: - from dimos.msgs.std_msgs.Float32 import Float32 + from dimos_generated.std_msgs.msg import Float32 logger = setup_logger() diff --git a/dimos/control/tasks/gripper_task/test_gripper_task.py b/dimos/control/tasks/gripper_task/test_gripper_task.py index 9407332121f..75f56b851f9 100644 --- a/dimos/control/tasks/gripper_task/test_gripper_task.py +++ b/dimos/control/tasks/gripper_task/test_gripper_task.py @@ -12,6 +12,7 @@ # See the License for the specific language governing permissions and # limitations under the License. +from dimos_generated.std_msgs.msg import Float32 import pytest from pytest_mock import MockerFixture @@ -26,7 +27,6 @@ ) from dimos.hardware.manipulators.spec import ManipulatorAdapter from dimos.hardware.spec import JointLimits -from dimos.msgs.std_msgs.Float32 import Float32 def _task() -> GripperControlTask: diff --git a/dimos/control/tasks/holonomic_pose_follower_task/holonomic_pose_follower_task.py b/dimos/control/tasks/holonomic_pose_follower_task/holonomic_pose_follower_task.py index 773a5abc2bd..e953e0ff094 100644 --- a/dimos/control/tasks/holonomic_pose_follower_task/holonomic_pose_follower_task.py +++ b/dimos/control/tasks/holonomic_pose_follower_task/holonomic_pose_follower_task.py @@ -26,6 +26,10 @@ from pathlib import Path as _FsPath from typing import Any, Literal +from dimos_generated.geometry_msgs.msg import Point, Pose, PoseStamped +from dimos_generated.nav_msgs.msg import Path +from dimos_generated.std_msgs.msg import Float32, Header + from dimos.control.benchmarking.tuning import TuningConfig from dimos.control.task import ( BaseControlTask, @@ -42,11 +46,7 @@ from dimos.control.tasks.holonomic_pose_follower_task.progress_reference import ( ProgressPathReference, ) -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.msgs.nav_msgs.Path import Path -from dimos.msgs.std_msgs.Float32 import Float32 +from dimos.msgs.geometry import quaternion_from_euler from dimos.protocol.service.spec import BaseConfig from dimos.utils.logging_config import setup_logger from dimos.utils.trigonometry import angle_diff @@ -79,8 +79,11 @@ def _clamp(v: float, limit: float) -> float: def _pose_stamped(pose: tuple[float, float, float]) -> PoseStamped: return PoseStamped( - position=Vector3(pose[0], pose[1], 0.0), - orientation=Quaternion.from_euler(Vector3(0.0, 0.0, pose[2])), + header=Header(frame_id=""), + pose=Pose( + position=Point(x=pose[0], y=pose[1], z=0.0), + orientation=quaternion_from_euler(0.0, 0.0, pose[2]), + ), ) @@ -417,7 +420,7 @@ def start_path(self, path: Path, current_odom: PoseStamped) -> bool: self._reference = reference # Re-project onto the new path. _v_path deliberately carries over, so an # in-motion replan does not re-ramp from rest. - reference.advance(float(current_odom.position.x), float(current_odom.position.y)) + reference.advance(float(current_odom.pose.position.x), float(current_odom.pose.position.y)) self._last_t = None self._last_pose = None self._last_pose_t = None diff --git a/dimos/control/tasks/holonomic_pose_follower_task/progress_reference.py b/dimos/control/tasks/holonomic_pose_follower_task/progress_reference.py index c754a72daa9..0f468391ccd 100644 --- a/dimos/control/tasks/holonomic_pose_follower_task/progress_reference.py +++ b/dimos/control/tasks/holonomic_pose_follower_task/progress_reference.py @@ -32,9 +32,10 @@ from dataclasses import dataclass import math +from dimos_generated.nav_msgs.msg import Path import numpy as np -from dimos.msgs.nav_msgs.Path import Path +from dimos.msgs.geometry import quaternion_euler _EPS = 1e-9 @@ -68,8 +69,8 @@ def __init__(self, path: Path, *, window_m: float = 1.0, back_m: float = 0.5) -> ys: list[float] = [] yaws: list[float] = [] for p in path.poses: - x, y = float(p.position.x), float(p.position.y) - yaw = float(p.orientation.euler[2]) + x, y = float(p.pose.position.x), float(p.pose.position.y) + yaw = float(quaternion_euler(p.pose.orientation)[2]) if xs and math.hypot(x - xs[-1], y - ys[-1]) < _EPS: # Coincident waypoint: keep the position once but adopt the # later pose's yaw (a stop-and-rotate hint survives as a yaw diff --git a/dimos/control/tasks/holonomic_pose_follower_task/test_holonomic_pose_follower_task.py b/dimos/control/tasks/holonomic_pose_follower_task/test_holonomic_pose_follower_task.py index 79d801c2940..1f8d17f1bfa 100644 --- a/dimos/control/tasks/holonomic_pose_follower_task/test_holonomic_pose_follower_task.py +++ b/dimos/control/tasks/holonomic_pose_follower_task/test_holonomic_pose_follower_task.py @@ -21,6 +21,9 @@ import math from types import SimpleNamespace +from dimos_generated.geometry_msgs.msg import Point, Pose, PoseStamped, Twist, Vector3 +from dimos_generated.nav_msgs.msg import Path +from dimos_generated.std_msgs.msg import Float32, Header import pytest from dimos.control.benchmarking.paths import ( @@ -42,12 +45,7 @@ DEFAULT_ARTIFACT_PATH, create_task, ) -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Twist import Twist -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.msgs.nav_msgs.Path import Path -from dimos.msgs.std_msgs.Float32 import Float32 +from dimos.msgs.geometry import quaternion_euler, quaternion_from_euler from dimos.utils.trigonometry import angle_diff _JOINTS = ["go2/vx", "go2/vy", "go2/wz"] @@ -56,8 +54,10 @@ def _pose(x=0.0, y=0.0, yaw=0.0): return PoseStamped( - position=Vector3(x, y, 0.0), - orientation=Quaternion.from_euler(Vector3(0.0, 0.0, yaw)), + header=Header(frame_id=""), + pose=Pose( + position=Point(x=x, y=y, z=0.0), orientation=quaternion_from_euler(0.0, 0.0, yaw) + ), ) @@ -106,7 +106,9 @@ def _run_closed_loop(task, path, max_ticks=2400, on_tick=None): TrajectoryTick( t=k * _DT, pose=_pose(plant.x, plant.y, plant.yaw), - cmd_twist=Twist(linear=Vector3(vx, vy, 0.0), angular=Vector3(0.0, 0.0, wz)), + cmd_twist=Twist( + linear=Vector3(x=vx, y=vy, z=0.0), angular=Vector3(x=0.0, y=0.0, z=wz) + ), actual_twist=Twist(), ) ) @@ -206,7 +208,7 @@ def test_strafes_holding_commanded_yaw(): task = _task() plant, executed = _run_closed_loop(task, strafe_line(length=2.0)) assert executed.arrived - max_yaw = max(abs(t.pose.orientation.euler[2]) for t in executed.ticks) + max_yaw = max(abs(quaternion_euler(t.pose.pose.orientation)[2]) for t in executed.ticks) assert max_yaw < 0.1, "robot rotated instead of strafing" assert plant.y == pytest.approx(2.0, abs=0.25) # The motion was carried by the lateral channel. @@ -221,7 +223,7 @@ def test_crab_walks_square_holding_heading(): task = _task() plant, executed = _run_closed_loop(task, path) assert executed.arrived - max_yaw = max(abs(t.pose.orientation.euler[2]) for t in executed.ticks) + max_yaw = max(abs(quaternion_euler(t.pose.pose.orientation)[2]) for t in executed.ticks) assert max_yaw < 0.1, "robot turned instead of crab-walking the square" score = score_run(path, executed) assert score.cte_rms < 0.15 @@ -284,7 +286,7 @@ def test_stops_inside_the_goal_when_the_plant_runs_hot(): break hot.step(vx, vy, wz, _DT) assert task.get_state() == "arrived" - goal = path.poses[-1].position + goal = path.poses[-1].pose.position rest_err = math.hypot(hot.x - goal.x, hot.y - goal.y) assert rest_err < task._config.goal_tolerance, f"rested {rest_err:.3f} m from goal" # Came to rest, not still coasting through. diff --git a/dimos/control/tasks/holonomic_pose_follower_task/test_progress_reference.py b/dimos/control/tasks/holonomic_pose_follower_task/test_progress_reference.py index b9aba1d71f1..d23fca46d50 100644 --- a/dimos/control/tasks/holonomic_pose_follower_task/test_progress_reference.py +++ b/dimos/control/tasks/holonomic_pose_follower_task/test_progress_reference.py @@ -19,21 +19,23 @@ import math +from dimos_generated.geometry_msgs.msg import Point, Pose, PoseStamped +from dimos_generated.nav_msgs.msg import Path +from dimos_generated.std_msgs.msg import Header import pytest from dimos.control.tasks.holonomic_pose_follower_task.progress_reference import ( ProgressPathReference, ) -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.msgs.nav_msgs.Path import Path +from dimos.msgs.geometry import quaternion_from_euler def _pose(x, y, yaw): return PoseStamped( - position=Vector3(x, y, 0.0), - orientation=Quaternion.from_euler(Vector3(0.0, 0.0, yaw)), + header=Header(frame_id=""), + pose=Pose( + position=Point(x=x, y=y, z=0.0), orientation=quaternion_from_euler(0.0, 0.0, yaw) + ), ) diff --git a/dimos/control/tasks/path_follower_task/path_follower_task.py b/dimos/control/tasks/path_follower_task/path_follower_task.py index 79c61c1b9fd..5536adb5b07 100644 --- a/dimos/control/tasks/path_follower_task/path_follower_task.py +++ b/dimos/control/tasks/path_follower_task/path_follower_task.py @@ -29,6 +29,9 @@ from dataclasses import dataclass, field, replace from typing import TYPE_CHECKING, Any, Literal +from dimos_generated.geometry_msgs.msg import Point, Pose, PoseStamped +from dimos_generated.nav_msgs.msg import Path +from dimos_generated.std_msgs.msg import Float32, Header import numpy as np from dimos.control.benchmarking.velocity_profile import ( @@ -52,11 +55,8 @@ VelocityTrackingPID, ) from dimos.core.global_config import global_config as _gc -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.msgs.nav_msgs.Path import Path -from dimos.msgs.std_msgs.Float32 import Float32 +from dimos.msgs.geometry import quaternion_euler, quaternion_from_euler +from dimos.msgs.time import time_from_seconds from dimos.navigation.replanning_a_star.controllers import PController from dimos.navigation.replanning_a_star.path_distancer import PathDistancer from dimos.protocol.service.spec import BaseConfig @@ -228,9 +228,11 @@ def compute(self, state: CoordinatorState) -> JointCommandOutput | None: self.start_path( path, PoseStamped( - ts=state.t_now, - position=Vector3(float(px), float(py), 0.0), - orientation=Quaternion.from_euler(Vector3(0.0, 0.0, float(pyaw))), + header=Header(frame_id="", stamp=time_from_seconds(state.t_now)), + pose=Pose( + position=Point(x=float(px), y=float(py), z=0.0), + orientation=quaternion_from_euler(0.0, 0.0, float(pyaw)), + ), ), ) if not self._running(): @@ -246,9 +248,11 @@ def compute(self, state: CoordinatorState) -> JointCommandOutput | None: yaw = pos.get(self._joint_names_list[2]) if x is not None and y is not None and yaw is not None: self._current_odom = PoseStamped( - ts=state.t_now, - position=Vector3(float(x), float(y), 0.0), - orientation=Quaternion.from_euler(Vector3(0.0, 0.0, float(yaw))), + header=Header(frame_id="", stamp=time_from_seconds(state.t_now)), + pose=Pose( + position=Point(x=float(x), y=float(y), z=0.0), + orientation=quaternion_from_euler(0.0, 0.0, float(yaw)), + ), ) if self._current_odom is None: return None @@ -283,8 +287,8 @@ def compute(self, state: CoordinatorState) -> JointCommandOutput | None: # PathSpeedCap.speed_limit_at — min over the next ~8 waypoints so # braking starts BEFORE a corner rather than at it. if self._velocity_profile is not None and self._velocity_profile_pts is not None: - x = self._current_odom.position.x - y = self._current_odom.position.y + x = self._current_odom.pose.position.x + y = self._current_odom.pose.position.y i = int(np.argmin(np.sum((self._velocity_profile_pts - np.array([x, y])) ** 2, axis=1))) j = min(len(self._velocity_profile), i + 8) vlim = float(np.min(self._velocity_profile[i:j])) @@ -294,7 +298,7 @@ def compute(self, state: CoordinatorState) -> JointCommandOutput | None: vx, vy, wz = vx * k, vy * k, wz * k elif self._profile_cap is not None: vx, vy, wz = self._profile_cap.cap( - self._current_odom.position.x, self._current_odom.position.y, vx, vy, wz + self._current_odom.pose.position.x, self._current_odom.pose.position.y, vx, vy, wz ) # Inner-loop gain compensation (mutually exclusive - PI wins if both @@ -340,8 +344,8 @@ def on_preempted(self, by_task: str, joints: frozenset[str]) -> None: def _step_initial_rotation(self) -> tuple[float, float, float]: assert self._path is not None and self._current_odom is not None - first_yaw = self._path.poses[0].orientation.euler[2] - robot_yaw = self._current_odom.orientation.euler[2] + first_yaw = quaternion_euler(self._path.poses[0].pose.orientation)[2] + robot_yaw = quaternion_euler(self._current_odom.pose.orientation)[2] yaw_err = angle_diff(first_yaw, robot_yaw) if abs(yaw_err) < self._config.orientation_tolerance: @@ -364,7 +368,7 @@ def _windowed_closest(self, pos: np.ndarray, window: int = 20) -> int: best_idx = lo best_d_sq = float("inf") for i in range(lo, hi): - p = self._path.poses[i].position + p = self._path.poses[i].pose.position d_sq = (p.x - pos[0]) ** 2 + (p.y - pos[1]) ** 2 if d_sq < best_d_sq: best_d_sq = d_sq @@ -376,7 +380,7 @@ def _step_path_following(self) -> tuple[float, float, float]: assert self._distancer is not None assert self._current_odom is not None - pos = np.array([self._current_odom.position.x, self._current_odom.position.y]) + pos = np.array([self._current_odom.pose.position.x, self._current_odom.pose.position.y]) closest = self._windowed_closest(pos) if closest > self._max_progress_idx: @@ -410,8 +414,8 @@ def _step_path_following(self) -> tuple[float, float, float]: def _step_final_rotation(self) -> tuple[float, float, float]: assert self._path is not None and self._current_odom is not None - goal_yaw = self._path.poses[-1].orientation.euler[2] - robot_yaw = self._current_odom.orientation.euler[2] + goal_yaw = quaternion_euler(self._path.poses[-1].pose.orientation)[2] + robot_yaw = quaternion_euler(self._current_odom.pose.orientation)[2] yaw_err = angle_diff(goal_yaw, robot_yaw) if abs(yaw_err) < self._config.orientation_tolerance: @@ -529,8 +533,8 @@ def start_path( self._velocity_profile = None self._velocity_profile_pts = None - first_yaw = path.poses[0].orientation.euler[2] - robot_yaw = current_odom.orientation.euler[2] + first_yaw = quaternion_euler(path.poses[0].pose.orientation)[2] + robot_yaw = quaternion_euler(current_odom.pose.orientation)[2] yaw_err = angle_diff(first_yaw, robot_yaw) self._controller.reset_yaw_error(yaw_err) diff --git a/dimos/control/tasks/path_follower_task/test_path_follower_task.py b/dimos/control/tasks/path_follower_task/test_path_follower_task.py index f6c2e06298e..8e21d67dee7 100644 --- a/dimos/control/tasks/path_follower_task/test_path_follower_task.py +++ b/dimos/control/tasks/path_follower_task/test_path_follower_task.py @@ -20,6 +20,9 @@ import math +from dimos_generated.geometry_msgs.msg import Point, Pose, PoseStamped +from dimos_generated.std_msgs.msg import Header + from dimos.control.benchmarking.paths import circle, straight_line from dimos.control.benchmarking.velocity_profile import VelocityProfileConfig from dimos.control.task import CoordinatorState, JointStateSnapshot @@ -29,9 +32,7 @@ PathFollowerTaskConfig, ) from dimos.core.global_config import global_config as _gc -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Vector3 import Vector3 +from dimos.msgs.geometry import quaternion_euler, quaternion_from_euler _JOINTS = ["go2/vx", "go2/vy", "go2/wz"] @@ -54,8 +55,13 @@ def _task(**overrides) -> PathFollowerTask: def _start_aligned(task: PathFollowerTask, path) -> None: odom = PoseStamped( - position=Vector3(path.poses[0].position.x, path.poses[0].position.y, 0.0), - orientation=Quaternion.from_euler(Vector3(0.0, 0.0, path.poses[0].orientation.euler[2])), + header=Header(frame_id=""), + pose=Pose( + position=Point(x=path.poses[0].pose.position.x, y=path.poses[0].pose.position.y, z=0.0), + orientation=quaternion_from_euler( + 0.0, 0.0, quaternion_euler(path.poses[0].pose.orientation)[2] + ), + ), ) assert task.start_path(path, odom) diff --git a/dimos/control/tasks/planar_base_trajectory_task/planar_base_trajectory_task.py b/dimos/control/tasks/planar_base_trajectory_task/planar_base_trajectory_task.py index 8636b29dc69..f1b65f8bf54 100644 --- a/dimos/control/tasks/planar_base_trajectory_task/planar_base_trajectory_task.py +++ b/dimos/control/tasks/planar_base_trajectory_task/planar_base_trajectory_task.py @@ -22,6 +22,8 @@ import threading from typing import Any +from dimos_generated.dimos_msgs.msg import TrajectoryStatus +from dimos_generated.trajectory_msgs.msg import JointTrajectory from pydantic import ConfigDict, NonNegativeFloat, PositiveFloat from dimos.control.task import ( @@ -35,8 +37,8 @@ TrajectoryExecutionResult, TrajectoryExecutionStatus, ) -from dimos.msgs.trajectory_msgs.JointTrajectory import JointTrajectory -from dimos.msgs.trajectory_msgs.TrajectoryStatus import TrajectoryState, TrajectoryStatus +from dimos.msgs.time import duration_from_seconds, header_now, to_nanoseconds +from dimos.msgs.trajectory import TrajectoryState, sample_trajectory, trajectory_duration from dimos.protocol.service.spec import BaseConfig from dimos.utils.logging_config import setup_logger from dimos.utils.trigonometry import angle_diff @@ -156,20 +158,23 @@ def cancel(self) -> bool: def get_status(self, t_now: float | None = None) -> TrajectoryStatus: with self._lock: - duration = self._trajectory.duration if self._trajectory is not None else 0.0 + duration = ( + trajectory_duration(self._trajectory) if self._trajectory is not None else 0.0 + ) elapsed = self._elapsed if ( t_now is not None and self._start_t is not None - and self._state is TrajectoryState.EXECUTING + and self._state == TrajectoryState.EXECUTING ): elapsed = t_now - self._start_t progress = 1.0 if duration <= 0.0 else min(1.0, elapsed / duration) return TrajectoryStatus( + header=header_now(), state=self._state, progress=progress, - time_elapsed=elapsed, - time_remaining=max(0.0, duration - elapsed), + time_elapsed=duration_from_seconds(elapsed), + time_remaining=duration_from_seconds(max(0.0, duration - elapsed)), error=self._error, ) @@ -221,7 +226,7 @@ def compute(self, state: CoordinatorState) -> JointCommandOutput | None: if self._start_t is None: self._start_t = state.t_now self._elapsed = state.t_now - self._start_t - reference, reference_velocity = self._trajectory.sample(self._elapsed) + reference, reference_velocity = sample_trajectory(self._trajectory, self._elapsed) position_error = math.hypot(reference[0] - pose[0], reference[1] - pose[1]) yaw_error = abs(angle_diff(reference[2], pose[2])) @@ -243,14 +248,14 @@ def compute(self, state: CoordinatorState) -> JointCommandOutput | None: state.t_now, f"base is {position_error:.3f} m / {yaw_error:.3f} rad off the trajectory", ) - if self._elapsed >= self._trajectory.duration: + if self._elapsed >= trajectory_duration(self._trajectory): if ( position_error < config.position_goal_tolerance and yaw_error < config.orientation_goal_tolerance ): self._stop(state.t_now, TrajectoryState.COMPLETED, "") return self._command(0.0, 0.0, 0.0) - if self._elapsed - self._trajectory.duration > config.settle_timeout: + if self._elapsed - trajectory_duration(self._trajectory) > config.settle_timeout: return self._fail(state.t_now, "base did not reach the end of the trajectory") # Settling is pure feedback; the last point's velocity would carry it past. reference_velocity = [0.0, 0.0, 0.0] @@ -327,18 +332,22 @@ def _trajectory_problem( ) -> str: if trajectory is None or not trajectory.points: return "Base trajectory has no points" - previous_time: float | None = None + previous_time: int | None = None for point in trajectory.points: if len(point.positions) != 3 or len(point.velocities) != 3: return "Base trajectory points must carry x, y and yaw" - values = (*point.positions, *point.velocities, point.time_from_start) + try: + point_time = to_nanoseconds(point.time_from_start) + except ValueError: + return "Base trajectory contains an invalid duration" + values = (*point.positions, *point.velocities, point_time) if not all(math.isfinite(value) for value in values): return "Base trajectory contains non-finite values" - if previous_time is None and point.time_from_start != 0.0: + if previous_time is None and point_time != 0.0: return "Base trajectory must start at t=0" - if previous_time is not None and point.time_from_start <= previous_time: + if previous_time is not None and point_time <= previous_time: return "Base trajectory has non-increasing timestamps" - previous_time = point.time_from_start + previous_time = point_time vx, vy, wz = point.velocities if ( math.hypot(vx, vy) > config.max_linear + _SPEED_SLACK diff --git a/dimos/control/tasks/planar_base_trajectory_task/test_planar_base_trajectory_task.py b/dimos/control/tasks/planar_base_trajectory_task/test_planar_base_trajectory_task.py index 833a3400fb7..4ac516a1b47 100644 --- a/dimos/control/tasks/planar_base_trajectory_task/test_planar_base_trajectory_task.py +++ b/dimos/control/tasks/planar_base_trajectory_task/test_planar_base_trajectory_task.py @@ -14,6 +14,7 @@ import math +from dimos_generated.trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint import pytest from dimos.control.task import CoordinatorState, JointStateSnapshot @@ -22,9 +23,8 @@ PlanarBaseTrajectoryTaskConfig, ) from dimos.control.tasks.trajectory_task.trajectory_task import TrajectoryExecutionStatus -from dimos.msgs.trajectory_msgs.JointTrajectory import JointTrajectory -from dimos.msgs.trajectory_msgs.TrajectoryPoint import TrajectoryPoint -from dimos.msgs.trajectory_msgs.TrajectoryStatus import TrajectoryState +from dimos.msgs.time import duration_from_seconds, header_now +from dimos.msgs.trajectory import TrajectoryState, sample_trajectory from dimos.utils.trigonometry import angle_diff JOINTS = ["base/vx", "base/vy", "base/wz"] @@ -58,14 +58,14 @@ def step(self, command): def _line(start, goal, duration, n=60): velocity = [(g - s) / duration for s, g in zip(start, goal, strict=True)] points = [ - TrajectoryPoint( - time_from_start=duration * i / (n - 1), + JointTrajectoryPoint( + time_from_start=duration_from_seconds(duration * i / (n - 1)), positions=[s + (g - s) * i / (n - 1) for s, g in zip(start, goal, strict=True)], velocities=velocity, ) for i in range(n) ] - return JointTrajectory(points=points, joint_names=["x", "y", "yaw"]) + return JointTrajectory(header=header_now(), points=points, joint_names=["x", "y", "yaw"]) def _run(task, base, seconds, t=100.0): @@ -94,7 +94,7 @@ def test_follows_a_timed_trajectory_across_the_yaw_seam(): task.execute(plan) _, t = _run(task, base, seconds=2.0) - reference, _ = plan.sample(2.0) + reference, _ = sample_trajectory(plan, 2.0) assert math.hypot(base.x - reference[0], base.y - reference[1]) < 0.01 assert abs(angle_diff(base.yaw, reference[2])) < 0.01 @@ -113,7 +113,7 @@ def test_feedback_pulls_an_offset_base_onto_the_plan(): _run(task, base, seconds=2.0) - reference, _ = plan.sample(2.0) + reference, _ = sample_trajectory(plan, 2.0) assert math.hypot(base.x - reference[0], base.y - reference[1]) < 0.02 assert abs(angle_diff(base.yaw, reference[2])) < 0.02 @@ -141,7 +141,7 @@ def test_a_second_trajectory_starts_on_its_first_tick(): plan = _line((base.x, base.y, base.yaw), (base.x + 1.0, base.y, base.yaw), 2.0) task.execute(plan) _, t = _run(task, base, seconds=1.0, t=t) - assert abs(base.x - plan.sample(1.0)[0][0]) < 0.01 + assert abs(base.x - sample_trajectory(plan, 1.0)[0][0]) < 0.01 _run(task, base, seconds=2.0, t=t) assert task.get_status().state == TrajectoryState.COMPLETED @@ -150,11 +150,16 @@ def test_a_second_trajectory_starts_on_its_first_tick(): def test_rejects_trajectories_it_cannot_follow(): nan_velocity = _line((0.0, 0.0, 0.0), (1.0, 0.0, 0.0), duration=2.0) - nan_velocity.points[5].velocities = [math.nan, 0.0, 0.0] + point = nan_velocity.points[5] + point.velocities = [math.nan, 0.0, 0.0] + nan_velocity.points[5] = point never_ends = _line((0.0, 0.0, 0.0), (1.0, 0.0, 0.0), duration=2.0) - never_ends.points[-1].time_from_start = math.inf + point = never_ends.points[-1] + point.time_from_start.nanosec = 1_000_000_000 + never_ends.points[-1] = point two_columns = JointTrajectory( - points=[TrajectoryPoint(positions=[0.0, 0.0], velocities=[0.0, 0.0])] + header=header_now(), + points=[JointTrajectoryPoint(positions=[0.0, 0.0], velocities=[0.0, 0.0])], ) diagonal_too_fast = _line((0.0, 0.0, 0.0), (1.0, 1.0, 0.0), duration=1.0) diff --git a/dimos/control/tasks/pose_target_ik.py b/dimos/control/tasks/pose_target_ik.py index c3804a64cf3..d294e1a7c89 100644 --- a/dimos/control/tasks/pose_target_ik.py +++ b/dimos/control/tasks/pose_target_ik.py @@ -19,6 +19,7 @@ from abc import abstractmethod from collections import deque from collections.abc import Iterable, Mapping, Sequence +import copy from dataclasses import dataclass, field import math import threading @@ -26,6 +27,9 @@ from typing import TYPE_CHECKING import attrs +from dimos_generated.geometry_msgs.msg import Pose, PoseStamped +from dimos_generated.sensor_msgs.msg import JointState +from dimos_generated.std_msgs.msg import Header import numpy as np import pink from pydantic import Field @@ -46,12 +50,10 @@ ) from dimos.manipulation.planning.spec.config import RobotModelConfig from dimos.manipulation.planning.spec.validation import prepare_robot_model -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.sensor_msgs.JointState import JointState +from dimos.msgs.geometry import pose_from_matrix, pose_matrix from dimos.protocol.service.spec import BaseConfig from dimos.robot.assets.model import RobotModel from dimos.utils.logging_config import setup_logger -from dimos.utils.transform_utils import matrix_to_pose, pose_to_matrix if TYPE_CHECKING: from numpy.typing import NDArray @@ -290,7 +292,7 @@ def step( ) -> JointState | None: """Advance the persistent command trajectory by one bounded QP step.""" with self._command_state_lock: - command_state = JointState(self._command_state or measured_state) + command_state = copy.deepcopy(self._command_state or measured_state) command_increment_history = tuple( increment.copy() for increment in self._command_increment_history ) @@ -315,7 +317,7 @@ def step( with self._command_state_lock: if self._command_state_generation != generation: return None - self._command_state = JointState(result.command) + self._command_state = copy.deepcopy(result.command) self._command_increment_history.append(result.bounded_increment.copy()) return result.command @@ -508,14 +510,13 @@ def _frame_poses( ) positions = _seed_positions_for_mapping(seed, context.robot.mapping) q = self._q_from_dimos_positions(context.robot, positions) - base_world = pose_to_matrix(robot_model.base_pose) + base_world = pose_matrix(robot_model.base_pose.pose) poses: dict[str, PoseStamped] = {} for frame_name, frame_context in context.frames.items(): - pose = matrix_to_pose(base_world @ self._current_frame_matrix(frame_context, q)) + pose = pose_from_matrix(base_world @ self._current_frame_matrix(frame_context, q)) poses[frame_name] = PoseStamped( - frame_id=robot_model.base_link, - position=pose.position, - orientation=pose.orientation, + header=Header(frame_id=robot_model.base_link), + pose=Pose(position=pose.position, orientation=pose.orientation), ) return poses @@ -727,7 +728,9 @@ def compute(self, state: CoordinatorState) -> JointCommandOutput | None: return None if result is None: return None - if result.name != list(self._joint_names) or len(result.position) != len(self._joint_names): + if list(result.name) != list(self._joint_names) or len(result.position) != len( + self._joint_names + ): logger.warning("Pink control step returned an invalid joint result", task=self._name) return None diff --git a/dimos/control/tasks/rpp_path_follower_task/rpp_path_follower_task.py b/dimos/control/tasks/rpp_path_follower_task/rpp_path_follower_task.py index 6c8dd1f7b51..6607ec70067 100644 --- a/dimos/control/tasks/rpp_path_follower_task/rpp_path_follower_task.py +++ b/dimos/control/tasks/rpp_path_follower_task/rpp_path_follower_task.py @@ -38,6 +38,9 @@ from pathlib import Path as _Path from typing import TYPE_CHECKING, Any +from dimos_generated.geometry_msgs.msg import Point, Pose, PoseStamped +from dimos_generated.nav_msgs.msg import Path + from dimos.control.benchmarking.tuning import TuningConfig from dimos.control.benchmarking.velocity_profile import PathSpeedCap, VelocityProfileConfig from dimos.control.tasks.feedforward_gain_compensator import ( @@ -49,10 +52,7 @@ PathFollowerTaskConfig, ) from dimos.core.global_config import global_config as _gc -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.msgs.nav_msgs.Path import Path +from dimos.msgs.geometry import quaternion_euler, quaternion_from_euler from dimos.protocol.service.spec import BaseConfig from dimos.utils.logging_config import setup_logger @@ -87,7 +87,7 @@ def _with_tangent_headings(path: Path) -> Path: if poses is None or len(poses) < 2: return path - yaws = [p.orientation.euler[2] for p in poses] + yaws = [quaternion_euler(p.pose.orientation)[2] for p in poses] if max(yaws) - min(yaws) > _HEADING_DEGENERATE_EPS: return path # planner provided real per-pose headings; leave them. @@ -95,27 +95,28 @@ def _with_tangent_headings(path: Path) -> Path: n = len(poses) for i, p in enumerate(poses): if i < n - 1: - dx = poses[i + 1].position.x - p.position.x - dy = poses[i + 1].position.y - p.position.y + dx = poses[i + 1].pose.position.x - p.pose.position.x + dy = poses[i + 1].pose.position.y - p.pose.position.y yaw = ( math.atan2(dy, dx) if (dx * dx + dy * dy) > 1e-12 # Coincident waypoints: keep the previous heading rather than # snapping to 0. - else (new_poses[-1].orientation.euler[2] if new_poses else 0.0) + else (quaternion_euler(new_poses[-1].pose.orientation)[2] if new_poses else 0.0) ) else: # Last pose has no next segment — settle to the approach heading. - yaw = new_poses[-1].orientation.euler[2] if new_poses else 0.0 + yaw = quaternion_euler(new_poses[-1].pose.orientation)[2] if new_poses else 0.0 new_poses.append( PoseStamped( - ts=p.ts, - frame_id=p.frame_id, - position=Vector3(p.position.x, p.position.y, p.position.z), - orientation=Quaternion.from_euler(Vector3(0.0, 0.0, yaw)), + header=p.header, + pose=Pose( + position=Point(x=p.pose.position.x, y=p.pose.position.y, z=p.pose.position.z), + orientation=quaternion_from_euler(0.0, 0.0, yaw), + ), ) ) - return Path(ts=path.ts, frame_id=path.frame_id, poses=new_poses) + return Path(header=path.header, poses=new_poses) class RPPPathFollowerTask(PathFollowerTask): diff --git a/dimos/control/tasks/rpp_path_follower_task/test_rpp_path_follower_task.py b/dimos/control/tasks/rpp_path_follower_task/test_rpp_path_follower_task.py index c2ceb44fca7..d65870a14fc 100644 --- a/dimos/control/tasks/rpp_path_follower_task/test_rpp_path_follower_task.py +++ b/dimos/control/tasks/rpp_path_follower_task/test_rpp_path_follower_task.py @@ -19,6 +19,8 @@ from types import SimpleNamespace +from dimos_generated.geometry_msgs.msg import Point, Pose, PoseStamped, Quaternion +from dimos_generated.std_msgs.msg import Header import pytest from dimos.control.benchmarking.paths import circle, straight_line @@ -41,9 +43,8 @@ RPPPathFollowerTask, create_task, ) -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Vector3 import Vector3 +from dimos.msgs.geometry import quaternion_euler, quaternion_from_euler +from dimos.msgs.time import time_from_seconds _JOINTS = ["go2/vx", "go2/vy", "go2/wz"] @@ -83,7 +84,10 @@ def _task(artifact_path: str, **params) -> RPPPathFollowerTask: def _odom(x=0.0, y=0.0, yaw=0.0) -> PoseStamped: return PoseStamped( - position=Vector3(x, y, 0.0), orientation=Quaternion.from_euler(Vector3(0.0, 0.0, yaw)) + header=Header(frame_id=""), + pose=Pose( + position=Point(x=x, y=y, z=0.0), orientation=quaternion_from_euler(0.0, 0.0, yaw) + ), ) @@ -232,20 +236,24 @@ def test_drives_to_arrival_forward_only(artifact_path): import math +from dimos_generated.nav_msgs.msg import Path + from dimos.control.tasks.rpp_path_follower_task.rpp_path_follower_task import ( _with_tangent_headings, ) -from dimos.msgs.nav_msgs.Path import Path def _ident_path(points): """Path with identity orientation on every pose (what MLSPlannerNative emits).""" return Path( - ts=0.0, - frame_id="odom", + header=Header(frame_id="odom"), poses=[ PoseStamped( - ts=0.0, position=Vector3(x, y, 0.0), orientation=Quaternion(0.0, 0.0, 0.0, 1.0) + header=Header(frame_id="", stamp=time_from_seconds(0.0)), + pose=Pose( + position=Point(x=x, y=y, z=0.0), + orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0), + ), ) for x, y in points ], @@ -255,7 +263,7 @@ def _ident_path(points): def test_degenerate_path_gets_tangent_headings(): # Straight +x then turn +y: first segments face 0 rad, the corner faces +pi/2. out = _with_tangent_headings(_ident_path([(0, 0), (1, 0), (2, 0), (2, 1), (2, 2)])) - yaws = [p.orientation.euler[2] for p in out.poses] + yaws = [quaternion_euler(p.pose.orientation)[2] for p in out.poses] assert math.isclose(yaws[0], 0.0, abs_tol=1e-6) assert math.isclose(yaws[1], 0.0, abs_tol=1e-6) assert math.isclose(yaws[2], math.pi / 2, abs_tol=1e-6) # tangent into the turn @@ -267,24 +275,25 @@ def test_degenerate_path_gets_tangent_headings(): def test_positions_unchanged_by_synthesis(): pts = [(0, 0), (1, 0), (1, 1)] out = _with_tangent_headings(_ident_path(pts)) - assert [(round(p.position.x, 6), round(p.position.y, 6)) for p in out.poses] == pts + assert [(round(p.pose.position.x, 6), round(p.pose.position.y, 6)) for p in out.poses] == pts def test_path_with_real_headings_is_untouched(): # A planner that already provides per-pose orientations must be left alone. oriented = Path( - ts=0.0, - frame_id="odom", + header=Header(frame_id="odom"), poses=[ PoseStamped( - ts=0.0, - position=Vector3(0, 0, 0), - orientation=Quaternion.from_euler(Vector3(0, 0, 0.3)), + header=Header(frame_id="", stamp=time_from_seconds(0.0)), + pose=Pose( + position=Point(x=0, y=0, z=0), orientation=quaternion_from_euler(0, 0, 0.3) + ), ), PoseStamped( - ts=0.0, - position=Vector3(1, 0, 0), - orientation=Quaternion.from_euler(Vector3(0, 0, 1.4)), + header=Header(frame_id="", stamp=time_from_seconds(0.0)), + pose=Pose( + position=Point(x=1, y=0, z=0), orientation=quaternion_from_euler(0, 0, 1.4) + ), ), ], ) diff --git a/dimos/control/tasks/teleop_ik_task/teleop_ik_task.py b/dimos/control/tasks/teleop_ik_task/teleop_ik_task.py index 0ce08bf6fed..752a2ca78cf 100644 --- a/dimos/control/tasks/teleop_ik_task/teleop_ik_task.py +++ b/dimos/control/tasks/teleop_ik_task/teleop_ik_task.py @@ -17,12 +17,15 @@ from __future__ import annotations from collections.abc import Iterable, Mapping +import copy from dataclasses import dataclass from enum import Enum import threading from typing import TYPE_CHECKING import attrs +from dimos_generated.geometry_msgs.msg import Pose, PoseStamped +from dimos_generated.std_msgs.msg import Header, UInt32 from dimos.control.task import CoordinatorState from dimos.control.tasks.pose_target_ik import ( @@ -35,8 +38,7 @@ ) from dimos.manipulation.planning.kinematics.config import PinkKinematicsConfig from dimos.manipulation.planning.spec.config import RobotModelConfig -from dimos.msgs.geometry_msgs.Pose import Pose -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped +from dimos.msgs.geometry import pose_from_matrix, pose_matrix from dimos.protocol.service.spec import BaseConfig from dimos.teleop.webxr.controller_types import Buttons @@ -173,12 +175,7 @@ def _on_controller_pose( ) -> bool: if hand not in self._bindings: return False - sample = PoseStamped( - ts=pose.ts if isinstance(pose, PoseStamped) else 0.0, - frame_id=pose.frame_id if isinstance(pose, PoseStamped) else "", - position=pose.position, - orientation=pose.orientation, - ) + sample = copy.deepcopy(pose) if isinstance(pose, PoseStamped) else PoseStamped(pose=pose) with self._lock: if self._session_state is _SessionState.ESTOPPED: return False @@ -187,11 +184,12 @@ def _on_controller_pose( state.last_update_time = t_now return True - def on_teleop_buttons(self, msg: Buttons, t_now: float) -> bool: + def on_teleop_buttons(self, msg: UInt32, t_now: float) -> bool: """Update the all-bound-hands deadman condition.""" + buttons = Buttons(data=msg.data) grip_by_hand = { - OperatorHand.LEFT: msg.left_grip, - OperatorHand.RIGHT: msg.right_grip, + OperatorHand.LEFT: buttons.left_grip, + OperatorHand.RIGHT: buttons.right_grip, } with self._lock: self._last_button_update_time = t_now @@ -267,11 +265,19 @@ def _frame_target_snapshot(self, state: CoordinatorState) -> FrameTargetSnapshot robot_reference = hand_state.robot_reference if current is None or controller_reference is None or robot_reference is None: return None - delta = current - controller_reference + current_matrix = pose_matrix(current.pose) + reference_matrix = pose_matrix(controller_reference.pose) + target_matrix = pose_matrix(robot_reference.pose) + target_matrix[:3, 3] += current_matrix[:3, 3] - reference_matrix[:3, 3] + target_matrix[:3, :3] = ( + current_matrix[:3, :3] @ reference_matrix[:3, :3].T @ target_matrix[:3, :3] + ) targets[binding.target_frame] = PoseStamped( - frame_id=self._teleop_config.robot_model.base_pose.frame_id, - position=robot_reference.position + delta.position, - orientation=delta.orientation * robot_reference.orientation, + header=Header( + frame_id=self._teleop_config.robot_model.base_pose.header.frame_id, + stamp=current.header.stamp, + ), + pose=pose_from_matrix(target_matrix), ) update_times.append(hand_state.last_update_time) return FrameTargetSnapshot( diff --git a/dimos/control/tasks/teleop_ik_task/test_teleop_ik_task.py b/dimos/control/tasks/teleop_ik_task/test_teleop_ik_task.py index fb88bd40486..a7bf6309e41 100644 --- a/dimos/control/tasks/teleop_ik_task/test_teleop_ik_task.py +++ b/dimos/control/tasks/teleop_ik_task/test_teleop_ik_task.py @@ -17,6 +17,9 @@ from pathlib import Path from typing import cast +from dimos_generated.geometry_msgs.msg import Point, Pose, PoseStamped, Quaternion +from dimos_generated.sensor_msgs.msg import JointState +from dimos_generated.std_msgs.msg import Header, UInt32 import pytest from pytest_mock import MockerFixture @@ -31,10 +34,6 @@ create_task, ) from dimos.manipulation.planning.spec.config import RobotModelConfig -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.msgs.sensor_msgs.JointState import JointState from dimos.robot.assets.model import RobotModel from dimos.teleop.webxr.controller_types import Buttons @@ -69,12 +68,18 @@ def _solver(mocker: MockerFixture) -> PinkPoseTargetSolver: solver = mocker.Mock(spec=PinkPoseTargetSolver) solver.frame_poses.return_value = { "left_tool": PoseStamped( - position=Vector3(1.0, 0.0, 0.0), - orientation=Quaternion(0.0, 0.0, 0.0, 1.0), + header=Header(frame_id=""), + pose=Pose( + position=Point(x=1.0, y=0.0, z=0.0), + orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0), + ), ), "right_tool": PoseStamped( - position=Vector3(-1.0, 0.0, 0.0), - orientation=Quaternion(0.0, 0.0, 0.0, 1.0), + header=Header(frame_id=""), + pose=Pose( + position=Point(x=-1.0, y=0.0, z=0.0), + orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0), + ), ), } solver.step.return_value = JointState( @@ -95,11 +100,11 @@ def _buttons( *, left: bool = False, right: bool = False, -) -> Buttons: +) -> UInt32: buttons = Buttons() buttons.left_grip = left buttons.right_grip = right - return buttons + return buttons.to_message() def test_face_buttons_do_not_engage_arm_teleop(mocker: MockerFixture) -> None: @@ -111,7 +116,7 @@ def test_face_buttons_do_not_engage_arm_teleop(mocker: MockerFixture) -> None: buttons = Buttons() buttons.right_primary = True - task.on_teleop_buttons(buttons, 1.0) + task.on_teleop_buttons(buttons.to_message(), 1.0) task.on_right_cartesian_command(_pose(0.5), 1.0) assert task.compute(_state()) is None @@ -119,8 +124,10 @@ def test_face_buttons_do_not_engage_arm_teleop(mocker: MockerFixture) -> None: def _pose(x: float) -> PoseStamped: return PoseStamped( - position=Vector3(x, 0.0, 0.0), - orientation=Quaternion(0.0, 0.0, 0.0, 1.0), + header=Header(frame_id=""), + pose=Pose( + position=Point(x=x, y=0.0, z=0.0), orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0) + ), ) @@ -179,7 +186,7 @@ def test_single_binding_tracks_relative_controller_motion(mocker: MockerFixture) assert output is not None target = solver.step.call_args.args[0]["right_tool"] - assert target.position.x == pytest.approx(-0.8) + assert target.pose.position.x == pytest.approx(-0.8) def test_bimanual_task_requires_both_hands_and_releases_atomically( @@ -306,7 +313,7 @@ def test_bimanual_timeout_clears_both_sides_and_reengagement_recaptures( def test_stale_deadman_stops_fresh_pose_streams( mocker: MockerFixture, bindings: tuple[TeleopHandBinding, ...], - buttons: Buttons, + buttons: UInt32, ) -> None: solver = _solver(mocker) task = TeleopIKTask("teleop", _config(bindings, timeout=0.2), solver=solver) diff --git a/dimos/control/tasks/test_pose_target_ik.py b/dimos/control/tasks/test_pose_target_ik.py index 5837d823fb1..80d24c85291 100644 --- a/dimos/control/tasks/test_pose_target_ik.py +++ b/dimos/control/tasks/test_pose_target_ik.py @@ -16,6 +16,9 @@ from pathlib import Path +from dimos_generated.geometry_msgs.msg import Pose, PoseStamped +from dimos_generated.sensor_msgs.msg import JointState +from dimos_generated.std_msgs.msg import Header import numpy as np import pytest from pytest_mock import MockerFixture @@ -31,8 +34,6 @@ ) from dimos.manipulation.planning.kinematics.pink_solver import _PinkSolverCore from dimos.manipulation.planning.spec.config import RobotModelConfig -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.sensor_msgs.JointState import JointState from dimos.robot.assets.model import RobotModel @@ -97,7 +98,9 @@ def _solver(mocker: MockerFixture, positions: list[float] | None = None) -> Pink solver.step.return_value = JointState( name=["arm/a", "arm/b"], position=positions or [0.01, -0.01] ) - solver.frame_poses.return_value = {"tool": PoseStamped(frame_id="base")} + solver.frame_poses.return_value = { + "tool": PoseStamped(header=Header(frame_id="base"), pose=Pose()) + } return solver @@ -116,7 +119,7 @@ def _snapshot( extra_joint_positions: dict[str, float] | None = None, ) -> FrameTargetSnapshot: return FrameTargetSnapshot( - targets=targets or {"tool": PoseStamped(frame_id="world")}, + targets=targets or {"tool": PoseStamped(header=Header(frame_id="world"), pose=Pose())}, last_update_time=last_update_time, extra_joint_positions=extra_joint_positions or {}, ) @@ -145,7 +148,7 @@ def advance(**kwargs: object) -> _StreamingStepResult: ) step = mocker.patch.object(solver, "_step_frame_targets", side_effect=advance) - targets = {"tool": PoseStamped()} + targets = {"tool": PoseStamped(header=Header(frame_id=""), pose=Pose())} assert solver.step(targets, JointState(name=["arm/a", "arm/b"], position=[0.0, 0.0]), 0.01) assert solver.step( @@ -180,7 +183,7 @@ def test_pose_target_solver_reset_reseeds_from_feedback(mocker: MockerFixture) - ), ], ) - targets = {"tool": PoseStamped()} + targets = {"tool": PoseStamped(header=Header(frame_id=""), pose=Pose())} assert solver.step(targets, JointState(name=["arm/a", "arm/b"], position=[0.0, 0.0]), 0.01) solver.reset() @@ -209,7 +212,7 @@ def reset_during_step(**_kwargs: object) -> _StreamingStepResult: mocker.patch.object(solver, "_step_frame_targets", side_effect=reset_during_step) result = solver.step( - {"tool": PoseStamped()}, + {"tool": PoseStamped(header=Header(frame_id=""), pose=Pose())}, JointState(name=["arm/a", "arm/b"], position=[0.0, 0.0]), 0.01, ) @@ -362,7 +365,7 @@ def test_current_frame_poses_uses_live_coordinator_seed(mocker: MockerFixture) - poses = task.current_frame_poses(_state(positions={"arm/a": 0.2, "arm/b": 0.3}), ["tool"]) - assert poses == {"tool": PoseStamped(frame_id="base")} + assert poses == {"tool": PoseStamped(header=Header(frame_id="base"), pose=Pose())} seed = solver.frame_poses.call_args.args[0] assert seed.name == ["arm/a", "arm/b"] assert seed.position == [0.2, 0.3] diff --git a/dimos/control/tasks/trajectory_task/test_trajectory_task.py b/dimos/control/tasks/trajectory_task/test_trajectory_task.py index cbef3f88cf9..cf126d5e73f 100644 --- a/dimos/control/tasks/trajectory_task/test_trajectory_task.py +++ b/dimos/control/tasks/trajectory_task/test_trajectory_task.py @@ -12,6 +12,8 @@ # See the License for the specific language governing permissions and # limitations under the License. +from dimos_generated.builtin_interfaces.msg import Duration +from dimos_generated.trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint import pytest from dimos.control.task import ( @@ -23,8 +25,7 @@ JointTrajectoryTaskConfig, TrajectoryExecutionStatus, ) -from dimos.msgs.trajectory_msgs.JointTrajectory import JointTrajectory -from dimos.msgs.trajectory_msgs.TrajectoryPoint import TrajectoryPoint +from dimos.msgs.time import duration_from_seconds, header_now @pytest.mark.parametrize("single_point", [False, True]) @@ -36,7 +37,9 @@ def test_jtt_does_not_pull_joint_back_after_another_task_moves_it(single_point, ) first_target = 1.0 if preempted else 0.1 first = JointTrajectory( - joint_names=["joint"], points=[TrajectoryPoint(positions=[first_target])] + header=header_now(), + joint_names=["joint"], + points=[JointTrajectoryPoint(positions=[first_target])], ) assert task.execute(first, {}).status is TrajectoryExecutionStatus.ACCEPTED output = task.compute(state) @@ -51,15 +54,16 @@ def test_jtt_does_not_pull_joint_back_after_another_task_moves_it(single_point, joints=JointStateSnapshot(joint_positions={"joint": -1.0}), t_now=1.0, dt=0.01 ) assert task.compute(state) is None - points = [TrajectoryPoint(positions=[-1.2])] + points = [JointTrajectoryPoint(positions=[-1.2])] if not single_point: points = [ - TrajectoryPoint(positions=[-1.0]), - TrajectoryPoint(positions=[-1.2], time_from_start=0.1), + JointTrajectoryPoint(positions=[-1.0]), + JointTrajectoryPoint(positions=[-1.2], time_from_start=duration_from_seconds(0.1)), ] assert ( task.execute( - JointTrajectory(joint_names=["joint"], points=points), state.joints.joint_positions + JointTrajectory(header=header_now(), joint_names=["joint"], points=points), + state.joints.joint_positions, ).status is TrajectoryExecutionStatus.ACCEPTED ) @@ -84,7 +88,9 @@ def test_completed_joint_reanchors_while_other_joint_keeps_command_continuity(): task = JointTrajectoryTask(JointTrajectoryTaskConfig(joint_names=["finished", "running"])) measured = JointStateSnapshot(joint_positions={"finished": 0.0, "running": 0.0}) initial = JointTrajectory( - joint_names=["finished", "running"], points=[TrajectoryPoint(positions=[0.1, 1.0])] + header=header_now(), + joint_names=["finished", "running"], + points=[JointTrajectoryPoint(positions=[0.1, 1.0])], ) assert task.execute(initial, {}).status is TrajectoryExecutionStatus.ACCEPTED output = task.compute(CoordinatorState(joints=measured, t_now=0.1, dt=0.1)) @@ -92,10 +98,11 @@ def test_completed_joint_reanchors_while_other_joint_keeps_command_continuity(): assert output.positions == pytest.approx([0.1, 0.1]) measured.joint_positions["finished"] = -1.0 replacement = JointTrajectory( + header=header_now(), joint_names=["finished"], points=[ - TrajectoryPoint(positions=[-1.0]), - TrajectoryPoint(positions=[0.1], time_from_start=2.0), + JointTrajectoryPoint(positions=[-1.0]), + JointTrajectoryPoint(positions=[0.1], time_from_start=duration_from_seconds(2.0)), ], ) assert ( @@ -116,7 +123,9 @@ def test_completed_joint_reanchors_while_other_joint_keeps_command_continuity(): ) def test_completed_trajectory_does_not_bypass_start_validation(positions, expected): task = JointTrajectoryTask(JointTrajectoryTaskConfig(joint_names=["joint"])) - initial = JointTrajectory(joint_names=["joint"], points=[TrajectoryPoint(positions=[0.1])]) + initial = JointTrajectory( + header=header_now(), joint_names=["joint"], points=[JointTrajectoryPoint(positions=[0.1])] + ) assert task.execute(initial, {}).status is TrajectoryExecutionStatus.ACCEPTED task.compute( CoordinatorState( @@ -124,10 +133,36 @@ def test_completed_trajectory_does_not_bypass_start_validation(positions, expect ) ) trajectory = JointTrajectory( + header=header_now(), joint_names=["joint"], points=[ - TrajectoryPoint(positions=[0.1]), - TrajectoryPoint(positions=[1.0], time_from_start=2.0), + JointTrajectoryPoint(positions=[0.1]), + JointTrajectoryPoint(positions=[1.0], time_from_start=duration_from_seconds(2.0)), ], ) assert task.execute(trajectory, positions).status is expected + + +@pytest.mark.parametrize("nanosec,accepted", [(0, True), (1_000_000_000, False)]) +def test_jtt_validates_normalized_durations_without_float_ordering(nanosec, accepted): + task = JointTrajectoryTask(JointTrajectoryTaskConfig(joint_names=["joint"])) + # Far enough out that float seconds cannot distinguish adjacent nanoseconds. + trajectory = JointTrajectory( + joint_names=["joint"], + points=[ + JointTrajectoryPoint(positions=[0.0]), + JointTrajectoryPoint( + positions=[0.1], time_from_start=Duration(sec=100_000_000, nanosec=nanosec) + ), + JointTrajectoryPoint( + positions=[0.2], time_from_start=Duration(sec=100_000_000, nanosec=1) + ), + ], + ) + result = task.execute(trajectory, {"joint": 0.0}) + expected = ( + TrajectoryExecutionStatus.ACCEPTED + if accepted + else TrajectoryExecutionStatus.INVALID_TRAJECTORY + ) + assert result.status == expected diff --git a/dimos/control/tasks/trajectory_task/trajectory_task.py b/dimos/control/tasks/trajectory_task/trajectory_task.py index 7a20538788e..50697d2da2a 100644 --- a/dimos/control/tasks/trajectory_task/trajectory_task.py +++ b/dimos/control/tasks/trajectory_task/trajectory_task.py @@ -28,6 +28,9 @@ import math from typing import TYPE_CHECKING, Annotated, Any +from dimos_generated.dimos_msgs.msg import TrajectoryStatus +from dimos_generated.sensor_msgs.msg import JointState +from dimos_generated.trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint from pydantic import BeforeValidator, ConfigDict, Field from pydantic.dataclasses import dataclass as pydantic_dataclass @@ -38,10 +41,8 @@ JointCommandOutput, ResourceClaim, ) -from dimos.msgs.sensor_msgs.JointState import JointState -from dimos.msgs.trajectory_msgs.JointTrajectory import JointTrajectory -from dimos.msgs.trajectory_msgs.TrajectoryPoint import TrajectoryPoint -from dimos.msgs.trajectory_msgs.TrajectoryStatus import TrajectoryState, TrajectoryStatus +from dimos.msgs.time import duration_from_seconds, header_now, to_nanoseconds +from dimos.msgs.trajectory import TrajectoryState, sample_trajectory, trajectory_duration from dimos.protocol.service.spec import BaseConfig from dimos.utils.logging_config import setup_logger @@ -241,8 +242,9 @@ def on_joint_command(self, msg: JointState, t_now: float) -> bool: if not selected: return False trajectory = JointTrajectory( + header=header_now(), joint_names=[name for name, _ in selected], - points=[TrajectoryPoint(positions=[position for _, position in selected])], + points=[JointTrajectoryPoint(positions=[position for _, position in selected])], ) return self.execute(trajectory, {}).status is TrajectoryExecutionStatus.ACCEPTED @@ -274,7 +276,7 @@ def compute(self, state: CoordinatorState) -> JointCommandOutput | None: self._pending_start = False elapsed = max(0.0, state.t_now - run.start_time) self._last_elapsed = max(self._last_elapsed, elapsed) - desired = run.trajectory.sample(elapsed)[0][index] + desired = sample_trajectory(run.trajectory, elapsed)[0][index] current = self._commanded_positions.get(joint_name) if current is None: current = state.joints.get_position(joint_name) @@ -287,7 +289,7 @@ def compute(self, state: CoordinatorState) -> JointCommandOutput | None: self._commanded_positions[joint_name] = commanded final_position = run.trajectory.points[-1].positions[index] - nominal_complete = elapsed >= run.trajectory.duration + nominal_complete = elapsed >= trajectory_duration(run.trajectory) reached = math.isclose(commanded, final_position, abs_tol=1e-9) if nominal_complete and reached: del self._motions[joint_name] @@ -355,9 +357,9 @@ def _validate_trajectory(self, trajectory: JointTrajectory) -> bool: logger.warning("Empty trajectory for %s", self._name) return False width = len(joint_names) - previous_time: float | None = None + previous_time: int | None = None for index, point in enumerate(trajectory.points): - if len(point.positions) != width or len(point.velocities) != width: + if len(point.positions) != width or len(point.velocities) not in (0, width): logger.warning("Trajectory point %d for %s has invalid width", index, self._name) return False if not all(math.isfinite(value) for value in point.positions): @@ -370,17 +372,19 @@ def _validate_trajectory(self, trajectory: JointTrajectory) -> bool: "Trajectory point %d for %s has non-finite velocities", index, self._name ) return False - if not math.isfinite(point.time_from_start): - logger.warning("Trajectory point %d for %s has non-finite time", index, self._name) + try: + point_time = to_nanoseconds(point.time_from_start) + except ValueError: + logger.warning("Trajectory point %d for %s has invalid duration", index, self._name) return False - if index == 0 and point.time_from_start != 0.0: + if index == 0 and point_time != 0.0: logger.warning("Trajectory for %s must start at t=0", self._name) return False - if previous_time is not None and point.time_from_start <= previous_time: + if previous_time is not None and point_time <= previous_time: logger.warning("Trajectory for %s has non-increasing timestamps", self._name) return False - previous_time = point.time_from_start - if len(trajectory.points) > 1 and trajectory.duration <= 0.0: + previous_time = point_time + if len(trajectory.points) > 1 and trajectory_duration(trajectory) <= 0.0: logger.warning("Trajectory for %s has nonpositive duration", self._name) return False return True @@ -448,14 +452,16 @@ def execute( trajectory = JointTrajectory( joint_names=list(trajectory.joint_names), points=[ - TrajectoryPoint( + JointTrajectoryPoint( time_from_start=trajectory.points[0].time_from_start, positions=first_positions, - velocities=list(trajectory.points[0].velocities), + velocities=trajectory.points[0].velocities, + accelerations=trajectory.points[0].accelerations, + effort=trajectory.points[0].effort, ), - *trajectory.points[1:], + *list(trajectory.points)[1:], ], - timestamp=trajectory.timestamp, + header=trajectory.header, ) run = _TrajectoryRun(trajectory) @@ -464,14 +470,14 @@ def execute( if len(trajectory.points) > 1 and joint_name not in self._commanded_positions: self._commanded_positions[joint_name] = current_positions[joint_name] self._trajectory = trajectory - self._last_duration = trajectory.duration + self._last_duration = trajectory_duration(trajectory) self._last_elapsed = 0.0 self._pending_start = True # Start time set on first compute() self._state = TrajectoryState.EXECUTING logger.info( f"Executing trajectory on {self._name}: " - f"{len(trajectory.points)} points, duration={trajectory.duration:.3f}s" + f"{len(trajectory.points)} points, duration={trajectory_duration(trajectory):.3f}s" ) return TrajectoryExecutionResult(TrajectoryExecutionStatus.ACCEPTED) @@ -528,7 +534,7 @@ def _active_run_status(self, t_now: float) -> tuple[float, float, float]: remaining_times: list[float] = [] for run in runs: elapsed = 0.0 if run.start_time is None else max(0.0, t_now - run.start_time) - duration = run.trajectory.duration + duration = trajectory_duration(run.trajectory) progress = 0.0 if duration <= 0.0 else min(1.0, elapsed / duration) progresses.append(progress) elapsed_times.append(elapsed) @@ -543,17 +549,21 @@ def get_status(self, t_now: float) -> TrajectoryStatus: if self._state == TrajectoryState.EXECUTING: progress, elapsed, remaining = self._active_run_status(t_now) return TrajectoryStatus( + header=header_now(), state=self._state, progress=progress, - time_elapsed=elapsed, - time_remaining=remaining, + time_elapsed=duration_from_seconds(elapsed), + time_remaining=duration_from_seconds(remaining), ) completed = self._state == TrajectoryState.COMPLETED return TrajectoryStatus( + header=header_now(), state=self._state, progress=1.0 if completed else 0.0, - time_elapsed=self._last_duration if completed else self._last_elapsed, - time_remaining=0.0, + time_elapsed=duration_from_seconds( + self._last_duration if completed else self._last_elapsed + ), + time_remaining=duration_from_seconds(0.0), ) diff --git a/dimos/control/tasks/velocity_profiler.py b/dimos/control/tasks/velocity_profiler.py index 00d481bb6de..51d00519e63 100644 --- a/dimos/control/tasks/velocity_profiler.py +++ b/dimos/control/tasks/velocity_profiler.py @@ -23,11 +23,10 @@ from __future__ import annotations +from dimos_generated.nav_msgs.msg import Path import numpy as np from numpy.typing import NDArray -from dimos.msgs.nav_msgs.Path import Path - class VelocityProfiler: """Compute an optimal speed profile along a path.""" @@ -60,7 +59,7 @@ def compute_profile(self, path: Path) -> NDArray[np.float64]: if len(path.poses) < 2: return np.array([self._min_speed]) - pts = np.array([[p.position.x, p.position.y] for p in path.poses]) + pts = np.array([[p.pose.position.x, p.pose.position.y] for p in path.poses]) curvatures = self._compute_curvatures(pts) max_speeds = self._curvature_speed_limits(curvatures) velocities = self._acceleration_pass(pts, max_speeds, forward=True) diff --git a/dimos/control/tasks/velocity_task/test_velocity_task.py b/dimos/control/tasks/velocity_task/test_velocity_task.py index 800595d0a6b..1a4fbaa9bdf 100644 --- a/dimos/control/tasks/velocity_task/test_velocity_task.py +++ b/dimos/control/tasks/velocity_task/test_velocity_task.py @@ -16,12 +16,13 @@ from __future__ import annotations +from dimos_generated.sensor_msgs.msg import JointState + from dimos.control.task import CoordinatorState, JointStateSnapshot from dimos.control.tasks.velocity_task.velocity_task import ( JointVelocityTask, JointVelocityTaskConfig, ) -from dimos.msgs.sensor_msgs.JointState import JointState def _task() -> JointVelocityTask: diff --git a/dimos/control/tasks/velocity_task/velocity_task.py b/dimos/control/tasks/velocity_task/velocity_task.py index e9538cd56c1..d88071be47d 100644 --- a/dimos/control/tasks/velocity_task/velocity_task.py +++ b/dimos/control/tasks/velocity_task/velocity_task.py @@ -38,7 +38,7 @@ from dimos.utils.logging_config import setup_logger if TYPE_CHECKING: - from dimos.msgs.sensor_msgs.JointState import JointState + from dimos_generated.sensor_msgs.msg import JointState logger = setup_logger() diff --git a/dimos/control/teleop_coordinator.py b/dimos/control/teleop_coordinator.py index af8baf2a694..1052d8fba20 100644 --- a/dimos/control/teleop_coordinator.py +++ b/dimos/control/teleop_coordinator.py @@ -14,12 +14,11 @@ """Control coordinator carrying spatial poses and keyboard end-effector twists.""" +from dimos_generated.geometry_msgs.msg import PoseStamped, TwistStamped +from dimos_generated.std_msgs.msg import Float32, UInt32 + from dimos.control.coordinator import ControlCoordinator from dimos.core.stream import In -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.geometry_msgs.TwistStamped import TwistStamped -from dimos.msgs.std_msgs.Float32 import Float32 -from dimos.teleop.webxr.controller_types import Buttons class TeleopControlCoordinator(ControlCoordinator): @@ -29,5 +28,5 @@ class TeleopControlCoordinator(ControlCoordinator): right_cartesian_command: In[PoseStamped] left_gripper_command: In[Float32] right_gripper_command: In[Float32] - teleop_buttons: In[Buttons] + teleop_buttons: In[UInt32] ee_twist_command: In[TwistStamped] diff --git a/dimos/control/test_control.py b/dimos/control/test_control.py index bff9528cf8f..2bda2c44147 100644 --- a/dimos/control/test_control.py +++ b/dimos/control/test_control.py @@ -23,6 +23,9 @@ from typing import Any from unittest.mock import MagicMock +from dimos_generated.geometry_msgs.msg import Twist, TwistStamped, Vector3 +from dimos_generated.sensor_msgs.msg import JointState +from dimos_generated.trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint import pytest from dimos.control._control_test_helpers import RecordingTask @@ -63,12 +66,8 @@ from dimos.hardware.manipulators.spec import ManipulatorAdapter from dimos.hardware.spec import JointLimits from dimos.hardware.whole_body.spec import MotorState, WholeBodyAdapter -from dimos.msgs.geometry_msgs.Twist import Twist -from dimos.msgs.geometry_msgs.TwistStamped import TwistStamped -from dimos.msgs.sensor_msgs.JointState import JointState -from dimos.msgs.trajectory_msgs.JointTrajectory import JointTrajectory -from dimos.msgs.trajectory_msgs.TrajectoryPoint import TrajectoryPoint -from dimos.msgs.trajectory_msgs.TrajectoryStatus import TrajectoryState +from dimos.msgs.time import duration_from_seconds, header_now, to_seconds +from dimos.msgs.trajectory import TrajectoryState @pytest.fixture @@ -115,17 +114,18 @@ def trajectory_task(): def simple_trajectory(): """Create a simple 2-point trajectory.""" return JointTrajectory( + header=header_now(), joint_names=["arm/joint1", "arm/joint2", "arm/joint3"], points=[ - TrajectoryPoint( + JointTrajectoryPoint( positions=[0.0, 0.0, 0.0], velocities=[0.0, 0.0, 0.0], - time_from_start=0.0, + time_from_start=duration_from_seconds(0.0), ), - TrajectoryPoint( + JointTrajectoryPoint( positions=[1.0, 0.5, 0.25], velocities=[0.0, 0.0, 0.0], - time_from_start=1.0, + time_from_start=duration_from_seconds(1.0), ), ], ) @@ -384,7 +384,7 @@ def test_map_twist_to_base_joints_routes_planar_twist_via_joint_command( dispatch = mocker.patch.object(coordinator, "_dispatch") coordinator._map_twist_to_base_joints( - Twist(linear=[1.0, 2.0, 0.0], angular=[0.0, 0.0, 3.0]) + Twist(linear=Vector3(x=1.0, y=2.0, z=0.0), angular=Vector3(x=0.0, y=0.0, z=3.0)) ) stream, joint_state = dispatch.call_args.args @@ -531,7 +531,11 @@ def test_removing_trajectory_task_allows_replacement(self, make_coordinator): def test_execute_and_cancel_without_trajectory_task_are_semantic(self, make_coordinator): coordinator = make_coordinator() - execute_result = coordinator.execute_trajectory(JointTrajectory()) + execute_result = coordinator.execute_trajectory( + JointTrajectory( + header=header_now(), + ) + ) cancel_result = coordinator.cancel_trajectory() assert execute_result.status is TrajectoryExecutionStatus.NO_TRAJECTORY_TASK @@ -624,21 +628,30 @@ def test_status_snapshot_is_non_destructive(self, trajectory_task, simple_trajec trajectory_task.compute(CoordinatorState(joints=MagicMock(), t_now=10.0, dt=0.01)) active = trajectory_task.get_status(10.25) - assert active.state is TrajectoryState.EXECUTING + assert active.state == TrajectoryState.EXECUTING assert active.progress == pytest.approx(0.25) trajectory_task.compute(CoordinatorState(joints=MagicMock(), t_now=11.5, dt=0.01)) terminal = trajectory_task.get_status(11.5) - assert terminal.state is TrajectoryState.COMPLETED + assert terminal.state == TrajectoryState.COMPLETED assert terminal.progress == pytest.approx(1.0) - assert trajectory_task.get_status(11.6).state is TrajectoryState.COMPLETED + assert trajectory_task.get_status(11.6).state == TrajectoryState.COMPLETED def test_execute_partial_subset_and_claims_full_configuration(self, trajectory_task): trajectory = JointTrajectory( + header=header_now(), joint_names=["arm/joint2", "arm/joint3"], points=[ - TrajectoryPoint(positions=[0.0, 0.0], velocities=[0.0, 0.0], time_from_start=0.0), - TrajectoryPoint(positions=[0.5, 1.0], velocities=[0.0, 0.0], time_from_start=1.0), + JointTrajectoryPoint( + positions=[0.0, 0.0], + velocities=[0.0, 0.0], + time_from_start=duration_from_seconds(0.0), + ), + JointTrajectoryPoint( + positions=[0.5, 1.0], + velocities=[0.0, 0.0], + time_from_start=duration_from_seconds(1.0), + ), ], ) @@ -684,41 +697,82 @@ def test_execute_accepts_start_position_at_tolerance(self, trajectory_task, simp "trajectory", [ JointTrajectory( + header=header_now(), joint_names=[], - points=[TrajectoryPoint(time_from_start=0.0, positions=[], velocities=[])], + points=[ + JointTrajectoryPoint( + time_from_start=duration_from_seconds(0.0), positions=[], velocities=[] + ) + ], ), JointTrajectory( + header=header_now(), joint_names=["arm/joint1", "arm/joint1"], points=[ - TrajectoryPoint( - time_from_start=0.0, positions=[0.0, 0.0], velocities=[0.0, 0.0] + JointTrajectoryPoint( + time_from_start=duration_from_seconds(0.0), + positions=[0.0, 0.0], + velocities=[0.0, 0.0], ) ], ), JointTrajectory( + header=header_now(), joint_names=["arm/missing"], - points=[TrajectoryPoint(time_from_start=0.0, positions=[0.0], velocities=[0.0])], + points=[ + JointTrajectoryPoint( + time_from_start=duration_from_seconds(0.0), + positions=[0.0], + velocities=[0.0], + ) + ], ), - JointTrajectory(joint_names=["arm/joint1"], points=[]), + JointTrajectory(header=header_now(), joint_names=["arm/joint1"], points=[]), JointTrajectory( + header=header_now(), joint_names=["arm/joint1"], - points=[TrajectoryPoint(time_from_start=0.0, positions=[], velocities=[0.0])], + points=[ + JointTrajectoryPoint( + time_from_start=duration_from_seconds(0.0), positions=[], velocities=[0.0] + ) + ], ), JointTrajectory( + header=header_now(), joint_names=["arm/joint1"], points=[ - TrajectoryPoint(time_from_start=0.0, positions=[float("nan")], velocities=[0.0]) + JointTrajectoryPoint( + time_from_start=duration_from_seconds(0.0), + positions=[float("nan")], + velocities=[0.0], + ) ], ), JointTrajectory( + header=header_now(), joint_names=["arm/joint1"], - points=[TrajectoryPoint(time_from_start=0.1, positions=[0.0], velocities=[0.0])], + points=[ + JointTrajectoryPoint( + time_from_start=duration_from_seconds(0.1), + positions=[0.0], + velocities=[0.0], + ) + ], ), JointTrajectory( + header=header_now(), joint_names=["arm/joint1"], points=[ - TrajectoryPoint(time_from_start=0.0, positions=[0.0], velocities=[0.0]), - TrajectoryPoint(time_from_start=0.0, positions=[1.0], velocities=[0.0]), + JointTrajectoryPoint( + time_from_start=duration_from_seconds(0.0), + positions=[0.0], + velocities=[0.0], + ), + JointTrajectoryPoint( + time_from_start=duration_from_seconds(0.0), + positions=[1.0], + velocities=[0.0], + ), ], ), ], @@ -737,10 +791,15 @@ def test_invalid_partial_inputs_reject_before_state_changes(self, trajectory_tas def test_compute_emits_active_subset_and_retains_final_target(self, trajectory_task): trajectory = JointTrajectory( + header=header_now(), joint_names=["arm/joint2"], points=[ - TrajectoryPoint(positions=[0.0], velocities=[0.0], time_from_start=0.0), - TrajectoryPoint(positions=[1.0], velocities=[0.0], time_from_start=1.0), + JointTrajectoryPoint( + positions=[0.0], velocities=[0.0], time_from_start=duration_from_seconds(0.0) + ), + JointTrajectoryPoint( + positions=[1.0], velocities=[0.0], time_from_start=duration_from_seconds(1.0) + ), ], ) assert ( @@ -764,17 +823,27 @@ def test_compute_emits_active_subset_and_retains_final_target(self, trajectory_t def test_replacement_reset_and_cancel_clear_active_subset(self, trajectory_task): first = JointTrajectory( + header=header_now(), joint_names=["arm/joint1"], points=[ - TrajectoryPoint(positions=[0.0], velocities=[0.0], time_from_start=0.0), - TrajectoryPoint(positions=[1.0], velocities=[0.0], time_from_start=1.0), + JointTrajectoryPoint( + positions=[0.0], velocities=[0.0], time_from_start=duration_from_seconds(0.0) + ), + JointTrajectoryPoint( + positions=[1.0], velocities=[0.0], time_from_start=duration_from_seconds(1.0) + ), ], ) second = JointTrajectory( + header=header_now(), joint_names=["arm/joint3"], points=[ - TrajectoryPoint(positions=[2.0], velocities=[0.0], time_from_start=0.0), - TrajectoryPoint(positions=[3.0], velocities=[0.0], time_from_start=1.0), + JointTrajectoryPoint( + positions=[2.0], velocities=[0.0], time_from_start=duration_from_seconds(0.0) + ), + JointTrajectoryPoint( + positions=[3.0], velocities=[0.0], time_from_start=duration_from_seconds(1.0) + ), ], ) assert ( @@ -858,8 +927,9 @@ def test_one_point_stream_target_is_velocity_bounded(self): ) ) target = JointTrajectory( + header=header_now(), joint_names=["arm/joint1"], - points=[TrajectoryPoint(positions=[1.0])], + points=[JointTrajectoryPoint(positions=[1.0])], ) state = JointStateSnapshot(joint_positions={"arm/joint1": 0.0}) @@ -901,16 +971,19 @@ def test_replacement_anchors_at_bounded_command_and_preserves_other_joints(self) ) state = JointStateSnapshot(joint_positions={"arm/joint1": 0.0, "arm/joint2": 0.0}) first = JointTrajectory( + header=header_now(), joint_names=["arm/joint1"], - points=[TrajectoryPoint(positions=[1.0])], + points=[JointTrajectoryPoint(positions=[1.0])], ) other = JointTrajectory( + header=header_now(), joint_names=["arm/joint2"], - points=[TrajectoryPoint(positions=[-1.0])], + points=[JointTrajectoryPoint(positions=[-1.0])], ) replacement = JointTrajectory( + header=header_now(), joint_names=["arm/joint1"], - points=[TrajectoryPoint(positions=[-1.0])], + points=[JointTrajectoryPoint(positions=[-1.0])], ) task.execute(first, {}) @@ -933,21 +1006,25 @@ def test_partial_replacement_status_includes_untouched_joint_run(self): ) state = JointStateSnapshot(joint_positions={"arm/joint1": 0.0, "arm/joint2": 0.0}) long_trajectory = JointTrajectory( + header=header_now(), joint_names=["arm/joint1", "arm/joint2"], points=[ - TrajectoryPoint(positions=[0.0, 0.0], velocities=[0.0, 0.0]), - TrajectoryPoint( + JointTrajectoryPoint(positions=[0.0, 0.0], velocities=[0.0, 0.0]), + JointTrajectoryPoint( positions=[10.0, 10.0], velocities=[0.0, 0.0], - time_from_start=10.0, + time_from_start=duration_from_seconds(10.0), ), ], ) replacement = JointTrajectory( + header=header_now(), joint_names=["arm/joint1"], points=[ - TrajectoryPoint(positions=[2.5], velocities=[0.0]), - TrajectoryPoint(positions=[3.5], velocities=[0.0], time_from_start=1.0), + JointTrajectoryPoint(positions=[2.5], velocities=[0.0]), + JointTrajectoryPoint( + positions=[3.5], velocities=[0.0], time_from_start=duration_from_seconds(1.0) + ), ], ) @@ -963,10 +1040,10 @@ def test_partial_replacement_status_includes_untouched_joint_run(self): status = task.get_status(13.5) - assert status.state is TrajectoryState.EXECUTING + assert status.state == TrajectoryState.EXECUTING assert status.progress == pytest.approx(0.35) - assert status.time_elapsed == pytest.approx(3.5) - assert status.time_remaining == pytest.approx(6.5) + assert to_seconds(status.time_elapsed) == pytest.approx(3.5) + assert to_seconds(status.time_remaining) == pytest.approx(6.5) def test_cancel_trajectory(self, trajectory_task, simple_trajectory): trajectory_task.execute(simple_trajectory, trajectory_start_positions(simple_trajectory)) @@ -1191,10 +1268,19 @@ def test_partial_trajectory_and_gripper_command_share_hardware_write(self, mocke limits=[(0.0, 1.0)], ) trajectory = JointTrajectory( + header=header_now(), joint_names=["arm/joint1", "arm/joint2"], points=[ - TrajectoryPoint(positions=[0.0, 0.0], velocities=[0.0, 0.0], time_from_start=0.0), - TrajectoryPoint(positions=[0.5, 0.5], velocities=[0.0, 0.0], time_from_start=1.0), + JointTrajectoryPoint( + positions=[0.0, 0.0], + velocities=[0.0, 0.0], + time_from_start=duration_from_seconds(0.0), + ), + JointTrajectoryPoint( + positions=[0.5, 0.5], + velocities=[0.0, 0.0], + time_from_start=duration_from_seconds(1.0), + ), ], ) tick_loop = TickLoop( @@ -1332,17 +1418,18 @@ def test_full_trajectory_execution(self, mock_adapter, wait_until): ) trajectory = JointTrajectory( + header=header_now(), joint_names=[f"arm/joint{i + 1}" for i in range(6)], points=[ - TrajectoryPoint( + JointTrajectoryPoint( positions=[0.0] * 6, velocities=[0.0] * 6, - time_from_start=0.0, + time_from_start=duration_from_seconds(0.0), ), - TrajectoryPoint( + JointTrajectoryPoint( positions=[0.5] * 6, velocities=[0.0] * 6, - time_from_start=0.5, + time_from_start=duration_from_seconds(0.5), ), ], ) diff --git a/dimos/control/test_coordinator_commands.py b/dimos/control/test_coordinator_commands.py index edae7d196f7..ec21eee326b 100644 --- a/dimos/control/test_coordinator_commands.py +++ b/dimos/control/test_coordinator_commands.py @@ -31,6 +31,7 @@ from collections.abc import Iterator from typing import Any +from dimos_generated.trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint import pytest import dimos.control.coordinator as coord_mod @@ -41,8 +42,7 @@ JointCommandOutput, ResourceClaim, ) -from dimos.msgs.trajectory_msgs.JointTrajectory import JointTrajectory -from dimos.msgs.trajectory_msgs.TrajectoryPoint import TrajectoryPoint +from dimos.msgs.time import duration_from_seconds, header_now ARM_JOINTS = frozenset({"arm/joint1", "arm/joint2"}) @@ -126,10 +126,11 @@ def record_time(self, t_now: float | None = None) -> float | None: def _trajectory() -> JointTrajectory: return JointTrajectory( + header=header_now(), joint_names=["arm/joint1", "arm/joint2"], points=[ - TrajectoryPoint(time_from_start=0.0, positions=[0.0, 0.0]), - TrajectoryPoint(time_from_start=1.0, positions=[0.1, 0.2]), + JointTrajectoryPoint(time_from_start=duration_from_seconds(0.0), positions=[0.0, 0.0]), + JointTrajectoryPoint(time_from_start=duration_from_seconds(1.0), positions=[0.1, 0.2]), ], ) diff --git a/dimos/control/test_coordinator_routing.py b/dimos/control/test_coordinator_routing.py index fd9eb830559..a2f7e9d6eda 100644 --- a/dimos/control/test_coordinator_routing.py +++ b/dimos/control/test_coordinator_routing.py @@ -28,6 +28,9 @@ import threading from typing import Any +from dimos_generated.geometry_msgs.msg import Pose, PoseStamped, Twist, TwistStamped, Vector3 +from dimos_generated.sensor_msgs.msg import JointState +from dimos_generated.std_msgs.msg import Header import pytest from dimos.control._control_test_helpers import RecordingTask @@ -47,10 +50,6 @@ from dimos.control.teleop_coordinator import TeleopControlCoordinator from dimos.core.stream import In from dimos.hardware.drive_trains.registry import twist_base_adapter_registry -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.geometry_msgs.Twist import Twist -from dimos.msgs.geometry_msgs.TwistStamped import TwistStamped -from dimos.msgs.sensor_msgs.JointState import JointState from dimos.teleop.webxr.controller_types import Buttons ARM_JOINTS = ["arm/joint1", "arm/joint2"] @@ -255,14 +254,14 @@ def test_dual_arm_ports_isolate_left_from_right(self, make_coordinator): ) coordinator.start() - taps["left_cartesian"].emit(PoseStamped()) + taps["left_cartesian"].emit(PoseStamped(header=Header(frame_id=""), pose=Pose())) left = coordinator.get_task("cartesian_left") right = coordinator.get_task("cartesian_right") assert len(left.cartesian_calls) == 1 assert right.cartesian_calls == [] - taps["right_cartesian"].emit(PoseStamped()) + taps["right_cartesian"].emit(PoseStamped(header=Header(frame_id=""), pose=Pose())) assert len(left.cartesian_calls) == 1 assert len(right.cartesian_calls) == 1 @@ -275,7 +274,7 @@ def test_cartesian_name_match_needs_no_stream_bind(self, make_coordinator): ) coordinator.start() - taps["cartesian_command"].emit(PoseStamped()) + taps["cartesian_command"].emit(PoseStamped(header=Header(frame_id=""), pose=Pose())) calls = coordinator.get_task("cart").cartesian_calls assert len(calls) == 1 @@ -293,12 +292,14 @@ def test_frame_id_is_not_consulted(self, make_coordinator, mocker): coordinator.start() warn = mocker.patch.object(coord_mod.logger, "warning") - taps["cartesian_command"].emit(PoseStamped(frame_id="some_other_task")) - taps["cartesian_command"].emit(PoseStamped(frame_id="")) + taps["cartesian_command"].emit( + PoseStamped(header=Header(frame_id="some_other_task"), pose=Pose()) + ) + taps["cartesian_command"].emit(PoseStamped(header=Header(frame_id=""), pose=Pose())) calls = coordinator.get_task("cart").cartesian_calls assert len(calls) == 2 - assert calls[0][0].frame_id == "some_other_task" + assert calls[0][0].header.frame_id == "some_other_task" assert not warn.called @@ -311,7 +312,7 @@ def test_buttons_reach_teleop_task(self, make_coordinator): ) coordinator.start() - taps["teleop_buttons"].emit(Buttons()) + taps["teleop_buttons"].emit(Buttons().to_message()) assert len(coordinator.get_task("teleop1").buttons_calls) == 1 @@ -341,7 +342,9 @@ def test_base_twist_maps_to_virtual_joint_velocities(self, make_coordinator): ) coordinator.start() - taps["twist_command"].emit(Twist(linear=[1.0, 2.0, 0.0], angular=[0.0, 0.0, 3.0])) + taps["twist_command"].emit( + Twist(linear=Vector3(x=1.0, y=2.0, z=0.0), angular=Vector3(x=0.0, y=0.0, z=3.0)) + ) assert coordinator.get_task("basevel")._velocities == [1.0, 2.0, 3.0] @@ -360,7 +363,9 @@ def test_base_twist_both_maps_joints_and_fans_out(self, make_coordinator): coordinator.add_task(capable, task_type="g1_groot_wbc") coordinator.start() - taps["twist_command"].emit(Twist(linear=[1.0, 2.0, 0.0], angular=[0.0, 0.0, 3.0])) + taps["twist_command"].emit( + Twist(linear=Vector3(x=1.0, y=2.0, z=0.0), angular=Vector3(x=0.0, y=0.0, z=3.0)) + ) assert coordinator.get_task("basevel")._velocities == [1.0, 2.0, 3.0] assert len(capable.velocity_commands) == 1 @@ -400,7 +405,9 @@ def test_base_suffix_subset_maps_only_declared_joints(self, make_coordinator): ) coordinator.start() - taps["twist_command"].emit(Twist(linear=[1.0, 2.0, 0.0], angular=[0.0, 0.0, 3.0])) + taps["twist_command"].emit( + Twist(linear=Vector3(x=1.0, y=2.0, z=0.0), angular=Vector3(x=0.0, y=0.0, z=3.0)) + ) assert coordinator.get_task("basevel")._velocities == [1.0, 3.0] @@ -420,7 +427,9 @@ def test_base_full_6d_twist_maps_all_axes(self, make_coordinator): ) coordinator.start() - taps["twist_command"].emit(Twist(linear=[1.0, 2.0, 3.0], angular=[4.0, 5.0, 6.0])) + taps["twist_command"].emit( + Twist(linear=Vector3(x=1.0, y=2.0, z=3.0), angular=Vector3(x=4.0, y=5.0, z=6.0)) + ) assert coordinator.get_task("basevel")._velocities == [1.0, 2.0, 3.0, 4.0, 5.0, 6.0] @@ -447,7 +456,9 @@ def test_twist_mapping_ignores_non_base_hardware(self, make_coordinator): ) coordinator.start() - taps["twist_command"].emit(Twist(linear=[1.0, 2.0, 0.0], angular=[0.0, 0.0, 3.0])) + taps["twist_command"].emit( + Twist(linear=Vector3(x=1.0, y=2.0, z=0.0), angular=Vector3(x=0.0, y=0.0, z=3.0)) + ) assert coordinator.get_task("basevel")._velocities == [1.0, 2.0, 3.0] assert coordinator.get_task("armvel")._velocities is None @@ -459,7 +470,9 @@ def test_twist_reaches_declaring_velocity_task_without_base(self, make_coordinat coordinator.start() assert taps["twist_command"].subscribed - taps["twist_command"].emit(Twist(linear=[1.0, 2.0, 0.0], angular=[0.0, 0.0, 3.0])) + taps["twist_command"].emit( + Twist(linear=Vector3(x=1.0, y=2.0, z=0.0), angular=Vector3(x=0.0, y=0.0, z=3.0)) + ) assert len(task.velocity_commands) == 1 vx, vy, wz, t_now = task.velocity_commands[0] @@ -476,7 +489,7 @@ def test_card_routed_twist_delivers_raw_msg_to_on_twist_command(self, make_coord coordinator.add_task(task, task_type="g1_groot_wbc") coordinator.start() - msg = Twist(linear=[1.0, 2.0, 0.0], angular=[0.0, 0.0, 3.0]) + msg = Twist(linear=Vector3(x=1.0, y=2.0, z=0.0), angular=Vector3(x=0.0, y=0.0, z=3.0)) taps["twist_command"].emit(msg) assert len(task.twist_msgs) == 1 @@ -490,7 +503,9 @@ def test_bare_set_velocity_command_stub_gets_nothing(self, make_coordinator): coordinator.start() assert taps["twist_command"].subscribed # the base keeps the stream alive - taps["twist_command"].emit(Twist(linear=[1.0, 2.0, 0.0], angular=[0.0, 0.0, 3.0])) + taps["twist_command"].emit( + Twist(linear=Vector3(x=1.0, y=2.0, z=0.0), angular=Vector3(x=0.0, y=0.0, z=3.0)) + ) assert bare.velocity_commands == [] @@ -515,7 +530,7 @@ def test_twist_delivery_concurrent_with_remove_hardware(self, make_coordinator): coordinator.start() assert taps["twist_command"].subscribed - msg = Twist(linear=[1.0, 0.0, 0.0], angular=[0.0, 0.0, 0.5]) + msg = Twist(linear=Vector3(x=1.0, y=0.0, z=0.0), angular=Vector3(x=0.0, y=0.0, z=0.5)) stop = threading.Event() def pump() -> None: @@ -653,7 +668,7 @@ def test_buttons_skip_card_less_tasks(self, make_coordinator): coordinator.add_task(cardless) coordinator.start() - taps["teleop_buttons"].emit(Buttons()) + taps["teleop_buttons"].emit(Buttons().to_message()) assert len(coordinator.get_task("teleop1").buttons_calls) == 1 assert cardless.buttons_calls == [] diff --git a/dimos/control/test_path_following_coordinator.py b/dimos/control/test_path_following_coordinator.py index 34962572cb8..a2bf5112858 100644 --- a/dimos/control/test_path_following_coordinator.py +++ b/dimos/control/test_path_following_coordinator.py @@ -19,6 +19,7 @@ from typing import Any +from dimos_generated.std_msgs.msg import Float32 import pytest from dimos.control.benchmarking.paths import straight_rotate @@ -27,7 +28,6 @@ from dimos.control.path_following_coordinator import PathFollowingCoordinator from dimos.control.tasks.registry import control_task_registry from dimos.control.tick_loop import TickLoop -from dimos.msgs.std_msgs.Float32 import Float32 JOINTS = make_twist_base_joints("go2") diff --git a/dimos/control/test_per_robot_joint_states.py b/dimos/control/test_per_robot_joint_states.py index bfec8d9f6f7..ca68cc8de15 100644 --- a/dimos/control/test_per_robot_joint_states.py +++ b/dimos/control/test_per_robot_joint_states.py @@ -21,6 +21,7 @@ from typing import Any from unittest.mock import MagicMock +from dimos_generated.sensor_msgs.msg import JointState import pytest from dimos.control.components import HardwareComponent, HardwareType, make_joints @@ -31,7 +32,6 @@ from dimos.core.stream import In, Out from dimos.hardware.manipulators.registry import adapter_registry from dimos.hardware.manipulators.spec import ManipulatorAdapter -from dimos.msgs.sensor_msgs.JointState import JointState LEFT_JOINTS = make_joints("left_arm", 2) RIGHT_JOINTS = make_joints("right_arm", 3) @@ -188,12 +188,12 @@ def test_each_port_gets_only_its_own_robot(self, make_coordinator, wait_until): left_msg = left.latest() assert list(left_msg.name) == LEFT_JOINTS - assert left_msg.frame_id == "left_arm" + assert left_msg.header.frame_id == "left_arm" assert list(left_msg.position) == LEFT_POSITIONS right_msg = right.latest() assert list(right_msg.name) == RIGHT_JOINTS - assert right_msg.frame_id == "right_arm" + assert right_msg.header.frame_id == "right_arm" assert list(right_msg.position) == RIGHT_POSITIONS assert set(merged.latest().name) == {*LEFT_JOINTS, *RIGHT_JOINTS} @@ -255,16 +255,16 @@ def test_message_carries_only_that_robots_values(self): assert list(left_msg.position) == [0.1, 0.2] assert list(left_msg.velocity) == [1.1, 1.2] assert list(left_msg.effort) == [2.1, 2.2] - assert left_msg.frame_id == "left_arm" + assert left_msg.header.frame_id == "left_arm" right_msg = by_id["right_arm"] assert list(right_msg.position) == [0.3, 0.4, 0.5] assert list(right_msg.velocity) == [1.3, 1.4, 1.5] assert list(right_msg.effort) == [2.3, 2.4, 2.5] - assert right_msg.frame_id == "right_arm" + assert right_msg.header.frame_id == "right_arm" - assert left_msg.ts == merged[0].ts - assert right_msg.ts == merged[0].ts + assert left_msg.header.stamp == merged[0].header.stamp + assert right_msg.header.stamp == merged[0].header.stamp def test_hardware_is_read_once_per_tick(self): published: list[tuple[str, JointState]] = [] diff --git a/dimos/control/tick_loop.py b/dimos/control/tick_loop.py index 668186922d9..d335b4b99e1 100644 --- a/dimos/control/tick_loop.py +++ b/dimos/control/tick_loop.py @@ -31,6 +31,9 @@ import time from typing import TYPE_CHECKING, NamedTuple +from dimos_generated.sensor_msgs.msg import JointState +from dimos_generated.std_msgs.msg import Header + from dimos.constants import DEFAULT_THREAD_JOIN_TIMEOUT from dimos.control.task import ( ControlTask, @@ -39,7 +42,7 @@ JointStateSnapshot, ResourceClaim, ) -from dimos.msgs.sensor_msgs.JointState import JointState +from dimos.msgs.time import time_from_seconds from dimos.utils.logging_config import setup_logger if TYPE_CHECKING: @@ -428,8 +431,7 @@ def _publish_joint_state(self, snapshot: JointStateSnapshot) -> None: """Publish aggregated JointState for external consumers.""" names = list(snapshot.joint_positions.keys()) msg = JointState( - ts=snapshot.timestamp, - frame_id=self._frame_id, + header=Header(stamp=time_from_seconds(snapshot.timestamp), frame_id=self._frame_id), name=names, position=[snapshot.joint_positions[n] for n in names], velocity=[snapshot.joint_velocities.get(n, 0.0) for n in names], @@ -450,8 +452,7 @@ def _publish_robot_joint_states( for hw_id, state in per_hardware.items(): names = list(state.keys()) msg = JointState( - ts=timestamp, - frame_id=hw_id, + header=Header(stamp=time_from_seconds(timestamp), frame_id=hw_id), name=names, position=[state[n].position for n in names], velocity=[state[n].velocity for n in names], diff --git a/dimos/core/coordination/module_coordinator.py b/dimos/core/coordination/module_coordinator.py index a4d6941b092..051c57c87bf 100644 --- a/dimos/core/coordination/module_coordinator.py +++ b/dimos/core/coordination/module_coordinator.py @@ -711,7 +711,7 @@ def _coerce_transport_to_backend(transport: Transport[Any]) -> Transport[Any]: if is_pickled: raw, msg_type = transport.topic, None else: - raw, msg_type = transport.topic.topic, transport.topic.lcm_type + raw, msg_type = transport.topic.topic, transport.topic.msg_type # Strip the Zenoh 'dimos/' namespace (if present) back to the logical name. # The factory re-applies the right prefix for the target backend. logical = raw[len("dimos/") :] if raw.startswith("dimos/") else raw diff --git a/dimos/core/coordination/test_module_coordinator.py b/dimos/core/coordination/test_module_coordinator.py index 8cc222147b9..64cdef9490e 100644 --- a/dimos/core/coordination/test_module_coordinator.py +++ b/dimos/core/coordination/test_module_coordinator.py @@ -18,6 +18,10 @@ from types import MappingProxyType from typing import Any, Protocol +from dimos_generated.geometry_msgs.msg import TransformStamped +from dimos_generated.sensor_msgs.msg import Image +from dimos_generated.std_msgs.msg import Header +from dimos_generated.tf2_msgs.msg import TFMessage from pydantic import BaseModel import pytest @@ -48,9 +52,6 @@ from dimos.core.stream import IO, In, Out, Stream from dimos.core.transport import CloudflareTransport, PubSubTransport from dimos.core.transport_factory import transport_topic -from dimos.msgs.geometry_msgs.Transform import Transform -from dimos.msgs.sensor_msgs.Image import Image -from dimos.msgs.tf2_msgs.TFMessage import TFMessage import dimos.robot.get_all_blueprints as resolver from dimos.spec.utils import Spec @@ -1122,7 +1123,11 @@ class IoTfPublisher(Module): @rpc def send(self, child: str) -> None: - self.tf.publish(TFMessage(Transform(frame_id="world", child_frame_id=child))) + self.tf.publish( + TFMessage( + transforms=[TransformStamped(header=Header(frame_id="world"), child_frame_id=child)] + ) + ) class IoTfEcho(Module): @@ -1140,7 +1145,11 @@ async def handle_tf(self, msg: TFMessage) -> None: @rpc def send(self, child: str) -> None: - self.tf.publish(TFMessage(Transform(frame_id="world", child_frame_id=child))) + self.tf.publish( + TFMessage( + transforms=[TransformStamped(header=Header(frame_id="world"), child_frame_id=child)] + ) + ) @rpc def seen(self) -> list[str]: diff --git a/dimos/core/coordination/test_worker.py b/dimos/core/coordination/test_worker.py index be05102be47..f9f2eaa080f 100644 --- a/dimos/core/coordination/test_worker.py +++ b/dimos/core/coordination/test_worker.py @@ -15,6 +15,7 @@ import threading from typing import TYPE_CHECKING +from dimos_generated.geometry_msgs.msg import Vector3 import pytest from dimos.core.coordination.worker_manager_python import WorkerManagerPython @@ -22,7 +23,6 @@ from dimos.core.global_config import GlobalConfig, global_config from dimos.core.module import Module from dimos.core.stream import In, Out -from dimos.msgs.geometry_msgs.Vector3 import Vector3 if TYPE_CHECKING: from dimos.core.resource_monitor.stats import WorkerStats diff --git a/dimos/core/test_baked_host.py b/dimos/core/test_baked_host.py index 119cb1b4045..ec8eb8379ae 100644 --- a/dimos/core/test_baked_host.py +++ b/dimos/core/test_baked_host.py @@ -20,13 +20,13 @@ import pickle from typing import get_args, get_origin, get_type_hints +from dimos_generated.geometry_msgs.msg import PoseStamped +from dimos_generated.sensor_msgs.msg import PointCloud2 import pytest from dimos.core.baked_host import BakedHost, baked_host from dimos.core.native_module import NativeModule, NativeModuleConfig from dimos.core.stream import IO, In, Out -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 class MapperConfig(NativeModuleConfig): diff --git a/dimos/core/test_cdr_transport.py b/dimos/core/test_cdr_transport.py new file mode 100644 index 00000000000..ed175046001 --- /dev/null +++ b/dimos/core/test_cdr_transport.py @@ -0,0 +1,104 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Generated CDR crosses actual transport implementations, including large images.""" + +from contextlib import ExitStack +import threading +import uuid + +from dimos_generated.geometry_msgs.msg import Point +from dimos_generated.sensor_msgs.msg import Image +import numpy as np +import pytest + +from dimos.core.transport import LCMTransport, SHMTransport, ZenohTransport +from dimos.protocol.service.zenohservice import ZenohSessionPool + + +@pytest.fixture(params=["lcm", "zenoh", "shm"]) +def transport_pair(request, lcm_url): + with ExitStack() as stack: + if request.param == "zenoh": + pool = ZenohSessionPool() + stack.callback(pool.close_all) + + def make(message_type): + topic = f"dimos/cdr/{uuid.uuid4().hex[:8]}" + transports = [] + for _ in range(2): + if request.param == "lcm": + transport = LCMTransport(topic, message_type, url=lcm_url) + elif request.param == "zenoh": + transport = ZenohTransport( + topic, + message_type, + session_pool=pool, + scouting=False, + multicast=False, + gossip=False, + connect=[], + ) + else: + transport = SHMTransport(topic, message_type, prefer="cpu") + stack.callback(transport.stop) + transport.start() + transports.append(transport) + return transports + + yield make + + +def test_generated_message_round_trip(transport_pair, retry_until): + publisher, subscriber = transport_pair(Point) + received = [] + ready = threading.Event() + + def on_message(message): + received.append((message.x, message.y, message.z)) + ready.set() + + subscriber.subscribe(on_message) + retry_until(ready, lambda: publisher.broadcast(None, Point(x=1.25, y=-2.5, z=3))) + + assert received[0] == (1.25, -2.5, 3.0) + + +def test_large_generated_image_preserves_pixels_and_header(transport_pair, retry_until): + publisher, subscriber = transport_pair(Image) + pixels = np.arange(640 * 480 * 3, dtype=np.uint8) + message = Image(width=640, height=480, encoding="rgb8", step=1920, data=pixels) + message.header.frame_id = "camera" + message.header.stamp.sec = 1_700_000_000 + message.header.stamp.nanosec = 123_456_789 + received = [] + ready = threading.Event() + + def on_message(value): + received.append(value) + ready.set() + + subscriber.subscribe(on_message) + retry_until(ready, lambda: publisher.broadcast(None, message), timeout=5) + + decoded = received[0] + assert (decoded.width, decoded.height, decoded.step, decoded.encoding) == ( + 640, + 480, + 1920, + "rgb8", + ) + assert decoded.header.frame_id == "camera" + assert (decoded.header.stamp.sec, decoded.header.stamp.nanosec) == (1_700_000_000, 123_456_789) + np.testing.assert_array_equal(decoded.data.view(), pixels) diff --git a/dimos/core/test_core.py b/dimos/core/test_core.py index 35ee597b915..ca91865689d 100644 --- a/dimos/core/test_core.py +++ b/dimos/core/test_core.py @@ -14,6 +14,8 @@ import time +from dimos_generated.geometry_msgs.msg import PoseStamped, Vector3 +from dimos_generated.sensor_msgs.msg import PointCloud2 import pytest from reactivex.disposable import Disposable @@ -22,16 +24,14 @@ from dimos.core.stream import In, Out from dimos.core.testing import MockRobotClient from dimos.core.transport import LCMTransport, pLCMTransport -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 -from dimos.robot.unitree.type.odometry import Odometry +from dimos.types.timestamped import TimestampedData class Navigation(Module): mov: Out[Vector3] lidar: In[PointCloud2] target_position: In[Vector3] - odometry: In[Odometry] + odometry: In[TimestampedData[PoseStamped]] odom_msg_count = 0 lidar_msg_count = 0 @@ -43,8 +43,9 @@ def navigate_to(self, target: Vector3) -> bool: ... def start(self) -> None: def _odom(msg) -> None: self.odom_msg_count += 1 - print("RCV:", (time.perf_counter() - msg.pubtime) * 1000, msg) - self.mov.publish(msg.position) + print("RCV:", (time.perf_counter() - msg.ts) * 1000, msg) + position = msg.value.pose.position + self.mov.publish(Vector3(x=position.x, y=position.y, z=position.z)) unsub = self.odometry.subscribe(_odom) self.register_disposable(Disposable(unsub)) @@ -106,11 +107,11 @@ def test_basic_deployment(dimos) -> None: robot.lidar.transport = LCMTransport("/lidar", PointCloud2) # odometry & mov using just a pickle over LCM - robot.odometry.transport = pLCMTransport("/odom") + robot.timed_odometry.transport = pLCMTransport("/odom") nav.mov.transport = pLCMTransport("/mov") nav.lidar.connect(robot.lidar) - nav.odometry.connect(robot.odometry) + nav.odometry.connect(robot.timed_odometry) robot.mov.connect(nav.mov) robot.start() diff --git a/dimos/core/test_native_module.py b/dimos/core/test_native_module.py index 8f060f691d6..9325ae7136b 100644 --- a/dimos/core/test_native_module.py +++ b/dimos/core/test_native_module.py @@ -27,6 +27,9 @@ from typing import Any from unittest.mock import Mock, patch +from dimos_generated.geometry_msgs.msg import Twist +from dimos_generated.sensor_msgs.msg import Imu, PointCloud2 +from dimos_generated.tf2_msgs.msg import TFMessage from pydantic import ValidationError import pytest @@ -40,10 +43,6 @@ from dimos.core.stream import IO, In, Out from dimos.core.transport import LCMTransport, ZenohTransport from dimos.core.transport_factory import make_transport, rpc_backend, transport_topic -from dimos.msgs.geometry_msgs.Twist import Twist -from dimos.msgs.sensor_msgs.Imu import Imu -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 -from dimos.msgs.tf2_msgs.TFMessage import TFMessage from dimos.protocol.pubsub.impl.zenohpubsub import QOS_NEVER_DROP, Topic as ZenohTopic from dimos.protocol.service import zenohservice from dimos.protocol.service.zenohservice import ZenohConfig @@ -172,8 +171,8 @@ def test_manual(dimos_cluster: ModuleCoordinator, args_file: str) -> None: native_module.stop() assert read_json_file(args_file) == { - "cmd_vel": "/cmd_vel#geometry_msgs.Twist", - "pointcloud": "/my/custom/lidar#sensor_msgs.PointCloud2", + "cmd_vel": "/cmd_vel#geometry_msgs/msg/Twist", + "pointcloud": "/my/custom/lidar#sensor_msgs/msg/PointCloud2", "output_file": args_file, "some_param": "2.5", } @@ -188,8 +187,8 @@ def test_io_port_topic_reaches_the_native_process() -> None: module.set_transport("tf", transports[1]) assert module._collect_topics() == { - "cmd_vel": "/cmd_vel#geometry_msgs.Twist", - "tf": "/tf#tf2_msgs.TFMessage", + "cmd_vel": "/cmd_vel#geometry_msgs/msg/Twist", + "tf": "/tf#tf2_msgs/msg/TFMessage", } finally: module.stop() @@ -205,7 +204,7 @@ def test_tf_topic_comes_from_the_declared_port_only() -> None: try: module.set_transport("cmd_vel", transport) - assert module._collect_topics() == {"cmd_vel": "/cmd_vel#geometry_msgs.Twist"} + assert module._collect_topics() == {"cmd_vel": "/cmd_vel#geometry_msgs/msg/Twist"} finally: module.stop() with contextlib.suppress(Exception): diff --git a/dimos/core/test_stream.py b/dimos/core/test_stream.py index 3f39f78e92d..9029eaaa16c 100644 --- a/dimos/core/test_stream.py +++ b/dimos/core/test_stream.py @@ -17,6 +17,8 @@ import time from typing import Any +from dimos_generated.geometry_msgs.msg import PoseStamped +from dimos_generated.sensor_msgs.msg import PointCloud2 import pytest from dimos.core.core import rpc @@ -24,13 +26,11 @@ from dimos.core.stream import In from dimos.core.testing import MockRobotClient from dimos.core.transport import LCMTransport, pLCMTransport -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 -from dimos.robot.unitree.type.odometry import Odometry class SubscriberBase(Module): - sub1_msgs: list[Odometry] - sub2_msgs: list[Odometry] + sub1_msgs: list[PoseStamped] + sub2_msgs: list[PoseStamped] def __init__(self, **kwargs: Any) -> None: self.sub1_msgs = [] @@ -75,7 +75,7 @@ def sub2_msgs_len(self) -> int: class ClassicSubscriber(SubscriberBase): - odom: In[Odometry] + odom: In[PoseStamped] unsub: Callable[[], None] | None = None unsub2: Callable[[], None] | None = None @@ -98,7 +98,7 @@ def unsub_all(self) -> None: class RXPYSubscriber(SubscriberBase): - odom: In[Odometry] + odom: In[PoseStamped] unsub: Callable[[], None] | None = None unsub2: Callable[[], None] | None = None @@ -142,7 +142,7 @@ class SpyLCMTransport(LCMTransport): active_subscribers: int = 0 def __reduce__(self): - return (SpyLCMTransport, (self.topic.topic, self.topic.lcm_type)) + return (SpyLCMTransport, (self.topic.topic, self.topic.msg_type)) def __init__(self, topic: str, type: type, **kwargs) -> None: super().__init__(topic, type, **kwargs) @@ -175,7 +175,7 @@ def test_subscription(dimos, subscriber_class) -> None: robot = dimos.deploy(MockRobotClient) robot.lidar.transport = SpyLCMTransport("/lidar", PointCloud2) - robot.odometry.transport = SpyLCMTransport("/odom", Odometry) + robot.odometry.transport = SpyLCMTransport("/odom", PoseStamped) robot.mov.transport = pLCMTransport("/mov") subscriber = dimos.deploy(subscriber_class) @@ -217,7 +217,7 @@ def test_get_next(dimos) -> None: robot = dimos.deploy(MockRobotClient) robot.lidar.transport = SpyLCMTransport("/lidar", PointCloud2) - robot.odometry.transport = SpyLCMTransport("/odom", Odometry) + robot.odometry.transport = SpyLCMTransport("/odom", PoseStamped) robot.mov.transport = pLCMTransport("/mov") subscriber = dimos.deploy(RXPYSubscriber) @@ -228,14 +228,14 @@ def test_get_next(dimos) -> None: odom = subscriber.get_next() - assert isinstance(odom, Odometry) + assert isinstance(odom, PoseStamped) assert subscriber.active_subscribers() == 0 time.sleep(0.2) next_odom = subscriber.get_next() - assert isinstance(next_odom, Odometry) + assert isinstance(next_odom, PoseStamped) assert subscriber.active_subscribers() == 0 assert next_odom != odom @@ -250,7 +250,7 @@ def test_hot_getter(dimos) -> None: robot = dimos.deploy(MockRobotClient) robot.lidar.transport = SpyLCMTransport("/lidar", PointCloud2) - robot.odometry.transport = SpyLCMTransport("/odom", Odometry) + robot.odometry.transport = SpyLCMTransport("/odom", PoseStamped) robot.mov.transport = pLCMTransport("/mov") subscriber = dimos.deploy(RXPYSubscriber) @@ -264,7 +264,7 @@ def test_hot_getter(dimos) -> None: odom = subscriber.get_hot() subscriber.stop_hot_getter() - assert isinstance(odom, Odometry) + assert isinstance(odom, PoseStamped) time.sleep(0.3) # there are no subs @@ -275,7 +275,7 @@ def test_hot_getter(dimos) -> None: time.sleep(0.3) next_odom = subscriber.get_hot() - assert isinstance(next_odom, Odometry) + assert isinstance(next_odom, PoseStamped) assert next_odom != odom subscriber.stop_hot_getter() diff --git a/dimos/core/test_transport_factory.py b/dimos/core/test_transport_factory.py index 69e8e7d519c..2bfb15cf492 100644 --- a/dimos/core/test_transport_factory.py +++ b/dimos/core/test_transport_factory.py @@ -14,6 +14,8 @@ from __future__ import annotations +from dimos_generated.geometry_msgs.msg import Twist +from dimos_generated.sensor_msgs.msg import Image import pytest from dimos.core.global_config import GlobalConfig @@ -31,8 +33,6 @@ session_config, transport_topic, ) -from dimos.msgs.geometry_msgs.Twist import Twist -from dimos.msgs.sensor_msgs.Image import Image from dimos.protocol.pubsub.impl.zenohpubsub import QOS_LATEST_WINS, QOS_NEVER_DROP from dimos.protocol.rpc.pubsubrpc import LCMRPC from dimos.protocol.rpc.zenohrpc import ZenohRPC diff --git a/dimos/core/test_zenoh_transport.py b/dimos/core/test_zenoh_transport.py index 31c1532292c..c47fe6e772f 100644 --- a/dimos/core/test_zenoh_transport.py +++ b/dimos/core/test_zenoh_transport.py @@ -19,6 +19,7 @@ from types import SimpleNamespace from typing import cast +from dimos_generated.sensor_msgs.msg import Image import numpy as np from pydantic import ValidationError import pytest @@ -39,7 +40,6 @@ pLCMTransport, pZenohTransport, ) -from dimos.msgs.sensor_msgs.Image import Image from dimos.protocol.pubsub.impl.zenohpubsub import ( QOS_LATEST_WINS, QOS_NEVER_DROP, @@ -49,15 +49,22 @@ class TypedMsg: - """A fake typed message with lcm_encode for testing.""" + """A generated-message codec contract for transport selection tests.""" + + msg_name = "test_msgs/msg/TypedMsg" + schema = "" @staticmethod - def lcm_encode() -> bytes: + def encode() -> bytes: return b"" + @staticmethod + def decode(data: bytes): + return TypedMsg() + class UntypedMsg: - """A message without lcm_encode. Triggers pickle transport.""" + """A Python-only message without a generated codec contract.""" pass @@ -189,7 +196,7 @@ def test_zenoh_transport_broadcast_and_subscribe(retry_until, session_pool, coll t.start() t.subscribe(collector.callback) - test_img = Image(np.zeros((2, 2, 3), dtype=np.uint8)) + test_img = Image(width=2, height=2, step=6, encoding="rgb8", data=np.zeros(12, dtype=np.uint8)) retry_until(collector.event, lambda: t.broadcast(None, test_img)) assert isinstance(collector.received[0], Image) t.stop() @@ -228,7 +235,7 @@ def test_zenoh_transport_pickle_preserves_topic_qos() -> None: t = ZenohTransport(ZenohTopic("dimos/camera/color", Image, qos=QOS_LATEST_WINS)) t2 = pickle.loads(pickle.dumps(t)) assert type(t2) is ZenohTransport - assert t2.topic == t.topic # topic, lcm_type and qos all round-trip + assert t2.topic == t.topic # topic, msg_type and qos all round-trip def test_pzenoh_transport_pickle_preserves_topic_qos() -> None: diff --git a/dimos/core/testing.py b/dimos/core/testing.py index ebf4aa93430..3fc4a9d34f5 100644 --- a/dimos/core/testing.py +++ b/dimos/core/testing.py @@ -16,19 +16,33 @@ import time from typing import Any +from dimos_generated.geometry_msgs.msg import PoseStamped, Vector3 +from dimos_generated.sensor_msgs.msg import PointCloud2 + from dimos.constants import DEFAULT_THREAD_JOIN_TIMEOUT from dimos.core.core import rpc from dimos.core.module import Module from dimos.core.stream import In, Out -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 -from dimos.robot.unitree.type.lidar import pointcloud2_from_webrtc_lidar -from dimos.robot.unitree.type.odometry import Odometry +from dimos.msgs.time import to_seconds +from dimos.robot.unitree.type.lidar import RawLidarMsg, pointcloud2_from_webrtc_lidar +from dimos.robot.unitree.type.odometry import RawOdometryMessage, pose_from_webrtc_odometry +from dimos.types.timestamped import TimestampedData from dimos.utils.testing.replay import SensorReplay +def _replay_lidar(raw: RawLidarMsg) -> TimestampedData[PointCloud2]: + cloud = pointcloud2_from_webrtc_lidar(raw) + return TimestampedData(cloud, to_seconds(cloud.header.stamp)) + + +def _replay_odometry(raw: RawOdometryMessage) -> TimestampedData[PoseStamped]: + pose = pose_from_webrtc_odometry(raw) + return TimestampedData(pose, to_seconds(pose.header.stamp)) + + class MockRobotClient(Module): - odometry: Out[Odometry] + odometry: Out[PoseStamped] + timed_odometry: Out[TimestampedData[PoseStamped]] lidar: Out[PointCloud2] mov: In[Vector3] @@ -59,8 +73,8 @@ def stop(self) -> None: super().stop() def odomloop(self) -> None: - odomdata = SensorReplay("raw_odometry_rotate_walk", autocast=Odometry.from_msg) - lidardata = SensorReplay("office_lidar", autocast=pointcloud2_from_webrtc_lidar) + odomdata = SensorReplay("raw_odometry_rotate_walk", autocast=_replay_odometry) + lidardata = SensorReplay("office_lidar", autocast=_replay_lidar) lidariter = lidardata.iterate() self._stop_event.clear() @@ -69,9 +83,11 @@ def odomloop(self) -> None: if self._stop_event.is_set(): return print(odom) - odom.pubtime = time.perf_counter() - self.odometry.publish(odom) + # Benchmark publication time belongs to the Python-object stream, + # keeping the generated pose's exact source header untouched. + self.timed_odometry.publish(TimestampedData(odom.value, time.perf_counter())) + self.odometry.publish(odom.value) lidarmsg = next(lidariter) - self.lidar.publish(lidarmsg) + self.lidar.publish(lidarmsg.value) time.sleep(0.1) diff --git a/dimos/core/transport.py b/dimos/core/transport.py index 378a1d15f58..16916d15b70 100644 --- a/dimos/core/transport.py +++ b/dimos/core/transport.py @@ -50,6 +50,13 @@ ) from dimos.stream.audio.base import AudioEvent from dimos.utils.logging_config import setup_logger +from dimos.web.relay_bridge.protocol import ( + FrameHeader, + ProtocolError, + decode_data_frame, + encode_data_frame, + peek_data_frame_lengths, +) logger = setup_logger() @@ -156,7 +163,7 @@ def stop(self) -> None: self._started = False def __reduce__(self): # type: ignore[no-untyped-def] - return (LCMTransport, (self.topic.topic, self.topic.lcm_type)) + return (LCMTransport, (self.topic.topic, self.topic.msg_type)) def broadcast(self, _, msg) -> None: # type: ignore[no-untyped-def] if not self._started: @@ -174,15 +181,20 @@ def subscribe( class JpegLcmTransport(LCMTransport): # type: ignore[type-arg] def __init__(self, topic: str, type: type, **kwargs) -> None: # type: ignore[no-untyped-def] + from dimos_generated.sensor_msgs.msg import CompressedImage, Image + from dimos.protocol.pubsub.impl.jpeg_lcm import ( JpegLCM, ) # ~330ms: deferred to avoid pulling in Image/cv2/rerun + if type is not Image: + raise TypeError("JpegLcmTransport requires generated sensor_msgs/Image") + self.image_type = type self.lcm = JpegLCM(**kwargs) # type: ignore[assignment] - super().__init__(topic, type) + super().__init__(topic, CompressedImage) def __reduce__(self): # type: ignore[no-untyped-def] - return (JpegLcmTransport, (self.topic.topic, self.topic.lcm_type)) + return (JpegLcmTransport, (self.topic.topic, self.image_type)) def start(self) -> None: self.lcm.start() @@ -229,26 +241,36 @@ def stop(self) -> None: class SHMTransport(PubSubTransport[T]): _started: bool = False - def __init__(self, topic: str, **kwargs) -> None: # type: ignore[no-untyped-def] - super().__init__(topic) + def __init__(self, topic: str, msg_type: type[DimosMsg] | None = None, **kwargs: Any) -> None: + self._base_topic = topic + self._msg_type = msg_type + super().__init__(str(LCMTopic(topic, msg_type))) self.shm = BytesSharedMemory(**kwargs) def __reduce__(self): # type: ignore[no-untyped-def] return ( functools.partial(SHMTransport, default_capacity=self.shm.config.default_capacity), - (self.topic,), + (self._base_topic, self._msg_type), ) def broadcast(self, _, msg) -> None: # type: ignore[no-untyped-def] if not self._started: self.start() - self.shm.publish(self.topic, msg) + payload = cast("DimosMsg", msg).encode() if self._msg_type is not None else msg + self.shm.publish(self.topic, payload) - def subscribe(self, callback: Callable[[T], None], selfstream: In[T] | None = None) -> None: # type: ignore[override] + def subscribe( + self, callback: Callable[[T], Any], selfstream: Stream[T] | None = None + ) -> Callable[[], None]: if not self._started: self.start() - return self.shm.subscribe(self.topic, lambda msg, topic: callback(msg)) # type: ignore[arg-type, return-value] + msg_type = self._msg_type + if msg_type is None: + return self.shm.subscribe(self.topic, lambda msg, topic: callback(cast("T", msg))) + return self.shm.subscribe( + self.topic, lambda msg, topic: callback(cast("T", msg_type.decode(msg))) + ) def start(self) -> None: self.shm.start() @@ -380,8 +402,8 @@ class WebRTCTransport(PubSubTransport[M]): also be used directly with an explicit ``config``. Two modes: * **Raw bytes** (``msg_type=None``): messages pass through as ``bytes``. - * **Typed LCM** (``msg_type=SomeMsg``): LCM-encoded on ``broadcast()``, - LCM-decoded on ``subscribe()`` (foreign types on the shared channel are skipped) — so multiple + * **Typed CDR** (``msg_type=SomeMsg``): CDR in the shared browser data-frame + format, with channel and type metadata checked before decoding. Multiple transports sharing one multiplexed DataChannel each receive only their own message type. @@ -408,6 +430,7 @@ def __init__( self._msg_type = msg_type self._config = config or self._config_cls(**config_kwargs) self._pubsub: WebRTCPubSub | None = None + self._frame_sequence = 0 # Guards first-use init: concurrent subscribe()/broadcast() must not # construct two WebRTCPubSub wrappers (one would silently orphan any # subscribe_all state). Never pickled — __reduce__ rebuilds via the @@ -421,7 +444,25 @@ def broadcast(self, _: Stream[M] | None, msg: M) -> None: if not self._started: self.start() assert self._pubsub is not None - data = msg.lcm_encode() if self._msg_type is not None else msg + data: bytes | M + if self._msg_type is not None: + if msg.msg_name != self._msg_type.msg_name: + raise ValueError("message type does not match declared WebRTC channel type") + with self._init_lock: + sequence = self._frame_sequence + self._frame_sequence += 1 + data = encode_data_frame( + FrameHeader( + ch=self.topic, + seq=sequence, + ts=time.time(), + delivery="latest", + meta={"type": self._msg_type.msg_name, "encoding": "cdr"}, + ), + msg.encode(), + ) + else: + data = msg self._pubsub.publish(self.topic, data) # type: ignore[arg-type] def subscribe( @@ -435,12 +476,20 @@ def subscribe( msg_type = self._msg_type def _typed_cb(data: bytes, _topic: str) -> None: - # The channel is multiplexed (e.g. the browser sends Twists and - # Poses on cmd_unreliable); lcm_decode verifies the wire - # fingerprint and raises on other types — skip those. try: - msg = msg_type.lcm_decode(data) - except ValueError: + lengths = peek_data_frame_lengths(data) + if lengths is None or lengths[2] != len(data): + return + frame = decode_data_frame(data) + meta = frame.header.meta or {} + if ( + frame.header.ch != self.topic + or meta.get("encoding") != "cdr" + or meta.get("type") != msg_type.msg_name + ): + return + msg = msg_type.decode(frame.payload) + except (ProtocolError, ValueError): return callback(msg) # type: ignore[arg-type] @@ -587,7 +636,7 @@ class CloudflareAudioTransport(WebRTCAudioTransport): class ZenohTransport(PubSubTransport[T]): - """Zenoh transport with LCM encoding for typed DimosMsg. + """Zenoh transport with CDR encoding for typed DimosMsg. Accepts either a plain topic string plus message type, or a full `ZenohTopic` carrying per-topic settings: `ZenohTransport(ZenohTopic("bla", diff --git a/dimos/core/transport_factory.py b/dimos/core/transport_factory.py index 56455728e97..f4839ca830e 100644 --- a/dimos/core/transport_factory.py +++ b/dimos/core/transport_factory.py @@ -27,6 +27,7 @@ pLCMTransport, pZenohTransport, ) +from dimos.msgs.protocol import DimosMsg from dimos.protocol.pubsub.impl.zenohpubsub import ( QOS_LATEST_WINS, QOS_NEVER_DROP, @@ -58,7 +59,7 @@ def transport_topic(name: str, g: GlobalConfig = global_config) -> str: # High-rate sensor streams: drop stale frames under congestion, never stall the # publisher. Matched by message type since that is what makes them high-rate. -_LATEST_WINS_TYPES = ("sensor_msgs.Image", "sensor_msgs.PointCloud2") +_LATEST_WINS_TYPES = ("sensor_msgs/msg/Image", "sensor_msgs/msg/PointCloud2") # Low-rate channels where a drop loses something that never comes back: a whole # turn of agent/human conversation, a one-shot robot action verb, or a # push-to-talk chunk (one gap discards the whole utterance). @@ -90,7 +91,7 @@ def make_transport( """Construct the active-backend pub/sub transport for a logical channel. A pickled (self-describing) transport is used when no `msg_type` is given or - the type has no `lcm_encode`. Otherwise a typed transport is used. + the type does not implement the generated message contract. Otherwise CDR is used. A channel name alone doesn't fully define a topic: backends have per-topic settings (Zenoh publisher QoS, for one) that live on their Topic objects. @@ -100,7 +101,7 @@ def make_transport( transport map. LCM (UDP multicast) has no per-topic settings. """ - use_pickled = msg_type is None or getattr(msg_type, "lcm_encode", None) is None + use_pickled = msg_type is None or not isinstance(msg_type, DimosMsg) topic = transport_topic(name, g) if g.transport == "zenoh": ztopic = ZenohTopic( diff --git a/dimos/e2e_tests/cdr_replay_fixture.py b/dimos/e2e_tests/cdr_replay_fixture.py new file mode 100644 index 00000000000..cb953a948cd --- /dev/null +++ b/dimos/e2e_tests/cdr_replay_fixture.py @@ -0,0 +1,62 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Deterministic generated-message replay for browser integration tests.""" + +import math +from pathlib import Path + +from dimos_generated.geometry_msgs.msg import Point, Pose, PoseStamped, Quaternion +from dimos_generated.sensor_msgs.msg import Image, PointCloud2 +from dimos_generated.std_msgs.msg import Header +import numpy as np + +from dimos.memory.codecs.cdr import CdrCodec +from dimos.memory.codecs.lz4 import Lz4Codec +from dimos.memory.store.sqlite import SqliteStore +from dimos.msgs.image import image_from_array +from dimos.msgs.pointcloud import pointcloud_from_xyz +from dimos.msgs.time import time_from_nanoseconds + + +def write_go2_cdr_replay(path: Path, *, duration_s: int = 180) -> None: + """Record varying camera pixels, world poses and lidar as typed CDR streams.""" + base_ns = 1_700_000_000_123_456_789 + points = np.array([[1, 0, 0], [1, 1, 0], [2, 0, 0]], dtype=np.float32) + with SqliteStore(path=str(path)) as store: + video = store.stream("color_image", Image, codec=Lz4Codec(CdrCodec(Image))) + odom = store.stream("odom", PoseStamped) + lidar = store.stream("lidar", PointCloud2) + for index in range(duration_s * 4): + stamp_ns = base_ns + index * 250_000_000 + stamp = time_from_nanoseconds(stamp_ns) + angle = index / 40 + header = Header(stamp=stamp, frame_id="world") + pose = PoseStamped( + header=header, + pose=Pose( + position=Point(x=math.cos(angle), y=math.sin(angle)), + orientation=Quaternion(z=math.sin(angle / 2), w=math.cos(angle / 2)), + ), + ) + pixels = np.full((240, 320, 3), index % 256, dtype=np.uint8) + pixels[:, (index * 7) % 320] = (255, 0, 127) + image = image_from_array( + pixels, encoding="rgb8", header=Header(stamp=stamp, frame_id="camera_optical") + ) + cloud = pointcloud_from_xyz(points, header=Header(stamp=stamp, frame_id="base_link")) + receipt = stamp_ns / 1e9 + video.append(image, ts=receipt) + odom.append(pose, ts=receipt) + lidar.append(cloud, ts=receipt) diff --git a/dimos/e2e_tests/conftest.py b/dimos/e2e_tests/conftest.py index 2a11bf58d3b..e6158fe07d3 100644 --- a/dimos/e2e_tests/conftest.py +++ b/dimos/e2e_tests/conftest.py @@ -15,25 +15,26 @@ from __future__ import annotations from collections.abc import Callable, Generator, Iterator +from pathlib import Path import subprocess import sys import threading import time from typing import TYPE_CHECKING, Any +from dimos_generated.geometry_msgs.msg import Point, Pose, PoseStamped +from dimos_generated.std_msgs.msg import Bool, Header import pytest from dimos.core.coordination.coordinator_rpc import CoordinatorRPC from dimos.core.global_config import global_config from dimos.core.transport_factory import make_transport +from dimos.e2e_tests.cdr_replay_fixture import write_go2_cdr_replay from dimos.e2e_tests.conf_types import StartPersonTrack from dimos.e2e_tests.dim_sim_client import DimSimClient from dimos.e2e_tests.dimos_cli_call import DimosCliCall from dimos.e2e_tests.lcm_spy import LcmSpy -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Vector3 import make_vector3 -from dimos.msgs.std_msgs.Bool import Bool +from dimos.msgs.geometry import quaternion_from_euler from dimos.simulation.mujoco.direct_cmd_vel_explorer import DirectCmdVelExplorer from dimos.simulation.mujoco.person_on_track import PersonTrackPublisher @@ -143,9 +144,8 @@ def stop_all() -> None: def _pose(x: float, y: float, theta: float) -> PoseStamped: return PoseStamped( - position=make_vector3(x, y, 0), - orientation=Quaternion.from_euler(make_vector3(0, 0, theta)), - frame_id="map", + header=Header(frame_id="map"), + pose=Pose(position=Point(x=x, y=y), orientation=quaternion_from_euler(0, 0, theta)), ) @@ -160,11 +160,11 @@ def lcm_spy() -> Iterator[LcmSpy]: @pytest.fixture def follow_points(lcm_spy: LcmSpy): def fun(*, points: list[tuple[float, float, float]], fail_message: str) -> None: - topic = "/goal_reached#std_msgs.Bool" + topic = "/goal_reached#std_msgs/msg/Bool" lcm_spy.save_topic(topic) for x, y, theta in points: - lcm_spy.publish("/goal_request#geometry_msgs.PoseStamped", _pose(x, y, theta)) + lcm_spy.publish("/goal_request#geometry_msgs/msg/PoseStamped", _pose(x, y, theta)) lcm_spy.wait_for_message_result( topic, Bool, @@ -296,7 +296,7 @@ def dim_sim(): @pytest.fixture def spawn_wall_on_pose(lcm_spy: LcmSpy, dim_sim: DimSimClient): """Spawn a dim_sim wall when the robot's /odom comes within `threshold` metres of `point`.""" - odom_topic = "/odom#geometry_msgs.PoseStamped" + odom_topic = "/odom#geometry_msgs/msg/PoseStamped" stop_event = threading.Event() workers: list[threading.Thread] = [] errors: list[BaseException] = [] @@ -309,9 +309,9 @@ def spawn(*, point, threshold, wall) -> None: def on_odom(data): if triggered.is_set(): return - pose = PoseStamped.lcm_decode(data) - dx = pose.x - px - dy = pose.y - py + pose = PoseStamped.decode(data) + dx = pose.pose.position.x - px + dy = pose.pose.position.y - py if dx * dx + dy * dy < threshold_sq: triggered.set() @@ -373,3 +373,11 @@ def explore() -> None: direct_cmd_vel_explorer.follow_points(points) return explore + + +@pytest.fixture(scope="session") +def cdr_go2_replay_db(tmp_path_factory: pytest.TempPathFactory) -> Path: + """Current generated-message replay, independent of retired LFS codecs.""" + path = tmp_path_factory.mktemp("go2-cdr") / "replay.db" + write_go2_cdr_replay(path) + return path diff --git a/dimos/e2e_tests/dim_sim_client.py b/dimos/e2e_tests/dim_sim_client.py index 6e8732899ca..a80d1481920 100644 --- a/dimos/e2e_tests/dim_sim_client.py +++ b/dimos/e2e_tests/dim_sim_client.py @@ -12,9 +12,11 @@ # See the License for the specific language governing permissions and # limitations under the License. +from dimos_generated.geometry_msgs.msg import Point, Pose, PoseStamped, Quaternion +from dimos_generated.std_msgs.msg import Header + from dimos.core.transport import PubSubTransport from dimos.core.transport_factory import make_transport -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped from dimos.simulation.dimsim.scene_client import SceneClient @@ -52,8 +54,7 @@ def add_wall(self, x1: float, y1: float, x2: float, y2: float) -> None: def publish_goal(self, x: float, y: float) -> None: self._goal_request.publish( PoseStamped( - position=(x, y, 0), - orientation=(0, 0, 0, 1), - frame_id="world", + header=Header(frame_id="world"), + pose=Pose(position=Point(x=x, y=y), orientation=Quaternion(w=1.0)), ) ) diff --git a/dimos/e2e_tests/lcm_spy.py b/dimos/e2e_tests/lcm_spy.py index c11b3d4b0ae..e0bd4ec26c9 100644 --- a/dimos/e2e_tests/lcm_spy.py +++ b/dimos/e2e_tests/lcm_spy.py @@ -20,9 +20,10 @@ import threading from typing import Any +from dimos_generated.geometry_msgs.msg import PoseStamped + from dimos.core.global_config import global_config from dimos.core.transport_factory import transport_topic -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped from dimos.msgs.protocol import DimosMsg from dimos.protocol.pubsub.impl.lcmpubsub import LCMPubSubBase, Topic from dimos.protocol.pubsub.impl.zenohpubsub import Topic as ZenohTopic, ZenohPubSubBase @@ -35,13 +36,13 @@ def _wire_topic(channel: str) -> ZenohTopic: ZenohTopic is an LCM Topic plus zenoh's key expression, so both buses take it. """ parsed = Topic.from_channel_str(transport_topic(channel)) - return ZenohTopic(parsed.topic, parsed.lcm_type) + return ZenohTopic(parsed.topic, parsed.msg_type) class LcmSpy: """Sniffs every message on the active transport's bus. - Topics are named the LCM way (`/odom#geometry_msgs.PoseStamped`); + Topics are named the LCM way (`/odom#geometry_msgs/msg/PoseStamped`); `transport_topic` maps them to the running backend's wire name. """ @@ -90,7 +91,7 @@ def _on_message(self, data: bytes, topic: Topic) -> None: listener(data) def publish(self, topic: str, msg: Any) -> None: - self._bus.publish(_wire_topic(topic), msg.lcm_encode()) + self._bus.publish(_wire_topic(topic), msg.encode()) def save_topic(self, topic: str) -> None: with self._saved_topics_lock: @@ -168,7 +169,7 @@ def wait_for_message_result( event = threading.Event() def listener(msg: bytes) -> None: - data = type.lcm_decode(msg) + data = type.decode(msg) if predicate(data): event.set() @@ -183,12 +184,12 @@ def wait_until_odom_position( self, x: float, y: float, threshold: float = 1, timeout: float = 60 ) -> None: def predicate(msg: PoseStamped) -> bool: - pos = msg.position + pos = msg.pose.position distance = math.sqrt((pos.x - x) ** 2 + (pos.y - y) ** 2) return distance < threshold self.wait_for_message_result( - "/odom#geometry_msgs.PoseStamped", + "/odom#geometry_msgs/msg/PoseStamped", PoseStamped, predicate, f"Failed to get to position x={x}, y={y}", diff --git a/dimos/e2e_tests/test_cdr_spy.py b/dimos/e2e_tests/test_cdr_spy.py new file mode 100644 index 00000000000..4c72e0b163d --- /dev/null +++ b/dimos/e2e_tests/test_cdr_spy.py @@ -0,0 +1,51 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from dimos_generated.geometry_msgs.msg import Point, Pose, PoseStamped +from dimos_generated.std_msgs.msg import Header + +from dimos.core.global_config import global_config +from dimos.e2e_tests.lcm_spy import LcmSpy + + +def test_spy_publishes_generated_cdr_on_qualified_channel(mocker, monkeypatch): + monkeypatch.setattr(global_config, "transport", "lcm") + bus = mocker.patch("dimos.e2e_tests.lcm_spy.LCMPubSubBase").return_value + spy = LcmSpy() + message = PoseStamped(header=Header(frame_id="map"), pose=Pose(position=Point(x=2, y=3))) + spy.publish("/goal_request#geometry_msgs/msg/PoseStamped", message) + topic, payload = bus.publish.call_args.args + assert str(topic) == "/goal_request#geometry_msgs/msg/PoseStamped" + decoded = PoseStamped.decode(payload) + assert (decoded.pose.position.x, decoded.pose.position.y, decoded.header.frame_id) == ( + 2, + 3, + "map", + ) + + +def test_spy_odometry_predicate_uses_nested_pose(mocker, monkeypatch): + monkeypatch.setattr(global_config, "transport", "lcm") + mocker.patch("dimos.e2e_tests.lcm_spy.LCMPubSubBase") + spy = LcmSpy() + wait = mocker.patch.object(spy, "wait_for_message_result") + spy.wait_until_odom_position(2, 3, threshold=0.5) + topic, message_type, predicate, _, timeout = wait.call_args.args + assert (topic, message_type, timeout) == ( + "/odom#geometry_msgs/msg/PoseStamped", + PoseStamped, + 60, + ) + assert predicate(PoseStamped(pose=Pose(position=Point(x=2, y=3)))) + assert not predicate(PoseStamped(pose=Pose(position=Point(x=3, y=3)))) diff --git a/dimos/e2e_tests/test_cockpit_browser.py b/dimos/e2e_tests/test_cockpit_browser.py index 14e0414af6a..f92de13ef7b 100644 --- a/dimos/e2e_tests/test_cockpit_browser.py +++ b/dimos/e2e_tests/test_cockpit_browser.py @@ -35,6 +35,7 @@ """ from collections.abc import Callable, Iterator +from pathlib import Path import pytest @@ -57,7 +58,7 @@ @pytest.fixture -def start_go2_replay() -> Iterator[Callable[[], DimosCliCall]]: +def start_go2_replay(cdr_go2_replay_db: Path) -> Iterator[Callable[[], DimosCliCall]]: calls: list[DimosCliCall] = [] def start() -> DimosCliCall: @@ -65,7 +66,14 @@ def start() -> DimosCliCall: call.simulator = None # --viewer none: no rerun viewer spawn (headless CI); the replay # connection means no MuJoCo and no hardware. - call.global_args = ["--robot-ip", "fake", "--viewer", "none"] + call.global_args = [ + "--robot-ip", + "fake", + "--viewer", + "none", + "--replay-db", + str(cdr_go2_replay_db), + ] call.extra_env = {"RELAYBRIDGEMODULE__OPEN_BROWSER": "false"} call.demo_args = ["run", "unitree-go2-basic", "--local-relay"] call.start() diff --git a/dimos/e2e_tests/test_control_coordinator.py b/dimos/e2e_tests/test_control_coordinator.py index 943e7ae66d4..2b19f9dbd75 100644 --- a/dimos/e2e_tests/test_control_coordinator.py +++ b/dimos/e2e_tests/test_control_coordinator.py @@ -20,6 +20,9 @@ import time +from dimos_generated.sensor_msgs.msg import JointState +from dimos_generated.trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint + from dimos.control.coordinator import ControlCoordinator from dimos.control.tasks.trajectory_task.trajectory_task import ( JOINT_TRAJECTORY_TASK_NAME, @@ -27,10 +30,8 @@ TrajectoryExecutionStatus, ) from dimos.core.rpc_client import RPCClient -from dimos.msgs.sensor_msgs.JointState import JointState -from dimos.msgs.trajectory_msgs.JointTrajectory import JointTrajectory -from dimos.msgs.trajectory_msgs.TrajectoryPoint import TrajectoryPoint -from dimos.msgs.trajectory_msgs.TrajectoryStatus import TrajectoryState +from dimos.msgs.time import duration_from_seconds, header_now +from dimos.msgs.trajectory import TrajectoryState class TestControlCoordinatorE2E: @@ -41,7 +42,7 @@ def test_coordinator_starts_and_responds_to_rpc( ) -> None: """Test that coordinator starts and responds to RPC queries.""" # Save topics we care about (topic names carry the type suffix) - joint_state_topic = "/coordinator_joint_state#sensor_msgs.JointState" + joint_state_topic = "/coordinator_joint_state#sensor_msgs/msg/JointState" lcm_spy.save_topic(joint_state_topic) # Start the mock coordinator blueprint @@ -77,14 +78,14 @@ def test_coordinator_executes_trajectory( ) -> None: """Test that coordinator executes a trajectory via RPC.""" # Save topics - lcm_spy.save_topic("/coordinator_joint_state#sensor_msgs.JointState") + lcm_spy.save_topic("/coordinator_joint_state#sensor_msgs/msg/JointState") # Start coordinator start_blueprint("coordinator-mock") wait_for_system_ready() # Wait for it to be ready - lcm_spy.wait_for_saved_topic("/coordinator_joint_state#sensor_msgs.JointState") + lcm_spy.wait_for_saved_topic("/coordinator_joint_state#sensor_msgs/msg/JointState") # Create RPC client client = RPCClient(None, ControlCoordinator) @@ -95,15 +96,16 @@ def test_coordinator_executes_trajectory( # Create a simple trajectory trajectory = JointTrajectory( + header=header_now(), joint_names=[f"arm/joint{i + 1}" for i in range(7)], points=[ - TrajectoryPoint( - time_from_start=0.0, + JointTrajectoryPoint( + time_from_start=duration_from_seconds(0.0), positions=[0.0] * 7, velocities=[0.0] * 7, ), - TrajectoryPoint( - time_from_start=0.5, + JointTrajectoryPoint( + time_from_start=duration_from_seconds(0.5), positions=[0.1] * 7, velocities=[0.0] * 7, ), @@ -129,7 +131,7 @@ def test_coordinator_joint_state_published( self, lcm_spy, start_blueprint, wait_for_system_ready ) -> None: """Test that joint state messages are published at expected rate.""" - joint_state_topic = "/coordinator_joint_state#sensor_msgs.JointState" + joint_state_topic = "/coordinator_joint_state#sensor_msgs/msg/JointState" lcm_spy.save_topic(joint_state_topic) # Start coordinator @@ -153,7 +155,7 @@ def test_coordinator_joint_state_published( with lcm_spy._messages_lock: raw_msg = lcm_spy.messages[joint_state_topic][0] - joint_state = JointState.lcm_decode(raw_msg) + joint_state = JointState.decode(raw_msg) assert len(joint_state.name) == 7 assert len(joint_state.position) == 7 assert "arm/joint1" in joint_state.name @@ -162,26 +164,27 @@ def test_coordinator_cancel_trajectory( self, lcm_spy, start_blueprint, wait_for_system_ready ) -> None: """Test that a running trajectory can be cancelled.""" - lcm_spy.save_topic("/coordinator_joint_state#sensor_msgs.JointState") + lcm_spy.save_topic("/coordinator_joint_state#sensor_msgs/msg/JointState") # Start coordinator start_blueprint("coordinator-mock") wait_for_system_ready() - lcm_spy.wait_for_saved_topic("/coordinator_joint_state#sensor_msgs.JointState") + lcm_spy.wait_for_saved_topic("/coordinator_joint_state#sensor_msgs/msg/JointState") client = RPCClient(None, ControlCoordinator) try: # Create a long trajectory (5 seconds) trajectory = JointTrajectory( + header=header_now(), joint_names=[f"arm/joint{i + 1}" for i in range(7)], points=[ - TrajectoryPoint( - time_from_start=0.0, + JointTrajectoryPoint( + time_from_start=duration_from_seconds(0.0), positions=[0.0] * 7, velocities=[0.0] * 7, ), - TrajectoryPoint( - time_from_start=5.0, + JointTrajectoryPoint( + time_from_start=duration_from_seconds(5.0), positions=[1.0] * 7, velocities=[0.0] * 7, ), @@ -208,12 +211,12 @@ def test_dual_arm_coordinator( self, lcm_spy, start_blueprint, wait_until, wait_for_system_ready ) -> None: """Test dual-arm coordinator moving both arms with one combined trajectory.""" - lcm_spy.save_topic("/coordinator_joint_state#sensor_msgs.JointState") + lcm_spy.save_topic("/coordinator_joint_state#sensor_msgs/msg/JointState") # Start dual-arm mock coordinator start_blueprint("coordinator-dual-mock") wait_for_system_ready() - lcm_spy.wait_for_saved_topic("/coordinator_joint_state#sensor_msgs.JointState") + lcm_spy.wait_for_saved_topic("/coordinator_joint_state#sensor_msgs/msg/JointState") client = RPCClient(None, ControlCoordinator) try: @@ -230,11 +233,16 @@ def test_dual_arm_coordinator( # One trajectory moving the left arm (7 joints) and right arm # (6 joints) together trajectory = JointTrajectory( + header=header_now(), joint_names=[f"left_arm/joint{i + 1}" for i in range(7)] + [f"right_arm/joint{i + 1}" for i in range(6)], points=[ - TrajectoryPoint(time_from_start=0.0, positions=[0.0] * 13), - TrajectoryPoint(time_from_start=0.5, positions=[0.2] * 7 + [0.3] * 6), + JointTrajectoryPoint( + time_from_start=duration_from_seconds(0.0), positions=[0.0] * 13 + ), + JointTrajectoryPoint( + time_from_start=duration_from_seconds(0.5), positions=[0.2] * 7 + [0.3] * 6 + ), ], ) diff --git a/dimos/e2e_tests/test_custom_channel_browser.py b/dimos/e2e_tests/test_custom_channel_browser.py index 029726696ca..2ff83eb292a 100644 --- a/dimos/e2e_tests/test_custom_channel_browser.py +++ b/dimos/e2e_tests/test_custom_channel_browser.py @@ -31,10 +31,12 @@ from collections.abc import Callable import struct +from dimos_generated.geometry_msgs.msg import Point, Pose, PoseStamped, Quaternion +from dimos_generated.nav_msgs.msg import Path +from dimos_generated.std_msgs.msg import Header import pytest -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.nav_msgs.Path import Path +from dimos.msgs.time import time_from_seconds from dimos.web.cockpit import Channel, cockpit from dimos.web.codecs import EncodedPayload, web_encoder from dimos.web.relay_bridge.locate import find_web_dir @@ -48,12 +50,20 @@ TOPIC = "/custom_path_e2e/nav_path" PATH_MSG = Path( - ts=42.5, - frame_id="world", + header=Header(stamp=time_from_seconds(42.5), frame_id="world"), poses=[ - PoseStamped(ts=42.5, position=[1.5, -2.5, 0.0], orientation=[0.0, 0.0, 0.0, 1.0]), - PoseStamped(ts=42.5, position=[2.0, 0.25, 0.0], orientation=[0.0, 0.0, 0.0, 1.0]), - PoseStamped(ts=42.5, position=[3.5, 4.0, 0.0], orientation=[0.0, 0.0, 0.0, 1.0]), + PoseStamped( + header=Header(stamp=time_from_seconds(42.5)), + pose=Pose(position=Point(x=1.5, y=-2.5), orientation=Quaternion(w=1)), + ), + PoseStamped( + header=Header(stamp=time_from_seconds(42.5)), + pose=Pose(position=Point(x=2.0, y=0.25), orientation=Quaternion(w=1)), + ), + PoseStamped( + header=Header(stamp=time_from_seconds(42.5)), + pose=Pose(position=Point(x=3.5, y=4.0), orientation=Quaternion(w=1)), + ), ], ) @@ -62,7 +72,7 @@ # pairs, meta.n = point count; web/examples/custom-path decodes it. @web_encoder("path.points.v1") def encode_path_points(msg: Path) -> EncodedPayload: - payload = b"".join(struct.pack(" relay -> schema-driven browser decoder, end to end. - -Marked web_browser: excluded from the default suite (needs the -`browser-tests` dependency group, Playwright browsers, and built web dists). -Locally: -`uv run --group browser-tests pytest -m web_browser dimos/e2e_tests/test_lcm_channel_browser.py`. +"""Generated CDR over LCM browser E2E. + +A typed channel advertises its complete ROS2 schema. The browser decodes the +Python CDR payload from the manifest alone; LCM remains the raw transport. +Requires the browser-tests group, installed browsers and built web dists. """ from collections.abc import Callable from pathlib import Path +from dimos_generated.geometry_msgs.msg import Point, Pose, PoseStamped, Quaternion +from dimos_generated.std_msgs.msg import Header import pytest -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped +from dimos.msgs.time import time_from_seconds from dimos.web.cockpit import Channel, cockpit pytest.importorskip("playwright") @@ -44,10 +38,11 @@ TOPIC = "/lcm_channel_e2e/lcm_pose" POSE = PoseStamped( - ts=42.5, frame_id="map", position=[1.5, -2.5, 0.25], orientation=[0.0, 0.0, 0.0, 1.0] + header=Header(stamp=time_from_seconds(42.5), frame_id="map"), + pose=Pose(position=Point(x=1.5, y=-2.5, z=0.25), orientation=Quaternion(w=1)), ) -# A page whose SDK and relay live on another origin, reading the decoded LCM +# A page whose SDK and relay live on another origin, reading the decoded CDR # value's fields straight off the store snapshot. SDK_PAGE = """ @@ -60,7 +55,7 @@ const v = snapshot.slot?.value; if (v === undefined || v === null) return; document.querySelector("#pose").textContent = - `x=${v.pose.position.x} frame=${v.header.frame_id} nsec=${v.header.stamp.nsec}`; + `x=${v.pose.position.x} frame=${v.header.frame_id} nanosec=${v.header.stamp.nanosec}`; }); @@ -83,11 +78,11 @@ def test_cockpit_channel_table_decodes_the_lcm_channel( chromium_page.get_by_test_id("view-channels").click() expect(chromium_page.get_by_test_id("ch-lcm_pose-seq")).not_to_have_text("-", timeout=120_000) value = chromium_page.get_by_test_id("ch-lcm_pose-value") - # The preview is the schema-driven decoder's: LCM field names, wire order. + # The preview is the schema-driven decoder's: generated ROS2 fields, decoded from the advertised schema. expect(value).to_contain_text("position: {x: 1.5, y: -2.5, z: 0.25}") expect(value).to_contain_text('frame_id: "map"') expect(chromium_page.get_by_test_id("ch-lcm_pose-decode-error")).to_have_count(0) - expect(chromium_page.get_by_text("geometry_msgs.PoseStamped.lcm.v1")).to_be_visible() + expect(chromium_page.get_by_text("geometry_msgs/msg/PoseStamped.cdr.v1")).to_be_visible() def test_sdk_page_reads_the_decoded_lcm_fields( @@ -97,5 +92,5 @@ def test_sdk_page_reads_the_decoded_lcm_fields( page.write_text(SDK_PAGE % {"url": cockpit_url}) chromium_page.goto(page.as_uri()) expect(chromium_page.locator("#pose")).to_have_text( - "x=1.5 frame=map nsec=500000000", timeout=120_000 + "x=1.5 frame=map nanosec=500000000", timeout=120_000 ) diff --git a/dimos/e2e_tests/test_manipulation_planning_groups.py b/dimos/e2e_tests/test_manipulation_planning_groups.py index a263c8da752..004e0018575 100644 --- a/dimos/e2e_tests/test_manipulation_planning_groups.py +++ b/dimos/e2e_tests/test_manipulation_planning_groups.py @@ -23,6 +23,7 @@ from collections.abc import Callable import time +from dimos_generated.sensor_msgs.msg import JointState import pytest from dimos.control.coordinator import ControlCoordinator @@ -36,11 +37,10 @@ ManipulationSnapshot, PlanningGroupInfo, ) -from dimos.msgs.sensor_msgs.JointState import JointState pytestmark = [pytest.mark.self_hosted] -JOINT_STATE_TOPIC = "/coordinator_joint_state#sensor_msgs.JointState" +JOINT_STATE_TOPIC = "/coordinator_joint_state#sensor_msgs/msg/JointState" BLUEPRINT = "openarm-planner-coordinator" LEFT_GROUP_ID = "left_arm" RIGHT_GROUP_ID = "right_arm" diff --git a/dimos/e2e_tests/test_map_click_browser.py b/dimos/e2e_tests/test_map_click_browser.py index 2152eb80e9c..0695f848b6f 100644 --- a/dimos/e2e_tests/test_map_click_browser.py +++ b/dimos/e2e_tests/test_map_click_browser.py @@ -18,13 +18,14 @@ import threading from typing import NamedTuple +from dimos_generated.geometry_msgs.msg import Point, PointStamped, Pose, PoseStamped, Quaternion +from dimos_generated.nav_msgs.msg import Path +from dimos_generated.std_msgs.msg import Header import pytest from reactivex.disposable import Disposable from dimos.core.transport import pLCMTransport -from dimos.msgs.geometry_msgs.PointStamped import PointStamped -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.nav_msgs.Path import Path +from dimos.msgs.time import time_from_seconds from dimos.web.cockpit import Map2D, cockpit from dimos.web.relay_bridge.e2e_support import stop_module from dimos.web.relay_bridge.gen_costmap_fixtures import grid_msg @@ -50,9 +51,13 @@ class MapBridge(NamedTuple): def _path(*xy: tuple[float, float]) -> Path: poses = [ - PoseStamped(ts=1.0, position=[x, y, 0.0], orientation=[0.0, 0.0, 0.0, 1.0]) for x, y in xy + PoseStamped( + header=Header(stamp=time_from_seconds(1.0)), + pose=Pose(position=Point(x=x, y=y), orientation=Quaternion(w=1)), + ) + for x, y in xy ] - return Path(ts=1.0, frame_id="world", poses=poses) + return Path(header=Header(stamp=time_from_seconds(1.0), frame_id="world"), poses=poses) @pytest.fixture(scope="module") @@ -125,9 +130,9 @@ def test_click_to_goal_and_cancel(map_bridge: MapBridge, chromium_page: Page) -> ) chromium_page.click(CANVAS) assert wait_until(lambda: len(clicks) == 1, timeout=15.0) - assert clicks[0].x == pytest.approx(0.75, abs=0.01) - assert clicks[0].y == pytest.approx(0.0, abs=0.01) - assert clicks[0].frame_id == "world" + assert clicks[0].point.x == pytest.approx(0.75, abs=0.01) + assert clicks[0].point.y == pytest.approx(0.0, abs=0.01) + assert clicks[0].header.frame_id == "world" cancel = chromium_page.get_by_test_id("map2d-global_costmap-cancel") expect(cancel).to_have_count(0) diff --git a/dimos/e2e_tests/test_sdk_browser.py b/dimos/e2e_tests/test_sdk_browser.py index 419ac776804..a5095781486 100644 --- a/dimos/e2e_tests/test_sdk_browser.py +++ b/dimos/e2e_tests/test_sdk_browser.py @@ -77,12 +77,19 @@ @pytest.fixture(scope="module") -def go2_replay_serve_dir() -> Iterator[DimosCliCall]: +def go2_replay_serve_dir(cdr_go2_replay_db: Path) -> Iterator[DimosCliCall]: call = DimosCliCall() call.simulator = None # --viewer none: no rerun viewer spawn (headless CI); the replay # connection means no MuJoCo and no hardware. - call.global_args = ["--robot-ip", "fake", "--viewer", "none"] + call.global_args = [ + "--robot-ip", + "fake", + "--viewer", + "none", + "--replay-db", + str(cdr_go2_replay_db), + ] call.extra_env = {"RELAYBRIDGEMODULE__OPEN_BROWSER": "false"} call.demo_args = [ "run", diff --git a/dimos/e2e_tests/test_security_module.py b/dimos/e2e_tests/test_security_module.py index 224ba0c1cad..c153dc44e66 100644 --- a/dimos/e2e_tests/test_security_module.py +++ b/dimos/e2e_tests/test_security_module.py @@ -15,7 +15,7 @@ from collections.abc import Callable import time -from dimos_lcm.std_msgs import String +from dimos_generated.std_msgs.msg import String import pytest from dimos.e2e_tests.conf_types import StartPersonTrack @@ -49,7 +49,7 @@ def test_security_module( "unitree-go2-security", ) - lcm_spy.save_topic("/security_state#std_msgs.String") + lcm_spy.save_topic("/security_state#std_msgs/msg/String") wait_for_system_ready(timeout=120.0) time.sleep(2) @@ -70,7 +70,7 @@ def predicate(s: String) -> bool: return s.data == "FOLLOWING" lcm_spy.wait_for_message_result( - "/security_state#std_msgs.String", + "/security_state#std_msgs/msg/String", String, predicate, "Failed to transition to FOLLOWING.", diff --git a/dimos/evals/agents/lib/plain_recording.py b/dimos/evals/agents/lib/plain_recording.py index 4b2abc83d4a..fffd61c3210 100644 --- a/dimos/evals/agents/lib/plain_recording.py +++ b/dimos/evals/agents/lib/plain_recording.py @@ -22,10 +22,11 @@ from pathlib import Path from typing import TYPE_CHECKING, Any +from dimos_generated.sensor_msgs.msg import Image, PointCloud2 import numpy as np -from dimos.msgs.sensor_msgs.Image import Image -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 +from dimos.msgs.image import image_to_rgb +from dimos.msgs.pointcloud import pointcloud_rgb, pointcloud_xyz if TYPE_CHECKING: from dimos.memory.stream import Stream @@ -48,10 +49,14 @@ def plain_recording(streams: Sequence[Stream[Any, Any]], directory: Path) -> Pat record: dict[str, Any] = { "stream": stream.name, "timestamp": obs.ts, - "frame_id": getattr(data, "frame_id", None), + "frame_id": data.header.frame_id + if isinstance(data, (Image, PointCloud2)) + else None, } if isinstance(data, PointCloud2): - points, colors = data.as_numpy() + points = pointcloud_xyz(data) + rgb = pointcloud_rgb(data) + colors = None if rgb is None else rgb.astype(np.float64) / 255.0 values = points if colors is None else np.column_stack((points, colors)) path = directory / f"{stem}.csv" np.savetxt( @@ -65,7 +70,7 @@ def plain_recording(streams: Sequence[Stream[Any, Any]], directory: Path) -> Pat record["format"] = "XYZ in source coordinates; optional RGB in [0,1]" elif isinstance(data, Image): path = directory / f"{stem}.png" - PILImage.fromarray(data.to_rgb().data).save(path) + PILImage.fromarray(image_to_rgb(data)).save(path) record["format"] = "RGB PNG" elif data is None or isinstance(data, (bool, int, float, str, list, dict)): path = directory / f"{stem}.json" diff --git a/dimos/evals/agents/question_answer.py b/dimos/evals/agents/question_answer.py index d0d372fb33b..b2c9d0b895e 100644 --- a/dimos/evals/agents/question_answer.py +++ b/dimos/evals/agents/question_answer.py @@ -12,14 +12,16 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""One model call over the encoded recording. The only place in -:mod:`dimos.evals` that calls ``agent_encode()`` — the surface under test.""" +"""One model call over generated image content and explicit custom agent encoders.""" from __future__ import annotations +import base64 import json from typing import TYPE_CHECKING, Any +from dimos_generated.geometry_msgs.msg import PoseStamped +from dimos_generated.sensor_msgs.msg import Image from pydantic import Field from dimos.evals.agents.lib.single_call import ( @@ -28,15 +30,35 @@ SingleCallAgentConfig, ) from dimos.evals.types import RunningEnvironment +from dimos.msgs.image import image_to_jpeg if TYPE_CHECKING: from dimos.memory.type.observation import Observation def _observation_blocks(obs: Observation[Any], stamp: str) -> Blocks: - """One observation as ``agent_encode()`` renders it. ``str(data)`` where a - type has no encoder: an encoding gap the eval surfaces by design.""" + """Render generated images as JPEG blocks and custom values through their encoders.""" data = obs.data + if isinstance(data, Image): + jpeg = base64.b64encode(image_to_jpeg(data, quality=80)).decode("ascii") + return [ + {"type": "text", "text": stamp}, + {"type": "image_url", "image_url": {"url": f"data:image/jpeg;base64,{jpeg}"}}, + ] + if isinstance(data, PoseStamped): + position = data.pose.position + orientation = data.pose.orientation + return [ + { + "type": "text", + "text": ( + f"{stamp} frame={data.header.frame_id!r} " + f"position=({position.x:.3f}, {position.y:.3f}, {position.z:.3f}) " + f"quaternion=({orientation.x:.3f}, {orientation.y:.3f}, " + f"{orientation.z:.3f}, {orientation.w:.3f})" + ), + } + ] encoded = data.agent_encode() if hasattr(data, "agent_encode") else None if isinstance(encoded, list): # e.g. Image -> image_url blocks return [{"type": "text", "text": stamp}, *encoded] diff --git a/dimos/evals/agents/test_no_dimos.py b/dimos/evals/agents/test_no_dimos.py index 064527287c6..f1261a62e6e 100644 --- a/dimos/evals/agents/test_no_dimos.py +++ b/dimos/evals/agents/test_no_dimos.py @@ -18,6 +18,8 @@ import os from pathlib import Path +from dimos_generated.sensor_msgs.msg import Image, PointCloud2 +from dimos_generated.std_msgs.msg import Header import numpy as np from PIL import Image as PILImage import pytest @@ -30,8 +32,9 @@ from dimos.evals.constants import NO_DIMOS_KEYWORDS from dimos.evals.types import RunningEnvironment from dimos.memory.store.sqlite import SqliteStore -from dimos.msgs.sensor_msgs.Image import Image, ImageFormat -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 +from dimos.msgs.image import image_from_array, image_to_rgb +from dimos.msgs.pointcloud import pointcloud_from_xyz +from dimos.msgs.time import time_from_seconds def test_no_dimos_defaults_the_excluded_keywords_but_keeps_explicit_ones() -> None: @@ -81,12 +84,19 @@ def test_recorded_observations_are_exported_as_plain_files(tmp_path: Path) -> No points = np.array([[1.125, -2.5, 3.0], [4.0, 5.0, 6.0]]) with SqliteStore(path=tmp_path / "source.db") as store: lidar = store.stream("lidar", PointCloud2) - lidar.append(PointCloud2.from_numpy(points, frame_id="world", timestamp=1.0), ts=1.0) - lidar.append(PointCloud2.from_numpy(points * 2, timestamp=2.0), ts=2.0) + lidar.append( + pointcloud_from_xyz( + points, header=Header(frame_id="world", stamp=time_from_seconds(1.0)) + ), + ts=1.0, + ) + lidar.append( + pointcloud_from_xyz(points * 2, header=Header(stamp=time_from_seconds(2.0))), ts=2.0 + ) images = store.stream("camera", Image) - images.append(Image.from_numpy(pixels, format=ImageFormat.RGB), ts=3.0) + images.append(image_from_array(pixels, encoding="rgb8"), ts=3.0) store.stream("secret", str).append("grader-only") - stored_pixels = images.first().data.to_rgb().data + stored_pixels = image_to_rgb(images.first().data) manifest = plain_recording((lidar.limit(1), images), tmp_path / "input") records = json.loads(manifest.read_text())["observations"] assert [record["stream"] for record in records] == ["lidar", "camera"] diff --git a/dimos/evals/agents/test_pi.py b/dimos/evals/agents/test_pi.py index 763445ac4e1..709bbdbd270 100644 --- a/dimos/evals/agents/test_pi.py +++ b/dimos/evals/agents/test_pi.py @@ -90,7 +90,7 @@ def test_recording_export_contains_only_selected_data(dataset: str, tmp_path: Pa with SqliteStore(path=str(exported), must_exist=True) as copy: assert copy.list_streams() == ["odom"] - assert [(obs.ts, obs.data.position.x) for obs in copy.streams.odom] == [ + assert [(obs.ts, obs.data.pose.position.x) for obs in copy.streams.odom] == [ (1000.0, 0.0), (1001.0, 1.0), ] diff --git a/dimos/evals/conftest.py b/dimos/evals/conftest.py index 22a7446c4bf..8ea5e741760 100644 --- a/dimos/evals/conftest.py +++ b/dimos/evals/conftest.py @@ -14,12 +14,11 @@ from pathlib import Path +from dimos_generated.geometry_msgs.msg import Point, Pose, PoseStamped, Quaternion +from dimos_generated.std_msgs.msg import Header import pytest from dimos.memory.store.sqlite import SqliteStore -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Vector3 import make_vector3 @pytest.fixture @@ -30,9 +29,8 @@ def dataset(tmp_path: Path) -> str: stream = store.stream("odom", PoseStamped) for i in range(5): pose = PoseStamped( - position=make_vector3(float(i), 0.0, 0.0), - orientation=Quaternion(0.0, 0.0, 0.0, 1.0), - frame_id="world", + header=Header(frame_id="world"), + pose=Pose(position=Point(x=float(i)), orientation=Quaternion(w=1.0)), ) stream.append(pose, ts=1000.0 + i) return str(path) diff --git a/dimos/evals/environments/dimsim.py b/dimos/evals/environments/dimsim.py index ab4581a0b40..db8c45ce2a6 100644 --- a/dimos/evals/environments/dimsim.py +++ b/dimos/evals/environments/dimsim.py @@ -23,9 +23,10 @@ from dimos.evals.environments.sim import Sim, SimConfig if TYPE_CHECKING: + from dimos_generated.geometry_msgs.msg import PoseStamped + from dimos.e2e_tests.dimos_cli_call import DimosCliCall from dimos.memory.store.base import Store - from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped class DimSimEnvironmentConfig(SimConfig): diff --git a/dimos/evals/environments/habitat.py b/dimos/evals/environments/habitat.py index 2ba0cd62f8c..4d6e8141e87 100644 --- a/dimos/evals/environments/habitat.py +++ b/dimos/evals/environments/habitat.py @@ -22,10 +22,11 @@ import time from typing import TYPE_CHECKING, Any +from dimos_generated.geometry_msgs.msg import PoseStamped from pydantic import model_validator from dimos.evals.environments.sim import Sim, SimConfig -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped +from dimos.msgs.time import to_seconds if TYPE_CHECKING: from dimos.e2e_tests.dimos_cli_call import DimosCliCall @@ -151,7 +152,10 @@ def wait_ready(self, recording: Store, *, deadline: float) -> None: try: pose = self.latest_pose(recording) image = recording.streams.color_image.last().data - if time.time() - min(pose.ts, image.ts) < 10.0: + if ( + time.time() - min(to_seconds(pose.header.stamp), to_seconds(image.header.stamp)) + < 10.0 + ): self._spawn = pose return except (LookupError, AttributeError): @@ -164,9 +168,7 @@ def latest_pose(self, recording: Store) -> PoseStamped: if "odometry" not in recording.streams: raise LookupError("No Habitat odometry recorded") odom = recording.streams.odometry.last().data - return PoseStamped( - ts=odom.ts, frame_id=odom.frame_id, position=odom.position, orientation=odom.orientation - ) + return PoseStamped(header=odom.header, pose=odom.pose.pose) def episode_metadata(self) -> dict[str, object]: """Record the launch overrides and initial observed pose. @@ -183,5 +185,11 @@ def episode_metadata(self) -> dict[str, object]: ), # Describes the scan generation method, not publication state. "point_cloud_source": "depth_unprojection", - "initial_observed_position_ros": list(self._spawn.position) if self._spawn else None, + "initial_observed_position_ros": [ + self._spawn.pose.position.x, + self._spawn.pose.position.y, + self._spawn.pose.position.z, + ] + if self._spawn + else None, } diff --git a/dimos/evals/environments/image_file.py b/dimos/evals/environments/image_file.py index 4a1a7e2a0ea..566e15699df 100644 --- a/dimos/evals/environments/image_file.py +++ b/dimos/evals/environments/image_file.py @@ -20,8 +20,15 @@ from pathlib import Path from typing import TYPE_CHECKING, Any +from dimos_generated.sensor_msgs.msg import Image +import numpy as np +from PIL import Image as PILImage + from dimos.evals.environments.base import Environment from dimos.evals.types import RunningEnvironment +from dimos.memory.store.memory import MemoryStore +from dimos.msgs.image import image_from_array +from dimos.msgs.time import header_now, to_seconds from dimos.protocol.service.spec import BaseConfig if TYPE_CHECKING: @@ -50,14 +57,13 @@ def preflight(self, agent: Agent) -> None: raise FileNotFoundError(f"image does not exist: {self.config.path}") def start(self, modules: Sequence[str]) -> RunningEnvironment: - from dimos.memory.store.memory import MemoryStore - from dimos.msgs.sensor_msgs.Image import Image - - image = Image.from_file(self.config.path) + with PILImage.open(self.config.path) as source: + pixels = np.asarray(source.convert("RGB")) + image = image_from_array(pixels, encoding="rgb8", header=header_now()) recording = MemoryStore() self._resources.callback(recording.stop) stream = recording.stream("image", Image) - stream.append(image, ts=image.ts) + stream.append(image, ts=to_seconds(image.header.stamp)) return RunningEnvironment( mcp_url="", streams=(stream,), artifacts={"image": self.config.path} ) diff --git a/dimos/evals/environments/sim.py b/dimos/evals/environments/sim.py index 138d3907b14..734772522b7 100644 --- a/dimos/evals/environments/sim.py +++ b/dimos/evals/environments/sim.py @@ -35,9 +35,10 @@ from dimos.protocol.service.spec import BaseConfig if TYPE_CHECKING: + from dimos_generated.geometry_msgs.msg import PoseStamped + from dimos.evals.agents.base import Agent from dimos.memory.store.base import Store - from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped class SimConfig(BaseConfig): @@ -188,7 +189,7 @@ def settle(self, budget_s: float) -> None: pose = self.latest_pose(self._recording) except LookupError: return - position = pose.position + position = pose.pose.position if ( anchor is None or math.hypot(position.x - anchor.x, position.y - anchor.y) > self.config.at_rest_m diff --git a/dimos/evals/environments/test_dimsim.py b/dimos/evals/environments/test_dimsim.py index 3a851b068e4..3ed0818edf7 100644 --- a/dimos/evals/environments/test_dimsim.py +++ b/dimos/evals/environments/test_dimsim.py @@ -14,12 +14,14 @@ from unittest.mock import Mock +from dimos_generated.geometry_msgs.msg import PoseStamped +from dimos_generated.std_msgs.msg import Header import pytest from dimos.e2e_tests.dimos_cli_call import DimosCliCall from dimos.evals.environments.dimsim import DimSimEnvironment from dimos.memory.store.memory import MemoryStore -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped +from dimos.msgs.time import time_from_seconds, to_seconds def test_dimsim_launch_setup_and_pose(mocker): @@ -37,9 +39,9 @@ def test_dimsim_launch_setup_and_pose(mocker): with MemoryStore() as store: with pytest.raises(LookupError): env.latest_pose(store) - pose = PoseStamped(ts=123, frame_id="world") + pose = PoseStamped(header=Header(stamp=time_from_seconds(123), frame_id="world")) store.stream("odom", PoseStamped).append(pose) - assert env.latest_pose(store).ts == 123 + assert to_seconds(env.latest_pose(store).header.stamp) == 123 finally: env.stop() client.stop.assert_called_once() diff --git a/dimos/evals/environments/test_habitat.py b/dimos/evals/environments/test_habitat.py index 8bd9e1bc756..d890b7e3525 100644 --- a/dimos/evals/environments/test_habitat.py +++ b/dimos/evals/environments/test_habitat.py @@ -16,14 +16,17 @@ from types import SimpleNamespace from unittest.mock import Mock +from dimos_generated.geometry_msgs.msg import Point, Pose, PoseWithCovariance +from dimos_generated.nav_msgs.msg import Odometry +from dimos_generated.sensor_msgs.msg import Image +from dimos_generated.std_msgs.msg import Header import numpy as np import pytest from dimos.e2e_tests.dimos_cli_call import DimosCliCall from dimos.evals.environments.habitat import HabitatEnvironment -from dimos.msgs.geometry_msgs.Pose import Pose -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.msgs.nav_msgs.Odometry import Odometry +from dimos.msgs.image import image_from_array +from dimos.msgs.time import time_from_seconds def environment(**kwargs): @@ -140,7 +143,6 @@ def test_readiness_failure_releases_resources(tmp_path, mocker): def test_readiness_and_pose_normalization(): from dimos.memory.store.memory import MemoryStore - from dimos.msgs.sensor_msgs.Image import Image env = environment() with MemoryStore() as store: @@ -148,14 +150,21 @@ def test_readiness_and_pose_normalization(): env.latest_pose(store) with pytest.raises(TimeoutError): env.wait_ready(store, deadline=time.monotonic()) - odom = Odometry(frame_id="world", pose=Pose(position=Vector3(1, 2, 3))) + odom = Odometry( + header=Header(frame_id="world", stamp=time_from_seconds(time.time())), + pose=PoseWithCovariance(pose=Pose(position=Point(x=1, y=2, z=3))), + ) store.stream("odometry", Odometry).append(odom) - store.stream("color_image", Image).append(Image(np.zeros((2, 2, 3), dtype=np.uint8))) + store.stream("color_image", Image).append( + image_from_array( + np.zeros((2, 2, 3), dtype=np.uint8), encoding="rgb8", header=odom.header + ) + ) env.wait_ready(store, deadline=time.monotonic() + 1) pose = env.latest_pose(store) - assert pose.ts == odom.ts - assert pose.frame_id == "world" - assert tuple(pose.position) == (1, 2, 3) + assert pose.header.stamp == odom.header.stamp + assert pose.header.frame_id == "world" + assert (pose.pose.position.x, pose.pose.position.y, pose.pose.position.z) == (1, 2, 3) assert env.episode_metadata()["initial_observed_position_ros"] == [1, 2, 3] diff --git a/dimos/evals/suites/dimsim_house.py b/dimos/evals/suites/dimsim_house.py index 0a6f8b79566..c043d6da94f 100644 --- a/dimos/evals/suites/dimsim_house.py +++ b/dimos/evals/suites/dimsim_house.py @@ -28,17 +28,19 @@ from __future__ import annotations from collections.abc import Callable +import math from typing import TYPE_CHECKING +from dimos_generated.geometry_msgs.msg import Vector3 + from dimos.evals.environments.dimsim import DimSimEnvironment from dimos.evals.scorers import ramp from dimos.evals.types import EvalCase, Outcome, Suite, recording -from dimos.msgs.geometry_msgs.Vector3 import Vector3 if TYPE_CHECKING: from dimos.e2e_tests.dim_sim_client import DimSimClient -BED = Vector3(-3.567, -1.332, 0.0) +BED = Vector3(x=-3.567, y=-1.332) _HOUSE_TOUR = [ (3.881, 4.803), @@ -69,8 +71,9 @@ def _explore_house(sim: DimSimClient) -> None: import time + from dimos_generated.geometry_msgs.msg import PoseStamped + from dimos.core.transport import LCMTransport - from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped from dimos.simulation.mujoco.direct_cmd_vel_explorer import DirectCmdVelExplorer # dimsim spawns the robot well after MCP is up — wait for odom before driving @@ -98,10 +101,10 @@ def _ended_near(target: Vector3) -> Callable[[Outcome], float]: def grade(o: Outcome) -> float: store = recording(o) try: - p = store.streams.odom.last().data.position + p = store.streams.odom.last().data.pose.position finally: store.stop() - d = Vector3(p.x - target.x, p.y - target.y, 0.0).length() + d = math.hypot(p.x - target.x, p.y - target.y) return ramp(max(0.0, d - 2.0), band=2.0) # full credit inside the e2e 2m threshold return grade diff --git a/dimos/evals/suites/dimsim_pointcloud_mapping.py b/dimos/evals/suites/dimsim_pointcloud_mapping.py index 30522d1828f..8cb30210118 100644 --- a/dimos/evals/suites/dimsim_pointcloud_mapping.py +++ b/dimos/evals/suites/dimsim_pointcloud_mapping.py @@ -61,7 +61,7 @@ def grade(o: Outcome) -> float: count = 0.0 store = recording(o) try: - path = [(p.data.position.x, p.data.position.y) for p in store.streams.odom] + path = [(p.data.pose.position.x, p.data.pose.position.y) for p in store.streams.odom] finally: store.stop() visited = sum( diff --git a/dimos/evals/test_evals.py b/dimos/evals/test_evals.py index 79e8cfabfd0..88fd6c663ff 100644 --- a/dimos/evals/test_evals.py +++ b/dimos/evals/test_evals.py @@ -29,9 +29,12 @@ from types import SimpleNamespace from typing import Any +from dimos_generated.geometry_msgs.msg import Point, Pose, PoseStamped, Quaternion +from dimos_generated.std_msgs.msg import Header from langchain_core.language_models.fake_chat_models import FakeListChatModel from langchain_core.messages import AIMessage, HumanMessage, ToolMessage import numpy as np +from PIL import Image as PILImage from pydantic import ValidationError import pytest from pytest_mock import MockerFixture @@ -41,7 +44,7 @@ from dimos.evals.agents.blind import BLIND_BLOCK, Blind from dimos.evals.agents.lib.trajectory_builder import TrajectoryBuilder from dimos.evals.agents.mcp_client_adapter import McpClientAdapter -from dimos.evals.agents.question_answer import QuestionAnswer +from dimos.evals.agents.question_answer import QuestionAnswer, _observation_blocks from dimos.evals.cli import load_agent from dimos.evals.environments.base import Environment from dimos.evals.environments.dataset import Dataset @@ -77,17 +80,12 @@ ) from dimos.memory.store.memory import MemoryStore from dimos.memory.store.sqlite import SqliteStore -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Vector3 import make_vector3 -from dimos.msgs.sensor_msgs.Image import Image def _pose(x: float, y: float) -> PoseStamped: return PoseStamped( - position=make_vector3(x, y, 0.0), - orientation=Quaternion(0.0, 0.0, 0.0, 1.0), - frame_id="world", + header=Header(frame_id="world"), + pose=Pose(position=Point(x=x, y=y), orientation=Quaternion(w=1.0)), ) @@ -198,7 +196,7 @@ def test_dataset_start_hands_out_the_selection(dataset: str) -> None: assert odom.name == "odom" observations = list(odom) assert [o.ts for o in observations] == [1000.0, 1001.0] - assert observations[0].data.position.x == 0.0 + assert observations[0].data.pose.position.x == 0.0 assert running.mcp_url == "" and not env.has_robot assert running.artifacts["recording"] == Path(dataset) finally: @@ -315,14 +313,14 @@ def test_sim_launches_base_blueprints_and_agent_modules_in_order( def test_image_file_environment(tmp_path: Path) -> None: path = tmp_path / "frame.png" - Image.from_numpy(np.full((8, 8, 3), 200, dtype=np.uint8)).save(str(path)) + PILImage.fromarray(np.full((8, 8, 3), 200, dtype=np.uint8)).save(path) env = ImageFile(path) env.preflight(QuestionAnswer()) running = env.start(()) try: (image_stream,) = running.streams (obs,) = list(image_stream) - assert obs.data.agent_encode()[0]["type"] == "image_url" + assert _observation_blocks(obs, "[t=0.0s]")[1]["type"] == "image_url" assert running.artifacts == {"image": path} finally: env.stop() @@ -664,7 +662,7 @@ def test_recording_helper_opens_the_artifact(dataset: str) -> None: with recording( Outcome(trajectory=_trajectory("", Path()), artifacts={"recording": Path(dataset)}) ) as store: - assert store.streams.odom.last().data.position.x == 4.0 + assert store.streams.odom.last().data.pose.position.x == 4.0 def test_count_rooms_grader_scores_reply_and_coverage(tmp_path: Path) -> None: diff --git a/dimos/evals/test_mem2_wiring.py b/dimos/evals/test_mem2_wiring.py index 1b4fe9c8cd9..39cfaf9ac5a 100644 --- a/dimos/evals/test_mem2_wiring.py +++ b/dimos/evals/test_mem2_wiring.py @@ -26,6 +26,9 @@ from pathlib import Path +from dimos_generated.geometry_msgs.msg import Point, Pose, PoseStamped, Quaternion +from dimos_generated.sensor_msgs.msg import Image +from dimos_generated.std_msgs.msg import Header from langchain_core.language_models.fake_chat_models import FakeListChatModel import numpy as np import pytest @@ -45,17 +48,14 @@ recording, ) from dimos.memory.store.sqlite import SqliteStore -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Vector3 import make_vector3 -from dimos.msgs.sensor_msgs.Image import Image +from dimos.msgs.image import image_from_array +from dimos.msgs.time import time_from_seconds def _pose(x: float, y: float) -> PoseStamped: return PoseStamped( - position=make_vector3(x, y, 0.0), - orientation=Quaternion(0.0, 0.0, 0.0, 1.0), - frame_id="world", + header=Header(frame_id="world"), + pose=Pose(position=Point(x=x, y=y), orientation=Quaternion(w=1.0)), ) @@ -74,7 +74,14 @@ def test_selected_streams_reach_the_prompt( frame = np.full((16, 16, 3), 200, dtype=np.uint8) images = store.stream("color_image", Image) for i in range(3): - images.append(Image.from_numpy(frame, frame_id="cam", ts=1000.0 + i), ts=1000.0 + i) + images.append( + image_from_array( + frame, + encoding="rgb8", + header=Header(frame_id="cam", stamp=time_from_seconds(1000.0 + i)), + ), + ts=1000.0 + i, + ) case = EvalCase( id="wiring", @@ -99,7 +106,8 @@ def test_selected_streams_reach_the_prompt( # the actual observation data crossed from memory into the prompt: assert len(image_blocks) == 2, "both selected image observations should be encoded" assert image_blocks[0]["image_url"]["url"].startswith("data:image/jpeg;base64,") - assert str(_pose(19.0, 2.5)) in text, "last odom pose must reach the prompt" + assert "position=(19.000, 2.500, 0.000)" in text, "last odom pose must reach the prompt" + assert "frame='world'" in text and "quaternion=(0.000, 0.000, 0.000, 1.000)" in text assert case.inputs in text @@ -132,7 +140,11 @@ def test_frames_per_stream_downsamples_not_truncates( for block in content if block["type"] == "text" and block["text"].startswith("[t=") ] - assert stamped == [f"[t={float(i):.1f}s] {_pose(float(i), 0.0)}" for i in positions] + assert stamped == [ + f"[t={float(i):.1f}s] frame='world' position=({float(i):.3f}, 0.000, 0.000) " + "quaternion=(0.000, 0.000, 0.000, 1.000)" + for i in positions + ] def test_grader_reads_the_history_the_environment_recorded( @@ -152,7 +164,7 @@ def run( def grade(outcome: Outcome) -> float: with recording(outcome) as rec: - poses = [obs.data.position.x for obs in rec.streams.odom] + poses = [obs.data.pose.position.x for obs in rec.streams.odom] assert poses == [5.0, 2.0, 1.0, 0.0] return ramp(abs(poses[-1]), band=2.0) diff --git a/dimos/evals/vqa/author.py b/dimos/evals/vqa/author.py index c114b4edcc1..295db4d4366 100644 --- a/dimos/evals/vqa/author.py +++ b/dimos/evals/vqa/author.py @@ -25,8 +25,9 @@ from dimos.evals.vqa.contracts import FamilySpec, QuestionProposal if TYPE_CHECKING: + from dimos_generated.sensor_msgs.msg import Image + from dimos.models.vl.base import VlModel - from dimos.msgs.sensor_msgs.Image import Image class QuestionAuthor(Protocol): diff --git a/dimos/evals/vqa/contracts.py b/dimos/evals/vqa/contracts.py index a5df7351e25..8dbc93e87d6 100644 --- a/dimos/evals/vqa/contracts.py +++ b/dimos/evals/vqa/contracts.py @@ -29,10 +29,11 @@ ) if TYPE_CHECKING: + from dimos_generated.sensor_msgs.msg import Image + from dimos.evals.vqa.pointcloud_frame import PointCloudFrame from dimos.evals.vqa.primitives.edge_tam import ObjectMaskEvidence from dimos.evals.vqa.primitives.range import ObjectRangeEvidence - from dimos.msgs.sensor_msgs.Image import Image NonEmptyString = Annotated[str, StringConstraints(strip_whitespace=True, min_length=1)] FamilyName = Literal[ diff --git a/dimos/evals/vqa/families.py b/dimos/evals/vqa/families.py index f706da2f8d1..6ab8d3c9fc1 100644 --- a/dimos/evals/vqa/families.py +++ b/dimos/evals/vqa/families.py @@ -31,9 +31,10 @@ ) if TYPE_CHECKING: + from dimos_generated.sensor_msgs.msg import Image + from dimos.evals.vqa.pointcloud_frame import PointCloudFrame from dimos.models.vl.base import VlModel - from dimos.msgs.sensor_msgs.Image import Image PRESENCE_FAMILY = FamilySpec( diff --git a/dimos/evals/vqa/generate.py b/dimos/evals/vqa/generate.py index 1c47d8c8004..359152fabbf 100644 --- a/dimos/evals/vqa/generate.py +++ b/dimos/evals/vqa/generate.py @@ -24,6 +24,8 @@ import tempfile from typing import TYPE_CHECKING +from dimos_generated.sensor_msgs.msg import Image +from PIL import Image as PILImage from pydantic import BaseModel, ConfigDict, Field, model_validator from dimos.constants import STATE_DIR @@ -49,7 +51,8 @@ ) from dimos.evals.vqa.primitives.edge_tam import EdgeTAMObjectMaskPipeline from dimos.evals.vqa.primitives.range import LidarRangeEstimator -from dimos.msgs.sensor_msgs.Image import Image +from dimos.msgs.image import image_to_rgb +from dimos.msgs.time import to_seconds if TYPE_CHECKING: from dimos.models.vl.base import VlModel @@ -395,8 +398,7 @@ def _write_frame(output: Path, frame: _GeneratedFrame) -> None: image_name = f"assets/frame-{frame.index:06d}.png" image_path = output / image_name - if not frame.image.save(str(image_path)): - raise RuntimeError(f"failed to write VQA image: {image_path}") + PILImage.fromarray(image_to_rgb(frame.image)).save(image_path) case_rows = [case.model_dump(mode="json") for case in frame.cases] label_rows = [label.model_dump(mode="json") for label in frame.labels] @@ -408,7 +410,7 @@ def _write_frame(output: Path, frame: _GeneratedFrame) -> None: { "frame_index": frame.index, "image": image_name, - "timestamp": frame.image.ts, + "timestamp": to_seconds(frame.image.header.stamp), "question_count": len(frame.cases), "rejected_question_count": sum( row.get("status") == "rejected" for row in frame.audit_rows diff --git a/dimos/evals/vqa/pointcloud_frame.py b/dimos/evals/vqa/pointcloud_frame.py index af7b355c4c2..9ed1527838b 100644 --- a/dimos/evals/vqa/pointcloud_frame.py +++ b/dimos/evals/vqa/pointcloud_frame.py @@ -21,15 +21,17 @@ from types import TracebackType from typing import TYPE_CHECKING, Any, cast +from dimos_generated.geometry_msgs.msg import PoseStamped, TransformStamped +from dimos_generated.sensor_msgs.msg import CameraInfo, Image, PointCloud2 +from dimos_generated.std_msgs.msg import Header import numpy as np from dimos.memory.cli.dataset import open_dataset from dimos.memory.store.base import Store from dimos.memory.tf import StreamTF -from dimos.msgs.geometry_msgs.Transform import Transform -from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo -from dimos.msgs.sensor_msgs.Image import Image -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 +from dimos.msgs.camera_info import camera_info_from_intrinsics +from dimos.msgs.geometry import pose_from_matrix, transform_from_pose, transform_matrix +from dimos.msgs.image import image_from_array, image_view if TYPE_CHECKING: from dimos.memory.stream import Stream @@ -45,7 +47,7 @@ class PointCloudFrame: image: Image pointcloud: PointCloud2 camera_info: CameraInfo - pointcloud_to_camera: Transform + pointcloud_to_camera: TransformStamped image_observation_timestamp: float pointcloud_observation_timestamp: float calibration_source: str @@ -184,8 +186,7 @@ def load(self, frame_index: int) -> PointCloudFrame: image_query = self._images.offset(frame_index).limit(1) image_obs = image_query.first() lidar_obs = _align_one(image_query, self._lidar, self._tolerance_s) - source_image = image_obs.data.copy() - source_image.ts = image_obs.ts + source_image = image_obs.data recorded_camera_info = self._recorded_camera_info recorded_tf = self._recorded_tf @@ -196,19 +197,22 @@ def load(self, frame_index: int) -> PointCloudFrame: pointcloud_to_camera = _recorded_pointcloud_to_camera( image_obs, lidar_obs, - camera_info.frame_id, + camera_info.header.frame_id, recorded_tf, self._tolerance_s, ) rectification = np.eye(4, dtype=np.float64) - rectification[:3, :3] = np.asarray(source_camera_info.R, dtype=np.float64).reshape(3, 3) - pointcloud_to_camera = Transform.from_matrix( - rectification @ pointcloud_to_camera.to_matrix(), - ts=image_obs.ts, - frame_id=camera_info.frame_id, - child_frame_id=lidar_obs.data.frame_id, + rectification[:3, :3] = np.asarray(source_camera_info.r, dtype=np.float64).reshape(3, 3) + pointcloud_to_camera = transform_from_pose( + PoseStamped( + header=Header(stamp=image.header.stamp, frame_id=camera_info.header.frame_id), + pose=pose_from_matrix( + rectification @ transform_matrix(pointcloud_to_camera.transform) + ), + ), + child_frame_id=lidar_obs.data.header.frame_id, ) - if not np.all(np.isfinite(pointcloud_to_camera.to_matrix())): + if not np.all(np.isfinite(transform_matrix(pointcloud_to_camera.transform))): raise PointCloudFrameUnavailableError( "recorded point-cloud-to-camera transform must be finite" ) @@ -231,8 +235,7 @@ def load_image(self, frame_index: int) -> Image: if self._images is None: raise RuntimeError("point-cloud frame loader must be started before loading frames") observation = self._images.offset(frame_index).limit(1).first() - image = observation.data.copy() - image.ts = observation.ts + image = observation.data recorded_camera_info = self._recorded_camera_info if recorded_camera_info is None: raise RuntimeError("camera calibration was not initialized") @@ -262,30 +265,32 @@ def rectify(self, image: Image, source: CameraInfo) -> tuple[Image, CameraInfo]: image.width, image.height, source.distortion_model, - tuple(source.K), - tuple(source.D), - tuple(source.R), - tuple(source.P), + tuple(source.k), + tuple(source.d), + tuple(source.r), + tuple(source.p), ) maps = self._maps.get(key) if maps is None: maps = _rectification_maps(image, source) self._maps[key] = maps map_x, map_y, output_matrix = maps - data = cv2.remap(image.data, map_x, map_y, interpolation=cv2.INTER_LINEAR) - frame_id = source.frame_id or image.frame_id + data = cv2.remap(image_view(image), map_x, map_y, interpolation=cv2.INTER_LINEAR) + frame_id = source.header.frame_id or image.header.frame_id if not frame_id: raise ValueError("camera calibration requires a camera frame_id") - camera_info = CameraInfo.from_intrinsics( + camera_info = camera_info_from_intrinsics( output_matrix[0, 0], output_matrix[1, 1], output_matrix[0, 2], output_matrix[1, 2], image.width, image.height, - frame_id=frame_id, - ).with_ts(image.ts) - return Image(data=data, format=image.format, frame_id=frame_id, ts=image.ts), camera_info + header=Header(stamp=image.header.stamp, frame_id=frame_id), + ) + return image_from_array( + data, encoding=image.encoding, header=camera_info.header + ), camera_info def _align_one( @@ -317,8 +322,8 @@ def _recorded_pointcloud_to_camera( camera_frame: str, tf: TFLookup, tolerance_s: float, -) -> Transform: - cloud_frame = lidar_obs.data.frame_id +) -> TransformStamped: + cloud_frame = lidar_obs.data.header.frame_id if not cloud_frame: raise PointCloudFrameUnavailableError("recorded point cloud requires a frame_id") camera_from_cloud = tf.get( @@ -341,15 +346,15 @@ def _rectification_maps( # OpenCV is intentionally lazy because importing it loads a large native library. import cv2 - intrinsics = np.asarray(source.K, dtype=np.float64) + intrinsics = np.asarray(source.k, dtype=np.float64) if intrinsics.size != 9: raise ValueError("camera intrinsics must contain nine values") matrix = intrinsics.reshape(3, 3) if not np.all(np.isfinite(matrix)) or matrix[0, 0] <= 0 or matrix[1, 1] <= 0: raise ValueError("camera intrinsics must be finite with positive focal lengths") - distortion = np.asarray(source.D, dtype=np.float64) - rectification = np.asarray(source.R, dtype=np.float64).reshape(3, 3) - projection = np.asarray(source.P, dtype=np.float64).reshape(3, 4)[:, :3] + distortion = np.asarray(source.d, dtype=np.float64) + rectification = np.asarray(source.r, dtype=np.float64).reshape(3, 3) + projection = np.asarray(source.p, dtype=np.float64).reshape(3, 4)[:, :3] output_matrix = projection if projection[0, 0] > 0 and projection[1, 1] > 0 else matrix size = (image.width, image.height) model = source.distortion_model.strip().lower() diff --git a/dimos/evals/vqa/primitives/edge_tam.py b/dimos/evals/vqa/primitives/edge_tam.py index dfb0f7ac831..d1e59a2f557 100644 --- a/dimos/evals/vqa/primitives/edge_tam.py +++ b/dimos/evals/vqa/primitives/edge_tam.py @@ -26,9 +26,10 @@ from dimos.perception.detection.type.detection2d.imageDetections2D import ImageDetections2D if TYPE_CHECKING: + from dimos_generated.sensor_msgs.msg import Image + from dimos.models.segmentation.edge_tam import EdgeTAMImageSegmenterCompatible from dimos.models.vl.base import VlModel - from dimos.msgs.sensor_msgs.Image import Image @dataclass(frozen=True) diff --git a/dimos/evals/vqa/primitives/range.py b/dimos/evals/vqa/primitives/range.py index 7677d6180e8..b13393f6d4f 100644 --- a/dimos/evals/vqa/primitives/range.py +++ b/dimos/evals/vqa/primitives/range.py @@ -23,6 +23,8 @@ from pydantic import BaseModel, ConfigDict, Field from dimos.evals.vqa.contracts import InsufficientEvidenceError, NonEmptyString +from dimos.msgs.geometry import transform_matrix +from dimos.msgs.pointcloud import pointcloud_xyz from dimos.perception.detection.type.detection2d.bbox import Bbox if TYPE_CHECKING: @@ -41,13 +43,15 @@ class _FrameProjection: def from_frame(cls, frame: PointCloudFrame) -> _FrameProjection: """Transform and project valid points into image coordinates.""" camera_info = frame.camera_info - intrinsics = np.asarray(camera_info.K, dtype=np.float64).reshape(3, 3) + intrinsics = np.asarray(camera_info.k, dtype=np.float64).reshape(3, 3) - raw_points, _ = frame.pointcloud.as_numpy() + raw_points = pointcloud_xyz(frame.pointcloud) points = np.asarray(raw_points, dtype=np.float64) points = points[np.all(np.isfinite(points), axis=1)] - transform = np.asarray(frame.pointcloud_to_camera.to_matrix(), dtype=np.float64) + transform = np.asarray( + transform_matrix(frame.pointcloud_to_camera.transform), dtype=np.float64 + ) homogeneous = np.column_stack((points, np.ones(len(points), dtype=np.float64))) camera_points = (transform @ homogeneous.T).T[:, :3] in_front = np.all(np.isfinite(camera_points), axis=1) & (camera_points[:, 2] > 0) diff --git a/dimos/evals/vqa/primitives/test_edge_tam.py b/dimos/evals/vqa/primitives/test_edge_tam.py index a29eeab2175..48fcabe3c42 100644 --- a/dimos/evals/vqa/primitives/test_edge_tam.py +++ b/dimos/evals/vqa/primitives/test_edge_tam.py @@ -14,38 +14,46 @@ from __future__ import annotations -from typing import TYPE_CHECKING, cast +from collections import Counter +from dimos_generated.geometry_msgs.msg import Quaternion, Transform, TransformStamped, Vector3 +from dimos_generated.sensor_msgs.msg import Image, PointCloud2 +from dimos_generated.std_msgs.msg import Header import numpy as np import pytest from dimos.evals.vqa.contracts import InsufficientEvidenceError from dimos.evals.vqa.pointcloud_frame import PointCloudFrame +from dimos.evals.vqa.primitives import range as range_primitives from dimos.evals.vqa.primitives.edge_tam import EdgeTAMObjectMaskPipeline from dimos.evals.vqa.primitives.range import LidarRangeEstimator from dimos.models.vl.base import VlModel -from dimos.msgs.geometry_msgs.Transform import Transform -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo -from dimos.msgs.sensor_msgs.Image import Image +from dimos.msgs.camera_info import camera_info_from_intrinsics +from dimos.msgs.image import image_from_array +from dimos.msgs.pointcloud import pointcloud_from_xyz +from dimos.msgs.time import time_from_seconds, to_seconds from dimos.perception.detection.type.detection2d.bbox import Bbox, Detection2DBBox from dimos.perception.detection.type.detection2d.imageDetections2D import ImageDetections2D from dimos.perception.detection.type.detection2d.seg import Detection2DSeg -if TYPE_CHECKING: - from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 + +def _cloud(points: np.ndarray) -> PointCloud2: + return pointcloud_from_xyz( + points, header=Header(frame_id="lidar", stamp=time_from_seconds(9.98)) + ) -class _PointCloud: - def __init__(self, points: np.ndarray) -> None: - self._points = points - self.read_count = 0 - self.frame_id = "lidar" - self.ts = 9.98 +@pytest.fixture +def cloud_reads(monkeypatch: pytest.MonkeyPatch) -> Counter[int]: + reads: Counter[int] = Counter() + original = range_primitives.pointcloud_xyz - def as_numpy(self) -> tuple[np.ndarray, None]: - self.read_count += 1 - return self._points, None + def counted(cloud: PointCloud2) -> np.ndarray: + reads[id(cloud)] += 1 + return original(cloud) + + monkeypatch.setattr(range_primitives, "pointcloud_xyz", counted) + return reads class _TestVlModel(VlModel): @@ -73,7 +81,7 @@ def query_detections( class_id=0, confidence=1.0, name=query, - ts=image.ts, + ts=to_seconds(image.header.stamp), image=image, ) for index, box in enumerate(self._boxes) @@ -98,7 +106,7 @@ def query_detections( class_id=0, confidence=1.0, name=query, - ts=image.ts, + ts=to_seconds(image.header.stamp), image=image, ) ], @@ -138,7 +146,7 @@ def segment( def _frame( - pointcloud: _PointCloud, + pointcloud: PointCloud2, *, index: int = 0, transform: Transform | None = None, @@ -147,13 +155,21 @@ def _frame( cx: float = 5.0, cy: float = 5.0, ) -> PointCloudFrame: - image = Image(data=np.zeros((10, 10, 3), dtype=np.uint8), ts=10.0) + image = image_from_array( + np.zeros((10, 10, 3), dtype=np.uint8), + encoding="rgb8", + header=Header(stamp=time_from_seconds(10)), + ) return PointCloudFrame( index=index, image=image, - pointcloud=cast("PointCloud2", pointcloud), - camera_info=CameraInfo.from_intrinsics(fx, fy, cx, cy, 10, 10), - pointcloud_to_camera=transform or Transform.identity(), + pointcloud=pointcloud, + camera_info=camera_info_from_intrinsics(fx, fy, cx, cy, 10, 10, header=Header()), + pointcloud_to_camera=TransformStamped( + header=Header(frame_id="optical"), + child_frame_id="lidar", + transform=transform or Transform(rotation=Quaternion(w=1)), + ), image_observation_timestamp=10.0, pointcloud_observation_timestamp=9.98, calibration_source="synthetic-calibration", @@ -176,7 +192,11 @@ def _range_estimator( def test_mask_estimator_batches_and_caches_without_pointcloud() -> None: - image = Image(data=np.zeros((10, 10, 3), dtype=np.uint8), ts=10.0) + image = image_from_array( + np.zeros((10, 10, 3), dtype=np.uint8), + encoding="rgb8", + header=Header(stamp=time_from_seconds(10)), + ) left_mask = np.zeros((10, 10), dtype=np.uint8) right_mask = np.zeros((10, 10), dtype=np.uint8) left_mask[:, :4] = 1 @@ -200,14 +220,18 @@ def test_mask_estimator_batches_and_caches_without_pointcloud() -> None: assert cached is left assert segmenter.call_count == 1 - next_image = Image(data=np.zeros((10, 10, 3), dtype=np.uint8), ts=11.0) + next_image = image_from_array( + np.zeros((10, 10, 3), dtype=np.uint8), + encoding="rgb8", + header=Header(stamp=time_from_seconds(11)), + ) estimator.estimate(next_image, "left person") assert segmenter.call_count == 2 def test_projects_transformed_points_and_rejects_invalid_projections() -> None: - pointcloud = _PointCloud( + pointcloud = _cloud( np.array( [ [0.0, 0.0, 1.0], @@ -218,7 +242,7 @@ def test_projects_transformed_points_and_rejects_invalid_projections() -> None: ) frame = _frame( pointcloud, - transform=Transform(translation=Vector3(1.0, 0.0, 0.0)), + transform=Transform(translation=Vector3(x=1.0)), cx=0.0, cy=0.0, ) @@ -234,7 +258,7 @@ def test_projects_transformed_points_and_rejects_invalid_projections() -> None: def test_mask_selection_uses_nearest_camera_z_point_per_pixel() -> None: - pointcloud = _PointCloud( + pointcloud = _cloud( np.array( [ [0.0, 0.0, 1.0], @@ -256,18 +280,18 @@ def test_mask_selection_uses_nearest_camera_z_point_per_pixel() -> None: @pytest.mark.parametrize("boxes", [[], [(0.0, 0.0, 4.0, 4.0), (5.0, 5.0, 9.0, 9.0)]]) -def test_requires_exactly_one_valid_detection(boxes: list[Bbox]) -> None: - cloud = _PointCloud(np.array([[0.0, 0.0, 1.0]])) +def test_requires_exactly_one_valid_detection(boxes: list[Bbox], cloud_reads: Counter[int]) -> None: + cloud = _cloud(np.array([[0.0, 0.0, 1.0]])) estimator = _range_estimator(_Detector(boxes), _Segmenter(_full_mask())) with pytest.raises(InsufficientEvidenceError, match="exactly one valid detected"): estimator.estimate(_frame(cloud), "cup") - assert cloud.read_count == 0 + assert cloud_reads[id(cloud)] == 0 def test_rejects_too_few_mask_supporting_points() -> None: - pointcloud = _PointCloud( + pointcloud = _cloud( np.array([[-2.0, 0.0, 1.0], [-1.5, 0.0, 1.0], [-1.0, 0.0, 1.0], [-0.5, 0.0, 1.0]]) ) estimator = _range_estimator(_Detector([(0.0, 0.0, 10.0, 10.0)]), _Segmenter(_full_mask())) @@ -284,13 +308,13 @@ def test_rejects_segmentation_mask_with_wrong_dimensions() -> None: ) with pytest.raises(InsufficientEvidenceError, match="one valid segmentation mask"): - estimator.estimate(_frame(_PointCloud(np.array([[0.0, 0.0, 1.0]]))), "bottle") + estimator.estimate(_frame(_cloud(np.array([[0.0, 0.0, 1.0]]))), "bottle") def test_returns_median_euclidean_range_and_auditable_quartiles() -> None: points = np.array([[u * z, 0.0, z] for u, z in zip(range(1, 6), [1, 2, 3, 4, 20], strict=True)]) expected_ranges = np.linalg.norm(points, axis=1) - frame = _frame(_PointCloud(points), fx=1.0, fy=1.0, cx=0.0, cy=0.0) + frame = _frame(_cloud(points), fx=1.0, fy=1.0, cx=0.0, cy=0.0) evidence = _range_estimator( _Detector([(0.0, 0.0, 9.0, 9.0)]), _Segmenter(_full_mask()) @@ -306,10 +330,12 @@ def test_returns_median_euclidean_range_and_auditable_quartiles() -> None: assert evidence.model_dump(mode="json")["mask_bbox_xyxy"] == [0.0, 0.0, 9.0, 9.0] -def test_projection_cache_reuses_across_objects_only_for_the_same_explicit_frame() -> None: +def test_projection_cache_reuses_across_objects_only_for_the_same_explicit_frame( + cloud_reads: Counter[int], +) -> None: points = np.array([[float(2 * x), 0.0, 2.0] for x in range(5)]) - first_cloud = _PointCloud(points) - second_cloud = _PointCloud(points) + first_cloud = _cloud(points) + second_cloud = _cloud(points) first_frame = _frame(first_cloud, index=3, fx=1.0, fy=1.0, cx=0.0, cy=0.0) second_frame = _frame(second_cloud, index=3, fx=1.0, fy=1.0, cx=0.0, cy=0.0) estimator = _range_estimator(_Detector([(0.0, 0.0, 9.0, 9.0)]), _Segmenter(_full_mask())) @@ -318,12 +344,12 @@ def test_projection_cache_reuses_across_objects_only_for_the_same_explicit_frame estimator.estimate(first_frame, "crate") estimator.estimate(second_frame, "box") - assert first_cloud.read_count == 1 - assert second_cloud.read_count == 1 + assert cloud_reads[id(first_cloud)] == 1 + assert cloud_reads[id(second_cloud)] == 1 -def test_estimate_many_batches_masks_and_reuses_one_projection() -> None: - cloud = _PointCloud(np.array([[-1.5, 0.0, 1.0], [3.0, 0.0, 3.0]])) +def test_estimate_many_batches_masks_and_reuses_one_projection(cloud_reads: Counter[int]) -> None: + cloud = _cloud(np.array([[-1.5, 0.0, 1.0], [3.0, 0.0, 3.0]])) left_mask = np.zeros((10, 10), dtype=np.uint8) right_mask = np.zeros((10, 10), dtype=np.uint8) left_mask[5, 2] = 1 @@ -346,4 +372,4 @@ def test_estimate_many_batches_masks_and_reuses_one_projection() -> None: assert right.object_name == "table" assert left.camera_range_m < right.camera_range_m assert segmenter.call_count == 1 - assert cloud.read_count == 1 + assert cloud_reads[id(cloud)] == 1 diff --git a/dimos/evals/vqa/test_contracts.py b/dimos/evals/vqa/test_contracts.py index e0ad255ed22..79fbd9807b5 100644 --- a/dimos/evals/vqa/test_contracts.py +++ b/dimos/evals/vqa/test_contracts.py @@ -18,7 +18,10 @@ import json from pathlib import Path +from dimos_generated.sensor_msgs.msg import Image +from dimos_generated.std_msgs.msg import Header import numpy as np +from PIL import Image as PILImage from pydantic import ValidationError import pytest @@ -52,11 +55,16 @@ from dimos.evals.vqa.primitives.range import ObjectRangeEvidence from dimos.evals.vqa.suite import load_suite from dimos.models.vl.base import VlModel -from dimos.msgs.sensor_msgs.Image import Image +from dimos.msgs.image import image_from_array, image_view +from dimos.msgs.time import time_from_seconds, to_seconds from dimos.perception.detection.type.detection2d.bbox import Bbox, Detection2DBBox from dimos.perception.detection.type.detection2d.imageDetections2D import ImageDetections2D +def _image(pixels: np.ndarray, *, ts: float = 0.0) -> Image: + return image_from_array(pixels, encoding="rgb8", header=Header(stamp=time_from_seconds(ts))) + + class _TestVlModel(VlModel): def __init__(self) -> None: pass @@ -148,7 +156,7 @@ def propose(self, image: Image, families: Sequence[FamilySpec]) -> Sequence[Ques class _EmptyThenAuthor: def propose(self, image: Image, families: Sequence[FamilySpec]) -> Sequence[QuestionProposal]: - if image.ts == 1.0: + if to_seconds(image.header.stamp) == 1.0: return () return (QuestionProposal(family="presence", object_names=("chair",)),) @@ -178,7 +186,7 @@ def query_detections( class_id=-1, confidence=1.0, name=query, - ts=image.ts, + ts=to_seconds(image.header.stamp), image=image, ) ) @@ -201,7 +209,7 @@ def query_detections( class_id=-1, confidence=1.0, name=query, - ts=image.ts, + ts=to_seconds(image.header.stamp), image=image, ) for index, box in enumerate(self._boxes) @@ -310,7 +318,7 @@ def estimate(self, image: Image, object_name: str) -> ObjectMaskEvidence: class_id=-1, confidence=1.0, name=object_name, - ts=image.ts, + ts=to_seconds(image.header.stamp), image=image, ) return ObjectMaskEvidence( @@ -439,7 +447,7 @@ def test_closest_object_proposal_requires_two_to_five_distinct_references() -> N def test_object_distance_requires_range_evidence() -> None: - image = Image.from_numpy(np.zeros((4, 4, 3), dtype=np.uint8)) + image = _image(np.zeros((4, 4, 3), dtype=np.uint8)) proposal = QuestionProposal(family="object_distance", object_names=("chair",)) with pytest.raises(InsufficientEvidenceError, match="image-aligned point-cloud"): @@ -456,7 +464,7 @@ def test_object_distance_requires_range_evidence() -> None: ), ) def test_object_distance_buckets_range_evidence(range_m: float, expected: str) -> None: - image = Image.from_numpy(np.zeros((4, 4, 3), dtype=np.uint8)) + image = _image(np.zeros((4, 4, 3), dtype=np.uint8)) proposal = QuestionProposal(family="object_distance", object_names=("chair",)) answer = answer_question( @@ -473,7 +481,7 @@ def test_object_distance_buckets_range_evidence(range_m: float, expected: str) - def test_object_distance_rejects_evidence_crossing_bucket_boundary() -> None: - image = Image.from_numpy(np.zeros((4, 4, 3), dtype=np.uint8)) + image = _image(np.zeros((4, 4, 3), dtype=np.uint8)) proposal = QuestionProposal(family="object_distance", object_names=("chair",)) with pytest.raises(InsufficientEvidenceError, match="uncertainty crosses"): @@ -503,7 +511,7 @@ def test_closest_object_compares_multiple_named_references( quartiles: tuple[tuple[float, float, float], ...], expected: str, ) -> None: - image = Image.from_numpy(np.zeros((4, 10, 3), dtype=np.uint8)) + image = _image(np.zeros((4, 10, 3), dtype=np.uint8)) proposal = QuestionProposal( family="closest_object", object_names=("left person", "right person", "chair"), @@ -524,7 +532,7 @@ def test_closest_object_compares_multiple_named_references( def test_closest_object_rejects_overlapping_range_intervals() -> None: - image = Image.from_numpy(np.zeros((4, 10, 3), dtype=np.uint8)) + image = _image(np.zeros((4, 10, 3), dtype=np.uint8)) proposal = QuestionProposal( family="closest_object", object_names=("left person", "right person", "chair"), @@ -553,7 +561,7 @@ def test_closest_object_rejects_reordered_range_results() -> None: with pytest.raises(ValueError, match="requested object order"): answer_question( proposal, - Image.from_numpy(np.zeros((4, 10, 3), dtype=np.uint8)), + _image(np.zeros((4, 10, 3), dtype=np.uint8)), _Detector(present=True), object(), _ReorderedClosestRangeEstimator((1.0, 1.1, 1.2), (2.0, 2.1, 2.2)), @@ -586,7 +594,7 @@ def test_family_answer_requires_case_insensitive_unique_choices() -> None: def test_presence_family_derives_answer_from_detector_evidence() -> None: - image = Image.from_numpy(np.zeros((4, 4, 3), dtype=np.uint8)) + image = _image(np.zeros((4, 4, 3), dtype=np.uint8)) proposal = QuestionProposal(family="presence", object_names=("chair",)) present = answer_question(proposal, image, _Detector(present=True)) @@ -610,7 +618,7 @@ def test_presence_family_derives_answer_from_detector_evidence() -> None: ), ) def test_horizontal_direction_uses_detection_center(box: Bbox, expected: str) -> None: - image = Image.from_numpy(np.zeros((60, 90, 3), dtype=np.uint8)) + image = _image(np.zeros((60, 90, 3), dtype=np.uint8)) proposal = QuestionProposal(family="horizontal_direction", object_names=("robot",)) answer = answer_question(proposal, image, _BoxesDetector((box,))) @@ -621,7 +629,7 @@ def test_horizontal_direction_uses_detection_center(box: Bbox, expected: str) -> def test_horizontal_direction_rejects_ambiguous_instances() -> None: - image = Image.from_numpy(np.zeros((60, 90, 3), dtype=np.uint8)) + image = _image(np.zeros((60, 90, 3), dtype=np.uint8)) proposal = QuestionProposal(family="horizontal_direction", object_names=("chair",)) detector = _BoxesDetector( ( @@ -639,7 +647,7 @@ def test_horizontal_direction_rejects_ambiguous_instances() -> None: ((1, "one"), (2, "two"), (3, "three"), (4, "four or more"), (5, "four or more")), ) def test_object_count_buckets_detected_instances(count: int, expected: str) -> None: - image = Image.from_numpy(np.zeros((60, 90, 3), dtype=np.uint8)) + image = _image(np.zeros((60, 90, 3), dtype=np.uint8)) proposal = QuestionProposal(family="object_count", object_names=("box",)) boxes = tuple((float(index), 0.0, float(index + 1), 1.0) for index in range(count)) @@ -652,7 +660,7 @@ def test_object_count_buckets_detected_instances(count: int, expected: str) -> N def test_object_count_requires_at_least_one_detection() -> None: - image = Image.from_numpy(np.zeros((60, 90, 3), dtype=np.uint8)) + image = _image(np.zeros((60, 90, 3), dtype=np.uint8)) proposal = QuestionProposal(family="object_count", object_names=("box",)) with pytest.raises(InsufficientEvidenceError, match="to count"): @@ -672,7 +680,7 @@ def test_image_coverage_chooses_scheme_with_largest_boundary_margin( boundaries: list[float], margin: float, ) -> None: - image = Image.from_numpy(np.zeros((10, 10, 3), dtype=np.uint8)) + image = _image(np.zeros((10, 10, 3), dtype=np.uint8)) proposal = QuestionProposal(family="image_coverage", object_names=("chair",)) answer = answer_question( @@ -694,7 +702,7 @@ def test_image_coverage_requires_mask_evidence() -> None: with pytest.raises(InsufficientEvidenceError, match="segmentation-mask"): answer_question( proposal, - Image.from_numpy(np.zeros((10, 10, 3), dtype=np.uint8)), + _image(np.zeros((10, 10, 3), dtype=np.uint8)), _Detector(present=True), ) @@ -710,7 +718,7 @@ def test_largest_visible_area_requires_distinct_references() -> None: def test_largest_visible_area_accepts_winner_at_twenty_percent_margin() -> None: - image = Image.from_numpy(np.zeros((10, 10, 3), dtype=np.uint8)) + image = _image(np.zeros((10, 10, 3), dtype=np.uint8)) proposal = QuestionProposal( family="largest_visible_area", object_names=("chair", "table", "box"), @@ -729,7 +737,7 @@ def test_largest_visible_area_accepts_winner_at_twenty_percent_margin() -> None: def test_largest_visible_area_rejects_close_mask_areas() -> None: - image = Image.from_numpy(np.zeros((10, 10, 3), dtype=np.uint8)) + image = _image(np.zeros((10, 10, 3), dtype=np.uint8)) proposal = QuestionProposal( family="largest_visible_area", object_names=("chair", "table"), @@ -745,7 +753,7 @@ def test_largest_visible_area_rejects_close_mask_areas() -> None: def test_openai_author_parses_constrained_proposals() -> None: - image = Image.from_numpy(np.zeros((4, 4, 3), dtype=np.uint8)) + image = _image(np.zeros((4, 4, 3), dtype=np.uint8)) author = OpenAIQuestionAuthor(_QuestionModel()) proposals = author.propose(image, AVAILABLE_FAMILIES) @@ -768,7 +776,7 @@ def test_openai_author_parses_constrained_proposals() -> None: def test_openai_author_keeps_valid_items_from_partly_invalid_response() -> None: - image = Image.from_numpy(np.zeros((4, 4, 3), dtype=np.uint8)) + image = _image(np.zeros((4, 4, 3), dtype=np.uint8)) author = OpenAIQuestionAuthor(_PartlyInvalidQuestionModel()) proposals = author.propose(image, AVAILABLE_FAMILIES) @@ -777,7 +785,7 @@ def test_openai_author_keeps_valid_items_from_partly_invalid_response() -> None: def test_openai_author_skips_unavailable_family() -> None: - image = Image.from_numpy(np.zeros((4, 4, 3), dtype=np.uint8)) + image = _image(np.zeros((4, 4, 3), dtype=np.uint8)) author = OpenAIQuestionAuthor(_UnavailableFamilyQuestionModel()) proposals = author.propose(image, (AVAILABLE_FAMILIES[0],)) @@ -941,7 +949,7 @@ def test_suite_reports_empty_cases_before_duplicate_labels(tmp_path: Path) -> No def test_generate_and_evaluate_one_image(tmp_path: Path) -> None: output = tmp_path / "nested" / "vqa" - image = Image.from_numpy(np.arange(48, dtype=np.uint8).reshape(4, 4, 3), ts=12.5) + image = _image(np.arange(48, dtype=np.uint8).reshape(4, 4, 3), ts=12.5) request = GenerationRequest(dataset="recording.db", image_index=4, output=output) result = generate_frames_dataset( @@ -954,7 +962,7 @@ def test_generate_and_evaluate_one_image(tmp_path: Path) -> None: assert result.cases[0].id == "frame-000004-chair-presence" image_path = output / "assets" / "frame-000004.png" assert image_path.is_file() - assert np.array_equal(Image.from_file(image_path).data, image.data) + assert np.array_equal(np.asarray(PILImage.open(image_path)), image_view(image)) assert (output / "cases.jsonl").is_file() assert (output / "labels.jsonl").is_file() assert (output / "audit" / "frame-000004" / "ground_truth.json").is_file() @@ -978,7 +986,7 @@ def test_generation_rejects_nonempty_output_without_modifying_it(tmp_path: Path) existing = output / "existing.txt" existing.write_text("keep me") request = GenerationRequest(dataset="recording.db", image_index=4, output=output) - image = Image.from_numpy(np.zeros((4, 4, 3), dtype=np.uint8)) + image = _image(np.zeros((4, 4, 3), dtype=np.uint8)) with pytest.raises(FileExistsError, match="output directory is not empty"): generate_frames_dataset( @@ -994,7 +1002,7 @@ def test_generation_rejects_nonempty_output_without_modifying_it(tmp_path: Path) def test_generation_audits_invalid_family_proposals_and_continues(tmp_path: Path) -> None: output = tmp_path / "vqa" - image = Image.from_numpy(np.zeros((4, 4, 3), dtype=np.uint8)) + image = _image(np.zeros((4, 4, 3), dtype=np.uint8)) request = GenerationRequest(dataset="recording.db", image_index=4, output=output) result = generate_frames_dataset( @@ -1012,7 +1020,7 @@ def test_generation_audits_invalid_family_proposals_and_continues(tmp_path: Path def test_generate_distance_case_from_pointcloud_frame(tmp_path: Path) -> None: output = tmp_path / "vqa" - image = Image.from_numpy(np.zeros((4, 4, 3), dtype=np.uint8), ts=12.5) + image = _image(np.zeros((4, 4, 3), dtype=np.uint8), ts=12.5) request = GenerationRequest(dataset="recording.db", image_index=4, output=output) result = generate_frames_dataset( @@ -1036,7 +1044,7 @@ def test_generate_distance_case_from_pointcloud_frame(tmp_path: Path) -> None: def test_generate_closest_object_case_from_pointcloud_frame(tmp_path: Path) -> None: output = tmp_path / "vqa" - image = Image.from_numpy(np.zeros((4, 10, 3), dtype=np.uint8), ts=12.5) + image = _image(np.zeros((4, 10, 3), dtype=np.uint8), ts=12.5) request = GenerationRequest(dataset="recording.db", image_index=4, output=output) result = generate_frames_dataset( @@ -1065,7 +1073,7 @@ def test_generate_closest_object_case_from_pointcloud_frame(tmp_path: Path) -> N def test_generate_image_coverage_case_without_pointcloud(tmp_path: Path) -> None: output = tmp_path / "vqa" - image = Image.from_numpy(np.zeros((10, 10, 3), dtype=np.uint8), ts=12.5) + image = _image(np.zeros((10, 10, 3), dtype=np.uint8), ts=12.5) result = generate_frames_dataset( GenerationRequest(dataset="recording.db", image_index=4, output=output), @@ -1086,7 +1094,7 @@ def test_generation_deduplicates_reordered_closest_object_references(tmp_path: P ( GenerationFrame( 4, - Image.from_numpy(np.zeros((4, 10, 3), dtype=np.uint8)), + _image(np.zeros((4, 10, 3), dtype=np.uint8)), object(), ), ), @@ -1108,7 +1116,7 @@ def test_generation_deduplicates_reordered_closest_object_references(tmp_path: P def test_generation_deduplicates_reordered_largest_area_references(tmp_path: Path) -> None: result = generate_frames_dataset( GenerationRequest(dataset="recording.db", image_index=4, output=tmp_path / "vqa"), - (GenerationFrame(4, Image.from_numpy(np.zeros((10, 10, 3), dtype=np.uint8))),), + (GenerationFrame(4, _image(np.zeros((10, 10, 3), dtype=np.uint8))),), _DuplicateLargestVisibleAreaAuthor(), _Detector(present=True), mask_estimator=_MaskEstimator({"chair": 60, "table": 40, "box": 10}), @@ -1120,7 +1128,7 @@ def test_generation_deduplicates_reordered_largest_area_references(tmp_path: Pat def test_frame_without_pointcloud_does_not_expose_distance_family(tmp_path: Path) -> None: output = tmp_path / "vqa" - image = Image.from_numpy(np.zeros((4, 4, 3), dtype=np.uint8), ts=12.5) + image = _image(np.zeros((4, 4, 3), dtype=np.uint8), ts=12.5) request = GenerationRequest(dataset="recording.db", image_index=4, output=output) author = _RecordingAuthor() @@ -1141,7 +1149,7 @@ def test_frame_without_pointcloud_exposes_mask_families_when_masks_are_available tmp_path: Path, ) -> None: output = tmp_path / "vqa" - image = Image.from_numpy(np.zeros((10, 10, 3), dtype=np.uint8), ts=12.5) + image = _image(np.zeros((10, 10, 3), dtype=np.uint8), ts=12.5) author = _RecordingAuthor() generate_frames_dataset( @@ -1171,8 +1179,8 @@ def test_generate_multiple_images_aggregates_frame_artifacts(tmp_path: Path) -> output=output, ) frames = ( - GenerationFrame(1, Image.from_numpy(np.zeros((4, 4, 3), dtype=np.uint8), ts=1.0)), - GenerationFrame(3, Image.from_numpy(np.zeros((4, 4, 3), dtype=np.uint8), ts=3.0)), + GenerationFrame(1, _image(np.zeros((4, 4, 3), dtype=np.uint8), ts=1.0)), + GenerationFrame(3, _image(np.zeros((4, 4, 3), dtype=np.uint8), ts=3.0)), ) result = generate_frames_dataset( @@ -1209,8 +1217,8 @@ def test_empty_frame_does_not_count_as_rejected_question(tmp_path: Path) -> None output = tmp_path / "vqa" request = GenerationRequest(dataset="recording.db", start=1, stop=3, output=output) frames = ( - GenerationFrame(1, Image.from_numpy(np.zeros((4, 4, 3), dtype=np.uint8), ts=1.0)), - GenerationFrame(2, Image.from_numpy(np.zeros((4, 4, 3), dtype=np.uint8), ts=2.0)), + GenerationFrame(1, _image(np.zeros((4, 4, 3), dtype=np.uint8), ts=1.0)), + GenerationFrame(2, _image(np.zeros((4, 4, 3), dtype=np.uint8), ts=2.0)), ) generate_frames_dataset( @@ -1228,7 +1236,7 @@ def test_empty_frame_does_not_count_as_rejected_question(tmp_path: Path) -> None def test_generation_keeps_answered_questions_and_audits_rejections(tmp_path: Path) -> None: output = tmp_path / "vqa" - image = Image.from_numpy(np.zeros((60, 90, 3), dtype=np.uint8), ts=12.5) + image = _image(np.zeros((60, 90, 3), dtype=np.uint8), ts=12.5) request = GenerationRequest(dataset="recording.db", image_index=4, output=output) detector = _BoxesDetector( ( @@ -1252,7 +1260,7 @@ def test_generation_keeps_answered_questions_and_audits_rejections(tmp_path: Pat def test_generation_propagates_detector_failures(tmp_path: Path) -> None: output = tmp_path / "vqa" - image = Image.from_numpy(np.zeros((4, 4, 3), dtype=np.uint8)) + image = _image(np.zeros((4, 4, 3), dtype=np.uint8)) request = GenerationRequest(dataset="recording.db", image_index=4, output=output) with pytest.raises(ValueError, match="detector broke"): @@ -1270,8 +1278,8 @@ def test_later_frame_failure_does_not_publish_partial_dataset(tmp_path: Path) -> output = tmp_path / "vqa" request = GenerationRequest(dataset="recording.db", start=1, stop=3, output=output) frames = ( - GenerationFrame(1, Image.from_numpy(np.zeros((4, 4, 3), dtype=np.uint8), ts=1.0)), - GenerationFrame(2, Image.from_numpy(np.zeros((4, 4, 3), dtype=np.uint8), ts=2.0)), + GenerationFrame(1, _image(np.zeros((4, 4, 3), dtype=np.uint8), ts=1.0)), + GenerationFrame(2, _image(np.zeros((4, 4, 3), dtype=np.uint8), ts=2.0)), ) with pytest.raises(ValueError, match="detector broke"): @@ -1287,7 +1295,7 @@ def test_later_frame_failure_does_not_publish_partial_dataset(tmp_path: Path) -> def test_generation_deduplicates_proposals_and_suffixes_id_collisions(tmp_path: Path) -> None: output = tmp_path / "vqa" - image = Image.from_numpy(np.zeros((4, 4, 3), dtype=np.uint8)) + image = _image(np.zeros((4, 4, 3), dtype=np.uint8)) request = GenerationRequest(dataset="recording.db", image_index=4, output=output) result = generate_frames_dataset( diff --git a/dimos/evals/vqa/test_pointcloud_frame.py b/dimos/evals/vqa/test_pointcloud_frame.py index bdb262edb31..119af072cff 100644 --- a/dimos/evals/vqa/test_pointcloud_frame.py +++ b/dimos/evals/vqa/test_pointcloud_frame.py @@ -14,6 +14,10 @@ from pathlib import Path +from dimos_generated.geometry_msgs.msg import Quaternion, Transform, TransformStamped, Vector3 +from dimos_generated.sensor_msgs.msg import CameraInfo, Image, PointCloud2 +from dimos_generated.std_msgs.msg import Header +from dimos_generated.tf2_msgs.msg import TFMessage import numpy as np import pytest @@ -25,12 +29,43 @@ ) from dimos.memory.store.memory import MemoryStore from dimos.memory.store.sqlite import SqliteStore -from dimos.msgs.geometry_msgs.Transform import Transform -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo -from dimos.msgs.sensor_msgs.Image import Image -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 -from dimos.msgs.tf2_msgs.TFMessage import TFMessage +from dimos.msgs.camera_info import camera_info_from_intrinsics +from dimos.msgs.geometry import inverse_transform, transform_matrix +from dimos.msgs.image import image_from_array, image_view +from dimos.msgs.pointcloud import pointcloud_from_xyz +from dimos.msgs.time import time_from_seconds, to_seconds + + +def _image(data, *, frame_id="", ts=0.0): + return image_from_array( + data, encoding="rgb8", header=Header(frame_id=frame_id, stamp=time_from_seconds(ts)) + ) + + +def _cloud(data, *, frame_id="", timestamp=0.0): + return pointcloud_from_xyz( + data, header=Header(frame_id=frame_id, stamp=time_from_seconds(timestamp)) + ) + + +def _calibration(fx, fy, cx, cy, width, height, *, frame_id=""): + result = camera_info_from_intrinsics( + max(fx, 1.0), max(fy, 1.0), cx, cy, width, height, header=Header(frame_id=frame_id) + ) + result.k[0] = fx + result.k[4] = fy + return result + + +def _transform(*, translation=None, frame_id="", child_frame_id="", ts=0.0): + return TransformStamped( + header=Header(frame_id=frame_id, stamp=time_from_seconds(ts)), + child_frame_id=child_frame_id, + transform=Transform( + translation=translation if translation is not None else Vector3(), + rotation=Quaternion(w=1.0), + ), + ) def _populate_required_streams( @@ -40,18 +75,18 @@ def _populate_required_streams( include_empty_pointlio: bool = False, resolvable_tf: bool = True, ) -> None: - image = Image.from_numpy( + image = _image( np.zeros((2, 2, 3), dtype=np.uint8), frame_id="camera_optical", ts=10.0, ) - cloud = PointCloud2.from_numpy( + cloud = _cloud( np.array([[0.0, 0.0, 1.0]]), frame_id="world", timestamp=10.0, ) - camera_info = CameraInfo.from_intrinsics(1.0, 1.0, 0.0, 0.0, 2, 2, frame_id="camera_optical") - transform = Transform( + camera_info = _calibration(1.0, 1.0, 0.0, 0.0, 2, 2, frame_id="camera_optical") + transform = _transform( frame_id="world" if resolvable_tf else "unrelated_parent", child_frame_id="camera_optical" if resolvable_tf else "unrelated_child", ts=10.0, @@ -68,7 +103,7 @@ def _populate_required_streams( if empty != "camera_info": camera_infos.append(camera_info, ts=10.0) if empty != "tf": - transforms.append(TFMessage(transform), ts=10.0) + transforms.append(TFMessage(transforms=[transform]), ts=10.0) if empty != "lidar": lidar.append(cloud, ts=10.0) @@ -85,16 +120,16 @@ def test_align_one_rejects_observation_outside_tolerance() -> None: def test_rectifier_rejects_invalid_camera_intrinsics() -> None: - image = Image.from_numpy(np.zeros((2, 2, 3), dtype=np.uint8)) - camera_info = CameraInfo.from_intrinsics(0.0, 1.0, 0.0, 0.0, 2, 2) + image = _image(np.zeros((2, 2, 3), dtype=np.uint8)) + camera_info = _calibration(0.0, 1.0, 0.0, 0.0, 2, 2) with pytest.raises(ValueError, match="positive focal lengths"): _ImageRectifier().rectify(image, camera_info) def test_rectifier_rejects_calibration_dimension_mismatch() -> None: - image = Image.from_numpy(np.zeros((2, 2, 3), dtype=np.uint8)) - camera_info = CameraInfo.from_intrinsics(1.0, 1.0, 0.0, 0.0, 1, 2) + image = _image(np.zeros((2, 2, 3), dtype=np.uint8)) + camera_info = _calibration(1.0, 1.0, 0.0, 0.0, 1, 2) with pytest.raises(ValueError, match="do not match image dimensions"): _ImageRectifier().rectify(image, camera_info) @@ -137,7 +172,7 @@ def test_uses_nonempty_lidar_when_pointlio_stream_is_empty(tmp_path: Path) -> No _populate_required_streams(store, include_empty_pointlio=True) with PointCloudFrameLoader(dataset) as loader: - assert loader.load(0).pointcloud.frame_id == "world" + assert loader.load(0).pointcloud.header.frame_id == "world" def test_unresolvable_tf_still_allows_image_only_fallback( @@ -153,28 +188,28 @@ def test_unresolvable_tf_still_allows_image_only_fallback( with loader: with pytest.raises(PointCloudFrameUnavailableError, match="cannot resolve"): loader.load(0) - assert loader.load_image(0).shape == (2, 2, 3) + assert image_view(loader.load_image(0)).shape == (2, 2, 3) def test_load_uses_recorded_camera_info_and_tf( monkeypatch: pytest.MonkeyPatch, tmp_path: Path ) -> None: dataset = tmp_path / "recording.db" - image = Image.from_numpy( + image = _image( np.zeros((2, 2, 3), dtype=np.uint8), frame_id="camera_optical", ts=10.0, ) - cloud = PointCloud2.from_numpy( + cloud = _cloud( np.array([[0.0, 0.0, 1.0]]), frame_id="world", timestamp=10.0, ) - camera_info = CameraInfo.from_intrinsics( + camera_info = _calibration( 1.0, 1.0, 0.0, 0.0, image.width, image.height, frame_id="camera_optical" ) - world_from_camera = Transform( - translation=Vector3(1.0, 2.0, 0.5), + world_from_camera = _transform( + translation=Vector3(x=1.0, y=2.0, z=0.5), frame_id="world", child_frame_id="camera_optical", ts=10.0, @@ -184,14 +219,16 @@ def test_load_uses_recorded_camera_info_and_tf( store.stream("color_image", Image).append(image, ts=10.0) store.stream("lidar", PointCloud2).append(cloud, ts=10.0) store.stream("camera_info", CameraInfo).append(camera_info, ts=9.0) - store.stream("tf", TFMessage).append(TFMessage(world_from_camera), ts=10.0) + store.stream("tf", TFMessage).append(TFMessage(transforms=[world_from_camera]), ts=10.0) loader = PointCloudFrameLoader(dataset) calibrations: list[CameraInfo] = [] def rectify(source: Image, calibration: CameraInfo) -> tuple[Image, CameraInfo]: calibrations.append(calibration) - return source, camera_info.with_ts(source.ts) + result = CameraInfo.decode(camera_info.encode()) + result.header.stamp = source.header.stamp + return source, result monkeypatch.setattr( loader._rectifier, @@ -203,46 +240,45 @@ def rectify(source: Image, calibration: CameraInfo) -> tuple[Image, CameraInfo]: assert calibrations == [camera_info] assert frame.calibration_source == "recorded" - assert frame.camera_info.ts == image.ts - assert np.allclose(frame.pointcloud_to_camera.to_matrix(), (-world_from_camera).to_matrix()) + assert frame.camera_info.header.stamp == image.header.stamp + assert np.allclose( + transform_matrix(frame.pointcloud_to_camera.transform), + transform_matrix(inverse_transform(world_from_camera).transform), + ) def test_load_resolves_tf_through_non_world_root( monkeypatch: pytest.MonkeyPatch, tmp_path: Path ) -> None: dataset = tmp_path / "recording.db" - image = Image.from_numpy( - np.zeros((2, 2, 3), dtype=np.uint8), frame_id="camera_optical", ts=10.0 - ) - cloud = PointCloud2.from_numpy(np.array([[0.0, 0.0, 1.0]]), frame_id="lidar", timestamp=10.0) - camera_info = CameraInfo.from_intrinsics(1.0, 1.0, 0.0, 0.0, 2, 2, frame_id="camera_optical") + image = _image(np.zeros((2, 2, 3), dtype=np.uint8), frame_id="camera_optical", ts=10.0) + cloud = _cloud(np.array([[0.0, 0.0, 1.0]]), frame_id="lidar", timestamp=10.0) + camera_info = _calibration(1.0, 1.0, 0.0, 0.0, 2, 2, frame_id="camera_optical") transforms = ( - Transform(frame_id="map", child_frame_id="camera_optical", ts=10.0), - Transform(frame_id="map", child_frame_id="lidar", ts=10.0), + _transform(frame_id="map", child_frame_id="camera_optical", ts=10.0), + _transform(frame_id="map", child_frame_id="lidar", ts=10.0), ) with SqliteStore(path=dataset) as store: store.stream("color_image", Image).append(image, ts=10.0) store.stream("lidar", PointCloud2).append(cloud, ts=10.0) store.stream("camera_info", CameraInfo).append(camera_info, ts=10.0) - store.stream("tf", TFMessage).append(TFMessage(*transforms), ts=10.0) + store.stream("tf", TFMessage).append(TFMessage(transforms=[*transforms]), ts=10.0) loader = PointCloudFrameLoader(dataset) monkeypatch.setattr(loader._rectifier, "rectify", lambda source, info: (source, info)) with loader: frame = loader.load(0) - assert np.allclose(frame.pointcloud_to_camera.to_matrix(), np.eye(4)) + assert np.allclose(transform_matrix(frame.pointcloud_to_camera.transform), np.eye(4)) def test_load_applies_camera_rectification_to_pointcloud_transform( monkeypatch: pytest.MonkeyPatch, tmp_path: Path ) -> None: dataset = tmp_path / "recording.db" - image = Image.from_numpy( - np.zeros((2, 2, 3), dtype=np.uint8), frame_id="camera_optical", ts=10.0 - ) - cloud = PointCloud2.from_numpy(np.array([[0.0, 0.0, 1.0]]), frame_id="lidar", timestamp=10.0) - camera_info = CameraInfo.from_intrinsics(1.0, 1.0, 0.0, 0.0, 2, 2, frame_id="camera_optical") + image = _image(np.zeros((2, 2, 3), dtype=np.uint8), frame_id="camera_optical", ts=10.0) + cloud = _cloud(np.array([[0.0, 0.0, 1.0]]), frame_id="lidar", timestamp=10.0) + camera_info = _calibration(1.0, 1.0, 0.0, 0.0, 2, 2, frame_id="camera_optical") rectification = np.array( [ [0.0, -1.0, 0.0], @@ -250,13 +286,15 @@ def test_load_applies_camera_rectification_to_pointcloud_transform( [0.0, 0.0, 1.0], ] ) - camera_info.R = rectification.ravel().tolist() + camera_info.r = rectification.ravel().tolist() with SqliteStore(path=dataset) as store: store.stream("color_image", Image).append(image, ts=10.0) store.stream("lidar", PointCloud2).append(cloud, ts=10.0) store.stream("camera_info", CameraInfo).append(camera_info, ts=10.0) store.stream("tf", TFMessage).append( - TFMessage(Transform(frame_id="camera_optical", child_frame_id="lidar", ts=10.0)), + TFMessage( + transforms=[_transform(frame_id="camera_optical", child_frame_id="lidar", ts=10.0)] + ), ts=10.0, ) @@ -265,7 +303,9 @@ def test_load_applies_camera_rectification_to_pointcloud_transform( with loader: frame = loader.load(0) - assert np.allclose(frame.pointcloud_to_camera.to_matrix()[:3, :3], rectification) + assert np.allclose( + transform_matrix(frame.pointcloud_to_camera.transform)[:3, :3], rectification + ) def test_load_selects_camera_info_for_each_image_timestamp( @@ -273,16 +313,16 @@ def test_load_selects_camera_info_for_each_image_timestamp( ) -> None: dataset = tmp_path / "recording.db" images = ( - Image.from_numpy(np.zeros((2, 2, 3), dtype=np.uint8), ts=10.0), - Image.from_numpy(np.zeros((2, 2, 3), dtype=np.uint8), ts=20.0), + _image(np.zeros((2, 2, 3), dtype=np.uint8), ts=10.0), + _image(np.zeros((2, 2, 3), dtype=np.uint8), ts=20.0), ) camera_infos = ( - CameraInfo.from_intrinsics(10.0, 10.0, 0.0, 0.0, 2, 2, frame_id="camera_early"), - CameraInfo.from_intrinsics(100.0, 100.0, 0.0, 0.0, 2, 2, frame_id="camera_late"), + _calibration(10.0, 10.0, 0.0, 0.0, 2, 2, frame_id="camera_early"), + _calibration(100.0, 100.0, 0.0, 0.0, 2, 2, frame_id="camera_late"), ) camera_transforms = ( - Transform(frame_id="world", child_frame_id="camera_early", ts=10.0), - Transform(frame_id="world", child_frame_id="camera_late", ts=20.0), + _transform(frame_id="world", child_frame_id="camera_early", ts=10.0), + _transform(frame_id="world", child_frame_id="camera_late", ts=20.0), ) with SqliteStore(path=dataset) as store: @@ -293,7 +333,7 @@ def test_load_selects_camera_info_for_each_image_timestamp( for image, timestamp in zip(images, (10.0, 20.0), strict=True): image_stream.append(image, ts=timestamp) lidar_stream.append( - PointCloud2.from_numpy( + _cloud( np.array([[0.0, 0.0, 1.0]]), frame_id="world", timestamp=timestamp, @@ -303,14 +343,18 @@ def test_load_selects_camera_info_for_each_image_timestamp( camera_info_stream.append(camera_infos[0], ts=9.0) camera_info_stream.append(camera_infos[1], ts=19.0) for transform in camera_transforms: - tf_stream.append(TFMessage(transform), ts=transform.ts) + tf_stream.append( + TFMessage(transforms=[transform]), ts=to_seconds(transform.header.stamp) + ) loader = PointCloudFrameLoader(dataset) calibrations: list[CameraInfo] = [] def rectify(source: Image, calibration: CameraInfo) -> tuple[Image, CameraInfo]: calibrations.append(calibration) - return source, calibration.with_ts(source.ts) + result = CameraInfo.decode(calibration.encode()) + result.header.stamp = source.header.stamp + return source, result monkeypatch.setattr(loader._rectifier, "rectify", rectify) with loader: @@ -318,5 +362,5 @@ def rectify(source: Image, calibration: CameraInfo) -> tuple[Image, CameraInfo]: late = loader.load(1) assert calibrations == list(camera_infos) - assert early.camera_info.frame_id == "camera_early" - assert late.camera_info.frame_id == "camera_late" + assert early.camera_info.header.frame_id == "camera_early" + assert late.camera_info.header.frame_id == "camera_late" diff --git a/dimos/experimental/isolated_python/example/contract.py b/dimos/experimental/isolated_python/example/contract.py index b8b4a664011..95ed29bf901 100644 --- a/dimos/experimental/isolated_python/example/contract.py +++ b/dimos/experimental/isolated_python/example/contract.py @@ -16,6 +16,8 @@ from typing import Protocol +from dimos_generated.std_msgs.msg import Int32 + from dimos.agents.annotation import skill from dimos.core.core import rpc from dimos.core.stream import In, Out @@ -23,7 +25,6 @@ IsolatedPythonModule, IsolatedPythonModuleConfig, ) -from dimos.msgs.std_msgs.Int32 import Int32 from dimos.spec.utils import Spec diff --git a/dimos/experimental/isolated_python/test_end_to_end.py b/dimos/experimental/isolated_python/test_end_to_end.py index 602b0e5213a..0ccf7d889bd 100644 --- a/dimos/experimental/isolated_python/test_end_to_end.py +++ b/dimos/experimental/isolated_python/test_end_to_end.py @@ -16,6 +16,8 @@ import subprocess import sys +from dimos_generated.std_msgs.msg import Int32 + from dimos.core.coordination.blueprints import autoconnect from dimos.core.coordination.module_coordinator import ModuleCoordinator from dimos.core.core import rpc @@ -23,7 +25,6 @@ from dimos.core.stream import In, Out from dimos.experimental.isolated_python.example.contract import ExampleExternal from dimos.experimental.isolated_python.example.support import Offset -from dimos.msgs.std_msgs.Int32 import Int32 from dimos.utils.testing.waiting import wait_until @@ -32,7 +33,7 @@ class Producer(Module): @rpc def publish(self, value: int) -> None: - self.value.publish(Int32(value)) + self.value.publish(Int32(data=value)) class Consumer(Module): diff --git a/dimos/experimental/isolated_python/test_generated_example.py b/dimos/experimental/isolated_python/test_generated_example.py new file mode 100644 index 00000000000..53c59e8a3eb --- /dev/null +++ b/dimos/experimental/isolated_python/test_generated_example.py @@ -0,0 +1,42 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +# Copyright 2026 Dimensional Inc. +# SPDX-License-Identifier: Apache-2.0 + +from pathlib import Path + +from dimos_generated.std_msgs.msg import Int32 + +from dimos.experimental.isolated_python.bootstrap import load_class +from dimos.experimental.isolated_python.example.contract import ExampleExternal + + +def test_external_example_uses_generated_contract_and_cdr(monkeypatch): + project = Path(__file__).parents[3] / "native/python/example" + monkeypatch.syspath_prepend(str(project)) + runtime_type = load_class("example_external.runtime:ExampleExternalRuntime") + runtime = runtime_type(_isolated_python_runtime=True, initial_multiplier=3) + received = [] + monkeypatch.setattr(runtime.doubled, "publish", received.append) + try: + streams = {s.name: s.type for s in ExampleExternal.blueprint().blueprints[0].streams} + assert streams["value"] is Int32 and streams["doubled"] is Int32 + runtime._publish_doubled(Int32.decode(Int32(data=-7).encode())) + assert len(received) == 1 + assert type(received[0]) is Int32 + assert Int32.decode(received[0].encode()).data == -21 + assert runtime.get_multiplier() == 3 + finally: + runtime.stop() diff --git a/dimos/experimental/memory/README.md b/dimos/experimental/memory/README.md index 61779f6fe0e..d530dc59a7e 100644 --- a/dimos/experimental/memory/README.md +++ b/dimos/experimental/memory/README.md @@ -1,14 +1,14 @@ # Experimental Native Memory Recorder The Rust recorder is an experimental high-throughput alternative to the Python -Memory2 recorder. It remains compatible with the existing Python readers while -its API and operational behavior are evaluated. Experimental imports may change -without compatibility aliases. +Memory recorder. Python supplies each stream's generated type name and complete +ROS message definition. The native process records custom messages without a +message-specific decoder or rebuild. ## Build and runtime packaging -The recorder is built as a locked Nix package. Nix supplies Rust, CMake, NASM, -SQLite, and the native libraries used by TurboJPEG, so none of those tools or +The recorder is built as a locked Nix package. Nix supplies Rust, Python for build-time message generation, +CMake, and SQLite, so none of those tools or development packages need to be installed on the host. The Python module builds the package automatically on first use. To build it @@ -39,8 +39,7 @@ from dimos.experimental.memory.rust_recorder import ( RustRecorder, RustSqliteStoreConfig, ) -from dimos.msgs.sensor_msgs.Image import Image -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 +from dimos_generated.sensor_msgs.msg import Image, PointCloud2 class SensorRecorder(RustRecorder): @@ -51,19 +50,19 @@ class SensorRecorder(RustRecorder): sensor_recorder = SensorRecorder.blueprint( store=RustSqliteStoreConfig(path="session.db"), encoding_threads=4, - stream_codecs={"lidar": "lz4+lcm"}, + stream_codecs={"lidar": "lz4+cdr"}, ) ``` -SQLite supports LCM-backed messages and the `lcm`, `jpeg`, and `lz4+lcm` -storage codecs. Images default to JPEG quality 50. Configure depth or other -lossless streams explicitly with `lz4+lcm`. The resulting artifact opens and -replays through the stable Python `SqliteStore` API. +SQLite stores generated CDR bytes by default. `lz4+cdr` adds lossless blob +compression. Raw images retain their encoding and pixels, including depth data. +For lossy compression, publish an explicit generated CompressedImage value. +The resulting artifact opens and replays through Python's SqliteStore. ## MCAP -Select MCAP to write the same Memory2 storage encodings into an indexed, -portable container: +Select MCAP for ROS2-profile recordings with CDR channels and embedded, complete +`ros2msg` schemas: ```python from dimos.experimental.memory.rust_recorder import RustMcapStoreConfig @@ -71,30 +70,29 @@ from dimos.experimental.memory.rust_recorder import RustMcapStoreConfig mcap_recorder = SensorRecorder.blueprint( store=RustMcapStoreConfig(path="session.mcap"), encoding_threads=4, - stream_codecs={"lidar": "lz4+lcm"}, ) ``` -MCAP stores each stream's selected `lcm`, `jpeg`, or `lz4+lcm` representation -in indexed Zstd chunks. Source time is the MCAP publish time and recorder -reception time is the log time. JPEG channels decode automatically. Supply -trusted codecs explicitly for LCM channels instead of trusting artifact -metadata: +The Python reader now expects standard ROS2-profile CDR channels with embedded +`ros2msg` schemas. Installed generated message packages supply the decoders: ```python -from dimos.memory.codecs.lcm import LcmCodec -from dimos.memory.codecs.lz4 import Lz4Codec from dimos.memory.store.mcap import McapStore -from dimos.msgs.sensor_msgs.Imu import Imu -store = McapStore( - path="session.mcap", - codecs={"imu": Lz4Codec(LcmCodec(Imu))}, -) +store = McapStore(path="session.mcap") ``` +MCAP applies Zstd chunk compression. It rejects per-payload wrappers such as +`lz4+cdr`. Standard compressed-image messages use their own declared schema. +See `examples/message-codegen/demo_native_recording.py` for a hardware-free +external-message recording demo on both LCM and Zenoh. + Append mode remains unsupported for MCAP. -Both backends preserve source timestamps for common stamped messages. Arbitrary +Both backends preserve source timestamps for supported stamped messages. MCAP +keeps source and reception time as integer nanoseconds; SQLite exposes floating +seconds for its existing query API while message payload stamps remain exact. +Unknown timestamp layouts use reception time. Negative source times work in +SQLite; MCAP rejects them because its time fields are unsigned. Arbitrary pickle payloads, Python `pose_setter_for` hooks, and spatial pose attachment remain Python-recorder features; unsupported combinations fail during startup. diff --git a/dimos/experimental/memory/rust/Cargo.toml b/dimos/experimental/memory/rust/Cargo.toml index c0655dc8a4e..2a7322024bb 100644 --- a/dimos/experimental/memory/rust/Cargo.toml +++ b/dimos/experimental/memory/rust/Cargo.toml @@ -10,7 +10,7 @@ anyhow = "1" crossbeam-channel = "0.5" crossbeam-utils = "0.8" dimos-module = { path = "../../../../native/rust/dimos-module" } -lcm-msgs = { git = "https://github.com/dimensionalOS/dimos-lcm.git", branch = "rust-codegen" } +dimos-generated-messages = { path = "../../../message_codegen" } lz4_flex = "0.14.0" mcap = { version = "0.25.0", default-features = false, features = ["zstd"] } rayon = "1" @@ -19,7 +19,6 @@ serde = { version = "1", features = ["derive"] } serde_json = "1" tokio = { version = "1", features = ["rt-multi-thread", "macros", "signal"] } tracing = "0.1" -turbojpeg = { version = "1.5.1", default-features = false, features = ["cmake"] } validator = { version = "0.21", features = ["derive"] } [dev-dependencies] diff --git a/dimos/experimental/memory/rust/flake.nix b/dimos/experimental/memory/rust/flake.nix index 33863c09638..e52218b3d4f 100644 --- a/dimos/experimental/memory/rust/flake.nix +++ b/dimos/experimental/memory/rust/flake.nix @@ -17,7 +17,7 @@ "aarch64-darwin" ] (system: let pkgs = nixpkgs.legacyPackages.${system}; - nativeDeps = [pkgs.cmake pkgs.nasm pkgs.pkg-config]; + nativeDeps = [pkgs.cmake pkgs.nasm pkgs.pkg-config pkgs.python3]; systemDeps = [pkgs.sqlite pkgs.sqlite.dev] ++ pkgs.lib.optionals pkgs.stdenv.hostPlatform.isDarwin [pkgs.libiconv]; @@ -29,6 +29,7 @@ fileset = pkgs.lib.fileset.unions [ ../../../../Cargo.lock ../../../../Cargo.toml + ../../../../dimos/message_codegen ../../../../dimos/experimental/memory/rust ../../../../native/rust/dimos-module ../../../../native/rust/dimos-lcm-transport @@ -47,7 +48,6 @@ cargoLock = { lockFile = ../../../../Cargo.lock; outputHashes = { - "dimos-lcm-0.1.0" = "sha256-GGkx4Mn6NYP6KZecmoRLKGWIih/+y8OgNn12DeXX6n8="; "pointlio-core-0.1.0" = "sha256-iC7nDbEipfi3cViK7fqKiy2hT9ENGi4Ge7L6Wt1W01Q="; }; }; diff --git a/dimos/experimental/memory/rust/src/decoding.rs b/dimos/experimental/memory/rust/src/decoding.rs index 089de093c90..2626582ecac 100644 --- a/dimos/experimental/memory/rust/src/decoding.rs +++ b/dimos/experimental/memory/rust/src/decoding.rs @@ -13,150 +13,121 @@ // limitations under the License. use anyhow::{Context, Result}; -use lcm_msgs::foxglove_msgs::CompressedVideo; -use lcm_msgs::geometry_msgs::{ - PointStamped, PoseStamped, PoseWithCovarianceStamped, TwistStamped, TwistWithCovarianceStamped, - WrenchStamped, +use dimos_generated_messages::dimos_msgs::msg::LineSegments3D; +use dimos_generated_messages::foxglove_msgs::msg::CompressedVideo; +use dimos_generated_messages::geometry_msgs::msg::{ + PointStamped, PoseStamped, PoseWithCovarianceStamped, TransformStamped, TwistStamped, + TwistWithCovarianceStamped, WrenchStamped, }; -use lcm_msgs::nav_msgs::{OccupancyGrid, Odometry, Path}; -use lcm_msgs::sensor_msgs::{ +use dimos_generated_messages::nav_msgs::msg::{OccupancyGrid, Odometry, Path}; +use dimos_generated_messages::sensor_msgs::msg::{ CameraInfo, CompressedImage, Image, Imu, JointState, Joy, PointCloud2, }; -use lcm_msgs::tf2_msgs::TFMessage; -use lcm_msgs::vision_msgs::{Detection2D, Detection2DArray, Detection3D, Detection3DArray}; - -use crate::{StreamConfig, IMAGE_PAYLOAD_TYPE}; +use dimos_generated_messages::tf2_msgs::msg::TFMessage; +use dimos_generated_messages::vision_msgs::msg::{ + Detection2D, Detection2DArray, Detection3D, Detection3DArray, +}; -/// Transport-decoded payload consumed by storage codecs. -#[derive(Debug)] -pub(crate) enum DecodedPayload { - /// An LCM payload whose storage codecs operate on its canonical wire form. - Lcm(Vec), - /// Images stay typed so JPEG encoding reuses the transport decode. - Image(Image), -} +use crate::StreamConfig; +use dimos_generated_messages::codec::Message; /// One decoded and timestamped observation before storage encoding. #[derive(Debug)] pub(crate) struct DecodedObservation { - pub(crate) ts: f64, - pub(crate) payload: DecodedPayload, + pub(crate) ts: i64, + pub(crate) payload: Vec, } /// Decode a transport packet once and normalize it into storage observations. pub(crate) fn decode( stream: &StreamConfig, data: &[u8], - reception_ts: f64, + reception_ts: i64, ) -> Result> { if stream.is_tf() { return decode_tf(data, reception_ts); } - if stream.payload_type == IMAGE_PAYLOAD_TYPE { - let image = Image::decode(data).context("invalid LCM Image")?; - let ts = header_timestamp( - image.header.stamp.sec, - image.header.stamp.nsec, - reception_ts, - ); - return Ok(vec![DecodedObservation { - ts, - payload: DecodedPayload::Image(image), - }]); - } - - let ts = source_timestamp(&stream.payload_type, data, reception_ts)?; + let ts = source_timestamp(&stream.schema_name, data, reception_ts)?; Ok(vec![DecodedObservation { ts, - payload: DecodedPayload::Lcm(data.to_vec()), + payload: data.to_vec(), }]) } -fn decode_tf(data: &[u8], reception_ts: f64) -> Result> { - let message = TFMessage::decode(data).context("invalid LCM TFMessage")?; - Ok(message +fn decode_tf(data: &[u8], _reception_ts: i64) -> Result> { + let message = TFMessage::decode(data).context("invalid CDR TFMessage")?; + message .transforms .into_iter() - .map(|transform| DecodedObservation { - ts: header_timestamp( - transform.header.stamp.sec, - transform.header.stamp.nsec, - reception_ts, - ), - payload: DecodedPayload::Lcm( - TFMessage { + .map(|transform| { + Ok(DecodedObservation { + ts: header_timestamp(transform.header.stamp.sec, transform.header.stamp.nanosec)?, + payload: TFMessage { transforms: vec![transform], } - .encode(), - ), + .encode()?, + }) }) - .collect()) + .collect() } -/// Read source time while transport-decoding common stamped LCM payloads. +/// Read source time while transport-decoding common stamped CDR payloads. /// -/// Unknown LCM payloads remain opaque and use reception time, matching the -/// Python recorder's fallback for messages without a ``ts`` attribute. -fn source_timestamp(payload_type: &str, data: &[u8], reception_ts: f64) -> Result { +/// Unknown CDR payloads remain opaque and use reception time, matching the +/// Python recorder's fallback for messages without a recognized timestamp layout. +fn source_timestamp(payload_type: &str, data: &[u8], reception_ts: i64) -> Result { macro_rules! stamped { ($message_type:ty) => {{ let message = <$message_type>::decode(data) - .with_context(|| format!("invalid LCM {payload_type}"))?; - (message.header.stamp.sec, message.header.stamp.nsec) + .with_context(|| format!("invalid CDR {payload_type}"))?; + (message.header.stamp.sec, message.header.stamp.nanosec) }}; } let (sec, nsec) = match payload_type { - "dimos.msgs.geometry_msgs.PointStamped.PointStamped" => stamped!(PointStamped), - "dimos.msgs.geometry_msgs.PoseStamped.PoseStamped" => stamped!(PoseStamped), - "dimos.msgs.geometry_msgs.PoseWithCovarianceStamped.PoseWithCovarianceStamped" => { + "geometry_msgs/msg/TransformStamped" => stamped!(TransformStamped), + "dimos_msgs/msg/LineSegments3D" => stamped!(LineSegments3D), + "geometry_msgs/msg/PointStamped" => stamped!(PointStamped), + "geometry_msgs/msg/PoseStamped" => stamped!(PoseStamped), + "geometry_msgs/msg/PoseWithCovarianceStamped" => { stamped!(PoseWithCovarianceStamped) } - "dimos.msgs.geometry_msgs.TwistStamped.TwistStamped" => stamped!(TwistStamped), - "dimos.msgs.geometry_msgs.TwistWithCovarianceStamped.TwistWithCovarianceStamped" => { + "geometry_msgs/msg/TwistStamped" => stamped!(TwistStamped), + "geometry_msgs/msg/TwistWithCovarianceStamped" => { stamped!(TwistWithCovarianceStamped) } - "dimos.msgs.geometry_msgs.WrenchStamped.WrenchStamped" => stamped!(WrenchStamped), - "dimos.msgs.nav_msgs.LineSegments3D.LineSegments3D" | "dimos.msgs.nav_msgs.Path.Path" => { + "geometry_msgs/msg/WrenchStamped" => stamped!(WrenchStamped), + "nav_msgs/msg/Path" => { stamped!(Path) } - "dimos.msgs.nav_msgs.OccupancyGrid.OccupancyGrid" => stamped!(OccupancyGrid), - "dimos.msgs.nav_msgs.Odometry.Odometry" => stamped!(Odometry), - "dimos.msgs.sensor_msgs.CameraInfo.CameraInfo" => stamped!(CameraInfo), - "dimos.msgs.sensor_msgs.CompressedImage.CompressedImage" => stamped!(CompressedImage), - "dimos.msgs.sensor_msgs.Imu.Imu" => stamped!(Imu), - "dimos.msgs.sensor_msgs.JointState.JointState" => stamped!(JointState), - "dimos.msgs.sensor_msgs.Joy.Joy" => stamped!(Joy), - "dimos.msgs.sensor_msgs.PointCloud2.PointCloud2" => stamped!(PointCloud2), - "dimos.msgs.vision_msgs.Detection2D.Detection2D" => stamped!(Detection2D), - "dimos.msgs.vision_msgs.Detection2DArray.Detection2DArray" => { + "nav_msgs/msg/OccupancyGrid" => stamped!(OccupancyGrid), + "nav_msgs/msg/Odometry" => stamped!(Odometry), + "sensor_msgs/msg/Image" => stamped!(Image), + "sensor_msgs/msg/CameraInfo" => stamped!(CameraInfo), + "sensor_msgs/msg/CompressedImage" => stamped!(CompressedImage), + "sensor_msgs/msg/Imu" => stamped!(Imu), + "sensor_msgs/msg/JointState" => stamped!(JointState), + "sensor_msgs/msg/Joy" => stamped!(Joy), + "sensor_msgs/msg/PointCloud2" => stamped!(PointCloud2), + "vision_msgs/msg/Detection2D" => stamped!(Detection2D), + "vision_msgs/msg/Detection2DArray" => { stamped!(Detection2DArray) } - "dimos.msgs.vision_msgs.Detection3D.Detection3D" => stamped!(Detection3D), - "dimos.msgs.vision_msgs.Detection3DArray.Detection3DArray" => { + "vision_msgs/msg/Detection3D" => stamped!(Detection3D), + "vision_msgs/msg/Detection3DArray" => { stamped!(Detection3DArray) } - "dimos.msgs.foxglove_msgs.CompressedVideo.CompressedVideo" => { + "foxglove_msgs/msg/CompressedVideo" => { let message = CompressedVideo::decode(data) - .context("invalid LCM foxglove_msgs.CompressedVideo")?; + .context("invalid CDR foxglove_msgs.CompressedVideo")?; (message.timestamp.sec, message.timestamp.nanosec) } - "dimos.msgs.geometry_msgs.Transform.Transform" => { - let message = TFMessage::decode(data).context("invalid LCM TFMessage")?; - let Some(transform) = message.transforms.first() else { - return Ok(reception_ts); - }; - (transform.header.stamp.sec, transform.header.stamp.nsec) - } _ => return Ok(reception_ts), }; - Ok(header_timestamp(sec, nsec, reception_ts)) + header_timestamp(sec, nsec) } -pub(crate) fn header_timestamp(sec: i32, nsec: i32, fallback: f64) -> f64 { - if sec > 0 { - f64::from(sec) + f64::from(nsec) / 1_000_000_000.0 - } else { - fallback - } +pub(crate) fn header_timestamp(sec: i32, nanosec: u32) -> Result { + anyhow::ensure!(nanosec < 1_000_000_000, "invalid ROS nanosecond field"); + Ok(i64::from(sec) * 1_000_000_000 + i64::from(nanosec)) } diff --git a/dimos/experimental/memory/rust/src/encoding.rs b/dimos/experimental/memory/rust/src/encoding.rs index a13360886ce..eb5dc4a96d0 100644 --- a/dimos/experimental/memory/rust/src/encoding.rs +++ b/dimos/experimental/memory/rust/src/encoding.rs @@ -14,19 +14,15 @@ use std::io::Write; -use anyhow::{anyhow, Context, Result}; -use lcm_msgs::sensor_msgs::Image; +use anyhow::{Context, Result}; use lz4_flex::frame::FrameEncoder; -use turbojpeg::{PixelFormat, Subsamp}; -use crate::decoding::{DecodedObservation, DecodedPayload}; +use crate::decoding::DecodedObservation; use crate::{Codec, StreamConfig}; -pub(crate) const JPEG_QUALITY: i32 = 50; - #[derive(Debug)] pub(crate) struct StoredObservation { - pub(crate) ts: f64, + pub(crate) ts: i64, pub(crate) data: Vec, } @@ -35,9 +31,8 @@ pub(crate) fn encode( observation: DecodedObservation, ) -> Result { let data = match stream.codec { - Codec::Lcm => lcm_encode(observation.payload), - Codec::Lz4Lcm => lz4_frame(&lcm_encode(observation.payload))?, - Codec::Jpeg => jpeg_encode(observation.payload)?, + Codec::Cdr => observation.payload, + Codec::Lz4Cdr => lz4_frame(&observation.payload)?, }; Ok(StoredObservation { ts: observation.ts, @@ -45,71 +40,6 @@ pub(crate) fn encode( }) } -fn lcm_encode(payload: DecodedPayload) -> Vec { - match payload { - DecodedPayload::Lcm(data) => data, - DecodedPayload::Image(image) => image.encode(), - } -} - -fn jpeg_encode(payload: DecodedPayload) -> Result> { - let DecodedPayload::Image(mut image) = payload else { - return Err(anyhow!("JPEG storage codec requires an Image payload")); - }; - if image.encoding == "jpeg" { - return Ok(image.encode()); - } - - let format = pixel_format(&image.encoding)?; - let pixels = normalize_pixels(&image)?; - let source = turbojpeg::Image { - pixels: pixels.as_slice(), - width: image.width as usize, - pitch: image.width as usize * format.size(), - height: image.height as usize, - format, - }; - let jpeg = turbojpeg::compress(source, JPEG_QUALITY, Subsamp::Sub2x1) - .context("TurboJPEG compression failed")?; - image.encoding = "jpeg".to_string(); - image.is_bigendian = 0; - image.step = 0; - image.data = jpeg.to_vec(); - Ok(image.encode()) -} - -fn pixel_format(encoding: &str) -> Result { - match encoding { - "rgb8" => Ok(PixelFormat::RGB), - "bgr8" => Ok(PixelFormat::BGR), - "rgba8" => Ok(PixelFormat::RGBA), - "bgra8" => Ok(PixelFormat::BGRA), - "mono8" | "mono16" | "16UC1" | "16SC1" => Ok(PixelFormat::GRAY), - other => Err(anyhow!( - "JPEG codec does not support image encoding {other:?}" - )), - } -} - -fn normalize_pixels(image: &Image) -> Result> { - match image.encoding.as_str() { - "mono16" | "16UC1" | "16SC1" => { - if !image.data.len().is_multiple_of(2) { - return Err(anyhow!("16-bit image has an odd byte count")); - } - let high_byte = usize::from(image.is_bigendian == 0); - Ok(image - .data - .as_chunks::<2>() - .0 - .iter() - .map(|pixel| pixel[high_byte]) - .collect()) - } - _ => Ok(image.data.clone()), - } -} - pub(crate) fn lz4_frame(data: &[u8]) -> Result> { let mut encoder = FrameEncoder::new(Vec::new()); encoder.write_all(data).context("LZ4 compression failed")?; diff --git a/dimos/experimental/memory/rust/src/lib.rs b/dimos/experimental/memory/rust/src/lib.rs index 0d49bd7beef..2024bfdfa46 100644 --- a/dimos/experimental/memory/rust/src/lib.rs +++ b/dimos/experimental/memory/rust/src/lib.rs @@ -35,8 +35,9 @@ mod decoding; mod encoding; pub mod store; -const TF_PAYLOAD_TYPE: &str = "dimos.msgs.tf2_msgs.TFMessage.TFMessage"; -const IMAGE_PAYLOAD_TYPE: &str = "dimos.msgs.sensor_msgs.Image.Image"; +const TF_PAYLOAD_TYPE: &str = "tf2_msgs/msg/TFMessage"; +#[cfg(test)] +const IMAGE_PAYLOAD_TYPE: &str = "sensor_msgs/msg/Image"; const QUEUE_CAPACITY: usize = 256; const WRITE_BATCH_SIZE: usize = 128; const FLUSH_INTERVAL: Duration = Duration::from_millis(100); @@ -44,20 +45,9 @@ const FLUSH_INTERVAL: Duration = Duration::from_millis(100); #[derive(Clone, Copy, Debug, Deserialize, Serialize, PartialEq, Eq)] #[serde(rename_all = "lowercase")] pub enum Codec { - Lcm, - Jpeg, - #[serde(rename = "lz4+lcm")] - Lz4Lcm, -} - -impl Codec { - pub(crate) fn id(self) -> &'static str { - match self { - Self::Lcm => "lcm", - Self::Jpeg => "jpeg", - Self::Lz4Lcm => "lz4+lcm", - } - } + Cdr, + #[serde(rename = "lz4+cdr")] + Lz4Cdr, } #[derive(Clone, Debug, Deserialize, Serialize)] @@ -66,12 +56,14 @@ pub struct StreamConfig { pub port: String, pub name: String, pub payload_type: String, + pub schema_name: String, + pub schema_definition: String, pub codec: Codec, } impl StreamConfig { pub fn is_tf(&self) -> bool { - self.payload_type == TF_PAYLOAD_TYPE + self.schema_name == TF_PAYLOAD_TYPE } } @@ -217,7 +209,7 @@ impl RecorderEngine { struct EncodedBatch { sequence: u64, stream: Arc, - reception_ts: f64, + reception_ts: i64, observations: Result, String>, } @@ -231,7 +223,7 @@ pub struct WriterStats { fn process( stream: &StreamConfig, data: &[u8], - reception_ts: f64, + reception_ts: i64, ) -> Result> { decoding::decode(stream, data, reception_ts)? .into_iter() @@ -239,11 +231,13 @@ fn process( .collect() } -fn wall_time() -> f64 { +fn wall_time() -> i64 { SystemTime::now() .duration_since(UNIX_EPOCH) .unwrap_or_default() - .as_secs_f64() + .as_nanos() + .try_into() + .expect("wall time exceeds signed nanosecond range") } struct RecorderInput { @@ -443,9 +437,11 @@ fn write_ready( mod tests { use std::io::Read; - use lcm_msgs::sensor_msgs::{Image, Imu}; - use lcm_msgs::std_msgs::{Header, Time}; - use lcm_msgs::tf2_msgs::TFMessage; + use dimos_generated_messages::builtin_interfaces::msg::Time; + use dimos_generated_messages::codec::Message; + use dimos_generated_messages::sensor_msgs::msg::{CompressedImage, Image, Imu}; + use dimos_generated_messages::std_msgs::msg::Header; + use dimos_generated_messages::tf2_msgs::msg::TFMessage; use lz4_flex::frame::FrameDecoder; use rusqlite::Connection; use tempfile::NamedTempFile; @@ -462,6 +458,18 @@ mod tests { "test.Raw".to_string() }, codec, + schema_name: if is_tf { + TFMessage::NAME + } else { + "test_msgs/msg/Raw" + } + .to_string(), + schema_definition: if is_tf { + TFMessage::SCHEMA + } else { + "uint8[] data\n" + } + .to_string(), }) } @@ -470,6 +478,14 @@ mod tests { port: name.to_string(), name: name.to_string(), payload_type: payload_type.to_string(), + schema_name: payload_type.to_string(), + schema_definition: match payload_type { + Image::NAME => Image::SCHEMA, + Imu::NAME => Imu::SCHEMA, + CompressedImage::NAME => CompressedImage::SCHEMA, + _ => "uint8[] data\n", + } + .to_string(), codec, }) } @@ -485,11 +501,13 @@ mod tests { } #[test] - fn jpeg_codec_preserves_the_lcm_envelope() { + fn raw_images_keep_their_declared_encoding_and_pixels() { let image = Image { header: Header { - seq: 9, - stamp: Time { sec: 12, nsec: 34 }, + stamp: Time { + sec: 12, + nanosec: 34, + }, frame_id: "camera".to_string(), }, height: 2, @@ -500,36 +518,40 @@ mod tests { data: vec![255, 0, 0, 0, 255, 0, 0, 0, 255, 255, 255, 255], }; - let stream = typed_stream("camera", IMAGE_PAYLOAD_TYPE, Codec::Jpeg); - let observations = process(&stream, &image.encode(), 100.0).unwrap(); + let stream = typed_stream("camera", IMAGE_PAYLOAD_TYPE, Codec::Cdr); + let observations = process(&stream, &image.encode().unwrap(), 100_000_000_000).unwrap(); let decoded = Image::decode(&observations[0].data).unwrap(); - assert_eq!(observations[0].ts, 12.000_000_034); + assert_eq!(observations[0].ts, 12_000_000_034); assert_eq!(decoded.header, image.header); - assert_eq!(decoded.encoding, "jpeg"); - assert_eq!(decoded.step, 0); - assert_eq!(&decoded.data[..2], &[0xff, 0xd8]); + assert_eq!(decoded, image); } #[test] fn tf_batches_become_individually_timestamped_observations() { - let mut first = lcm_msgs::geometry_msgs::TransformStamped::default(); - first.header.stamp = Time { sec: 10, nsec: 5 }; + let mut first = dimos_generated_messages::geometry_msgs::msg::TransformStamped::default(); + first.header.stamp = Time { + sec: 10, + nanosec: 5, + }; first.child_frame_id = "first".to_string(); - let mut second = lcm_msgs::geometry_msgs::TransformStamped::default(); - second.header.stamp = Time { sec: 20, nsec: 7 }; + let mut second = dimos_generated_messages::geometry_msgs::msg::TransformStamped::default(); + second.header.stamp = Time { + sec: 20, + nanosec: 7, + }; second.child_frame_id = "second".to_string(); let message = TFMessage { transforms: vec![first, second], }; - let stream = stream("tf", Codec::Lcm, true); - let data = message.encode(); + let stream = stream("tf", Codec::Cdr, true); + let data = message.encode().unwrap(); - let observations = process(&stream, &data, 100.0).unwrap(); + let observations = process(&stream, &data, 100_000_000_000).unwrap(); assert_eq!(observations.len(), 2); - assert_eq!(observations[0].ts, 10.000_000_005); - assert_eq!(observations[1].ts, 20.000_000_007); + assert_eq!(observations[0].ts, 10_000_000_005); + assert_eq!(observations[1].ts, 20_000_000_007); let decoded: Vec = observations .iter() .map(|observation| TFMessage::decode(&observation.data).unwrap()) @@ -543,23 +565,32 @@ mod tests { #[test] fn stamped_sensor_messages_preserve_their_source_timestamp() { let mut message = Imu::default(); - message.header.stamp = Time { sec: 42, nsec: 25 }; + message.header.stamp = Time { + sec: 42, + nanosec: 25, + }; let stream = Arc::new(StreamConfig { port: "imu".to_string(), name: "imu".to_string(), - payload_type: "dimos.msgs.sensor_msgs.Imu.Imu".to_string(), - codec: Codec::Lcm, + payload_type: "dimos_generated.sensor_msgs.msg.Imu".to_string(), + schema_name: Imu::NAME.to_string(), + schema_definition: Imu::SCHEMA.to_string(), + codec: Codec::Cdr, }); - let observations = decoding::decode(&stream, &message.encode(), 100.0).unwrap(); - assert_eq!(observations[0].ts, 42.000_000_025); + let observations = + decoding::decode(&stream, &message.encode().unwrap(), 100_000_000_000).unwrap(); + assert_eq!(observations[0].ts, 42_000_000_025); } #[test] - fn lcm_storage_encoding_consumes_the_decoded_image() { + fn cdr_storage_preserves_image_bytes() { let image = Image { header: Header { - stamp: Time { sec: 42, nsec: 25 }, + stamp: Time { + sec: 42, + nanosec: 25, + }, ..Header::default() }, height: 1, @@ -569,12 +600,12 @@ mod tests { data: vec![1, 2, 3], ..Image::default() }; - let stream = typed_stream("camera", IMAGE_PAYLOAD_TYPE, Codec::Lcm); + let stream = typed_stream("camera", IMAGE_PAYLOAD_TYPE, Codec::Cdr); - let observations = process(&stream, &image.encode(), 100.0).unwrap(); + let observations = process(&stream, &image.encode().unwrap(), 100_000_000_000).unwrap(); assert_eq!(observations.len(), 1); - assert_eq!(observations[0].ts, 42.000_000_025); + assert_eq!(observations[0].ts, 42_000_000_025); assert_eq!(Image::decode(&observations[0].data).unwrap(), image); } @@ -586,7 +617,7 @@ mod tests { path: file.path().to_string_lossy().into_owned(), }, encoding_threads: 2, - streams: vec![(*stream("samples", Codec::Lcm, false)).clone()], + streams: vec![(*stream("samples", Codec::Cdr, false)).clone()], }; let (sender, receiver) = bounded(8); let (permit_sender, permit_receiver) = bounded(8); @@ -597,10 +628,10 @@ mod tests { sender .send(EncodedBatch { sequence, - stream: stream("samples", Codec::Lcm, false), - reception_ts: sequence as f64, + stream: stream("samples", Codec::Cdr, false), + reception_ts: sequence as i64 * 1_000_000_000, observations: Ok(vec![StoredObservation { - ts: sequence as f64, + ts: sequence as i64 * 1_000_000_000, data: vec![sequence as u8], }]), }) @@ -649,7 +680,7 @@ mod tests { fn tf_observations_use_empty_jsonb_tags_without_a_reception_index() { let file = NamedTempFile::new().unwrap(); let connection = Connection::open(file.path()).unwrap(); - let tf = stream("tf", Codec::Lcm, true); + let tf = stream("tf", Codec::Cdr, true); drop(connection); let mut recording_store = store::open( &store::RecordingStoreConfig::Sqlite { @@ -662,8 +693,8 @@ mod tests { recording_store .write_batch(&[Observation { stream: Arc::clone(&tf), - source_ts: 1.0, - reception_ts: 100.0, + source_ts: 1_000_000_000, + reception_ts: 100_000_000_000, data: vec![1, 2, 3], }]) .unwrap(); @@ -691,7 +722,7 @@ mod tests { #[test] fn mcap_store_writes_indexed_storage_encoded_messages() { let file = NamedTempFile::new().unwrap(); - let samples = stream("samples", Codec::Lz4Lcm, false); + let samples = stream("samples", Codec::Cdr, false); let mut recording_store = store::open( &store::RecordingStoreConfig::Mcap { path: file.path().to_string_lossy().into_owned(), @@ -700,12 +731,12 @@ mod tests { 1, ) .unwrap(); - let encoded = encoding::lz4_frame(&[1, 2, 3]).unwrap(); + let encoded = vec![0, 1, 0, 0, 3, 0, 0, 0, 1, 2, 3]; recording_store .write_batch(&[Observation { stream: Arc::clone(&samples), - source_ts: 12.5, - reception_ts: 13.0, + source_ts: 1_700_000_000_123_456_789, + reception_ts: 1_700_000_000_987_654_321, data: encoded.clone(), }]) .unwrap(); @@ -728,17 +759,21 @@ mod tests { assert_eq!(messages.len(), 1); let message = &messages[0]; assert_eq!(message.channel.topic, "samples"); - assert_eq!(message.channel.message_encoding, "lz4+lcm"); + assert_eq!(message.channel.message_encoding, "cdr"); + let schema = message.channel.schema.as_ref().unwrap(); + assert_eq!(schema.name, "test_msgs/msg/Raw"); + assert_eq!(schema.encoding, "ros2msg"); + assert_eq!(schema.data.as_ref(), b"uint8[] data\n"); assert_eq!(message.channel.metadata["dimos.payload_type"], "test.Raw"); - assert_eq!(message.log_time, 13_000_000_000); - assert_eq!(message.publish_time, 12_500_000_000); + assert_eq!(message.log_time, 1_700_000_000_987_654_321); + assert_eq!(message.publish_time, 1_700_000_000_123_456_789); assert_eq!(message.data.as_ref(), encoded); } #[test] fn engine_uses_the_configured_worker_count_and_flushes_on_shutdown() { let file = NamedTempFile::new().unwrap(); - let raw = stream("raw", Codec::Lcm, false); + let raw = stream("raw", Codec::Cdr, false); let config = RecorderConfig { store: store::RecordingStoreConfig::Sqlite { path: file.path().to_string_lossy().into_owned(), @@ -761,7 +796,7 @@ mod tests { #[test] fn encoding_failure_fails_the_recording_with_stream_and_sequence() { let file = NamedTempFile::new().unwrap(); - let image = typed_stream("camera", IMAGE_PAYLOAD_TYPE, Codec::Jpeg); + let image = typed_stream("camera", IMAGE_PAYLOAD_TYPE, Codec::Cdr); let config = RecorderConfig { store: store::RecordingStoreConfig::Sqlite { path: file.path().to_string_lossy().into_owned(), @@ -770,14 +805,14 @@ mod tests { streams: vec![(*image).clone()], }; let engine = RecorderEngine::start(config).unwrap(); - engine.handle().record(image, b"not an lcm image"); + engine.handle().record(image, b"not a CDR image"); let error = engine.shutdown().unwrap_err(); let message = format!("{error:#}"); assert!(message.contains("camera")); assert!(message.contains("sequence 0")); - assert!(message.contains("invalid LCM Image")); + assert!(message.contains("invalid CDR sensor_msgs/msg/Image")); } #[test] @@ -792,4 +827,74 @@ mod tests { assert!(validator::Validate::validate(&config).is_err()); } + #[test] + fn stamped_cdr_preserves_zero_negative_and_exact_epoch_nanoseconds_in_both_endians() { + let stream = typed_stream("imu", Imu::NAME, Codec::Cdr); + for (sec, nanosec, expected) in [ + (0, 0, 0), + (-1, 500_000_000, -500_000_000), + (1_700_000_000, 123_456_789, 1_700_000_000_123_456_789), + ] { + let mut message = Imu::default(); + message.header.stamp = Time { sec, nanosec }; + for little_endian in [false, true] { + let bytes = message.encode_endian(little_endian).unwrap(); + let observations = process(&stream, &bytes, 99).unwrap(); + assert_eq!(observations[0].ts, expected); + assert_eq!(observations[0].data, bytes); + } + } + } + + #[test] + fn compressed_image_keeps_its_standard_schema_and_payload() { + let message = CompressedImage { + header: Header { + stamp: Time { sec: 2, nanosec: 3 }, + ..Default::default() + }, + format: "rgb8; jpeg compressed bgr8".into(), + data: vec![0xff, 0xd8, 0xff, 0xd9], + }; + let stream = typed_stream("camera", CompressedImage::NAME, Codec::Cdr); + let observations = process(&stream, &message.encode().unwrap(), 99).unwrap(); + assert_eq!(observations[0].ts, 2_000_000_003); + assert_eq!( + CompressedImage::decode(&observations[0].data).unwrap(), + message + ); + } + + #[test] + fn unknown_custom_type_is_opaque_and_uses_reception_nanoseconds() { + let stream = stream("custom", Codec::Cdr, false); + let bytes = vec![0, 1, 0, 0, 3, 0, 0, 0, 1, 2, 3]; + let observations = process(&stream, &bytes, 1_700_000_000_987_654_321).unwrap(); + assert_eq!(observations[0].ts, 1_700_000_000_987_654_321); + assert_eq!(observations[0].data, bytes); + } + + #[test] + fn invalid_ros_nanoseconds_are_rejected() { + let mut message = Imu::default(); + message.header.stamp.nanosec = 1_000_000_000; + let stream = typed_stream("imu", Imu::NAME, Codec::Cdr); + assert!(process(&stream, &message.encode().unwrap(), 99).is_err()); + } + + #[test] + fn mcap_rejects_payload_wrappers_before_touching_the_artifact() { + let file = NamedTempFile::new().unwrap(); + std::fs::write(file.path(), b"existing").unwrap(); + let stream = stream("custom", Codec::Lz4Cdr, false); + let result = store::open( + &store::RecordingStoreConfig::Mcap { + path: file.path().to_string_lossy().into_owned(), + }, + &[(*stream).clone()], + 1, + ); + assert!(result.is_err()); + assert_eq!(std::fs::read(file.path()).unwrap(), b"existing"); + } } diff --git a/dimos/experimental/memory/rust/src/store/mcap.rs b/dimos/experimental/memory/rust/src/store/mcap.rs index 13d69e892e8..d3d026b4344 100644 --- a/dimos/experimental/memory/rust/src/store/mcap.rs +++ b/dimos/experimental/memory/rust/src/store/mcap.rs @@ -15,14 +15,13 @@ use std::collections::{BTreeMap, HashMap}; use std::fs::File; use std::io::BufWriter; -use std::time::Duration; use anyhow::{Context, Result}; use mcap::records::MessageHeader; use mcap::{Compression, WriteOptions, Writer}; use super::{Observation, RecordingStore}; -use crate::StreamConfig; +use crate::{Codec, StreamConfig}; pub struct McapRecordingStore { writer: Writer>, @@ -32,9 +31,19 @@ pub struct McapRecordingStore { impl McapRecordingStore { pub fn open(path: &str, streams: &[StreamConfig], compression_threads: usize) -> Result { + for stream in streams { + anyhow::ensure!( + stream.codec == Codec::Cdr, + "MCAP requires cdr channels; use chunk compression instead of a payload wrapper" + ); + anyhow::ensure!( + !stream.schema_name.is_empty() && !stream.schema_definition.is_empty(), + "MCAP requires a complete message schema" + ); + } let file = File::create(path).with_context(|| format!("failed to create {path}"))?; let options = WriteOptions::new() - .profile("dimos") + .profile("ros2") .library("dimos-memory-recorder") .compression(Some(Compression::Zstd)) .compression_threads(compression_threads.try_into().unwrap_or(u32::MAX)); @@ -53,7 +62,12 @@ impl McapRecordingStore { "publish_time".to_string(), ), ]); - let channel = writer.add_channel(0, &stream.name, stream.codec.id(), &metadata)?; + let schema = writer.add_schema( + &stream.schema_name, + "ros2msg", + stream.schema_definition.as_bytes(), + )?; + let channel = writer.add_channel(schema, &stream.name, "cdr", &metadata)?; channels.insert(stream.name.clone(), channel); } Ok(Self { @@ -76,8 +90,10 @@ impl RecordingStore for McapRecordingStore { &MessageHeader { channel_id, sequence: *sequence, - log_time: timestamp_ns(observation.reception_ts), - publish_time: timestamp_ns(observation.source_ts), + log_time: u64::try_from(observation.reception_ts) + .context("MCAP reception time precedes Unix epoch")?, + publish_time: u64::try_from(observation.source_ts) + .context("MCAP source time precedes Unix epoch")?, }, &observation.data, )?; @@ -91,13 +107,3 @@ impl RecordingStore for McapRecordingStore { Ok(()) } } - -fn timestamp_ns(timestamp: f64) -> u64 { - if !timestamp.is_finite() || timestamp <= 0.0 { - return 0; - } - Duration::from_secs_f64(timestamp) - .as_nanos() - .try_into() - .unwrap_or(u64::MAX) -} diff --git a/dimos/experimental/memory/rust/src/store/mod.rs b/dimos/experimental/memory/rust/src/store/mod.rs index 78c08a53e28..d0371887b64 100644 --- a/dimos/experimental/memory/rust/src/store/mod.rs +++ b/dimos/experimental/memory/rust/src/store/mod.rs @@ -41,8 +41,8 @@ impl RecordingStoreConfig { /// One ordered observation ready for durable storage. pub struct Observation { pub stream: Arc, - pub source_ts: f64, - pub reception_ts: f64, + pub source_ts: i64, + pub reception_ts: i64, pub data: Vec, } diff --git a/dimos/experimental/memory/rust/src/store/sqlite.rs b/dimos/experimental/memory/rust/src/store/sqlite.rs index 813d0451c0b..059ab59bbd9 100644 --- a/dimos/experimental/memory/rust/src/store/sqlite.rs +++ b/dimos/experimental/memory/rust/src/store/sqlite.rs @@ -90,13 +90,15 @@ fn insert_observation(connection: &Connection, observation: &Observation) -> Res if observation.stream.is_tf() { connection.execute( &format!(r#"INSERT INTO "{name}" (ts) VALUES (?1)"#), - params![observation.source_ts], + params![observation.source_ts as f64 / 1_000_000_000.0], )?; } else { - let tags = serde_json::json!({"reception_ts": observation.reception_ts}).to_string(); + let tags = + serde_json::json!({"reception_ts": observation.reception_ts as f64 / 1_000_000_000.0}) + .to_string(); connection.execute( &format!(r#"INSERT INTO "{name}" (ts, tags) VALUES (?1, jsonb(?2))"#), - params![observation.source_ts, tags], + params![observation.source_ts as f64 / 1_000_000_000.0, tags], )?; } let id = connection.last_insert_rowid(); diff --git a/dimos/experimental/memory/rust_cli_recorder.py b/dimos/experimental/memory/rust_cli_recorder.py index cbb50ccb4af..52d4923798a 100644 --- a/dimos/experimental/memory/rust_cli_recorder.py +++ b/dimos/experimental/memory/rust_cli_recorder.py @@ -33,7 +33,6 @@ from dimos.experimental.memory.rust_recorder import RustStreamSpec from dimos.memory.store.sqlite import SqliteStore from dimos.memory.tap import matching, recording_dir -from dimos.msgs.sensor_msgs.Image import Image from dimos.utils.logging_config import setup_logger logger = setup_logger() @@ -99,9 +98,11 @@ def make_plan(transports: dict[tuple[str, type], Any]) -> RustRecordingPlan: for index, ((name, payload_type), transport) in enumerate(transports.items()): if name not in selected: continue - if not hasattr(payload_type, "lcm_encode") or not hasattr(payload_type, "lcm_decode"): + if not all( + hasattr(payload_type, member) for member in ("encode", "decode", "msg_name", "schema") + ): logger.info( - "--record: stream is not a DimOS LCM message type; skipped", + "--record: stream is not a generated CDR message type; skipped", stream=name, payload_type=str(payload_type), ) @@ -117,11 +118,11 @@ def make_plan(transports: dict[tuple[str, type], Any]) -> RustRecordingPlan: backends.add(backend) port = f"stream_{index}" streams.append( - RustStreamSpec( + RustStreamSpec.from_type( port=port, name=name, - payload_type=f"{payload_type.__module__}.{payload_type.__qualname__}", - codec="jpeg" if issubclass(payload_type, Image) else "lcm", + payload_type=payload_type, + codec="cdr", ) ) topics[port] = transport.channel diff --git a/dimos/experimental/memory/rust_recorder.py b/dimos/experimental/memory/rust_recorder.py index 667945cce6d..396d5512dca 100644 --- a/dimos/experimental/memory/rust_recorder.py +++ b/dimos/experimental/memory/rust_recorder.py @@ -12,7 +12,7 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""Experimental native recorder for high-throughput LCM-backed memory streams.""" +"""Experimental native recorder for high-throughput CDR memory streams.""" from __future__ import annotations @@ -20,6 +20,7 @@ from pathlib import Path from typing import Annotated, Any, Literal, TypeAlias +from dimos_generated.tf2_msgs.msg import TFMessage from pydantic import BaseModel, ConfigDict, Field, field_validator, model_validator from dimos.constants import DIMOS_PROJECT_ROOT @@ -27,16 +28,14 @@ from dimos.core.module import Module from dimos.core.native_module import NativeModule, NativeModuleConfig from dimos.core.stream import In -from dimos.memory.module import OnExisting +from dimos.memory.recording_policy import OnExisting from dimos.memory.store.sqlite import SqliteStore -from dimos.msgs.sensor_msgs.Image import Image -from dimos.msgs.tf2_msgs.TFMessage import TFMessage from dimos.utils.data import backup_file from dimos.utils.logging_config import setup_logger logger = setup_logger() -_SUPPORTED_NATIVE_CODECS = {"lcm", "jpeg", "lz4+lcm"} +_SUPPORTED_NATIVE_CODECS = {"cdr", "lz4+cdr"} class RustStreamSpec(BaseModel): @@ -46,6 +45,21 @@ class RustStreamSpec(BaseModel): name: str payload_type: str codec: str + schema_name: str = Field(min_length=1) + schema_definition: str = Field(min_length=1) + + @classmethod + def from_type( + cls, *, port: str, name: str, payload_type: type[Any], codec: str + ) -> RustStreamSpec: + return cls( + port=port, + name=name, + codec=codec, + payload_type=f"{payload_type.__module__}.{payload_type.__qualname__}", + schema_name=payload_type.msg_name, + schema_definition=payload_type.schema, + ) class RustStoreConfig(BaseModel): @@ -86,7 +100,7 @@ class RustMcapStoreConfig(RustStoreConfig): class RustRecorderConfig(NativeModuleConfig): - """Compatibility-first configuration for :class:`RustRecorder`. + """Configuration for :class:`RustRecorder`. Python owns artifact lifecycle and stream registration. The native process receives only ``store``, ``encoding_threads``, and the internally resolved @@ -158,11 +172,11 @@ class RustRecorder(NativeModule): class CameraRecorder(RustRecorder): color_image: In[Image] - Both stores use the Python Mem2 codec configuration: images default to - JPEG, and ``stream_codecs`` may select ``lcm`` or ``lz4+lcm``. MCAP uses - indexed Zstd chunks around those storage-encoded observations. The native + Both stores accept generated CDR messages with complete schema metadata. + SQLite additionally accepts ``lz4+cdr`` blob compression. MCAP uses + indexed Zstd chunks around unwrapped CDR observations. The native path preserves source timestamps for common stamped message types. Other - LCM messages use their reception timestamp. Spatial pose attachment is not + timestamp layouts use their reception timestamp. Spatial pose attachment is not supported yet. """ @@ -188,6 +202,17 @@ def start(self) -> None: self.config.streams = specs super().start() + def _collect_topics(self) -> dict[str, str]: + """Launch only ports declared in the prepared recording configuration. + + A coordinator can wire the inherited TF input even when record_tf is + false. Passing that extra topic makes the native module reject startup. + """ + selected = {spec.port for spec in self.config.streams} + return { + name: topic for name, topic in super()._collect_topics().items() if name in selected + } + def _stream_specs(self) -> list[RustStreamSpec]: specs: list[RustStreamSpec] = [] for port_name, port in self.inputs.items(): @@ -196,11 +221,11 @@ def _stream_specs(self) -> list[RustStreamSpec]: if port is self.tf: if self.config.record_tf: specs.append( - RustStreamSpec( + RustStreamSpec.from_type( port=port_name, name="tf", - payload_type=f"{TFMessage.__module__}.{TFMessage.__qualname__}", - codec="lcm", + payload_type=TFMessage, + codec="cdr", ) ) continue @@ -209,10 +234,10 @@ def _stream_specs(self) -> list[RustStreamSpec]: codec = self.config.stream_codecs.get(stream_name, self._default_codec(port.type)) self._validate_codec(stream_name, port.type, codec) specs.append( - RustStreamSpec( + RustStreamSpec.from_type( port=port_name, name=stream_name, - payload_type=f"{port.type.__module__}.{port.type.__qualname__}", + payload_type=port.type, codec=codec, ) ) @@ -227,30 +252,27 @@ def _stream_specs(self) -> list[RustStreamSpec]: @staticmethod def _default_codec(payload_type: type[Any]) -> str: - if issubclass(payload_type, Image): - return "jpeg" - if hasattr(payload_type, "lcm_encode") and hasattr(payload_type, "lcm_decode"): - return "lcm" + if all( + hasattr(payload_type, member) for member in ("encode", "decode", "msg_name", "schema") + ): + return "cdr" raise TypeError( - f"RustRecorder only supports LCM-backed messages, got {payload_type.__qualname__}" + f"RustRecorder requires a generated CDR message, got {payload_type.__qualname__}" ) @staticmethod def _validate_codec(stream_name: str, payload_type: type[Any], codec: str) -> None: if codec not in _SUPPORTED_NATIVE_CODECS: raise ValueError( - f"Unsupported native codec {codec!r} for stream {stream_name!r}; " - f"choose one of {sorted(_SUPPORTED_NATIVE_CODECS)}" - ) - if codec == "jpeg" and not issubclass(payload_type, Image): - raise TypeError(f"JPEG codec requires Image, got {payload_type.__qualname__}") - if not hasattr(payload_type, "lcm_encode") or not hasattr(payload_type, "lcm_decode"): - raise TypeError( - f"Native codec {codec!r} requires an LCM-backed type, " - f"got {payload_type.__qualname__}" + f"Unsupported native codec {codec!r} for stream {stream_name!r}; choose one of {sorted(_SUPPORTED_NATIVE_CODECS)}" ) + RustRecorder._default_codec(payload_type) def _prepare_store(self, specs: list[RustStreamSpec]) -> None: + if self.config.store.kind == "mcap" and any(spec.codec != "cdr" for spec in specs): + raise ValueError( + "MCAP requires cdr channels; use chunk compression instead of payload wrappers" + ) path = Path(self.config.store.path) if self.config.store.kind == "mcap" and self.config.on_existing is OnExisting.APPEND: raise ValueError("MCAP append is unsupported; choose overwrite, backup, or error") diff --git a/dimos/experimental/memory/test_rust_cli_recorder.py b/dimos/experimental/memory/test_rust_cli_recorder.py index a767baa222a..d6040cf1f89 100644 --- a/dimos/experimental/memory/test_rust_cli_recorder.py +++ b/dimos/experimental/memory/test_rust_cli_recorder.py @@ -21,6 +21,8 @@ from types import SimpleNamespace from typing import Any +from dimos_generated.geometry_msgs.msg import PoseStamped +from dimos_generated.sensor_msgs.msg import Image import pytest from pytest_mock import MockerFixture @@ -30,8 +32,6 @@ from dimos.experimental.memory.rust_cli_recorder import RustRecordingPlan, RustRecordingSession from dimos.experimental.memory.rust_recorder import RustStreamSpec from dimos.memory.store.sqlite import SqliteStore -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.sensor_msgs.Image import Image from dimos.protocol.pubsub.impl.zenohpubsub import Topic as ZenohTopic @@ -67,8 +67,8 @@ def test_plan_uses_actual_lcm_channels_and_memory_codecs(tmp_path: Path) -> None assert plan.backend == "lcm" assert plan.topics == {"stream_0": odom.channel, "stream_1": camera.channel} assert [(stream.name, stream.codec) for stream in plan.streams] == [ - ("odom", "lcm"), - ("color_image", "jpeg"), + ("odom", "cdr"), + ("color_image", "cdr"), ] assert plan.path == tmp_path / "memory.db" @@ -190,11 +190,11 @@ def _plan(tmp_path: Path) -> RustRecordingPlan: backend="lcm", topics={"stream_0": "/odom"}, streams=[ - RustStreamSpec( + RustStreamSpec.from_type( port="stream_0", name="odom", - payload_type="dimos.msgs.geometry_msgs.PoseStamped.PoseStamped", - codec="lcm", + payload_type=PoseStamped, + codec="cdr", ) ], payload_types={"odom": PoseStamped}, diff --git a/dimos/experimental/memory/test_rust_recorder.py b/dimos/experimental/memory/test_rust_recorder.py index c8a8e40997e..8f7daeaa4bf 100644 --- a/dimos/experimental/memory/test_rust_recorder.py +++ b/dimos/experimental/memory/test_rust_recorder.py @@ -17,6 +17,8 @@ from pathlib import Path from typing import Any, TypeVar +from dimos_generated.geometry_msgs.msg import PoseStamped +from dimos_generated.sensor_msgs.msg import Image import pytest from dimos.core.stream import In @@ -26,10 +28,8 @@ RustRecorderConfig, RustSqliteStoreConfig, ) -from dimos.memory.module import OnExisting +from dimos.memory.recording_policy import OnExisting from dimos.memory.store.sqlite import SqliteStore -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.sensor_msgs.Image import Image class SampleRustRecorder(RustRecorder): @@ -84,7 +84,7 @@ def test_specs_use_native_defaults_remapping_and_configured_workers( record_tf=False, encoding_threads=7, stream_remapping={"odometry": "pose"}, - stream_codecs={"color_image": "lz4+lcm"}, + stream_codecs={"color_image": "lz4+cdr"}, ) connect(recorder, color_image="/camera", odometry="/odom") @@ -101,14 +101,18 @@ def test_specs_use_native_defaults_remapping_and_configured_workers( { "port": "color_image", "name": "color_image", - "payload_type": "dimos.msgs.sensor_msgs.Image.Image", - "codec": "lz4+lcm", + "payload_type": f"{Image.__module__}.{Image.__qualname__}", + "schema_name": Image.msg_name, + "schema_definition": Image.schema, + "codec": "lz4+cdr", }, { "port": "odometry", "name": "pose", - "payload_type": "dimos.msgs.geometry_msgs.PoseStamped.PoseStamped", - "codec": "lcm", + "payload_type": f"{PoseStamped.__module__}.{PoseStamped.__qualname__}", + "schema_name": PoseStamped.msg_name, + "schema_definition": PoseStamped.schema, + "codec": "cdr", }, ] assert set(config) == {"encoding_threads", "store", "streams"} @@ -126,7 +130,7 @@ def test_store_preparation_creates_a_python_readable_registry( connect(recorder, color_image="/camera", odometry="/odom") specs = recorder._stream_specs() - assert [spec.codec for spec in specs] == ["jpeg", "lcm"] + assert [spec.codec for spec in specs] == ["cdr", "cdr"] recorder._prepare_store(specs) @@ -166,7 +170,7 @@ def test_unsupported_python_payload_fails_before_native_process_starts( ) connect(recorder, values="/values") - with pytest.raises(TypeError, match="only supports LCM-backed messages"): + with pytest.raises(TypeError, match="requires a generated CDR message"): recorder._stream_specs() @@ -204,7 +208,7 @@ def test_invalid_codec_fails_during_preflight(tmp_path: Path, make_recorder: Any recorder._stream_specs() -def test_jpeg_requires_an_image_stream(tmp_path: Path, make_recorder: Any) -> None: +def test_obsolete_jpeg_codec_is_rejected(tmp_path: Path, make_recorder: Any) -> None: recorder = make_recorder( SampleRustRecorder, store=RustSqliteStoreConfig(path=str(tmp_path / "recording.db")), @@ -213,7 +217,7 @@ def test_jpeg_requires_an_image_stream(tmp_path: Path, make_recorder: Any) -> No ) connect(recorder, odometry="/odom") - with pytest.raises(TypeError, match="JPEG codec requires Image"): + with pytest.raises(ValueError, match="Unsupported native codec"): recorder._stream_specs() @@ -243,7 +247,7 @@ def test_mcap_store_uses_python_codec_defaults_and_does_not_precreate_the_artifa specs = recorder._stream_specs() recorder._prepare_store(specs) - assert [spec.codec for spec in specs] == ["jpeg", "lcm"] + assert [spec.codec for spec in specs] == ["cdr", "cdr"] assert not path.exists() recorder.config.streams = specs assert recorder.config.to_config_dict()["store"] == { @@ -258,11 +262,13 @@ def test_mcap_accepts_storage_codecs_and_rejects_append(tmp_path: Path, make_rec SampleRustRecorder, store=RustMcapStoreConfig(path=str(path)), record_tf=False, - stream_codecs={"color_image": "jpeg"}, + stream_codecs={"color_image": "lz4+cdr"}, ) connect(recorder, color_image="/camera") - assert [spec.codec for spec in recorder._stream_specs()] == ["jpeg"] + with pytest.raises(ValueError, match="MCAP requires cdr"): + recorder._prepare_store(recorder._stream_specs()) + assert not path.exists() append_recorder = make_recorder( SampleRustRecorder, @@ -304,3 +310,18 @@ def test_duplicate_remapped_stream_names_fail_before_launch( with pytest.raises(ValueError, match="Duplicate recorded stream names"): recorder._stream_specs() + + +@pytest.mark.parametrize("record_tf", [False, True]) +def test_launch_topics_match_selected_streams_with_connected_tf(tmp_path, make_recorder, record_tf): + recorder = make_recorder( + SampleRustRecorder, + store=RustMcapStoreConfig(path=str(tmp_path / "recording.mcap")), + record_tf=record_tf, + ) + connect(recorder, odometry="/odom", tf="/tf") + recorder.config.streams = recorder._stream_specs() + topics = recorder._collect_topics() + expected = {"odometry": "/odom", "tf": "/tf"} if record_tf else {"odometry": "/odom"} + assert topics == expected + assert set(topics) == {spec.port for spec in recorder.config.streams} diff --git a/dimos/experimental/memory/test_rust_recorder_e2e.py b/dimos/experimental/memory/test_rust_recorder_e2e.py index d26a261651d..e6faa59c1de 100644 --- a/dimos/experimental/memory/test_rust_recorder_e2e.py +++ b/dimos/experimental/memory/test_rust_recorder_e2e.py @@ -25,6 +25,12 @@ from typing import cast import uuid +from dimos_generated.builtin_interfaces.msg import Time +from dimos_generated.geometry_msgs.msg import Transform, TransformStamped, Vector3 +from dimos_generated.sensor_msgs.msg import Image, Imu +from dimos_generated.std_msgs.msg import Header +from dimos_generated.tf2_msgs.msg import TFMessage +from mcap.reader import make_reader import numpy as np import pytest @@ -40,23 +46,23 @@ RustRecordingStoreConfig, RustSqliteStoreConfig, ) -from dimos.memory.codecs.lcm import LcmCodec -from dimos.memory.codecs.lz4 import Lz4Codec from dimos.memory.store.mcap import McapStore from dimos.memory.store.sqlite import SqliteStore from dimos.memory.type.observation import Observation -from dimos.msgs.geometry_msgs.Transform import Transform -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.msgs.sensor_msgs.Image import Image, ImageFormat -from dimos.msgs.sensor_msgs.Imu import Imu -from dimos.msgs.tf2_msgs.TFMessage import TFMessage +from dimos.msgs.image import image_from_array, image_view +from dimos.msgs.time import time_from_seconds from dimos.protocol.pubsub.impl.zenohpubsub import Topic as ZenohTopic from dimos.protocol.service.zenohservice import ZenohConfig, ZenohSessionPool pytestmark = pytest.mark.self_hosted _RUST_PACKAGE = DIMOS_PROJECT_ROOT / "dimos" / "experimental" / "memory" / "rust" -_EXECUTABLE = _RUST_PACKAGE / "result" / "bin" / "dimos-memory-recorder" +_EXECUTABLE = Path( + os.environ.get( + "DIMOS_RECORDER_TEST_EXECUTABLE", + str(_RUST_PACKAGE / "result" / "bin" / "dimos-memory-recorder"), + ) +) _MCAP_AVAILABLE = importlib.util.find_spec("mcap") is not None @@ -155,7 +161,7 @@ def test_rust_artifact_is_readable_by_python_memory2( store=store, record_tf=False, encoding_threads=2, - stream_codecs={"imu": "lz4+lcm"}, + stream_codecs={"imu": "lz4+cdr" if store_kind == "sqlite" else "cdr"}, session=ZenohConfig( mode="peer", connect=[], @@ -214,15 +220,13 @@ def test_rust_artifact_is_readable_by_python_memory2( _wait_for_log(process, "memory recorder ready") expected = Imu( - ts=12.5, - frame_id="imu_link", - angular_velocity=Vector3(1.0, 2.0, 3.0), + header=Header(stamp=Time(sec=1700000000, nanosec=123456789), frame_id="imu_link"), + angular_velocity=Vector3(x=1.0, y=2.0, z=3.0), ) - expected_image = Image( - data=np.full((16, 16, 3), [20, 80, 140], dtype=np.uint8), - format=ImageFormat.RGB, - frame_id="camera", - ts=12.75, + expected_image = image_from_array( + np.full((16, 16, 3), [20, 80, 140], dtype=np.uint8), + encoding="rgb8", + header=Header(frame_id="camera", stamp=time_from_seconds(12.75)), ) publisher.broadcast(None, expected) image_publisher.broadcast(None, expected_image) @@ -242,24 +246,25 @@ def test_rust_artifact_is_readable_by_python_memory2( if store_kind == "sqlite": memory = SqliteStore(path=str(artifact)) else: - memory = McapStore( - path=str(artifact), - codecs={"imu": Lz4Codec(LcmCodec(Imu))}, - ) + with artifact.open("rb") as file: + recorded_imu = [ + row + for _schema, channel, row in make_reader(file).iter_messages() + if channel.topic == "imu" + ] + assert len(recorded_imu) == 1 + assert recorded_imu[0].publish_time == 1700000000123456789 + memory = McapStore(path=str(artifact)) with memory: observation = cast("Observation[Imu]", memory.stream("imu").first()) - assert observation.ts == 12.5 - assert observation.data.lcm_encode() == expected.lcm_encode() + assert observation.ts == 1700000000.123456789 + assert observation.data.encode() == expected.encode() image_observation = cast("Observation[Image]", memory.stream("color_image").first()) decoded_image = image_observation.data assert image_observation.ts == 12.75 - assert decoded_image.frame_id == "camera" - assert decoded_image.format is ImageFormat.RGB - assert decoded_image.data.shape == expected_image.data.shape - assert ( - np.mean(np.abs(decoded_image.data.astype(float) - expected_image.data.astype(float))) - < 5 - ) + assert decoded_image.header == expected_image.header + assert decoded_image.encoding == "rgb8" + np.testing.assert_array_equal(image_view(decoded_image), image_view(expected_image)) @pytest.mark.parametrize( @@ -288,7 +293,10 @@ def test_cli_recording_uses_existing_binary_for_both_formats( channel = f"/rust-recorder-{uuid.uuid4().hex[:8]}" publisher: LCMTransport[Imu] = LCMTransport(channel, Imu, url=lcm_url) session = RustRecordingSession(rust_cli_recorder.make_plan({("imu", Imu): publisher})) - expected = Imu(ts=22.5, frame_id="imu_link", angular_velocity=Vector3(1.0, 2.0, 3.0)) + expected = Imu( + header=Header(stamp=time_from_seconds(22.5), frame_id="imu_link"), + angular_velocity=Vector3(x=1, y=2, z=3), + ) try: publisher.start() session.start() @@ -308,11 +316,11 @@ def test_cli_recording_uses_existing_binary_for_both_formats( if store_kind == "sqlite": memory = SqliteStore(path=str(artifact)) else: - memory = McapStore(path=str(artifact), codecs={"imu": LcmCodec(Imu)}) + memory = McapStore(path=str(artifact)) with memory: observation = cast("Observation[Imu]", memory.stream("imu").first()) assert observation.ts == 22.5 - assert observation.data.lcm_encode() == expected.lcm_encode() + assert observation.data.encode() == expected.encode() def test_tf_records_over_zenoh_and_replays_through_python( @@ -368,18 +376,18 @@ def test_tf_records_over_zenoh_and_replays_through_python( stderr=subprocess.PIPE, ) expected = TFMessage( - Transform( - translation=Vector3(1.0, 2.0, 3.0), - frame_id="world", - child_frame_id="base_link", - ts=10.25, - ), - Transform( - translation=Vector3(4.0, 5.0, 6.0), - frame_id="base_link", - child_frame_id="camera", - ts=11.5, - ), + transforms=[ + TransformStamped( + header=Header(frame_id="world", stamp=time_from_seconds(10.25)), + child_frame_id="base_link", + transform=Transform(translation=Vector3(x=1, y=2, z=3)), + ), + TransformStamped( + header=Header(frame_id="base_link", stamp=time_from_seconds(11.5)), + child_frame_id="camera", + transform=Transform(translation=Vector3(x=4, y=5, z=6)), + ), + ] ) try: assert process.stdin is not None diff --git a/dimos/experimental/robot/bosdyn/spot/effectors/high_level.py b/dimos/experimental/robot/bosdyn/spot/effectors/high_level.py index b713e1cdd1d..a465b40124c 100644 --- a/dimos/experimental/robot/bosdyn/spot/effectors/high_level.py +++ b/dimos/experimental/robot/bosdyn/spot/effectors/high_level.py @@ -43,6 +43,21 @@ import time from typing import Any +from dimos_generated.geometry_msgs.msg import ( + Point, + Pose, + PoseWithCovariance, + Quaternion, + TransformStamped, + Twist, + TwistWithCovariance, + Vector3, +) +from dimos_generated.nav_msgs.msg import Odometry +from dimos_generated.sensor_msgs.msg import CameraInfo, Image +from dimos_generated.std_msgs.msg import Header +from dimos_generated.tf2_msgs.msg import TFMessage + from dimos.agents.annotation import skill from dimos.core.core import rpc from dimos.core.stream import In, Out @@ -71,15 +86,8 @@ rotate_camera_info_quarter_turns, rotate_image_quarter_turns, ) -from dimos.msgs.geometry_msgs.Pose import Pose -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Transform import Transform -from dimos.msgs.geometry_msgs.Twist import Twist -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.msgs.nav_msgs.Odometry import Odometry -from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo -from dimos.msgs.sensor_msgs.Image import Image -from dimos.msgs.tf2_msgs.TFMessage import TFMessage +from dimos.msgs.geometry import transform_from_odometry +from dimos.msgs.time import time_from_seconds, to_seconds from dimos.protocol.tf.static_tf_publisher import StaticTfPublisher, StaticTfPublisherConfig from dimos.utils.logging_config import setup_logger @@ -168,7 +176,7 @@ def __init__(self, *args: Any, **kwargs: Any) -> None: # never reaches a half-initialised SDK. self._ready = asyncio.Event() - def transforms(self) -> list[Transform]: + def transforms(self) -> list[TransformStamped]: """Static base_link -> camera-optical extrinsics parsed from the URDF. `StaticTfPublisher` republishes these on a fixed interval; the moving @@ -410,10 +418,12 @@ async def _poll_images(self) -> None: image = decode_image(response, frame_id, time_converter) if image is None: continue - if last_published_ts.get(source_name) == image.ts: + if last_published_ts.get(source_name) == to_seconds(image.header.stamp): continue - last_published_ts[source_name] = image.ts - camera_info = camera_info_from_response(response, frame_id, image.ts) + last_published_ts[source_name] = to_seconds(image.header.stamp) + camera_info = camera_info_from_response( + response, frame_id, to_seconds(image.header.stamp) + ) if quarter_turns: image = rotate_image_quarter_turns(image, quarter_turns) if camera_info is not None: @@ -466,44 +476,26 @@ async def _poll_odom(self) -> None: def _publish_odom(self, vision_tform_body: Any, velocity: Any, ts: float) -> None: pose = Pose( - position=[vision_tform_body.x, vision_tform_body.y, vision_tform_body.z], - orientation=[ - vision_tform_body.rot.x, - vision_tform_body.rot.y, - vision_tform_body.rot.z, - vision_tform_body.rot.w, - ], + position=Point(x=vision_tform_body.x, y=vision_tform_body.y, z=vision_tform_body.z), + orientation=Quaternion( + x=vision_tform_body.rot.x, + y=vision_tform_body.rot.y, + z=vision_tform_body.rot.z, + w=vision_tform_body.rot.w, + ), ) twist = Twist( - linear=[velocity.linear.x, velocity.linear.y, velocity.linear.z], - angular=[velocity.angular.x, velocity.angular.y, velocity.angular.z], + linear=Vector3(x=velocity.linear.x, y=velocity.linear.y, z=velocity.linear.z), + angular=Vector3(x=velocity.angular.x, y=velocity.angular.y, z=velocity.angular.z), ) odometry = Odometry( - ts=ts, - frame_id=self.config.odom_frame_id, + header=Header(stamp=time_from_seconds(ts), frame_id=self.config.odom_frame_id), child_frame_id=self.config.base_frame_id, - pose=pose, - twist=twist, + pose=PoseWithCovariance(pose=pose), + twist=TwistWithCovariance(twist=twist), ) self.odometry.publish(odometry) - self.tf.publish( - TFMessage( - Transform( - translation=Vector3( - vision_tform_body.x, vision_tform_body.y, vision_tform_body.z - ), - rotation=Quaternion( - vision_tform_body.rot.x, - vision_tform_body.rot.y, - vision_tform_body.rot.z, - vision_tform_body.rot.w, - ), - frame_id=self.config.odom_frame_id, - child_frame_id=self.config.base_frame_id, - ts=ts, - ) - ) - ) + self.tf.publish(TFMessage(transforms=[transform_from_odometry(odometry)])) @rpc async def move(self, twist: Twist, duration: float = 0.0) -> bool: @@ -569,7 +561,7 @@ async def move_velocity( yaw: Rotational velocity (rad/s). duration: Seconds to move. 0 uses one `cmd_vel_timeout` window. """ - twist = Twist(linear=Vector3(x, y, 0), angular=Vector3(0, 0, yaw)) + twist = Twist(linear=Vector3(x=x, y=y), angular=Vector3(z=yaw)) if await self.move(twist, duration=duration): return f"Moving with velocity=({x}, {y}, {yaw}) for {duration} seconds" return f"Failed to move with velocity=({x}, {y}, {yaw})" diff --git a/dimos/experimental/robot/bosdyn/spot/effectors/test_high_level.py b/dimos/experimental/robot/bosdyn/spot/effectors/test_high_level.py new file mode 100644 index 00000000000..fe8905fbc63 --- /dev/null +++ b/dimos/experimental/robot/bosdyn/spot/effectors/test_high_level.py @@ -0,0 +1,65 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""The Spot SDK boundary publishes generated odometry and its matching TF edge.""" + +from types import SimpleNamespace + +from dimos_generated.nav_msgs.msg import Odometry +from dimos_generated.tf2_msgs.msg import TFMessage +import pytest + +from dimos.experimental.robot.bosdyn.spot.effectors.high_level import SpotHighLevel + + +@pytest.fixture +def module(): + instance = SpotHighLevel(odom_frame_id="vision", base_frame_id="body") + try: + yield instance + finally: + instance.stop() + + +def test_odometry_and_tf_preserve_sdk_pose_velocity_and_source_time(module, mocker): + odometry_publish = mocker.patch.object(module.odometry, "publish") + tf_publish = mocker.patch.object(module.tf, "publish") + pose = SimpleNamespace(x=1.0, y=2.0, z=3.0, rot=SimpleNamespace(x=0, y=0, z=0.6, w=0.8)) + velocity = SimpleNamespace( + linear=SimpleNamespace(x=4, y=5, z=6), angular=SimpleNamespace(x=7, y=8, z=9) + ) + + module._publish_odom(pose, velocity, -0.5) + + odometry_publish.assert_called_once() + tf_publish.assert_called_once() + odometry = Odometry.decode(odometry_publish.call_args.args[0].encode()) + tf = TFMessage.decode(tf_publish.call_args.args[0].encode()) + assert odometry.header.frame_id == "vision" + assert odometry.child_frame_id == "body" + assert (odometry.header.stamp.sec, odometry.header.stamp.nanosec) == (-1, 500000000) + assert ( + odometry.pose.pose.position.x, + odometry.pose.pose.position.y, + odometry.pose.pose.position.z, + ) == (1, 2, 3) + assert odometry.pose.pose.orientation.z == 0.6 + assert odometry.twist.twist.linear.y == 5 + assert odometry.twist.twist.angular.z == 9 + assert len(tf.transforms) == 1 + edge = tf.transforms[0] + assert edge.header == odometry.header + assert edge.child_frame_id == odometry.child_frame_id + assert edge.transform.translation.x == odometry.pose.pose.position.x + assert edge.transform.rotation == odometry.pose.pose.orientation diff --git a/dimos/experimental/robot/bosdyn/spot/recorder.py b/dimos/experimental/robot/bosdyn/spot/recorder.py index 8a7f0e3968d..9e865c1024d 100644 --- a/dimos/experimental/robot/bosdyn/spot/recorder.py +++ b/dimos/experimental/robot/bosdyn/spot/recorder.py @@ -25,17 +25,17 @@ from datetime import datetime from pathlib import Path +from dimos_generated.nav_msgs.msg import Odometry +from dimos_generated.sensor_msgs.msg import CameraInfo, Image from pydantic import Field from dimos.core.stream import In from dimos.experimental.robot.bosdyn.spot.config import CAMERA_STREAM_SUFFIXES -from dimos.memory.module import OnExisting, Recorder, RecorderConfig -from dimos.msgs.nav_msgs.Odometry import Odometry -from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo -from dimos.msgs.sensor_msgs.Image import Image +from dimos.memory.module import Recorder, RecorderConfig +from dimos.memory.recording_policy import OnExisting # jpeg codec quantises depth it to ~25cm and adds block artifacts (horrible) -LOSSLESS_CODEC = "lz4+lcm" +LOSSLESS_CODEC = "lz4+cdr" class SpotRecorderConfig(RecorderConfig): diff --git a/dimos/experimental/robot/bosdyn/spot/replay.py b/dimos/experimental/robot/bosdyn/spot/replay.py index e3da31702b8..14d1e45b92f 100644 --- a/dimos/experimental/robot/bosdyn/spot/replay.py +++ b/dimos/experimental/robot/bosdyn/spot/replay.py @@ -27,9 +27,13 @@ from __future__ import annotations from collections.abc import AsyncIterator -import math from pathlib import Path +from dimos_generated.geometry_msgs.msg import TransformStamped +from dimos_generated.nav_msgs.msg import Odometry +from dimos_generated.sensor_msgs.msg import CameraInfo, Image +from dimos_generated.tf2_msgs.msg import TFMessage + from dimos.constants import RECORDINGS_DIR from dimos.core.module import Module, ModuleConfig from dimos.core.stream import Out @@ -37,15 +41,9 @@ CAMERA_STREAM_SUFFIXES, FRONT_CAMERA_ROTATE_UPRIGHT, ) +from dimos.experimental.robot.bosdyn.spot.utils import roll_optical_frame from dimos.memory.replay import resolve_db_path from dimos.memory.store.sqlite import SqliteStore -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Transform import Transform -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.msgs.nav_msgs.Odometry import Odometry -from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo -from dimos.msgs.sensor_msgs.Image import Image -from dimos.msgs.tf2_msgs.TFMessage import TFMessage from dimos.utils.logging_config import setup_logger logger = setup_logger() @@ -122,23 +120,18 @@ def _resolve_db_path(self) -> Path: def _republish_tf(self, message: TFMessage) -> None: self.tf.publish( - TFMessage(*(self._roll_optical_frame(transform) for transform in message.transforms)) + TFMessage( + transforms=[self._roll_optical_frame(transform) for transform in message.transforms] + ) ) - def _roll_optical_frame(self, transform: Transform) -> Transform: + def _roll_optical_frame(self, transform: TransformStamped) -> TransformStamped: if not self.config.roll_front_frames: return transform turns = _OPTICAL_FRAME_ROLL_TURNS.get(transform.child_frame_id) if not turns: return transform - roll = Quaternion.from_euler(Vector3(0.0, 0.0, turns * math.pi / 2)) - return Transform( - translation=transform.translation, - rotation=transform.rotation * roll, - frame_id=transform.frame_id, - child_frame_id=transform.child_frame_id, - ts=transform.ts, - ) + return roll_optical_frame(transform, turns) async def main(self) -> AsyncIterator[None]: db_path = self._resolve_db_path() diff --git a/dimos/experimental/robot/bosdyn/spot/rerun.py b/dimos/experimental/robot/bosdyn/spot/rerun.py index 26cfe3efa29..a40b790fb30 100644 --- a/dimos/experimental/robot/bosdyn/spot/rerun.py +++ b/dimos/experimental/robot/bosdyn/spot/rerun.py @@ -18,15 +18,17 @@ from typing import TYPE_CHECKING, Any +from dimos.msgs.time import to_seconds from dimos.protocol.tf.tf import MultiTBuffer +from dimos.visualization.rerun.message_helpers import image_archetype, tf_archetypes if TYPE_CHECKING: from collections.abc import Callable + from dimos_generated.sensor_msgs.msg import CameraInfo, Image + from dimos_generated.tf2_msgs.msg import TFMessage import rerun.blueprint as rrb - from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo - from dimos.msgs.tf2_msgs.TFMessage import TFMessage from dimos.visualization.rerun.bridge import RerunData # Optical tf frame_id (SpotHighLevelConfig defaults) -> stream-name suffix. @@ -88,11 +90,11 @@ def _camera_entity(origin: str) -> str: def _tf_to_rerun(tf_message: TFMessage) -> RerunData: """Log tf as usual, and keep a copy for the camera pose lookups.""" _tf_buffer.receive_tfmessage(tf_message) - return tf_message.to_rerun() + return [(path, archetype) for path, archetype in tf_archetypes(tf_message)] def _camera_info_pinhole(camera_info: CameraInfo, origin: Callable[[str], str]) -> RerunData | None: - suffix = _OPTICAL_FRAME_TO_SUFFIX.get(camera_info.frame_id) + suffix = _OPTICAL_FRAME_TO_SUFFIX.get(camera_info.header.frame_id) if suffix is None: return None import rerun as rr @@ -101,8 +103,8 @@ def _camera_info_pinhole(camera_info: CameraInfo, origin: Callable[[str], str]) ( _camera_entity(origin(suffix)), rr.Pinhole( - focal_length=[camera_info.K[0], camera_info.K[4]], - principal_point=[camera_info.K[2], camera_info.K[5]], + focal_length=[camera_info.k[0], camera_info.k[4]], + principal_point=[camera_info.k[2], camera_info.k[5]], width=camera_info.width, height=camera_info.height, image_plane_distance=_FRUSTUM_PLANE_DISTANCE, @@ -237,11 +239,11 @@ class _ImageBakedIntoAnchor: def __init__(self, origin: str) -> None: self.origin = origin - def __call__(self, image: Any) -> RerunData | None: + def __call__(self, image: Image) -> RerunData | None: anchor = _anchor_frame() if anchor is None: return None - pose = _tf_buffer.get(anchor, image.frame_id, image.ts) + pose = _tf_buffer.get(anchor, image.header.frame_id, to_seconds(image.header.stamp)) if pose is None: return None import rerun as rr @@ -250,10 +252,21 @@ def __call__(self, image: Any) -> RerunData | None: ( self.origin, rr.Transform3D( - translation=[pose.translation.x, pose.translation.y, pose.translation.z], - rotation=pose.rotation.to_rerun(), + translation=[ + pose.transform.translation.x, + pose.transform.translation.y, + pose.transform.translation.z, + ], + quaternion=rr.Quaternion( + xyzw=[ + pose.transform.rotation.x, + pose.transform.rotation.y, + pose.transform.rotation.z, + pose.transform.rotation.w, + ] + ), parent_frame=f"tf#/{anchor}", ), ), - (_camera_entity(self.origin), image.to_rerun()), + (_camera_entity(self.origin), image_archetype(image)), ] diff --git a/dimos/experimental/robot/bosdyn/spot/test_utils.py b/dimos/experimental/robot/bosdyn/spot/test_utils.py index 2be357127bc..4acbe37c943 100644 --- a/dimos/experimental/robot/bosdyn/spot/test_utils.py +++ b/dimos/experimental/robot/bosdyn/spot/test_utils.py @@ -12,9 +12,30 @@ # See the License for the specific language governing permissions and # limitations under the License. +import math from pathlib import Path +import sys +from types import ModuleType, SimpleNamespace -from dimos.experimental.robot.bosdyn.spot.utils import camera_mount_transforms +import cv2 +from dimos_generated.builtin_interfaces.msg import Time +from dimos_generated.geometry_msgs.msg import Transform, TransformStamped, Vector3 +from dimos_generated.sensor_msgs.msg import Image +from dimos_generated.std_msgs.msg import Header +import numpy as np +import pytest + +from dimos.experimental.robot.bosdyn.spot.utils import ( + camera_info_from_response, + camera_mount_transforms, + decode_image, + roll_optical_frame, + rotate_camera_info_quarter_turns, + rotate_image_quarter_turns, +) +from dimos.msgs.camera_info import camera_info_from_intrinsics +from dimos.msgs.geometry import yaw +from dimos.msgs.image import image_from_array, image_view def test_camera_mount_transforms_uses_loaded_robot_topology(tmp_path: Path) -> None: @@ -42,6 +63,123 @@ def test_camera_mount_transforms_uses_loaded_robot_topology(tmp_path: Path) -> N transforms = camera_mount_transforms(urdf, "body", ["camera_optical"]) assert len(transforms) == 1 - assert transforms[0].frame_id == "body" + assert transforms[0].header.frame_id == "body" assert transforms[0].child_frame_id == "camera_optical" - assert transforms[0].translation.to_list() == [1.0, 2.0, 0.0] + translation = transforms[0].transform.translation + assert (translation.x, translation.y, translation.z) == (1.0, 2.0, 0.0) + + +@pytest.mark.parametrize("turns", [-1, 0, 1]) +def test_optical_roll_preserves_mount_and_exact_stamp(turns): + edge = TransformStamped( + header=Header(frame_id="body", stamp=Time(sec=1700000000, nanosec=123456789)), + child_frame_id="optical", + transform=Transform(translation=Vector3(x=1, y=2, z=3)), + ) + rolled = roll_optical_frame(edge, turns) + assert rolled.header == edge.header + assert rolled.child_frame_id == edge.child_frame_id + assert rolled.transform.translation == edge.transform.translation + assert yaw(rolled.transform.rotation) == pytest.approx(turns * math.pi / 2) + assert edge.transform.rotation.w == 1 + + +@pytest.fixture +def image_sdk(monkeypatch): + constants = SimpleNamespace( + FORMAT_JPEG=1, + FORMAT_RAW=2, + PIXEL_FORMAT_GREYSCALE_U8=1, + PIXEL_FORMAT_RGB_U8=3, + PIXEL_FORMAT_RGBA_U8=4, + PIXEL_FORMAT_DEPTH_U16=5, + PIXEL_FORMAT_GREYSCALE_U16=6, + ) + root = ModuleType("bosdyn") + root.__path__ = [] + api = ModuleType("bosdyn.api") + api.image_pb2 = SimpleNamespace(Image=constants) + monkeypatch.setitem(sys.modules, "bosdyn", root) + monkeypatch.setitem(sys.modules, "bosdyn.api", api) + return constants + + +@pytest.mark.parametrize( + "encoding,pixel_format,dtype,channels", + [ + ("mono8", 1, np.uint8, 1), + ("rgb8", 3, np.uint8, 3), + ("rgba8", 4, np.uint8, 4), + ("16UC1", 5, np.uint16, 1), + ("mono16", 6, np.uint16, 1), + ], +) +def test_sdk_raw_camera_boundary_emits_generated_pixels( + image_sdk, encoding, pixel_format, dtype, channels +): + shape = (2, 3) if channels == 1 else (2, 3, channels) + pixels = np.arange(np.prod(shape), dtype=dtype).reshape(shape) + shot = SimpleNamespace( + format=image_sdk.FORMAT_RAW, + pixel_format=pixel_format, + rows=2, + cols=3, + data=pixels.tobytes(), + ) + response = SimpleNamespace(shot=SimpleNamespace(image=shot, acquisition_time="robot-clock")) + clock = SimpleNamespace(local_seconds_from_robot_timestamp=lambda stamp: 1700000000.25) + value = decode_image(response, "camera-optical", clock) + assert type(value) is Image and value.encoding == encoding + decoded = Image.decode(value.encode()) + assert decoded.header.frame_id == "camera-optical" + assert decoded.header.stamp == Time(sec=1700000000, nanosec=250000000) + np.testing.assert_array_equal(image_view(decoded), pixels) + + +def test_sdk_jpeg_camera_boundary_decodes_generated_image(image_sdk): + pixels = np.full((8, 8, 3), [20, 80, 140], dtype=np.uint8) + ok, jpeg = cv2.imencode(".jpg", pixels) + assert ok + shot = SimpleNamespace(format=image_sdk.FORMAT_JPEG, pixel_format=3, data=jpeg.tobytes()) + response = SimpleNamespace(shot=SimpleNamespace(image=shot, acquisition_time="robot-clock")) + clock = SimpleNamespace(local_seconds_from_robot_timestamp=lambda stamp: 1700000000.25) + value = decode_image(response, "camera-optical", clock) + assert type(value) is Image and value.encoding == "bgr8" + np.testing.assert_allclose(image_view(Image.decode(value.encode())), pixels, atol=2) + + +@pytest.mark.parametrize("turns", [-1, 0, 1, 2, 4]) +def test_generated_camera_rotation_preserves_depth_header_and_intrinsics(turns): + header = Header(frame_id="optical", stamp=Time(sec=-1, nanosec=123456789)) + pixels = np.arange(6, dtype=np.uint16).reshape(2, 3) * 1000 + image = image_from_array(pixels, encoding="16UC1", header=header) + info = camera_info_from_intrinsics(10, 20, 1, 0.5, 3, 2, header=header) + before_image, before_info = image.encode(), info.encode() + rotated = rotate_image_quarter_turns(image, turns) + calibration = rotate_camera_info_quarter_turns(info, turns) + np.testing.assert_array_equal(image_view(rotated), np.rot90(pixels, k=turns)) + assert rotated.header == calibration.header == header + assert (calibration.height, calibration.width) == image_view(rotated).shape + expected_fx, expected_fy, cx, cy, width, height = 10, 20, 1, 0.5, 3, 2 + for _ in range(turns % 4): + expected_fx, expected_fy = expected_fy, expected_fx + cx, cy = cy, width - 1 - cx + width, height = height, width + assert list(calibration.k) == [expected_fx, 0, cx, 0, expected_fy, cy, 0, 0, 1] + assert image.encode() == before_image and info.encode() == before_info + + +def test_sdk_calibration_boundary_emits_plain_generated_value(): + source = SimpleNamespace( + HasField=lambda name: name == "pinhole", + cols=6, + rows=4, + pinhole=SimpleNamespace( + intrinsics=SimpleNamespace( + focal_length=SimpleNamespace(x=10, y=20), principal_point=SimpleNamespace(x=3, y=2) + ) + ), + ) + value = camera_info_from_response(SimpleNamespace(source=source), "optical", 1700000000.25) + assert list(value.k) == [10, 0, 3, 0, 20, 2, 0, 0, 1] + assert value.header.stamp == Time(sec=1700000000, nanosec=250000000) diff --git a/dimos/experimental/robot/bosdyn/spot/utils.py b/dimos/experimental/robot/bosdyn/spot/utils.py index d5e6f77fab4..30e5d7c80b3 100644 --- a/dimos/experimental/robot/bosdyn/spot/utils.py +++ b/dimos/experimental/robot/bosdyn/spot/utils.py @@ -20,13 +20,15 @@ from pathlib import Path from typing import Any +from dimos_generated.geometry_msgs.msg import Transform, TransformStamped, Vector3 +from dimos_generated.sensor_msgs.msg import CameraInfo, Image +from dimos_generated.std_msgs.msg import Header import numpy as np -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Transform import Transform -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo -from dimos.msgs.sensor_msgs.Image import Image, ImageFormat +from dimos.msgs.camera_info import camera_info_from_intrinsics +from dimos.msgs.geometry import compose_transforms, quaternion_from_euler +from dimos.msgs.image import image_from_array, image_view +from dimos.msgs.time import time_from_seconds from dimos.robot.assets.model import JointDescription, RobotModel from dimos.utils.logging_config import setup_logger @@ -56,8 +58,12 @@ def decode_image(response: Any, frame_id: str, time_converter: Any) -> Image | N if decoded is None: logger.error(f"Failed to decode JPEG image from {frame_id}") return None - image_format = ImageFormat.GRAY if decoded.ndim == 2 else ImageFormat.BGR - return Image.from_numpy(decoded, format=image_format, frame_id=frame_id, ts=ts) + image_format = "mono8" if decoded.ndim == 2 else "bgr8" + return image_from_array( + decoded, + encoding=image_format, + header=Header(frame_id=frame_id, stamp=time_from_seconds(ts)), + ) if shot.format != image_pb2.Image.FORMAT_RAW: logger.error(f"Unsupported Spot image encoding {shot.format} from {frame_id}") @@ -74,21 +80,27 @@ def decode_image(response: Any, frame_id: str, time_converter: Any) -> Image | N if channels == 1 else array.reshape(shot.rows, shot.cols, channels) ) - return Image.from_numpy(array, format=image_format, frame_id=frame_id, ts=ts) + return image_from_array( + array, encoding=image_format, header=Header(frame_id=frame_id, stamp=time_from_seconds(ts)) + ) -def joint_to_transform(joint: JointDescription) -> Transform: - return Transform( - translation=Vector3(*joint.origin_xyz), - rotation=Quaternion.from_euler(Vector3(*joint.origin_rpy)), - frame_id=joint.parent_link, +def joint_to_transform(joint: JointDescription) -> TransformStamped: + return TransformStamped( + header=Header(frame_id=joint.parent_link), child_frame_id=joint.child_link, + transform=Transform( + translation=Vector3( + x=joint.origin_xyz[0], y=joint.origin_xyz[1], z=joint.origin_xyz[2] + ), + rotation=quaternion_from_euler(*joint.origin_rpy), + ), ) def camera_mount_transforms( urdf_path: str | Path, base_frame_id: str, optical_frames: list[str] -) -> list[Transform]: +) -> list[TransformStamped]: """Compose each base_frame_id -> optical_frame extrinsic from the URDF's fixed joints. Walks the fixed-joint chain (base_link -> body -> {pos}_camera -> optical) up @@ -98,7 +110,7 @@ def camera_mount_transforms( model = RobotModel.from_file(urdf_path).load() urdf_root = model.root_link joint_by_child = {joint.child_link: joint for joint in model.joints} - transforms: list[Transform] = [] + transforms: list[TransformStamped] = [] for optical_frame in optical_frames: chain: list[JointDescription] = [] current = optical_frame @@ -112,13 +124,13 @@ def camera_mount_transforms( edges = [joint_to_transform(joint) for joint in reversed(chain)] composed = edges[0] for edge in edges[1:]: - composed = composed + edge - composed.frame_id = base_frame_id + composed = compose_transforms(composed, edge) + composed.header.frame_id = base_frame_id transforms.append(composed) return transforms -def roll_optical_frame(transform: Transform, quarter_turns: int) -> Transform: +def roll_optical_frame(transform: TransformStamped, quarter_turns: int) -> TransformStamped: """Roll a camera's optical frame `quarter_turns` * 90° about its viewing (z) axis. Pairs with `rotate_image_quarter_turns`: rotating the image alone leaves the 3D @@ -127,20 +139,18 @@ def roll_optical_frame(transform: Transform, quarter_turns: int) -> Transform: """ if not quarter_turns: return transform - roll = Quaternion.from_euler(Vector3(0.0, 0.0, quarter_turns * math.pi / 2)) - return Transform( - translation=transform.translation, - rotation=transform.rotation * roll, - frame_id=transform.frame_id, + roll = TransformStamped( + header=Header(frame_id=transform.child_frame_id), child_frame_id=transform.child_frame_id, - ts=transform.ts, + transform=Transform(rotation=quaternion_from_euler(0, 0, quarter_turns * math.pi / 2)), ) + return compose_transforms(transform, roll) def rotate_image_quarter_turns(image: Image, quarter_turns: int) -> Image: """Rotate an Image by `quarter_turns` * 90° CCW (negative for CW).""" - rotated = np.rot90(image.data, k=quarter_turns) - return Image.from_numpy(rotated, format=image.format, frame_id=image.frame_id, ts=image.ts) + rotated = np.rot90(image_view(image), k=quarter_turns) + return image_from_array(rotated, encoding=image.encoding, header=image.header) def rotate_camera_info_quarter_turns(info: CameraInfo, quarter_turns: int) -> CameraInfo: @@ -149,15 +159,15 @@ def rotate_camera_info_quarter_turns(info: CameraInfo, quarter_turns: int) -> Ca Each CCW quarter turn swaps the focal lengths and remaps the principal point so the intrinsics stay consistent with the rotated pixel grid (width/height swap). """ - fx, fy, cx, cy = info.K[0], info.K[4], info.K[2], info.K[5] + fx, fy, cx, cy = info.k[0], info.k[4], info.k[2], info.k[5] width, height = info.width, info.height for _ in range(quarter_turns % 4): fx, fy = fy, fx cx, cy = cy, (width - 1) - cx width, height = height, width - return CameraInfo.from_intrinsics( - fx=fx, fy=fy, cx=cx, cy=cy, width=width, height=height, frame_id=info.frame_id - ).with_ts(info.ts) + return camera_info_from_intrinsics( + fx=fx, fy=fy, cx=cx, cy=cy, width=width, height=height, header=info.header + ) def camera_info_from_response(response: Any, source_name: str, ts: float) -> CameraInfo | None: @@ -166,30 +176,30 @@ def camera_info_from_response(response: Any, source_name: str, ts: float) -> Cam if not source.HasField("pinhole"): return None intrinsics = source.pinhole.intrinsics - info = CameraInfo.from_intrinsics( + info = camera_info_from_intrinsics( fx=intrinsics.focal_length.x, fy=intrinsics.focal_length.y, cx=intrinsics.principal_point.x, cy=intrinsics.principal_point.y, width=source.cols, height=source.rows, - frame_id=source_name, + header=Header(frame_id=source_name, stamp=time_from_seconds(ts)), ) - return info.with_ts(ts) + return info def clamp(value: float, low: float, high: float) -> float: return max(low, min(high, value)) -def raw_layout(pixel_format: int) -> tuple[Any, int, ImageFormat]: +def raw_layout(pixel_format: int) -> tuple[Any, int, str]: from bosdyn.api import image_pb2 # type: ignore[import-not-found] - layouts: dict[int, tuple[Any, int, ImageFormat]] = { - image_pb2.Image.PIXEL_FORMAT_GREYSCALE_U8: (np.uint8, 1, ImageFormat.GRAY), - image_pb2.Image.PIXEL_FORMAT_GREYSCALE_U16: (np.uint16, 1, ImageFormat.GRAY16), - image_pb2.Image.PIXEL_FORMAT_DEPTH_U16: (np.uint16, 1, ImageFormat.DEPTH16), - image_pb2.Image.PIXEL_FORMAT_RGB_U8: (np.uint8, 3, ImageFormat.RGB), - image_pb2.Image.PIXEL_FORMAT_RGBA_U8: (np.uint8, 4, ImageFormat.RGBA), + layouts: dict[int, tuple[Any, int, str]] = { + image_pb2.Image.PIXEL_FORMAT_GREYSCALE_U8: (np.uint8, 1, "mono8"), + image_pb2.Image.PIXEL_FORMAT_GREYSCALE_U16: (np.uint16, 1, "mono16"), + image_pb2.Image.PIXEL_FORMAT_DEPTH_U16: (np.uint16, 1, "16UC1"), + image_pb2.Image.PIXEL_FORMAT_RGB_U8: (np.uint8, 3, "rgb8"), + image_pb2.Image.PIXEL_FORMAT_RGBA_U8: (np.uint8, 4, "rgba8"), } - return layouts.get(pixel_format, (None, 0, ImageFormat.GRAY)) + return layouts.get(pixel_format, (None, 0, "mono8")) diff --git a/dimos/experimental/security_demo/conftest.py b/dimos/experimental/security_demo/conftest.py index 364a90e6d32..6e7cc79d682 100644 --- a/dimos/experimental/security_demo/conftest.py +++ b/dimos/experimental/security_demo/conftest.py @@ -14,11 +14,13 @@ from __future__ import annotations +from dimos_generated.sensor_msgs.msg import CameraInfo +import numpy as np +from PIL import Image as PILImage import pytest from dimos.experimental.security_demo.security_module import SecurityModule -from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo -from dimos.msgs.sensor_msgs.Image import Image +from dimos.msgs.image import image_from_array from dimos.perception.detection.detectors.yolo import Yolo2DDetector from dimos.perception.detection.type.detection2d.bbox import Detection2DBBox from dimos.utils.data import get_data @@ -33,12 +35,18 @@ def yolo_detector(): @pytest.fixture(scope="session") def person_image(): - return Image.from_file(get_data("security_detection.png")) + return image_from_array( + np.asarray(PILImage.open(get_data("security_detection.png")).convert("RGB")), + encoding="rgb8", + ) @pytest.fixture(scope="session") def empty_image(): - return Image.from_file(get_data("security_no_detection.png")) + return image_from_array( + np.asarray(PILImage.open(get_data("security_no_detection.png")).convert("RGB")), + encoding="rgb8", + ) @pytest.fixture() diff --git a/dimos/experimental/security_demo/depth_estimator.py b/dimos/experimental/security_demo/depth_estimator.py index 588057d7112..3fc069f1fcb 100644 --- a/dimos/experimental/security_demo/depth_estimator.py +++ b/dimos/experimental/security_demo/depth_estimator.py @@ -17,12 +17,13 @@ from collections.abc import Callable import threading +from dimos_generated.sensor_msgs.msg import Image import numpy as np from PIL import Image as PILImage import torch from dimos.constants import DEFAULT_THREAD_JOIN_TIMEOUT -from dimos.msgs.sensor_msgs.Image import Image, ImageFormat +from dimos.msgs.image import image_from_array, image_to_rgb _DEPTH_MODEL_NAME = "depth-anything/Depth-Anything-V2-Small-hf" _DEPTH_MAX_WIDTH = 640 @@ -86,8 +87,7 @@ def _loop(self) -> None: self._process(image) def _process(self, image: Image) -> None: - rgb = image.to_rgb() - pil_image = PILImage.fromarray(rgb.data) + pil_image = PILImage.fromarray(image_to_rgb(image)) if pil_image.width > _DEPTH_MAX_WIDTH: scale = _DEPTH_MAX_WIDTH / pil_image.width new_h = int(pil_image.height * scale) @@ -105,8 +105,4 @@ def _process(self, image: Image) -> None: ).squeeze() depth_np = depth.cpu().numpy().astype(np.float32) - self._publish( - Image.from_numpy( - depth_np, format=ImageFormat.DEPTH, frame_id=image.frame_id, ts=image.ts - ) - ) + self._publish(image_from_array(depth_np, encoding="32FC1", header=image.header)) diff --git a/dimos/experimental/security_demo/security_module.py b/dimos/experimental/security_demo/security_module.py index c353ea645cf..87f1ba8269e 100644 --- a/dimos/experimental/security_demo/security_module.py +++ b/dimos/experimental/security_demo/security_module.py @@ -20,7 +20,7 @@ from dimos.constants import DEFAULT_THREAD_JOIN_TIMEOUT import numpy as np -from dimos_lcm.std_msgs import String, Bool +from dimos_generated.std_msgs.msg import String, Bool from reactivex.disposable import Disposable from dimos.agents.annotation import skill @@ -32,17 +32,18 @@ from dimos.models.segmentation.edge_tam import EdgeTAMProcessor from dimos.perception.detection.detectors.person.yolo import YoloPersonDetector from dimos.perception.detection.type.detection2d.person import Detection2DPerson -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.geometry_msgs.Twist import Twist -from dimos.msgs.nav_msgs.OccupancyGrid import OccupancyGrid -from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo -from dimos.msgs.sensor_msgs.Image import Image +from dimos_generated.geometry_msgs.msg import PoseStamped +from dimos_generated.geometry_msgs.msg import Twist +from dimos_generated.nav_msgs.msg import OccupancyGrid +from dimos_generated.sensor_msgs.msg import CameraInfo +from dimos_generated.sensor_msgs.msg import Image from dimos.navigation.patrolling.create_patrol_router import create_patrol_router from dimos.navigation.patrolling.routers.patrol_router import PatrolRouter from dimos.agents.skills.speak_skill_spec import SpeakSkillSpec from dimos.navigation.replanning_a_star.module_spec import ReplanningAStarPlannerSpec from dimos.navigation.visual_servoing.visual_servoing_2d import VisualServoing2D from dimos.utils.logging_config import setup_logger +from dimos.msgs.image import image_to_bgr, image_from_array from dimos.navigation.patrolling.constants import EXTRA_CLEARANCE if TYPE_CHECKING: @@ -253,7 +254,7 @@ def _main_loop(self) -> None: case "FOLLOWING": self._follow_step() - self.cmd_vel.publish(Twist.zero()) + self.cmd_vel.publish(Twist()) self._transition_to("IDLE") def _patrol_step(self) -> None: @@ -298,11 +299,11 @@ def _patrol_step(self) -> None: area=f"{best.bbox_2d_volume():.0f}px", ) - annotated = image.data.copy() + annotated = image_to_bgr(image) best.draw_on(annotated) if isinstance(best, Detection2DPerson): _draw_skeleton(annotated, best) - self.detection.publish(Image.from_numpy(annotated, format=image.format)) + self.detection.publish(image_from_array(annotated, encoding="bgr8", header=image.header)) # Init EdgeTAM with YOLO bbox for continuous tracking box = np.array(list(best.bbox), dtype=np.float32) @@ -327,7 +328,7 @@ def _follow_step(self) -> None: detections = self._tracker.process_image(latest_image) if len(detections) == 0: - self.cmd_vel.publish(Twist.zero()) + self.cmd_vel.publish(Twist()) self._speak_skill.speak("Lost sight of intruder, resuming patrol", blocking=False) self._router.reset() self._has_active_goal = False @@ -338,7 +339,7 @@ def _follow_step(self) -> None: twist = self._visual_servo.compute_twist(best.bbox, latest_image.width) self.cmd_vel.publish(twist) - overlay = latest_image.data.copy() + overlay = image_to_bgr(latest_image) if hasattr(best, "mask") and best.mask is not None: mask_bool = best.mask > 0 green = np.zeros_like(overlay) @@ -356,7 +357,7 @@ def _follow_step(self) -> None: _draw_skeleton(overlay, person) self.tracking_image.publish( - Image.from_numpy(overlay, format=latest_image.format, ts=latest_image.ts) + image_from_array(overlay, encoding="bgr8", header=latest_image.header) ) time.sleep(1.0 / self.config.follow_frequency) @@ -382,7 +383,7 @@ def _transition_to(self, new_state: State) -> None: self._state = new_state logger.info("state transition", old=old, new=new_state) - self.security_state.publish(String(new_state)) + self.security_state.publish(String(data=new_state)) def _stop_security_patrol_internal(self) -> None: self._stop_event.set() diff --git a/dimos/experimental/security_demo/test_security_module.py b/dimos/experimental/security_demo/test_security_module.py index 307595402ef..fa308ff475a 100644 --- a/dimos/experimental/security_demo/test_security_module.py +++ b/dimos/experimental/security_demo/test_security_module.py @@ -16,10 +16,9 @@ import threading +from dimos_generated.geometry_msgs.msg import Point, Pose, PoseStamped, Quaternion, Twist, Vector3 import pytest -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.geometry_msgs.Twist import Twist from dimos.perception.detection.type.detection2d.imageDetections2D import ImageDetections2D @@ -50,7 +49,9 @@ def test_patrol_step_transitions_to_following_on_detection( module._state = "PATROLLING" module._has_active_goal = True module._latest_image = person_image - module._latest_pose = PoseStamped(position=[1, 2, 0], orientation=[0, 0, 0, 1]) + module._latest_pose = PoseStamped( + pose=Pose(position=Point(x=1, y=2), orientation=Quaternion(w=1)) + ) det = make_detection() module._detector.process_image.return_value = ImageDetections2D( @@ -78,7 +79,7 @@ def test_patrol_step_requests_goal_when_no_active_goal(security_module): module._has_active_goal = False module._latest_image = None # causes early return after goal logic - goal = PoseStamped(position=[5, 5, 0], orientation=[0, 0, 0, 1]) + goal = PoseStamped(pose=Pose(position=Point(x=5, y=5), orientation=Quaternion(w=1))) module._router.next_goal.return_value = goal module._patrol_step() @@ -100,7 +101,7 @@ def test_follow_step_publishes_twist_when_tracking( image=person_image, detections=[det] ) - twist = Twist(linear=[0.3, 0, 0], angular=[0, 0, 0.1]) + twist = Twist(linear=Vector3(x=0.3), angular=Vector3(z=0.1)) module._visual_servo.compute_twist.return_value = twist mocker.patch("dimos.experimental.security_demo.security_module.time.sleep") @@ -125,7 +126,7 @@ def test_follow_step_transitions_to_patrolling_on_person_lost(security_module, p # Zero twist should be published to stop the robot published_twist = module.cmd_vel.publish.call_args[0][0] - assert published_twist.is_zero() + assert published_twist == Twist() module._speak_skill.speak.assert_called_with( "Lost sight of intruder, resuming patrol", blocking=False @@ -157,7 +158,7 @@ def patrol_side_effect(): # Verify zero twist published on shutdown last_twist = module.cmd_vel.publish.call_args[0][0] - assert last_twist.is_zero() + assert last_twist == Twist() # Verify state transitions: PATROLLING then IDLE state_values = [call.args[0].data for call in module.security_state.publish.call_args_list] diff --git a/dimos/experimental/world_belief/absence.py b/dimos/experimental/world_belief/absence.py index 8003d671346..2802ea3ced6 100644 --- a/dimos/experimental/world_belief/absence.py +++ b/dimos/experimental/world_belief/absence.py @@ -19,11 +19,12 @@ import numpy as np if TYPE_CHECKING: + from dimos_generated.geometry_msgs.msg import TransformStamped, Vector3 + from dimos_generated.sensor_msgs.msg import CameraInfo from numpy.typing import NDArray - from dimos.msgs.geometry_msgs.Transform import Transform - from dimos.msgs.geometry_msgs.Vector3 import Vector3 - from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo +from dimos.msgs.camera_info import intrinsic_matrix +from dimos.msgs.geometry import inverse_transform, transform_matrix OUT_OF_VIEW = "out_of_view" PRESENT = "present" @@ -38,20 +39,20 @@ def classify_visibility( center_world: Vector3, camera_info: CameraInfo, - world_from_camera: Transform, + world_from_camera: TransformStamped, depth_m: NDArray[np.float32], *, near_extent_m: float = 0.0, ) -> str: """Classify a world point as PRESENT, ABSENT, OCCLUDED, or OUT_OF_VIEW. ``near_extent_m`` widens the expected PRESENT depth band.""" - cam_from_world = world_from_camera.inverse() - p = cam_from_world.rotation.rotate_vector(center_world) + cam_from_world.translation - x, y, z = p.x, p.y, p.z + cam_from_world = inverse_transform(world_from_camera) + matrix = transform_matrix(cam_from_world.transform) + x, y, z = (matrix @ np.array([center_world.x, center_world.y, center_world.z, 1.0]))[:3] if z <= _NEAR_M: # Too close to camera; sensor is unreliable return OUT_OF_VIEW - k = camera_info.get_K_matrix() + k = intrinsic_matrix(camera_info) u = k[0, 0] * x / z + k[0, 2] v = k[1, 1] * y / z + k[1, 2] h, w = depth_m.shape[:2] diff --git a/dimos/experimental/world_belief/recall.py b/dimos/experimental/world_belief/recall.py index 7d989b0c6bb..450a4a5482e 100644 --- a/dimos/experimental/world_belief/recall.py +++ b/dimos/experimental/world_belief/recall.py @@ -23,8 +23,10 @@ import re from typing import TYPE_CHECKING, Any, cast +from dimos_generated.sensor_msgs.msg import Image + from dimos.memory.embed import EmbedImages -from dimos.msgs.sensor_msgs.Image import Image +from dimos.msgs.image import image_brightness, image_resize_to_fit, image_sharpness from dimos.utils.logging_config import setup_logger if TYPE_CHECKING: @@ -86,14 +88,14 @@ def best_in_new_buckets(upstream: Iterator[Any]) -> Iterator[Any]: if bucket in indexed_buckets: continue indexed_buckets.add(bucket) - yield max(observations, key=lambda obs: obs.data.sharpness) + yield max(observations, key=lambda obs: image_sharpness(obs.data)) pipeline = ( # Skip pose-less frames — they can't answer "where" (world-frame frames need no pose). src_stream.filter( - lambda obs: obs.pose is not None or (obs.data.frame_id or "") in ("", "world") + lambda obs: obs.pose is not None or (obs.data.header.frame_id or "") in ("", "world") ) - .filter(lambda obs: obs.data.brightness > 0.1) + .filter(lambda obs: image_brightness(obs.data) > 0.1) .transform(best_in_new_buckets) .transform(EmbedImages(model)) ) @@ -102,7 +104,7 @@ def best_in_new_buckets(upstream: Iterator[Any]) -> Iterator[Any]: embedded = cast("EmbeddedObservation[Image]", obs) payload = obs.data if index_store is not None: - payload, _scale = payload.resize_to_fit(thumbnail_px, thumbnail_px) + payload, _scale = image_resize_to_fit(payload, thumbnail_px, thumbnail_px) index.append( payload, ts=obs.ts, diff --git a/dimos/experimental/world_belief/scene_scan.py b/dimos/experimental/world_belief/scene_scan.py index f386e32e145..9b38aafacb8 100644 --- a/dimos/experimental/world_belief/scene_scan.py +++ b/dimos/experimental/world_belief/scene_scan.py @@ -18,14 +18,18 @@ from dataclasses import dataclass import itertools +import math from typing import TYPE_CHECKING, Any +from dimos_generated.geometry_msgs.msg import PoseStamped, TransformStamped +from dimos_generated.sensor_msgs.msg import CameraInfo, Image +from dimos_generated.std_msgs.msg import Header import numpy as np from dimos.experimental.world_belief.world_belief import WorldBelief -from dimos.msgs.geometry_msgs.Transform import Transform -from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo -from dimos.msgs.sensor_msgs.Image import Image, ImageFormat +from dimos.msgs.geometry import point_distance, quaternion_angle, transform_from_pose +from dimos.msgs.image import image_from_array, image_to_rgb, image_view +from dimos.msgs.time import time_from_seconds from dimos.perception.experimental.object import Object from dimos.utils.logging_config import setup_logger @@ -156,10 +160,7 @@ def warmup(self) -> None: set_prompts = getattr(detector, "set_prompts", None) if self._text_prompts and callable(set_prompts): set_prompts(text=list(self._text_prompts)) - dummy = Image( - data=np.zeros((64, 64, 3), dtype=np.uint8), - format=ImageFormat.RGB, - ) + dummy = image_from_array(np.zeros((64, 64, 3), dtype=np.uint8), encoding="rgb8") detector_warmup = getattr(detector, "warmup", None) if callable(detector_warmup): detector_warmup(dummy) @@ -237,7 +238,9 @@ def scan( last_source_ts = obs_ts if last_source_ts is None else max(last_source_ts, obs_ts) if obs.pose is None: if poseless_in_target is None: - frame_id = obs.data.frame_id or "" # one decode, first pose-less frame only + frame_id = ( + obs.data.header.frame_id or "" + ) # one decode, first pose-less frame only poseless_in_target = frame_id in ("", self._target_frame) if not poseless_in_target: skipped += 1 @@ -325,8 +328,8 @@ def _select_frames(self, frames: list[Any]) -> list[Any]: p0 is not None and p1 is not None and ( - p0.position.distance(p1.position) / dt > _TRANS_SPEED - or p0.orientation.angle_to(p1.orientation) / dt > _ROT_SPEED + point_distance(p0.position, p1.position) / dt > _TRANS_SPEED + or quaternion_angle(p0.orientation, p1.orientation) / dt > _ROT_SPEED ) ) period = 1.0 / (_MOVING_HZ if moving else self._stationary_hz) @@ -344,7 +347,7 @@ def _detect_frame( depth_stream: Any, depth_tolerance: float, detector: Detector, - ) -> tuple[list[Object], Any, Transform | None] | None: + ) -> tuple[list[Object], Any, TransformStamped | None] | None: """Detect + NMS + 2D→3D lift + embed one frame; None if no depth within tolerance.""" color_img: Image = obs.data depth_img = self._nearest_depth(depth_stream, obs.ts, depth_tolerance) @@ -352,12 +355,19 @@ def _detect_frame( return None process = getattr(detector, "predict_image", detector.process_image) detections: ImageDetections2D[Any] = process(color_img) - frame_id = color_img.frame_id or "" + frame_id = color_img.header.frame_id or "" in_target = frame_id in ("", self._target_frame) - camera_transform = None if in_target else Transform.from_pose(frame_id, obs.pose) - if camera_transform is not None: - camera_transform.frame_id = self._target_frame - camera_transform.ts = float(obs.ts) + camera_transform = None + if not in_target: + if obs.pose is None: + return None + camera_transform = transform_from_pose( + PoseStamped( + header=Header(frame_id=self._target_frame, stamp=time_from_seconds(obs.ts)), + pose=obs.pose, + ), + child_frame_id=frame_id, + ) objects = Object.from_2d_to_list( detections_2d=detections, color_image=color_img, @@ -366,7 +376,9 @@ def _detect_frame( camera_transform=camera_transform, ) for obj in objects: - obj.frame_id = obj.pose.frame_id = obj.pointcloud.frame_id = self._target_frame + obj.frame_id = obj.pose.header.frame_id = obj.pointcloud.header.frame_id = ( + self._target_frame + ) objects = [o for o in objects if float(o.confidence) >= self._detector_conf] # Remove overlapping prompts and same-depth contained regions; a contained object # at a different depth survives. @@ -377,7 +389,10 @@ def _detect_frame( _bbox_iou(o.bbox, k.bbox) > _NMS_IOU or ( _bbox_containment(o.bbox, k.bbox) >= _NMS_CONTAINMENT - and o.center.distance(k.center) <= _SUBREGION_COLOCATED_M + and math.dist( + (o.center.x, o.center.y, o.center.z), (k.center.x, k.center.y, k.center.z) + ) + <= _SUBREGION_COLOCATED_M ) for k in kept ): @@ -390,7 +405,7 @@ def _detect_frame( x1 <= 3 or y1 <= 3 or x2 >= color_img.width - 3 or y2 >= color_img.height - 3 ) self._attach_embeddings(kept, color_img) - depth_arr = np.asarray(depth_img.to_opencv(), dtype=np.float32) + depth_arr = np.asarray(image_view(depth_img), dtype=np.float32) return kept, depth_arr, camera_transform def _attach_embeddings(self, objects: list[Object], color_img: Image) -> None: @@ -401,11 +416,12 @@ def _attach_embeddings(self, objects: list[Object], color_img: Image) -> None: crops: list[Image] = [] idxs: list[int] = [] for i, obj in enumerate(objects): - crop = obj.cropped_image(padding=0).to_rgb() + crop_source = obj.cropped_image(padding=0) + crop = image_from_array( + image_to_rgb(crop_source), encoding="rgb8", header=color_img.header + ) if crop.width < 2 or crop.height < 2: continue - crop.frame_id = color_img.frame_id - crop.ts = color_img.ts crops.append(crop) idxs.append(i) if not crops: @@ -427,9 +443,9 @@ def _nearest_depth(depth_stream: Any, ts: float, tolerance: float) -> Image | No return None depth_obs = min(candidates, key=lambda o: abs(float(o.ts) - ts)) raw: Image = depth_obs.data - cv = raw.to_opencv() - if raw.format == ImageFormat.DEPTH16: + cv = image_view(raw) + if raw.encoding == "16UC1": cv = cv.astype("float32") / 1000.0 elif cv.dtype != "float32": cv = cv.astype("float32") - return Image(data=cv, format=ImageFormat.DEPTH, frame_id=raw.frame_id, ts=raw.ts) + return image_from_array(cv, encoding="32FC1", header=raw.header) diff --git a/dimos/experimental/world_belief/test_absence.py b/dimos/experimental/world_belief/test_absence.py new file mode 100644 index 00000000000..0f7172da35d --- /dev/null +++ b/dimos/experimental/world_belief/test_absence.py @@ -0,0 +1,63 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from dimos_generated.geometry_msgs.msg import Quaternion, Transform, TransformStamped, Vector3 +from dimos_generated.std_msgs.msg import Header +import numpy as np +import pytest + +from dimos.experimental.world_belief.absence import ( + ABSENT, + OCCLUDED, + OUT_OF_VIEW, + PRESENT, + classify_visibility, +) +from dimos.msgs.camera_info import camera_info_from_intrinsics + + +@pytest.mark.parametrize( + "depth,expected", [(3.0, ABSENT), (1.0, PRESENT), (0.5, OCCLUDED), (0.0, OCCLUDED)] +) +def test_visibility_inverts_generated_camera_transform(depth, expected): + calibration = camera_info_from_intrinsics( + 100, 100, 16, 16, 32, 32, header=Header(frame_id="camera") + ) + world_from_camera = TransformStamped( + header=Header(frame_id="world"), + child_frame_id="camera", + transform=Transform(translation=Vector3(x=10), rotation=Quaternion(w=1)), + ) + # In world coordinates x=10; after inverse transform it is on the optical axis. + center = Vector3(x=10, z=1) + depth_m = np.full((32, 32), depth, dtype=np.float32) + assert classify_visibility(center, calibration, world_from_camera, depth_m) == expected + assert ( + classify_visibility(Vector3(x=10, z=-1), calibration, world_from_camera, depth_m) + == OUT_OF_VIEW + ) + + +def test_visibility_requires_all_valid_depth_samples_to_show_absence(): + calibration = camera_info_from_intrinsics( + 100, 100, 16, 16, 32, 32, header=Header(frame_id="camera") + ) + identity = TransformStamped( + header=Header(frame_id="world"), + child_frame_id="camera", + transform=Transform(rotation=Quaternion(w=1)), + ) + depth = np.full((32, 32), 3, dtype=np.float32) + depth[16, 16] = 1 + assert classify_visibility(Vector3(z=1), calibration, identity, depth) == PRESENT diff --git a/dimos/experimental/world_belief/test_generated_recall.py b/dimos/experimental/world_belief/test_generated_recall.py new file mode 100644 index 00000000000..9b696b79c4b --- /dev/null +++ b/dimos/experimental/world_belief/test_generated_recall.py @@ -0,0 +1,90 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from dimos_generated.geometry_msgs.msg import Pose, Quaternion +from dimos_generated.sensor_msgs.msg import Image +from dimos_generated.std_msgs.msg import Header +import numpy as np +import pytest + +from dimos.experimental.world_belief.recall import build_frame_clip_index, index_stream_name +from dimos.memory.store.memory import MemoryStore +from dimos.msgs.image import image_from_array, image_resize_to_fit, image_view +from dimos.msgs.time import time_from_seconds + + +class _EmbeddingModel: + def __init__(self) -> None: + self.seen: list[Image] = [] + + def embed(self, *images: Image) -> list[np.ndarray]: + self.seen.extend(images) + return [np.array([1.0, 0.0], dtype=np.float32) for _ in images] + + +def test_index_selects_sharp_generated_frame_and_preserves_stamp_and_thumbnail() -> None: + source, destination = MemoryStore(), MemoryStore() + try: + frames = source.stream("color_image", Image) + sharp = np.tile(np.array([[64, 255], [255, 64]], dtype=np.uint8), (4, 8)) + sharp = np.repeat(sharp[:, :, None], 3, axis=2) + image = image_from_array( + sharp, encoding="rgb8", header=Header(stamp=time_from_seconds(0.2), frame_id="camera") + ) + identity = Pose(orientation=Quaternion(w=1.0)) + frames.append( + image_from_array(np.full_like(sharp, 150), encoding="rgb8", header=image.header), + ts=0.1, + pose=identity, + ) + frames.append(image, ts=0.2, pose=identity) + # A bright pose-less camera frame and a dark posed frame are ineligible. + frames.append(image, ts=1.1) + frames.append( + image_from_array(np.zeros_like(sharp), encoding="rgb8", header=image.header), + ts=2.1, + pose=identity, + ) + model = _EmbeddingModel() + assert ( + build_frame_clip_index( + source, model=model, index_store=destination, thumbnail_px=4, source_tag="test" + ) + == 1 + ) + assert len(model.seen) == 1 + assert np.array_equal(image_view(model.seen[0]), sharp) + result = destination.stream( + index_stream_name("openai/clip-vit-base-patch32"), Image + ).first() + assert (result.data.width, result.data.height) == (4, 2) + assert result.data.header == image.header + assert result.ts == 0.2 + assert result.tags["rec"] == "test" + assert ( + build_frame_clip_index( + source, model=model, index_store=destination, thumbnail_px=4, source_tag="test" + ) + == 0 + ) + finally: + source.stop() + destination.stop() + + +def test_generated_resize_keeps_small_images_and_rejects_invalid_dimensions() -> None: + image = image_from_array(np.ones((2, 3, 3), dtype=np.uint8), encoding="bgr8") + assert image_resize_to_fit(image, 5, 5) == (image, 1.0) + with pytest.raises(ValueError, match="positive"): + image_resize_to_fit(image, 0, 5) diff --git a/dimos/experimental/world_belief/world_belief.py b/dimos/experimental/world_belief/world_belief.py index dcbe3cf1e36..30d980069a3 100644 --- a/dimos/experimental/world_belief/world_belief.py +++ b/dimos/experimental/world_belief/world_belief.py @@ -22,6 +22,7 @@ from typing import TYPE_CHECKING, Any import uuid +from dimos_generated.geometry_msgs.msg import Vector3 import numpy as np from scipy.optimize import linear_sum_assignment @@ -36,16 +37,16 @@ normalize_embedding, ) from dimos.models.embedding.base import Embedding -from dimos.msgs.geometry_msgs.Vector3 import Vector3 +from dimos.msgs.geometry import pose_from_transform, pose_matrix +from dimos.msgs.pointcloud import pointcloud_xyz from dimos.protocol.service.spec import BaseConfig from dimos.utils.logging_config import setup_logger if TYPE_CHECKING: + from dimos_generated.geometry_msgs.msg import Pose, TransformStamped + from dimos_generated.sensor_msgs.msg import CameraInfo from numpy.typing import NDArray - from dimos.msgs.geometry_msgs.Pose import Pose - from dimos.msgs.geometry_msgs.Transform import Transform - from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo from dimos.perception.experimental.object import Object logger = setup_logger() @@ -89,7 +90,10 @@ class WorldBeliefConfig(BaseConfig): def camera_viewpoint(pose: Pose) -> Viewpoint: """Return camera position and forward axis for viewpoint diversity.""" - return pose.position, pose.orientation.rotate_vector(Vector3.unit_z()) + forward = pose_matrix(pose)[:3, 2] + return Vector3(x=pose.position.x, y=pose.position.y, z=pose.position.z), Vector3( + x=forward[0], y=forward[1], z=forward[2] + ) def is_novel_viewpoint( @@ -103,8 +107,10 @@ def is_novel_viewpoint( pos, fwd = viewpoint cos_thresh = math.cos(math.radians(angle_deg)) for prev_pos, prev_fwd in existing: - near_in_space = pos.distance(prev_pos) < min_baseline_m - near_in_angle = fwd.dot(prev_fwd) > cos_thresh + near_in_space = ( + math.dist((pos.x, pos.y, pos.z), (prev_pos.x, prev_pos.y, prev_pos.z)) < min_baseline_m + ) + near_in_angle = (fwd.x * prev_fwd.x + fwd.y * prev_fwd.y + fwd.z * prev_fwd.z) > cos_thresh if near_in_space and near_in_angle: return False return True @@ -287,7 +293,7 @@ def observe( objects: list[Object], *, frame_ts: float, - camera_transform: Transform | None, + camera_transform: TransformStamped | None, camera_info: CameraInfo, depth_m: NDArray[np.float32], ) -> None: @@ -298,7 +304,9 @@ def observe( previous_ts = self._now # Without a camera pose, fold identity but not viewpoint or absence evidence. viewpoint = ( - camera_viewpoint(camera_transform.to_pose()) if camera_transform is not None else None + camera_viewpoint(pose_from_transform(camera_transform).pose) + if camera_transform is not None + else None ) for obj in objects: # Normalize the center before association and publication. @@ -509,8 +517,7 @@ def _hit( track.obj.pointcloud = obj.pointcloud track.obj.ts = obj.ts track.obj.frame_id = obj.frame_id - track.obj.pose.ts = obj.pose.ts - track.obj.pose.frame_id = obj.pose.frame_id + track.obj.pose.header = obj.pose.header track.obj.image = obj.image track.obj.mask = obj.mask track.obj.bbox = obj.bbox @@ -557,12 +564,13 @@ def _hit( def _observation_center(obj: Object) -> Vector3: """Return the median world-space surface point, falling back to ``obj.center``.""" try: - pts = np.asarray(obj.pointcloud.pointcloud.points) + pts = pointcloud_xyz(obj.pointcloud) except Exception: return obj.center if pts.ndim != 2 or len(pts) < 8: return obj.center - return Vector3(np.median(pts, axis=0)) + center = np.median(pts, axis=0) + return Vector3(x=center[0], y=center[1], z=center[2]) def _geometry_suspect(self, track: _Track, obj: Object) -> bool: """Reject partial or >2x median-size geometry without folding it into the reference.""" @@ -581,7 +589,7 @@ def _vote_absence( self, matched_tracks: set[int], camera_info: CameraInfo, - world_from_camera: Transform, + world_from_camera: TransformStamped, depth_m: NDArray[np.float32], ) -> None: for track in self._tracks.values(): diff --git a/dimos/experimental/world_belief/worldbelief_module.py b/dimos/experimental/world_belief/worldbelief_module.py index 0cda1e0e2f4..bf2a670cadf 100644 --- a/dimos/experimental/world_belief/worldbelief_module.py +++ b/dimos/experimental/world_belief/worldbelief_module.py @@ -23,6 +23,9 @@ import threading from typing import TYPE_CHECKING, Any, ClassVar +from dimos_generated.sensor_msgs.msg import Image, PointCloud2 +from dimos_generated.std_msgs.msg import Header +from dimos_generated.vision_msgs.msg import Detection3DArray from pydantic import Field from dimos.agents.annotation import skill @@ -41,9 +44,8 @@ WorldBeliefRecorderSpec, ) from dimos.memory.module import MemoryModuleConfig -from dimos.msgs.sensor_msgs.Image import Image -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 -from dimos.msgs.vision_msgs.Detection3DArray import Detection3DArray +from dimos.msgs.pointcloud import voxel_downsample_cloud +from dimos.msgs.time import time_from_seconds from dimos.perception.experimental.object import Object from dimos.utils.logging_config import setup_logger @@ -313,9 +315,8 @@ def scan(self, prompt: list[str] | None = None, window: float = 60.0) -> SkillRe self.detections_3d.publish( to_detection3d_array(present, frame_id=frame_id, ts=result.as_of_ts) ) - cloud = aggregate_pointclouds(present).voxel_downsample(0.005) - cloud.frame_id = frame_id - cloud.ts = result.as_of_ts + cloud = voxel_downsample_cloud(aggregate_pointclouds(present), 0.005) + cloud.header = Header(frame_id=frame_id, stamp=time_from_seconds(result.as_of_ts)) self.pointcloud.publish(cloud) self.objects.publish(present) return SkillResult.ok( @@ -392,7 +393,11 @@ def recall(self, text: str, k: int = 20) -> dict[str, Any] | None: "when_ts": round(float(hit.ts), 3), "where_camera": None if pose is None - else [round(pose.x, 3), round(pose.y, 3), round(pose.z, 3)], + else [ + round(pose.pose.position.x, 3), + round(pose.pose.position.y, 3), + round(pose.pose.position.z, 3), + ], "where_object": None if obj_center is None else [ diff --git a/dimos/experimental/world_belief/worldbelief_recorder.py b/dimos/experimental/world_belief/worldbelief_recorder.py index 276b0897d1c..cfe344623a8 100644 --- a/dimos/experimental/world_belief/worldbelief_recorder.py +++ b/dimos/experimental/world_belief/worldbelief_recorder.py @@ -18,14 +18,13 @@ from pathlib import Path from typing import Any, Protocol +from dimos_generated.geometry_msgs.msg import Pose, Quaternion +from dimos_generated.sensor_msgs.msg import CameraInfo, Image, JointState + from dimos.constants import STATE_DIR from dimos.core.core import rpc from dimos.core.stream import In from dimos.memory.module import Recorder, RecorderConfig, pose_setter_for -from dimos.msgs.geometry_msgs.Transform import Transform -from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo -from dimos.msgs.sensor_msgs.Image import Image -from dimos.msgs.sensor_msgs.JointState import JointState from dimos.spec.utils import Spec from dimos.utils.logging_config import setup_logger @@ -85,9 +84,9 @@ def recording_path(self) -> str: def _prepare_streams(self) -> None: super()._prepare_streams() depth = self.config.stream_remapping.get("depth_image", "depth_image") - self.store.stream(depth, Image, codec="lz4+lcm") + self.store.stream(depth, Image, codec="lz4+cdr") @pose_setter_for("coordinator_joint_state") async def _proprio_pose(self, msg: Any) -> Any: """Use an identity pose for proprioceptive joint-state records.""" - return Transform.identity().to_pose() + return Pose(orientation=Quaternion(w=1.0)) diff --git a/dimos/experimental/world_belief/xarm6_blueprint.py b/dimos/experimental/world_belief/xarm6_blueprint.py index 8292a164684..543bb16ef40 100644 --- a/dimos/experimental/world_belief/xarm6_blueprint.py +++ b/dimos/experimental/world_belief/xarm6_blueprint.py @@ -17,7 +17,9 @@ from __future__ import annotations from functools import partial -from typing import TYPE_CHECKING, Any, cast +from typing import TYPE_CHECKING, Any + +from dimos_generated.geometry_msgs.msg import Quaternion, Transform, TransformStamped, Vector3 from dimos.agents.mcp.mcp_server import McpServer from dimos.constants import STATE_DIR @@ -30,19 +32,19 @@ ) from dimos.hardware.sensors.camera.realsense.camera import RealSenseCamera from dimos.manipulation.manipulation_module import ManipulationModule -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Transform import Transform -from dimos.msgs.geometry_msgs.Vector3 import Vector3 from dimos.robot.manipulators.common.blueprints import coordinator, trajectory_task from dimos.robot.manipulators.xarm.config import make_xarm6_model_config, xarm6_hardware from dimos.visualization.rerun.bridge import RerunBridgeModule +from dimos.visualization.rerun.message_helpers import camera_pinhole if TYPE_CHECKING: import rerun.blueprint as rrb -XARM6_WORLDBELIEF_CAMERA_TRANSFORM = Transform( - translation=Vector3(x=0.06693724, y=-0.0309563, z=0.00691482), - rotation=Quaternion(0.70513398, 0.00535696, 0.70897578, -0.01052180), +XARM6_WORLDBELIEF_CAMERA_TRANSFORM = TransformStamped( + transform=Transform( + translation=Vector3(x=0.06693724, y=-0.0309563, z=0.00691482), + rotation=Quaternion(x=0.70513398, y=0.00535696, z=0.70897578, w=-0.01052180), + ) ) @@ -59,10 +61,7 @@ def _topic_to_entity(topic: Any) -> str: def _camera_info_to_rerun(msg: Any, image_topic: str) -> list[tuple[str, Any]]: - return cast( - "list[tuple[str, Any]]", - msg.to_rerun(image_topic=image_topic, optical_frame=getattr(msg, "frame_id", None)), - ) + return [(image_topic, camera_pinhole(msg))] def _rerun_blueprint() -> rrb.Blueprint: diff --git a/dimos/hardware/drive_trains/flowbase/config.py b/dimos/hardware/drive_trains/flowbase/config.py index 494247d6c74..a002be9596d 100644 --- a/dimos/hardware/drive_trains/flowbase/config.py +++ b/dimos/hardware/drive_trains/flowbase/config.py @@ -14,9 +14,7 @@ """FlowBase platform constants.""" -from dimos.msgs.geometry_msgs.Pose import Pose -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Vector3 import Vector3 +from dimos_generated.geometry_msgs.msg import Point, Pose, Quaternion -# Mid-360 mount pose on the FlowBase (position + orientation) in the base frame. -FLOWBASE_MID360_MOUNT = Pose(0.20, -0.20, 0.10, *Quaternion.from_euler(Vector3(0, 0, 0))) +# Mid-360 mount pose on the FlowBase in the base frame. +FLOWBASE_MID360_MOUNT = Pose(position=Point(x=0.20, y=-0.20, z=0.10), orientation=Quaternion(w=1.0)) diff --git a/dimos/hardware/drive_trains/transport/adapter.py b/dimos/hardware/drive_trains/transport/adapter.py index 921cd90d378..77b9dcd457e 100644 --- a/dimos/hardware/drive_trains/transport/adapter.py +++ b/dimos/hardware/drive_trains/transport/adapter.py @@ -23,10 +23,10 @@ import threading from typing import Any +from dimos_generated.geometry_msgs.msg import PoseStamped, Twist, Vector3 + from dimos.core.transport_factory import make_transport -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.geometry_msgs.Twist import Twist -from dimos.msgs.geometry_msgs.Vector3 import Vector3 +from dimos.msgs.geometry import yaw from dimos.utils.logging_config import setup_logger logger = setup_logger() @@ -141,7 +141,11 @@ def read_enabled(self) -> bool: def _on_odom(self, msg: PoseStamped) -> None: with self._lock: - self._latest_odom = [msg.x, msg.y, msg.yaw] + self._latest_odom = [ + msg.pose.position.x, + msg.pose.position.y, + yaw(msg.pose.orientation), + ] def transport_lcm_factory(**kwargs: Any) -> TransportTwistAdapter: diff --git a/dimos/hardware/drive_trains/transport/test_adapter.py b/dimos/hardware/drive_trains/transport/test_adapter.py new file mode 100644 index 00000000000..eb2f7d8905d --- /dev/null +++ b/dimos/hardware/drive_trains/transport/test_adapter.py @@ -0,0 +1,63 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import math + +from dimos_generated.geometry_msgs.msg import Point, Pose, PoseStamped, Quaternion, Twist, Vector3 +import pytest + +from dimos.hardware.drive_trains.transport.adapter import TransportTwistAdapter + + +def test_nested_pose_becomes_planar_odometry_without_sharing_state(): + adapter = TransportTwistAdapter() + assert adapter.read_odometry() is None + adapter._on_odom( + PoseStamped( + pose=Pose( + position=Point(x=2.0, y=-1.0), + orientation=Quaternion(z=math.sqrt(0.5), w=math.sqrt(0.5)), + ) + ) + ) + odometry = adapter.read_odometry() + assert odometry == pytest.approx([2.0, -1.0, math.pi / 2]) + odometry[0] = 99.0 + assert adapter.read_odometry()[0] == 2.0 + + +@pytest.mark.parametrize( + "dof,values,expected", + [ + (1, [0.1], Twist(linear=Vector3(x=0.1))), + (2, [0.1, 0.2], Twist(linear=Vector3(x=0.1, y=0.2))), + (3, [0.1, 0.2, 0.3], Twist(linear=Vector3(x=0.1, y=0.2), angular=Vector3(z=0.3))), + ], +) +def test_generated_commands_preserve_enable_and_stop_contract( + monkeypatch, mocker, dof, values, expected +): + adapter = TransportTwistAdapter(dof=dof) + transport = mocker.Mock() + monkeypatch.setattr(adapter, "_cmd_vel_transport", transport) + assert not adapter.write_velocities(values) + transport.publish.assert_not_called() + adapter.write_enable(True) + assert adapter.write_velocities(values) + message = transport.publish.call_args.args[0] + assert Twist.decode(message.encode()) == expected + assert adapter.read_velocities() == values + adapter.write_enable(False) + assert transport.publish.call_args.args[0] == Twist() + assert adapter.read_velocities() == [0.0] * dof diff --git a/dimos/hardware/manipulators/spec.py b/dimos/hardware/manipulators/spec.py index 4ca30e43a38..60deec651b4 100644 --- a/dimos/hardware/manipulators/spec.py +++ b/dimos/hardware/manipulators/spec.py @@ -27,9 +27,6 @@ from typing import Protocol, runtime_checkable from dimos.hardware.spec import JointLimits as _JointLimits -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Transform import Transform -from dimos.msgs.geometry_msgs.Vector3 import Vector3 class DriverStatus(Enum): @@ -65,14 +62,6 @@ class ManipulatorInfo: serial_number: str | None = None -def default_base_transform() -> Transform: - """Default identity transform for arm mounting.""" - return Transform( - translation=Vector3(0.0, 0.0, 0.0), - rotation=Quaternion(0.0, 0.0, 0.0, 1.0), - ) - - @runtime_checkable class ManipulatorAdapter(Protocol): """Protocol for hardware-specific IO. diff --git a/dimos/hardware/sensors/camera/gstreamer/gstreamer_camera.py b/dimos/hardware/sensors/camera/gstreamer/gstreamer_camera.py index 055d50f9602..4bf7bbf239b 100644 --- a/dimos/hardware/sensors/camera/gstreamer/gstreamer_camera.py +++ b/dimos/hardware/sensors/camera/gstreamer/gstreamer_camera.py @@ -20,13 +20,16 @@ import time from typing import Any +from dimos_generated.sensor_msgs.msg import Image +from dimos_generated.std_msgs.msg import Header import numpy as np from dimos.constants import DEFAULT_THREAD_JOIN_TIMEOUT from dimos.core.core import rpc from dimos.core.module import Module, ModuleConfig from dimos.core.stream import Out -from dimos.msgs.sensor_msgs.Image import Image, ImageFormat +from dimos.msgs.image import image_from_array +from dimos.msgs.time import time_from_seconds from dimos.utils.logging_config import setup_logger # Add system path for gi module if needed @@ -279,11 +282,10 @@ def _on_new_sample(self, appsink): # type: ignore[no-untyped-def] image_array = data.reshape((height, width, 3)) # Create an Image message with the absolute timestamp - image_msg = Image( - data=image_array.copy(), # Make a copy to ensure data persistence - format=ImageFormat.BGR, - frame_id=self.frame_id, - ts=timestamp, + image_msg = image_from_array( + image_array, + encoding="bgr8", + header=Header(frame_id=self.frame_id, stamp=time_from_seconds(timestamp)), ) # Publish the image diff --git a/dimos/hardware/sensors/camera/gstreamer/gstreamer_camera_test_script.py b/dimos/hardware/sensors/camera/gstreamer/gstreamer_camera_test_script.py index 7bef41e5e1c..5c7babb8ece 100755 --- a/dimos/hardware/sensors/camera/gstreamer/gstreamer_camera_test_script.py +++ b/dimos/hardware/sensors/camera/gstreamer/gstreamer_camera_test_script.py @@ -18,10 +18,12 @@ import logging import time +from dimos_generated.sensor_msgs.msg import Image + from dimos.core.coordination.module_coordinator import ModuleCoordinator from dimos.core.transport import LCMTransport from dimos.hardware.sensors.camera.gstreamer.gstreamer_camera import GstreamerCameraModule -from dimos.msgs.sensor_msgs.Image import Image +from dimos.msgs.time import to_seconds from dimos.protocol.service.lcmservice import autoconf from dimos.utils.logging_config import setup_logger @@ -81,25 +83,27 @@ def main() -> None: # Counter for received frames frame_count = [0] last_log_time = [time.time()] - first_timestamp = [None] + first_timestamp: list[float | None] = [None] - def on_frame(msg) -> None: # type: ignore[no-untyped-def] + def on_frame(msg: Image) -> None: frame_count[0] += 1 current_time = time.time() # Capture first timestamp to show absolute timestamps are preserved if first_timestamp[0] is None: - first_timestamp[0] = msg.ts - logger.info(f"First frame absolute timestamp: {msg.ts:.6f}") + first_timestamp[0] = to_seconds(msg.header.stamp) + logger.info(f"First frame absolute timestamp: {to_seconds(msg.header.stamp):.6f}") # Log stats every 2 seconds if current_time - last_log_time[0] >= 2.0: fps = frame_count[0] / (current_time - last_log_time[0]) - timestamp_delta = msg.ts - first_timestamp[0] + initial_stamp = first_timestamp[0] + assert initial_stamp is not None + timestamp_delta = to_seconds(msg.header.stamp) - initial_stamp logger.info( f"Received {frame_count[0]} frames - FPS: {fps:.1f} - " f"Resolution: {msg.width}x{msg.height} - " - f"Timestamp: {msg.ts:.3f} (delta: {timestamp_delta:.3f}s)" + f"Timestamp: {to_seconds(msg.header.stamp):.3f} (delta: {timestamp_delta:.3f}s)" ) frame_count[0] = 0 last_log_time[0] = current_time diff --git a/dimos/hardware/sensors/camera/gstreamer/test_generated_frames.py b/dimos/hardware/sensors/camera/gstreamer/test_generated_frames.py new file mode 100644 index 00000000000..cc413b20089 --- /dev/null +++ b/dimos/hardware/sensors/camera/gstreamer/test_generated_frames.py @@ -0,0 +1,84 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import importlib.util +from pathlib import Path +import sys +from types import ModuleType, SimpleNamespace +from unittest.mock import Mock + +from dimos_generated.sensor_msgs.msg import Image +import numpy as np +import pytest + +from dimos.msgs.image import image_view +from dimos.msgs.time import to_nanoseconds + + +@pytest.fixture +def camera_module(monkeypatch): + gi = ModuleType("gi") + gi.require_version = Mock() + repository = ModuleType("gi.repository") + repository.GLib = Mock() + repository.Gst = SimpleNamespace( + init=Mock(), + FlowReturn=SimpleNamespace(OK=0, ERROR=-1), + MapFlags=SimpleNamespace(READ=1), + CLOCK_TIME_NONE=-1, + ) + monkeypatch.setitem(sys.modules, "gi", gi) + monkeypatch.setitem(sys.modules, "gi.repository", repository) + spec = importlib.util.spec_from_file_location( + "_gstreamer_generated_test", Path(__file__).with_name("gstreamer_camera.py") + ) + module = importlib.util.module_from_spec(spec) + monkeypatch.setitem(sys.modules, spec.name, module) + spec.loader.exec_module(module) + return module + + +def test_generated_bgr_frame_owns_pixels_after_sdk_buffer_unmap(camera_module, monkeypatch): + camera = camera_module.GstreamerCameraModule(frame_id="camera_test") + camera.running = True + pixels = np.arange(18, dtype=np.uint8).reshape(2, 3, 3) + raw = bytearray(pixels.tobytes()) + mapped = SimpleNamespace(data=raw) + buffer = Mock(pts=1700000000125000000) + buffer.map.return_value = (True, mapped) + buffer.unmap.side_effect = lambda info: raw.__setitem__(slice(None), bytes(18)) + caps = Mock() + caps.get_structure.return_value.get_value.side_effect = {"width": 3, "height": 2}.__getitem__ + sample = Mock() + sample.get_buffer.return_value = buffer + sample.get_caps.return_value = caps + sink = Mock() + sink.emit.return_value = sample + received = [] + monkeypatch.setattr(camera.video, "publish", received.append) + try: + assert camera._on_new_sample(sink) == 0 + assert len(received) == 1 and type(received[0]) is Image + value = Image.decode(received[0].encode()) + assert value.header.frame_id == "camera_test" + assert value.encoding == "bgr8" and value.step == 9 + assert to_nanoseconds(value.header.stamp) == 1700000000125000000 + np.testing.assert_array_equal(image_view(value), pixels) + buffer.unmap.assert_called_once_with(mapped) + # Existing invalid/pre-2000 timestamp rejection occurs before mapping. + buffer.pts = 1 + assert camera._on_new_sample(sink) == 0 and len(received) == 1 + assert buffer.map.call_count == 1 + finally: + camera.stop() diff --git a/dimos/hardware/sensors/camera/module.py b/dimos/hardware/sensors/camera/module.py index 83020165da7..2ef9856b843 100644 --- a/dimos/hardware/sensors/camera/module.py +++ b/dimos/hardware/sensors/camera/module.py @@ -13,8 +13,13 @@ # limitations under the License. from collections.abc import Callable +from copy import copy import time +from dimos_generated.geometry_msgs.msg import Quaternion, Transform, TransformStamped +from dimos_generated.sensor_msgs.msg import CameraInfo, Image +from dimos_generated.std_msgs.msg import Header +from dimos_generated.tf2_msgs.msg import TFMessage from pydantic import Field import reactivex as rx @@ -25,28 +30,24 @@ from dimos.core.stream import Out from dimos.hardware.sensors.camera.spec import CameraHardware from dimos.hardware.sensors.camera.webcam import Webcam -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Transform import Transform -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo -from dimos.msgs.sensor_msgs.Image import Image, sharpness_barrier -from dimos.msgs.tf2_msgs.TFMessage import TFMessage +from dimos.msgs.image import image_sharpness +from dimos.msgs.time import time_from_nanoseconds from dimos.spec import perception +from dimos.utils.reactive import quality_barrier from dimos.visualization.vis_module import vis_module -def default_transform() -> Transform: - return Transform( - translation=Vector3(0.0, 0.0, 0.0), - rotation=Quaternion(0.0, 0.0, 0.0, 1.0), - frame_id="base_link", +def default_transform() -> TransformStamped: + return TransformStamped( + header=Header(frame_id="base_link"), child_frame_id="camera_link", + transform=Transform(rotation=Quaternion(w=1.0)), ) class CameraModuleConfig(ModuleConfig): frame_id: str = "camera_link" - transform: Transform | None = Field(default_factory=default_transform) + transform: TransformStamped | None = Field(default_factory=default_transform) hardware: Callable[[], CameraHardware] | CameraHardware = Webcam frequency: float = 0.0 # Hz, 0 means no limit @@ -71,7 +72,7 @@ def start(self) -> None: stream = self.hardware.image_stream() if self.config.frequency > 0: - stream = stream.pipe(sharpness_barrier(self.config.frequency)) + stream = stream.pipe(quality_barrier(image_sharpness, self.config.frequency)) self.register_disposable( stream.subscribe(self.color_image.publish), @@ -82,24 +83,21 @@ def start(self) -> None: ) def publish_metadata(self) -> None: - camera_info = self.hardware.camera_info.with_ts(time.time()) + camera_info = copy(self.hardware.camera_info) + camera_info.header.stamp = time_from_nanoseconds(time.time_ns()) self.camera_info.publish(camera_info) if not self.config.transform: return - camera_link = self.config.transform - camera_link.ts = camera_info.ts - - camera_optical = Transform( - translation=Vector3(0.0, 0.0, 0.0), - rotation=Quaternion(-0.5, 0.5, -0.5, 0.5), - frame_id="camera_link", - child_frame_id="camera_optical", - ts=camera_link.ts, + camera_link = copy(self.config.transform) + camera_link.header.stamp = camera_info.header.stamp + camera_optical = TransformStamped( + header=Header(frame_id=camera_link.child_frame_id, stamp=camera_info.header.stamp), + child_frame_id=camera_info.header.frame_id, + transform=Transform(rotation=Quaternion(x=-0.5, y=0.5, z=-0.5, w=0.5)), ) - - self.tf.publish(TFMessage(camera_link, camera_optical)) + self.tf.publish(TFMessage(transforms=[camera_link, camera_optical])) @rpc def stop(self) -> None: diff --git a/dimos/hardware/sensors/camera/realsense/blueprints.py b/dimos/hardware/sensors/camera/realsense/blueprints.py index 5f4c8e38648..c7c449b58fd 100644 --- a/dimos/hardware/sensors/camera/realsense/blueprints.py +++ b/dimos/hardware/sensors/camera/realsense/blueprints.py @@ -25,14 +25,15 @@ from typing import Any +from dimos_generated.geometry_msgs.msg import Quaternion, Transform, TransformStamped +from dimos_generated.sensor_msgs.msg import PointCloud2 +from dimos_generated.std_msgs.msg import Header + from dimos.core.coordination.blueprints import autoconnect from dimos.core.global_config import global_config from dimos.hardware.sensors.camera.realsense.camera import RealSenseCamera -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Transform import Transform -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 from dimos.protocol.tf.static_tf_publisher import StaticTfPublisher +from dimos.visualization.rerun.message_helpers import cloud_archetype from dimos.visualization.vis_module import vis_module # Dot radius in screen-space UI points. @@ -46,13 +47,12 @@ class RealSenseMountTf(StaticTfPublisher): path from world to it and draws nothing. """ - def transforms(self) -> list[Transform]: + def transforms(self) -> list[TransformStamped]: return [ - Transform( - translation=Vector3(0.0, 0.0, 0.0), - rotation=Quaternion(0.0, 0.0, 0.0, 1.0), - frame_id="world", + TransformStamped( + header=Header(frame_id="world"), child_frame_id="camera_link", + transform=Transform(rotation=Quaternion(w=1.0)), ) ] @@ -78,7 +78,7 @@ def _rerun_blueprint() -> Any: def _cloud(cloud: PointCloud2) -> Any: # The default "spheres" mode is sized for sparse lidar; a dense RGBD cloud # reads better as flat dots that keep their size as you zoom. - return cloud.to_rerun(mode="points", ui_radius=_CLOUD_RADIUS_UI) + return cloud_archetype(cloud, mode="points", ui_radius=_CLOUD_RADIUS_UI) _vis = vis_module( diff --git a/dimos/hardware/sensors/camera/realsense/camera.py b/dimos/hardware/sensors/camera/realsense/camera.py index 279a64f0167..cef39ecda9f 100644 --- a/dimos/hardware/sensors/camera/realsense/camera.py +++ b/dimos/hardware/sensors/camera/realsense/camera.py @@ -19,17 +19,14 @@ from typing import TYPE_CHECKING, Any +from dimos_generated.dimos_msgs.msg import ImuInfo +from dimos_generated.sensor_msgs.msg import CameraInfo, Image, Imu, PointCloud2 +from dimos_generated.tf2_msgs.msg import TFMessage from pydantic import Field from dimos.core.native_module import NativeModule, NativeModuleConfig from dimos.core.stream import Out from dimos.hardware.sensors.camera.spec import DepthCameraConfig -from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo -from dimos.msgs.sensor_msgs.Image import Image -from dimos.msgs.sensor_msgs.Imu import Imu -from dimos.msgs.sensor_msgs.ImuInfo import ImuInfo -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 -from dimos.msgs.tf2_msgs.TFMessage import TFMessage from dimos.spec import perception diff --git a/dimos/hardware/sensors/camera/realsense/rust/Cargo.lock b/dimos/hardware/sensors/camera/realsense/rust/Cargo.lock index ad82645d0df..a8ea3f2c5b4 100644 --- a/dimos/hardware/sensors/camera/realsense/rust/Cargo.lock +++ b/dimos/hardware/sensors/camera/realsense/rust/Cargo.lock @@ -220,6 +220,20 @@ name = "bytemuck" version = "1.25.2" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "95832e849adfb21180ccb6826a99da14e5d266ae5c2e668e1602cf234f153797" +dependencies = [ + "bytemuck_derive", +] + +[[package]] +name = "bytemuck_derive" +version = "1.12.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "6a1f896587b6f2c069c73d2f0913e2d590c3990285cd2f0b6aa02b786b4c679c" +dependencies = [ + "proc-macro2", + "quote", + "syn 3.0.4", +] [[package]] name = "byteorder" @@ -540,9 +554,17 @@ dependencies = [ ] [[package]] -name = "dimos-lcm" +name = "dimos-generated-messages" +version = "0.1.0" +dependencies = [ + "re_cdr", + "serde", + "serde-big-array", +] + +[[package]] +name = "dimos-lcm-transport" version = "0.1.0" -source = "git+https://github.com/dimensionalOS/dimos-lcm.git?branch=rust-codegen#04d78e8622500244123ba9cefa4c51b4cb454549" dependencies = [ "byteorder", "socket2 0.5.10", @@ -553,9 +575,9 @@ dependencies = [ name = "dimos-module" version = "0.1.0" dependencies = [ - "dimos-lcm", + "dimos-generated-messages", + "dimos-lcm-transport", "dimos-module-macros", - "lcm-msgs", "nalgebra", "rayon", "serde", @@ -582,8 +604,8 @@ dependencies = [ name = "dimos-realsense" version = "0.1.0" dependencies = [ + "dimos-generated-messages", "dimos-module", - "lcm-msgs", "pkg-config", "realsense-rust", "realsense-sys", @@ -1355,14 +1377,6 @@ version = "1.5.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "bbd2bcb4c963f2ddae06a2efc7e9f3591312473c50c6685e1f298068316e66fe" -[[package]] -name = "lcm-msgs" -version = "0.1.0" -source = "git+https://github.com/dimensionalOS/dimos-lcm.git?branch=rust-codegen#04d78e8622500244123ba9cefa4c51b4cb454549" -dependencies = [ - "byteorder", -] - [[package]] name = "libc" version = "0.2.189" @@ -2154,6 +2168,18 @@ dependencies = [ "yasna", ] +[[package]] +name = "re_cdr" +version = "0.1.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "bba10c21433b597481f366b978cdf2617e56785b38cb0dcc3833c6b8ad7e061a" +dependencies = [ + "bytemuck", + "byteorder", + "serde", + "thiserror 2.0.20", +] + [[package]] name = "realsense-rust" version = "1.3.0" @@ -2532,6 +2558,15 @@ dependencies = [ "serde_derive", ] +[[package]] +name = "serde-big-array" +version = "0.5.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "11fc7cc2c76d73e0f27ee52abbd64eec84d46f370c88371120433196934e4b7f" +dependencies = [ + "serde", +] + [[package]] name = "serde_core" version = "1.0.229" diff --git a/dimos/hardware/sensors/camera/realsense/rust/Cargo.toml b/dimos/hardware/sensors/camera/realsense/rust/Cargo.toml index db7e2d03df8..4f66e1f7922 100644 --- a/dimos/hardware/sensors/camera/realsense/rust/Cargo.toml +++ b/dimos/hardware/sensors/camera/realsense/rust/Cargo.toml @@ -19,24 +19,24 @@ path = "dimos_realsense::RealSense" python = "dimos.hardware.sensors.camera.realsense.camera:RealSenseCamera" [package.metadata.dimos.module.realsense.inputs] -tf = "tf2_msgs.TFMessage" +tf = "tf2_msgs/msg/TFMessage" [package.metadata.dimos.module.realsense.outputs] -color_image = "sensor_msgs.Image" -depth_image = "sensor_msgs.Image" -infrared_left = "sensor_msgs.Image" -infrared_right = "sensor_msgs.Image" -imu = "sensor_msgs.Imu" -imu_info = "sensor_msgs.ImuInfo" -pointcloud = "sensor_msgs.PointCloud2" -camera_info = "sensor_msgs.CameraInfo" -depth_camera_info = "sensor_msgs.CameraInfo" -infrared_left_camera_info = "sensor_msgs.CameraInfo" -infrared_right_camera_info = "sensor_msgs.CameraInfo" +color_image = "sensor_msgs/msg/Image" +depth_image = "sensor_msgs/msg/Image" +infrared_left = "sensor_msgs/msg/Image" +infrared_right = "sensor_msgs/msg/Image" +imu = "sensor_msgs/msg/Imu" +imu_info = "dimos_msgs/msg/ImuInfo" +pointcloud = "sensor_msgs/msg/PointCloud2" +camera_info = "sensor_msgs/msg/CameraInfo" +depth_camera_info = "sensor_msgs/msg/CameraInfo" +infrared_left_camera_info = "sensor_msgs/msg/CameraInfo" +infrared_right_camera_info = "sensor_msgs/msg/CameraInfo" [dependencies] dimos-module = { path = "../../../../../../native/rust/dimos-module" } -lcm-msgs = { git = "https://github.com/dimensionalOS/dimos-lcm.git", branch = "rust-codegen" } +dimos-generated-messages = { path = "../../../../../message_codegen" } realsense-rust = "1.3" realsense-sys = "2.56" tokio = { version = "1", features = ["rt-multi-thread", "macros", "signal"] } diff --git a/dimos/hardware/sensors/camera/realsense/rust/flake.nix b/dimos/hardware/sensors/camera/realsense/rust/flake.nix index 3fa255d6046..5bdf5bfd3a7 100644 --- a/dimos/hardware/sensors/camera/realsense/rust/flake.nix +++ b/dimos/hardware/sensors/camera/realsense/rust/flake.nix @@ -11,7 +11,7 @@ # Also works when NativeModule builds from a regular Python environment. # Use the Nix compiler/linker so libc matches the camera libraries. devShells = forAll (pkgs: { - default = pkgs.mkShell { packages = [ pkgs.cargo pkgs.rustc pkgs.clippy pkgs.librealsense pkgs.pkg-config ]; }; + default = pkgs.mkShell { packages = [ pkgs.python3 pkgs.cargo pkgs.rustc pkgs.clippy pkgs.librealsense pkgs.pkg-config ]; }; }); }; } diff --git a/dimos/hardware/sensors/camera/realsense/rust/src/main.rs b/dimos/hardware/sensors/camera/realsense/rust/src/main.rs index 36de88262f3..a6d22933cf4 100644 --- a/dimos/hardware/sensors/camera/realsense/rust/src/main.rs +++ b/dimos/hardware/sensors/camera/realsense/rust/src/main.rs @@ -18,20 +18,26 @@ use std::collections::{BTreeSet, HashMap, HashSet, VecDeque}; use std::ffi::{CStr, CString}; -use std::io::Write; use std::os::raw::c_void; use std::ptr::{self, NonNull}; use std::sync::atomic::{AtomicBool, Ordering}; use std::sync::{mpsc, Arc, Mutex}; use std::time::{Duration, SystemTime, UNIX_EPOCH}; +use dimos_generated_messages::builtin_interfaces::msg::Time; +use dimos_generated_messages::dimos_msgs::msg::ImuInfo; +use dimos_generated_messages::geometry_msgs::msg::{ + Quaternion as QuaternionMsg, Vector3 as Vector3Msg, +}; +use dimos_generated_messages::sensor_msgs::msg::{ + CameraInfo, Image, Imu, PointCloud2, PointField, RegionOfInterest, +}; +use dimos_generated_messages::std_msgs::msg::Header; +use dimos_module::cdr; use dimos_module::nalgebra::{ Isometry3, Matrix3, Quaternion, Rotation3, Translation3, UnitQuaternion, Vector3, }; use dimos_module::{native_config, run_with_transport, Module, Output, Tf, Transform}; -use lcm_msgs::geometry_msgs::{Quaternion as QuaternionMsg, Vector3 as Vector3Msg}; -use lcm_msgs::sensor_msgs::{CameraInfo, Image, Imu, PointCloud2, PointField, RegionOfInterest}; -use lcm_msgs::std_msgs::{Header, Time}; use realsense_rust::base::{Rs2Extrinsics, Rs2Intrinsics}; use realsense_rust::config::Config as RsConfig; use realsense_rust::context::Context; @@ -56,9 +62,6 @@ const IMU_CLOCK_WINDOW_SECONDS: i64 = 30; const FRAME_TIMEOUT: Duration = Duration::from_secs(1); // Body (REP-103) from optical axes, x y z w. const OPTICAL_ROTATION: [f64; 4] = [-0.5, 0.5, -0.5, 0.5]; -// lcm-gen fingerprint of sensor_msgs.ImuInfo; it has no generated bindings. -const IMU_INFO_FINGERPRINT: u64 = 0xe6aa5563f2a33280; - // ---- config, field for field with RealSenseCameraConfig ---- #[derive(Debug, Clone, Deserialize, Serialize)] @@ -170,36 +173,6 @@ impl Config { } } -// ---- sensor_msgs.ImuInfo, hand-encoded ---- - -#[derive(Clone)] -struct ImuInfo { - header: Header, - gyro_noise_density: f64, - gyro_random_walk: f64, - accel_noise_density: f64, - accel_random_walk: f64, - frequency: f64, -} - -impl ImuInfo { - fn encode(&self) -> Vec { - let mut buf = Vec::with_capacity(8 + self.header.encoded_size() + 40); - buf.write_all(&IMU_INFO_FINGERPRINT.to_be_bytes()).unwrap(); - self.header.encode_one(&mut buf).unwrap(); - for v in [ - self.gyro_noise_density, - self.gyro_random_walk, - self.accel_noise_density, - self.accel_random_walk, - self.frequency, - ] { - buf.write_all(&v.to_be_bytes()).unwrap(); - } - buf - } -} - // ---- module ---- /// What the capture thread learns at start and the tickers publish from. @@ -250,27 +223,27 @@ struct Outs { #[derive(Module)] #[module(name = "realsense", setup = start, teardown = stop)] struct RealSense { - #[output(encode = Image::encode)] + #[output(encode = cdr::encode)] color_image: Output, - #[output(encode = Image::encode)] + #[output(encode = cdr::encode)] depth_image: Output, - #[output(encode = Image::encode)] + #[output(encode = cdr::encode)] infrared_left: Output, - #[output(encode = Image::encode)] + #[output(encode = cdr::encode)] infrared_right: Output, - #[output(encode = Imu::encode)] + #[output(encode = cdr::encode)] imu: Output, - #[output(encode = ImuInfo::encode)] + #[output(encode = cdr::encode)] imu_info: Output, - #[output(encode = PointCloud2::encode)] + #[output(encode = cdr::encode)] pointcloud: Output, - #[output(encode = CameraInfo::encode)] + #[output(encode = cdr::encode)] camera_info: Output, - #[output(encode = CameraInfo::encode)] + #[output(encode = cdr::encode)] depth_camera_info: Output, - #[output(encode = CameraInfo::encode)] + #[output(encode = cdr::encode)] infrared_left_camera_info: Output, - #[output(encode = CameraInfo::encode)] + #[output(encode = cdr::encode)] infrared_right_camera_info: Output, #[tf] tf: Tf, @@ -360,10 +333,9 @@ fn unix_now() -> f64 { fn header(frame_id: &str, ts: f64) -> Header { let sec = ts.trunc(); Header { - seq: 0, stamp: Time { sec: sec as i32, - nsec: ((ts - sec) * 1e9) as i32, + nanosec: ((ts - sec) * 1e9) as u32, }, frame_id: frame_id.to_string(), } @@ -390,11 +362,11 @@ fn image( }; Image { header: header(frame_id, ts), - height: frame.height() as i32, - width: frame.width() as i32, + height: frame.height() as u32, + width: frame.width() as u32, encoding: encoding.to_string(), is_bigendian: 0, - step: (frame.width() * bytes_per_pixel) as i32, + step: (frame.width() * bytes_per_pixel) as u32, data: data.to_vec(), } } @@ -411,13 +383,13 @@ fn camera_info(intrinsics: &Rs2Intrinsics, frame_id: &str) -> CameraInfo { }; CameraInfo { header: header(frame_id, 0.0), - height: intrinsics.height() as i32, - width: intrinsics.width() as i32, + height: intrinsics.height() as u32, + width: intrinsics.width() as u32, distortion_model: model.to_string(), - D: intrinsics.0.coeffs.iter().map(|&c| c as f64).collect(), - K: [fx, 0.0, cx, 0.0, fy, cy, 0.0, 0.0, 1.0], - R: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0], - P: [fx, 0.0, cx, 0.0, 0.0, fy, cy, 0.0, 0.0, 0.0, 1.0, 0.0], + d: intrinsics.0.coeffs.iter().map(|&c| c as f64).collect(), + k: [fx, 0.0, cx, 0.0, fy, cy, 0.0, 0.0, 1.0], + r: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0], + p: [fx, 0.0, cx, 0.0, 0.0, fy, cy, 0.0, 0.0, 0.0, 1.0, 0.0], binning_x: 0, binning_y: 0, roi: RegionOfInterest { @@ -677,7 +649,7 @@ fn capture_thread(cfg: &Config, shared: &Shared, handle: &Handle, outs: &Outs) { // The pair is rectified, so the whole of their offset is along x. if let (Some(info), Ok(ext)) = (infra2_info.as_mut(), p2.extrinsics(p1)) { let baseline = (ext.translation()[0] as f64).abs(); - info.P[3] = -info.P[0] * baseline; + info.p[3] = -info.p[0] * baseline; } } } @@ -937,10 +909,10 @@ fn pointcloud_thread(cfg: &Config, shared: &Shared, handle: &Handle, outs: &Outs } fn empty_cloud(header: &Header) -> PointCloud2 { - let field = |name: &str, offset: i32| PointField { + let field = |name: &str, offset: u32| PointField { name: name.to_string(), offset, - datatype: PointField::FLOAT32 as u8, + datatype: PointField::FLOAT32, count: 1, }; PointCloud2 { @@ -968,8 +940,8 @@ fn rgbd_to_cloud( stride: usize, depth_trunc_m: f32, ) -> PointCloud2 { - let (fx, fy) = (info.K[0] as f32, info.K[4] as f32); - let (cx, cy) = (info.K[2] as f32, info.K[5] as f32); + let (fx, fy) = (info.k[0] as f32, info.k[4] as f32); + let (cx, cy) = (info.k[2] as f32, info.k[5] as f32); let (w, h) = (rgbd.depth.width as usize, rgbd.depth.height as usize); let rgb = &rgbd.color.data; if rgb.len() != w * h * 3 || rgbd.depth.data.len() != w * h * 2 { @@ -1020,13 +992,13 @@ fn rgbd_to_cloud( ((c[0] * 255.0) as u32) << 16 | ((c[1] * 255.0) as u32) << 8 | (c[2] * 255.0) as u32; data.extend_from_slice(&packed.to_le_bytes()); } - let field = |name: &str, offset: i32| PointField { + let field = |name: &str, offset: u32| PointField { name: name.to_string(), offset, - datatype: PointField::FLOAT32 as u8, + datatype: PointField::FLOAT32, count: 1, }; - let n = points.len() as i32; + let n = points.len() as u32; PointCloud2 { header: rgbd.depth.header.clone(), height: if n == 0 { 0 } else { 1 }, @@ -1406,10 +1378,9 @@ mod tests { accel_random_walk: 4.0, frequency: 5.0, }; - let bytes = msg.encode(); - assert_eq!(&bytes[..8], &IMU_INFO_FINGERPRINT.to_be_bytes()); - assert_eq!(bytes.len(), 8 + msg.header.encoded_size() + 40); - assert_eq!(&bytes[bytes.len() - 8..], &5.0f64.to_be_bytes()); + let bytes = cdr::encode(&msg).unwrap(); + assert_eq!(&bytes[..4], &[0, 1, 0, 0]); + assert_eq!(cdr::decode::(&bytes).unwrap(), msg); } } @@ -1427,20 +1398,20 @@ mod bench { let rgbd = Rgbd { color: Image { header: header("f", 0.0), - height: h as i32, - width: w as i32, + height: h as u32, + width: w as u32, encoding: "rgb8".into(), is_bigendian: 0, - step: (w * 3) as i32, + step: (w * 3) as u32, data: vec![128; w * h * 3], }, depth: Image { header: header("f", 0.0), - height: h as i32, - width: w as i32, + height: h as u32, + width: w as u32, encoding: "16UC1".into(), is_bigendian: 0, - step: (w * 2) as i32, + step: (w * 2) as u32, data: depth, }, }; @@ -1463,7 +1434,7 @@ mod bench { let cloud = rgbd_to_cloud(&rgbd, &info, 0.001, stride, 5.0); let build = t.elapsed(); let t = std::time::Instant::now(); - let bytes = cloud.encode(); + let bytes = cdr::encode(&cloud).unwrap(); eprintln!( "stride={stride} points={} build={:?} encode={:?} bytes={}", cloud.width, diff --git a/dimos/hardware/sensors/camera/spec.py b/dimos/hardware/sensors/camera/spec.py index ee6d2c9ee3b..613d912b439 100644 --- a/dimos/hardware/sensors/camera/spec.py +++ b/dimos/hardware/sensors/camera/spec.py @@ -14,14 +14,13 @@ from abc import ABC, abstractmethod +from dimos_generated.geometry_msgs.msg import Quaternion +from dimos_generated.sensor_msgs.msg import CameraInfo, Image from reactivex.observable import Observable -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo -from dimos.msgs.sensor_msgs.Image import Image from dimos.protocol.service.spec import BaseConfig, Configurable -OPTICAL_ROTATION = Quaternion(-0.5, 0.5, -0.5, 0.5) +OPTICAL_ROTATION = Quaternion(x=-0.5, y=0.5, z=-0.5, w=0.5) class CameraConfig(BaseConfig): diff --git a/dimos/hardware/sensors/camera/test_module.py b/dimos/hardware/sensors/camera/test_module.py new file mode 100644 index 00000000000..ec1c5b30519 --- /dev/null +++ b/dimos/hardware/sensors/camera/test_module.py @@ -0,0 +1,59 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Camera metadata uses the same generated header and frame chain as its images.""" + +from dimos_generated.sensor_msgs.msg import CameraInfo +from dimos_generated.tf2_msgs.msg import TFMessage +import numpy as np +import pytest + +from dimos.hardware.sensors.camera.module import CameraModule +from dimos.hardware.sensors.camera.webcam import Webcam +from dimos.msgs.geometry import transform_matrix + + +@pytest.fixture +def module(): + camera = CameraModule() + camera.hardware = Webcam(frame_id_prefix="robot") + try: + yield camera + finally: + camera.stop() + + +def test_metadata_preserves_calibration_and_does_not_mutate_mount_template(module, mocker): + info_publish = mocker.patch.object(module.camera_info, "publish") + tf_publish = mocker.patch.object(module.tf, "publish") + mocker.patch( + "dimos.hardware.sensors.camera.module.time.time_ns", return_value=1700000000123456789 + ) + original = module.config.transform.encode() + + module.publish_metadata() + + info_publish.assert_called_once() + tf_publish.assert_called_once() + info = CameraInfo.decode(info_publish.call_args.args[0].encode()) + transforms = TFMessage.decode(tf_publish.call_args.args[0].encode()).transforms + assert (info.header.stamp.sec, info.header.stamp.nanosec) == (1700000000, 123456789) + assert len(transforms) == 2 + assert [(edge.header.frame_id, edge.child_frame_id) for edge in transforms] == [ + ("base_link", "camera_link"), + ("camera_link", "robot/camera_optical"), + ] + assert all(edge.header.stamp == info.header.stamp for edge in transforms) + assert module.config.transform.encode() == original + np.testing.assert_allclose(transform_matrix(transforms[0].transform), np.eye(4)) diff --git a/dimos/hardware/sensors/camera/test_webcam.py b/dimos/hardware/sensors/camera/test_webcam.py index 6e390d078c7..44264d4af3e 100644 --- a/dimos/hardware/sensors/camera/test_webcam.py +++ b/dimos/hardware/sensors/camera/test_webcam.py @@ -12,27 +12,53 @@ # See the License for the specific language governing permissions and # limitations under the License. +from dimos_generated.sensor_msgs.msg import CameraInfo, Image +from dimos_generated.std_msgs.msg import Header +import numpy as np +import pytest + from dimos.hardware.sensors.camera.webcam import Webcam -from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo +from dimos.msgs.camera_info import camera_info_from_fov +from dimos.msgs.image import image_view def test_webcam_without_intrinsics_reports_a_nominal_pinhole() -> None: info = Webcam(width=1280, height=720).camera_info assert (info.width, info.height) == (1280, 720) - assert info.K[0] > 0 - assert info.frame_id == "camera_optical" + assert info.k[0] > 0 + assert info.header.frame_id == "camera_optical" def test_webcam_keeps_configured_intrinsics() -> None: - configured = CameraInfo.from_fov(90.0, 640, 480) + configured = camera_info_from_fov(90.0, 640, 480, header=Header()) assert Webcam(camera_info=configured).camera_info is configured def test_webcam_with_size_but_no_focal_length_gets_a_nominal_pinhole() -> None: info = Webcam(camera_info=CameraInfo(width=1280, height=720)).camera_info - assert info.K[0] > 0 + assert info.k[0] > 0 def test_webcam_stereo_slice_halves_the_nominal_pinhole_width() -> None: info = Webcam(width=1280, height=720, stereo_slice="left").camera_info assert (info.width, info.height) == (640, 720) + + +@pytest.mark.parametrize("side,start", [("left", 0), ("right", 2)]) +def test_capture_converts_bgr_and_crops_before_cdr_encoding(side, start, mocker): + camera = Webcam(stereo_slice=side, frame_id_prefix="robot") + pixels = np.arange(2 * 4 * 3, dtype=np.uint8).reshape(2, 4, 3) + capture = mocker.Mock() + capture.read.return_value = (True, pixels) + camera._capture = capture + mocker.patch("dimos.msgs.time.time.time_ns", return_value=1700000000123456789) + try: + message = Image.decode(camera.capture_frame().encode()) + finally: + camera.stop() + assert message.encoding == "rgb8" + assert (message.width, message.height, message.step) == (2, 2, 6) + assert message.header.frame_id == "robot/camera_optical" + assert (message.header.stamp.sec, message.header.stamp.nanosec) == (1700000000, 123456789) + np.testing.assert_array_equal(image_view(message), pixels[:, start : start + 2, ::-1]) + capture.release.assert_called_once() diff --git a/dimos/hardware/sensors/camera/webcam.py b/dimos/hardware/sensors/camera/webcam.py index 8523f356b6d..a64f6081b9d 100644 --- a/dimos/hardware/sensors/camera/webcam.py +++ b/dimos/hardware/sensors/camera/webcam.py @@ -18,13 +18,16 @@ import time from typing import Annotated, Any, Literal +from dimos_generated.sensor_msgs.msg import CameraInfo, Image +from dimos_generated.std_msgs.msg import Header from pydantic import BeforeValidator, Field from reactivex import create from reactivex.observable import Observable from dimos.hardware.sensors.camera.spec import CameraConfig, CameraHardware -from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo -from dimos.msgs.sensor_msgs.Image import Image, ImageFormat +from dimos.msgs.camera_info import camera_info_from_fov +from dimos.msgs.image import image_from_array +from dimos.msgs.time import header_now from dimos.utils.reactive import backpressure @@ -142,22 +145,13 @@ def capture_frame(self) -> Image: # Convert BGR to RGB (OpenCV uses BGR by default) frame_rgb = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB) - # Create Image message - # Using Image.from_numpy() since it's designed for numpy arrays - # Setting format to RGB since we converted from BGR->RGB above - image = Image.from_numpy( - frame_rgb, - format=ImageFormat.RGB, # We converted to RGB above - frame_id=self._frame("camera_optical"), # Standard frame ID for camera images - ts=time.time(), # Current timestamp - ) - if self.config.stereo_slice in ("left", "right"): - half_width = image.width // 2 - if self.config.stereo_slice == "left": - image = image.crop(0, 0, half_width, image.height) - else: - image = image.crop(half_width, 0, half_width, image.height) + half_width = frame_rgb.shape[1] // 2 + start = 0 if self.config.stereo_slice == "left" else half_width + frame_rgb = frame_rgb[:, start : start + half_width] + image = image_from_array( + frame_rgb, encoding="rgb8", header=header_now(self._frame("camera_optical")) + ) self._emitted_size = (image.width, image.height) return image @@ -190,7 +184,7 @@ def _capture_loop(self) -> None: @property def camera_info(self) -> CameraInfo: info = self.config.camera_info - if info.width and info.height and info.K[0] > 0 and info.K[4] > 0: + if info.width and info.height and info.k[0] > 0 and info.k[4] > 0: return info # No intrinsics configured: a nominal pinhole so the image still renders. # Sized from the frames actually emitted (the stereo slice halves them). @@ -199,8 +193,12 @@ def camera_info(self) -> CameraInfo: else: width = self.config.width // 2 if self.config.stereo_slice else self.config.width height = self.config.height - return CameraInfo.from_fov( - 60.0, width, height, axis="horizontal", frame_id=self._frame("camera_optical") + return camera_info_from_fov( + 60.0, + width, + height, + axis="horizontal", + header=Header(frame_id=self._frame("camera_optical")), ) def emit(self, image: Image) -> None: ... diff --git a/dimos/hardware/sensors/camera/zed/camera.py b/dimos/hardware/sensors/camera/zed/camera.py index fd777339dfc..b240f7adcad 100644 --- a/dimos/hardware/sensors/camera/zed/camera.py +++ b/dimos/hardware/sensors/camera/zed/camera.py @@ -18,6 +18,10 @@ import threading import time +from dimos_generated.geometry_msgs.msg import Quaternion, Transform, TransformStamped, Vector3 +from dimos_generated.sensor_msgs.msg import CameraInfo, Image, PointCloud2 +from dimos_generated.std_msgs.msg import Header +from dimos_generated.tf2_msgs.msg import TFMessage from pydantic import Field import pyzed.sl as sl import reactivex as rx @@ -33,13 +37,11 @@ DepthCameraConfig, DepthCameraHardware, ) -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Transform import Transform -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo -from dimos.msgs.sensor_msgs.Image import Image, ImageFormat -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 -from dimos.msgs.tf2_msgs.TFMessage import TFMessage +from dimos.msgs.camera_info import camera_info_with_stamp +from dimos.msgs.geometry import compose_transforms, inverse_transform +from dimos.msgs.image import image_from_array +from dimos.msgs.pointcloud import pointcloud_from_rgbd, voxel_downsample_cloud +from dimos.msgs.time import time_from_seconds from dimos.spec import perception from dimos.utils.reactive import backpressure @@ -47,8 +49,8 @@ def default_base_transform() -> Transform: """Default identity transform for camera mounting.""" return Transform( - translation=Vector3(0.0, 0.0, 0.0), - rotation=Quaternion(0.0, 0.0, 0.0, 1.0), + translation=Vector3(), + rotation=Quaternion(w=1.0), ) @@ -131,11 +133,13 @@ def __init__(self, *args, **kwargs) -> None: # type: ignore[no-untyped-def] def _publish_camera_info(self) -> None: ts = time.time() if self._color_camera_info: - self._color_camera_info.ts = ts - self.camera_info.publish(self._color_camera_info) + self.camera_info.publish( + camera_info_with_stamp(self._color_camera_info, time_from_seconds(ts)) + ) if self._depth_camera_info: - self._depth_camera_info.ts = ts - self.depth_camera_info.publish(self._depth_camera_info) + self.depth_camera_info.publish( + camera_info_with_stamp(self._depth_camera_info, time_from_seconds(ts)) + ) @rpc def start(self) -> None: @@ -232,10 +236,11 @@ def _intrinsics_to_camera_info( height=self._stream_height, width=self._stream_width, distortion_model="plumb_bob", - D=D, - K=K, - P=P, - frame_id=frame_id, + d=D, + k=K, + r=[1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0], + p=P, + header=Header(frame_id=frame_id), ) def _get_extrinsics(self) -> None: @@ -251,15 +256,16 @@ def _extrinsics_to_transform( frame_id: str, child_frame_id: str, ts: float, - ) -> Transform: + ) -> TransformStamped: translation = extrinsics.get_translation().get() - quat = extrinsics.get_orientation().get() # [x, y, z, w] - return Transform( - translation=Vector3(*translation), - rotation=Quaternion(quat[0], quat[1], quat[2], quat[3]), - frame_id=frame_id, + quat = extrinsics.get_orientation().get() + return TransformStamped( + header=Header(frame_id=frame_id, stamp=time_from_seconds(ts)), child_frame_id=child_frame_id, - ts=ts, + transform=Transform( + translation=Vector3(x=translation[0], y=translation[1], z=translation[2]), + rotation=Quaternion(x=quat[0], y=quat[1], z=quat[2], w=quat[3]), + ), ) def _enable_tracking(self) -> None: @@ -301,11 +307,10 @@ def _capture_loop(self) -> None: if color_data.ndim == 3 and color_data.shape[2] == 4: color_data = color_data[:, :, :3] color_data = cv2.cvtColor(color_data, cv2.COLOR_BGR2RGB) - color_img = Image( - data=color_data, - format=ImageFormat.RGB, - frame_id=self._color_optical_frame, - ts=ts, + color_img = image_from_array( + color_data, + encoding="rgb8", + header=Header(frame_id=self._color_optical_frame, stamp=time_from_seconds(ts)), ) self.color_image.publish(color_img) @@ -320,11 +325,10 @@ def _capture_loop(self) -> None: if self.config.align_depth_to_color else self._depth_optical_frame ) - depth_img = Image( - data=depth_data, - format=ImageFormat.DEPTH, - frame_id=depth_frame_id, - ts=ts, + depth_img = image_from_array( + depth_data, + encoding="32FC1", + header=Header(frame_id=depth_frame_id, stamp=time_from_seconds(ts)), ) self.depth_image.publish(depth_img) @@ -335,97 +339,62 @@ def _capture_loop(self) -> None: self._publish_tf(ts) - def _tracking_transform(self, ts: float) -> Transform | None: + def _tracking_transform(self, ts: float) -> TransformStamped | None: if not self._tracking_enabled or self._zed is None or self._pose is None: return None state = self._zed.get_position(self._pose, sl.REFERENCE_FRAME.WORLD) if state != sl.POSITIONAL_TRACKING_STATE.OK: return None - translation = self._pose.get_translation().get().tolist() - rotation = self._pose.get_orientation().get().tolist() - world_to_camera = Transform( - translation=Vector3(*translation), - rotation=Quaternion(*rotation), - frame_id=self.config.world_frame, - child_frame_id=self._camera_link, - ts=ts, + world_to_camera = self._extrinsics_to_transform( + self._pose, self.config.world_frame, self._camera_link, ts ) if self.config.base_transform is None: return world_to_camera - - base_to_camera = Transform( - translation=self.config.base_transform.translation, - rotation=self.config.base_transform.rotation, - frame_id=self.config.base_frame_id, + base_to_camera = TransformStamped( + header=Header(frame_id=self.config.base_frame_id, stamp=time_from_seconds(ts)), child_frame_id=self._camera_link, - ts=ts, + transform=self.config.base_transform, ) - camera_to_base = base_to_camera.inverse() - world_to_base = world_to_camera + camera_to_base - world_to_base.frame_id = self.config.world_frame - world_to_base.child_frame_id = self.config.base_frame_id - world_to_base.ts = ts - return world_to_base + return compose_transforms(world_to_camera, inverse_transform(base_to_camera)) def _publish_tf(self, ts: float) -> None: - transforms = [] - + transforms: list[TransformStamped] = [] + stamp = time_from_seconds(ts) if self.config.base_transform is not None: - base_to_camera = Transform( - translation=self.config.base_transform.translation, - rotation=self.config.base_transform.rotation, - frame_id=self.config.base_frame_id, - child_frame_id=self._camera_link, - ts=ts, + transforms.append( + TransformStamped( + header=Header(frame_id=self.config.base_frame_id, stamp=stamp), + child_frame_id=self._camera_link, + transform=self.config.base_transform, + ) ) - transforms.append(base_to_camera) - - # camera_imu_transform is IMU -> left_camera (coordinate transform), - # we need to invert to get the pose of left camera in camera_link frame - camera_link_to_depth = self._extrinsics_to_transform( - self._camera_link_to_color_extrinsics, - self._camera_link, - self._depth_frame, - ts, - ).inverse() - camera_link_to_depth.frame_id = self._camera_link - camera_link_to_depth.child_frame_id = self._depth_frame - transforms.append(camera_link_to_depth) - - depth_to_depth_optical = Transform( - translation=Vector3(0.0, 0.0, 0.0), - rotation=OPTICAL_ROTATION, - frame_id=self._depth_frame, - child_frame_id=self._depth_optical_frame, - ts=ts, - ) - transforms.append(depth_to_depth_optical) - - color_tf = self._extrinsics_to_transform( - self._camera_link_to_color_extrinsics, - self._camera_link, - self._color_frame, - ts, - ).inverse() - color_tf.frame_id = self._camera_link - color_tf.child_frame_id = self._color_frame - transforms.append(color_tf) - - color_to_color_optical = Transform( - translation=Vector3(0.0, 0.0, 0.0), - rotation=OPTICAL_ROTATION, - frame_id=self._color_frame, - child_frame_id=self._color_optical_frame, - ts=ts, - ) - transforms.append(color_to_color_optical) + # SDK extrinsics are the IMU-to-left-camera coordinate transform; + # retain the inverse used by the existing camera-link pose convention. + for frame, optical_frame in ( + (self._depth_frame, self._depth_optical_frame), + (self._color_frame, self._color_optical_frame), + ): + extrinsics = inverse_transform( + self._extrinsics_to_transform( + self._camera_link_to_color_extrinsics, self._camera_link, frame, ts + ) + ) + extrinsics.header.frame_id = self._camera_link + extrinsics.child_frame_id = frame + transforms.append(extrinsics) + transforms.append( + TransformStamped( + header=Header(frame_id=frame, stamp=stamp), + child_frame_id=optical_frame, + transform=Transform(rotation=OPTICAL_ROTATION), + ) + ) tracking_tf = self._tracking_transform(ts) if tracking_tf is not None: transforms.append(tracking_tf) - - self.tf.publish(TFMessage(*transforms)) + self.tf.publish(TFMessage(transforms=transforms)) def _generate_pointcloud(self) -> None: with self._pointcloud_lock: @@ -436,13 +405,13 @@ def _generate_pointcloud(self) -> None: return try: - pcd = PointCloud2.from_rgbd( - color_image=color_img, - depth_image=depth_img, - camera_info=self._color_camera_info, + pcd = pointcloud_from_rgbd( + color=color_img, + depth=depth_img, + calibration=self._color_camera_info, depth_scale=self._depth_scale, ) - pcd = pcd.voxel_downsample(0.005) + pcd = voxel_downsample_cloud(pcd, 0.005) self.pointcloud.publish(pcd) except Exception as e: print(f"Pointcloud generation error: {e}") diff --git a/dimos/hardware/sensors/camera/zed/compat.py b/dimos/hardware/sensors/camera/zed/compat.py index f7e78fc5d62..2ee92c2341f 100644 --- a/dimos/hardware/sensors/camera/zed/compat.py +++ b/dimos/hardware/sensors/camera/zed/compat.py @@ -16,7 +16,7 @@ from pathlib import Path -from dimos.msgs.sensor_msgs.CameraInfo import CalibrationProvider +from dimos.msgs.camera_info import CalibrationProvider try: import pyzed.sl # noqa: F401 diff --git a/dimos/hardware/sensors/camera/zed/test_generated_tf.py b/dimos/hardware/sensors/camera/zed/test_generated_tf.py new file mode 100644 index 00000000000..84ea16dd3ae --- /dev/null +++ b/dimos/hardware/sensors/camera/zed/test_generated_tf.py @@ -0,0 +1,110 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from enum import Enum +import importlib.util +from pathlib import Path +import sys +from types import ModuleType, SimpleNamespace +from unittest.mock import Mock + +from dimos_generated.geometry_msgs.msg import Transform, Vector3 +from dimos_generated.tf2_msgs.msg import TFMessage +import numpy as np +import pytest + +from dimos.msgs.geometry import transform_matrix +from dimos.msgs.time import time_from_seconds + + +@pytest.fixture +def camera_module(monkeypatch): + # Load the real camera class with only the optional SDK boundary mocked. + # Neither Camera() nor start() is invoked. + sl = ModuleType("pyzed.sl") + sl.DEPTH_MODE = Enum("DEPTH_MODE", {"NEURAL": 1}) + sl.REFERENCE_FRAME = SimpleNamespace(WORLD=1) + sl.POSITIONAL_TRACKING_STATE = SimpleNamespace(OK=1) + package = ModuleType("pyzed") + package.sl = sl + monkeypatch.setitem(sys.modules, "pyzed", package) + monkeypatch.setitem(sys.modules, "pyzed.sl", sl) + spec = importlib.util.spec_from_file_location( + "_zed_generated_tf_test", Path(__file__).with_name("camera.py") + ) + module = importlib.util.module_from_spec(spec) + monkeypatch.setitem(sys.modules, spec.name, module) + spec.loader.exec_module(module) + return module + + +def sdk_pose(translation): + return SimpleNamespace( + get_translation=lambda: SimpleNamespace(get=lambda: np.asarray(translation)), + get_orientation=lambda: SimpleNamespace(get=lambda: np.asarray([0.0, 0.0, 0.0, 1.0])), + ) + + +@pytest.mark.parametrize("mount", [None, Transform(translation=Vector3(x=0.25))]) +def test_tracking_uses_generated_frame_chain(camera_module, mount): + camera = camera_module.ZEDCamera(base_transform=mount) + camera._zed = Mock() + camera._zed.get_position.return_value = 1 + camera._pose = sdk_pose([1.0, 2.0, 3.0]) + camera._tracking_enabled = True + try: + result = camera._tracking_transform(1700000000.125) + assert result.header.frame_id == "world" + assert result.header.stamp == time_from_seconds(1700000000.125) + assert result.child_frame_id == ("camera_link" if mount is None else "base_link") + assert result.transform.translation.x == (1.0 if mount is None else 0.75) + assert result.transform.translation.y == 2.0 + camera._zed.get_position.return_value = 0 + assert camera._tracking_transform(2.0) is None + finally: + camera._zed = None + camera.stop() + + +def test_generated_tf_preserves_mount_extrinsics_and_optical_frames(camera_module, monkeypatch): + camera = camera_module.ZEDCamera() + camera._camera_link_to_color_extrinsics = sdk_pose([0.1, 0.2, 0.3]) + received = [] + monkeypatch.setattr(camera.tf, "publish", received.append) + try: + camera._publish_tf(1700000000.125) + message = TFMessage.decode(received[0].encode()) + assert len(message.transforms) == 5 + frames = [(t.header.frame_id, t.child_frame_id) for t in message.transforms] + assert frames == [ + ("base_link", "camera_link"), + ("camera_link", "camera_depth_frame"), + ("camera_depth_frame", "camera_depth_optical_frame"), + ("camera_link", "camera_color_frame"), + ("camera_color_frame", "camera_color_optical_frame"), + ] + assert all(t.header.stamp == time_from_seconds(1700000000.125) for t in message.transforms) + np.testing.assert_array_equal(transform_matrix(message.transforms[0].transform), np.eye(4)) + for index in (1, 3): + np.testing.assert_allclose( + transform_matrix(message.transforms[index].transform)[:3, 3], [-0.1, -0.2, -0.3] + ) + for index in (2, 4): + np.testing.assert_allclose( + transform_matrix(message.transforms[index].transform)[:3, :3], + [[0.0, 0.0, 1.0], [-1.0, 0.0, 0.0], [0.0, -1.0, 0.0]], + atol=1e-15, + ) + finally: + camera.stop() diff --git a/dimos/hardware/sensors/camera/zed/test_zed.py b/dimos/hardware/sensors/camera/zed/test_zed.py index a98055a3559..a0ef345891c 100644 --- a/dimos/hardware/sensors/camera/zed/test_zed.py +++ b/dimos/hardware/sensors/camera/zed/test_zed.py @@ -13,13 +13,11 @@ # See the License for the specific language governing permissions and # limitations under the License. -import pytest +from dimos_generated.sensor_msgs.msg import CameraInfo from dimos.hardware.sensors.camera.zed import compat as zed -from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo -@pytest.mark.skipif(not zed.HAS_ZED_SDK, reason="ZED SDK not installed") def test_zed_import_and_calibration_access() -> None: """Test that zed module can be imported and calibrations accessed.""" # Test that CameraInfo is accessible @@ -42,3 +40,11 @@ def test_zed_import_and_calibration_access() -> None: assert camera_info_snake is camera_info_pascal print("✓ ZED import and calibration access test passed!") + + +def test_calibration_is_generated_cdr_with_default_optical_frame(): + value = zed.CameraInfo.SingleWebcam + assert type(value) is CameraInfo + assert value.header.frame_id == "camera_optical" + assert value.k[0] == 379.45267 + assert CameraInfo.decode(value.encode()) == value diff --git a/dimos/hardware/sensors/lidar/common/point_cloud_utils.hpp b/dimos/hardware/sensors/lidar/common/point_cloud_utils.hpp index 1a9d43d2139..f9b95a3e4d3 100644 --- a/dimos/hardware/sensors/lidar/common/point_cloud_utils.hpp +++ b/dimos/hardware/sensors/lidar/common/point_cloud_utils.hpp @@ -5,29 +5,32 @@ #pragma once -#include +#include +#include +#include #include #include +#include -#include "sensor_msgs/PointCloud2.hpp" -#include "sensor_msgs/PointField.hpp" -#include "std_msgs/Header.hpp" -#include "std_msgs/Time.hpp" namespace dimos { -inline std_msgs::Time time_from_seconds(double t) { - std_msgs::Time ts; - ts.sec = static_cast(t); - ts.nsec = static_cast((t - ts.sec) * 1e9); +inline builtin_interfaces::msg::Time time_from_seconds(double t) { + if (!std::isfinite(t)) throw std::invalid_argument("timestamp must be finite"); + double seconds = std::floor(t); + auto nanoseconds = std::llround((t - seconds) * 1e9); + if (nanoseconds == 1000000000) { ++seconds; nanoseconds = 0; } + if (seconds < std::numeric_limits::min() || seconds > std::numeric_limits::max()) + throw std::invalid_argument("timestamp exceeds ROS Time range"); + builtin_interfaces::msg::Time ts; + ts.sec = static_cast(seconds); + ts.nanosec = static_cast(nanoseconds); return ts; } -// Stamped Header with a process-wide auto-incrementing sequence number. -inline std_msgs::Header make_header(const std::string& frame_id, double ts) { - static std::atomic seq{0}; - std_msgs::Header h; - h.seq = seq.fetch_add(1, std::memory_order_relaxed); +// ROS Header carries the source timestamp and coordinate frame. +inline std_msgs::msg::Header make_header(const std::string& frame_id, double ts) { + std_msgs::msg::Header h; h.stamp = time_from_seconds(ts); h.frame_id = frame_id; return h; @@ -39,22 +42,23 @@ inline constexpr int32_t kXyziPointStep = kXyziFieldCount * sizeof(float); // Empty XYZI PointCloud2 sized for num_points. The caller fills each point // through xyzi_point() below. -inline sensor_msgs::PointCloud2 make_xyzi_cloud(const std::string& frame_id, double ts, +inline sensor_msgs::msg::PointCloud2 make_xyzi_cloud(const std::string& frame_id, double ts, int num_points) { - sensor_msgs::PointCloud2 pc; + if (num_points < 0 || static_cast(num_points) * kXyziPointStep > std::numeric_limits::max()) + throw std::invalid_argument("point cloud row exceeds ROS uint32 dimensions"); + sensor_msgs::msg::PointCloud2 pc; pc.header = make_header(frame_id, ts); pc.height = 1; pc.width = num_points; pc.is_bigendian = 0; pc.is_dense = 1; - pc.fields_length = kXyziFieldCount; pc.fields.resize(kXyziFieldCount); auto make_field = [](const std::string& name, int32_t index) { - sensor_msgs::PointField f; + sensor_msgs::msg::PointField f; f.name = name; f.offset = index * static_cast(sizeof(float)); - f.datatype = sensor_msgs::PointField::FLOAT32; + f.datatype = sensor_msgs::msg::PointField::FLOAT32; f.count = 1; return f; }; @@ -65,13 +69,12 @@ inline sensor_msgs::PointCloud2 make_xyzi_cloud(const std::string& frame_id, dou pc.point_step = kXyziPointStep; pc.row_step = pc.point_step * num_points; - pc.data_length = pc.row_step; - pc.data.resize(pc.data_length); + pc.data.resize(pc.row_step); return pc; } // The x/y/z/intensity slot of point i in a cloud from make_xyzi_cloud. -inline float* xyzi_point(sensor_msgs::PointCloud2& pc, int i) { +inline float* xyzi_point(sensor_msgs::msg::PointCloud2& pc, int i) { return reinterpret_cast(pc.data.data() + i * kXyziPointStep); } diff --git a/dimos/hardware/sensors/lidar/fastlio2/cpp/CMakeLists.txt b/dimos/hardware/sensors/lidar/fastlio2/cpp/CMakeLists.txt index 91e96cb082d..5b953318420 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/cpp/CMakeLists.txt +++ b/dimos/hardware/sensors/lidar/fastlio2/cpp/CMakeLists.txt @@ -39,13 +39,7 @@ if(NOT FASTLIO_DIR) set(FASTLIO_DIR ${CMAKE_CURRENT_SOURCE_DIR}/fast-lio-non-ros) endif() -# dimos-lcm C++ message headers -FetchContent_Declare(dimos_lcm - GIT_REPOSITORY https://github.com/dimensionalOS/dimos-lcm.git - GIT_TAG main - GIT_SHALLOW TRUE -) -FetchContent_MakeAvailable(dimos_lcm) + # The header-only dimos native C++ SDK. Overridable so nix can point at the # flake input instead of the in-repo relative path. @@ -87,7 +81,6 @@ endif() target_include_directories(fastlio2_native PRIVATE ${FASTLIO_DIR}/include ${FASTLIO_DIR}/src - ${dimos_lcm_SOURCE_DIR}/generated/cpp_lcm_msgs ${LCM_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIR} ${PCL_INCLUDE_DIRS} diff --git a/dimos/hardware/sensors/lidar/fastlio2/cpp/flake.lock b/dimos/hardware/sensors/lidar/fastlio2/cpp/flake.lock index 7adfee54000..e520f5b284a 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/cpp/flake.lock +++ b/dimos/hardware/sensors/lidar/fastlio2/cpp/flake.lock @@ -1,22 +1,5 @@ { "nodes": { - "dimos-lcm": { - "flake": false, - "locked": { - "lastModified": 1769774949, - "narHash": "sha256-icRK7jerqNlwK1WZBrnIP04I2WozzFqTD7qsmnPxQuo=", - "owner": "dimensionalOS", - "repo": "dimos-lcm", - "rev": "0aa72b7b1bd3a65f50f5c03485ee9b728df56afe", - "type": "github" - }, - "original": { - "owner": "dimensionalOS", - "ref": "main", - "repo": "dimos-lcm", - "type": "github" - } - }, "fast-lio": { "flake": false, "locked": { @@ -163,7 +146,6 @@ }, "root": { "inputs": { - "dimos-lcm": "dimos-lcm", "fast-lio": "fast-lio", "flake-utils": "flake-utils", "lcm-extended": "lcm-extended", diff --git a/dimos/hardware/sensors/lidar/fastlio2/cpp/flake.nix b/dimos/hardware/sensors/lidar/fastlio2/cpp/flake.nix index 4ae6a9d0798..34ecabe9f0c 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/cpp/flake.nix +++ b/dimos/hardware/sensors/lidar/fastlio2/cpp/flake.nix @@ -8,10 +8,6 @@ livox-sdk.url = "path:../../livox/cpp"; livox-sdk.inputs.nixpkgs.follows = "nixpkgs"; livox-sdk.inputs.flake-utils.follows = "flake-utils"; - dimos-lcm = { - url = "github:dimensionalOS/dimos-lcm/main"; - flake = false; - }; # Standalone Boost.PFR, consumed by the SDK via a FetchContent source override. pfr = { url = "github:apolukhin/pfr_non_boost/2.3.2"; @@ -30,7 +26,7 @@ }; }; - outputs = { self, nixpkgs, zenoh, flake-utils, livox-sdk, dimos-lcm, pfr, fast-lio, lcm-extended, ... }: + outputs = { self, nixpkgs, zenoh, flake-utils, livox-sdk, pfr, fast-lio, lcm-extended, ... }: flake-utils.lib.eachDefaultSystem (system: let # Overlay fixes for darwin-broken nixpkgs recipes in our transitive @@ -83,6 +79,7 @@ nativeBuildInputs = [ pkgs.cmake pkgs.pkg-config ]; buildInputs = [ + (import ../../../../../../native/cpp/messages.nix { inherit pkgs; }) livox-sdk2 lcm pkgs.glib @@ -97,7 +94,6 @@ cmakeFlags = [ "-DCMAKE_POLICY_VERSION_MINIMUM=3.5" - "-DFETCHCONTENT_SOURCE_DIR_DIMOS_LCM=${dimos-lcm}" "-DFETCHCONTENT_SOURCE_DIR_PFR=${pfr}" "-DFASTLIO_DIR=${fast-lio}" "-DLIVOX_COMMON_DIR=${livox-common}" diff --git a/dimos/hardware/sensors/lidar/fastlio2/cpp/main.cpp b/dimos/hardware/sensors/lidar/fastlio2/cpp/main.cpp index e45adcf03ba..b8c89ab074f 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/cpp/main.cpp +++ b/dimos/hardware/sensors/lidar/fastlio2/cpp/main.cpp @@ -21,10 +21,8 @@ #include "point_cloud_utils.hpp" #include "dimos/native.hpp" +#include -#include "nav_msgs/Odometry.hpp" -#include "sensor_msgs/PointCloud2.hpp" -#include "std_msgs/Header.hpp" // FAST-LIO (header-only core, compiled sources linked via CMake) #include "fast_lio.hpp" @@ -108,8 +106,8 @@ class FastLio2 : public Module { public: void build(Builder& builder, Config& config) override { cfg_ = config.parse(); - lidar_ = builder.output("lidar"); - odometry_ = builder.output("odometry"); + lidar_ = builder.output("lidar"); + odometry_ = builder.output("odometry"); frame_interval_ = std::chrono::microseconds(static_cast(1e6 / cfg_.frequency)); @@ -371,7 +369,7 @@ class FastLio2 : public Module { void publish_pointcloud(const PointCloudXYZI::Ptr& cloud, double ts) { int num_points = static_cast(cloud->size()); - sensor_msgs::PointCloud2 pc = make_xyzi_cloud(cfg_.sensor_frame_id, ts, num_points); + sensor_msgs::msg::PointCloud2 pc = make_xyzi_cloud(cfg_.sensor_frame_id, ts, num_points); for (int i = 0; i < num_points; ++i) { float* dst = xyzi_point(pc, i); @@ -386,7 +384,7 @@ class FastLio2 : public Module { // Publish odometry as frame_id (fixed) -> sensor_frame_id (moving sensor). void publish_odometry(const custom_messages::Odometry& odom, double ts) { - nav_msgs::Odometry msg; + nav_msgs::msg::Odometry msg; msg.header = make_header(cfg_.frame_id, ts); msg.child_frame_id = cfg_.sensor_frame_id; @@ -409,14 +407,14 @@ class FastLio2 : public Module { msg.twist.twist.angular.x = 0; msg.twist.twist.angular.y = 0; msg.twist.twist.angular.z = 0; - std::memset(msg.twist.covariance, 0, sizeof(msg.twist.covariance)); + msg.twist.covariance.fill(0); odometry_.publish(msg); } FastLio2Config cfg_; - Output lidar_; - Output odometry_; + Output lidar_; + Output odometry_; std::unique_ptr fast_lio_; // All four come from config, set in build(). diff --git a/dimos/hardware/sensors/lidar/fastlio2/module.py b/dimos/hardware/sensors/lidar/fastlio2/module.py index c179a982ace..4f0a8b8c38f 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/module.py +++ b/dimos/hardware/sensors/lidar/fastlio2/module.py @@ -25,9 +25,11 @@ from __future__ import annotations import os -import time from typing import TYPE_CHECKING +from dimos_generated.nav_msgs.msg import Odometry +from dimos_generated.sensor_msgs.msg import PointCloud2 +from dimos_generated.tf2_msgs.msg import TFMessage from pydantic import Field from reactivex.disposable import Disposable @@ -47,12 +49,7 @@ SDK_POINT_DATA_PORT, SDK_PUSH_MSG_PORT, ) -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Transform import Transform -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.msgs.nav_msgs.Odometry import Odometry -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 -from dimos.msgs.tf2_msgs.TFMessage import TFMessage +from dimos.msgs.geometry import transform_from_odometry from dimos.spec import perception @@ -141,17 +138,7 @@ def start(self) -> None: ) def _on_odom_for_tf(self, msg: Odometry) -> None: - self.tf.publish( - TFMessage( - Transform( - frame_id=self.frame_id, - child_frame_id=self.config.sensor_frame_id, - translation=Vector3(msg.pose.position), - rotation=Quaternion(msg.pose.orientation), - ts=msg.ts or time.time(), - ) - ) - ) + self.tf.publish(TFMessage(transforms=[transform_from_odometry(msg)])) @rpc def stop(self) -> None: diff --git a/dimos/hardware/sensors/lidar/fastlio2/recorder.py b/dimos/hardware/sensors/lidar/fastlio2/recorder.py index f6400df3545..319f029e645 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/recorder.py +++ b/dimos/hardware/sensors/lidar/fastlio2/recorder.py @@ -24,11 +24,13 @@ from __future__ import annotations +from dimos_generated.geometry_msgs.msg import Pose +from dimos_generated.nav_msgs.msg import Odometry +from dimos_generated.sensor_msgs.msg import PointCloud2 + from dimos.core.stream import In -from dimos.memory.module import OnExisting, Recorder, RecorderConfig, pose_setter_for -from dimos.msgs.geometry_msgs.Pose import Pose -from dimos.msgs.nav_msgs.Odometry import Odometry -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 +from dimos.memory.module import Recorder, RecorderConfig, pose_setter_for +from dimos.memory.recording_policy import OnExisting class FastLio2RecorderConfig(RecorderConfig): diff --git a/dimos/hardware/sensors/lidar/fastlio2/tools/pcap_to_db.py b/dimos/hardware/sensors/lidar/fastlio2/tools/pcap_to_db.py index 0c42313508b..c2bddba6b96 100644 --- a/dimos/hardware/sensors/lidar/fastlio2/tools/pcap_to_db.py +++ b/dimos/hardware/sensors/lidar/fastlio2/tools/pcap_to_db.py @@ -148,12 +148,13 @@ def _write_rrd(db_path: Path, odom_stream: str, lidar_stream: str, voxel: float) FastLio2 publishes its cloud in the sensor/body frame, so each frame is transformed to world by its pose here, then voxel-deduped. Best-effort: any failure is non-fatal to the recording. Returns the .rrd path, or None.""" + from dimos_generated.nav_msgs.msg import Odometry + from dimos_generated.sensor_msgs.msg import PointCloud2 import numpy as np import rerun as rr from dimos.memory.store.sqlite import SqliteStore - from dimos.msgs.nav_msgs.Odometry import Odometry - from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 + from dimos.msgs.pointcloud import pointcloud_xyz from dimos.visualization.rerun.init import rerun_init store = SqliteStore(path=str(db_path), must_exist=True) @@ -188,7 +189,7 @@ def _write_rrd(db_path: Path, odom_stream: str, lidar_stream: str, voxel: float) j = int(np.argmin(np.abs(ots - lid.ts))) if abs(ots[j] - lid.ts) > _POSE_MATCH_TOL: continue - pts = np.asarray(lid.data.as_numpy()[0])[:, :3].astype(np.float64) + pts = pointcloud_xyz(lid.data).astype(np.float64) if pts.shape[0] == 0: continue world = pts @ _quat_to_rot(*oquat[j]).T + opos[j] diff --git a/dimos/hardware/sensors/lidar/livox/module.py b/dimos/hardware/sensors/lidar/livox/module.py index 1cd44231f5f..85b7cd6003d 100644 --- a/dimos/hardware/sensors/lidar/livox/module.py +++ b/dimos/hardware/sensors/lidar/livox/module.py @@ -31,6 +31,7 @@ import sys from typing import TYPE_CHECKING, Any, Literal +from dimos_generated.sensor_msgs.msg import Imu, PointCloud2 from pydantic import Field, field_validator from dimos.constants import DIMOS_PROJECT_ROOT @@ -49,8 +50,6 @@ SDK_POINT_DATA_PORT, SDK_PUSH_MSG_PORT, ) -from dimos.msgs.sensor_msgs.Imu import Imu -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 from dimos.spec import perception diff --git a/dimos/hardware/sensors/lidar/livox/rust/Cargo.toml b/dimos/hardware/sensors/lidar/livox/rust/Cargo.toml index 5a373b33d93..430ffcc400a 100644 --- a/dimos/hardware/sensors/lidar/livox/rust/Cargo.toml +++ b/dimos/hardware/sensors/lidar/livox/rust/Cargo.toml @@ -17,14 +17,14 @@ path = "dimos_livox::module::Mid360" python = "dimos.hardware.sensors.lidar.livox.module:Mid360" [package.metadata.dimos.module.mid360.outputs] -lidar = "sensor_msgs.PointCloud2" -imu = "sensor_msgs.Imu" +lidar = "sensor_msgs/msg/PointCloud2" +imu = "sensor_msgs/msg/Imu" [dependencies] crc = "3" dimos-module = { path = "../../../../../../native/rust/dimos-module" } etherparse = "0.21.0" -lcm-msgs = { git = "https://github.com/dimensionalOS/dimos-lcm.git", branch = "rust-codegen" } +dimos-generated-messages = { path = "../../../../../message_codegen" } pcap-parser = "0.17.0" serde = { version = "1", features = ["derive"] } serde_json = "1" diff --git a/dimos/hardware/sensors/lidar/livox/rust/src/module.rs b/dimos/hardware/sensors/lidar/livox/rust/src/module.rs index 053ca087d0b..7d592db444c 100644 --- a/dimos/hardware/sensors/lidar/livox/rust/src/module.rs +++ b/dimos/hardware/sensors/lidar/livox/rust/src/module.rs @@ -18,10 +18,12 @@ use crate::live::{LiveConfig, LiveSource, Ports}; use crate::pcap::PcapSource; use crate::pipeline::{imu_records, Frame, ImuRecord, PacketSource}; use crate::wire::{DataPacket, DataType}; +use dimos_generated_messages::builtin_interfaces::msg::Time; +use dimos_generated_messages::geometry_msgs::msg::{Quaternion, Vector3}; +use dimos_generated_messages::sensor_msgs::msg::{Imu, PointCloud2, PointField}; +use dimos_generated_messages::std_msgs::msg::Header; +use dimos_module::cdr; use dimos_module::{native_config, Module, Output}; -use lcm_msgs::geometry_msgs::{Quaternion, Vector3}; -use lcm_msgs::sensor_msgs::{Imu, PointCloud2, PointField}; -use lcm_msgs::std_msgs::{Header, Time}; use serde::{Deserialize, Serialize}; use std::net::Ipv4Addr; use std::sync::atomic::{AtomicBool, Ordering}; @@ -95,10 +97,10 @@ enum PointFormat { } impl PointFormat { - fn point_step(self) -> i32 { + fn point_step(self) -> u32 { match self { - PointFormat::Full => (OFFSET_FULL_LINE + 1) as i32, - PointFormat::Minimal | PointFormat::Legacy => (OFFSET_FOURTH + 4) as i32, + PointFormat::Full => (OFFSET_FULL_LINE + 1) as u32, + PointFormat::Minimal | PointFormat::Legacy => (OFFSET_FOURTH + 4) as u32, } } } @@ -121,10 +123,10 @@ impl Config { #[derive(Module)] #[module(name = "mid360", setup = start, handle = wait, teardown = stop)] pub struct Mid360 { - #[output(encode = PointCloud2::encode)] + #[output(encode = cdr::encode)] lidar: Output, - #[output(encode = Imu::encode)] + #[output(encode = cdr::encode)] imu: Output, #[config] @@ -273,10 +275,9 @@ fn run_pipeline( fn header(frame_id: &str, ts_ns: u64) -> Header { Header { - seq: 0, stamp: Time { sec: (ts_ns / 1_000_000_000) as i32, - nsec: (ts_ns % 1_000_000_000) as i32, + nanosec: (ts_ns % 1_000_000_000) as u32, }, frame_id: frame_id.to_string(), } @@ -320,13 +321,13 @@ const OFFSET_FULL_LINE: usize = 21; fn cloud_fields(format: PointFormat) -> Vec { let make_field = |name: &str, offset: usize, datatype: u8| PointField { name: name.into(), - offset: offset as i32, + offset: offset as u32, datatype, count: 1, }; - let f32t = PointField::FLOAT32 as u8; - let u32t = PointField::UINT32 as u8; - let u8t = PointField::UINT8 as u8; + let f32t = PointField::FLOAT32; + let u32t = PointField::UINT32; + let u8t = PointField::UINT8; let mut fields = vec![ make_field("x", OFFSET_X, f32t), make_field("y", OFFSET_Y, f32t), @@ -370,7 +371,7 @@ fn cloud_message(format: PointFormat, frame_id: &str, frame: &Frame) -> PointClo } } - let point_count = frame.points.len() as i32; + let point_count = frame.points.len() as u32; PointCloud2 { header: header(frame_id, frame.start_ns), height: 1, @@ -416,7 +417,7 @@ mod tests { assert_eq!(cloud.width, 2); assert_eq!(cloud.header.frame_id, "lidar_link"); assert_eq!(cloud.header.stamp.sec, 1_700_000_000); - assert_eq!(cloud.header.stamp.nsec, 123_000_000); + assert_eq!(cloud.header.stamp.nanosec, 123_000_000); let names: Vec<&str> = cloud.fields.iter().map(|f| f.name.as_str()).collect(); assert_eq!( names, @@ -467,7 +468,7 @@ mod tests { }; let msg = imu_message("imu_link", &record); assert_eq!(msg.header.stamp.sec, 5); - assert_eq!(msg.header.stamp.nsec, 500_000_000); + assert_eq!(msg.header.stamp.nanosec, 500_000_000); assert_eq!(msg.orientation_covariance[0], -1.0); assert_eq!(msg.orientation.w, 1.0); assert_eq!(msg.angular_velocity.z, 0.3); diff --git a/dimos/hardware/sensors/lidar/livox/test_e2e.py b/dimos/hardware/sensors/lidar/livox/test_e2e.py index 13a7596a37f..f8587222fdc 100644 --- a/dimos/hardware/sensors/lidar/livox/test_e2e.py +++ b/dimos/hardware/sensors/lidar/livox/test_e2e.py @@ -30,6 +30,7 @@ import subprocess import time +from dimos_generated.sensor_msgs.msg import Imu, PointCloud2 import lcm as lcm_module import pytest @@ -42,8 +43,7 @@ SDK_POINT_DATA_PORT, ) from dimos.hardware.sensors.lidar.virtual_mid360.module import VirtualMid360Config -from dimos.msgs.sensor_msgs.Imu import Imu -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 +from dimos.msgs.pointcloud import pointcloud_view, pointcloud_xyz from dimos.utils.data import get_data _RELEASE = DIMOS_PROJECT_ROOT / "target" / "release" @@ -172,13 +172,13 @@ def on_msg(_ch: str, data: bytes, topic: str = topic) -> None: raw[topic].append(data) msg_type = topics[topic] - lc.subscribe(f"{topic}#sensor_msgs.{msg_type.__name__}", on_msg) + lc.subscribe(f"{topic}#{msg_type.msg_name}", on_msg) end = time.monotonic() + seconds while time.monotonic() < end: if all(len(raw[topic]) >= count for topic, count in enough.items()): break lc.handle_timeout(200) - return {topic: [topics[topic].lcm_decode(data) for data in raw[topic]] for topic in topics} + return {topic: [topics[topic].decode(data) for data in raw[topic]] for topic in topics} def _magnitude(sample: Imu) -> float: @@ -187,12 +187,11 @@ def _magnitude(sample: Imu) -> float: def _assert_cloud_shape(cloud: PointCloud2, min_points: int) -> None: - assert cloud.frame_id == "lidar_link" - points, _ = cloud.as_numpy() + assert cloud.header.frame_id == "lidar_link" + points = pointcloud_xyz(cloud) assert len(points) > min_points # Full point format carries per-point deskew offsets within one frame. - offsets = cloud.offset_times_u32() - assert offsets is not None + offsets = pointcloud_view(cloud)["offset_time"] assert offsets.max() < 150_000_000, "offsets exceed one frame interval" @@ -209,8 +208,8 @@ def test_real_capture_replay_publishes_streams(spawn: Spawn) -> None: ) blob = { "topics": { - "lidar": "/e2e_real_lidar#sensor_msgs.PointCloud2", - "imu": "/e2e_real_imu#sensor_msgs.Imu", + "lidar": "/e2e_real_lidar#sensor_msgs/msg/PointCloud2", + "imu": "/e2e_real_imu#sensor_msgs/msg/Imu", }, "config": config.to_config_dict(), "session": {"id": "pointlio-e2e", "links": []}, @@ -234,7 +233,7 @@ def test_real_capture_replay_publishes_streams(spawn: Spawn) -> None: median = magnitudes[len(magnitudes) // 2] assert 5.0 < median < 15.0, f"median accel magnitude {median}" assert imus[0].orientation_covariance[0] == -1.0 - assert imus[0].frame_id == "imu_link" + assert imus[0].header.frame_id == "imu_link" @pytest.mark.native_e2e @@ -263,8 +262,8 @@ def test_live_loopback_handshake_and_stream(spawn: Spawn, synth_pcap: Path) -> N ) driver_blob = { "topics": { - "lidar": "/e2e_live_lidar#sensor_msgs.PointCloud2", - "imu": "/e2e_live_imu#sensor_msgs.Imu", + "lidar": "/e2e_live_lidar#sensor_msgs/msg/PointCloud2", + "imu": "/e2e_live_imu#sensor_msgs/msg/Imu", }, "config": config.to_config_dict(), "session": {"id": "pointlio-e2e", "links": []}, @@ -289,7 +288,7 @@ def test_live_loopback_handshake_and_stream(spawn: Spawn, synth_pcap: Path) -> N # The synthetic capture is a 5 m ring at z ~1 m, so a decode regression # in scaling or field layout shows up as broken geometry here. - points, _ = clouds[len(clouds) // 2].as_numpy() + points = pointcloud_xyz(clouds[len(clouds) // 2]) radii = sorted(math.hypot(x, y) for x, y, _z in points) assert abs(radii[len(radii) // 2] - 5.0) < 0.1, f"median ring radius {radii[len(radii) // 2]}" z_values = sorted(z for _x, _y, z in points) @@ -299,4 +298,4 @@ def test_live_loopback_handshake_and_stream(spawn: Spawn, synth_pcap: Path) -> N sample = imus[len(imus) // 2] assert abs(_magnitude(sample) - GRAVITY_MS2) < 1e-3 assert sample.orientation_covariance[0] == -1.0 - assert sample.frame_id == "imu_link" + assert sample.header.frame_id == "imu_link" diff --git a/dimos/hardware/sensors/lidar/pointlio/cpp/CMakeLists.txt b/dimos/hardware/sensors/lidar/pointlio/cpp/CMakeLists.txt index 4814d411187..4e24a76ec0e 100644 --- a/dimos/hardware/sensors/lidar/pointlio/cpp/CMakeLists.txt +++ b/dimos/hardware/sensors/lidar/pointlio/cpp/CMakeLists.txt @@ -38,12 +38,7 @@ if(NOT FASTLIO_DIR) set(FASTLIO_DIR ${CMAKE_CURRENT_SOURCE_DIR}/fast-lio-non-ros) endif() -FetchContent_Declare(dimos_lcm - GIT_REPOSITORY https://github.com/dimensionalOS/dimos-lcm.git - GIT_TAG main - GIT_SHALLOW TRUE -) -FetchContent_MakeAvailable(dimos_lcm) + # The header-only dimos native C++ SDK. Overridable so nix can point at the # flake input instead of the in-repo relative path. @@ -86,7 +81,6 @@ target_include_directories(pointlio_native PRIVATE ${FASTLIO_DIR}/include ${FASTLIO_DIR}/include/IKFoM/IKFoM_toolkit ${FASTLIO_DIR}/src - ${dimos_lcm_SOURCE_DIR}/generated/cpp_lcm_msgs ${LCM_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIR} ${PCL_INCLUDE_DIRS} diff --git a/dimos/hardware/sensors/lidar/pointlio/cpp/flake.lock b/dimos/hardware/sensors/lidar/pointlio/cpp/flake.lock index 73bd9ebd6a5..fd804a38357 100644 --- a/dimos/hardware/sensors/lidar/pointlio/cpp/flake.lock +++ b/dimos/hardware/sensors/lidar/pointlio/cpp/flake.lock @@ -1,22 +1,5 @@ { "nodes": { - "dimos-lcm": { - "flake": false, - "locked": { - "lastModified": 1769774949, - "narHash": "sha256-icRK7jerqNlwK1WZBrnIP04I2WozzFqTD7qsmnPxQuo=", - "owner": "dimensionalOS", - "repo": "dimos-lcm", - "rev": "0aa72b7b1bd3a65f50f5c03485ee9b728df56afe", - "type": "github" - }, - "original": { - "owner": "dimensionalOS", - "ref": "main", - "repo": "dimos-lcm", - "type": "github" - } - }, "fast-lio": { "flake": false, "locked": { @@ -163,7 +146,6 @@ }, "root": { "inputs": { - "dimos-lcm": "dimos-lcm", "fast-lio": "fast-lio", "flake-utils": "flake-utils", "lcm-extended": "lcm-extended", diff --git a/dimos/hardware/sensors/lidar/pointlio/cpp/flake.nix b/dimos/hardware/sensors/lidar/pointlio/cpp/flake.nix index e1077483151..727395bbd57 100644 --- a/dimos/hardware/sensors/lidar/pointlio/cpp/flake.nix +++ b/dimos/hardware/sensors/lidar/pointlio/cpp/flake.nix @@ -8,10 +8,6 @@ livox-sdk.url = "path:../../livox/cpp"; livox-sdk.inputs.nixpkgs.follows = "nixpkgs"; livox-sdk.inputs.flake-utils.follows = "flake-utils"; - dimos-lcm = { - url = "github:dimensionalOS/dimos-lcm/main"; - flake = false; - }; # Standalone Boost.PFR, consumed by the SDK via a FetchContent source override. pfr = { url = "github:apolukhin/pfr_non_boost/2.3.2"; @@ -29,7 +25,7 @@ }; }; - outputs = { self, nixpkgs, zenoh, flake-utils, livox-sdk, dimos-lcm, pfr, fast-lio, lcm-extended, ... }: + outputs = { self, nixpkgs, zenoh, flake-utils, livox-sdk, pfr, fast-lio, lcm-extended, ... }: flake-utils.lib.eachDefaultSystem (system: let # Overlay fixes for darwin-broken nixpkgs recipes in our transitive @@ -92,6 +88,7 @@ nativeBuildInputs = [ pkgs.cmake pkgs.pkg-config ]; buildInputs = [ + (import ../../../../../../native/cpp/messages.nix { inherit pkgs; }) livox-sdk2 lcm pkgs.glib @@ -107,7 +104,6 @@ cmakeFlags = [ "-DCMAKE_POLICY_VERSION_MINIMUM=3.5" - "-DFETCHCONTENT_SOURCE_DIR_DIMOS_LCM=${dimos-lcm}" "-DFETCHCONTENT_SOURCE_DIR_PFR=${pfr}" "-DFASTLIO_DIR=${fast-lio-patched}" "-DLIVOX_COMMON_DIR=${livox-common}" diff --git a/dimos/hardware/sensors/lidar/pointlio/cpp/main.cpp b/dimos/hardware/sensors/lidar/pointlio/cpp/main.cpp index a4361b92fba..d9793a538c8 100644 --- a/dimos/hardware/sensors/lidar/pointlio/cpp/main.cpp +++ b/dimos/hardware/sensors/lidar/pointlio/cpp/main.cpp @@ -21,10 +21,8 @@ #include "point_cloud_utils.hpp" #include "dimos/native.hpp" +#include -#include "nav_msgs/Odometry.hpp" -#include "sensor_msgs/PointCloud2.hpp" -#include "std_msgs/Header.hpp" // Point-LIO (header-only core, compiled sources linked via CMake) #include "pointlio.hpp" @@ -147,8 +145,8 @@ class PointLioModule : public Module { public: void build(Builder& builder, Config& config) override { cfg_ = config.parse(); - lidar_ = builder.output("lidar"); - odometry_ = builder.output("odometry"); + lidar_ = builder.output("lidar"); + odometry_ = builder.output("odometry"); frame_interval_ = std::chrono::microseconds(static_cast(1e6 / cfg_.frequency)); @@ -476,7 +474,7 @@ class PointLioModule : public Module { void publish_pointcloud(const PointCloudXYZI::Ptr& cloud, double ts) { int num_points = static_cast(cloud->size()); - sensor_msgs::PointCloud2 pc = make_xyzi_cloud(cfg_.sensor_frame_id, ts, num_points); + sensor_msgs::msg::PointCloud2 pc = make_xyzi_cloud(cfg_.sensor_frame_id, ts, num_points); for (int i = 0; i < num_points; ++i) { float* dst = xyzi_point(pc, i); @@ -491,7 +489,7 @@ class PointLioModule : public Module { // Publish odometry as frame_id (fixed) -> sensor_frame_id (moving sensor). void publish_odometry(const custom_messages::Odometry& odom, double ts) { - nav_msgs::Odometry msg; + nav_msgs::msg::Odometry msg; msg.header = make_header(cfg_.frame_id, ts); msg.child_frame_id = cfg_.sensor_frame_id; @@ -514,14 +512,14 @@ class PointLioModule : public Module { msg.twist.twist.angular.x = odom.twist.twist.angular.x; msg.twist.twist.angular.y = odom.twist.twist.angular.y; msg.twist.twist.angular.z = odom.twist.twist.angular.z; - std::memset(msg.twist.covariance, 0, sizeof(msg.twist.covariance)); + msg.twist.covariance.fill(0); odometry_.publish(msg); } PointLioConfig cfg_; - Output lidar_; - Output odometry_; + Output lidar_; + Output odometry_; std::unique_ptr point_lio_; // All four come from config, set in build(). diff --git a/dimos/hardware/sensors/lidar/pointlio/module.py b/dimos/hardware/sensors/lidar/pointlio/module.py index 0db4fc6b58c..8aea59b8fac 100644 --- a/dimos/hardware/sensors/lidar/pointlio/module.py +++ b/dimos/hardware/sensors/lidar/pointlio/module.py @@ -35,6 +35,9 @@ import os from typing import TYPE_CHECKING, Literal +from dimos_generated.nav_msgs.msg import Odometry +from dimos_generated.sensor_msgs.msg import Imu, PointCloud2 +from dimos_generated.tf2_msgs.msg import TFMessage from pydantic import BaseModel, Field from reactivex.disposable import Disposable @@ -55,13 +58,7 @@ SDK_POINT_DATA_PORT, SDK_PUSH_MSG_PORT, ) -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Transform import Transform -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.msgs.nav_msgs.Odometry import Odometry -from dimos.msgs.sensor_msgs.Imu import Imu -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 -from dimos.msgs.tf2_msgs.TFMessage import TFMessage +from dimos.msgs.geometry import transform_from_odometry from dimos.spec import perception # Human-readable enums; the C++ binary (main.cpp) maps these strings to @@ -183,19 +180,7 @@ def start(self) -> None: ) def _on_odom_for_tf(self, msg: Odometry) -> None: - self.tf.publish( - TFMessage( - Transform( - frame_id=self.frame_id, - child_frame_id=self.config.sensor_frame_id, - translation=Vector3(msg.pose.position), - rotation=Quaternion(msg.pose.orientation), - # Match the odometry ts exactly; no `or time.time()` fallback (a - # real ts of 0.0 must not become wall time). - ts=msg.ts, - ) - ) - ) + self.tf.publish(TFMessage(transforms=[transform_from_odometry(msg)])) @rpc def stop(self) -> None: diff --git a/dimos/hardware/sensors/lidar/pointlio/recorder.py b/dimos/hardware/sensors/lidar/pointlio/recorder.py index 5ef22f8bc17..57ff0a32135 100644 --- a/dimos/hardware/sensors/lidar/pointlio/recorder.py +++ b/dimos/hardware/sensors/lidar/pointlio/recorder.py @@ -24,11 +24,13 @@ from __future__ import annotations +from dimos_generated.geometry_msgs.msg import Pose +from dimos_generated.nav_msgs.msg import Odometry +from dimos_generated.sensor_msgs.msg import PointCloud2 + from dimos.core.stream import In -from dimos.memory.module import OnExisting, Recorder, RecorderConfig, pose_setter_for -from dimos.msgs.geometry_msgs.Pose import Pose -from dimos.msgs.nav_msgs.Odometry import Odometry -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 +from dimos.memory.module import Recorder, RecorderConfig, pose_setter_for +from dimos.memory.recording_policy import OnExisting class PointlioRecorderConfig(RecorderConfig): diff --git a/dimos/hardware/sensors/lidar/pointlio/rust/Cargo.toml b/dimos/hardware/sensors/lidar/pointlio/rust/Cargo.toml index 6a1af9d487f..0ba6773dc26 100644 --- a/dimos/hardware/sensors/lidar/pointlio/rust/Cargo.toml +++ b/dimos/hardware/sensors/lidar/pointlio/rust/Cargo.toml @@ -21,19 +21,19 @@ path = "dimos_pointlio::module::PointLioModule" python = "dimos.hardware.sensors.lidar.pointlio.module:PointLioRust" [package.metadata.dimos.module.pointlio.inputs] -lidar_raw = "sensor_msgs.PointCloud2" -imu = "sensor_msgs.Imu" -tf = "tf2_msgs.TFMessage" +lidar_raw = "sensor_msgs/msg/PointCloud2" +imu = "sensor_msgs/msg/Imu" +tf = "tf2_msgs/msg/TFMessage" [package.metadata.dimos.module.pointlio.outputs] -lidar = "sensor_msgs.PointCloud2" -odometry = "nav_msgs.Odometry" +lidar = "sensor_msgs/msg/PointCloud2" +odometry = "nav_msgs/msg/Odometry" [dependencies] dimos-livox = { path = "../../livox/rust" } pointlio-core = { git = "https://github.com/dimensionalOS/dimos-module-pointlio.git", rev = "3c1dcf8b9dfe6402aaf141e116f4caef6c5c8088" } dimos-module = { path = "../../../../../../native/rust/dimos-module" } -lcm-msgs = { git = "https://github.com/dimensionalOS/dimos-lcm.git", branch = "rust-codegen" } +dimos-generated-messages = { path = "../../../../../message_codegen" } serde = { version = "1", features = ["derive"] } serde_json = "1" tokio = { version = "1", features = ["rt-multi-thread", "macros"] } diff --git a/dimos/hardware/sensors/lidar/pointlio/rust/src/module.rs b/dimos/hardware/sensors/lidar/pointlio/rust/src/module.rs index 675051f4b21..1293e5e3181 100644 --- a/dimos/hardware/sensors/lidar/pointlio/rust/src/module.rs +++ b/dimos/hardware/sensors/lidar/pointlio/rust/src/module.rs @@ -19,11 +19,15 @@ use dimos_module::{native_config, Input, Module, Output, Tf, Transform}; /// The driver publishes linear acceleration in m/s^2; Point-LIO wants it in g. const GRAVITY_MS2: f64 = 9.80665; -use lcm_msgs::geometry_msgs::{Point, Pose, PoseWithCovariance, Twist, TwistWithCovariance}; -use lcm_msgs::geometry_msgs::{Quaternion as QuatMsg, Vector3}; -use lcm_msgs::nav_msgs::Odometry; -use lcm_msgs::sensor_msgs::{Imu, PointCloud2, PointField}; -use lcm_msgs::std_msgs::{Header, Time}; +use dimos_generated_messages::builtin_interfaces::msg::Time; +use dimos_generated_messages::geometry_msgs::msg::{ + Point, Pose, PoseWithCovariance, Twist, TwistWithCovariance, +}; +use dimos_generated_messages::geometry_msgs::msg::{Quaternion as QuatMsg, Vector3}; +use dimos_generated_messages::nav_msgs::msg::Odometry; +use dimos_generated_messages::sensor_msgs::msg::{Imu, PointCloud2, PointField}; +use dimos_generated_messages::std_msgs::msg::Header; +use dimos_module::cdr; use pointlio_core::{LivoxPoint, PointLio, PointXYZI}; use serde::{Deserialize, Serialize}; use std::time::Duration; @@ -117,16 +121,16 @@ fn core_config_parses(cfg: &Config) -> Result<(), ValidationError> { #[derive(Module)] #[module(name = "pointlio", setup = start)] pub struct PointLioModule { - #[input(decode = PointCloud2::decode)] + #[input(decode = cdr::decode)] lidar_raw: Input, - #[input(decode = Imu::decode)] + #[input(decode = cdr::decode)] imu: Input, - #[output(encode = PointCloud2::encode)] + #[output(encode = cdr::encode)] lidar: Output, - #[output(encode = Odometry::encode)] + #[output(encode = cdr::encode)] odometry: Output, #[tf] @@ -239,20 +243,19 @@ fn due(last: &mut Option, ts: f64, hz: f64) -> bool { } fn stamp_secs(t: &Time) -> f64 { - f64::from(t.sec) + f64::from(t.nsec) * 1e-9 + f64::from(t.sec) + f64::from(t.nanosec) * 1e-9 } fn stamp_ns(t: &Time) -> u64 { - t.sec as u64 * 1_000_000_000 + t.nsec as u64 + t.sec as u64 * 1_000_000_000 + t.nanosec as u64 } fn header(frame_id: &str, ts: f64) -> Header { let sec = ts.floor(); Header { - seq: 0, stamp: Time { sec: sec as i32, - nsec: (((ts - sec) * 1e9).round() as i32).min(999_999_999), + nanosec: (((ts - sec) * 1e9).round() as u32).min(999_999_999), }, frame_id: frame_id.to_string(), } @@ -270,11 +273,15 @@ struct Layout { fn layout(cloud: &PointCloud2) -> Result { let step = cloud.point_step as usize; - let find = |name: &str, datatype: i8| -> Result, String> { - let Some(f) = cloud.fields.iter().find(|f| f.name == name) else { + let find = |name: &str, datatype: u8| -> Result, String> { + let mut matches = cloud.fields.iter().filter(|field| field.name == name); + let Some(f) = matches.next() else { return Ok(None); }; - if f.datatype != datatype as u8 { + if matches.next().is_some() || f.count != 1 { + return Err(format!("field {name} must be unique and scalar")); + } + if f.datatype != datatype { return Err(format!( "field {name} has datatype {}, want {datatype}", f.datatype @@ -291,12 +298,9 @@ fn layout(cloud: &PointCloud2) -> Result { } Ok(Some(offset)) }; - let need = |name: &str, datatype: i8| { + let need = |name: &str, datatype: u8| { find(name, datatype)?.ok_or_else(|| format!("cloud has no {name} field")) }; - if cloud.is_bigendian { - return Err("big-endian clouds are not supported".into()); - } Ok(Layout { x: need("x", PointField::FLOAT32)?, y: need("y", PointField::FLOAT32)?, @@ -311,19 +315,40 @@ fn layout(cloud: &PointCloud2) -> Result { fn livox_points(cloud: &PointCloud2) -> Result, String> { let l = layout(cloud)?; let step = cloud.point_step as usize; - let n = cloud.width as usize * cloud.height as usize; - if step == 0 || cloud.data.len() < n * step { - return Err(format!( - "data has {} bytes for {n} points of {step}", - cloud.data.len() - )); + let width = cloud.width as usize; + let row_step = cloud.row_step as usize; + let row_bytes = width + .checked_mul(step) + .ok_or("point cloud row size overflow")?; + let total = row_step + .checked_mul(cloud.height as usize) + .ok_or("point cloud data size overflow")?; + if step == 0 || row_step < row_bytes || cloud.data.len() != total { + return Err("point cloud dimensions/strides do not match data".into()); } - let f32_at = |p: &[u8], off: usize| f32::from_le_bytes(p[off..off + 4].try_into().unwrap()); - let u32_at = |p: &[u8], off: usize| u32::from_le_bytes(p[off..off + 4].try_into().unwrap()); + if width == 0 || cloud.height == 0 { + return Ok(Vec::new()); + } + let f32_at = |p: &[u8], off: usize| { + let bytes = p[off..off + 4].try_into().unwrap(); + if cloud.is_bigendian { + f32::from_be_bytes(bytes) + } else { + f32::from_le_bytes(bytes) + } + }; + let u32_at = |p: &[u8], off: usize| { + let bytes = p[off..off + 4].try_into().unwrap(); + if cloud.is_bigendian { + u32::from_be_bytes(bytes) + } else { + u32::from_le_bytes(bytes) + } + }; Ok(cloud .data - .chunks_exact(step) - .take(n) + .chunks_exact(row_step) + .flat_map(|row| row[..row_bytes].chunks_exact(step)) .map(|p| LivoxPoint { x: f32_at(p, l.x), y: f32_at(p, l.y), @@ -403,20 +428,20 @@ fn cloud_message(frame_id: &str, ts: f64, cloud: &[PointXYZI]) -> PointCloud2 { .enumerate() .map(|(i, name)| PointField { name: (*name).into(), - offset: (i * 4) as i32, - datatype: PointField::FLOAT32 as u8, + offset: (i * 4) as u32, + datatype: PointField::FLOAT32, count: 1, }) .collect(); - let n = cloud.len() as i32; + let n = cloud.len() as u32; PointCloud2 { header: header(frame_id, ts), height: 1, width: n, fields, is_bigendian: false, - point_step: STEP as i32, - row_step: STEP as i32 * n, + point_step: STEP as u32, + row_step: STEP as u32 * n, data, is_dense: true, } @@ -426,11 +451,11 @@ fn cloud_message(frame_id: &str, ts: f64, cloud: &[PointXYZI]) -> PointCloud2 { mod tests { use super::*; - fn field(name: &str, offset: usize, datatype: i8) -> PointField { + fn field(name: &str, offset: usize, datatype: u8) -> PointField { PointField { name: name.into(), - offset: offset as i32, - datatype: datatype as u8, + offset: offset as u32, + datatype, count: 1, } } @@ -439,16 +464,60 @@ mod tests { PointCloud2 { header: header("lidar_link", 1.5), height: 1, - width: (data.len() / step) as i32, + width: (data.len() / step) as u32, fields, is_bigendian: false, - point_step: step as i32, - row_step: data.len() as i32, + point_step: step as u32, + row_step: data.len() as u32, data, is_dense: true, } } + #[test] + fn padded_rows_and_big_endian_preserve_deskew_offsets() { + for big in [false, true] { + let mut data = vec![0; 48]; + for row in 0..2 { + for (axis, value) in [1f32 + row as f32, 2., 3.].into_iter().enumerate() { + let bytes = if big { + value.to_be_bytes() + } else { + value.to_le_bytes() + }; + data[row * 24 + axis * 4..row * 24 + axis * 4 + 4].copy_from_slice(&bytes); + } + let offset = 42u32 + row as u32; + let bytes = if big { + offset.to_be_bytes() + } else { + offset.to_le_bytes() + }; + data[row * 24 + 12..row * 24 + 16].copy_from_slice(&bytes); + } + let mut message = cloud( + vec![ + field("x", 0, PointField::FLOAT32), + field("y", 4, PointField::FLOAT32), + field("z", 8, PointField::FLOAT32), + field("offset_time", 12, PointField::UINT32), + ], + 16, + data, + ); + message.width = 1; + message.height = 2; + message.row_step = 24; + message.is_bigendian = big; + let points = livox_points(&message).unwrap(); + assert_eq!(points.len(), 2); + assert_eq!((points[0].x, points[1].x), (1., 2.)); + assert_eq!((points[0].offset_time, points[1].offset_time), (42, 43)); + message.row_step = 15; + assert!(livox_points(&message).is_err()); + } + } + #[test] fn full_layout_is_read_by_name() { // Driver `full`: x y z intensity offset_time tag line, 22 B. diff --git a/dimos/hardware/sensors/lidar/pointlio/scripts/pcap_to_db.py b/dimos/hardware/sensors/lidar/pointlio/scripts/pcap_to_db.py index 3ca9552686f..5b683fa9450 100644 --- a/dimos/hardware/sensors/lidar/pointlio/scripts/pcap_to_db.py +++ b/dimos/hardware/sensors/lidar/pointlio/scripts/pcap_to_db.py @@ -228,12 +228,13 @@ def _write_rrd(db_path: Path, odom_stream: str, lidar_stream: str, voxel: float) Point-LIO publishes its cloud in the sensor/body frame, so each frame is transformed to world by its pose here, then voxel-deduped. Best-effort: any failure is non-fatal to the recording. Returns the .rrd path, or None.""" + from dimos_generated.nav_msgs.msg import Odometry + from dimos_generated.sensor_msgs.msg import PointCloud2 import numpy as np import rerun as rr from dimos.memory.store.sqlite import SqliteStore - from dimos.msgs.nav_msgs.Odometry import Odometry - from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 + from dimos.msgs.pointcloud import pointcloud_xyz from dimos.visualization.rerun.init import rerun_init store = SqliteStore(path=str(db_path), must_exist=True) @@ -268,7 +269,7 @@ def _write_rrd(db_path: Path, odom_stream: str, lidar_stream: str, voxel: float) j = int(np.argmin(np.abs(ots - lid.ts))) if abs(ots[j] - lid.ts) > _POSE_MATCH_TOL: continue - pts = np.asarray(lid.data.as_numpy()[0])[:, :3].astype(np.float64) + pts = pointcloud_xyz(lid.data).astype(np.float64) if pts.shape[0] == 0: continue world = pts @ _quat_to_rot(*oquat[j]).T + opos[j] diff --git a/dimos/hardware/sensors/lidar/test_generated_recorders.py b/dimos/hardware/sensors/lidar/test_generated_recorders.py new file mode 100644 index 00000000000..09137c3ff48 --- /dev/null +++ b/dimos/hardware/sensors/lidar/test_generated_recorders.py @@ -0,0 +1,93 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +# Copyright 2026 Dimensional Inc. +# SPDX-License-Identifier: Apache-2.0 + +from dimos_generated.geometry_msgs.msg import Point, Pose, PoseWithCovariance, Quaternion +from dimos_generated.nav_msgs.msg import Odometry +from dimos_generated.sensor_msgs.msg import CameraInfo, Image, PointCloud2 +from dimos_generated.std_msgs.msg import Header +import numpy as np +import pytest + +from dimos.hardware.sensors.camera.realsense.blueprints import RealSenseMountTf, _cloud +from dimos.hardware.sensors.lidar.fastlio2.recorder import FastLio2Recorder +from dimos.hardware.sensors.lidar.pointlio.recorder import PointlioRecorder +from dimos.mapping.relocalization.blueprints import RecordingPlayer, _fine_points +from dimos.msgs.geometry import transform_matrix +from dimos.msgs.pointcloud import pointcloud_from_xyz +from dimos.robot.assembly.mid360_realsense_30 import Mid360RealsenseRecorder +from dimos.robot.unitree.g1.g1_recorder import G1Recorder, G1RecorderConfig + + +@pytest.mark.asyncio +@pytest.mark.parametrize("recorder_type", [FastLio2Recorder, PointlioRecorder]) +async def test_generated_recorders_anchor_clouds_to_last_odometry(recorder_type, tmp_path): + recorder = recorder_type(db_path=tmp_path / "unused.db") + try: + cloud = pointcloud_from_xyz(np.array([[1, 2, 3]], dtype=np.float32), header=Header()) + assert await recorder._lidar_pose(cloud) is None + pose = Pose(position=Point(x=2, y=3), orientation=Quaternion(w=1)) + odometry = Odometry(header=Header(frame_id="world"), pose=PoseWithCovariance(pose=pose)) + assert await recorder._odom_pose(odometry) == pose + assert await recorder._lidar_pose(cloud) == pose + assert not (tmp_path / "unused.db").exists() + finally: + recorder.stop() + + +def test_recorder_and_replay_ports_use_generated_message_identities(): + for recorder_type in [FastLio2Recorder, PointlioRecorder]: + streams = { + stream.name: stream.type for stream in recorder_type.blueprint().blueprints[0].streams + } + prefix = "fastlio" if recorder_type is FastLio2Recorder else "pointlio" + assert streams[f"{prefix}_odometry"] is Odometry + assert streams[f"{prefix}_lidar"] is PointCloud2 + streams = { + stream.name: stream.type + for stream in Mid360RealsenseRecorder.blueprint().blueprints[0].streams + } + assert streams["color_image"] is Image + assert streams["realsense_depth_image"] is Image + assert streams["realsense_pointcloud"] is PointCloud2 + assert streams["realsense_camera_info"] is CameraInfo + streams = { + stream.name: stream.type for stream in RecordingPlayer.blueprint().blueprints[0].streams + } + assert streams["lidar"] is PointCloud2 + + +def test_realsense_mount_is_an_identity_and_generated_cloud_views_are_typed(): + mount = RealSenseMountTf() + try: + edge = mount.transforms()[0] + assert edge.header.frame_id == "world" and edge.child_frame_id == "camera_link" + np.testing.assert_allclose(transform_matrix(edge.transform), np.eye(4)) + finally: + mount.stop() + cloud = pointcloud_from_xyz(np.array([[1, 2, 3]], dtype=np.float32), header=Header()) + before = cloud.encode() + assert _cloud(cloud).positions is not None + assert _fine_points(cloud).positions is not None + assert cloud.encode() == before + + +def test_g1_depth_recorder_declares_generated_lossless_cdr(): + config = G1RecorderConfig() + assert config.stream_codecs["realsense_depth_image"] == "lz4+cdr" + streams = {s.name: s.type for s in G1Recorder.blueprint().blueprints[0].streams} + assert streams["realsense_depth_image"] is Image + assert streams["realsense_camera_info"] is CameraInfo diff --git a/dimos/hardware/whole_body/transport/adapter.py b/dimos/hardware/whole_body/transport/adapter.py index 2ad2b223b84..2b496609a1f 100644 --- a/dimos/hardware/whole_body/transport/adapter.py +++ b/dimos/hardware/whole_body/transport/adapter.py @@ -24,12 +24,13 @@ import threading from typing import Any +from dimos_generated.dimos_msgs.msg import MotorCommandArray +from dimos_generated.sensor_msgs.msg import Imu, JointState + from dimos.core.transport_factory import make_transport from dimos.hardware.spec import JointLimits from dimos.hardware.whole_body.spec import IMUState, MotorCommand, MotorState -from dimos.msgs.sensor_msgs.Imu import Imu -from dimos.msgs.sensor_msgs.JointState import JointState -from dimos.msgs.sensor_msgs.MotorCommandArray import MotorCommandArray +from dimos.msgs.time import header_now from dimos.utils.logging_config import setup_logger logger = setup_logger() @@ -135,6 +136,7 @@ def write_motor_commands(self, commands: list[MotorCommand]) -> bool: return False msg = MotorCommandArray( + header=header_now(), q=[c.q for c in commands], dq=[c.dq for c in commands], kp=[c.kp for c in commands], diff --git a/dimos/hardware/whole_body/transport/test_adapter.py b/dimos/hardware/whole_body/transport/test_adapter.py new file mode 100644 index 00000000000..b779185731b --- /dev/null +++ b/dimos/hardware/whole_body/transport/test_adapter.py @@ -0,0 +1,54 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from dimos_generated.geometry_msgs.msg import Quaternion, Vector3 +from dimos_generated.sensor_msgs.msg import Imu, JointState +import pytest + +from dimos.hardware.whole_body.spec import IMUState, MotorCommand, MotorState +from dimos.hardware.whole_body.transport.adapter import TransportWholeBodyAdapter + + +def test_unconnected_adapter_does_not_send_commands(): + adapter = TransportWholeBodyAdapter(dof=2) + assert not adapter.write_motor_commands([MotorCommand(), MotorCommand()]) + assert not adapter.has_motor_states() + assert adapter.read_motor_states() == [MotorState(), MotorState()] + + +@pytest.mark.parametrize("field", ["position", "velocity", "effort"]) +def test_short_feedback_preserves_last_complete_snapshot(field): + adapter = TransportWholeBodyAdapter(dof=2) + message = JointState(position=[1.0, 2.0], velocity=[3.0, 4.0], effort=[5.0, 6.0]) + adapter._on_motor_states(message) + setattr(message, field, [0.0]) + adapter._on_motor_states(message) + assert adapter.read_motor_states() == [ + MotorState(q=1.0, dq=3.0, tau=5.0), + MotorState(q=2.0, dq=4.0, tau=6.0), + ] + + +def test_generated_imu_converts_xyzw_to_hardware_wxyz(): + adapter = TransportWholeBodyAdapter(dof=2) + adapter._on_imu( + Imu( + orientation=Quaternion(x=0.1, y=0.2, z=0.3, w=0.4), + angular_velocity=Vector3(x=1.0, y=2.0, z=3.0), + linear_acceleration=Vector3(x=4.0, y=5.0, z=6.0), + ) + ) + assert adapter.read_imu() == IMUState( + quaternion=(0.4, 0.1, 0.2, 0.3), gyroscope=(1.0, 2.0, 3.0), accelerometer=(4.0, 5.0, 6.0) + ) diff --git a/dimos/imitation/collection/episode_monitor.py b/dimos/imitation/collection/episode_monitor.py index 03e171bbfc5..d46297efa04 100644 --- a/dimos/imitation/collection/episode_monitor.py +++ b/dimos/imitation/collection/episode_monitor.py @@ -27,6 +27,7 @@ import time from typing import Any, Literal, TypeAlias +from dimos_generated.std_msgs.msg import UInt32 from pydantic import BaseModel, Field, field_validator from reactivex.abc import DisposableBase from reactivex.disposable import Disposable @@ -94,7 +95,7 @@ def _validate_button_map(cls, value: dict[EpisodeCommand, str]) -> dict[EpisodeC class EpisodeMonitorModule(Module): config: EpisodeMonitorModuleConfig - teleop_buttons: In[Buttons] + teleop_buttons: In[UInt32] # TODO: no KeyPress producer exists yet — add a pygame keyboard module that # publishes KeyPress so this port is actually fed (today only buttons drive it). keyboard: In[KeyPress] @@ -161,8 +162,9 @@ def stop(self) -> None: # ── port handlers ──────────────────────────────────────────────────────── - def _on_buttons(self, msg: Buttons) -> None: + def _on_buttons(self, msg: UInt32) -> None: """Rising-edge detect against `config.button_map`; advance state machine.""" + buttons = Buttons(data=msg.data) ts = time.time() # Edge-detect under the lock (it shares `_prev_bits` with reset_counters), # then fire transitions outside it — `_transition` takes the same lock. @@ -173,7 +175,7 @@ def _on_buttons(self, msg: Buttons) -> None: for event_name, alias_or_attr in self.config.button_map.items(): attr = BUTTON_ALIASES.get(alias_or_attr, alias_or_attr) try: - pressed = bool(getattr(msg, attr)) + pressed = bool(getattr(buttons, attr)) except AttributeError: continue prev = self._prev_bits.get(attr, False) diff --git a/dimos/imitation/collection/recorder.py b/dimos/imitation/collection/recorder.py index 6379464db49..69a44838d0a 100644 --- a/dimos/imitation/collection/recorder.py +++ b/dimos/imitation/collection/recorder.py @@ -25,12 +25,12 @@ from __future__ import annotations +from dimos_generated.geometry_msgs.msg import Pose +from dimos_generated.sensor_msgs.msg import Image, JointState + from dimos.core.stream import In from dimos.imitation.collection.episode_monitor import EpisodeStatus from dimos.memory.module import Recorder, RecorderConfig -from dimos.msgs.geometry_msgs.Pose import Pose -from dimos.msgs.sensor_msgs.Image import Image -from dimos.msgs.sensor_msgs.JointState import JointState class CollectionRecorderConfig(RecorderConfig): @@ -46,6 +46,11 @@ class CollectionRecorder(Recorder): coordinator_joint_state: In[JointState] # observation + action (measured/next state) status: In[EpisodeStatus] # episode start/save/discard segmentation + def _resolve_ts(self, name: str, msg: object) -> float: + if isinstance(msg, EpisodeStatus): + return msg.ts + return super()._resolve_ts(name, msg) + async def _resolve_pose(self, name: str, msg: object, ts: float) -> Pose | None: if name in self.config.poseless_streams: return None diff --git a/dimos/imitation/collection/test_blueprint.py b/dimos/imitation/collection/test_blueprint.py index 43e352dd10b..e041ab7791b 100644 --- a/dimos/imitation/collection/test_blueprint.py +++ b/dimos/imitation/collection/test_blueprint.py @@ -14,6 +14,7 @@ from __future__ import annotations +from dimos_generated.sensor_msgs.msg import JointState import pytest from dimos.core.coordination.blueprints import Blueprint @@ -23,7 +24,6 @@ ) from dimos.imitation.collection.episode_monitor import EpisodeMonitorModule from dimos.imitation.collection.recorder import CollectionRecorder -from dimos.msgs.sensor_msgs.JointState import JointState from dimos.teleop.webxr.extensions import ArmTeleopModule AGGREGATE = "coordinator_joint_state" diff --git a/dimos/imitation/collection/test_episode_monitor.py b/dimos/imitation/collection/test_episode_monitor.py index fb0549c9d2a..ef8fbfc0fe8 100644 --- a/dimos/imitation/collection/test_episode_monitor.py +++ b/dimos/imitation/collection/test_episode_monitor.py @@ -80,8 +80,8 @@ def _press(monitor: EpisodeMonitorModule, alias: str) -> None: released = Buttons() pressed = Buttons() pressed.set_attribute(attr, True) - monitor._on_buttons(released) - monitor._on_buttons(pressed) + monitor._on_buttons(released.to_message()) + monitor._on_buttons(pressed.to_message()) def test_toggle_starts_then_saves(make_monitor: Callable[..., EpisodeMonitorModule]) -> None: @@ -130,8 +130,8 @@ def test_no_event_without_rising_edge( m = make_monitor() pressed = Buttons() pressed.right_secondary = True # B held - m._on_buttons(pressed) - m._on_buttons(pressed) # still held — no new edge + m._on_buttons(pressed.to_message()) + m._on_buttons(pressed.to_message()) # still held — no new edge assert [e.last_event for e in _events(m)] == ["start"] diff --git a/dimos/imitation/dataprep/core.py b/dimos/imitation/dataprep/core.py index 7db49cfcc1a..3107436a9a4 100644 --- a/dimos/imitation/dataprep/core.py +++ b/dimos/imitation/dataprep/core.py @@ -31,11 +31,13 @@ from pathlib import Path from typing import TYPE_CHECKING, Any, Literal +from dimos_generated.sensor_msgs.msg import CompressedImage, Image import numpy as np from numpy.typing import NDArray from pydantic import BaseModel, ConfigDict, Field from dimos.constants import STATE_DIR +from dimos.msgs.image import image_from_compressed, image_view from dimos.protocol.service.spec import BaseConfig if TYPE_CHECKING: @@ -144,7 +146,8 @@ def resolve_field(msg: Any, ref: StreamField) -> NDArray[Any]: Single source of truth for obs/action construction across train and live inference. Behavior: - `ref.field is None`: best-effort coerce the whole message - (Image → `.data`, ndarray pass-through, list/tuple → asarray). + (generated Image → pixel view, CompressedImage → decoded pixels, + ndarray pass-through, list/tuple → asarray). - `ref.field` set: `getattr(msg, ref.field)` (or `msg[ref.field]` for dict payloads) then coerce. """ @@ -155,10 +158,14 @@ def resolve_field(msg: Any, ref: StreamField) -> NDArray[Any]: else: value = getattr(msg, ref.field) + if isinstance(value, Image): + return image_view(value) + if isinstance(value, CompressedImage): + return image_view(image_from_compressed(value)) if isinstance(value, np.ndarray): return value if hasattr(value, "data") and isinstance(value.data, np.ndarray): - # e.g. Image → use its underlying ndarray + # Application payloads may expose an underlying ndarray. return value.data return np.asarray(value) diff --git a/dimos/imitation/dataprep/test_generated_images.py b/dimos/imitation/dataprep/test_generated_images.py new file mode 100644 index 00000000000..7f4e9a4f72e --- /dev/null +++ b/dimos/imitation/dataprep/test_generated_images.py @@ -0,0 +1,51 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +# Copyright 2026 Dimensional Inc. +# SPDX-License-Identifier: Apache-2.0 + +from dimos_generated.sensor_msgs.msg import Image +from dimos_generated.std_msgs.msg import Header +import numpy as np +import pytest + +from dimos.imitation.dataprep.core import StreamField, is_image_array, resolve_field +from dimos.msgs.image import compressed_image_from_image, image_from_array + + +@pytest.mark.parametrize("compressed", [False, True]) +def test_generated_camera_field_is_a_pixel_array_after_cdr(compressed): + pixels = np.full((8, 10, 3), [30, 90, 150], dtype=np.uint8) + image = image_from_array(pixels, encoding="bgr8", header=Header(frame_id="camera")) + message = compressed_image_from_image(image, quality=100) if compressed else image + message = type(message).decode(message.encode()) + before = message.encode() + array = resolve_field(message, StreamField(stream="camera")) + assert array.shape == pixels.shape and array.dtype == pixels.dtype + assert is_image_array(array) + np.testing.assert_allclose(array, pixels, atol=2 if compressed else 0) + assert message.encode() == before + + +def test_generated_mono16_camera_respects_row_padding_and_endian(): + pixels = np.array([[1, 65535], [256, 4096]], dtype=">u2") + padded = np.zeros((2, 6), dtype=np.uint8) + padded[:, :4] = pixels.view(np.uint8).reshape(2, 4) + message = Image( + width=2, height=2, encoding="mono16", is_bigendian=1, step=6, data=padded.ravel() + ) + array = resolve_field({"camera": message}, StreamField(stream="dict", field="camera")) + assert is_image_array(array) + np.testing.assert_array_equal(array, pixels) + assert array.strides == (6, 2) diff --git a/dimos/imitation/policy/lerobot/module.py b/dimos/imitation/policy/lerobot/module.py index 9f23719c5e6..5e6978c1b1a 100644 --- a/dimos/imitation/policy/lerobot/module.py +++ b/dimos/imitation/policy/lerobot/module.py @@ -19,6 +19,9 @@ from pathlib import Path from typing import Protocol, TypedDict +from dimos_generated.sensor_msgs.msg import Image, JointState +from dimos_generated.std_msgs.msg import UInt32 +from dimos_generated.trajectory_msgs.msg import JointTrajectory from pydantic import Field, field_validator from dimos.control.tasks.trajectory_task.trajectory_task import ( @@ -31,9 +34,6 @@ IsolatedPythonModule, IsolatedPythonModuleConfig, ) -from dimos.msgs.sensor_msgs.Image import Image -from dimos.msgs.sensor_msgs.JointState import JointState -from dimos.msgs.trajectory_msgs.JointTrajectory import JointTrajectory from dimos.spec.utils import Spec from dimos.teleop.webxr.controller_types import BUTTON_ALIASES, Buttons @@ -119,8 +119,8 @@ class LeRobotPolicyModule(IsolatedPythonModule): color_image: In[Image] coordinator_joint_state: In[JointState] - button_pressed: In[Buttons] - teleop_buttons: In[Buttons] + button_pressed: In[UInt32] + teleop_buttons: In[UInt32] _control: PolicyControlSpec diff --git a/dimos/imitation/test_datacollection_e2e.py b/dimos/imitation/test_datacollection_e2e.py index 7c931ce403d..f17dd378a13 100644 --- a/dimos/imitation/test_datacollection_e2e.py +++ b/dimos/imitation/test_datacollection_e2e.py @@ -22,6 +22,8 @@ from typing import Any import cv2 +from dimos_generated.sensor_msgs.msg import Image, JointState +from dimos_generated.std_msgs.msg import Header import h5py import numpy as np import pyarrow.parquet as pq @@ -44,14 +46,13 @@ extract_episodes, ) from dimos.memory.store.sqlite import SqliteStore -from dimos.msgs.sensor_msgs.Image import Image, ImageFormat -from dimos.msgs.sensor_msgs.JointState import JointState +from dimos.msgs.image import image_from_array, image_view +from dimos.msgs.time import time_from_seconds from dimos.utils.testing.waiting import wait_until pytestmark = [ pytest.mark.skipif_macos, pytest.mark.skipif_aarch64, - pytest.mark.skipif_no_turbojpeg, ] @@ -162,18 +163,16 @@ def publish(name: str, message: Any) -> None: pixel = int(base + frame) * 4 publish( "color_image", - Image( - data=np.full((16, 16, 3), pixel, dtype=np.uint8), - format=ImageFormat.RGB, - frame_id="camera", - ts=ts, + image_from_array( + np.full((16, 16, 3), pixel, dtype=np.uint8), + encoding="rgb8", + header=Header(frame_id="camera", stamp=time_from_seconds(ts)), ), ) publish( "coordinator_joint_state", JointState( - ts=ts, - frame_id="arm", + header=Header(frame_id="arm", stamp=time_from_seconds(ts)), name=["joint_0", "joint_1"], position=[base + frame, base + 100.0 + frame], velocity=[0.0, 0.0], @@ -238,7 +237,7 @@ def recorded_session( (108.0, 110.0, True, "place"), ] recorded_images: dict[float, np.ndarray[Any, Any]] = { - observation.ts: observation.data.data + observation.ts: image_view(observation.data) for observation in store.stream("color_image", Image).to_list() } diff --git a/dimos/manipulation/conftest.py b/dimos/manipulation/conftest.py index dff9c169ab6..706bbec57fb 100644 --- a/dimos/manipulation/conftest.py +++ b/dimos/manipulation/conftest.py @@ -19,6 +19,7 @@ from typing import Any, Protocol, cast from unittest.mock import DEFAULT, MagicMock +from dimos_generated.dimos_msgs.msg import TrajectoryStatus import pytest from pytest_mock import MockerFixture @@ -32,7 +33,8 @@ from dimos.manipulation.manipulation_module import ManipulationModule from dimos.manipulation.planning.groups.models import PlanningGroupDefinition from dimos.manipulation.planning.spec.config import RobotModelConfig -from dimos.msgs.trajectory_msgs.TrajectoryStatus import TrajectoryState, TrajectoryStatus +from dimos.msgs.time import header_now +from dimos.msgs.trajectory import TrajectoryState from dimos.robot.assets.model import RobotModel @@ -55,7 +57,9 @@ def invoke(task: str, method: str, args: dict | None = None): return TrajectoryCancellationResult(TrajectoryCancellationStatus.ALREADY_STOPPED) return DEFAULT - coordinator.task_invoke.return_value = TrajectoryStatus(state=TrajectoryState.IDLE) + coordinator.task_invoke.return_value = TrajectoryStatus( + header=header_now(), state=TrajectoryState.IDLE + ) coordinator.task_invoke.side_effect = invoke return coordinator diff --git a/dimos/manipulation/control/coordinator_client.py b/dimos/manipulation/control/coordinator_client.py index 93a22be8f0d..fc118d6dd36 100644 --- a/dimos/manipulation/control/coordinator_client.py +++ b/dimos/manipulation/control/coordinator_client.py @@ -58,9 +58,10 @@ from dimos.manipulation.planning.trajectory_generator.joint_trajectory_generator import ( JointTrajectoryGenerator, ) +from dimos.msgs.trajectory import sample_trajectory, trajectory_duration if TYPE_CHECKING: - from dimos.msgs.trajectory_msgs.JointTrajectory import JointTrajectory + from dimos_generated.trajectory_msgs.msg import JointTrajectory class CoordinatorClient: @@ -294,7 +295,7 @@ def preview_trajectory(trajectory: JointTrajectory, joint_names: list[str]) -> N print("\n" + "=" * 70) print("GENERATED TRAJECTORY") print("=" * 70) - print(f"Duration: {trajectory.duration:.3f}s") + print(f"Duration: {trajectory_duration(trajectory):.3f}s") print(f"Points: {len(trajectory.points)}") print("-" * 70) print(f"{'Time':>6} | {header_str} (degrees)") @@ -302,8 +303,8 @@ def preview_trajectory(trajectory: JointTrajectory, joint_names: list[str]) -> N num_samples = min(10, max(len(trajectory.points) // 10, 5)) for i in range(num_samples + 1): - t = (i / num_samples) * trajectory.duration - q_ref, _ = trajectory.sample(t) + t = (i / num_samples) * trajectory_duration(trajectory) + q_ref, _ = sample_trajectory(trajectory, t) q_deg = [f"{math.degrees(q):7.1f}" for q in q_ref] print(f"{t:6.2f} | {' '.join(q_deg)}") @@ -432,7 +433,7 @@ def run(self) -> None: if result.status is TrajectoryExecutionStatus.ACCEPTED: print( "Trajectory accepted " - f"(expected duration: {self._generated_trajectory.duration:.2f}s)" + f"(expected duration: {trajectory_duration(self._generated_trajectory):.2f}s)" ) else: print(f"Failed to start trajectory: {result.message or result.status.name}") diff --git a/dimos/manipulation/control/servo_control/README.md b/dimos/manipulation/control/servo_control/README.md index 2e0847f4d7c..bce981006a9 100644 --- a/dimos/manipulation/control/servo_control/README.md +++ b/dimos/manipulation/control/servo_control/README.md @@ -117,7 +117,7 @@ print("Target reached!") ### Using Quaternions ```python -from dimos.msgs.geometry_msgs import Quaternion +from dimos_generated.geometry_msgs.msg import Quaternion # Create quaternion (identity rotation) quat = Quaternion(x=0, y=0, z=0, w=1) @@ -131,19 +131,20 @@ controller.set_target_pose( ### Using PoseStamped Messages ```python -from dimos.msgs.geometry_msgs import PoseStamped +from dimos_generated.geometry_msgs.msg import Point, Pose, PoseStamped +from dimos.msgs.geometry import vector_array, quaternion_array +from dimos.msgs.time import header_now # Create target pose target = PoseStamped( - frame_id="world", - position=[0.3, 0.2, 0.5], - orientation=[0, 0, 0, 1] # quaternion + header=header_now(frame_id="world"), + pose=Pose(position=Point(x=0.3,y=0.2,z=0.5)), ) # Option 1: Via RPC controller.set_target_pose( - position=list(target.position), - orientation=list(target.orientation) + position=vector_array(target.pose.position).tolist(), + orientation=quaternion_array(target.pose.orientation).tolist() ) # Option 2: Via topic (if connected) diff --git a/dimos/manipulation/control/servo_control/cartesian_motion_controller.py b/dimos/manipulation/control/servo_control/cartesian_motion_controller.py index c0d858a0e40..457cd4e97a0 100644 --- a/dimos/manipulation/control/servo_control/cartesian_motion_controller.py +++ b/dimos/manipulation/control/servo_control/cartesian_motion_controller.py @@ -31,19 +31,19 @@ import time from typing import Any +from dimos_generated.dimos_msgs.msg import JointCommand, RobotState +from dimos_generated.geometry_msgs.msg import Point, Pose, PoseStamped, Quaternion, Twist, Vector3 +from dimos_generated.sensor_msgs.msg import JointState +from dimos_generated.std_msgs.msg import Header + from dimos.constants import DEFAULT_THREAD_JOIN_TIMEOUT from dimos.core.core import rpc from dimos.core.module import Module, ModuleConfig from dimos.core.stream import In, Out from dimos.manipulation.control.arm_driver_spec import ArmDriverSpec -from dimos.msgs.geometry_msgs.Pose import Pose -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Twist import Twist -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.msgs.sensor_msgs.JointCommand import JointCommand -from dimos.msgs.sensor_msgs.JointState import JointState -from dimos.msgs.sensor_msgs.RobotState import RobotState +from dimos.manipulation.planning.utils.kinematics_utils import compute_pose_error +from dimos.msgs.geometry import pose_matrix, quaternion_euler, quaternion_from_euler +from dimos.msgs.time import time_from_seconds from dimos.utils.logging_config import setup_logger from dimos.utils.simple_controller import PIDController @@ -257,12 +257,18 @@ def set_target_pose( # Detect if orientation is euler (3 elements) or quaternion (4 elements) if len(orientation) == 3: # Convert euler to quaternion using Pose's built-in conversion - euler_angles = Vector3(orientation[0], orientation[1], orientation[2]) - quat = Quaternion.from_euler(euler_angles) + euler_angles = Vector3(x=orientation[0], y=orientation[1], z=orientation[2]) + quat = quaternion_from_euler(euler_angles.x, euler_angles.y, euler_angles.z) orientation = [quat.x, quat.y, quat.z, quat.w] target = PoseStamped( - ts=time.time(), frame_id=frame_id, position=position, orientation=orientation + header=Header(frame_id=frame_id, stamp=time_from_seconds(time.time())), + pose=Pose( + position=Point(x=position[0], y=position[1], z=position[2]), + orientation=Quaternion( + x=orientation[0], y=orientation[1], z=orientation[2], w=orientation[3] + ), + ), ) with self._target_lock: @@ -309,7 +315,7 @@ def is_converged(self) -> bool: if not target_pose or not current_pose: return False - pos_error, ori_error = self._compute_pose_error(current_pose, target_pose) + pos_error, ori_error = self._compute_pose_error(current_pose, target_pose.pose) return ( pos_error < self.config.position_tolerance and ori_error < self.config.orientation_tolerance @@ -397,20 +403,21 @@ def _control_loop(self) -> None: current_pose_list[2] / 1000.0, ] euler_angles = Vector3( - current_pose_list[3], current_pose_list[4], current_pose_list[5] + x=current_pose_list[3], y=current_pose_list[4], z=current_pose_list[5] ) - quat = Quaternion.from_euler(euler_angles) + quat = quaternion_from_euler(euler_angles.x, euler_angles.y, euler_angles.z) self._current_tcp_pose = Pose( - position=position_m, - orientation=[quat.x, quat.y, quat.z, quat.w], + position=Point(x=position_m[0], y=position_m[1], z=position_m[2]), + orientation=Quaternion(x=quat.x, y=quat.y, z=quat.z, w=quat.w), ) # Publish current pose for target setters to use current_pose_stamped = PoseStamped( - ts=current_time, - frame_id="world", - position=position_m, - orientation=[quat.x, quat.y, quat.z, quat.w], + header=Header(frame_id="world", stamp=time_from_seconds(current_time)), + pose=Pose( + position=Point(x=position_m[0], y=position_m[1], z=position_m[2]), + orientation=Quaternion(x=quat.x, y=quat.y, z=quat.z, w=quat.w), + ), ) self.current_pose.publish(current_pose_stamped) else: @@ -439,7 +446,7 @@ def _control_loop(self) -> None: # Compute Cartesian error pos_error_mag, ori_error_mag = self._compute_pose_error( - self._current_tcp_pose, target_pose + self._current_tcp_pose, target_pose.pose ) # Log error periodically (every 1 second) @@ -447,7 +454,7 @@ def _control_loop(self) -> None: self._last_error_log_time = 0.0 if current_time - self._last_error_log_time > 1.0: logger.info( - f"Curr=[{self._current_tcp_pose.x:.3f},{self._current_tcp_pose.y:.3f},{self._current_tcp_pose.z:.3f}]m Tgt=[{target_pose.x:.3f},{target_pose.y:.3f},{target_pose.z:.3f}]m Err={pos_error_mag * 1000:.1f}mm" + f"Curr=[{self._current_tcp_pose.position.x:.3f},{self._current_tcp_pose.position.y:.3f},{self._current_tcp_pose.position.z:.3f}]m Tgt=[{target_pose.pose.position.x:.3f},{target_pose.pose.position.y:.3f},{target_pose.pose.position.z:.3f}]m Err={pos_error_mag * 1000:.1f}mm" ) self._last_error_log_time = current_time @@ -486,7 +493,7 @@ def _control_loop(self) -> None: # Generate Cartesian velocity command cartesian_twist = self._compute_cartesian_velocity( - self._current_tcp_pose, target_pose, dt + self._current_tcp_pose, target_pose.pose, dt ) # Publish debug twist @@ -505,12 +512,12 @@ def _control_loop(self) -> None: # Convert Pose to xArm format: [x, y, z, roll, pitch, yaw] # Note: xArm IK expects position in mm, so convert from m to mm next_pose_list = [ - next_pose.x * 1000.0, # m to mm - next_pose.y * 1000.0, # m to mm - next_pose.z * 1000.0, # m to mm - next_pose.roll, - next_pose.pitch, - next_pose.yaw, + next_pose.position.x * 1000.0, # m to mm + next_pose.position.y * 1000.0, # m to mm + next_pose.position.z * 1000.0, # m to mm + quaternion_euler(next_pose.orientation)[0], + quaternion_euler(next_pose.orientation)[1], + quaternion_euler(next_pose.orientation)[2], ] logger.debug( @@ -541,7 +548,7 @@ def _control_loop(self) -> None: # Publish joint command joint_cmd = JointCommand( - timestamp=current_time, + header=Header(stamp=time_from_seconds(current_time)), positions=list(target_joints), ) @@ -573,24 +580,7 @@ def _compute_pose_error(self, current_pose: Pose, target_pose: Pose) -> tuple[fl Returns: Tuple of (position_error_magnitude, orientation_error_magnitude) """ - # Position error (Euclidean distance) - pos_error = Vector3( - target_pose.x - current_pose.x, - target_pose.y - current_pose.y, - target_pose.z - current_pose.z, - ) - pos_error_mag = math.sqrt(pos_error.x**2 + pos_error.y**2 + pos_error.z**2) - - # Orientation error (angle between quaternions) - # q_error = q_current^-1 * q_target - q_current_inv = current_pose.orientation.conjugate() - q_error = q_current_inv * target_pose.orientation - - # Extract angle from axis-angle representation - # For quaternion [x, y, z, w], angle = 2 * acos(w) - ori_error_mag = 2 * math.acos(min(1.0, abs(q_error.w))) - - return pos_error_mag, ori_error_mag + return compute_pose_error(pose_matrix(current_pose), pose_matrix(target_pose)) def _compute_cartesian_velocity( self, current_pose: Pose, target_pose: Pose, dt: float @@ -607,9 +597,9 @@ def _compute_cartesian_velocity( Twist message with linear and angular velocities """ # Position error - error_x = target_pose.x - current_pose.x - error_y = target_pose.y - current_pose.y - error_z = target_pose.z - current_pose.z + error_x = target_pose.position.x - current_pose.position.x + error_y = target_pose.position.y - current_pose.position.y + error_z = target_pose.position.z - current_pose.position.z # Compute linear velocities via PID vel_x = self._pid_x.update(error_x, dt) # type: ignore[no-untyped-call] @@ -618,9 +608,18 @@ def _compute_cartesian_velocity( # Orientation error (convert to euler for simpler PID) # This is an approximation; axis-angle would be more accurate - error_roll = self._normalize_angle(target_pose.roll - current_pose.roll) - error_pitch = self._normalize_angle(target_pose.pitch - current_pose.pitch) - error_yaw = self._normalize_angle(target_pose.yaw - current_pose.yaw) + error_roll = self._normalize_angle( + quaternion_euler(target_pose.orientation)[0] + - quaternion_euler(current_pose.orientation)[0] + ) + error_pitch = self._normalize_angle( + quaternion_euler(target_pose.orientation)[1] + - quaternion_euler(current_pose.orientation)[1] + ) + error_yaw = self._normalize_angle( + quaternion_euler(target_pose.orientation)[2] + - quaternion_euler(current_pose.orientation)[2] + ) # Compute angular velocities via PID omega_x = self._pid_roll.update(error_roll, dt) # type: ignore[no-untyped-call] @@ -628,7 +627,8 @@ def _compute_cartesian_velocity( omega_z = self._pid_yaw.update(error_yaw, dt) # type: ignore[no-untyped-call] return Twist( - linear=Vector3(vel_x, vel_y, vel_z), angular=Vector3(omega_x, omega_y, omega_z) + linear=Vector3(x=vel_x, y=vel_y, z=vel_z), + angular=Vector3(x=omega_x, y=omega_y, z=omega_z), ) def _integrate_velocity(self, current_pose: Pose, velocity: Twist, dt: float) -> Pose: @@ -645,27 +645,27 @@ def _integrate_velocity(self, current_pose: Pose, velocity: Twist, dt: float) -> """ # Integrate position (simple Euler integration) next_position = Vector3( - current_pose.x + velocity.linear.x * dt, - current_pose.y + velocity.linear.y * dt, - current_pose.z + velocity.linear.z * dt, + x=current_pose.position.x + velocity.linear.x * dt, + y=current_pose.position.y + velocity.linear.y * dt, + z=current_pose.position.z + velocity.linear.z * dt, ) # Integrate orientation (simple euler integration - good for small dt) - next_roll = current_pose.roll + velocity.angular.x * dt - next_pitch = current_pose.pitch + velocity.angular.y * dt - next_yaw = current_pose.yaw + velocity.angular.z * dt + next_roll = quaternion_euler(current_pose.orientation)[0] + velocity.angular.x * dt + next_pitch = quaternion_euler(current_pose.orientation)[1] + velocity.angular.y * dt + next_yaw = quaternion_euler(current_pose.orientation)[2] + velocity.angular.z * dt - euler_angles = Vector3(next_roll, next_pitch, next_yaw) - next_orientation = Quaternion.from_euler(euler_angles) + euler_angles = Vector3(x=next_roll, y=next_pitch, z=next_yaw) + next_orientation = quaternion_from_euler(euler_angles.x, euler_angles.y, euler_angles.z) return Pose( - position=next_position, - orientation=[ - next_orientation.x, - next_orientation.y, - next_orientation.z, - next_orientation.w, - ], + position=Point(x=next_position.x, y=next_position.y, z=next_position.z), + orientation=Quaternion( + x=next_orientation.x, + y=next_orientation.y, + z=next_orientation.z, + w=next_orientation.w, + ), ) @staticmethod diff --git a/dimos/manipulation/control/test_generated_legacy_controllers.py b/dimos/manipulation/control/test_generated_legacy_controllers.py new file mode 100644 index 00000000000..75dc43288b7 --- /dev/null +++ b/dimos/manipulation/control/test_generated_legacy_controllers.py @@ -0,0 +1,106 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from types import SimpleNamespace +from unittest.mock import MagicMock + +from dimos_generated.dimos_msgs.msg import JointCommand +from dimos_generated.geometry_msgs.msg import Point, Pose, PoseStamped, Quaternion +from dimos_generated.sensor_msgs.msg import JointState +from dimos_generated.trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint +import pytest + +from dimos.manipulation.control.servo_control.cartesian_motion_controller import ( + CartesianMotionController, +) +from dimos.manipulation.control.trajectory_controller.joint_trajectory_controller import ( + JointTrajectoryController, +) +from dimos.msgs.time import duration_from_seconds +from dimos.msgs.trajectory import TrajectoryState + + +def test_trajectory_loop_publishes_generated_sample_with_source_time( + monkeypatch: pytest.MonkeyPatch, +) -> None: + controller = JointTrajectoryController() + controller._trajectory = JointTrajectory( + joint_names=["a", "b"], + points=[ + JointTrajectoryPoint(positions=[0.0, 0.0], time_from_start=duration_from_seconds(0)), + JointTrajectoryPoint(positions=[1.0, 2.0], time_from_start=duration_from_seconds(1)), + ], + ) + controller._state = TrajectoryState.EXECUTING + controller._start_time = 100.0 + controller.joint_position_command = MagicMock() + controller.joint_position_command.publish.side_effect = ( + lambda message: controller._stop_event.set() + ) + monkeypatch.setattr( + "dimos.manipulation.control.trajectory_controller.joint_trajectory_controller.time", + SimpleNamespace(time=lambda: 100.5, sleep=lambda seconds: None), + ) + monkeypatch.setattr("dimos.msgs.time.time.time_ns", lambda: 100500000000) + try: + controller._execution_loop() + controller.joint_position_command.publish.assert_called_once() + message = JointCommand.decode( + controller.joint_position_command.publish.call_args.args[0].encode() + ) + assert list(message.positions) == [0.5, 1.0] + assert (message.header.stamp.sec, message.header.stamp.nanosec) == (100, 500000000) + finally: + controller.stop() + + +def test_cartesian_generated_command_preserves_joint_count_and_pid_clamp( + monkeypatch: pytest.MonkeyPatch, +) -> None: + controller = CartesianMotionController() + driver = MagicMock() + driver.get_forward_kinematics.return_value = (0, [0.0] * 6) + driver.get_inverse_kinematics.return_value = (0, [0.1, 0.2, 0.3]) + controller._arm_driver = driver + controller._latest_joint_state = JointState(position=[0.0, 0.0]) + controller._target_pose_ = PoseStamped( + pose=Pose(position=Point(x=0.1), orientation=Quaternion(w=1.0)) + ) + controller._is_tracking = True + controller._last_target_time = 100.0 + controller.current_pose = MagicMock() + controller.cartesian_velocity = MagicMock() + controller.joint_position_command = MagicMock() + controller.joint_position_command.publish.side_effect = ( + lambda message: controller._stop_event.set() + ) + monkeypatch.setattr( + "dimos.manipulation.control.servo_control.cartesian_motion_controller.time", + SimpleNamespace(time=lambda: 100.5), + ) + try: + controller._control_loop() + controller.joint_position_command.publish.assert_called_once() + message = JointCommand.decode( + controller.joint_position_command.publish.call_args.args[0].encode() + ) + assert list(message.positions) == [0.1, 0.2] + assert (message.header.stamp.sec, message.header.stamp.nanosec) == (100, 500000000) + velocity = controller.cartesian_velocity.publish.call_args.args[0] + assert velocity.linear.x == pytest.approx(controller.config.max_linear_velocity) + # FK/IK only invoked on the fake driver; no driver actuation interface exists here. + assert driver.get_forward_kinematics.call_count == 1 + assert driver.get_inverse_kinematics.call_count == 1 + finally: + controller.stop() diff --git a/dimos/manipulation/control/trajectory_controller/joint_trajectory_controller.py b/dimos/manipulation/control/trajectory_controller/joint_trajectory_controller.py index 5676c741393..52f6c30b923 100644 --- a/dimos/manipulation/control/trajectory_controller/joint_trajectory_controller.py +++ b/dimos/manipulation/control/trajectory_controller/joint_trajectory_controller.py @@ -33,15 +33,16 @@ import time from typing import Any +from dimos_generated.dimos_msgs.msg import JointCommand, RobotState, TrajectoryStatus +from dimos_generated.sensor_msgs.msg import JointState +from dimos_generated.trajectory_msgs.msg import JointTrajectory + from dimos.constants import DEFAULT_THREAD_JOIN_TIMEOUT from dimos.core.core import rpc from dimos.core.module import Module, ModuleConfig from dimos.core.stream import In, Out -from dimos.msgs.sensor_msgs.JointCommand import JointCommand -from dimos.msgs.sensor_msgs.JointState import JointState -from dimos.msgs.sensor_msgs.RobotState import RobotState -from dimos.msgs.trajectory_msgs.JointTrajectory import JointTrajectory -from dimos.msgs.trajectory_msgs.TrajectoryStatus import TrajectoryState, TrajectoryStatus +from dimos.msgs.time import duration_from_seconds, header_now +from dimos.msgs.trajectory import TrajectoryState, sample_trajectory, trajectory_duration from dimos.utils.logging_config import setup_logger logger = setup_logger() @@ -178,7 +179,7 @@ def execute_trajectory(self, trajectory: JointTrajectory) -> bool: return False # Validate trajectory - if trajectory is None or trajectory.duration <= 0: + if trajectory is None or trajectory_duration(trajectory) <= 0: logger.warning("Invalid trajectory: None or zero duration") return False @@ -198,7 +199,7 @@ def execute_trajectory(self, trajectory: JointTrajectory) -> bool: logger.info( f"Executing trajectory: {len(trajectory.points)} points, " - f"duration={trajectory.duration:.3f}s" + f"duration={trajectory_duration(trajectory):.3f}s" ) return True @@ -255,18 +256,19 @@ def get_status(self) -> TrajectoryStatus: if self._trajectory is not None and self._state == TrajectoryState.EXECUTING: time_elapsed = time.time() - self._start_time - time_remaining = max(0.0, self._trajectory.duration - time_elapsed) + time_remaining = max(0.0, trajectory_duration(self._trajectory) - time_elapsed) progress = ( - min(1.0, time_elapsed / self._trajectory.duration) - if self._trajectory.duration > 0 + min(1.0, time_elapsed / trajectory_duration(self._trajectory)) + if trajectory_duration(self._trajectory) > 0 else 1.0 ) return TrajectoryStatus( + header=header_now(), state=self._state, progress=progress, - time_elapsed=time_elapsed, - time_remaining=time_remaining, + time_elapsed=duration_from_seconds(time_elapsed), + time_remaining=duration_from_seconds(time_remaining), error=self._error_message, ) @@ -281,7 +283,7 @@ def _on_robot_state(self, msg: RobotState) -> None: def _on_trajectory(self, msg: JointTrajectory) -> None: """Callback when trajectory is received via topic.""" logger.info( - f"Received trajectory via topic: {len(msg.points)} points, duration={msg.duration:.3f}s" + f"Received trajectory via topic: {len(msg.points)} points, duration={trajectory_duration(msg):.3f}s" ) self.execute_trajectory(msg) @@ -313,17 +315,17 @@ def _execution_loop(self) -> None: if self._trajectory is None: self._state = TrajectoryState.FAULT logger.error("Trajectory is None during execution") - elif t >= self._trajectory.duration: + elif t >= trajectory_duration(self._trajectory): self._state = TrajectoryState.COMPLETED logger.info( - f"Trajectory completed: duration={self._trajectory.duration:.3f}s" + f"Trajectory completed: duration={trajectory_duration(self._trajectory):.3f}s" ) else: # Sample trajectory - q_ref, _qd_ref = self._trajectory.sample(t) + q_ref, _qd_ref = sample_trajectory(self._trajectory, t) # Create and publish command (outside lock would be better but simpler here) - cmd = JointCommand(positions=q_ref, timestamp=time.time()) + cmd = JointCommand(positions=q_ref, header=header_now()) # Publish - must release lock first for thread safety trajectory_active = True diff --git a/dimos/manipulation/execution_manager.py b/dimos/manipulation/execution_manager.py index 78f37be4119..d9535409222 100644 --- a/dimos/manipulation/execution_manager.py +++ b/dimos/manipulation/execution_manager.py @@ -21,6 +21,9 @@ import threading import time +from dimos_generated.dimos_msgs.msg import TrajectoryStatus +from dimos_generated.trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint + from dimos.constants import DEFAULT_THREAD_JOIN_TIMEOUT from dimos.control.coordinator import ControlCoordinator from dimos.control.tasks.trajectory_task.trajectory_task import ( @@ -32,9 +35,7 @@ ) from dimos.manipulation.manipulation_spec import ExecutionResult, ExecutionStatus from dimos.manipulation.planning.spec.models import GeneratedPlan -from dimos.msgs.trajectory_msgs.JointTrajectory import JointTrajectory -from dimos.msgs.trajectory_msgs.TrajectoryPoint import TrajectoryPoint -from dimos.msgs.trajectory_msgs.TrajectoryStatus import TrajectoryState, TrajectoryStatus +from dimos.msgs.trajectory import TrajectoryState from dimos.utils.logging_config import setup_logger logger = setup_logger() @@ -249,7 +250,7 @@ def cancel(self, timeout: float = 1.0) -> ExecutionResult: ExecutionStatus.FAULT, }: return latest - if status.state is TrajectoryState.IDLE: + if status.state == TrajectoryState.IDLE: result = ExecutionResult(ExecutionStatus.NO_EXECUTION, cancellation.message) self._store(result, active=False) return result @@ -306,7 +307,7 @@ def _combine(self, statuses: dict[str, TrajectoryStatus]) -> ExecutionResult: other for other, other_status in statuses.items() if other != task - and other_status.state is TrajectoryState.EXECUTING + and other_status.state == TrajectoryState.EXECUTING and not self._cancel_task(other) ] if failed: @@ -317,7 +318,7 @@ def _combine(self, statuses: dict[str, TrajectoryStatus]) -> ExecutionResult: ) mapped = self._result_from_status(status).status return ExecutionResult(mapped, f"{task}: {status.error}", trajectory_status=primary) - if all(status.state is TrajectoryState.COMPLETED for status in statuses.values()): + if all(status.state == TrajectoryState.COMPLETED for status in statuses.values()): return ExecutionResult(ExecutionStatus.COMPLETED, trajectory_status=primary) return ExecutionResult(ExecutionStatus.EXECUTING, trajectory_status=primary) @@ -409,7 +410,7 @@ def _result_from_status(status: TrajectoryStatus) -> ExecutionResult: TrajectoryState.COMPLETED: ExecutionStatus.COMPLETED, TrajectoryState.ABORTED: ExecutionStatus.ABORTED, TrajectoryState.FAULT: ExecutionStatus.FAULT, - }[status.state] + }[TrajectoryState(status.state)] return ExecutionResult(mapped, status.error, trajectory_status=status) def _store(self, result: ExecutionResult, *, active: bool, run_id: int | None = None) -> None: @@ -428,7 +429,7 @@ def _prepare_trajectory(self, plan: GeneratedPlan) -> list[tuple[str, JointTraje if not plan.is_success(): raise _PlanRejectedError("Generated plan status is not successful") - names = plan.trajectory.joint_names + names = list(plan.trajectory.joint_names) unknown = [name for name in names if name not in self._joint_names] if unknown: raise _PlanRejectedError(f"Generated trajectory has unknown joints: {unknown}") @@ -476,12 +477,18 @@ def _columns(trajectory: JointTrajectory, columns: list[int]) -> JointTrajectory return JointTrajectory( joint_names=[trajectory.joint_names[index] for index in columns], points=[ - TrajectoryPoint( + JointTrajectoryPoint( time_from_start=point.time_from_start, positions=[point.positions[index] for index in columns], - velocities=[point.velocities[index] for index in columns], + velocities=[point.velocities[index] for index in columns] + if point.velocities + else [], + accelerations=[point.accelerations[index] for index in columns] + if point.accelerations + else [], + effort=[point.effort[index] for index in columns] if point.effort else [], ) for point in trajectory.points ], - timestamp=trajectory.timestamp, + header=trajectory.header, ) diff --git a/dimos/manipulation/grasping/grasp_gen_spec.py b/dimos/manipulation/grasping/grasp_gen_spec.py index d3071d76d8e..2ac410fd0a4 100644 --- a/dimos/manipulation/grasping/grasp_gen_spec.py +++ b/dimos/manipulation/grasping/grasp_gen_spec.py @@ -14,9 +14,10 @@ from typing import Protocol -from dimos.msgs.geometry_msgs.PoseArray import PoseArray -from dimos.msgs.manipulation_msgs.GraspCandidateArray import GraspCandidateArray -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 +from dimos_generated.dimos_msgs.msg import GraspCandidateArray +from dimos_generated.geometry_msgs.msg import PoseArray +from dimos_generated.sensor_msgs.msg import PointCloud2 + from dimos.spec.utils import Spec diff --git a/dimos/manipulation/grasping/grasp_gen_x/module.py b/dimos/manipulation/grasping/grasp_gen_x/module.py index 7a608e7a90d..4e73f119615 100644 --- a/dimos/manipulation/grasping/grasp_gen_x/module.py +++ b/dimos/manipulation/grasping/grasp_gen_x/module.py @@ -18,6 +18,8 @@ from typing import Annotated, Literal, TypeAlias +from dimos_generated.dimos_msgs.msg import GraspCandidateArray +from dimos_generated.sensor_msgs.msg import PointCloud2 import numpy as np from pydantic import Field, FiniteFloat, field_validator @@ -27,8 +29,6 @@ IsolatedPythonModuleConfig, ) from dimos.manipulation.grasping.grasp_gen_spec import GraspGenSpec -from dimos.msgs.manipulation_msgs.GraspCandidateArray import GraspCandidateArray -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 from dimos.protocol.service.spec import BaseConfig GRASPGENX_MODEL_REPO = "adithyamurali/GraspGenXModel" diff --git a/dimos/manipulation/grasping/grasp_gen_x/test_module.py b/dimos/manipulation/grasping/grasp_gen_x/test_module.py index 55d7e4c301f..edcbdf69d0d 100644 --- a/dimos/manipulation/grasping/grasp_gen_x/test_module.py +++ b/dimos/manipulation/grasping/grasp_gen_x/test_module.py @@ -23,15 +23,15 @@ import sys from typing import Any +from dimos_generated.dimos_msgs.msg import GraspCandidate, GraspCandidateArray +from dimos_generated.geometry_msgs.msg import Point, Pose +from dimos_generated.std_msgs.msg import Header import numpy as np import pytest from dimos.manipulation.grasping.grasp_gen_spec import GraspGenSpec, LegacyGraspGenSpec from dimos.manipulation.grasping.grasp_gen_x.module import GraspGenXConfig -from dimos.msgs.geometry_msgs.Pose import Pose -from dimos.msgs.manipulation_msgs.GraspCandidate import GraspCandidate -from dimos.msgs.manipulation_msgs.GraspCandidateArray import GraspCandidateArray -from dimos.msgs.std_msgs.Header import Header +from dimos.msgs.time import time_from_seconds def config(**overrides: Any) -> GraspGenXConfig: @@ -68,15 +68,20 @@ def test_public_adapter_import_does_not_load_optional_runtime() -> None: def test_messages_round_trip_empty_and_score() -> None: - value = GraspCandidateArray(Header(3.0, "camera"), [GraspCandidate(Pose(1, 2, 3), 0.25)]) + value = GraspCandidateArray( + header=Header(stamp=time_from_seconds(3.0), frame_id="camera"), + candidates=[GraspCandidate(pose=Pose(position=Point(x=1, y=2)), score=0.25)], + ) decoded = GraspCandidateArray.decode(value.encode()) assert decoded.header.frame_id == "camera" - assert decoded.header.timestamp == pytest.approx(3.0) + assert decoded.header.stamp == time_from_seconds(3.0) assert decoded.candidates[0].score == pytest.approx(0.25) assert ( GraspCandidateArray.decode( - GraspCandidateArray(Header(3.0, "camera"), []).encode() + GraspCandidateArray( + header=Header(stamp=time_from_seconds(3.0), frame_id="camera"), candidates=[] + ).encode() ).candidates == [] ) diff --git a/dimos/manipulation/grasping/grasping.py b/dimos/manipulation/grasping/grasping.py index 9fd1cc9178a..2e0683efc97 100644 --- a/dimos/manipulation/grasping/grasping.py +++ b/dimos/manipulation/grasping/grasping.py @@ -19,20 +19,22 @@ from __future__ import annotations +import math from typing import TYPE_CHECKING +from dimos_generated.geometry_msgs.msg import PoseArray + from dimos.agents.annotation import skill from dimos.core.core import rpc from dimos.core.module import Module from dimos.core.stream import Out from dimos.manipulation.grasping.grasp_gen_spec import LegacyGraspGenSpec -from dimos.msgs.geometry_msgs.PoseArray import PoseArray +from dimos.msgs.geometry import quaternion_euler from dimos.perception.experimental.object_scene_registration_spec import ObjectSceneRegistrationSpec from dimos.utils.logging_config import setup_logger -from dimos.utils.transform_utils import quaternion_to_euler if TYPE_CHECKING: - from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 + from dimos_generated.sensor_msgs.msg import PointCloud2 logger = setup_logger() @@ -129,9 +131,9 @@ def _format_grasp_result(self, grasps: PoseArray, object_name: str) -> str: """Format grasp result for agent/human consumption.""" best = grasps.poses[0] pos = best.position - rpy = quaternion_to_euler(best.orientation, degrees=True) + rpy = tuple(math.degrees(value) for value in quaternion_euler(best.orientation)) return ( f"Generated {len(grasps.poses)}" f"Best grasp: pos=({pos.x:.4f}, {pos.y:.4f}, {pos.z:.4f}), " - f"rpy=({rpy.x:.1f}, {rpy.y:.1f}, {rpy.z:.1f}) degrees" + f"rpy=({rpy[0]:.1f}, {rpy[1]:.1f}, {rpy[2]:.1f}) degrees" ) diff --git a/dimos/manipulation/grasping/heuristic_grasp.py b/dimos/manipulation/grasping/heuristic_grasp.py index aff5442661e..74fec3113db 100644 --- a/dimos/manipulation/grasping/heuristic_grasp.py +++ b/dimos/manipulation/grasping/heuristic_grasp.py @@ -18,19 +18,18 @@ import math +from dimos_generated.dimos_msgs.msg import GraspCandidate, GraspCandidateArray +from dimos_generated.geometry_msgs.msg import Point, Pose +from dimos_generated.sensor_msgs.msg import PointCloud2 import numpy as np from numpy.typing import NDArray from dimos.core.core import rpc from dimos.core.module import Module from dimos.manipulation.grasping.grasp_gen_spec import GraspGenSpec -from dimos.msgs.geometry_msgs.Pose import Pose -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.msgs.manipulation_msgs.GraspCandidate import GraspCandidate -from dimos.msgs.manipulation_msgs.GraspCandidateArray import GraspCandidateArray -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 -from dimos.msgs.std_msgs.Header import Header +from dimos.msgs.geometry import quaternion_from_euler +from dimos.msgs.pointcloud import pointcloud_xyz +from dimos.msgs.time import to_nanoseconds class HeuristicGraspModule(Module, GraspGenSpec): @@ -41,11 +40,10 @@ class HeuristicGraspModule(Module, GraspGenSpec): @rpc def propose_grasps(self, object_pointcloud: PointCloud2) -> GraspCandidateArray: - if object_pointcloud.ts is None or not math.isfinite(float(object_pointcloud.ts)): - raise ValueError("object pointcloud must have a finite timestamp") - if not object_pointcloud.frame_id: + to_nanoseconds(object_pointcloud.header.stamp) + if not object_pointcloud.header.frame_id: raise ValueError("object pointcloud frame_id must not be empty") - points = object_pointcloud.points_f32() + points = pointcloud_xyz(object_pointcloud).astype(np.float32) if points.ndim != 2 or points.shape[1] != 3 or len(points) < 3: raise ValueError("object pointcloud must contain at least three XYZ points") if not np.all(np.isfinite(points)): @@ -55,12 +53,14 @@ def propose_grasps(self, object_pointcloud: PointCloud2) -> GraspCandidateArray: center_xy = np.median(xy, axis=0) low_z, high_z = np.quantile(points[:, 2], [0.05, 0.95]) pose = Pose( - Vector3(float(center_xy[0]), float(center_xy[1]), float((low_z + high_z) / 2.0)), - Quaternion.from_euler(Vector3(-math.pi, 0.0, self._narrow_axis_yaw(xy))), + position=Point( + x=float(center_xy[0]), y=float(center_xy[1]), z=float((low_z + high_z) / 2.0) + ), + orientation=quaternion_from_euler(-math.pi, 0.0, self._narrow_axis_yaw(xy)), ) return GraspCandidateArray( - Header(float(object_pointcloud.ts), object_pointcloud.frame_id), - [GraspCandidate(pose, score=1.0)], + header=object_pointcloud.header, + candidates=[GraspCandidate(pose=pose, score=1.0)], ) @staticmethod diff --git a/dimos/manipulation/grasping/test_heuristic_grasp.py b/dimos/manipulation/grasping/test_heuristic_grasp.py index 9862b615c44..5fa2c160067 100644 --- a/dimos/manipulation/grasping/test_heuristic_grasp.py +++ b/dimos/manipulation/grasping/test_heuristic_grasp.py @@ -15,20 +15,25 @@ from collections.abc import Iterator import math +from dimos_generated.builtin_interfaces.msg import Time +from dimos_generated.sensor_msgs.msg import PointCloud2 +from dimos_generated.std_msgs.msg import Header import numpy as np import pytest from dimos.manipulation.grasping.grasp_gen_spec import GraspGenSpec from dimos.manipulation.grasping.heuristic_grasp import HeuristicGraspModule -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 +from dimos.msgs.geometry import pose_matrix +from dimos.msgs.pointcloud import pointcloud_from_xyz from dimos.spec.utils import spec_annotation_compliance def _cloud( - points: np.ndarray, *, frame_id: str = "world", timestamp: float | None = 1.0 + points: np.ndarray, *, frame_id: str = "world", stamp: Time | None = None ) -> PointCloud2: - return PointCloud2.from_numpy(points.astype(np.float32), frame_id=frame_id, timestamp=timestamp) + return pointcloud_from_xyz( + points.astype(np.float32), header=Header(frame_id=frame_id, stamp=stamp or Time(sec=1)) + ) @pytest.fixture @@ -58,13 +63,13 @@ def test_heuristic_grasp_proposes_centered_top_down_pose(module: HeuristicGraspM assert len(proposals.candidates) == 1 assert proposals.header.frame_id == "world" - assert proposals.header.timestamp == pytest.approx(1.0) + assert proposals.header.stamp == Time(sec=1) pose = proposals.candidates[0].pose assert pose.position.x == pytest.approx(0.0) assert pose.position.y == pytest.approx(0.0) assert pose.position.z == pytest.approx(0.15) - approach = pose.orientation.rotate_vector(Vector3(0.0, 0.0, 1.0)) - assert approach.z == pytest.approx(-1.0) + approach = pose_matrix(pose)[:3, 2] + assert approach[2] == pytest.approx(-1.0) assert proposals.candidates[0].score == pytest.approx(1.0) @@ -82,10 +87,10 @@ def test_heuristic_grasp_aligns_jaw_axis_with_narrow_axis(module: HeuristicGrasp ) ) - jaw_axis = proposals.candidates[0].pose.orientation.rotate_vector(Vector3(0.0, 1.0, 0.0)) - assert abs(jaw_axis.x) == pytest.approx(1.0) - assert jaw_axis.y == pytest.approx(0.0, abs=1e-6) - assert jaw_axis.z == pytest.approx(0.0, abs=1e-6) + jaw_axis = pose_matrix(proposals.candidates[0].pose)[:3, 1] + assert abs(jaw_axis[0]) == pytest.approx(1.0) + assert jaw_axis[1] == pytest.approx(0.0, abs=1e-6) + assert jaw_axis[2] == pytest.approx(0.0, abs=1e-6) def test_heuristic_grasp_canonicalizes_pca_eigenvector_sign( @@ -104,27 +109,23 @@ def test_heuristic_grasp_canonicalizes_pca_eigenvector_sign( @pytest.mark.parametrize( - "points, frame_id, timestamp, error", + "points, frame_id, stamp, error", [ - ( - np.asarray([[0.0, 0.0, 0.0], [1.0, 1.0, 1.0]]), - "world", - 1.0, - "at least three", - ), - (np.asarray([[0.0, 0.0, math.nan]] * 3), "world", 1.0, "finite"), - (np.zeros((3, 3)), "", 1.0, "frame_id"), - (np.zeros((3, 3)), "world", None, "timestamp"), - (np.zeros((3, 3)), "world", math.nan, "finite timestamp"), - (np.zeros((3, 3)), "world", math.inf, "finite timestamp"), + (np.zeros((2, 3)), "world", Time(), "at least three"), + (np.asarray([[0.0, 0.0, math.nan]] * 3), "world", Time(), "finite"), + (np.zeros((3, 3)), "", Time(), "frame_id"), + (np.zeros((3, 3)), "world", Time(nanosec=1000000000), "nanosec"), ], ) -def test_heuristic_grasp_rejects_invalid_pointclouds( - module: HeuristicGraspModule, - points: np.ndarray, - frame_id: str, - timestamp: float | None, - error: str, -) -> None: +def test_heuristic_grasp_rejects_invalid_pointclouds(module, points, frame_id, stamp, error): with pytest.raises(ValueError, match=error): - module.propose_grasps(_cloud(points, frame_id=frame_id, timestamp=timestamp)) + module.propose_grasps(_cloud(points, frame_id=frame_id, stamp=stamp)) + + +@pytest.mark.parametrize("stamp", [Time(), Time(sec=1700000000, nanosec=123456789)]) +def test_heuristic_grasp_preserves_exact_header_without_aliasing(module, stamp): + cloud = _cloud(np.zeros((3, 3)), stamp=stamp) + proposals = module.propose_grasps(cloud) + assert proposals.header == cloud.header + proposals.header.frame_id = "changed" + assert cloud.header.frame_id == "world" diff --git a/dimos/manipulation/manipulation_module.py b/dimos/manipulation/manipulation_module.py index ad148bcf709..ebedf27ea92 100644 --- a/dimos/manipulation/manipulation_module.py +++ b/dimos/manipulation/manipulation_module.py @@ -19,6 +19,7 @@ import asyncio from collections import Counter from collections.abc import Mapping, Sequence +import copy from enum import Enum import math import threading @@ -26,6 +27,19 @@ import traceback from typing import Any, Literal, TypeAlias +from dimos_generated.dimos_msgs.msg import GraspCandidateArray +from dimos_generated.geometry_msgs.msg import ( + Point, + Pose, + PoseStamped, + Quaternion, + Transform, + TransformStamped, + Vector3, +) +from dimos_generated.sensor_msgs.msg import JointState, PointCloud2 +from dimos_generated.std_msgs.msg import Header +from dimos_generated.tf2_msgs.msg import TFMessage import numpy as np from pydantic import Field, model_validator @@ -90,15 +104,9 @@ ) from dimos.manipulation.visualization.factory import create_manipulation_visualization from dimos.manipulation.visualization.operator import ManipulationOperator -from dimos.msgs.geometry_msgs.Pose import Pose -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Transform import Transform -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.msgs.manipulation_msgs.GraspCandidateArray import GraspCandidateArray -from dimos.msgs.sensor_msgs.JointState import JointState -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 -from dimos.msgs.tf2_msgs.TFMessage import TFMessage +from dimos.msgs.geometry import transform_from_pose +from dimos.msgs.pointcloud import pointcloud_xyz +from dimos.msgs.time import time_from_nanoseconds from dimos.perception.experimental.object import Object as DetObject from dimos.utils.logging_config import setup_logger @@ -159,7 +167,7 @@ class ManipulationModuleConfig(ModuleConfig): # to a link the model already publishes -- an eye-in-hand camera, say. One # publisher for the whole chain: a second module publishing the mount at its # own rate leaves the two edges of one chain stamped up to a period apart. - static_transforms: list[Transform] = Field(default_factory=list) + static_transforms: list[TransformStamped] = Field(default_factory=list) # Frame the voxel_map port's clouds must already be expressed in. world_frame: str = "world" # Edge length of a voxel_map cell (meters). Must match the mapper's @@ -308,12 +316,12 @@ def _initialize_planning(self) -> None: fz = self.config.floor_z thickness = 0.2 floor_pose = Pose( - Vector3(0.7, 0.0, fz - thickness / 2), - Quaternion(0.0, 0.0, 0.0, 1.0), + position=Point(x=0.7, y=0.0, z=fz - thickness / 2), + orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0), ) floor_obs = Obstacle( name="floor", - pose=floor_pose, + pose=PoseStamped(header=Header(frame_id="world"), pose=floor_pose), obstacle_type=ObstacleType.BOX, dimensions=(0.6, 1.2, thickness), ) @@ -356,6 +364,7 @@ def _on_joint_state(self, msg: JointState) -> None: return indices = [name_to_idx[name] for name in names] state = JointState( + header=msg.header, name=list(names), position=[msg.position[index] for index in indices], velocity=[msg.velocity[index] for index in indices] @@ -377,30 +386,29 @@ def _tf_publish_loop(self) -> None: try: if self._world_monitor is None: break - transforms: list[Transform] = [] + transforms: list[TransformStamped] = [] config = self.config.model for group in self._world_monitor.planning_groups.list(): if not group.has_pose_target or group.tip_link is None: continue ee_pose = self._world_monitor.get_group_ee_pose(group.id) if ee_pose is not None and group.tip_link is not None: - ee_tf = Transform.from_pose(group.tip_link, ee_pose) - ee_tf.frame_id = "world" + ee_tf = transform_from_pose(ee_pose, child_frame_id=group.tip_link) transforms.append(ee_tf) for link_name in config.tf_extra_links: link_pose = self._world_monitor.get_link_pose(link_name) if link_pose is not None: - link_tf = Transform.from_pose(link_name, link_pose) - link_tf.frame_id = "world" + link_tf = transform_from_pose(link_pose, child_frame_id=link_name) transforms.append(link_tf) - now = time.time() + now = time.time_ns() for static in self.config.static_transforms: - static.ts = now + static = copy.deepcopy(static) + static.header.stamp = time_from_nanoseconds(now) transforms.append(static) if transforms: - self.tf.publish(TFMessage(*transforms)) + self.tf.publish(TFMessage(transforms=transforms)) except Exception as e: logger.warning("TF publish failed", error=str(e)) @@ -529,7 +537,7 @@ def get_current_joints(self) -> list[float] | None: return list(state.position) return None - def get_ee_pose(self, group_id: PlanningGroupID | None = None) -> Pose | None: + def get_ee_pose(self, group_id: PlanningGroupID | None = None) -> PoseStamped | None: """Get a planning group's current tip pose.""" if self._world_monitor: try: @@ -812,7 +820,7 @@ def generate_plan_to_joint_targets( def generate_plan_to_pose_targets( self, - pose_targets: Mapping[PlanningGroupID, Pose], + pose_targets: Mapping[PlanningGroupID, PoseStamped], auxiliary_groups: Sequence[PlanningGroupID] = (), speed_scale: float | None = None, ) -> GeneratedPlan | None: @@ -822,14 +830,7 @@ def generate_plan_to_pose_targets( if not pose_targets: self._fail("At least one pose target is required") return None - stamped_targets = { - group_id: PoseStamped( - frame_id="world", - position=pose.position, - orientation=pose.orientation, - ) - for group_id, pose in pose_targets.items() - } + stamped_targets = dict(pose_targets) auxiliary_ids = tuple(auxiliary_groups) group_ids = tuple(dict.fromkeys((*stamped_targets.keys(), *auxiliary_ids))) planning = self._begin_group_planning(speed_scale) @@ -927,12 +928,21 @@ def move_linear( plan_result = PlanResult(PlanStatus.AMBIGUOUS_GROUP, group.message) return MoveResult(plan_result, None, delta, check_collision) resolved_speed = self.config.linear_speed_scale if speed_scale is None else speed_scale - relative = Transform( - translation=Vector3(*delta), - rotation=Quaternion(0.0, 0.0, 0.0, 1.0), + relative = TransformStamped( + header=Header(frame_id="world"), + transform=Transform( + translation=Vector3(x=delta[0], y=delta[1], z=delta[2]), + rotation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0), + ), + child_frame_id="", ) plan = self.generate_cartesian_plan( - {group.id: (Transform.identity(), relative)}, + { + group.id: ( + TransformStamped(header=Header(frame_id="world"), child_frame_id=""), + relative, + ) + }, CartesianPathConfig(), speed_scale=resolved_speed, check_collision=check_collision, @@ -1227,7 +1237,10 @@ def add_obstacle( obstacle = Obstacle( name=name, obstacle_type=obstacle_type, - pose=PoseStamped(position=pose.position, orientation=pose.orientation), + pose=PoseStamped( + header=Header(frame_id=""), + pose=Pose(position=pose.position, orientation=pose.orientation), + ), dimensions=tuple(dimensions) if dimensions else (), mesh_path=mesh_path, ) @@ -1262,7 +1275,10 @@ def update_obstacle( obstacle = Obstacle( name=name, obstacle_type=obstacle_type, - pose=PoseStamped(position=pose.position, orientation=pose.orientation), + pose=PoseStamped( + header=Header(frame_id=""), + pose=Pose(position=pose.position, orientation=pose.orientation), + ), dimensions=tuple(dimensions) if dimensions else (), color=rgba, mesh_path=mesh_path, @@ -1283,17 +1299,17 @@ def _apply_voxel_map(self, cloud: PointCloud2) -> None: if self._world_monitor is None: return frame = self.config.world_frame - if cloud.frame_id != frame: + if cloud.header.frame_id != frame: # The points are metric positions in the planning frame. Registering # them through a guessed transform would invent geometry. logger.warning( "Voxel map is in frame '%s', not the planning frame '%s'; dropped a map.", - cloud.frame_id, + cloud.header.frame_id, frame, ) return - points = cloud.points_f32() + points = pointcloud_xyz(cloud) if not len(points): # An empty map is how a mapper says the space it owns is now clear. self._world_monitor.remove_obstacle(VOXEL_MAP_OBSTACLE_ID) @@ -1305,7 +1321,7 @@ def _apply_voxel_map(self, cloud: PointCloud2) -> None: obstacle = Obstacle( name=VOXEL_MAP_OBSTACLE_ID, obstacle_type=ObstacleType.OCTREE, - pose=PoseStamped(frame_id=frame), + pose=PoseStamped(header=Header(frame_id=frame), pose=Pose()), points=tuple(map(tuple, points.tolist())), octree_resolution=self.config.voxel_map_resolution, ) @@ -1330,7 +1346,10 @@ def update_obstacle_pose(self, name: str, pose: Pose) -> bool: return False return self._world_monitor.update_obstacle_pose( name, - PoseStamped(position=pose.position, orientation=pose.orientation), + PoseStamped( + header=Header(frame_id=""), + pose=Pose(position=pose.position, orientation=pose.orientation), + ), ) @rpc diff --git a/dimos/manipulation/manipulation_skills.py b/dimos/manipulation/manipulation_skills.py index 1d8ac303701..ffbd309d51d 100644 --- a/dimos/manipulation/manipulation_skills.py +++ b/dimos/manipulation/manipulation_skills.py @@ -16,6 +16,10 @@ from __future__ import annotations +from dimos_generated.geometry_msgs.msg import Point, Pose, PoseStamped +from dimos_generated.sensor_msgs.msg import JointState +from dimos_generated.std_msgs.msg import Header + from dimos.agents.annotation import skill from dimos.agents.capabilities import CAP_MOVEMENT from dimos.agents.skill_result import SkillResult @@ -29,10 +33,7 @@ ) from dimos.manipulation.planning.spec.models import PlanningGroupID from dimos.manipulation.skill_errors import ManipulationSkillError -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.msgs.sensor_msgs.JointState import JointState +from dimos.msgs.geometry import quaternion_euler, quaternion_from_euler class ManipulationSkills(Module): @@ -157,20 +158,17 @@ def move_to_pose( if current is None: return SkillResult.fail("INVALID_STATE", "End-effector pose is unavailable") if roll is None and pitch is None and yaw is None: - orientation = current.orientation + orientation = current.pose.orientation else: - euler = current.orientation.to_euler() - orientation = Quaternion.from_euler( - Vector3( - euler.x if roll is None else roll, - euler.y if pitch is None else pitch, - euler.z if yaw is None else yaw, - ) + euler = quaternion_euler(current.pose.orientation) + orientation = quaternion_from_euler( + euler[0] if roll is None else roll, + euler[1] if pitch is None else pitch, + euler[2] if yaw is None else yaw, ) target = PoseStamped( - frame_id="world", - position=Vector3(x, y, z), - orientation=orientation, + header=Header(frame_id="world"), + pose=Pose(position=Point(x=x, y=y, z=z), orientation=orientation), ) plan = self.manipulation.plan_to_poses({group_id: target}) if failure := self._planning_result(plan): diff --git a/dimos/manipulation/manipulation_spec.py b/dimos/manipulation/manipulation_spec.py index 9c5e0307dcd..c77ebc0a1d6 100644 --- a/dimos/manipulation/manipulation_spec.py +++ b/dimos/manipulation/manipulation_spec.py @@ -21,12 +21,13 @@ from enum import Enum, auto from typing import Protocol +from dimos_generated.dimos_msgs.msg import GraspCandidateArray, TrajectoryStatus +from dimos_generated.geometry_msgs.msg import PoseStamped +from dimos_generated.sensor_msgs.msg import JointState + from dimos.control.tasks.trajectory_task.trajectory_task import TrajectoryExecutionResult from dimos.manipulation.planning.spec.models import GeneratedPlan, PlanningGroupID -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.manipulation_msgs.GraspCandidateArray import GraspCandidateArray -from dimos.msgs.sensor_msgs.JointState import JointState -from dimos.msgs.trajectory_msgs.TrajectoryStatus import TrajectoryStatus +from dimos.msgs.trajectory import trajectory_duration from dimos.spec.utils import Spec @@ -108,11 +109,20 @@ class PlanningGroupState: joint_presets: Mapping[str, JointState] = field(default_factory=dict) def __repr__(self) -> str: + joints = ( + None + if self.joints is None + else dict(zip(self.joints.name, self.joints.position, strict=True)) + ) + presets = { + name: dict(zip(state.name, state.position, strict=True)) + for name, state in self.joint_presets.items() + } return ( - f"PlanningGroupState(joints={self.joints!r}, " + f"PlanningGroupState(joints={joints!r}, " f"end_effector_pose={self.end_effector_pose!r}, " f"gripper_position={self.gripper_position!r}, " - f"joint_presets={dict(self.joint_presets)!r})" + f"joint_presets={presets!r})" ) @@ -153,7 +163,7 @@ def __repr__(self) -> str: return f"PlanResult({self.status.name}, message={self.message!r})" return ( f"PlanResult({self.status.name}, groups={self.plan.group_ids!r}, " - f"waypoints={len(self.plan.path)}, duration={self.plan.trajectory.duration:.3g}s, " + f"waypoints={len(self.plan.path)}, duration={trajectory_duration(self.plan.trajectory):.3g}s, " f"path_length={self.plan.path_length:.3g}, planning_time={self.plan.planning_time:.3g}s, " f"iterations={self.plan.iterations}, message={self.message!r})" ) diff --git a/dimos/manipulation/pick_and_place_module.py b/dimos/manipulation/pick_and_place_module.py index 549a27cac28..d9f07eaf028 100644 --- a/dimos/manipulation/pick_and_place_module.py +++ b/dimos/manipulation/pick_and_place_module.py @@ -16,8 +16,12 @@ from __future__ import annotations +import math from typing import Any, Literal +from dimos_generated.dimos_msgs.msg import GraspCandidateArray +from dimos_generated.geometry_msgs.msg import Point, Pose, PoseStamped, Vector3 +from dimos_generated.std_msgs.msg import Header from pydantic import Field from dimos.agents.annotation import skill @@ -36,10 +40,12 @@ from dimos.manipulation.manipulation_spec import ManipulationSpec from dimos.manipulation.planning.spec.models import PlanningGroupID from dimos.manipulation.skill_errors import ManipulationSkillError -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.msgs.manipulation_msgs.GraspCandidateArray import GraspCandidateArray +from dimos.msgs.geometry import ( + pose_matrix, + quaternion_euler, + quaternion_from_euler, + translate_pose_local, +) from dimos.perception.experimental.object_scene_registration_spec import ObjectSceneRegistrationSpec @@ -96,7 +102,7 @@ def scan_objects(self, prompts: list[str]) -> SkillResult[ManipulationSkillError ] self._objects = {str(obj["object_id"]): obj for obj in objects if "object_id" in obj} return SkillResult.ok( - f"Detected {detections.detections_length} object(s)", + f"Detected {len(detections.detections)} object(s)", prompts=prompts, objects=list(self._objects.values()), ) @@ -129,6 +135,10 @@ def pick_object( "OBJECT_NOT_DETECTED", f"No pointcloud for object_id: {object_id}" ) candidates = self._grasp_generator.propose_grasps(pointcloud) + for candidate in candidates.candidates: + if not math.isfinite(candidate.score): + raise ValueError("GraspCandidate.score must be finite") + pose_matrix(candidate.pose) except (RuntimeError, ValueError) as exc: return SkillResult.fail("GRASP_GENERATION_FAILED", str(exc)) self._grasp_candidates = candidates @@ -149,13 +159,15 @@ def pick_object( return failure unreachable: SkillResult[ManipulationSkillError] | None = None - for rank, candidate in enumerate(candidates.candidates[: self.config.max_grasp_attempts]): + for rank, candidate in enumerate( + list(candidates.candidates)[: self.config.max_grasp_attempts] + ): grasp = self._apply_yaw_policy( PoseStamped( - ts=candidates.header.timestamp, - frame_id=candidates.header.frame_id, - position=candidate.pose.position, - orientation=candidate.pose.orientation, + header=candidates.header, + pose=Pose( + position=candidate.pose.position, orientation=candidate.pose.orientation + ), ), group, ) @@ -215,9 +227,10 @@ def place_at( "ROBOT_NOT_FOUND", "Gripper-capable planning group is missing or ambiguous" ) place = PoseStamped( - frame_id=self.config.planning_frame, - position=Vector3(x, y, z), - orientation=self._selected_grasp.orientation, + header=Header(frame_id=self.config.planning_frame), + pose=Pose( + position=Point(x=x, y=y, z=z), orientation=self._selected_grasp.pose.orientation + ), ) preplace = self._offset_pose(place, self.config.pregrasp_offset) if failure := self._move(preplace, group): @@ -252,22 +265,21 @@ def _apply_yaw_policy(self, pose: PoseStamped, group: PlanningGroupID) -> PoseSt current = self._manipulation.get_state().groups[group].end_effector_pose if current is None: return pose - euler = pose.orientation.to_euler() - current_euler = current.orientation.to_euler() + euler = quaternion_euler(pose.pose.orientation) + current_euler = quaternion_euler(current.pose.orientation) return PoseStamped( - ts=pose.ts, - frame_id=pose.frame_id, - position=pose.position, - orientation=Quaternion.from_euler(Vector3(euler.x, euler.y, current_euler.z)), + header=pose.header, + pose=Pose( + position=pose.pose.position, + orientation=quaternion_from_euler(euler[0], euler[1], current_euler[2]), + ), ) @staticmethod def _offset_pose(pose: PoseStamped, offset: float) -> PoseStamped: return PoseStamped( - ts=pose.ts, - frame_id=pose.frame_id, - position=pose.position + pose.orientation.rotate_vector(Vector3(0.0, 0.0, -offset)), - orientation=pose.orientation, + header=pose.header, + pose=translate_pose_local(pose.pose, Vector3(z=-offset)), ) def _servo( @@ -280,9 +292,9 @@ def _servo( rejected. This leg is short, straight, and deliberately ends in contact. """ result = self._manipulation.move_linear( - end.position.x - start.position.x, - end.position.y - start.position.y, - end.position.z - start.position.z, + end.pose.position.x - start.pose.position.x, + end.pose.position.y - start.pose.position.y, + end.pose.position.z - start.pose.position.z, planning_group, check_collision=False, ) diff --git a/dimos/manipulation/planning/README.md b/dimos/manipulation/planning/README.md index 3d997bebbb9..7b4182eb448 100644 --- a/dimos/manipulation/planning/README.md +++ b/dimos/manipulation/planning/README.md @@ -72,15 +72,15 @@ manual motion and explicit planning, preview, and execution through `arm.rpc`. from dimos.manipulation import ManipulationModule from dimos.manipulation.planning.groups.models import PlanningGroupDefinition from dimos.manipulation.planning.spec import RobotModelConfig -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.msgs.sensor_msgs.JointState import JointState +from dimos_generated.geometry_msgs.msg import PoseStamped +from dimos_generated.geometry_msgs.msg import Quaternion +from dimos_generated.geometry_msgs.msg import Vector3 +from dimos_generated.sensor_msgs.msg import JointState from dimos.robot.assets.model import RobotModel config = RobotModelConfig( model=RobotModel.from_file("/path/to/xarm7.urdf"), - base_pose=PoseStamped(position=Vector3(), orientation=Quaternion()), + base_pose=PoseStamped(), joint_names=["joint1", "joint2", "joint3", "joint4", "joint5", "joint6", "joint7"], base_link="link_base", planning_groups=[ diff --git a/dimos/manipulation/planning/groups/test_planning_groups.py b/dimos/manipulation/planning/groups/test_planning_groups.py index 84567f296b2..c93b5f8f331 100644 --- a/dimos/manipulation/planning/groups/test_planning_groups.py +++ b/dimos/manipulation/planning/groups/test_planning_groups.py @@ -17,6 +17,9 @@ from dataclasses import dataclass from pathlib import Path +from dimos_generated.builtin_interfaces.msg import Time +from dimos_generated.sensor_msgs.msg import JointState +from dimos_generated.std_msgs.msg import Header import pytest from dimos.manipulation.planning.groups.discovery import ( @@ -34,7 +37,6 @@ normalize_joint_target, ) from dimos.manipulation.planning.spec.config import RobotModelConfig -from dimos.msgs.sensor_msgs.JointState import JointState from dimos.robot.assets.model import JointDescription, RobotModel @@ -165,8 +167,8 @@ def test_normalize_joint_target_accepts_exact_or_unnamed_target() -> None: JointState(name=["left/j1", "left/j2"], position=[1.0, 2.0]), ) unnamed = normalize_joint_target(group, JointState(position=[3.0, 4.0])) - assert named.name == ["left/j1", "left/j2"] - assert unnamed.name == ["left/j1", "left/j2"] + assert list(named.name) == ["left/j1", "left/j2"] + assert list(unnamed.name) == ["left/j1", "left/j2"] with pytest.raises(ValueError, match="missing joints"): normalize_joint_target( group, @@ -177,10 +179,26 @@ def test_normalize_joint_target_accepts_exact_or_unnamed_target() -> None: def test_state_projection_requires_exact_canonical_names() -> None: state = JointState(name=["right/j1", "left/j2", "left/j1"], position=[3.0, 2.0, 1.0]) projected = filter_joint_state_to_selected_joints(state, ("left/j1", "right/j1")) - assert projected.name == ["left/j1", "right/j1"] - assert projected.position == [1.0, 3.0] + assert list(projected.name) == ["left/j1", "right/j1"] + assert list(projected.position) == [1.0, 3.0] assert joint_state_to_ordered_positions( state, joint_names=("left/j1", "left/j2", "right/j1") ).tolist() == [1.0, 2.0, 3.0] with pytest.raises(ValueError, match="missing"): filter_joint_state_to_selected_joints(state, ("missing",)) + + +def test_projection_and_target_normalization_copy_source_header(): + group = PlanningGroup("arm", ("arm/a", "arm/b"), "base", "tool") + source = JointState( + header=Header(stamp=Time(sec=-1, nanosec=999999999), frame_id="robot"), + name=["arm/b", "arm/a"], + position=[2.0, 1.0], + ) + projected = filter_joint_state_to_selected_joints(source, ["arm/a"]) + normalized = normalize_joint_target(group, source) + assert projected.header == normalized.header == source.header + assert list(projected.position) == [1.0] + assert list(normalized.position) == [1.0, 2.0] + source.header.frame_id = "changed" + assert projected.header.frame_id == normalized.header.frame_id == "robot" diff --git a/dimos/manipulation/planning/groups/utils.py b/dimos/manipulation/planning/groups/utils.py index cb59f885157..a92768aff10 100644 --- a/dimos/manipulation/planning/groups/utils.py +++ b/dimos/manipulation/planning/groups/utils.py @@ -16,11 +16,11 @@ from collections.abc import Sequence +from dimos_generated.sensor_msgs.msg import JointState import numpy as np from numpy.typing import NDArray from dimos.manipulation.planning.groups.models import PlanningGroup -from dimos.msgs.sensor_msgs.JointState import JointState def filter_joint_state_to_selected_joints( @@ -32,6 +32,7 @@ def filter_joint_state_to_selected_joints( if missing: raise ValueError(f"Joint state is missing selected joints: {missing}") return JointState( + header=joint_state.header, name=list(joint_names), position=[float(positions_by_name[name]) for name in joint_names], ) @@ -45,7 +46,9 @@ def normalize_joint_target(group: PlanningGroup, target: JointState) -> JointSta f"Target for '{group.id}' has {len(target.position)} positions, " f"expected {len(group.joint_names)}" ) - return JointState(name=list(group.joint_names), position=list(target.position)) + return JointState( + header=target.header, name=list(group.joint_names), position=list(target.position) + ) if len(target.name) != len(target.position): raise ValueError( f"Target for '{group.id}' has {len(target.name)} names but " @@ -59,6 +62,7 @@ def normalize_joint_target(group: PlanningGroup, target: JointState) -> JointSta if extra: raise ValueError(f"Target for '{group.id}' has extra joints: {sorted(extra)}") return JointState( + header=target.header, name=list(group.joint_names), position=[float(positions[name]) for name in group.joint_names], ) diff --git a/dimos/manipulation/planning/kinematics/drake_optimization_ik.py b/dimos/manipulation/planning/kinematics/drake_optimization_ik.py index e65afde96aa..80a4f9ee35b 100644 --- a/dimos/manipulation/planning/kinematics/drake_optimization_ik.py +++ b/dimos/manipulation/planning/kinematics/drake_optimization_ik.py @@ -19,6 +19,8 @@ from collections.abc import Mapping, Sequence from typing import TYPE_CHECKING, cast +from dimos_generated.geometry_msgs.msg import PoseStamped +from dimos_generated.sensor_msgs.msg import JointState import numpy as np from dimos.manipulation.planning.groups.models import PlanningGroup @@ -31,9 +33,7 @@ from dimos.manipulation.planning.spec.models import IKResult from dimos.manipulation.planning.spec.protocols import WorldSpec from dimos.manipulation.planning.utils.kinematics_utils import compute_pose_error -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.geometry_msgs.Transform import Transform -from dimos.msgs.sensor_msgs.JointState import JointState +from dimos.msgs.geometry import pose_matrix from dimos.utils.logging_config import setup_logger if TYPE_CHECKING: @@ -98,11 +98,8 @@ def solve( "DrakeOptimizationIK requires exactly one pose-targetable planning group for legacy solve()", ) - # Convert PoseStamped to 4x4 matrix via Transform - target_matrix = Transform( - translation=target_pose.position, - rotation=target_pose.orientation, - ).to_matrix() + # Convert PoseStamped to 4x4 matrix via TransformStamped + target_matrix = pose_matrix(target_pose.pose) # Get joint limits lower_limits, upper_limits = world.get_prepared_model().joint_space.position_limits() @@ -113,7 +110,7 @@ def solve( seed = world.get_joint_state(ctx) # Extract joint names and seed positions - joint_names = seed.name + joint_names = list(seed.name) seed_positions = np.array(seed.position, dtype=np.float64) # Target transform @@ -201,10 +198,7 @@ def solve_pose_targets( ) lower_limits, upper_limits = world.get_prepared_model().joint_space.position_limits() - target_matrix = Transform( - translation=request.target_pose.position, - rotation=request.target_pose.orientation, - ).to_matrix() + target_matrix = pose_matrix(request.target_pose.pose) target_transform = RigidTransform(target_matrix) locked_positions = { index: float(request.seed_positions[index]) @@ -328,7 +322,7 @@ def _solve_single( joint_solution = np.clip(joint_solution, lower_limits, upper_limits) # Compute actual error using FK - solution_state = JointState({"name": joint_names, "position": joint_solution.tolist()}) + solution_state = JointState(name=joint_names, position=joint_solution.tolist()) with world.scratch_context() as ctx: world.set_joint_state(ctx, solution_state) actual_matrix = world.get_link_pose(ctx, target_frame_name) @@ -356,7 +350,7 @@ def _create_success_result( ) -> IKResult: return IKResult( status=IKStatus.SUCCESS, - joint_state=JointState({"name": joint_names, "position": joint_positions.tolist()}), + joint_state=JointState(name=joint_names, position=joint_positions.tolist()), position_error=position_error, orientation_error=orientation_error, iterations=iterations, diff --git a/dimos/manipulation/planning/kinematics/jacobian_ik.py b/dimos/manipulation/planning/kinematics/jacobian_ik.py index 07029aeca99..fd37e8b44bb 100644 --- a/dimos/manipulation/planning/kinematics/jacobian_ik.py +++ b/dimos/manipulation/planning/kinematics/jacobian_ik.py @@ -42,17 +42,14 @@ compute_pose_error, damped_pseudoinverse, ) +from dimos.msgs.geometry import pose_matrix from dimos.utils.logging_config import setup_logger -from dimos.utils.transform_utils import pose_to_matrix if TYPE_CHECKING: from numpy.typing import NDArray -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.geometry_msgs.Transform import Transform -from dimos.msgs.geometry_msgs.Twist import Twist -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.msgs.sensor_msgs.JointState import JointState +from dimos_generated.geometry_msgs.msg import PoseStamped, Twist, Vector3 +from dimos_generated.sensor_msgs.msg import JointState logger = setup_logger() @@ -136,7 +133,7 @@ def solve( seed = world.get_joint_state(ctx) # Extract joint names for creating random seeds - joint_names = seed.name + joint_names = list(seed.name) best_result: IKResult | None = None best_error = float("inf") @@ -148,9 +145,7 @@ def solve( else: # Random seed within joint limits random_positions = np.random.uniform(lower_limits, upper_limits) - current_seed = JointState( - {"name": joint_names, "position": random_positions.tolist()} - ) + current_seed = JointState(name=joint_names, position=random_positions.tolist()) # Solve iterative IK result = self.solve_iterative( @@ -215,9 +210,7 @@ def solve_pose_targets( if request is None: return _create_failure_result(IKStatus.NO_SOLUTION, "Invalid pose target request") - full_seed = JointState( - {"name": request.joint_names, "position": request.seed_positions.tolist()} - ) + full_seed = JointState(name=request.joint_names, position=request.seed_positions.tolist()) result = self.solve_iterative( world=world, target_pose=request.target_pose, @@ -236,9 +229,7 @@ def solve_pose_targets( full_positions[request.group_indices] = np.asarray( result.joint_state.position, dtype=np.float64 ) - full_state = JointState( - {"name": request.joint_names, "position": full_positions.tolist()} - ) + full_state = JointState(name=request.joint_names, position=full_positions.tolist()) if not world.check_config_collision_free(full_state): return _create_failure_result(IKStatus.COLLISION, "IK solution is in collision") return result @@ -271,12 +262,9 @@ def solve_iterative( IKResult with solution or failure status """ # Convert to internal representation - target_matrix = Transform( - translation=target_pose.position, - rotation=target_pose.orientation, - ).to_matrix() + target_matrix = pose_matrix(target_pose.pose) current_joints = np.array(seed.position, dtype=np.float64) - joint_names = seed.name + joint_names = list(seed.name) active_joint_indices = active_joint_indices or list(range(len(joint_names))) result_joint_names = list(group.joint_names) if group is not None else joint_names @@ -286,15 +274,13 @@ def solve_iterative( for iteration in range(max_iterations): with world.scratch_context() as ctx: # Set current position (convert to JointState for API) - current_state = JointState( - {"name": joint_names, "position": current_joints.tolist()} - ) + current_state = JointState(name=joint_names, position=current_joints.tolist()) world.set_joint_state(ctx, current_state) if group is None: - current_pose = pose_to_matrix(world.get_ee_pose(ctx)) + current_pose = pose_matrix(world.get_ee_pose(ctx).pose) else: - current_pose = pose_to_matrix(world.get_group_ee_pose(ctx, group.id)) + current_pose = pose_matrix(world.get_group_ee_pose(ctx, group.id).pose) # Compute error pos_error, ori_error = compute_pose_error(current_pose, target_matrix) @@ -345,12 +331,12 @@ def solve_iterative( # Compute final error with world.scratch_context() as ctx: - final_state = JointState({"name": joint_names, "position": current_joints.tolist()}) + final_state = JointState(name=joint_names, position=current_joints.tolist()) world.set_joint_state(ctx, final_state) if group is None: - final_pose = pose_to_matrix(world.get_ee_pose(ctx)) + final_pose = pose_matrix(world.get_ee_pose(ctx).pose) else: - final_pose = pose_to_matrix(world.get_group_ee_pose(ctx, group.id)) + final_pose = pose_matrix(world.get_group_ee_pose(ctx, group.id).pose) pos_error, ori_error = compute_pose_error(final_pose, target_matrix) return _create_failure_result( @@ -416,7 +402,7 @@ def solve_differential( if max_ratio > 1.0: q_dot = q_dot / max_ratio - return JointState({"name": joint_names, "velocity": q_dot.tolist()}) + return JointState(name=joint_names, velocity=q_dot.tolist()) def solve_differential_position_only( self, @@ -462,7 +448,7 @@ def solve_differential_position_only( # Compute joint velocities q_dot = J_pinv @ vel_array - return JointState({"name": joint_names, "velocity": q_dot.tolist()}) + return JointState(name=joint_names, velocity=q_dot.tolist()) # Result Helpers @@ -478,7 +464,7 @@ def _create_success_result( """Create a successful IK result.""" return IKResult( status=IKStatus.SUCCESS, - joint_state=JointState({"name": joint_names, "position": joint_positions.tolist()}), + joint_state=JointState(name=joint_names, position=joint_positions.tolist()), position_error=position_error, orientation_error=orientation_error, iterations=iterations, diff --git a/dimos/manipulation/planning/kinematics/pink_ik.py b/dimos/manipulation/planning/kinematics/pink_ik.py index c182ec2b66c..667a562cefc 100644 --- a/dimos/manipulation/planning/kinematics/pink_ik.py +++ b/dimos/manipulation/planning/kinematics/pink_ik.py @@ -20,6 +20,8 @@ from dataclasses import replace from typing import TYPE_CHECKING +from dimos_generated.geometry_msgs.msg import PoseStamped +from dimos_generated.sensor_msgs.msg import JointState import numpy as np import pink from pink.exceptions import NoSolutionFound @@ -41,8 +43,6 @@ from dimos.manipulation.planning.spec.models import IKResult from dimos.manipulation.planning.spec.protocols import WorldSpec from dimos.manipulation.planning.utils.kinematics_utils import compute_pose_error -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.sensor_msgs.JointState import JointState if TYPE_CHECKING: from numpy.typing import NDArray @@ -408,7 +408,7 @@ def _success( ) -> IKResult: return IKResult( status=IKStatus.SUCCESS, - joint_state=JointState({"name": joint_names, "position": joint_positions.tolist()}), + joint_state=JointState(name=joint_names, position=joint_positions.tolist()), position_error=position_error, orientation_error=orientation_error, iterations=iterations, @@ -430,10 +430,7 @@ def _selected_result(result: IKResult, selected_names: Sequence[str]) -> IKResul return IKResult( status=result.status, joint_state=JointState( - { - "name": list(selected_names), - "position": [float(positions[name]) for name in selected_names], - } + name=list(selected_names), position=[float(positions[name]) for name in selected_names] ), position_error=result.position_error, orientation_error=result.orientation_error, diff --git a/dimos/manipulation/planning/kinematics/pink_solver.py b/dimos/manipulation/planning/kinematics/pink_solver.py index fb21dad90dd..bb8f01a0997 100644 --- a/dimos/manipulation/planning/kinematics/pink_solver.py +++ b/dimos/manipulation/planning/kinematics/pink_solver.py @@ -33,15 +33,16 @@ ) raise ImportError(msg) from exc +from dimos_generated.geometry_msgs.msg import PoseStamped +from dimos_generated.sensor_msgs.msg import JointState + from dimos.manipulation.planning.kinematics.config import PinkKinematicsConfig from dimos.manipulation.planning.spec.config import RobotModelConfig from dimos.manipulation.planning.spec.joint_space import CoordinateTopology, JointSpace from dimos.manipulation.planning.spec.validation import PreparedRobotModel from dimos.manipulation.planning.utils.mesh_utils import prepare_urdf_for_drake -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.sensor_msgs.JointState import JointState +from dimos.msgs.geometry import pose_matrix from dimos.utils.logging_config import setup_logger -from dimos.utils.transform_utils import pose_to_matrix if TYPE_CHECKING: from numpy.typing import NDArray @@ -314,8 +315,8 @@ def _current_frame_matrix( def _target_in_model_frame( self, config: RobotModelConfig, target_pose: PoseStamped ) -> NDArray[np.float64]: - target_world = pose_to_matrix(target_pose) - base_world = pose_to_matrix(config.base_pose) + target_world = pose_matrix(target_pose.pose) + base_world = pose_matrix(config.base_pose.pose) target_model: NDArray[np.float64] = np.asarray( np.linalg.inv(base_world) @ target_world, dtype=np.float64 ) diff --git a/dimos/manipulation/planning/kinematics/pinocchio_ik.py b/dimos/manipulation/planning/kinematics/pinocchio_ik.py index 79eb831eef1..cb8d64526c5 100644 --- a/dimos/manipulation/planning/kinematics/pinocchio_ik.py +++ b/dimos/manipulation/planning/kinematics/pinocchio_ik.py @@ -35,6 +35,7 @@ from pathlib import Path from typing import TYPE_CHECKING, Any +from dimos_generated.geometry_msgs.msg import PoseStamped import numpy as np from numpy.linalg import norm, solve import pinocchio @@ -42,11 +43,9 @@ from dimos.utils.logging_config import setup_logger if TYPE_CHECKING: + from dimos_generated.geometry_msgs.msg import Pose from numpy.typing import NDArray - from dimos.msgs.geometry_msgs.Pose import Pose - from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped - logger = setup_logger() @@ -211,8 +210,9 @@ def forward_kinematics(self, joint_positions: NDArray[np.floating[Any]]) -> pino def pose_to_se3(pose: Pose | PoseStamped) -> pinocchio.SE3: """Convert Pose or PoseStamped to pinocchio SE3""" - position = np.array([pose.x, pose.y, pose.z]) - quat = pose.orientation + value = pose.pose if isinstance(pose, PoseStamped) else pose + position = np.array([value.position.x, value.position.y, value.position.z]) + quat = value.orientation rotation = pinocchio.Quaternion(quat.w, quat.x, quat.y, quat.z).toRotationMatrix() return pinocchio.SE3(rotation, position) diff --git a/dimos/manipulation/planning/kinematics/test_drake_optimization_ik_selection.py b/dimos/manipulation/planning/kinematics/test_drake_optimization_ik_selection.py index 51133160860..c1a4b6354ed 100644 --- a/dimos/manipulation/planning/kinematics/test_drake_optimization_ik_selection.py +++ b/dimos/manipulation/planning/kinematics/test_drake_optimization_ik_selection.py @@ -17,6 +17,9 @@ from contextlib import contextmanager from pathlib import Path +from dimos_generated.geometry_msgs.msg import Pose, PoseStamped +from dimos_generated.sensor_msgs.msg import JointState +from dimos_generated.std_msgs.msg import Header import numpy as np from dimos.manipulation.planning.groups.models import PlanningGroup @@ -31,8 +34,6 @@ ) from dimos.manipulation.planning.spec.models import IKResult from dimos.manipulation.planning.spec.validation import PreparedRobotModel -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.sensor_msgs.JointState import JointState from dimos.robot.assets.model import LoadedRobotModel, RobotModel @@ -43,10 +44,8 @@ def __init__(self) -> None: joint_names=["arm/base", "arm/shoulder", "arm/elbow", "arm/wrist"], ) self.current_state = JointState( - { - "name": ["arm/base", "arm/shoulder", "arm/elbow", "arm/wrist"], - "position": [1.0, 2.0, 3.0, 4.0], - } + name=["arm/base", "arm/shoulder", "arm/elbow", "arm/wrist"], + position=[1.0, 2.0, 3.0, 4.0], ) self.collision_checked_state: JointState | None = None self.prepared = PreparedRobotModel( @@ -96,10 +95,8 @@ def fake_solve_single(self, **kwargs) -> IKResult: return IKResult( status=IKStatus.SUCCESS, joint_state=JointState( - { - "name": ["arm/base", "arm/shoulder", "arm/elbow", "arm/wrist"], - "position": [10.0, 20.0, 30.0, 40.0], - } + name=["arm/base", "arm/shoulder", "arm/elbow", "arm/wrist"], + position=[10.0, 20.0, 30.0, 40.0], ), position_error=0.0, orientation_error=0.0, @@ -110,8 +107,8 @@ def fake_solve_single(self, **kwargs) -> IKResult: result = DrakeOptimizationIK().solve_pose_targets( world=world, # type: ignore[arg-type] - pose_targets={group: PoseStamped()}, - seed=JointState({"name": ["arm/shoulder"], "position": [22.0]}), + pose_targets={group: PoseStamped(header=Header(frame_id=""), pose=Pose())}, + seed=JointState(name=["arm/shoulder"], position=[22.0]), check_collision=False, max_attempts=1, ) diff --git a/dimos/manipulation/planning/kinematics/test_jacobian_ik_selection.py b/dimos/manipulation/planning/kinematics/test_jacobian_ik_selection.py index 8f8b000da8d..e75e7551b61 100644 --- a/dimos/manipulation/planning/kinematics/test_jacobian_ik_selection.py +++ b/dimos/manipulation/planning/kinematics/test_jacobian_ik_selection.py @@ -19,6 +19,9 @@ from contextlib import nullcontext from pathlib import Path +from dimos_generated.geometry_msgs.msg import Point, Pose, PoseStamped, Quaternion +from dimos_generated.sensor_msgs.msg import JointState +from dimos_generated.std_msgs.msg import Header import numpy as np from dimos.manipulation.planning.groups.models import PlanningGroup, PlanningGroupDefinition @@ -31,15 +34,16 @@ JointSpace, ) from dimos.manipulation.planning.spec.validation import PreparedRobotModel -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.msgs.sensor_msgs.JointState import JointState from dimos.robot.assets.model import LoadedRobotModel, RobotModel def _pose(x: float = 0.0) -> PoseStamped: - return PoseStamped(position=Vector3(x, 0.0, 0.0), orientation=Quaternion(0.0, 0.0, 0.0, 1.0)) + return PoseStamped( + header=Header(frame_id=""), + pose=Pose( + position=Point(x=x, y=0.0, z=0.0), orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0) + ), + ) def _group(tip_link: str | None = "tool") -> PlanningGroup: @@ -93,7 +97,7 @@ def scratch_context(self) -> nullcontext[None]: def get_joint_state(self, ctx: object) -> JointState: return JointState( - {"name": ["arm/joint_a", "arm/joint_b", "arm/gripper"], "position": [0.0, 0.0, 0.9]} + name=["arm/joint_a", "arm/joint_b", "arm/gripper"], position=[0.0, 0.0, 0.9] ) def set_joint_state(self, ctx: object, joint_state: JointState) -> None: @@ -117,7 +121,7 @@ def test_solve_pose_targets_filters_to_group_and_uses_group_world_methods() -> N world=world, pose_targets={_group(): _pose()}, seed=JointState( - {"name": ["arm/joint_a", "arm/joint_b", "arm/gripper"], "position": [0.0, 0.0, 0.9]} + name=["arm/joint_a", "arm/joint_b", "arm/gripper"], position=[0.0, 0.0, 0.9] ), max_attempts=1, ) diff --git a/dimos/manipulation/planning/kinematics/test_pink_ik.py b/dimos/manipulation/planning/kinematics/test_pink_ik.py index 467f7e469f9..b2370c81485 100644 --- a/dimos/manipulation/planning/kinematics/test_pink_ik.py +++ b/dimos/manipulation/planning/kinematics/test_pink_ik.py @@ -24,6 +24,9 @@ from types import MappingProxyType, ModuleType, SimpleNamespace from typing import Any, cast +from dimos_generated.geometry_msgs.msg import Point, Pose, PoseStamped, Quaternion +from dimos_generated.sensor_msgs.msg import JointState +from dimos_generated.std_msgs.msg import Header import numpy as np from pink.exceptions import NoSolutionFound import pytest @@ -64,12 +67,8 @@ ) from dimos.manipulation.planning.spec.models import IKResult from dimos.manipulation.planning.spec.validation import PreparedRobotModel, prepare_robot_model -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.msgs.sensor_msgs.JointState import JointState +from dimos.msgs.geometry import pose_from_matrix from dimos.robot.assets.model import LoadedRobotModel, PlanarBaseDefinition, RobotModel -from dimos.utils.transform_utils import matrix_to_pose _TRACKING_ERROR_RAD = np.deg2rad(10.0) @@ -396,7 +395,10 @@ def _install_fake_modules(mocker: MockerFixture, converge: bool = True) -> _Fake def _robot_config() -> RobotModelConfig: return RobotModelConfig( model=RobotModel.from_file(Path("/tmp/fake.urdf")), - base_pose=PoseStamped(position=Vector3(), orientation=Quaternion(0.0, 0.0, 0.0, 1.0)), + base_pose=PoseStamped( + header=Header(frame_id=""), + pose=Pose(position=Point(), orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0)), + ), joint_names=["joint_a", "joint_b", "joint_c"], base_link="base", planning_groups=[ @@ -548,7 +550,7 @@ def scratch_context(self) -> nullcontext[None]: def get_joint_state(self, ctx: object) -> JointState: self.joint_state_calls += 1 - return JointState({"name": ["joint_b", "joint_c", "joint_a"], "position": [0.0, 0.0, 0.0]}) + return JointState(name=["joint_b", "joint_c", "joint_a"], position=[0.0, 0.0, 0.0]) def get_joint_limits(self) -> tuple[np.ndarray, np.ndarray]: return np.array([-1.0, -1.0, -1.0]), np.array([1.0, 1.0, 1.0]) @@ -775,7 +777,7 @@ def apply_increment(**kwargs: Any) -> None: commands.append( ik.step_frame_targets( robot_model=_robot_config(), - frame_targets={"tool": PoseStamped()}, + frame_targets={"tool": PoseStamped(header=Header(frame_id=""), pose=Pose())}, controlled_joints=["joint_a"], command_state=commands[-1], measured_state=initial, @@ -816,7 +818,7 @@ def apply_increment(**kwargs: Any) -> None: commands.append( ik.step_frame_targets( robot_model=_robot_config(), - frame_targets={"tool": PoseStamped()}, + frame_targets={"tool": PoseStamped(header=Header(frame_id=""), pose=Pose())}, controlled_joints=["joint_a"], command_state=commands[-1], measured_state=initial, @@ -853,7 +855,7 @@ def apply_increment(**kwargs: Any) -> None: initial = JointState(name=["joint_a"], position=[0.0]) first = ik.step_frame_targets( robot_model=_robot_config(), - frame_targets={"tool": PoseStamped()}, + frame_targets={"tool": PoseStamped(header=Header(frame_id=""), pose=Pose())}, controlled_joints=["joint_a"], command_state=initial, measured_state=initial, @@ -865,7 +867,7 @@ def apply_increment(**kwargs: Any) -> None: second = ik.step_frame_targets( robot_model=_robot_config(), - frame_targets={"tool": PoseStamped()}, + frame_targets={"tool": PoseStamped(header=Header(frame_id=""), pose=Pose())}, controlled_joints=["joint_a"], command_state=first, measured_state=JointState(name=["joint_a"], position=[-0.1]), @@ -875,8 +877,8 @@ def apply_increment(**kwargs: Any) -> None: joint_command_filter_cutoff_hz=5.0, ) - assert first.position == pytest.approx([0.2]) - assert second.position == pytest.approx([0.05]) + assert list(first.position) == pytest.approx([0.2]) + assert list(second.position) == pytest.approx([0.05]) def test_step_frame_targets_preserves_controlled_joint_order( @@ -893,8 +895,11 @@ def test_step_frame_targets_preserves_controlled_joint_order( robot_model=_robot_config(), frame_targets={ "tool": PoseStamped( - position=Vector3(0.1, 0.2, 0.3), - orientation=Quaternion(0.0, 0.0, 0.0, 1.0), + header=Header(frame_id=""), + pose=Pose( + position=Point(x=0.1, y=0.2, z=0.3), + orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0), + ), ) }, controlled_joints=["joint_c", "joint_a"], @@ -905,7 +910,7 @@ def test_step_frame_targets_preserves_controlled_joint_order( ) assert result.name == ["joint_c", "joint_a"] - assert result.position == pytest.approx([0.1, 0.08]) + assert list(result.position) == pytest.approx([0.1, 0.08]) def test_step_frame_targets_builds_both_frame_tasks_with_tuning( @@ -932,12 +937,15 @@ def test_step_frame_targets_builds_both_frame_tasks_with_tuning( robot_model=_robot_config(), frame_targets={ "tool": PoseStamped( - position=Vector3(0.1, 0.2, 0.3), - orientation=Quaternion(0.0, 0.0, 0.0, 1.0), + header=Header(frame_id=""), + pose=Pose( + position=Point(x=0.1, y=0.2, z=0.3), + orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0), + ), ), "base": PoseStamped( - position=Vector3(), - orientation=Quaternion(0.0, 0.0, 0.0, 1.0), + header=Header(frame_id=""), + pose=Pose(position=Point(), orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0)), ), }, controlled_joints=["joint_a", "joint_b", "joint_c"], @@ -1009,12 +1017,16 @@ def test_streaming_reuses_task_stack_across_steps( mocker.patch.object(ik, "_get_control_context", return_value=context) create_tasks = mocker.spy(ik, "_create_tasks") first_target = PoseStamped( - position=Vector3(0.1, 0.2, 0.3), - orientation=Quaternion(0.0, 0.0, 0.0, 1.0), + header=Header(frame_id=""), + pose=Pose( + position=Point(x=0.1, y=0.2, z=0.3), orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0) + ), ) second_target = PoseStamped( - position=Vector3(0.3, 0.2, 0.1), - orientation=Quaternion(0.0, 0.0, 0.0, 1.0), + header=Header(frame_id=""), + pose=Pose( + position=Point(x=0.3, y=0.2, z=0.1), orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0) + ), ) ik.step_frame_targets( @@ -1053,8 +1065,11 @@ def test_task_hooks_receive_read_only_stack_and_successful_velocity( _robot_config(), { "tool": PoseStamped( - position=Vector3(0.1, 0.2, 0.3), - orientation=Quaternion(0.0, 0.0, 0.0, 1.0), + header=Header(frame_id=""), + pose=Pose( + position=Point(x=0.1, y=0.2, z=0.3), + orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0), + ), ) }, ["joint_a", "joint_b", "joint_c"], @@ -1082,7 +1097,7 @@ def test_after_solve_hook_is_not_called_when_solver_raises( with pytest.raises(RuntimeError, match="no solution"): ik.step_frame_targets( _robot_config(), - {"tool": PoseStamped()}, + {"tool": PoseStamped(header=Header(frame_id=""), pose=Pose())}, ["joint_a", "joint_b", "joint_c"], JointState( name=["joint_a", "joint_b", "joint_c"], @@ -1136,7 +1151,7 @@ def step_outside_limits(**kwargs: Any) -> None: result = ik.step_frame_targets( robot_model=_robot_config(), - frame_targets={"tool": PoseStamped()}, + frame_targets={"tool": PoseStamped(header=Header(frame_id=""), pose=Pose())}, controlled_joints=["joint_a", "joint_b", "joint_c"], command_state=JointState( name=["joint_a", "joint_b", "joint_c"], @@ -1149,7 +1164,7 @@ def step_outside_limits(**kwargs: Any) -> None: max_command_tracking_error_rad=_TRACKING_ERROR_RAD, ) - assert result.position == pytest.approx([-0.9999, 0.1, _TRACKING_ERROR_RAD]) + assert list(result.position) == pytest.approx([-0.9999, 0.1, _TRACKING_ERROR_RAD]) def test_step_frame_targets_rejects_feedback_beyond_tolerance( @@ -1166,7 +1181,7 @@ def test_step_frame_targets_rejects_feedback_beyond_tolerance( with pytest.raises(PinkJointLimitError, match="joint_a.*lower limit"): ik.step_frame_targets( robot_model=_robot_config(), - frame_targets={"tool": PoseStamped()}, + frame_targets={"tool": PoseStamped(header=Header(frame_id=""), pose=Pose())}, controlled_joints=["joint_a"], command_state=JointState(name=["joint_a"], position=[0.0]), measured_state=JointState(name=["joint_a"], position=[-1.0011]), @@ -1194,14 +1209,14 @@ def step_unbounded(**kwargs: Any) -> None: result = ik.step_frame_targets( robot_model=_robot_config(), - frame_targets={"tool": PoseStamped()}, + frame_targets={"tool": PoseStamped(header=Header(frame_id=""), pose=Pose())}, controlled_joints=["joint_b"], command_state=JointState(name=["joint_b"], position=[5.0]), measured_state=JointState(name=["joint_b"], position=[5.0]), max_command_tracking_error_rad=_TRACKING_ERROR_RAD, ) - assert result.position == pytest.approx([5.1]) + assert list(result.position) == pytest.approx([5.1]) def test_validate_frame_targets_rejects_margin_wider_than_joint_range( @@ -1241,7 +1256,7 @@ def test_step_frame_targets_rejects_unknown_frame(mocker: MockerFixture, tmp_pat with pytest.raises(ValueError, match="missing_frame"): _StreamingTestPinkIK(PinkIKConfig()).step_frame_targets( robot_model=config, - frame_targets={"missing_frame": PoseStamped()}, + frame_targets={"missing_frame": PoseStamped(header=Header(frame_id=""), pose=Pose())}, controlled_joints=config.joint_names, command_state=JointState(name=config.joint_names, position=[0.0, 0.0, 0.0]), measured_state=JointState(name=config.joint_names, position=[0.0, 0.0, 0.0]), @@ -1274,7 +1289,7 @@ def test_solve_targets_returns_successful_ik_result(mocker: MockerFixture) -> No assert result.status == IKStatus.SUCCESS assert result.joint_state is not None assert result.joint_state.name == ["joint_a", "joint_b", "joint_c"] - assert result.joint_state.position == pytest.approx([0.2, 0.1, 0.3]) + assert list(result.joint_state.position) == pytest.approx([0.2, 0.1, 0.3]) def test_planning_uses_named_stack_and_task_lifecycle_hooks( @@ -1385,11 +1400,16 @@ def is_collision_free(self, ctx: object) -> bool: [0.05003819, 0.15888552, 0.11027428, -0.10991712, -0.07993349, 0.14942138, -0.19789388] ) target_q = ik._q_from_dimos_positions(context, target_positions) - target_pose = matrix_to_pose(ik._current_frame_matrix(context, target_q)) + target_pose = pose_from_matrix(ik._current_frame_matrix(context, target_q)) result = ik.solve_pose_targets( cast("Any", World()), - {group: PoseStamped(position=target_pose.position, orientation=target_pose.orientation)}, + { + group: PoseStamped( + header=Header(frame_id=""), + pose=Pose(position=target_pose.position, orientation=target_pose.orientation), + ) + }, seed=seed, check_collision=False, max_attempts=1, @@ -1410,8 +1430,11 @@ def test_solve_rejects_collision_candidate(mocker: MockerFixture) -> None: result = ik.solve( world=cast("Any", _FakeWorld(collision_free=False)), target_pose=PoseStamped( - position=Vector3(0.1, 0.0, 0.0), - orientation=Quaternion(0.0, 0.0, 0.0, 1.0), + header=Header(frame_id=""), + pose=Pose( + position=Point(x=0.1, y=0.0, z=0.0), + orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0), + ), ), check_collision=True, max_attempts=1, @@ -1452,8 +1475,11 @@ def fake_solve_targets(**_: object) -> IKResult: result = ik.solve( world=cast("Any", _FakeWorld(collision_free=True)), target_pose=PoseStamped( - position=Vector3(0.1, 0.0, 0.0), - orientation=Quaternion(0.0, 0.0, 0.0, 1.0), + header=Header(frame_id=""), + pose=Pose( + position=Point(x=0.1, y=0.0, z=0.0), + orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0), + ), ), check_collision=True, max_attempts=2, @@ -1520,7 +1546,7 @@ def test_solve_pose_targets_uses_group_tip_and_filters_group_joints( return_value=IKResult( status=IKStatus.SUCCESS, joint_state=JointState( - {"name": ["joint_a", "joint_b", "joint_c"], "position": [0.1, 0.2, 0.3]} + name=["joint_a", "joint_b", "joint_c"], position=[0.1, 0.2, 0.3] ), ), ) @@ -1530,10 +1556,11 @@ def test_solve_pose_targets_uses_group_tip_and_filters_group_joints( world=cast("Any", world), pose_targets={ world.groups["manipulator"]: PoseStamped( - position=Vector3(), orientation=Quaternion(0.0, 0.0, 0.0, 1.0) + header=Header(frame_id=""), + pose=Pose(position=Point(), orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0)), ) }, - seed=JointState({"name": ["joint_a", "joint_b", "joint_c"], "position": [0.0, 0.0, 0.0]}), + seed=JointState(name=["joint_a", "joint_b", "joint_c"], position=[0.0, 0.0, 0.0]), max_attempts=1, ) @@ -1553,7 +1580,8 @@ def test_solve_pose_targets_rejects_group_without_tip(mocker: MockerFixture) -> world=cast("Any", world), pose_targets={ world.groups["no_tip"]: PoseStamped( - position=Vector3(), orientation=Quaternion(0.0, 0.0, 0.0, 1.0) + header=Header(frame_id=""), + pose=Pose(position=Point(), orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0)), ) }, ) @@ -1571,7 +1599,7 @@ def test_solve_pose_targets_partial_seed_reads_world_state(mocker: MockerFixture return_value=IKResult( status=IKStatus.SUCCESS, joint_state=JointState( - {"name": ["joint_a", "joint_b", "joint_c"], "position": [0.1, 0.2, 0.3]} + name=["joint_a", "joint_b", "joint_c"], position=[0.1, 0.2, 0.3] ), ), ) @@ -1581,10 +1609,11 @@ def test_solve_pose_targets_partial_seed_reads_world_state(mocker: MockerFixture world=cast("Any", world), pose_targets={ world.groups["manipulator"]: PoseStamped( - position=Vector3(), orientation=Quaternion(0.0, 0.0, 0.0, 1.0) + header=Header(frame_id=""), + pose=Pose(position=Point(), orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0)), ) }, - seed=JointState({"name": ["joint_a"], "position": [0.0]}), + seed=JointState(name=["joint_a"], position=[0.0]), max_attempts=1, ) @@ -1602,7 +1631,7 @@ def test_solve_pose_targets_multi_target_uses_multi_frame_solve(mocker: MockerFi return_value=IKResult( status=IKStatus.SUCCESS, joint_state=JointState( - {"name": ["joint_a", "joint_b", "joint_c"], "position": [0.1, 0.2, 0.3]} + name=["joint_a", "joint_b", "joint_c"], position=[0.1, 0.2, 0.3] ), position_error=0.0, orientation_error=0.0, @@ -1613,13 +1642,15 @@ def test_solve_pose_targets_multi_target_uses_multi_frame_solve(mocker: MockerFi world=cast("Any", world), pose_targets={ world.groups["manipulator"]: PoseStamped( - position=Vector3(), orientation=Quaternion(0.0, 0.0, 0.0, 1.0) + header=Header(frame_id=""), + pose=Pose(position=Point(), orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0)), ), world.groups["wrist"]: PoseStamped( - position=Vector3(), orientation=Quaternion(0.0, 0.0, 0.0, 1.0) + header=Header(frame_id=""), + pose=Pose(position=Point(), orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0)), ), }, - seed=JointState({"name": ["joint_a", "joint_b", "joint_c"], "position": [0.0, 0.0, 0.0]}), + seed=JointState(name=["joint_a", "joint_b", "joint_c"], position=[0.0, 0.0, 0.0]), max_attempts=1, ) @@ -1642,7 +1673,7 @@ def test_solve_pose_targets_checks_multi_group_solution_together( return_value=IKResult( status=IKStatus.SUCCESS, joint_state=JointState( - {"name": ["joint_a", "joint_b", "joint_c"], "position": [0.1, 0.2, 0.3]} + name=["joint_a", "joint_b", "joint_c"], position=[0.1, 0.2, 0.3] ), ), ) @@ -1651,18 +1682,15 @@ def test_solve_pose_targets_checks_multi_group_solution_together( world=cast("Any", world), pose_targets={ world.groups["manipulator"]: PoseStamped( - position=Vector3(), orientation=Quaternion(0.0, 0.0, 0.0, 1.0) + header=Header(frame_id=""), + pose=Pose(position=Point(), orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0)), ), world.groups["wrist"]: PoseStamped( - position=Vector3(), orientation=Quaternion(0.0, 0.0, 0.0, 1.0) + header=Header(frame_id=""), + pose=Pose(position=Point(), orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0)), ), }, - seed=JointState( - { - "name": ["joint_a", "joint_b", "joint_c"], - "position": [0.0, 0.0, 0.0], - } - ), + seed=JointState(name=["joint_a", "joint_b", "joint_c"], position=[0.0, 0.0, 0.0]), max_attempts=1, ) @@ -1681,7 +1709,7 @@ def test_solve_pose_targets_auxiliary_only_retains_seed_selection_order( world=cast("Any", world), pose_targets={}, auxiliary_groups=[world.groups["no_tip"], world.groups["manipulator"]], - seed=JointState({"name": ["joint_a", "joint_b", "joint_c"], "position": [0.1, 0.2, 0.3]}), + seed=JointState(name=["joint_a", "joint_b", "joint_c"], position=[0.1, 0.2, 0.3]), ) assert result.status == IKStatus.SUCCESS @@ -1691,7 +1719,7 @@ def test_solve_pose_targets_auxiliary_only_retains_seed_selection_order( def _solved_joint_state() -> JointState: - return JointState({"name": ["joint_a", "joint_b", "joint_c"], "position": [0.1, 0.2, 0.3]}) + return JointState(name=["joint_a", "joint_b", "joint_c"], position=[0.1, 0.2, 0.3]) def _qp_infeasible() -> NoSolutionFound: @@ -1726,8 +1754,11 @@ def fake_solve_targets(**_: object) -> IKResult: result = ik.solve( world=cast("Any", _FakeWorld(collision_free=True)), target_pose=PoseStamped( - position=Vector3(0.1, 0.0, 0.0), - orientation=Quaternion(0.0, 0.0, 0.0, 1.0), + header=Header(frame_id=""), + pose=Pose( + position=Point(x=0.1, y=0.0, z=0.0), + orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0), + ), ), check_collision=True, max_attempts=2, @@ -1752,8 +1783,11 @@ def fake_solve_targets(**_: object) -> IKResult: result = ik.solve( world=cast("Any", _FakeWorld(collision_free=True)), target_pose=PoseStamped( - position=Vector3(0.1, 0.0, 0.0), - orientation=Quaternion(0.0, 0.0, 0.0, 1.0), + header=Header(frame_id=""), + pose=Pose( + position=Point(x=0.1, y=0.0, z=0.0), + orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0), + ), ), check_collision=True, max_attempts=3, @@ -1774,8 +1808,11 @@ def test_solve_does_not_retry_unexpected_exception(mocker: MockerFixture) -> Non result = ik.solve( world=cast("Any", _FakeWorld(collision_free=True)), target_pose=PoseStamped( - position=Vector3(0.1, 0.0, 0.0), - orientation=Quaternion(0.0, 0.0, 0.0, 1.0), + header=Header(frame_id=""), + pose=Pose( + position=Point(x=0.1, y=0.0, z=0.0), + orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0), + ), ), check_collision=True, max_attempts=3, @@ -1796,8 +1833,11 @@ def test_solve_mapping_value_error_fails_without_retrying(mocker: MockerFixture) result = ik.solve( world=cast("Any", _FakeWorld(collision_free=True)), target_pose=PoseStamped( - position=Vector3(0.1, 0.0, 0.0), - orientation=Quaternion(0.0, 0.0, 0.0, 1.0), + header=Header(frame_id=""), + pose=Pose( + position=Point(x=0.1, y=0.0, z=0.0), + orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0), + ), ), check_collision=True, max_attempts=5, @@ -1809,7 +1849,7 @@ def test_solve_mapping_value_error_fails_without_retrying(mocker: MockerFixture) def _pose_targets_seed() -> JointState: - return JointState({"name": ["joint_a", "joint_b", "joint_c"], "position": [0.0, 0.0, 0.0]}) + return JointState(name=["joint_a", "joint_b", "joint_c"], position=[0.0, 0.0, 0.0]) def test_solve_pose_targets_retries_after_no_solution_found(mocker: MockerFixture) -> None: @@ -1838,7 +1878,8 @@ def fake_solve_targets(**_: object) -> IKResult: world=cast("Any", world), pose_targets={ world.groups["manipulator"]: PoseStamped( - position=Vector3(), orientation=Quaternion(0.0, 0.0, 0.0, 1.0) + header=Header(frame_id=""), + pose=Pose(position=Point(), orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0)), ) }, seed=_pose_targets_seed(), @@ -1866,7 +1907,8 @@ def fake_solve_targets(**_: object) -> IKResult: world=cast("Any", world), pose_targets={ world.groups["manipulator"]: PoseStamped( - position=Vector3(), orientation=Quaternion(0.0, 0.0, 0.0, 1.0) + header=Header(frame_id=""), + pose=Pose(position=Point(), orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0)), ) }, seed=_pose_targets_seed(), @@ -1892,7 +1934,8 @@ def test_solve_pose_targets_does_not_retry_unexpected_exception( world=cast("Any", world), pose_targets={ world.groups["manipulator"]: PoseStamped( - position=Vector3(), orientation=Quaternion(0.0, 0.0, 0.0, 1.0) + header=Header(frame_id=""), + pose=Pose(position=Point(), orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0)), ) }, seed=_pose_targets_seed(), @@ -1918,7 +1961,8 @@ def test_solve_pose_targets_mapping_value_error_fails_without_retrying( world=cast("Any", world), pose_targets={ world.groups["manipulator"]: PoseStamped( - position=Vector3(), orientation=Quaternion(0.0, 0.0, 0.0, 1.0) + header=Header(frame_id=""), + pose=Pose(position=Point(), orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0)), ) }, seed=_pose_targets_seed(), diff --git a/dimos/manipulation/planning/kinematics/utils.py b/dimos/manipulation/planning/kinematics/utils.py index 8fff0b12b6d..d32e40c02d4 100644 --- a/dimos/manipulation/planning/kinematics/utils.py +++ b/dimos/manipulation/planning/kinematics/utils.py @@ -17,6 +17,8 @@ from collections.abc import Mapping, Sequence from dataclasses import dataclass +from dimos_generated.geometry_msgs.msg import PoseStamped +from dimos_generated.sensor_msgs.msg import JointState import numpy as np from numpy.typing import NDArray @@ -25,8 +27,6 @@ from dimos.manipulation.planning.spec.enums import IKStatus from dimos.manipulation.planning.spec.models import IKResult from dimos.manipulation.planning.spec.protocols import WorldSpec -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.sensor_msgs.JointState import JointState @dataclass(frozen=True) diff --git a/dimos/manipulation/planning/monitor/robot_state_monitor.py b/dimos/manipulation/planning/monitor/robot_state_monitor.py index 837ed36de7b..a2d13a271bc 100644 --- a/dimos/manipulation/planning/monitor/robot_state_monitor.py +++ b/dimos/manipulation/planning/monitor/robot_state_monitor.py @@ -26,12 +26,13 @@ from __future__ import annotations +import copy import time from typing import TYPE_CHECKING +from dimos_generated.sensor_msgs.msg import JointState import numpy as np -from dimos.msgs.sensor_msgs.JointState import JointState from dimos.utils.logging_config import setup_logger if TYPE_CHECKING: @@ -84,6 +85,7 @@ def __init__( self._timeout = timeout # Latest state + self._latest_state: JointState | None = None self._latest_positions: NDArray[np.float64] | None = None self._latest_velocities: NDArray[np.float64] | None = None self._last_update_time: float | None = None @@ -142,14 +144,16 @@ def on_joint_state(self, msg: JointState) -> None: self._latest_positions = positions self._latest_velocities = velocities self._last_update_time = current_time + joint_state = JointState( + header=msg.header, + name=self._joint_names, + position=positions.tolist(), + velocity=velocities.tolist() if velocities is not None else [], + ) + self._latest_state = copy.deepcopy(joint_state) # Sync to world's live context (for visualization) try: - # Create JointState for world sync (API uses JointState) - joint_state = JointState( - name=self._joint_names, - position=positions.tolist(), - ) self._world.sync_from_joint_state(joint_state) except Exception as e: logger.error(f"Failed to sync joint state to live context: {e}") @@ -213,6 +217,19 @@ def _extract_velocities(self, msg: JointState) -> NDArray[np.float64] | None: return np.array(velocities, dtype=np.float64) + def get_current_joint_state(self) -> JointState | None: + """Copy one atomic sample, preserving its source header and joint ordering.""" + with self._lock: + state = self._latest_state + if state is None: + return None + return JointState( + header=state.header, + name=state.name, + position=state.position, + velocity=state.velocity, + ) + def get_current_positions(self) -> NDArray[np.float64] | None: """Get current joint positions (thread-safe). diff --git a/dimos/manipulation/planning/monitor/test_robot_state_monitor.py b/dimos/manipulation/planning/monitor/test_robot_state_monitor.py new file mode 100644 index 00000000000..f22d99c99d4 --- /dev/null +++ b/dimos/manipulation/planning/monitor/test_robot_state_monitor.py @@ -0,0 +1,78 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import threading + +from dimos_generated.builtin_interfaces.msg import Time +from dimos_generated.sensor_msgs.msg import JointState +from dimos_generated.std_msgs.msg import Header +import pytest + +from dimos.manipulation.planning.monitor.robot_state_monitor import RobotStateMonitor +from dimos.manipulation.planning.spec.protocols import WorldSpec + + +@pytest.fixture() +def monitor(mocker): + world = mocker.Mock(spec=WorldSpec) + monitor = RobotStateMonitor(world, threading.RLock(), ["arm/a", "arm/b"]) + monitor.start() + try: + yield monitor, world + finally: + monitor.stop() + + +def test_generated_feedback_reorders_joints_and_preserves_source_header(monitor): + monitor, world = monitor + received = [] + monitor.add_state_callback(received.append) + source = JointState( + header=Header(stamp=Time(sec=1700000000, nanosec=123456789), frame_id="robot"), + name=["arm/b", "other", "arm/a"], + position=[2.0, 9.0, 1.0], + velocity=[0.2, 0.9, 0.1], + ) + monitor.on_joint_state(JointState.decode(source.encode())) + assert monitor.get_current_positions().tolist() == [1.0, 2.0] + assert monitor.get_current_velocities().tolist() == [0.1, 0.2] + expected = JointState( + header=source.header, name=["arm/a", "arm/b"], position=[1.0, 2.0], velocity=[0.1, 0.2] + ) + world.sync_from_joint_state.assert_called_once_with(expected) + assert received == [expected] + snapshot = monitor.get_current_joint_state() + assert snapshot == expected + snapshot.header.stamp.nanosec = 0 + snapshot.position[0] = 99 + received[0].position[0] = 42 + assert monitor.get_current_joint_state() == expected + source.header.stamp.nanosec = 0 + assert received[0].header.stamp.nanosec == 123456789 + + +def test_missing_joint_does_not_replace_valid_state(monitor): + monitor, world = monitor + monitor.on_joint_state(JointState(name=["arm/a", "arm/b"], position=[1.0, 2.0])) + monitor.on_joint_state(JointState(name=["arm/a"], position=[9.0])) + assert monitor.get_current_positions().tolist() == [1.0, 2.0] + assert world.sync_from_joint_state.call_count == 1 + + +def test_stopped_monitor_does_not_sync(monitor): + monitor, world = monitor + monitor.stop() + monitor.on_joint_state(JointState(name=["arm/a", "arm/b"], position=[1.0, 2.0])) + assert monitor.get_current_positions() is None + world.sync_from_joint_state.assert_not_called() diff --git a/dimos/manipulation/planning/monitor/test_world_monitor.py b/dimos/manipulation/planning/monitor/test_world_monitor.py index fb2c0aae1e8..b854c8c32cb 100644 --- a/dimos/manipulation/planning/monitor/test_world_monitor.py +++ b/dimos/manipulation/planning/monitor/test_world_monitor.py @@ -15,9 +15,14 @@ from __future__ import annotations from pathlib import Path -from types import SimpleNamespace from typing import Any +from dimos_generated.builtin_interfaces.msg import Time +from dimos_generated.geometry_msgs.msg import Point, Pose, PoseStamped, Quaternion, Vector3 +from dimos_generated.sensor_msgs.msg import Image, JointState +from dimos_generated.std_msgs.msg import Header +from dimos_generated.trajectory_msgs.msg import JointTrajectory +from dimos_generated.vision_msgs.msg import BoundingBox3D, Detection3D import numpy as np import pytest from pytest_mock import MockerFixture @@ -37,13 +42,9 @@ ) from dimos.manipulation.planning.spec.protocols import VisualizationSpec from dimos.manipulation.planning.utils import mesh_utils -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.msgs.sensor_msgs.Image import Image -from dimos.msgs.sensor_msgs.JointState import JointState -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 -from dimos.msgs.trajectory_msgs.JointTrajectory import JointTrajectory +from dimos.msgs.geometry import quaternion_from_matrix +from dimos.msgs.pointcloud import pointcloud_from_xyz, pointcloud_to_open3d +from dimos.msgs.time import header_now from dimos.perception.experimental.object import Object from dimos.robot.assets.model import RobotModel @@ -134,7 +135,13 @@ def get_ee_pose(self, ctx): def get_group_ee_pose(self, ctx, group_id): self.calls.append(("get_group_ee_pose", ctx, group_id)) - return PoseStamped(position=Vector3(1, 2, 3), orientation=Quaternion([0, 0, 0, 1])) + return PoseStamped( + header=Header(frame_id=""), + pose=Pose( + position=Point(x=1, y=2, z=3), + orientation=Quaternion(x=0, y=0, z=0, w=1), + ), + ) def get_link_pose(self, ctx, link_name): return [] @@ -224,7 +231,13 @@ def _robot_config() -> RobotModelConfig: ) return RobotModelConfig( model=RobotModel.from_file(model_path), - base_pose=PoseStamped(position=Vector3(), orientation=Quaternion([0, 0, 0, 1])), + base_pose=PoseStamped( + header=Header(frame_id=""), + pose=Pose( + position=Point(), + orientation=Quaternion(x=0, y=0, z=0, w=1), + ), + ), joint_names=["j1", "j2"], base_link="base", planning_groups=[ @@ -292,7 +305,7 @@ def test_world_monitor_syncs_planning_scene_to_visualization() -> None: def test_world_monitor_forwards_raw_trajectory_preview_protocol() -> None: fake_viz = FakeViz() monitor = world_monitor_module.WorldMonitor(world=FakeWorld(), visualization=fake_viz) # type: ignore[arg-type] - trajectory = JointTrajectory(joint_names=["j1"], points=[]) + trajectory = JointTrajectory(header=header_now(), joint_names=["j1"], points=[]) assert isinstance(fake_viz, VisualizationSpec) monitor.cancel_preview_animation() @@ -327,7 +340,13 @@ def test_obstacle_monitor_routes_mutations_through_parent_world_monitor( obstacle_monitor = parent.obstacle_monitor assert obstacle_monitor is not None - pose = PoseStamped(position=Vector3(1, 2, 3), orientation=Quaternion([0, 0, 0, 1])) + pose = PoseStamped( + header=Header(frame_id=""), + pose=Pose( + position=Point(x=1, y=2, z=3), + orientation=Quaternion(x=0, y=0, z=0, w=1), + ), + ) obstacle_monitor.on_collision_object( CollisionObjectMessage( id="source-id", @@ -473,7 +492,7 @@ def test_group_ee_pose_uses_current_state_when_no_joint_state_is_provided() -> N set_calls = [call for call in fake_world.calls if call[0] == "set_joint_state"] assert set_calls[0][2].name == ["j1", "j2", "j3"] assert set_calls[0][2].position == [0.1, 0.2, 0.3] - assert pose.position.x == 1 + assert pose.pose.position.x == 1 def test_group_ee_pose_without_joint_state_rejects_stale_state(mocker) -> None: @@ -501,7 +520,7 @@ def test_group_kinematics_with_full_state_does_not_require_current_state() -> No set_calls = [call for call in fake_world.calls if call[0] == "set_joint_state"] assert set_calls[0][2].name == ["j1", "j2", "j3"] assert set_calls[0][2].position == [0.1, 0.2, 0.3] - assert pose.position.x == 1 + assert pose.pose.position.x == 1 def test_group_kinematics_route_full_state_to_backend() -> None: @@ -523,7 +542,7 @@ def test_group_kinematics_route_full_state_to_backend() -> None: assert set_calls[0][2].position == [0.9, 0.8, 0.3] assert set_calls[1][2].name == ["j1", "j2", "j3"] assert set_calls[1][2].position == [0.4, 0.3, 0.3] - assert pose.position.x == 1 + assert pose.pose.position.x == 1 assert jacobian.shape == (6, 2) assert ("get_group_ee_pose", "scratch", "manipulator") in fake_world.calls assert ("get_group_jacobian", "scratch", "manipulator") in fake_world.calls @@ -587,7 +606,13 @@ def test_world_monitor_updates_obstacle_pose_with_backend_result(mocker: MockerF world = FakeWorld() viz = FakeViz() monitor = world_monitor_module.WorldMonitor(world=world, visualization=viz) # type: ignore[arg-type] - pose = PoseStamped(position=Vector3(1, 2, 3), orientation=Quaternion([0, 0, 0, 1])) + pose = PoseStamped( + header=Header(frame_id=""), + pose=Pose( + position=Point(x=1, y=2, z=3), + orientation=Quaternion(x=0, y=0, z=0, w=1), + ), + ) update = mocker.patch.object(world, "update_obstacle_pose", side_effect=[True, False]) assert monitor.update_obstacle_pose("obstacle-id", pose) is True @@ -608,7 +633,7 @@ def test_world_monitor_forwards_only_successful_complete_updates( obstacle = Obstacle( name="box", obstacle_type=ObstacleType.BOX, - pose=PoseStamped(), + pose=PoseStamped(header=Header(frame_id=""), pose=Pose()), dimensions=(1.0, 1.0, 1.0), ) update = mocker.patch.object(world, "update_obstacle", side_effect=[True, False]) @@ -629,7 +654,13 @@ def test_obstacle_monitor_routes_complete_and_rejects_incomplete_updates( parent.start_obstacle_monitor() monitor = parent.obstacle_monitor assert monitor is not None - pose = PoseStamped(position=Vector3(1, 2, 3), orientation=Quaternion([0, 0, 0, 1])) + pose = PoseStamped( + header=Header(frame_id=""), + pose=Pose( + position=Point(x=1, y=2, z=3), + orientation=Quaternion(x=0, y=0, z=0, w=1), + ), + ) monitor.on_collision_object( CollisionObjectMessage( @@ -678,7 +709,13 @@ def test_obstacle_monitor_adds_complete_unknown_update_and_stops_on_failed_updat parent.start_obstacle_monitor() monitor = parent.obstacle_monitor assert monitor is not None - pose = PoseStamped(position=Vector3(1, 2, 3), orientation=Quaternion([0, 0, 0, 1])) + pose = PoseStamped( + header=Header(frame_id=""), + pose=Pose( + position=Point(x=1, y=2, z=3), + orientation=Quaternion(x=0, y=0, z=0, w=1), + ), + ) monitor.on_collision_object( CollisionObjectMessage( @@ -717,7 +754,13 @@ def test_world_monitor_clear_updates_world_tracking_and_survives_visualization_e monitor.start_obstacle_monitor() obstacle_monitor = monitor.obstacle_monitor assert obstacle_monitor is not None - pose = PoseStamped(position=Vector3(1, 2, 3), orientation=Quaternion([0, 0, 0, 1])) + pose = PoseStamped( + header=Header(frame_id=""), + pose=Pose( + position=Point(x=1, y=2, z=3), + orientation=Quaternion(x=0, y=0, z=0, w=1), + ), + ) obstacle_monitor.on_collision_object( CollisionObjectMessage( id="source", @@ -727,11 +770,14 @@ def test_world_monitor_clear_updates_world_tracking_and_survives_visualization_e dimensions=(1.0, 2.0, 3.0), ) ) - detection = SimpleNamespace( + detection = Detection3D( id="detection", - bbox=SimpleNamespace( - center=SimpleNamespace(position=Vector3(4, 5, 6), orientation=Quaternion([0, 0, 0, 1])), - size=Vector3(1, 1, 1), + bbox=BoundingBox3D( + center=Pose( + position=Point(x=4, y=5, z=6), + orientation=Quaternion(x=0, y=0, z=0, w=1), + ), + size=Vector3(x=1, y=1, z=1), ), ) obstacle_monitor.on_detections([detection]) # type: ignore[arg-type] @@ -794,7 +840,13 @@ def test_world_obstacle_monitor_rejects_invalid_add_and_handles_update_and_callb remove = mocker.patch.object(parent, "remove_obstacle", return_value=True) callback = mocker.Mock(side_effect=RuntimeError("callback failed")) obstacle_monitor.add_obstacle_callback(callback) - pose = PoseStamped(position=Vector3(1, 2, 3), orientation=Quaternion([0, 0, 0, 1])) + pose = PoseStamped( + header=Header(frame_id=""), + pose=Pose( + position=Point(x=1, y=2, z=3), + orientation=Quaternion(x=0, y=0, z=0, w=1), + ), + ) obstacle_monitor.on_collision_object(CollisionObjectMessage(id="bad", operation="add")) assert obstacle_monitor.get_obstacle_count() == 0 @@ -829,17 +881,19 @@ def test_world_obstacle_monitor_detection_add_update_and_stale_cleanup( add = mocker.patch.object(parent, "add_obstacle", return_value="first-id") update = mocker.patch.object(parent, "update_obstacle_pose") remove = mocker.patch.object(parent, "remove_obstacle", return_value=False) - timestamps = iter([1.0, 1.0, 1.0, 1.0, 1.0, 10.0]) - monkeypatch.setattr( - "dimos.manipulation.planning.monitor.world_obstacle_monitor.time.time", - lambda: next(timestamps, 10.0), + clock = mocker.patch( + "dimos.manipulation.planning.monitor.world_obstacle_monitor.time.time", return_value=1.0 + ) + center = Pose( + position=Point(x=1, y=2, z=3), + orientation=Quaternion(x=0, y=0, z=0, w=1), ) - center = SimpleNamespace(position=Vector3(1, 2, 3), orientation=Quaternion([0, 0, 0, 1])) - bbox = SimpleNamespace(center=center, size=Vector3(1, 2, 3)) - detection = SimpleNamespace(id="det-1", bbox=bbox) + bbox = BoundingBox3D(center=center, size=Vector3(x=1, y=2, z=3)) + detection = Detection3D(id="det-1", bbox=bbox) obstacle_monitor.on_detections([detection]) # type: ignore[arg-type] - obstacle_monitor.on_detections([detection]) # type: ignore[arg-type] + obstacle_monitor.on_detections([detection]) + clock.return_value = 10.0 obstacle_monitor.on_detections([]) assert add.call_count == 1 @@ -878,19 +932,21 @@ def test_mesh_obstacle_is_placed_at_the_hull_centroid_without_the_bbox_rotation( add_obstacle = mocker.patch.object(parent, "add_obstacle", return_value="parent-id") points = _tilted_box_cloud() - cloud = PointCloud2.from_numpy(points, frame_id="world") + cloud = pointcloud_from_xyz(points, header=Header(frame_id="world")) # Mirrors Object.from_detections with use_aabb=False: pose carries the # oriented-box center and rotation. - obb = cloud.pointcloud.get_oriented_bounding_box() + obb = pointcloud_to_open3d(cloud).get_oriented_bounding_box() obj = Object( object_id="tilted-box", name="box", - center=Vector3(obb.center), - size=Vector3(obb.extent), + center=Vector3(x=obb.center[0], y=obb.center[1], z=obb.center[2]), + size=Vector3(x=obb.extent[0], y=obb.extent[1], z=obb.extent[2]), pose=PoseStamped( - frame_id="world", - position=Vector3(obb.center), - orientation=Quaternion.from_rotation_matrix(np.asarray(obb.R)), + header=Header(frame_id="world"), + pose=Pose( + position=Point(x=obb.center[0], y=obb.center[1], z=obb.center[2]), + orientation=quaternion_from_matrix(np.asarray(obb.R)), + ), ), pointcloud=cloud, bbox=(0.0, 0.0, 1.0, 1.0), @@ -910,23 +966,67 @@ def test_mesh_obstacle_is_placed_at_the_hull_centroid_without_the_bbox_rotation( centroid = points.mean(axis=0) assert obstacle.obstacle_type == ObstacleType.MESH np.testing.assert_allclose( - [obstacle.pose.position.x, obstacle.pose.position.y, obstacle.pose.position.z], + [ + obstacle.pose.pose.position.x, + obstacle.pose.pose.position.y, + obstacle.pose.pose.position.z, + ], centroid, atol=1e-3, ) np.testing.assert_allclose( [ - obstacle.pose.orientation.x, - obstacle.pose.orientation.y, - obstacle.pose.orientation.z, - obstacle.pose.orientation.w, + obstacle.pose.pose.orientation.x, + obstacle.pose.pose.orientation.y, + obstacle.pose.pose.orientation.z, + obstacle.pose.pose.orientation.w, ], [0.0, 0.0, 0.0, 1.0], atol=1e-9, ) - assert obstacle.pose.frame_id == "world" + assert obstacle.pose.header.frame_id == "world" # Teeth: the pose the old code used is a genuinely different placement, so # this cannot pass on the bug. assert np.linalg.norm(np.asarray(obb.center) - centroid) > 3e-3 - assert abs(Quaternion.from_rotation_matrix(np.asarray(obb.R)).w) < 0.999 + assert abs(quaternion_from_matrix(np.asarray(obb.R)).w) < 0.999 + + +@pytest.mark.parametrize("use_current_state", [False, True]) +def test_group_pose_preserves_joint_sample_stamp_and_backend_frame(use_current_state): + world = FakeWorld() + monitor = world_monitor_module.WorldMonitor(world=world) + monitor.load_model(_three_joint_reordered_group_config()) + state = JointState( + header=Header(frame_id="encoder", stamp=Time(sec=1700000000, nanosec=123456789)), + name=["j1", "j2", "j3"], + position=[0.1, 0.2, 0.3], + ) + if use_current_state: + monitor.start_state_monitor() + monitor.on_joint_state(state) + pose = monitor.get_group_ee_pose("manipulator", None if use_current_state else state) + assert pose.header.stamp == state.header.stamp + assert pose.header.frame_id == "" + pose.header.stamp.nanosec = 0 + assert state.header.stamp.nanosec == 123456789 + + +def test_link_pose_preserves_joint_sample_stamp(mocker): + world = FakeWorld() + monitor = world_monitor_module.WorldMonitor(world=world) + monitor.load_model(_three_joint_reordered_group_config()) + matrix = np.eye(4) + matrix[:3, 3] = [1, 2, 3] + mocker.patch.object(world, "get_link_pose", return_value=matrix) + state = JointState( + header=Header(stamp=Time(sec=-1, nanosec=987654321)), + name=["j1", "j2", "j3"], + position=[0.1, 0.2, 0.3], + ) + pose = monitor.get_link_pose("ee", state) + assert pose is not None + assert pose.header == Header(frame_id="world", stamp=state.header.stamp) + assert pose.pose.position == Point(x=1, y=2, z=3) + pose.header.stamp.nanosec = 0 + assert state.header.stamp.nanosec == 987654321 diff --git a/dimos/manipulation/planning/monitor/world_monitor.py b/dimos/manipulation/planning/monitor/world_monitor.py index 2f495fd6e34..d0023878e7a 100644 --- a/dimos/manipulation/planning/monitor/world_monitor.py +++ b/dimos/manipulation/planning/monitor/world_monitor.py @@ -20,6 +20,12 @@ import threading from typing import TYPE_CHECKING, Any +from dimos_generated.dimos_msgs.msg import GraspCandidateArray +from dimos_generated.geometry_msgs.msg import Point, Pose, PoseStamped, Quaternion +from dimos_generated.sensor_msgs.msg import JointState +from dimos_generated.std_msgs.msg import Header +from dimos_generated.trajectory_msgs.msg import JointTrajectory + from dimos.constants import DEFAULT_THREAD_JOIN_TIMEOUT from dimos.manipulation.planning.groups.identifiers import assert_valid_group_id from dimos.manipulation.planning.groups.registry import PlanningGroupRegistry @@ -33,16 +39,13 @@ ) from dimos.manipulation.planning.spec.protocols import VisualizationSpec, WorldSpec from dimos.manipulation.planning.spec.validation import PreparedRobotModel, prepare_robot_model -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.manipulation_msgs.GraspCandidateArray import GraspCandidateArray -from dimos.msgs.sensor_msgs.JointState import JointState -from dimos.msgs.trajectory_msgs.JointTrajectory import JointTrajectory +from dimos.msgs.geometry import quaternion_from_matrix from dimos.utils.logging_config import setup_logger if TYPE_CHECKING: from collections.abc import Generator + from dimos_generated.vision_msgs.msg import Detection3D import numpy as np from numpy.typing import NDArray @@ -53,7 +56,6 @@ Obstacle, PlanningGroupID, ) - from dimos.msgs.vision_msgs.Detection3D import Detection3D from dimos.perception.experimental.object import Object logger = setup_logger() @@ -267,7 +269,7 @@ def on_collision_object(self, msg: CollisionObjectMessage) -> None: self._obstacle_monitor.on_collision_object(msg) def on_detections(self, detections: list[Detection3D]) -> None: - """Handle perception detections (Detection3D from dimos.msgs.vision_msgs).""" + """Handle perception detections (generated Detection3D).""" if self._obstacle_monitor is not None: self._obstacle_monitor.on_detections(detections) @@ -323,14 +325,9 @@ def list_added_obstacles(self) -> list[dict[str, Any]]: def get_current_joint_state(self) -> JointState | None: """Get current joint state. Returns None if not yet received.""" if self._state_monitor is not None: - positions = self._state_monitor.get_current_positions() - velocities = self._state_monitor.get_current_velocities() - if positions is not None: - return JointState( - name=self.get_model_config().joint_names, - position=positions.tolist(), - velocity=velocities.tolist() if velocities is not None else [], - ) + state = self._state_monitor.get_current_joint_state() + if state is not None: + return state # Fall back to world's live context with self._lock: @@ -375,11 +372,9 @@ def _validate_planning_group_config(self, config: RobotModelConfig) -> None: def get_current_velocities(self) -> JointState | None: """Get current joint velocities as JointState. Returns None if not available.""" if self._state_monitor is not None: - velocities = self._state_monitor.get_current_velocities() - if velocities is not None: - return JointState( - name=self.get_model_config().joint_names, velocity=velocities.tolist() - ) + state = self._state_monitor.get_current_joint_state() + if state is not None and state.velocity: + return JointState(header=state.header, name=state.name, velocity=state.velocity) return None def wait_for_state(self, timeout: float = 1.0) -> bool: @@ -460,7 +455,11 @@ def get_group_ee_pose( raise ValueError("Current model state is unavailable") self._world.set_joint_state(ctx, joint_state) - return self._world.get_group_ee_pose(ctx, group_id) + pose = self._world.get_group_ee_pose(ctx, group_id) + return PoseStamped( + header=Header(frame_id=pose.header.frame_id, stamp=joint_state.header.stamp), + pose=pose.pose, + ) def get_link_pose( self, link_name: str, joint_state: JointState | None = None @@ -484,11 +483,18 @@ def get_link_pose( pos = mat[:3, 3] rot = mat[:3, :3] - quat = Quaternion.from_rotation_matrix(rot) + quat = quaternion_from_matrix(rot) + header = Header(frame_id="world") + if joint_state is not None: + header.stamp = joint_state.header.stamp return PoseStamped( - frame_id="world", - position=[float(pos[0]), float(pos[1]), float(pos[2])], - orientation=[float(quat.x), float(quat.y), float(quat.z), float(quat.w)], + header=header, + pose=Pose( + position=Point(x=float(pos[0]), y=float(pos[1]), z=float(pos[2])), + orientation=Quaternion( + x=float(quat.x), y=float(quat.y), z=float(quat.z), w=float(quat.w) + ), + ), ) def get_jacobian(self, joint_state: JointState) -> NDArray[np.float64]: diff --git a/dimos/manipulation/planning/monitor/world_obstacle_monitor.py b/dimos/manipulation/planning/monitor/world_obstacle_monitor.py index 41ff2b2fbcc..2f5f1a29da9 100644 --- a/dimos/manipulation/planning/monitor/world_obstacle_monitor.py +++ b/dimos/manipulation/planning/monitor/world_obstacle_monitor.py @@ -30,22 +30,23 @@ import time from typing import TYPE_CHECKING, Any +from dimos_generated.geometry_msgs.msg import Point, Pose, PoseStamped, Quaternion + from dimos.manipulation.planning.spec.enums import ObstacleType from dimos.manipulation.planning.spec.models import ( DEFAULT_OBSTACLE_RGBA, CollisionObjectMessage, Obstacle, ) -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Vector3 import Vector3 +from dimos.msgs.pointcloud import pointcloud_xyz from dimos.utils.logging_config import setup_logger if TYPE_CHECKING: from collections.abc import Callable + from dimos_generated.vision_msgs.msg import Detection3D + from dimos.manipulation.planning.monitor.world_monitor import WorldMonitor - from dimos.msgs.vision_msgs.Detection3D import Detection3D from dimos.perception.experimental.object import Object logger = setup_logger() @@ -56,7 +57,7 @@ class WorldObstacleMonitor: This class handles updates from: - Explicit collision objects (CollisionObjectMessage) - - Perception detections (Detection3D from dimos.msgs.vision_msgs) + - Perception detections (generated Detection3D) ## Thread Safety @@ -267,7 +268,7 @@ def on_detections(self, detections: list[Detection3D]) -> None: - Removes obstacles for detections that are no longer present Args: - detections: List of Detection3D messages from dimos.msgs.vision_msgs + detections: List of generated Detection3D messages """ if not self._running: return @@ -313,11 +314,7 @@ def on_detections(self, detections: list[Detection3D]) -> None: def _detection3d_to_pose(self, detection: Detection3D) -> PoseStamped: """Convert Detection3D bbox.center to PoseStamped.""" - center = detection.bbox.center - return PoseStamped( - position=center.position, - orientation=center.orientation, - ) + return PoseStamped(header=detection.header, pose=detection.bbox.center) def _detection_to_obstacle(self, detection: Detection3D) -> Obstacle: """Convert Detection3D to Obstacle.""" @@ -644,8 +641,8 @@ def _object_to_obstacle(self, obj: object) -> Obstacle: pointcloud_to_convex_hull_obj, ) - points, _ = obj.pointcloud.as_numpy() - if points is not None and points.shape[0] >= 4: + points = pointcloud_xyz(obj.pointcloud) + if points.shape[0] >= 4: # Keyed on the object's stable unique id: rescans overwrite # in place, and no two objects share a file. hull = pointcloud_to_convex_hull_obj(points, cache_key=name) @@ -658,10 +655,13 @@ def _object_to_obstacle(self, obj: object) -> Obstacle: name=name, obstacle_type=ObstacleType.MESH, pose=PoseStamped( - ts=obj.pose.ts, - frame_id=obj.pose.frame_id, - position=Vector3(hull.centroid), - orientation=Quaternion(0.0, 0.0, 0.0, 1.0), + header=obj.pose.header, + pose=Pose( + position=Point( + x=hull.centroid[0], y=hull.centroid[1], z=hull.centroid[2] + ), + orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0), + ), ), color=(0.2, 0.8, 0.2, 0.6), mesh_path=hull.path, @@ -673,7 +673,7 @@ def _object_to_obstacle(self, obj: object) -> Obstacle: return Obstacle( name=name, obstacle_type=ObstacleType.BOX, - pose=obj.pose or PoseStamped(position=obj.center), + pose=obj.pose, dimensions=(float(obj.size.x), float(obj.size.y), float(obj.size.z)), color=(0.2, 0.8, 0.2, 0.6), ) diff --git a/dimos/manipulation/planning/planners/roboplan_planner.py b/dimos/manipulation/planning/planners/roboplan_planner.py index 2c006997830..ecdd485859b 100644 --- a/dimos/manipulation/planning/planners/roboplan_planner.py +++ b/dimos/manipulation/planning/planners/roboplan_planner.py @@ -23,6 +23,8 @@ import numpy as np +from dimos.msgs.geometry import transform_matrix + try: import roboplan.cartesian_planning as roboplan_cartesian import roboplan.core as roboplan_core @@ -33,6 +35,9 @@ "Install the manipulation extra before selecting the roboplan backend." ) from exc +from dimos_generated.geometry_msgs.msg import PoseStamped, TransformStamped +from dimos_generated.sensor_msgs.msg import JointState + from dimos.manipulation.planning.groups.models import PlanningGroup, PlanningGroupSelection from dimos.manipulation.planning.planners.roboplan_config import ( RoboPlanCartesianPathConfig, @@ -55,11 +60,8 @@ RoboPlanContext, RoboPlanWorld, ) -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.geometry_msgs.Transform import Transform -from dimos.msgs.sensor_msgs.JointState import JointState +from dimos.msgs.geometry import pose_matrix from dimos.utils.logging_config import setup_logger -from dimos.utils.transform_utils import pose_to_matrix if TYPE_CHECKING: from numpy.typing import NDArray @@ -374,8 +376,8 @@ def _validate_cartesian_request( waypoint_type = ( PoseStamped if isinstance(target[0], PoseStamped) - else Transform - if isinstance(target[0], Transform) + else TransformStamped + if isinstance(target[0], TransformStamped) else None ) if waypoint_type is None or any( @@ -385,10 +387,10 @@ def _validate_cartesian_request( status=PlanningStatus.INVALID_GOAL, message=( f"Cartesian target for '{group_id}' must contain only PoseStamped " - "waypoints or only Transform waypoints" + "waypoints or only TransformStamped waypoints" ), ) - if any(waypoint.frame_id != "world" for waypoint in target): + if any(waypoint.header.frame_id != "world" for waypoint in target): return PlanningResult( status=PlanningStatus.UNSUPPORTED, message="Cartesian planning supports only world-frame waypoints", @@ -420,7 +422,7 @@ def _build_cartesian_path( if group.tip_link is None: raise ValueError(f"Planning group '{group.id}' has no TCP tip link") start_pose = self._world.get_group_ee_pose(ctx, group.id) - start_matrix = np.asarray(pose_to_matrix(start_pose), dtype=np.float64) + start_matrix = np.asarray(pose_matrix(start_pose.pose), dtype=np.float64) target_matrices = [ self._resolve_cartesian_waypoint(start_matrix, waypoint) for waypoint in target ] @@ -436,12 +438,12 @@ def _build_cartesian_path( def _resolve_cartesian_waypoint( self, start_matrix: NDArray[np.float64], - waypoint: PoseStamped | Transform, + waypoint: PoseStamped | TransformStamped, ) -> NDArray[np.float64]: if isinstance(waypoint, PoseStamped): - return np.asarray(pose_to_matrix(waypoint), dtype=np.float64) + return np.asarray(pose_matrix(waypoint.pose), dtype=np.float64) target_matrix = start_matrix.copy() - delta_matrix = np.asarray(waypoint.to_matrix(), dtype=np.float64) + delta_matrix = np.asarray(transform_matrix(waypoint.transform), dtype=np.float64) target_matrix[:3, 3] += delta_matrix[:3, 3] target_matrix[:3, :3] = delta_matrix[:3, :3] @ start_matrix[:3, :3] return target_matrix @@ -695,15 +697,15 @@ def _validate_shortcut_path( if not shortened_path: raise ValueError("RoboPlan path shortcutter returned an empty path") if not np.allclose( - shortened_path[0].position, - original_path[0].position, + np.asarray(shortened_path[0].position, dtype=np.float64), + np.asarray(original_path[0].position, dtype=np.float64), atol=1e-9, rtol=0.0, ): raise ValueError("RoboPlan path shortcutter changed the start configuration") if not np.allclose( - shortened_path[-1].position, - original_path[-1].position, + np.asarray(shortened_path[-1].position, dtype=np.float64), + np.asarray(original_path[-1].position, dtype=np.float64), atol=1e-9, rtol=0.0, ): diff --git a/dimos/manipulation/planning/planners/rrt_planner.py b/dimos/manipulation/planning/planners/rrt_planner.py index 796a1a6b164..8be423a13eb 100644 --- a/dimos/manipulation/planning/planners/rrt_planner.py +++ b/dimos/manipulation/planning/planners/rrt_planner.py @@ -25,6 +25,7 @@ import time from typing import TYPE_CHECKING +from dimos_generated.sensor_msgs.msg import JointState import numpy as np from dimos.manipulation.planning.groups.models import PlanningGroupSelection @@ -43,7 +44,6 @@ ) from dimos.manipulation.planning.spec.protocols import WorldSpec from dimos.manipulation.planning.utils.path_utils import compute_path_length -from dimos.msgs.sensor_msgs.JointState import JointState from dimos.utils.logging_config import setup_logger if TYPE_CHECKING: @@ -104,7 +104,7 @@ def plan_joint_path( # Extract positions as numpy arrays for internal computation q_start = np.array(start.position, dtype=np.float64) q_goal = np.array(goal.position, dtype=np.float64) - joint_names = start.name # Store for converting back to JointState + joint_names = list(start.name) # Store for converting back to JointState error = self._validate_inputs(world, start, goal) if error is not None: @@ -471,8 +471,8 @@ def _extend_tree( new_config = nearest.config + step_size * (diff / dist) # Check validity of edge using context-free method - start_state = JointState({"name": joint_names, "position": nearest.config.tolist()}) - end_state = JointState({"name": joint_names, "position": new_config.tolist()}) + start_state = JointState(name=joint_names, position=nearest.config.tolist()) + end_state = JointState(name=joint_names, position=new_config.tolist()) if world.check_edge_collision_free(start_state, end_state, self._collision_step_size): new_node = TreeNode(config=new_config, parent=nearest) nearest.children.append(new_node) @@ -519,7 +519,7 @@ def _extract_path( full_path_arrays = start_path + list(reversed(goal_path)) # Convert to list of JointState - return [JointState({"name": joint_names, "position": q.tolist()}) for q in full_path_arrays] + return [JointState(name=joint_names, position=q.tolist()) for q in full_path_arrays] def _simplify_path( self, diff --git a/dimos/manipulation/planning/planners/selected_joint_space.py b/dimos/manipulation/planning/planners/selected_joint_space.py index 2a1ac19bfa9..ff792ae0f84 100644 --- a/dimos/manipulation/planning/planners/selected_joint_space.py +++ b/dimos/manipulation/planning/planners/selected_joint_space.py @@ -18,6 +18,7 @@ from dataclasses import dataclass +from dimos_generated.sensor_msgs.msg import JointState import numpy as np from numpy.typing import NDArray @@ -25,7 +26,6 @@ from dimos.manipulation.planning.spec.joint_space import JointSpace from dimos.manipulation.planning.spec.models import JointPath from dimos.manipulation.planning.spec.protocols import WorldSpec -from dimos.msgs.sensor_msgs.JointState import JointState @dataclass(frozen=True) diff --git a/dimos/manipulation/planning/planners/test_rrt_planner_selection.py b/dimos/manipulation/planning/planners/test_rrt_planner_selection.py index 64ffb146ab9..fe5c85c0d92 100644 --- a/dimos/manipulation/planning/planners/test_rrt_planner_selection.py +++ b/dimos/manipulation/planning/planners/test_rrt_planner_selection.py @@ -20,6 +20,17 @@ import math from pathlib import Path +from dimos_generated.geometry_msgs.msg import ( + Point, + Pose, + PoseStamped, + Quaternion, + Transform, + TransformStamped, + Vector3, +) +from dimos_generated.sensor_msgs.msg import JointState +from dimos_generated.std_msgs.msg import Header import numpy as np import pytest from pytest_mock import MockerFixture @@ -40,16 +51,14 @@ JointSpace, ) from dimos.manipulation.planning.spec.validation import PreparedRobotModel -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Transform import Transform -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.msgs.sensor_msgs.JointState import JointState from dimos.robot.assets.model import LoadedRobotModel, PlanarBaseDefinition, RobotModel def _pose() -> PoseStamped: - return PoseStamped(position=Vector3(), orientation=Quaternion(0.0, 0.0, 0.0, 1.0)) + return PoseStamped( + header=Header(frame_id=""), + pose=Pose(position=Point(), orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0)), + ) def _group(name: str, joints: tuple[str, ...]) -> PlanningGroup: @@ -103,9 +112,7 @@ def scratch_context(self) -> nullcontext[None]: return nullcontext(None) def get_joint_state(self, ctx: object) -> JointState: - return JointState( - {"name": ["arm/joint_a", "arm/joint_b", "arm/gripper"], "position": self.current} - ) + return JointState(name=["arm/joint_a", "arm/joint_b", "arm/gripper"], position=self.current) def get_joint_limits(self) -> tuple[np.ndarray, np.ndarray]: return np.array([-1.0, -1.0, -1.0]), np.array([1.0, 1.0, 1.0]) @@ -197,14 +204,14 @@ def check_config_collision_free(self, joint_state: JointState) -> bool: ("start", "goal", "expected_start", "expected_goal"), [ ( - JointState({"position": [0.1, 0.2]}), - JointState({"position": [0.3, 0.4]}), + JointState(position=[0.1, 0.2]), + JointState(position=[0.3, 0.4]), [0.1, 0.2], [0.3, 0.4], ), ( - JointState({"name": ["arm/joint_b", "arm/joint_a"], "position": [0.2, 0.1]}), - JointState({"name": ["arm/joint_b", "arm/joint_a"], "position": [0.4, 0.3]}), + JointState(name=["arm/joint_b", "arm/joint_a"], position=[0.2, 0.1]), + JointState(name=["arm/joint_b", "arm/joint_a"], position=[0.4, 0.3]), [0.1, 0.2], [0.3, 0.4], ), @@ -229,22 +236,20 @@ def test_plan_selected_joint_path_normalizes_target_forms( ("start", "goal", "status", "message"), [ ( - JointState({"name": ["arm/joint_a"], "position": [0.0]}), - JointState({"position": [0.0, 0.0]}), + JointState(name=["arm/joint_a"], position=[0.0]), + JointState(position=[0.0, 0.0]), PlanningStatus.INVALID_START, "missing", ), ( - JointState({"position": [0.0, 0.0]}), - JointState( - {"name": ["arm/joint_a", "arm/joint_b", "arm/extra"], "position": [0.0, 0.0, 0.0]} - ), + JointState(position=[0.0, 0.0]), + JointState(name=["arm/joint_a", "arm/joint_b", "arm/extra"], position=[0.0, 0.0, 0.0]), PlanningStatus.INVALID_GOAL, "extra", ), ( - JointState({"name": ["arm/joint_a", "joint_b"], "position": [0.0, 0.0]}), - JointState({"position": [0.0, 0.0]}), + JointState(name=["arm/joint_a", "joint_b"], position=[0.0, 0.0]), + JointState(position=[0.0, 0.0]), PlanningStatus.INVALID_START, "missing", ), @@ -270,8 +275,8 @@ def test_plan_selected_joint_path_rejects_noncanonical_names() -> None: result = RRTConnectPlanner().plan_selected_joint_path( _World(), selection, - JointState({"name": ["joint_a", "gripper"], "position": [0.0, 0.0]}), - JointState({"position": [0.1, 0.2]}), + JointState(name=["joint_a", "gripper"], position=[0.0, 0.0]), + JointState(position=[0.1, 0.2]), ) assert result.status == PlanningStatus.INVALID_START @@ -285,8 +290,8 @@ def test_plan_selected_joint_path_direct_edge_projects_full_state_with_unselecte result = RRTConnectPlanner().plan_selected_joint_path( world, PlanningGroupSelection.from_groups((group,)), - JointState({"position": [0.1, 0.2]}), - JointState({"position": [0.3, 0.4]}), + JointState(position=[0.1, 0.2]), + JointState(position=[0.3, 0.4]), ) assert result.status == PlanningStatus.SUCCESS @@ -305,11 +310,15 @@ def test_plan_cartesian_path_is_explicitly_unsupported() -> None: result = RRTConnectPlanner().plan_cartesian_path( _World(), selection, - JointState({"position": [0.0, 0.0]}), + JointState(position=[0.0, 0.0]), { group.id: ( - Transform.identity(), - Transform(translation=Vector3(0.1, 0.0, 0.0)), + TransformStamped(header=Header(frame_id="world"), child_frame_id=""), + TransformStamped( + header=Header(frame_id="world"), + transform=Transform(translation=Vector3(x=0.1, y=0.0, z=0.0)), + child_frame_id="", + ), ) }, RoboPlanCartesianPathConfig(), diff --git a/dimos/manipulation/planning/spec/config.py b/dimos/manipulation/planning/spec/config.py index 79b6da3ea71..2f75d302a38 100644 --- a/dimos/manipulation/planning/spec/config.py +++ b/dimos/manipulation/planning/spec/config.py @@ -18,13 +18,13 @@ from pathlib import Path +from dimos_generated.geometry_msgs.msg import PoseStamped from pydantic import Field from dimos.core.module import ModuleConfig from dimos.manipulation.grasp_verification import GraspVerificationConfig from dimos.manipulation.planning.groups.identifiers import assert_valid_joint_names from dimos.manipulation.planning.groups.models import PlanningGroupDefinition -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped from dimos.robot.assets.model import RobotModel diff --git a/dimos/manipulation/planning/spec/joint_space.py b/dimos/manipulation/planning/spec/joint_space.py index 98cd89f3d3c..f1c0374ab4e 100644 --- a/dimos/manipulation/planning/spec/joint_space.py +++ b/dimos/manipulation/planning/spec/joint_space.py @@ -23,13 +23,13 @@ import math from typing import Annotated, Literal +from dimos_generated.sensor_msgs.msg import JointState import numpy as np from numpy.typing import NDArray from pydantic import ConfigDict, Field, model_validator from pydantic.dataclasses import dataclass as pydantic_dataclass from typing_extensions import Self -from dimos.msgs.sensor_msgs.JointState import JointState from dimos.utils.trigonometry import angle_diff @@ -137,7 +137,7 @@ def normalize_positions( def from_joint_state(self, state: JointState) -> NDArray[np.float64]: """Extract and validate this space's coordinates from a named state.""" if not state.name: - return self.normalize_positions(state.position) + return self.normalize_positions(np.asarray(state.position, dtype=np.float64)) if len(state.name) != len(set(state.name)): raise ValueError("Joint state contains duplicate coordinate names") positions = dict(zip(state.name, state.position, strict=True)) diff --git a/dimos/manipulation/planning/spec/models.py b/dimos/manipulation/planning/spec/models.py index 125b5939fb7..0ae43a458bb 100644 --- a/dimos/manipulation/planning/spec/models.py +++ b/dimos/manipulation/planning/spec/models.py @@ -28,15 +28,14 @@ ) if TYPE_CHECKING: + from dimos_generated.geometry_msgs.msg import PoseStamped, TransformStamped + from dimos_generated.sensor_msgs.msg import JointState + from dimos_generated.trajectory_msgs.msg import JointTrajectory import numpy as np from numpy.typing import NDArray from dimos.manipulation.planning.groups.models import PlanningGroup from dimos.manipulation.planning.spec.validation import PreparedRobotModel - from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped - from dimos.msgs.geometry_msgs.Transform import Transform - from dimos.msgs.sensor_msgs.JointState import JointState - from dimos.msgs.trajectory_msgs.JointTrajectory import JointTrajectory PlanningGroupID: TypeAlias = str @@ -49,10 +48,10 @@ """List of joint states forming a path (each waypoint has names + positions)""" -CartesianWaypoint: TypeAlias = "PoseStamped | Transform" +CartesianWaypoint: TypeAlias = "PoseStamped | TransformStamped" """One absolute TCP pose or relative rigid displacement from the planning start.""" -CartesianTarget: TypeAlias = "Sequence[PoseStamped] | Sequence[Transform]" +CartesianTarget: TypeAlias = "Sequence[PoseStamped] | Sequence[TransformStamped]" """Ordered homogeneous Cartesian waypoints for one planning group.""" diff --git a/dimos/manipulation/planning/spec/protocols.py b/dimos/manipulation/planning/spec/protocols.py index 4891ef23305..bf80e9b6a03 100644 --- a/dimos/manipulation/planning/spec/protocols.py +++ b/dimos/manipulation/planning/spec/protocols.py @@ -26,6 +26,10 @@ if TYPE_CHECKING: from contextlib import AbstractContextManager + from dimos_generated.dimos_msgs.msg import GraspCandidateArray + from dimos_generated.geometry_msgs.msg import PoseStamped + from dimos_generated.sensor_msgs.msg import JointState + from dimos_generated.trajectory_msgs.msg import JointTrajectory import numpy as np from numpy.typing import NDArray @@ -42,10 +46,6 @@ VisualizationStateFrame, ) from dimos.manipulation.planning.spec.validation import PreparedRobotModel - from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped - from dimos.msgs.manipulation_msgs.GraspCandidateArray import GraspCandidateArray - from dimos.msgs.sensor_msgs.JointState import JointState - from dimos.msgs.trajectory_msgs.JointTrajectory import JointTrajectory @runtime_checkable diff --git a/dimos/manipulation/planning/spec/test_joint_space.py b/dimos/manipulation/planning/spec/test_joint_space.py index 8bad5f17f83..4b3c5563f81 100644 --- a/dimos/manipulation/planning/spec/test_joint_space.py +++ b/dimos/manipulation/planning/spec/test_joint_space.py @@ -18,13 +18,13 @@ from pathlib import Path from typing import Any +from dimos_generated.sensor_msgs.msg import JointState import numpy as np import pytest from dimos.manipulation.planning.spec.config import RobotModelConfig from dimos.manipulation.planning.spec.joint_space import CoordinateTopology, JointCoordinate from dimos.manipulation.planning.spec.validation import prepare_robot_model -from dimos.msgs.sensor_msgs.JointState import JointState from dimos.robot.assets.model import RobotModel diff --git a/dimos/manipulation/planning/trajectory_generator/joint_trajectory_generator.py b/dimos/manipulation/planning/trajectory_generator/joint_trajectory_generator.py index e635ebbd03c..81226240591 100644 --- a/dimos/manipulation/planning/trajectory_generator/joint_trajectory_generator.py +++ b/dimos/manipulation/planning/trajectory_generator/joint_trajectory_generator.py @@ -32,8 +32,10 @@ import math -from dimos.msgs.trajectory_msgs.JointTrajectory import JointTrajectory -from dimos.msgs.trajectory_msgs.TrajectoryPoint import TrajectoryPoint +from dimos_generated.trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint + +from dimos.msgs.time import duration_from_seconds, header_now, to_seconds +from dimos.msgs.trajectory import trajectory_duration class JointTrajectoryGenerator: @@ -112,13 +114,13 @@ def generate(self, waypoints: list[list[float]]) -> JointTrajectory: if not waypoints or len(waypoints) < 2: raise ValueError("Need at least 2 waypoints") - all_points: list[TrajectoryPoint] = [] + all_points: list[JointTrajectoryPoint] = [] current_time = 0.0 # Add first waypoint all_points.append( - TrajectoryPoint( - time_from_start=0.0, + JointTrajectoryPoint( + time_from_start=duration_from_seconds(0.0), positions=list(waypoints[0]), velocities=[0.0] * self.num_joints, ) @@ -136,14 +138,14 @@ def generate(self, waypoints: list[list[float]]) -> JointTrajectory: all_points.extend(segment_points[1:]) current_time += segment_duration - return JointTrajectory(points=all_points) + return JointTrajectory(header=header_now(), points=all_points) def _generate_segment( self, start: list[float], end: list[float], start_time: float, - ) -> tuple[list[TrajectoryPoint], float]: + ) -> tuple[list[JointTrajectoryPoint], float]: """ Generate trajectory points for a single segment using trapezoidal profile. @@ -153,7 +155,7 @@ def _generate_segment( start_time: Time offset for this segment Returns: - Tuple of (list of TrajectoryPoints, segment duration) + Tuple of (list of JointTrajectoryPoints, segment duration) """ # Calculate displacement for each joint displacements = [end[j] - start[j] for j in range(self.num_joints)] @@ -172,7 +174,7 @@ def _generate_segment( segment_duration = max(segment_duration, 0.01) # Generate points along the segment - points: list[TrajectoryPoint] = [] + points: list[JointTrajectoryPoint] = [] for i in range(self.points_per_segment + 1): # Normalized time [0, 1] @@ -204,8 +206,8 @@ def _generate_segment( velocities.append(vel) points.append( - TrajectoryPoint( - time_from_start=t, + JointTrajectoryPoint( + time_from_start=duration_from_seconds(t), positions=positions, velocities=velocities, ) @@ -429,7 +431,7 @@ def preview(self, trajectory: JointTrajectory) -> str: lines = [ "Trajectory Preview", "=" * 60, - f"Duration: {trajectory.duration:.3f}s", + f"Duration: {trajectory_duration(trajectory):.3f}s", f"Points: {len(trajectory.points)}", "", "Waypoints (time -> positions):", @@ -447,7 +449,7 @@ def preview(self, trajectory: JointTrajectory) -> str: pt = trajectory.points[i] pos_str = ", ".join(f"{p:+.3f}" for p in pt.positions) vel_str = ", ".join(f"{v:+.3f}" for v in pt.velocities) - lines.append(f" t={pt.time_from_start:6.3f}s: pos=[{pos_str}]") + lines.append(f" t={to_seconds(pt.time_from_start):6.3f}s: pos=[{pos_str}]") lines.append(f" vel=[{vel_str}]") lines.append("-" * 60) diff --git a/dimos/manipulation/planning/trajectory_generator/parametrizer.py b/dimos/manipulation/planning/trajectory_generator/parametrizer.py index 3497b29e94d..6cc100c94ba 100644 --- a/dimos/manipulation/planning/trajectory_generator/parametrizer.py +++ b/dimos/manipulation/planning/trajectory_generator/parametrizer.py @@ -16,14 +16,16 @@ from abc import ABC, abstractmethod from collections.abc import Sequence +import copy import math +from dimos_generated.sensor_msgs.msg import JointState +from dimos_generated.trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint + from dimos.manipulation.planning.groups.models import PlanningGroupSelection from dimos.manipulation.planning.spec.models import GeneratedPlan, PlanningResult from dimos.manipulation.planning.spec.protocols import WorldSpec -from dimos.msgs.sensor_msgs.JointState import JointState -from dimos.msgs.trajectory_msgs.JointTrajectory import JointTrajectory -from dimos.msgs.trajectory_msgs.TrajectoryPoint import TrajectoryPoint +from dimos.msgs.time import duration_from_seconds, header_now, to_nanoseconds _TRAJECTORY_POSITION_TOLERANCE = 1e-6 @@ -49,7 +51,7 @@ def materialize_plan( f"Cannot materialize unsuccessful planning result: {result.status.name}" ) - path = [JointState(state) for state in result.path] + path = [copy.deepcopy(state) for state in result.path] self._validate_selected_path(path, selection.joint_names) if result.timestamps is None: trajectory = self._parametrize_path(world, selection, tuple(path), speed_scale) @@ -58,8 +60,8 @@ def materialize_plan( self._validate_trajectory( trajectory, selection.joint_names, - expected_start=path[0].position, - expected_goal=path[-1].position, + expected_start=list(path[0].position), + expected_goal=list(path[-1].position), ) return GeneratedPlan( @@ -127,7 +129,7 @@ def _timed_trajectory( ) -> JointTrajectory: if len(timestamps) != len(path): raise TrajectoryParametrizationError("Planner must return one timestamp per waypoint") - points: list[TrajectoryPoint] = [] + points: list[JointTrajectoryPoint] = [] for waypoint_index, (state, timestamp) in enumerate(zip(path, timestamps, strict=True)): velocities = list(state.velocity) if len(velocities) != len(selection.joint_names): @@ -135,13 +137,14 @@ def _timed_trajectory( f"Waypoint {waypoint_index} velocity dimension mismatch" ) points.append( - TrajectoryPoint( - time_from_start=float(timestamp), + JointTrajectoryPoint( + time_from_start=duration_from_seconds(float(timestamp)), positions=list(state.position), velocities=velocities, ) ) return JointTrajectory( + header=header_now(), joint_names=list(selection.joint_names), points=points, ) @@ -162,36 +165,38 @@ def _validate_trajectory( ) if not trajectory.points: raise TrajectoryParametrizationError("Generated trajectory has no points") - previous_time: float | None = None + previous_time: int | None = None for point_index, point in enumerate(trajectory.points): if len(point.positions) != len(expected) or len(point.velocities) != len(expected): raise TrajectoryParametrizationError( f"Generated point {point_index} dimension mismatch" ) cls._assert_finite_sequence( - point.positions, + list(point.positions), f"Generated point {point_index} positions", ) cls._assert_finite_sequence( - point.velocities, + list(point.velocities), f"Generated point {point_index} velocities", ) - if not math.isfinite(point.time_from_start): + try: + point_time = to_nanoseconds(point.time_from_start) + except ValueError as exc: raise TrajectoryParametrizationError( - f"Generated point {point_index} time is non-finite" - ) - if point_index == 0 and point.time_from_start != 0.0: + f"Generated point {point_index} duration is invalid" + ) from exc + if point_index == 0 and point_time != 0.0: raise TrajectoryParametrizationError("Generated trajectory must start at time 0") - if previous_time is not None and point.time_from_start <= previous_time: + if previous_time is not None and point_time <= previous_time: raise TrajectoryParametrizationError( "Generated trajectory times must be strictly increasing" ) - previous_time = point.time_from_start - if not cls._positions_close(trajectory.points[0].positions, expected_start): + previous_time = point_time + if not cls._positions_close(list(trajectory.points[0].positions), expected_start): raise TrajectoryParametrizationError( "Generated trajectory does not preserve the path start" ) - if not cls._positions_close(trajectory.points[-1].positions, expected_goal): + if not cls._positions_close(list(trajectory.points[-1].positions), expected_goal): raise TrajectoryParametrizationError( "Generated trajectory does not preserve the path goal" ) diff --git a/dimos/manipulation/planning/trajectory_generator/roboplan_toppra_parametrizer.py b/dimos/manipulation/planning/trajectory_generator/roboplan_toppra_parametrizer.py index b41e16975e3..061f9d57496 100644 --- a/dimos/manipulation/planning/trajectory_generator/roboplan_toppra_parametrizer.py +++ b/dimos/manipulation/planning/trajectory_generator/roboplan_toppra_parametrizer.py @@ -19,6 +19,8 @@ import sys from typing import Any +from dimos_generated.sensor_msgs.msg import JointState +from dimos_generated.trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint import numpy as np import roboplan.core as roboplan_core import roboplan.toppra as roboplan_toppra @@ -35,9 +37,7 @@ ) from dimos.manipulation.planning.world.roboplan_model import RoboPlanGroup, RoboPlanModel from dimos.manipulation.planning.world.roboplan_world import RoboPlanWorld -from dimos.msgs.sensor_msgs.JointState import JointState -from dimos.msgs.trajectory_msgs.JointTrajectory import JointTrajectory -from dimos.msgs.trajectory_msgs.TrajectoryPoint import TrajectoryPoint +from dimos.msgs.time import duration_from_seconds, header_now @dataclass(frozen=True) @@ -231,8 +231,8 @@ def _canonical_result( [joint_space.normalize_positions(position) for position in canonical_positions] ) points = [ - TrajectoryPoint( - time_from_start=time, + JointTrajectoryPoint( + time_from_start=duration_from_seconds(time), positions=list(position), velocities=[float(velocity[index]) for index in output_indices], ) @@ -244,6 +244,7 @@ def _canonical_result( ) ] return JointTrajectory( + header=header_now(), joint_names=list(selection.joint_names), points=points, ) diff --git a/dimos/manipulation/planning/trajectory_generator/simple_parametrizer.py b/dimos/manipulation/planning/trajectory_generator/simple_parametrizer.py index 4f06d783c27..b1f2218ee67 100644 --- a/dimos/manipulation/planning/trajectory_generator/simple_parametrizer.py +++ b/dimos/manipulation/planning/trajectory_generator/simple_parametrizer.py @@ -16,6 +16,9 @@ import math +from dimos_generated.sensor_msgs.msg import JointState +from dimos_generated.trajectory_msgs.msg import JointTrajectory + from dimos.manipulation.planning.groups.models import PlanningGroupSelection from dimos.manipulation.planning.spec.protocols import WorldSpec from dimos.manipulation.planning.trajectory_generator.config import ( @@ -28,8 +31,6 @@ BaseTrajectoryParametrizer, TrajectoryParametrizationError, ) -from dimos.msgs.sensor_msgs.JointState import JointState -from dimos.msgs.trajectory_msgs.JointTrajectory import JointTrajectory class SimpleTrapezoidParametrizer(BaseTrajectoryParametrizer): @@ -71,7 +72,7 @@ def _parametrize_path( return JointTrajectory( joint_names=list(selection.joint_names), points=generated.points, - timestamp=generated.timestamp, + header=generated.header, ) @staticmethod diff --git a/dimos/manipulation/planning/trajectory_generator/test_parametrizer.py b/dimos/manipulation/planning/trajectory_generator/test_parametrizer.py index cb303cde22a..7c53e113f86 100644 --- a/dimos/manipulation/planning/trajectory_generator/test_parametrizer.py +++ b/dimos/manipulation/planning/trajectory_generator/test_parametrizer.py @@ -16,6 +16,8 @@ from unittest.mock import MagicMock +from dimos_generated.sensor_msgs.msg import JointState +from dimos_generated.trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint import pytest from dimos.manipulation.planning.groups.models import ( @@ -29,9 +31,7 @@ BaseTrajectoryParametrizer, TrajectoryParametrizationError, ) -from dimos.msgs.sensor_msgs.JointState import JointState -from dimos.msgs.trajectory_msgs.JointTrajectory import JointTrajectory -from dimos.msgs.trajectory_msgs.TrajectoryPoint import TrajectoryPoint +from dimos.msgs.time import duration_from_seconds, header_now, to_seconds class _FixedParametrizer(BaseTrajectoryParametrizer): @@ -73,15 +73,16 @@ def _path() -> list[JointState]: def _output() -> JointTrajectory: return JointTrajectory( + header=header_now(), joint_names=["arm/a", "arm/b"], points=[ - TrajectoryPoint( - time_from_start=0.0, + JointTrajectoryPoint( + time_from_start=duration_from_seconds(0.0), positions=[0.0, 0.0], velocities=[0.0, 0.0], ), - TrajectoryPoint( - time_from_start=0.5, + JointTrajectoryPoint( + time_from_start=duration_from_seconds(0.5), positions=[0.4, 0.0], velocities=[0.0, 0.0], ), @@ -143,7 +144,7 @@ def test_timed_planner_result_bypasses_backend_path_conversion() -> None: ) assert parametrizer.calls == [] - assert [point.time_from_start for point in plan.trajectory.points] == [ + assert [to_seconds(point.time_from_start) for point in plan.trajectory.points] == [ 0.0, 0.75, ] @@ -168,7 +169,9 @@ def test_timed_planner_result_requires_velocity_for_each_joint() -> None: def test_rejects_backend_trajectory_with_nonincreasing_time() -> None: output = _output() - output.points[-1].time_from_start = 0.0 + point = output.points[-1] + point.time_from_start = duration_from_seconds(0.0) + output.points[-1] = point parametrizer = _FixedParametrizer(output) with pytest.raises(TrajectoryParametrizationError, match="strictly increasing"): @@ -181,7 +184,9 @@ def test_rejects_backend_trajectory_with_nonincreasing_time() -> None: def test_rejects_backend_trajectory_that_changes_path_goal() -> None: output = _output() - output.points[-1].positions = [0.3, 0.0] + point = output.points[-1] + point.positions = [0.3, 0.0] + output.points[-1] = point parametrizer = _FixedParametrizer(output) with pytest.raises(TrajectoryParametrizationError, match="path goal"): diff --git a/dimos/manipulation/planning/trajectory_generator/test_roboplan_toppra_parametrizer.py b/dimos/manipulation/planning/trajectory_generator/test_roboplan_toppra_parametrizer.py index 52b77ea4325..2359a76e8c7 100644 --- a/dimos/manipulation/planning/trajectory_generator/test_roboplan_toppra_parametrizer.py +++ b/dimos/manipulation/planning/trajectory_generator/test_roboplan_toppra_parametrizer.py @@ -27,6 +27,8 @@ pytest.importorskip("roboplan.toppra") +from dimos_generated.sensor_msgs.msg import JointState + from dimos.manipulation.planning.groups.models import ( PlanningGroup, PlanningGroupDefinition, @@ -53,7 +55,6 @@ ) from dimos.manipulation.planning.world.roboplan_model import RoboPlanGroup, RoboPlanModel from dimos.manipulation.planning.world.roboplan_world import RoboPlanWorld -from dimos.msgs.sensor_msgs.JointState import JointState from dimos.robot.assets.model import PlanarBaseDefinition, RobotModel pytestmark = pytest.mark.self_hosted @@ -319,7 +320,7 @@ def test_roboplan_toppra_parametrizes_unbounded_planar_base(tmp_path: Path) -> N world, selection, result ) - positions = [point.positions for point in plan.trajectory.points] + positions = [list(point.positions) for point in plan.trajectory.points] assert positions[0] == pytest.approx(start) assert positions[-1] == pytest.approx(goal) assert max(abs(current[2] - previous[2]) for previous, current in pairwise(positions)) < math.pi diff --git a/dimos/manipulation/planning/trajectory_generator/test_simple_parametrizer.py b/dimos/manipulation/planning/trajectory_generator/test_simple_parametrizer.py index ad82cf9510b..376bb2dca19 100644 --- a/dimos/manipulation/planning/trajectory_generator/test_simple_parametrizer.py +++ b/dimos/manipulation/planning/trajectory_generator/test_simple_parametrizer.py @@ -17,6 +17,9 @@ from pathlib import Path from unittest.mock import MagicMock +from dimos_generated.geometry_msgs.msg import Pose, PoseStamped +from dimos_generated.sensor_msgs.msg import JointState +from dimos_generated.std_msgs.msg import Header import pytest from pytest_mock import MockerFixture @@ -43,8 +46,6 @@ from dimos.manipulation.planning.trajectory_generator.simple_parametrizer import ( SimpleTrapezoidParametrizer, ) -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.sensor_msgs.JointState import JointState from dimos.robot.assets.model import LoadedRobotModel, RobotModel @@ -64,7 +65,7 @@ def _selection() -> PlanningGroupSelection: def _world(*, velocity: float = 2.0, acceleration: float = 6.0) -> WorldSpec: config = RobotModelConfig( model=RobotModel.from_file(Path("/robot.urdf")), - base_pose=PoseStamped(), + base_pose=PoseStamped(header=Header(frame_id=""), pose=Pose()), joint_names=["arm/a", "arm/b"], base_link="base", ) diff --git a/dimos/manipulation/planning/utils/path_utils.py b/dimos/manipulation/planning/utils/path_utils.py index 072e8cc7a42..290c00949aa 100644 --- a/dimos/manipulation/planning/utils/path_utils.py +++ b/dimos/manipulation/planning/utils/path_utils.py @@ -30,10 +30,9 @@ from typing import TYPE_CHECKING +from dimos_generated.sensor_msgs.msg import JointState import numpy as np -from dimos.msgs.sensor_msgs.JointState import JointState - if TYPE_CHECKING: from numpy.typing import NDArray diff --git a/dimos/manipulation/planning/utils/point_cloud_self_filter.py b/dimos/manipulation/planning/utils/point_cloud_self_filter.py index 626c6d630e1..fdfc6825d02 100644 --- a/dimos/manipulation/planning/utils/point_cloud_self_filter.py +++ b/dimos/manipulation/planning/utils/point_cloud_self_filter.py @@ -22,6 +22,10 @@ from io import BytesIO from typing import Any +from dimos_generated.geometry_msgs.msg import TransformStamped +from dimos_generated.sensor_msgs.msg import PointCloud2 +from dimos_generated.std_msgs.msg import Header +from dimos_generated.tf2_msgs.msg import TFMessage import numpy as np from pydantic import Field import trimesh @@ -30,9 +34,9 @@ from dimos.core.core import rpc from dimos.core.module import Module, ModuleConfig from dimos.core.stream import In, Out -from dimos.msgs.geometry_msgs.Transform import Transform -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 -from dimos.msgs.tf2_msgs.TFMessage import TFMessage +from dimos.msgs.geometry import transform_matrix +from dimos.msgs.pointcloud import pointcloud_from_xyz, pointcloud_xyz, select_points +from dimos.msgs.time import to_seconds from dimos.protocol.tf.tf import TF from dimos.robot.assets.model import RobotModel from dimos.utils.logging_config import setup_logger @@ -101,13 +105,17 @@ async def handle_pointcloud(self, cloud: PointCloud2) -> None: def filter_cloud(self, cloud: PointCloud2) -> tuple[PointCloud2, PointCloud2] | None: """Filter one capture and build the matching world-frame clear mask.""" config = self.config - points = cloud.points_f32() + points = pointcloud_xyz(cloud) keep = np.ones(len(points), dtype=bool) current_clear_keys: set[tuple[int, int, int]] = set() for geometry in self._collision_geometry: - sensor_from_link = self._lookup(cloud.frame_id, geometry.link, cloud.ts) - world_from_link = self._lookup(config.world_frame, geometry.link, cloud.ts) + sensor_from_link = self._lookup( + cloud.header.frame_id, geometry.link, to_seconds(cloud.header.stamp) + ) + world_from_link = self._lookup( + config.world_frame, geometry.link, to_seconds(cloud.header.stamp) + ) if sensor_from_link is None or world_from_link is None: logger.warning( "Dropping cloud: capture-time TF unavailable for robot link %s", geometry.link @@ -115,7 +123,9 @@ def filter_cloud(self, cloud: PointCloud2) -> tuple[PointCloud2, PointCloud2] | return None if len(points): - sensor_from_geometry = sensor_from_link.to_matrix() @ geometry.link_from_geometry + sensor_from_geometry = ( + transform_matrix(sensor_from_link.transform) @ geometry.link_from_geometry + ) local = _transform_points(points, np.linalg.inv(sensor_from_geometry)) keep &= ~_points_inside( local, @@ -125,7 +135,9 @@ def filter_cloud(self, cloud: PointCloud2) -> tuple[PointCloud2, PointCloud2] | config.padding_m, ) - world_from_geometry = world_from_link.to_matrix() @ geometry.link_from_geometry + world_from_geometry = ( + transform_matrix(world_from_link.transform) @ geometry.link_from_geometry + ) world_samples = _transform_points(geometry.clear_samples, world_from_geometry) keys = np.floor(world_samples / config.voxel_size).astype(np.int32) current_clear_keys.update(map(tuple, keys.tolist())) @@ -137,25 +149,14 @@ def filter_cloud(self, cloud: PointCloud2) -> tuple[PointCloud2, PointCloud2] | clear_points = ( np.asarray(sorted(clear_keys), dtype=np.float32).reshape((-1, 3)) + 0.5 ) * config.voxel_size - clear_mask = PointCloud2.from_numpy( + clear_mask = pointcloud_from_xyz( clear_points, - frame_id=config.world_frame, - timestamp=cloud.ts, - ) - - intensities = cloud.intensities_f32() - filtered = PointCloud2.from_numpy( - points[keep], - frame_id=cloud.frame_id, - timestamp=cloud.ts, - intensities=intensities[keep] if intensities is not None else None, + header=Header(frame_id=config.world_frame, stamp=cloud.header.stamp), ) - for name, values in cloud.pointcloud_tensor.point.items(): - if name not in ("positions", "intensities"): - filtered.pointcloud_tensor.point[name] = values[keep] + filtered = select_points(cloud, keep) return filtered, clear_mask - def _lookup(self, parent_frame: str, child_frame: str, stamp: float) -> Transform | None: + def _lookup(self, parent_frame: str, child_frame: str, stamp: float) -> TransformStamped | None: config = self.config return self.tfbuffer.get( parent_frame, diff --git a/dimos/manipulation/planning/utils/test_point_cloud_self_filter.py b/dimos/manipulation/planning/utils/test_point_cloud_self_filter.py index b24d352e307..ea35ae0c105 100644 --- a/dimos/manipulation/planning/utils/test_point_cloud_self_filter.py +++ b/dimos/manipulation/planning/utils/test_point_cloud_self_filter.py @@ -29,10 +29,13 @@ "yourdfpy", reason="yourdfpy is unavailable in the Linux ARM test environment" ) +from dimos_generated.geometry_msgs.msg import Transform, TransformStamped, Vector3 +from dimos_generated.sensor_msgs.msg import PointCloud2 +from dimos_generated.std_msgs.msg import Header + from dimos.manipulation.planning.utils.point_cloud_self_filter import PointCloudSelfFilter -from dimos.msgs.geometry_msgs.Transform import Transform -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 +from dimos.msgs.pointcloud import pointcloud_from_xyz, pointcloud_xyz +from dimos.msgs.time import time_from_seconds from dimos.protocol.tf.tf import MultiTBuffer from dimos.robot.assets.model import RobotModel @@ -81,25 +84,23 @@ def _place_arm(module: PointCloudSelfFilter, at: tuple[float, float, float], ts: """Put the arm at `at` in both the camera and world frames.""" for parent in ("camera", "world"): module.tfbuffer.receive_transform( - Transform( - translation=Vector3(*at), - frame_id=parent, + TransformStamped( + transform=Transform(translation=Vector3(x=at[0], y=at[1], z=at[2])), + header=Header(frame_id=parent, stamp=time_from_seconds(ts)), child_frame_id="arm", - ts=ts, ) ) def _cloud(points: list[list[float]], ts: float = 1.0) -> PointCloud2: - return PointCloud2.from_numpy( + return pointcloud_from_xyz( np.asarray(points, dtype=np.float32).reshape((-1, 3)), - frame_id="camera", - timestamp=ts, + header=Header(frame_id="camera", stamp=time_from_seconds(ts)), ) def _keys(cloud: PointCloud2, voxel_size: float) -> set[tuple[int, int, int]]: - points = cloud.points_f32() + points = pointcloud_xyz(cloud) if not len(points): return set() return {tuple(k) for k in np.floor(points / voxel_size).astype(int).tolist()} @@ -115,7 +116,7 @@ def test_points_on_the_robot_are_dropped_and_the_rest_survive( assert result is not None filtered, _ = result - np.testing.assert_allclose(filtered.points_f32(), [[2.0, 0.0, 0.0]], atol=1e-6) + np.testing.assert_allclose(pointcloud_xyz(filtered), [[2.0, 0.0, 0.0]], atol=1e-6) def test_the_mask_also_covers_where_the_robot_just_was( @@ -155,7 +156,7 @@ def test_the_mask_quantizes_the_way_the_mapper_does( result = module.filter_cloud(_cloud([])) assert result is not None - points = result[1].points_f32() + points = pointcloud_xyz(result[1]) offsets = points / 0.05 - np.floor(points / 0.05) np.testing.assert_allclose(offsets, 0.5, atol=1e-5) diff --git a/dimos/manipulation/planning/world/drake_world.py b/dimos/manipulation/planning/world/drake_world.py index 84181a117c6..0e685aaa7af 100644 --- a/dimos/manipulation/planning/world/drake_world.py +++ b/dimos/manipulation/planning/world/drake_world.py @@ -25,6 +25,8 @@ from typing import TYPE_CHECKING, Any import xml.etree.ElementTree as ET +from dimos_generated.geometry_msgs.msg import Point, Pose, Quaternion +from dimos_generated.std_msgs.msg import Header import numpy as np from dimos.manipulation.planning.groups.identifiers import assert_valid_group_id @@ -39,6 +41,9 @@ validate_obstacle, ) from dimos.manipulation.planning.utils.mesh_utils import prepare_urdf_for_drake +from dimos.msgs.geometry import pose_matrix +from dimos.msgs.time import to_seconds +from dimos.msgs.trajectory import trajectory_duration from dimos.robot.assets.model import LoadedRobotModel from dimos.utils.logging_config import setup_logger @@ -47,11 +52,10 @@ from numpy.typing import NDArray -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.geometry_msgs.Transform import Transform -from dimos.msgs.manipulation_msgs.GraspCandidateArray import GraspCandidateArray -from dimos.msgs.sensor_msgs.JointState import JointState -from dimos.msgs.trajectory_msgs.JointTrajectory import JointTrajectory +from dimos_generated.dimos_msgs.msg import GraspCandidateArray +from dimos_generated.geometry_msgs.msg import PoseStamped +from dimos_generated.sensor_msgs.msg import JointState +from dimos_generated.trajectory_msgs.msg import JointTrajectory if TYPE_CHECKING: from dimos.manipulation.planning.spec.models import ( @@ -537,11 +541,8 @@ def _add_obstacle_to_meshcat(self, obstacle: Obstacle, obstacle_id: str) -> None def _pose_to_rigid_transform(self, pose: PoseStamped) -> Any: """Convert PoseStamped to Drake RigidTransform.""" - pose_matrix = Transform( - translation=pose.position, - rotation=pose.orientation, - ).to_matrix() - return RigidTransform(pose_matrix) + matrix = pose_matrix(pose.pose) + return RigidTransform(matrix) def _create_shape(self, obstacle: Obstacle) -> Any: """Create Drake shape from obstacle specification.""" @@ -559,11 +560,8 @@ def _create_shape(self, obstacle: Obstacle) -> Any: raise ValueError(f"Unsupported obstacle type: {obstacle.obstacle_type}") def _validate_obstacle(self, obstacle: Obstacle, *, allow_empty_name: bool = False) -> None: - pose_matrix = Transform( - translation=obstacle.pose.position, - rotation=obstacle.pose.orientation, - ).to_matrix() - validate_obstacle(obstacle, pose_matrix, allow_empty_name=allow_empty_name) + matrix = pose_matrix(obstacle.pose.pose) + validate_obstacle(obstacle, matrix, allow_empty_name=allow_empty_name) def _remove_obstacle_geometry(self, obstacle_data: _ObstacleData) -> None: self._scene_graph.RemoveGeometry( @@ -994,9 +992,13 @@ def _get_group_ee_pose(self, ctx: Context, group_id: PlanningGroupID) -> PoseSta quat = X_WE.rotation().ToQuaternion() # Drake returns [w, x, y, z] return PoseStamped( - frame_id="world", - position=[float(pos[0]), float(pos[1]), float(pos[2])], - orientation=[float(quat.x()), float(quat.y()), float(quat.z()), float(quat.w())], + header=Header(frame_id="world"), + pose=Pose( + position=Point(x=float(pos[0]), y=float(pos[1]), z=float(pos[2])), + orientation=Quaternion( + x=float(quat.x()), y=float(quat.y()), z=float(quat.z()), w=float(quat.w()) + ), + ), ) def get_link_pose(self, ctx: Context, link_name: str) -> NDArray[np.float64]: @@ -1179,9 +1181,9 @@ def animate_trajectory( trajectory_indices = self._trajectory_indices(trajectory) playback_scale = 1.0 if duration is not None: - if duration <= 0.0 or trajectory.duration <= 0.0: + if duration <= 0.0 or trajectory_duration(trajectory) <= 0.0: raise ValueError("preview duration must be positive") - playback_scale = duration / trajectory.duration + playback_scale = duration / trajectory_duration(trajectory) with self._lock: assert self._plant_context is not None assert self._live_context is not None @@ -1199,7 +1201,7 @@ def animate_trajectory( ) try: - previous_time = trajectory.points[0].time_from_start + previous_time = to_seconds(trajectory.points[0].time_from_start) for frame_index, point in enumerate(trajectory.points): with self._lock: if self._preview_animation_generation != generation: @@ -1213,7 +1215,7 @@ def animate_trajectory( self._set_preview_positions(self._plant_context, positions) self._publish_visualization() if frame_index < len(trajectory.points) - 1: - next_time = trajectory.points[frame_index + 1].time_from_start + next_time = to_seconds(trajectory.points[frame_index + 1].time_from_start) time.sleep((next_time - previous_time) * playback_scale) previous_time = next_time finally: diff --git a/dimos/manipulation/planning/world/roboplan_model.py b/dimos/manipulation/planning/world/roboplan_model.py index ec1a229c38c..2355f2d4c09 100644 --- a/dimos/manipulation/planning/world/roboplan_model.py +++ b/dimos/manipulation/planning/world/roboplan_model.py @@ -24,6 +24,7 @@ import xml.etree.ElementTree as ET from xml.sax.saxutils import escape +from dimos_generated.geometry_msgs.msg import PoseStamped import numpy as np from dimos.manipulation.planning.groups.models import PlanningGroup @@ -32,8 +33,8 @@ from dimos.manipulation.planning.spec.models import PlanningGroupID from dimos.manipulation.planning.spec.validation import PreparedRobotModel from dimos.manipulation.planning.utils.mesh_utils import prepare_urdf_for_drake +from dimos.msgs.geometry import pose_matrix from dimos.robot.assets.model import LoadedRobotModel -from dimos.utils.transform_utils import pose_to_matrix ROBOPLAN_WORLD_FRAME = "dimos_world" @@ -337,8 +338,8 @@ def _apply_collision_exclusions(scene: Any, srdf: str) -> None: scene.setCollisions(first, second, False) -def _pose_attributes(pose: Any) -> dict[str, str]: - matrix = pose_to_matrix(pose) +def _pose_attributes(pose: PoseStamped) -> dict[str, str]: + matrix = pose_matrix(pose.pose) sy = float(np.hypot(matrix[0, 0], matrix[1, 0])) if sy > 1e-9: rpy = ( diff --git a/dimos/manipulation/planning/world/roboplan_world.py b/dimos/manipulation/planning/world/roboplan_world.py index a58bd390eb2..e037c45d3ad 100644 --- a/dimos/manipulation/planning/world/roboplan_world.py +++ b/dimos/manipulation/planning/world/roboplan_world.py @@ -28,6 +28,8 @@ from threading import RLock from typing import TYPE_CHECKING, Any +from dimos_generated.geometry_msgs.msg import Point, Pose, Quaternion +from dimos_generated.std_msgs.msg import Header import numpy as np try: @@ -38,6 +40,9 @@ "Install the manipulation extra before selecting the roboplan backend." ) from exc +from dimos_generated.geometry_msgs.msg import PoseStamped +from dimos_generated.sensor_msgs.msg import JointState + from dimos.manipulation.planning.groups.models import PlanningGroup from dimos.manipulation.planning.groups.registry import PlanningGroupRegistry from dimos.manipulation.planning.groups.utils import joint_state_to_ordered_positions @@ -58,10 +63,8 @@ RoboPlanModel, build_roboplan_model, ) -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.sensor_msgs.JointState import JointState +from dimos.msgs.geometry import pose_from_matrix, pose_matrix from dimos.utils.logging_config import setup_logger -from dimos.utils.transform_utils import matrix_to_pose, pose_to_matrix if TYPE_CHECKING: from collections.abc import Generator @@ -200,7 +203,7 @@ def update_obstacle_pose(self, obstacle_id: str, pose: PoseStamped) -> bool: with self._lock: self._require_finalized() replacement_pose = deepcopy(pose) - matrix = pose_to_matrix(replacement_pose) + matrix = pose_matrix(replacement_pose.pose) if not np.isfinite(matrix).all(): raise ValueError("Obstacle pose must contain only finite values") if obstacle_id not in self._obstacles: @@ -352,16 +355,18 @@ def get_group_ee_pose(self, ctx: RoboPlanContext, group_id: PlanningGroupID) -> if group.tip_link is None: raise ValueError(f"Planning group '{group_id}' has no tip link") mat = self.get_link_pose(ctx, group.tip_link) - pose = matrix_to_pose(mat) + pose = pose_from_matrix(mat) return PoseStamped( - frame_id="world", - position=[pose.position.x, pose.position.y, pose.position.z], - orientation=[ - pose.orientation.x, - pose.orientation.y, - pose.orientation.z, - pose.orientation.w, - ], + header=Header(frame_id="world"), + pose=Pose( + position=Point(x=pose.position.x, y=pose.position.y, z=pose.position.z), + orientation=Quaternion( + x=pose.orientation.x, + y=pose.orientation.y, + z=pose.orientation.z, + w=pose.orientation.w, + ), + ), ) def get_link_pose(self, ctx: RoboPlanContext, link_name: str) -> NDArray[np.float64]: @@ -417,7 +422,7 @@ def get_group_jacobian( def _validate_model_config(self, config: RobotModelConfig) -> None: if not config.joint_names: raise ValueError("RoboPlanWorld requires explicit joint_names") - if config.base_pose.frame_id not in ("", "world"): + if config.base_pose.header.frame_id not in ("", "world"): raise ValueError("RoboPlanWorld base_pose frame_id must be empty or 'world'") def _validate_planning_group_config(self, config: RobotModelConfig) -> None: @@ -540,7 +545,7 @@ def _call_path_collision_checker( def _add_obstacle_to_scene(self, obstacle: Obstacle, obstacle_id: str) -> None: scene = self._require_scene() - matrix = pose_to_matrix(obstacle.pose) + matrix = pose_matrix(obstacle.pose.pose) color = np.asarray(obstacle.color, dtype=np.float64) if obstacle.obstacle_type == ObstacleType.BOX: self._require_dimensions(obstacle, 3) @@ -595,7 +600,7 @@ def _add_obstacle_to_scene(self, obstacle: Obstacle, obstacle_id: str) -> None: def _validate_obstacle(self, obstacle: Obstacle, *, allow_empty_name: bool = False) -> None: validate_obstacle( - obstacle, pose_to_matrix(obstacle.pose), allow_empty_name=allow_empty_name + obstacle, pose_matrix(obstacle.pose.pose), allow_empty_name=allow_empty_name ) def _require_dimensions(self, obstacle: Obstacle, n_dims: int) -> None: diff --git a/dimos/manipulation/planning/world/test_drake_world_planning_groups.py b/dimos/manipulation/planning/world/test_drake_world_planning_groups.py index 0f377b3b415..1693fa99a38 100644 --- a/dimos/manipulation/planning/world/test_drake_world_planning_groups.py +++ b/dimos/manipulation/planning/world/test_drake_world_planning_groups.py @@ -17,6 +17,10 @@ from dataclasses import replace from pathlib import Path +from dimos_generated.geometry_msgs.msg import Point, Pose, PoseStamped, Quaternion +from dimos_generated.sensor_msgs.msg import JointState +from dimos_generated.std_msgs.msg import Header +from dimos_generated.trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint import numpy as np import pytest @@ -26,10 +30,7 @@ from dimos.manipulation.planning.spec.models import Obstacle from dimos.manipulation.planning.spec.validation import prepare_robot_model from dimos.manipulation.planning.world.drake_world import DRAKE_AVAILABLE, DrakeWorld -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.sensor_msgs.JointState import JointState -from dimos.msgs.trajectory_msgs.JointTrajectory import JointTrajectory -from dimos.msgs.trajectory_msgs.TrajectoryPoint import TrajectoryPoint +from dimos.msgs.time import duration_from_seconds, header_now from dimos.robot.assets.model import PlanarBaseDefinition, RobotModel requires_drake = pytest.mark.skipif( @@ -40,10 +41,19 @@ def _trajectory(names: list[str], first: list[float], second: list[float]) -> JointTrajectory: return JointTrajectory( + header=header_now(), joint_names=names, points=[ - TrajectoryPoint(time_from_start=0.0, positions=first, velocities=[0.0] * len(names)), - TrajectoryPoint(time_from_start=2.0, positions=second, velocities=[0.0] * len(names)), + JointTrajectoryPoint( + time_from_start=duration_from_seconds(0.0), + positions=first, + velocities=[0.0] * len(names), + ), + JointTrajectoryPoint( + time_from_start=duration_from_seconds(2.0), + positions=second, + velocities=[0.0] * len(names), + ), ], ) @@ -123,7 +133,10 @@ def _config( ) -> RobotModelConfig: return RobotModelConfig( model=RobotModel.from_file(path).with_default_joint_acceleration_limit(2.0), - base_pose=PoseStamped(position=[0, 0, 0], orientation=[0, 0, 0, 1]), + base_pose=PoseStamped( + header=Header(frame_id=""), + pose=Pose(position=Point(x=0, y=0, z=0), orientation=Quaternion(x=0, y=0, z=0, w=1)), + ), joint_names=joints or ["joint1", "joint2"], base_link="base_link", planning_groups=groups, @@ -214,7 +227,10 @@ def test_drake_obstacle_ids_are_world_owned_and_invalid_insertions_are_rejected( obstacle = Obstacle( name="box", obstacle_type=ObstacleType.BOX, - pose=PoseStamped(position=[2, 0, 0], orientation=[0, 0, 0, 1]), + pose=PoseStamped( + header=Header(frame_id=""), + pose=Pose(position=Point(x=2, y=0, z=0), orientation=Quaternion(x=0, y=0, z=0, w=1)), + ), dimensions=(0.1, 0.1, 0.1), ) unnamed = replace(obstacle, name="") @@ -240,10 +256,15 @@ def test_drake_obstacle_ids_are_world_owned_and_invalid_insertions_are_rejected( assert world.remove_obstacle("missing") is False assert world.update_obstacle(replace(obstacle, name="missing")) is False assert world.update_obstacle_pose("missing", obstacle.pose) is False - moved_pose = PoseStamped(position=[0.0, 0.0, 0.0], orientation=[0.0, 0.0, 0.0, 1.0]) + moved_pose = PoseStamped( + header=Header(frame_id=""), + pose=Pose( + position=Point(x=0.0, y=0.0, z=0.0), orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0) + ), + ) assert world.update_obstacle_pose("box", moved_pose) assert world._obstacles["box"].geometry_id != original_geometry_id - assert world.get_obstacles()[0].pose.position.x == pytest.approx(0.0) + assert world.get_obstacles()[0].pose.pose.position.x == pytest.approx(0.0) assert not world.check_config_collision_free(joint_state) replacement = replace( @@ -272,7 +293,10 @@ def test_drake_obstacle_replacement_failure_invalidates_world( obstacle = Obstacle( name="box", obstacle_type=ObstacleType.BOX, - pose=PoseStamped(position=[0, 0, 0], orientation=[0, 0, 0, 1]), + pose=PoseStamped( + header=Header(frame_id=""), + pose=Pose(position=Point(x=0, y=0, z=0), orientation=Quaternion(x=0, y=0, z=0, w=1)), + ), dimensions=(0.1, 0.1, 0.1), ) world.add_obstacle(obstacle) @@ -296,13 +320,13 @@ def test_drake_group_fk_uses_tip_link_and_unique_pose_group(tmp_path: Path) -> N _load(world, _config(urdf, [_arm_group("joint1", "joint2")])) world.finalize() ctx = world.get_live_context() - world.set_joint_state(ctx, JointState({"name": ["joint1", "joint2"], "position": [0.0, 0.0]})) + world.set_joint_state(ctx, JointState(name=["joint1", "joint2"], position=[0.0, 0.0])) group_pose = world.get_group_ee_pose(ctx, "arm") default_pose = world.get_ee_pose(ctx) - assert group_pose.position.x == pytest.approx(2.0) - assert default_pose.position.x == pytest.approx(group_pose.position.x) + assert group_pose.pose.position.x == pytest.approx(2.0) + assert default_pose.pose.position.x == pytest.approx(group_pose.pose.position.x) assert world.get_jacobian(ctx).shape == (6, 2) @@ -317,7 +341,12 @@ def test_drake_applies_config_base_pose_when_urdf_has_world_base_joint( world, RobotModelConfig( model=RobotModel.from_file(urdf).with_default_joint_acceleration_limit(2.0), - base_pose=PoseStamped(position=[0, 0.5, 0], orientation=[0, 0, 0, 1]), + base_pose=PoseStamped( + header=Header(frame_id=""), + pose=Pose( + position=Point(x=0, y=0.5, z=0), orientation=Quaternion(x=0, y=0, z=0, w=1) + ), + ), joint_names=["joint1", "joint2"], base_link="base_link", planning_groups=[_arm_group("joint1", "joint2")], @@ -349,7 +378,12 @@ def test_drake_planar_base_coordinates_move_original_robot_root(tmp_path: Path) .with_default_joint_acceleration_limit(2.0) .with_planar_base(planar_base) ), - base_pose=PoseStamped(position=[0, 0, 0.5], orientation=[0, 0, 0, 1]), + base_pose=PoseStamped( + header=Header(frame_id=""), + pose=Pose( + position=Point(x=0, y=0, z=0.5), orientation=Quaternion(x=0, y=0, z=0, w=1) + ), + ), joint_names=joint_names, base_link=planar_base.root_link, planning_groups=[ @@ -373,9 +407,9 @@ def test_drake_planar_base_coordinates_move_original_robot_root(tmp_path: Path) tool_pose = world.get_group_ee_pose(context, "mobile_arm") np.testing.assert_allclose(original_root_pose[:3, 3], [1.0, 2.0, 0.5], atol=1e-8) - assert tool_pose.position.x == pytest.approx(1.0) - assert tool_pose.position.y == pytest.approx(4.0) - assert tool_pose.position.z == pytest.approx(0.5) + assert tool_pose.pose.position.x == pytest.approx(1.0) + assert tool_pose.pose.position.y == pytest.approx(4.0) + assert tool_pose.pose.position.z == pytest.approx(0.5) @requires_drake @@ -395,7 +429,7 @@ def test_drake_group_jacobian_shape_and_group_local_order(tmp_path: Path) -> Non ) world.finalize() ctx = world.get_live_context() - world.set_joint_state(ctx, JointState({"name": ["joint1", "joint2"], "position": [0.0, 0.0]})) + world.set_joint_state(ctx, JointState(name=["joint1", "joint2"], position=[0.0, 0.0])) forward_jacobian = world.get_group_jacobian(ctx, "wrist_forward") reverse_jacobian = world.get_group_jacobian(ctx, "wrist_reverse") diff --git a/dimos/manipulation/sdk.py b/dimos/manipulation/sdk.py index bfc8814155e..ae31e9b4b92 100644 --- a/dimos/manipulation/sdk.py +++ b/dimos/manipulation/sdk.py @@ -19,6 +19,9 @@ import math from typing import TYPE_CHECKING +from dimos_generated.geometry_msgs.msg import Point, Pose, PoseStamped, Quaternion +from dimos_generated.sensor_msgs.msg import JointState +from dimos_generated.std_msgs.msg import Header import numpy as np from numpy.typing import ArrayLike, NDArray @@ -34,9 +37,6 @@ ) from dimos.manipulation.planning.groups.utils import joint_state_to_ordered_positions from dimos.manipulation.planning.spec.models import PlanningGroupID -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.sensor_msgs.JointState import JointState if TYPE_CHECKING: from dimos.porcelain.dimos import Dimos @@ -165,14 +165,17 @@ def move_pose( This targets an endpoint; use ``move_linear`` for a straight translation. """ xyz = _vector(position, 3, "position") - rotation = ( - self.pose().orientation - if orientation is None - else Quaternion(_vector(orientation, 4, "orientation")) - ) - if math.hypot(*rotation.to_tuple()) == 0.0: + if orientation is None: + rotation = self.pose().pose.orientation + else: + x, y, z, w = _vector(orientation, 4, "orientation") + rotation = Quaternion(x=x, y=y, z=z, w=w) + if math.hypot(rotation.x, rotation.y, rotation.z, rotation.w) == 0.0: raise ValueError("orientation must have nonzero norm") - target = PoseStamped(frame_id="world", position=xyz, orientation=rotation) + target = PoseStamped( + header=Header(frame_id="world"), + pose=Pose(position=Point(x=xyz[0], y=xyz[1], z=xyz[2]), orientation=rotation), + ) plan = self.rpc.plan_to_poses({self.info.id: target}, speed_scale=speed_scale) return self._execute("move_pose", plan, timeout) diff --git a/dimos/manipulation/test_execution_manager.py b/dimos/manipulation/test_execution_manager.py index 013cb178110..42570bb7652 100644 --- a/dimos/manipulation/test_execution_manager.py +++ b/dimos/manipulation/test_execution_manager.py @@ -16,6 +16,11 @@ from unittest.mock import MagicMock +from dimos_generated.builtin_interfaces.msg import Duration, Time +from dimos_generated.dimos_msgs.msg import TrajectoryStatus +from dimos_generated.sensor_msgs.msg import JointState +from dimos_generated.std_msgs.msg import Header +from dimos_generated.trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint import pytest from dimos.control.coordinator import ControlCoordinator @@ -30,10 +35,8 @@ from dimos.manipulation.manipulation_spec import ExecutionStatus from dimos.manipulation.planning.spec.enums import PlanningStatus from dimos.manipulation.planning.spec.models import GeneratedPlan -from dimos.msgs.sensor_msgs.JointState import JointState -from dimos.msgs.trajectory_msgs.JointTrajectory import JointTrajectory -from dimos.msgs.trajectory_msgs.TrajectoryPoint import TrajectoryPoint -from dimos.msgs.trajectory_msgs.TrajectoryStatus import TrajectoryState, TrajectoryStatus +from dimos.msgs.time import duration_from_seconds, header_now, to_seconds +from dimos.msgs.trajectory import TrajectoryState def _plan( @@ -41,12 +44,16 @@ def _plan( status: PlanningStatus = PlanningStatus.SUCCESS, ) -> GeneratedPlan: points = [ - TrajectoryPoint(positions=[0.0] * len(names), time_from_start=0.0), - TrajectoryPoint(positions=[1.0] * len(names), time_from_start=1.0), + JointTrajectoryPoint( + positions=[0.0] * len(names), time_from_start=duration_from_seconds(0.0) + ), + JointTrajectoryPoint( + positions=[1.0] * len(names), time_from_start=duration_from_seconds(1.0) + ), ] return GeneratedPlan( group_ids=("both_arms",), - trajectory=JointTrajectory(joint_names=list(names), points=points), + trajectory=JointTrajectory(header=header_now(), joint_names=list(names), points=points), path=[JointState(name=list(names), position=point.positions) for point in points], status=status, ) @@ -62,7 +69,7 @@ def invoke(task: str, method: str, args: dict | None = None): return TrajectoryExecutionResult(TrajectoryExecutionStatus.ACCEPTED) if method == "cancel": return TrajectoryCancellationResult(TrajectoryCancellationStatus.ALREADY_STOPPED) - return TrajectoryStatus(state=TrajectoryState.IDLE) + return TrajectoryStatus(header=header_now(), state=TrajectoryState.IDLE) coordinator.task_invoke.side_effect = invoke return coordinator @@ -169,7 +176,9 @@ def _task_invoke(self, task, method, args=None): return self.cancellations.get( task, TrajectoryCancellationResult(TrajectoryCancellationStatus.CANCELLED) ) - return TrajectoryStatus(state=self.states[task], error=self.errors.get(task, "")) + return TrajectoryStatus( + header=header_now(), state=self.states[task], error=self.errors.get(task, "") + ) def manager(self) -> PlanExecutionManager: return PlanExecutionManager( @@ -190,7 +199,10 @@ def test_whole_body_plan_splits_into_joint_and_base_columns() -> None: assert robot.dispatched[JOINT_TRAJECTORY_TASK_NAME].joint_names == ["left/j1"] assert robot.dispatched["base_traj"].joint_names == list(BASE) - assert [p.time_from_start for p in robot.dispatched["base_traj"].points] == [0.0, 1.0] + assert [to_seconds(p.time_from_start) for p in robot.dispatched["base_traj"].points] == [ + 0.0, + 1.0, + ] robot.states = dict.fromkeys(robot.states, TrajectoryState.COMPLETED) assert manager.wait().status is ExecutionStatus.COMPLETED @@ -220,7 +232,7 @@ def test_a_refused_part_cancels_the_parts_already_dispatched() -> None: assert result.status is ExecutionStatus.REJECTED assert "too fast" in result.message - assert robot.states[JOINT_TRAJECTORY_TASK_NAME] is TrajectoryState.ABORTED + assert robot.states[JOINT_TRAJECTORY_TASK_NAME] == TrajectoryState.ABORTED def test_a_refused_part_is_uncertain_when_the_others_cannot_be_confirmed_stopped() -> None: @@ -255,3 +267,27 @@ def test_a_poll_from_a_finished_run_does_not_disturb_the_next_one() -> None: assert manager.status is ExecutionStatus.ACCEPTED assert set(robot.states.values()) == {TrajectoryState.EXECUTING} + + +def test_split_cdr_plan_preserves_header_durations_and_optional_point_fields(): + robot = _WholeBody() + manager = robot.manager() + plan = _plan(("base/yaw", "left/j1", "base/x", "base/y")) + plan.trajectory.header = Header(stamp=Time(sec=1700000000, nanosec=123456789), frame_id="robot") + points = list(plan.trajectory.points) + for point in points: + point.velocities = [0.1, 0.2, 0.3, 0.4] + point.accelerations = [1.0, 2.0, 3.0, 4.0] + point.effort = [5.0, 6.0, 7.0, 8.0] + points[-1].time_from_start = Duration(sec=1, nanosec=9) + plan.trajectory.points = points + try: + assert manager.execute(plan, blocking=False).status == ExecutionStatus.ACCEPTED + part = JointTrajectory.decode(robot.dispatched["base_traj"].encode()) + assert part.header == plan.trajectory.header + assert part.points[-1].time_from_start == Duration(sec=1, nanosec=9) + assert list(part.points[-1].velocities) == [0.3, 0.4, 0.1] + assert list(part.points[-1].accelerations) == [3.0, 4.0, 1.0] + assert list(part.points[-1].effort) == [7.0, 8.0, 5.0] + finally: + manager.cancel() diff --git a/dimos/manipulation/test_generated_plan_materialization.py b/dimos/manipulation/test_generated_plan_materialization.py index 4d405769d24..3b5becd08cb 100644 --- a/dimos/manipulation/test_generated_plan_materialization.py +++ b/dimos/manipulation/test_generated_plan_materialization.py @@ -17,6 +17,18 @@ from pathlib import Path from unittest.mock import MagicMock +from dimos_generated.geometry_msgs.msg import ( + Point, + Pose, + PoseStamped, + Quaternion, + Transform, + TransformStamped, + Vector3, +) +from dimos_generated.sensor_msgs.msg import JointState +from dimos_generated.std_msgs.msg import Header +from dimos_generated.trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint import pytest from dimos.manipulation.manipulation_module import ManipulationState @@ -38,13 +50,7 @@ from dimos.manipulation.planning.trajectory_generator.simple_parametrizer import ( SimpleTrapezoidParametrizer, ) -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Transform import Transform -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.msgs.sensor_msgs.JointState import JointState -from dimos.msgs.trajectory_msgs.JointTrajectory import JointTrajectory -from dimos.msgs.trajectory_msgs.TrajectoryPoint import TrajectoryPoint +from dimos.msgs.time import duration_from_seconds, header_now, to_seconds from dimos.robot.assets.model import LoadedRobotModel, RobotModel @@ -66,21 +72,24 @@ def __init__( def generate(self, waypoints: list[list[float]]) -> JointTrajectory: RecordingGenerator.calls.append(waypoints) return JointTrajectory( + header=header_now(), points=[ - TrajectoryPoint( - time_from_start=float(index), + JointTrajectoryPoint( + time_from_start=duration_from_seconds(float(index)), positions=list(point), velocities=[0.0] * self.num_joints, ) for index, point in enumerate(waypoints) - ] + ], ) def _model() -> RobotModelConfig: return RobotModelConfig( model=RobotModel.from_file(Path("/robot.urdf")), - base_pose=PoseStamped(position=Vector3(), orientation=Quaternion()), + base_pose=PoseStamped( + header=Header(frame_id=""), pose=Pose(position=Point(), orientation=Quaternion()) + ), joint_names=["left/a", "left/b", "right/c"], base_link="base", planning_groups=[ @@ -162,7 +171,7 @@ def test_materializes_once_with_reordered_groups_heterogeneous_limits_and_distin assert module._last_plan is not None assert module._last_plan.path is not module._last_plan.trajectory.points assert module._last_plan.trajectory.joint_names == names - assert module._last_plan.trajectory.points[-1].time_from_start == 1.0 + assert to_seconds(module._last_plan.trajectory.points[-1].time_from_start) == 1.0 def test_cartesian_plan_preserves_planner_timestamps_and_velocities(monkeypatch, module_factory): @@ -184,8 +193,12 @@ def test_cartesian_plan_preserves_planner_timestamps_and_velocities(monkeypatch, plan = module.generate_cartesian_plan( { "left_arm": ( - Transform.identity(), - Transform(translation=Vector3(0.01, 0.0, 0.0)), + TransformStamped(header=Header(frame_id="world"), child_frame_id=""), + TransformStamped( + header=Header(frame_id="world"), + transform=Transform(translation=Vector3(x=0.01, y=0.0, z=0.0)), + child_frame_id="", + ), ) }, RoboPlanCartesianPathConfig( @@ -196,7 +209,7 @@ def test_cartesian_plan_preserves_planner_timestamps_and_velocities(monkeypatch, ) assert plan is not None - assert [point.time_from_start for point in plan.trajectory.points] == [0.0, 0.25] + assert [to_seconds(point.time_from_start) for point in plan.trajectory.points] == [0.0, 0.25] assert [point.velocities for point in plan.trajectory.points] == [ [0.0, 0.0], [0.4, 0.2], diff --git a/dimos/manipulation/test_manipulation_module.py b/dimos/manipulation/test_manipulation_module.py index 448d74ec487..15c15fa915b 100644 --- a/dimos/manipulation/test_manipulation_module.py +++ b/dimos/manipulation/test_manipulation_module.py @@ -24,6 +24,9 @@ import importlib.util from unittest.mock import DEFAULT, MagicMock +from dimos_generated.dimos_msgs.msg import TrajectoryStatus +from dimos_generated.geometry_msgs.msg import Point, Pose, Quaternion +from dimos_generated.sensor_msgs.msg import JointState import pytest from dimos.control.coordinator import ControlCoordinator @@ -40,11 +43,8 @@ from dimos.manipulation.manipulation_spec import ExecutionStatus from dimos.manipulation.planning.planners.config import RRTConnectPlannerConfig from dimos.manipulation.planning.spec.config import RobotModelConfig -from dimos.msgs.geometry_msgs.Pose import Pose -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.msgs.sensor_msgs.JointState import JointState -from dimos.msgs.trajectory_msgs.TrajectoryStatus import TrajectoryState, TrajectoryStatus +from dimos.msgs.time import header_now +from dimos.msgs.trajectory import TrajectoryState, trajectory_duration from dimos.robot.manipulators.xarm.config import make_xarm7_model_config pytestmark = pytest.mark.self_hosted @@ -96,7 +96,9 @@ def invoke(task: str, method: str, args: dict | None = None): return TrajectoryCancellationResult(TrajectoryCancellationStatus.ALREADY_STOPPED) return DEFAULT - coordinator.task_invoke.return_value = TrajectoryStatus(state=TrajectoryState.COMPLETED) + coordinator.task_invoke.return_value = TrajectoryStatus( + header=header_now(), state=TrajectoryState.COMPLETED + ) coordinator.task_invoke.side_effect = invoke mod = ManipulationModule( model=xarm7_config, @@ -157,7 +159,7 @@ def test_plan_to_joints(self, module, joint_state_zeros): assert module._last_plan is not None assert len(module._last_plan.trajectory.points) > 1 - assert module._last_plan.trajectory.duration > 0 + assert trajectory_duration(module._last_plan.trajectory) > 0 assert module._last_plan.group_ids == ("manipulator",) def test_plan_to_explicit_joint_target(self, module, joint_state_zeros): @@ -177,10 +179,7 @@ def test_add_and_remove_obstacle(self, module, joint_state_zeros): """Test adding and removing obstacles.""" module._on_joint_state(joint_state_zeros) - pose = Pose( - position=Vector3(0.5, 0.0, 0.3), - orientation=Quaternion(), # default is identity (w=1) - ) + pose = Pose(position=Point(x=0.5, y=0.0, z=0.3), orientation=Quaternion()) obstacle_id = module.add_obstacle("test_box", pose, "box", [0.1, 0.1, 0.1]) assert obstacle_id != "" diff --git a/dimos/manipulation/test_manipulation_monitor_preview.py b/dimos/manipulation/test_manipulation_monitor_preview.py index 85a1949afc0..20a06dd1793 100644 --- a/dimos/manipulation/test_manipulation_monitor_preview.py +++ b/dimos/manipulation/test_manipulation_monitor_preview.py @@ -19,6 +19,11 @@ from pathlib import Path from unittest.mock import MagicMock +from dimos_generated.builtin_interfaces.msg import Time +from dimos_generated.geometry_msgs.msg import Point, Pose, PoseStamped, Quaternion +from dimos_generated.sensor_msgs.msg import JointState +from dimos_generated.std_msgs.msg import Header +from dimos_generated.trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint import pytest from dimos.manipulation.manipulation_module import ManipulationModule @@ -30,12 +35,7 @@ from dimos.manipulation.planning.spec.enums import PlanningStatus from dimos.manipulation.planning.spec.models import GeneratedPlan from dimos.manipulation.planning.spec.protocols import VisualizationSpec -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.msgs.sensor_msgs.JointState import JointState -from dimos.msgs.trajectory_msgs.JointTrajectory import JointTrajectory -from dimos.msgs.trajectory_msgs.TrajectoryPoint import TrajectoryPoint +from dimos.msgs.time import duration_from_seconds, header_now from dimos.robot.assets.model import RobotModel @@ -44,7 +44,9 @@ def canonical_model_config() -> RobotModelConfig: """Create a model whose joint names match coordinator-facing names.""" return RobotModelConfig( model=RobotModel.from_file(Path("/path/to/robot.urdf")), - base_pose=PoseStamped(position=Vector3(), orientation=Quaternion()), + base_pose=PoseStamped( + header=Header(frame_id=""), pose=Pose(position=Point(), orientation=Quaternion()) + ), joint_names=["left/joint1", "left/joint2", "left/joint3"], base_link="link_base", planning_groups=[ @@ -61,7 +63,9 @@ def canonical_model_config() -> RobotModelConfig: def _one_joint_config(name: str = "arm") -> RobotModelConfig: return RobotModelConfig( model=RobotModel.from_file(Path("/path")), - base_pose=PoseStamped(position=Vector3(), orientation=Quaternion()), + base_pose=PoseStamped( + header=Header(frame_id=""), pose=Pose(position=Point(), orientation=Quaternion()) + ), joint_names=["j0"], base_link="base_link", planning_groups=[ @@ -86,10 +90,11 @@ def _install_generated_plan( ) module._last_plan = GeneratedPlan( trajectory=JointTrajectory( + header=header_now(), joint_names=config.joint_names, points=[ - TrajectoryPoint( - time_from_start=float(index), + JointTrajectoryPoint( + time_from_start=duration_from_seconds(float(index)), positions=list(point), velocities=[0.0 for _ in config.joint_names], ) @@ -127,9 +132,12 @@ def _make_path(*points: list[float]) -> list[JointState]: def _make_trajectory(*points: tuple[float, list[float]]) -> JointTrajectory: joint_names = [f"j{i}" for i in range(len(points[0][1]))] if points else [] return JointTrajectory( + header=header_now(), joint_names=joint_names, points=[ - TrajectoryPoint(time_from_start=time_from_start, positions=positions) + JointTrajectoryPoint( + time_from_start=duration_from_seconds(time_from_start), positions=positions + ) for time_from_start, positions in points ], ) @@ -187,6 +195,7 @@ def test_routes_canonical_state_in_model_order(self, canonical_model_config, mod module.config.model = canonical_model_config msg = JointState( + header=Header(stamp=Time(sec=1700000000, nanosec=123456789), frame_id="robot"), name=["left/joint3", "unrelated", "left/joint1", "left/joint2"], position=[0.3, 9.0, 0.1, 0.2], velocity=[3.0, 9.0, 1.0, 2.0], @@ -197,6 +206,9 @@ def test_routes_canonical_state_in_model_order(self, canonical_model_config, mod assert state.name == canonical_model_config.joint_names assert state.position == [0.1, 0.2, 0.3] assert state.velocity == [1.0, 2.0, 3.0] + assert state.header == msg.header + msg.header.frame_id = "changed" + assert state.header.frame_id == "robot" def test_aliases_coordinator_names_onto_model_joints( self, canonical_model_config, module_factory @@ -280,7 +292,9 @@ def test_visualization_routing_and_stop_all_monitors(self): monitor.cancel_preview_animation() path = _make_path([1.0], [2.0], [3.0]) plan = GeneratedPlan( - trajectory=JointTrajectory(), + trajectory=JointTrajectory( + header=header_now(), + ), group_ids=("manipulator",), status=PlanningStatus.SUCCESS, path=path, @@ -303,7 +317,12 @@ def test_visualization_none_is_noop(self): assert monitor.get_visualization_url() is None monitor.update_visualization_state() monitor.cancel_preview_animation() - monitor.animate_trajectory(JointTrajectory(), 1.0) + monitor.animate_trajectory( + JointTrajectory( + header=header_now(), + ), + 1.0, + ) monitor.start_visualization_thread() assert monitor._viz_thread is None @@ -320,7 +339,13 @@ def test_preview_without_a_plan_returns_rejected(self, module_factory): def test_clear_planned_path_invalidates_before_dismissing_preview(self, module_factory): module = module_factory() - plan = GeneratedPlan(trajectory=JointTrajectory(), group_ids=("manipulator",), path=[]) + plan = GeneratedPlan( + trajectory=JointTrajectory( + header=header_now(), + ), + group_ids=("manipulator",), + path=[], + ) module._last_plan = plan module._world_monitor = MagicMock() plan_during_dismissal: list[GeneratedPlan | None] = [] @@ -337,7 +362,11 @@ def test_clear_planned_path_invalidates_before_dismissing_preview(self, module_f def test_clear_planned_path_clears_without_a_world_monitor(self, module_factory): module = module_factory() module._last_plan = GeneratedPlan( - trajectory=JointTrajectory(), group_ids=("manipulator",), path=[] + trajectory=JointTrajectory( + header=header_now(), + ), + group_ids=("manipulator",), + path=[], ) assert module.clear_planned_path().succeeded diff --git a/dimos/manipulation/test_manipulation_primitives.py b/dimos/manipulation/test_manipulation_primitives.py index 4a928b75966..1124b1bcaa2 100644 --- a/dimos/manipulation/test_manipulation_primitives.py +++ b/dimos/manipulation/test_manipulation_primitives.py @@ -18,6 +18,10 @@ from pathlib import Path from unittest.mock import MagicMock +from dimos_generated.geometry_msgs.msg import TransformStamped +from dimos_generated.sensor_msgs.msg import JointState +from dimos_generated.std_msgs.msg import Header +from dimos_generated.trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint import pytest from pytest_mock import MockerFixture @@ -36,10 +40,7 @@ from dimos.manipulation.planning.spec.config import RobotModelConfig from dimos.manipulation.planning.spec.enums import PlanningStatus from dimos.manipulation.planning.spec.models import GeneratedPlan -from dimos.msgs.geometry_msgs.Transform import Transform -from dimos.msgs.sensor_msgs.JointState import JointState -from dimos.msgs.trajectory_msgs.JointTrajectory import JointTrajectory -from dimos.msgs.trajectory_msgs.TrajectoryPoint import TrajectoryPoint +from dimos.msgs.time import duration_from_seconds, header_now from dimos.robot.assets.model import RobotModel from dimos.robot.manipulators.openyam.config import make_openyam_model_config @@ -69,10 +70,11 @@ def _plan() -> GeneratedPlan: group_ids=("tool",), status=PlanningStatus.SUCCESS, trajectory=JointTrajectory( + header=header_now(), joint_names=names, points=[ - TrajectoryPoint(positions=[0.0], time_from_start=0.0), - TrajectoryPoint(positions=[0.1], time_from_start=1.0), + JointTrajectoryPoint(positions=[0.0], time_from_start=duration_from_seconds(0.0)), + JointTrajectoryPoint(positions=[0.1], time_from_start=duration_from_seconds(1.0)), ], ), ) @@ -103,10 +105,10 @@ def test_move_linear_uses_world_relative_target_and_default_speed( assert result.succeeded targets, config = generate.call_args.args start, relative = targets["tool"] - assert start == Transform.identity() - assert relative.translation.x == pytest.approx(0.02) - assert relative.translation.y == pytest.approx(0.0) - assert relative.translation.z == pytest.approx(-0.01) + assert start == TransformStamped(header=Header(frame_id="world"), child_frame_id="") + assert relative.transform.translation.x == pytest.approx(0.02) + assert relative.transform.translation.y == pytest.approx(0.0) + assert relative.transform.translation.z == pytest.approx(-0.01) assert generate.call_args.kwargs["check_collision"] is False assert generate.call_args.kwargs["speed_scale"] == pytest.approx(0.5) execute.assert_called_once_with(blocking=False, timeout=None, plan_id=generated.plan_id) diff --git a/dimos/manipulation/test_manipulation_spec.py b/dimos/manipulation/test_manipulation_spec.py index 915cb70a842..a9c383835d9 100644 --- a/dimos/manipulation/test_manipulation_spec.py +++ b/dimos/manipulation/test_manipulation_spec.py @@ -20,6 +20,9 @@ from types import UnionType from typing import get_args, get_origin, get_type_hints +from dimos_generated.sensor_msgs.msg import JointState +from dimos_generated.trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint + from dimos.manipulation.manipulation_module import ManipulationModule import dimos.manipulation.manipulation_spec as manipulation_spec from dimos.manipulation.manipulation_spec import ( @@ -37,9 +40,7 @@ ) from dimos.manipulation.planning.spec.enums import PlanningStatus from dimos.manipulation.planning.spec.models import GeneratedPlan -from dimos.msgs.sensor_msgs.JointState import JointState -from dimos.msgs.trajectory_msgs.JointTrajectory import JointTrajectory -from dimos.msgs.trajectory_msgs.TrajectoryPoint import TrajectoryPoint +from dimos.msgs.time import duration_from_seconds, header_now from dimos.spec.utils import spec_annotation_compliance @@ -124,10 +125,17 @@ def test_plan_result_repr_summarizes_trajectory_without_dumping_points() -> None plan = GeneratedPlan( group_ids=("arm/manipulator",), trajectory=JointTrajectory( + header=header_now(), joint_names=["arm/j0"], points=[ - TrajectoryPoint(positions=[0.0], velocities=[0.0], time_from_start=0.0), - TrajectoryPoint(positions=[123.456], velocities=[0.0], time_from_start=1.0), + JointTrajectoryPoint( + positions=[0.0], velocities=[0.0], time_from_start=duration_from_seconds(0.0) + ), + JointTrajectoryPoint( + positions=[123.456], + velocities=[0.0], + time_from_start=duration_from_seconds(1.0), + ), ], ), path=[ @@ -145,7 +153,7 @@ def test_plan_result_repr_summarizes_trajectory_without_dumping_points() -> None assert "waypoints=2" in text assert "path_length=123" in text assert "iterations=7" in text - assert "TrajectoryPoint" not in text + assert "JointTrajectoryPoint" not in text assert "positions" not in text diff --git a/dimos/manipulation/test_manipulation_unit.py b/dimos/manipulation/test_manipulation_unit.py index 0606038bb28..5f0e45bb5b2 100644 --- a/dimos/manipulation/test_manipulation_unit.py +++ b/dimos/manipulation/test_manipulation_unit.py @@ -21,6 +21,18 @@ import time from unittest.mock import ANY, DEFAULT, MagicMock, call +from dimos_generated.dimos_msgs.msg import TrajectoryStatus +from dimos_generated.geometry_msgs.msg import ( + Point, + Pose, + PoseStamped, + Quaternion, + Transform, + TransformStamped, +) +from dimos_generated.sensor_msgs.msg import JointState, PointCloud2 +from dimos_generated.std_msgs.msg import Header +from dimos_generated.trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint import numpy as np import pytest from pytest_mock import MockerFixture @@ -67,16 +79,9 @@ from dimos.manipulation.planning.trajectory_generator.simple_parametrizer import ( SimpleTrapezoidParametrizer, ) -from dimos.msgs.geometry_msgs.Pose import Pose -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Transform import Transform -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.msgs.sensor_msgs.JointState import JointState -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 -from dimos.msgs.trajectory_msgs.JointTrajectory import JointTrajectory -from dimos.msgs.trajectory_msgs.TrajectoryPoint import TrajectoryPoint -from dimos.msgs.trajectory_msgs.TrajectoryStatus import TrajectoryState, TrajectoryStatus +from dimos.msgs.pointcloud import pointcloud_from_xyz +from dimos.msgs.time import duration_from_seconds, header_now, time_from_seconds, to_nanoseconds +from dimos.msgs.trajectory import TrajectoryState from dimos.robot.assets.model import LoadedRobotModel, RobotModel @@ -96,7 +101,9 @@ def invoke(task: str, method: str, args: dict | None = None): return DEFAULT coordinator.task_invoke.side_effect = invoke - coordinator.task_invoke.return_value = TrajectoryStatus(state=TrajectoryState.IDLE) + coordinator.task_invoke.return_value = TrajectoryStatus( + header=header_now(), state=TrajectoryState.IDLE + ) return coordinator @@ -105,7 +112,9 @@ def robot_config(): """Create a robot config for testing.""" return RobotModelConfig( model=RobotModel.from_file(Path("/path/to/robot.urdf")), - base_pose=PoseStamped(position=Vector3(), orientation=Quaternion()), + base_pose=PoseStamped( + header=Header(frame_id=""), pose=Pose(position=Point(), orientation=Quaternion()) + ), joint_names=["joint1", "joint2", "joint3"], base_link="link_base", planning_groups=[ @@ -122,7 +131,9 @@ def robot_config(): def _one_joint_config() -> RobotModelConfig: return RobotModelConfig( model=RobotModel.from_file(Path("/path")), - base_pose=PoseStamped(position=Vector3(), orientation=Quaternion()), + base_pose=PoseStamped( + header=Header(frame_id=""), pose=Pose(position=Point(), orientation=Quaternion()) + ), joint_names=["j0"], base_link="base_link", planning_groups=[ @@ -162,10 +173,11 @@ def _install_generated_plan( ) module._last_plan = GeneratedPlan( trajectory=JointTrajectory( + header=header_now(), joint_names=config.joint_names, points=[ - TrajectoryPoint( - time_from_start=float(index), + JointTrajectoryPoint( + time_from_start=duration_from_seconds(float(index)), positions=list(point), velocities=[0.0 for _ in config.joint_names], ) @@ -187,10 +199,11 @@ def _install_generated_plan( def _generated_plan_trajectory(joint_names: list[str], *points: list[float]) -> JointTrajectory: return JointTrajectory( + header=header_now(), joint_names=joint_names, points=[ - TrajectoryPoint( - time_from_start=float(index), + JointTrajectoryPoint( + time_from_start=duration_from_seconds(float(index)), positions=list(point), velocities=[0.0 for _ in joint_names], ) @@ -202,9 +215,12 @@ def _generated_plan_trajectory(joint_names: list[str], *points: list[float]) -> def _make_trajectory(*points: tuple[float, list[float]]) -> JointTrajectory: joint_names = [f"j{i}" for i in range(len(points[0][1]))] if points else [] return JointTrajectory( + header=header_now(), joint_names=joint_names, points=[ - TrajectoryPoint(time_from_start=time_from_start, positions=positions) + JointTrajectoryPoint( + time_from_start=duration_from_seconds(time_from_start), positions=positions + ) for time_from_start, positions in points ], ) @@ -243,10 +259,9 @@ class TestVoxelMap: @staticmethod def _cloud(points: list[list[float]], frame_id: str = "world") -> PointCloud2: - return PointCloud2.from_numpy( + return pointcloud_from_xyz( np.asarray(points, dtype=np.float32).reshape((-1, 3)), - frame_id=frame_id, - timestamp=1.0, + header=Header(stamp=time_from_seconds(1.0), frame_id=frame_id), ) def test_a_map_becomes_one_octree_obstacle(self, module_factory) -> None: @@ -326,7 +341,9 @@ async def test_perception_objects_are_refreshable_and_queryable(self, module_fac module = module_factory() module._world_monitor = MagicMock(spec=WorldMonitor) detected = MagicMock() - pose = PoseStamped(position=Vector3(0.4, 0.1, 0.2)) + pose = PoseStamped( + header=Header(frame_id=""), pose=Pose(position=Point(x=0.4, y=0.1, z=0.2)) + ) obstacle = Obstacle(name="object-1", pose=pose, obstacle_type=ObstacleType.BOX) module._world_monitor.refresh_obstacles.return_value = [{"object_id": "object-1"}] module._world_monitor.world.get_obstacles.return_value = [obstacle] @@ -344,8 +361,7 @@ def test_complete_update_forwards_new_obstacle_value(self, module_factory) -> No module._world_monitor = MagicMock(spec=WorldMonitor) module._world_monitor.update_obstacle.return_value = True pose = Pose( - position=Vector3(1.0, 2.0, 3.0), - orientation=Quaternion(0.0, 0.0, 0.0, 1.0), + position=Point(x=1.0, y=2.0, z=3.0), orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0) ) result = module.update_obstacle( @@ -362,7 +378,7 @@ def test_complete_update_forwards_new_obstacle_value(self, module_factory) -> No assert obstacle.obstacle_type == ObstacleType.SPHERE assert obstacle.dimensions == (0.4,) assert obstacle.color == (0.1, 0.2, 0.3, 0.9) - assert obstacle.pose.position.x == pytest.approx(1.0) + assert obstacle.pose.pose.position.x == pytest.approx(1.0) assert module.update_obstacle("default-color", pose, "box", [1.0, 1.0, 1.0]) default_obstacle = module._world_monitor.update_obstacle.call_args.args[0] @@ -373,8 +389,7 @@ def test_pose_update_forwards_only_name_and_pose(self, module_factory) -> None: module._world_monitor = MagicMock(spec=WorldMonitor) module._world_monitor.update_obstacle_pose.return_value = True pose = Pose( - position=Vector3(4.0, 5.0, 6.0), - orientation=Quaternion(0.0, 0.0, 0.0, 1.0), + position=Point(x=4.0, y=5.0, z=6.0), orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0) ) result = module.update_obstacle_pose("moving-shape", pose) @@ -382,9 +397,9 @@ def test_pose_update_forwards_only_name_and_pose(self, module_factory) -> None: assert result is True name, stamped = module._world_monitor.update_obstacle_pose.call_args.args assert name == "moving-shape" - assert stamped.position.x == pytest.approx(4.0) - assert stamped.position.y == pytest.approx(5.0) - assert stamped.position.z == pytest.approx(6.0) + assert stamped.pose.position.x == pytest.approx(4.0) + assert stamped.pose.position.y == pytest.approx(5.0) + assert stamped.pose.position.z == pytest.approx(6.0) def test_complete_update_rejects_unknown_shape_before_world_mutation( self, module_factory @@ -441,7 +456,11 @@ def test_cancel_hides_active_plan_preview(self, module_factory): module = module_factory() module._state = ManipulationState.EXECUTING module._last_plan = GeneratedPlan( - trajectory=JointTrajectory(), group_ids=("manipulator",), path=[] + trajectory=JointTrajectory( + header=header_now(), + ), + group_ids=("manipulator",), + path=[], ) module._world_monitor = MagicMock() @@ -454,7 +473,9 @@ def test_cancel_completed_execution_cancels_coordinator_task(self, module_factor config = _one_joint_config() _install_generated_plan(module, config, [0.0], [0.1]) coordinator = _control_coordinator(cancel_status=TrajectoryCancellationStatus.CANCELLED) - coordinator.task_invoke.return_value = TrajectoryStatus(state=TrajectoryState.ABORTED) + coordinator.task_invoke.return_value = TrajectoryStatus( + header=header_now(), state=TrajectoryState.ABORTED + ) module._control_coordinator = coordinator module._initialize_execution() @@ -515,7 +536,7 @@ def test_reset_cancels_an_active_trajectory(self, module_factory): cancel_status=TrajectoryCancellationStatus.CANCELLED ) module._control_coordinator.task_invoke.return_value = TrajectoryStatus( - state=TrajectoryState.ABORTED + header=header_now(), state=TrajectoryState.ABORTED ) module._initialize_execution() module.execute(blocking=False) @@ -694,7 +715,8 @@ def test_inverse_kinematics_uses_current_seed_without_planning( module._kinematics.solve_pose_targets.return_value = expected pose = PoseStamped( - frame_id="world", position=Vector3(x=0.45, y=0.0, z=0.25), orientation=Quaternion() + header=Header(frame_id="world"), + pose=Pose(position=Point(x=0.45, y=0.0, z=0.25), orientation=Quaternion()), ) module._state = operation_state result = module.inverse_kinematics({"manipulator": pose}) @@ -710,8 +732,8 @@ def test_inverse_kinematics_uses_current_seed_without_planning( assert kwargs["check_collision"] is True [(group, target_pose)] = kwargs["pose_targets"].items() assert group.id == "manipulator" - assert target_pose.frame_id == "world" - assert target_pose.position.x == 0.45 + assert target_pose.header.frame_id == "world" + assert target_pose.pose.position.x == 0.45 def test_inverse_kinematics_returns_failure_without_joint_state( self, robot_config, module_factory @@ -727,7 +749,8 @@ def test_inverse_kinematics_returns_failure_without_joint_state( module._kinematics = MagicMock() pose = PoseStamped( - frame_id="world", position=Vector3(x=0.45, y=0.0, z=0.25), orientation=Quaternion() + header=Header(frame_id="world"), + pose=Pose(position=Point(x=0.45, y=0.0, z=0.25), orientation=Quaternion()), ) result = module.inverse_kinematics({"manipulator": pose}) @@ -750,7 +773,7 @@ def test_inverse_kinematics_accepts_explicit_seed_without_current_state( explicit_seed = JointState(name=robot_config.joint_names, position=[0.2, 0.1, 0.0]) pending_plan = GeneratedPlan( group_ids=("manipulator",), - trajectory=JointTrajectory(joint_names=robot_config.joint_names), + trajectory=JointTrajectory(header=header_now(), joint_names=robot_config.joint_names), path=[explicit_seed], status=PlanningStatus.SUCCESS, ) @@ -761,7 +784,8 @@ def test_inverse_kinematics_accepts_explicit_seed_without_current_state( module._kinematics.solve_pose_targets.return_value = expected pose = PoseStamped( - frame_id="world", position=Vector3(x=0.45, y=0.0, z=0.25), orientation=Quaternion() + header=Header(frame_id="world"), + pose=Pose(position=Point(x=0.45, y=0.0, z=0.25), orientation=Quaternion()), ) result = module.inverse_kinematics({"manipulator": pose}, seed=explicit_seed) @@ -892,7 +916,10 @@ def test_generate_pose_plan_uses_group_ik_and_selected_path(self, robot_config, ik_goal, ], ) - pose = Pose(position=Vector3(x=0.45, y=0.0, z=0.25), orientation=Quaternion()) + pose = PoseStamped( + header=Header(frame_id="world"), + pose=Pose(position=Point(x=0.45, y=0.0, z=0.25), orientation=Quaternion()), + ) plan = module.generate_plan_to_pose_targets({"manipulator": pose}) @@ -902,7 +929,7 @@ def test_generate_pose_plan_uses_group_ik_and_selected_path(self, robot_config, target_groups = list(ik_kwargs["pose_targets"].keys()) assert [group.id for group in target_groups] == ["manipulator"] target_pose = ik_kwargs["pose_targets"][target_groups[0]] - assert target_pose.position.x == 0.45 + assert target_pose.pose.position.x == 0.45 assert ik_kwargs["seed"].name == [ "joint1", "joint2", @@ -988,8 +1015,10 @@ def test_tf_loop_publishes_every_pose_group_for_bimanual_model( module._world_monitor = MagicMock(spec=WorldMonitor) module._world_monitor.planning_groups = PlanningGroupRegistry(model.planning_groups) module._world_monitor.get_group_ee_pose.side_effect = [ - PoseStamped(position=Vector3(0.4, 0.2, 0.3)), - PoseStamped(position=Vector3(0.4, -0.2, 0.3)), + PoseStamped(header=Header(frame_id=""), pose=Pose(position=Point(x=0.4, y=0.2, z=0.3))), + PoseStamped( + header=Header(frame_id=""), pose=Pose(position=Point(x=0.4, y=-0.2, z=0.3)) + ), ] publish = mocker.patch.object(module.tf, "publish") @@ -1016,12 +1045,16 @@ def test_tf_loop_publishes_mount_edges_alongside_the_robot( module = module_factory() module.config.model = model module.config.static_transforms = [ - Transform(frame_id="left/tool", child_frame_id="camera_link") + TransformStamped( + header=Header(frame_id="left/tool"), + child_frame_id="camera_link", + transform=Transform(), + ) ] module._world_monitor = MagicMock(spec=WorldMonitor) module._world_monitor.planning_groups = PlanningGroupRegistry(model.planning_groups) module._world_monitor.get_group_ee_pose.return_value = PoseStamped( - position=Vector3(0.4, 0.2, 0.3) + header=Header(frame_id=""), pose=Pose(position=Point(x=0.4, y=0.2, z=0.3)) ) publish = mocker.patch.object(module.tf, "publish") @@ -1031,14 +1064,14 @@ def stop_after_first_iteration(_period: float) -> bool: mocker.patch.object(module._tf_stop_event, "wait", side_effect=stop_after_first_iteration) module._tf_stop_event.clear() - before = time.time() + before = time.time_ns() module._tf_publish_loop() - published = list(publish.call_args.args[0]) + published = list(publish.call_args.args[0].transforms) mount = next(t for t in published if t.child_frame_id == "camera_link") - assert mount.frame_id == "left/tool" - assert mount.ts >= before + assert mount.header.frame_id == "left/tool" + assert to_nanoseconds(mount.header.stamp) >= before def test_get_ee_pose_fails_safely_without_pose_group(self, robot_config, module_factory): no_pose_config = RobotModelConfig( @@ -1109,7 +1142,9 @@ def test_inverse_kinematics_preserves_backend_failure_detail( status=IKStatus.NO_SOLUTION, message="target is outside the workspace" ) - result = module.inverse_kinematics({"manipulator": PoseStamped(frame_id="world")}) + result = module.inverse_kinematics( + {"manipulator": PoseStamped(header=Header(frame_id="world"), pose=Pose())} + ) assert result.status == IKStatus.NO_SOLUTION assert result.message == "target is outside the workspace" diff --git a/dimos/manipulation/test_pick_and_place_unit.py b/dimos/manipulation/test_pick_and_place_unit.py index bbd1e110a0c..b7792c082ee 100644 --- a/dimos/manipulation/test_pick_and_place_unit.py +++ b/dimos/manipulation/test_pick_and_place_unit.py @@ -17,18 +17,16 @@ from typing import Any from unittest.mock import MagicMock +from dimos_generated.dimos_msgs.msg import GraspCandidate, GraspCandidateArray +from dimos_generated.geometry_msgs.msg import Point, Pose, PoseStamped +from dimos_generated.std_msgs.msg import Header import pytest from dimos.manipulation.grasp_verification import GripperSettle from dimos.manipulation.manipulation_skills import ManipulationSkills from dimos.manipulation.pick_and_place_module import PickAndPlaceModule -from dimos.msgs.geometry_msgs.Pose import Pose -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.msgs.manipulation_msgs.GraspCandidate import GraspCandidate -from dimos.msgs.manipulation_msgs.GraspCandidateArray import GraspCandidateArray -from dimos.msgs.std_msgs.Header import Header +from dimos.msgs.geometry import quaternion_euler, quaternion_from_euler +from dimos.msgs.time import time_from_seconds @pytest.fixture @@ -45,7 +43,8 @@ def module() -> Iterator[PickAndPlaceModule]: "arm/tool": SimpleNamespace( gripper_position=0.5, end_effector_pose=PoseStamped( - frame_id="world", orientation=Quaternion.from_euler(Vector3(0, 0, 0.7)) + header=Header(frame_id="world"), + pose=Pose(orientation=quaternion_from_euler(0, 0, 0.7)), ), ) } @@ -62,7 +61,7 @@ def module() -> Iterator[PickAndPlaceModule]: instance._objects = {"cup-1": {"object_id": "cup-1", "name": "cup"}} instance._scene.get_object_pointcloud_by_object_id.return_value = MagicMock() instance._grasp_generator.propose_grasps.return_value = GraspCandidateArray( - Header(1.0, "world"), [_candidate(0.1)] + header=Header(stamp=time_from_seconds(1.0), frame_id="world"), candidates=[_candidate(0.1)] ) yield instance instance.stop() @@ -79,8 +78,11 @@ def settle(read: Any, target: float, config: Any, **_: Any) -> GripperSettle: def _candidate(x: float, score: float = 1.0) -> GraspCandidate: return GraspCandidate( - Pose(Vector3(x, 0.0, 0.2), Quaternion.from_euler(Vector3(-3.141592653589793, 0.0, 0.0))), - score, + pose=Pose( + position=Point(x=x, y=0.0, z=0.2), + orientation=quaternion_from_euler(-3.141592653589793, 0.0, 0.0), + ), + score=score, ) @@ -108,7 +110,7 @@ def test_pick_object_uses_first_provider_candidate( grasp_generator: Any = module._grasp_generator first, second = _candidate(0.1, 0.1), _candidate(0.2, 0.9) grasp_generator.propose_grasps.return_value = GraspCandidateArray( - Header(1.0, "world"), [first, second] + header=Header(stamp=time_from_seconds(1.0), frame_id="world"), candidates=[first, second] ) result = module.pick_object("cup-1") @@ -116,14 +118,14 @@ def test_pick_object_uses_first_provider_candidate( assert result.is_success() assert module.get_grasp_candidates().candidates == [first, second] assert module._selected_grasp is not None - assert module._selected_grasp.position.x == pytest.approx(0.1) + assert module._selected_grasp.pose.position.x == pytest.approx(0.1) assert result.metadata["rank"] == 0 def test_pick_object_rejects_non_planning_frame(module: PickAndPlaceModule) -> None: grasp_generator: Any = module._grasp_generator grasp_generator.propose_grasps.return_value = GraspCandidateArray( - Header(1.0, "camera"), [_candidate(0.1)] + header=Header(stamp=time_from_seconds(1.0), frame_id="camera"), candidates=[_candidate(0.1)] ) result = module.pick_object("cup-1") @@ -138,7 +140,8 @@ def test_pick_falls_through_to_the_next_reachable_candidate( """A learned provider's best-scoring pose is not always kinematically reachable.""" manipulation: Any = module._manipulation module._grasp_generator.propose_grasps.return_value = GraspCandidateArray( - Header(1.0, "world"), [_candidate(0.1, score=0.9), _candidate(0.3, score=0.4)] + header=Header(stamp=time_from_seconds(1.0), frame_id="world"), + candidates=[_candidate(0.1, score=0.9), _candidate(0.3, score=0.4)], ) manipulation.plan_to_poses.side_effect = [ SimpleNamespace(succeeded=False, message="unreachable"), @@ -158,7 +161,8 @@ def test_pick_stops_walking_candidates_on_a_drive_fault(module: PickAndPlaceModu """An execution fault would repeat for every candidate, so it is not a demotion.""" manipulation: Any = module._manipulation module._grasp_generator.propose_grasps.return_value = GraspCandidateArray( - Header(1.0, "world"), [_candidate(0.1), _candidate(0.3)] + header=Header(stamp=time_from_seconds(1.0), frame_id="world"), + candidates=[_candidate(0.1), _candidate(0.3)], ) manipulation.execute.return_value = SimpleNamespace(succeeded=False, message="drive fault") @@ -174,7 +178,8 @@ def test_pick_reports_no_reachable_candidate_when_every_attempt_fails( ) -> None: manipulation: Any = module._manipulation module._grasp_generator.propose_grasps.return_value = GraspCandidateArray( - Header(1.0, "world"), [_candidate(0.1), _candidate(0.3)] + header=Header(stamp=time_from_seconds(1.0), frame_id="world"), + candidates=[_candidate(0.1), _candidate(0.3)], ) manipulation.plan_to_poses.return_value = SimpleNamespace( succeeded=False, message="unreachable" @@ -201,7 +206,7 @@ def test_proposals_reach_the_viewer_as_they_are_generated(module: PickAndPlaceMo def test_pick_object_rejects_empty_candidates(module: PickAndPlaceModule) -> None: module._grasp_generator.propose_grasps.return_value = GraspCandidateArray( - Header(1.0, "world"), [] + header=Header(stamp=time_from_seconds(1.0), frame_id="world"), candidates=[] ) result = module.pick_object("cup-1") @@ -213,14 +218,14 @@ def test_pick_object_rejects_empty_candidates(module: PickAndPlaceModule) -> Non def test_pick_preserves_current_yaw_when_configured(module: PickAndPlaceModule) -> None: module.config.yaw_policy = "preserve_current" module._grasp_generator.propose_grasps.return_value = GraspCandidateArray( - Header(1.0, "world"), - [ + header=Header(stamp=time_from_seconds(1.0), frame_id="world"), + candidates=[ GraspCandidate( - Pose( - Vector3(0.1, 0.0, 0.2), - Quaternion.from_euler(Vector3(-3.141592653589793, 0.0, 0.1)), + pose=Pose( + position=Point(x=0.1, y=0.0, z=0.2), + orientation=quaternion_from_euler(-3.141592653589793, 0.0, 0.1), ), - 1.0, + score=1.0, ) ], ) @@ -229,15 +234,15 @@ def test_pick_preserves_current_yaw_when_configured(module: PickAndPlaceModule) assert result.is_success() assert module._selected_grasp is not None - assert module._selected_grasp.orientation.to_euler().z == pytest.approx(0.7) + assert quaternion_euler(module._selected_grasp.pose.orientation)[2] == pytest.approx(0.7) assert module._holding_object def test_place_uses_local_axis_and_clears_held_state(module: PickAndPlaceModule) -> None: manipulation: Any = module._manipulation module._selected_grasp = PoseStamped( - frame_id="world", - orientation=Quaternion.from_euler(Vector3(-3.141592653589793, 0.0, 0.0)), + header=Header(frame_id="world"), + pose=Pose(orientation=quaternion_from_euler(-3.141592653589793, 0.0, 0.0)), ) module._holding_object = True @@ -245,14 +250,14 @@ def test_place_uses_local_axis_and_clears_held_state(module: PickAndPlaceModule) assert result.is_success() preplace = manipulation.plan_to_poses.call_args_list[0].args[0]["arm/tool"] - assert preplace.position.z == pytest.approx(0.3) + assert preplace.pose.position.z == pytest.approx(0.3) assert not module._holding_object assert module._selected_grasp is None def test_scan_failure_clears_stale_selection(module: PickAndPlaceModule) -> None: scene: Any = module._scene - module._selected_grasp = PoseStamped(frame_id="world") + module._selected_grasp = PoseStamped(header=Header(frame_id="world"), pose=Pose()) scene.scan_scene.side_effect = RuntimeError("No aligned RGB-D frame") result = module.scan_objects(["cup"]) @@ -266,7 +271,7 @@ def test_scan_failure_clears_stale_selection(module: PickAndPlaceModule) -> None def test_pick_rejects_when_already_holding(module: PickAndPlaceModule) -> None: manipulation: Any = module._manipulation module._holding_object = True - module._selected_grasp = PoseStamped(frame_id="world") + module._selected_grasp = PoseStamped(header=Header(frame_id="world"), pose=Pose()) pick = module.pick_object("cup-1") @@ -275,7 +280,7 @@ def test_pick_rejects_when_already_holding(module: PickAndPlaceModule) -> None: def test_failed_pick_clears_previous_selection(module: PickAndPlaceModule) -> None: - module._selected_grasp = PoseStamped(frame_id="world") + module._selected_grasp = PoseStamped(header=Header(frame_id="world"), pose=Pose()) result = module.pick_object("missing") @@ -398,7 +403,7 @@ def settle(read: Any, target: float, config: Any, **_: Any) -> GripperSettle: def test_place_retains_held_state_when_release_fails( module: PickAndPlaceModule, monkeypatch: pytest.MonkeyPatch ) -> None: - module._selected_grasp = PoseStamped(frame_id="world") + module._selected_grasp = PoseStamped(header=Header(frame_id="world"), pose=Pose()) module._holding_object = True def settle(read: Any, target: float, config: Any, **_: Any) -> GripperSettle: @@ -427,3 +432,14 @@ def test_motion_skills_declare_movement_capability() -> None: ] assert all(skill.__skill_uses__ == ["movement"] for skill in skills) + + +@pytest.mark.parametrize("score", [float("nan"), float("inf"), -float("inf")]) +def test_pick_rejects_invalid_score_before_commanding_motion(module, score): + module._grasp_generator.propose_grasps.return_value = GraspCandidateArray( + header=Header(frame_id="world"), candidates=[_candidate(0.1, score=score)] + ) + result = module.pick_object("cup-1") + assert result.error_code == "GRASP_GENERATION_FAILED" + module._manipulation.plan_to_poses.assert_not_called() + module._manipulation.set_gripper_position.assert_not_called() diff --git a/dimos/manipulation/test_plan_execution.py b/dimos/manipulation/test_plan_execution.py index cbd20a1116d..0676e7e494d 100644 --- a/dimos/manipulation/test_plan_execution.py +++ b/dimos/manipulation/test_plan_execution.py @@ -16,6 +16,9 @@ from unittest.mock import DEFAULT, MagicMock +from dimos_generated.dimos_msgs.msg import TrajectoryStatus +from dimos_generated.sensor_msgs.msg import JointState +from dimos_generated.trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint import pytest from dimos.control.coordinator import ControlCoordinator @@ -30,27 +33,26 @@ from dimos.manipulation.planning.spec.enums import PlanningStatus from dimos.manipulation.planning.spec.models import GeneratedPlan from dimos.manipulation.visualization.operator import ManipulationOperator -from dimos.msgs.sensor_msgs.JointState import JointState -from dimos.msgs.trajectory_msgs.JointTrajectory import JointTrajectory -from dimos.msgs.trajectory_msgs.TrajectoryPoint import TrajectoryPoint -from dimos.msgs.trajectory_msgs.TrajectoryStatus import TrajectoryState, TrajectoryStatus +from dimos.msgs.time import duration_from_seconds, header_now +from dimos.msgs.trajectory import TrajectoryState from dimos.robot.assets.model import PlanarBaseDefinition def _plan(final_position: float = 1.0, joint_name: str = "arm/j0") -> GeneratedPlan: names = [joint_name] trajectory = JointTrajectory( + header=header_now(), joint_names=names, points=[ - TrajectoryPoint( + JointTrajectoryPoint( positions=[0.0], velocities=[0.0], - time_from_start=0.0, + time_from_start=duration_from_seconds(0.0), ), - TrajectoryPoint( + JointTrajectoryPoint( positions=[final_position], velocities=[0.0], - time_from_start=1.0, + time_from_start=duration_from_seconds(1.0), ), ], ) @@ -149,7 +151,7 @@ def test_execute_sends_planar_base_columns_to_the_base_task(module_factory) -> N coordinator.task_invoke.side_effect = lambda task, method, args: ( TrajectoryExecutionResult(TrajectoryExecutionStatus.ACCEPTED) if method == "execute" - else TrajectoryStatus(state=state) + else TrajectoryStatus(header=header_now(), state=state) ) module = _module_with_coordinator(coordinator, module_factory) planar_base = PlanarBaseDefinition( @@ -165,12 +167,12 @@ def test_execute_sends_planar_base_columns_to_the_base_task(module_factory) -> N module._initialize_execution() names = ["arm/j0", *planar_base.joint_names] points = [ - TrajectoryPoint(positions=[0.0] * 4, time_from_start=0.0), - TrajectoryPoint(positions=[1.0] * 4, time_from_start=1.0), + JointTrajectoryPoint(positions=[0.0] * 4, time_from_start=duration_from_seconds(0.0)), + JointTrajectoryPoint(positions=[1.0] * 4, time_from_start=duration_from_seconds(1.0)), ] module._last_plan = GeneratedPlan( group_ids=("manipulator",), - trajectory=JointTrajectory(joint_names=names, points=points), + trajectory=JointTrajectory(header=header_now(), joint_names=names, points=points), path=[JointState(name=names, position=point.positions) for point in points], status=PlanningStatus.SUCCESS, ) @@ -249,10 +251,12 @@ def read_status(): return module.get_state().operation_status.name assert operator.execute(_plan()) is True - coordinator.task_invoke.return_value = TrajectoryStatus(state=TrajectoryState.EXECUTING) + coordinator.task_invoke.return_value = TrajectoryStatus( + header=header_now(), state=TrajectoryState.EXECUTING + ) assert read_status() == "EXECUTING" - coordinator.task_invoke.return_value = TrajectoryStatus(state=terminal) + coordinator.task_invoke.return_value = TrajectoryStatus(header=header_now(), state=terminal) assert read_status() == operation assert module.get_state().execution_status.name == terminal.name coordinator.task_invoke.reset_mock() diff --git a/dimos/manipulation/test_planning_factory.py b/dimos/manipulation/test_planning_factory.py index d8985706595..0b2f3376079 100644 --- a/dimos/manipulation/test_planning_factory.py +++ b/dimos/manipulation/test_planning_factory.py @@ -23,6 +23,8 @@ from typing import Any from unittest.mock import ANY +from dimos_generated.geometry_msgs.msg import Point, Pose, PoseStamped, Quaternion +from dimos_generated.std_msgs.msg import Header from pydantic import ValidationError import pytest from pytest_mock import MockerFixture @@ -50,9 +52,6 @@ SimpleTrapezoidParametrizationConfig, TrajectoryParametrizationConfig, ) -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Vector3 import Vector3 from dimos.robot.assets.model import RobotModel @@ -75,7 +74,9 @@ def _make(**kwargs: Any) -> ManipulationModule: def robot_config() -> RobotModelConfig: return RobotModelConfig( model=RobotModel.from_file(Path("/path/to/robot.urdf")), - base_pose=PoseStamped(position=Vector3(), orientation=Quaternion()), # type: ignore[call-arg] + base_pose=PoseStamped( + header=Header(frame_id=""), pose=Pose(position=Point(), orientation=Quaternion()) + ), # type: ignore[call-arg] joint_names=["joint1", "joint2"], planning_groups=[ PlanningGroupDefinition( diff --git a/dimos/manipulation/test_roboplan.py b/dimos/manipulation/test_roboplan.py index 279d5adb4c3..5c378022349 100644 --- a/dimos/manipulation/test_roboplan.py +++ b/dimos/manipulation/test_roboplan.py @@ -26,6 +26,17 @@ from typing import Any, ClassVar import xml.etree.ElementTree as ET +from dimos_generated.geometry_msgs.msg import ( + Point, + Pose, + PoseStamped, + Quaternion, + Transform, + TransformStamped, + Vector3, +) +from dimos_generated.sensor_msgs.msg import JointState +from dimos_generated.std_msgs.msg import Header import numpy as np import pytest from pytest_mock import MockerFixture @@ -45,13 +56,8 @@ from dimos.manipulation.planning.spec.enums import ObstacleType, PlanningStatus from dimos.manipulation.planning.spec.models import Obstacle from dimos.manipulation.planning.spec.validation import MAX_OCTREE_POINTS, prepare_robot_model -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Transform import Transform -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.msgs.sensor_msgs.JointState import JointState +from dimos.msgs.geometry import pose_matrix, quaternion_from_euler from dimos.robot.assets.model import RobotModel -from dimos.utils.transform_utils import pose_to_matrix class FakeJointConfiguration: @@ -486,7 +492,9 @@ def robot_config(tmp_path: Path) -> RobotModelConfig: ) return RobotModelConfig( model=RobotModel.from_file(model_path).with_default_joint_acceleration_limit(2.0), - base_pose=PoseStamped(position=Vector3(), orientation=Quaternion()), # type: ignore[call-arg] + base_pose=PoseStamped( + header=Header(frame_id=""), pose=Pose(position=Point(), orientation=Quaternion()) + ), # type: ignore[call-arg] joint_names=["joint1", "joint2"], base_link="base", planning_groups=[ @@ -573,16 +581,14 @@ def _selection( ) -def _relative_target(*waypoints: Transform) -> tuple[Transform, ...]: - return (Transform.identity(), *waypoints) +def _relative_target(*waypoints: TransformStamped) -> tuple[TransformStamped, ...]: + return (TransformStamped(header=Header(frame_id="world"), child_frame_id=""), *waypoints) def _absolute_target(*waypoints: PoseStamped) -> tuple[PoseStamped, ...]: return ( PoseStamped( - frame_id="world", - position=Vector3(), - orientation=Quaternion(), + header=Header(frame_id="world"), pose=Pose(position=Point(), orientation=Quaternion()) ), *waypoints, ) @@ -696,22 +702,27 @@ def test_obstacle_mutation_updates_scene_and_stored_pose( obstacle = Obstacle( name="box", obstacle_type=ObstacleType.BOX, - pose=PoseStamped(position=Vector3(), orientation=Quaternion()), # type: ignore[call-arg] + pose=PoseStamped( + header=Header(frame_id=""), pose=Pose(position=Point(), orientation=Quaternion()) + ), # type: ignore[call-arg] dimensions=(0.1, 0.2, 0.3), ) assert world.add_obstacle(obstacle) == "box" assert add_box.call_args.args[2] == "dimos_world" assert "box" in world._scene.geometry - updated_pose = PoseStamped(position=Vector3(1, 0, 0), orientation=Quaternion()) # type: ignore[call-arg] + updated_pose = PoseStamped( + header=Header(frame_id=""), + pose=Pose(position=Point(x=1, y=0, z=0), orientation=Quaternion()), + ) # type: ignore[call-arg] assert world.update_obstacle_pose( "box", updated_pose, ) np.testing.assert_allclose( - pose_to_matrix(world.get_obstacles()[0].pose), - pose_to_matrix(updated_pose), + pose_matrix(world.get_obstacles()[0].pose.pose), + pose_matrix(updated_pose.pose), ) - np.testing.assert_allclose(world._scene.geometry["box"], pose_to_matrix(updated_pose)) + np.testing.assert_allclose(world._scene.geometry["box"], pose_matrix(updated_pose.pose)) assert world.add_obstacle(obstacle) is None assert world.remove_obstacle("box") assert world.get_obstacles() == [] @@ -726,7 +737,9 @@ def test_octree_obstacle_reaches_the_scene_as_occupied_cells( obstacle = Obstacle( name="voxel-map", obstacle_type=ObstacleType.OCTREE, - pose=PoseStamped(position=Vector3(), orientation=Quaternion()), + pose=PoseStamped( + header=Header(frame_id=""), pose=Pose(position=Point(), orientation=Quaternion()) + ), points=((0.0, 0.0, 0.0), (0.05, 0.0, 0.0)), octree_resolution=0.05, ) @@ -754,7 +767,9 @@ def test_octree_obstacle_survives_the_deepcopy_and_equality_the_world_does( obstacle = Obstacle( name="voxel-map", obstacle_type=ObstacleType.OCTREE, - pose=PoseStamped(position=Vector3(), orientation=Quaternion()), + pose=PoseStamped( + header=Header(frame_id=""), pose=Pose(position=Point(), orientation=Quaternion()) + ), points=((0.0, 0.0, 0.0),), octree_resolution=0.05, ) @@ -776,7 +791,9 @@ def octree(**kwargs: object) -> Obstacle: base: dict[str, object] = { "name": "voxel-map", "obstacle_type": ObstacleType.OCTREE, - "pose": PoseStamped(position=Vector3(), orientation=Quaternion()), + "pose": PoseStamped( + header=Header(frame_id=""), pose=Pose(position=Point(), orientation=Quaternion()) + ), "points": ((0.0, 0.0, 0.0),), "octree_resolution": 0.05, } @@ -805,7 +822,9 @@ def test_obstacle_operations_require_finalization( obstacle = Obstacle( name="box", obstacle_type=ObstacleType.BOX, - pose=PoseStamped(position=Vector3(), orientation=Quaternion()), + pose=PoseStamped( + header=Header(frame_id=""), pose=Pose(position=Point(), orientation=Quaternion()) + ), dimensions=(0.1, 0.2, 0.3), ) @@ -831,7 +850,9 @@ def test_failed_obstacle_add_rolls_back_and_can_be_retried( obstacle = Obstacle( name="retryable", obstacle_type=ObstacleType.BOX, - pose=PoseStamped(position=Vector3(), orientation=Quaternion()), + pose=PoseStamped( + header=Header(frame_id=""), pose=Pose(position=Point(), orientation=Quaternion()) + ), dimensions=(0.1, 0.2, 0.3), ) add_box = mocker.patch.object( @@ -859,7 +880,9 @@ def test_concurrent_remove_waits_for_obstacle_add( obstacle = Obstacle( name="concurrent", obstacle_type=ObstacleType.BOX, - pose=PoseStamped(position=Vector3(), orientation=Quaternion()), + pose=PoseStamped( + header=Header(frame_id=""), pose=Pose(position=Point(), orientation=Quaternion()) + ), dimensions=(0.1, 0.2, 0.3), ) backend_add_started = threading.Event() @@ -907,7 +930,9 @@ def test_obstacle_ids_are_world_owned_and_invalid_insertions_are_rejected( unnamed = Obstacle( name="", obstacle_type=ObstacleType.BOX, - pose=PoseStamped(position=Vector3(), orientation=Quaternion()), # type: ignore[call-arg] + pose=PoseStamped( + header=Header(frame_id=""), pose=Pose(position=Point(), orientation=Quaternion()) + ), # type: ignore[call-arg] dimensions=(0.1, 0.1, 0.1), ) named = replace(unnamed, name="world-owned") @@ -927,7 +952,9 @@ def test_complete_update_rejects_invalid_obstacle_values( valid = Obstacle( name="shape", obstacle_type=ObstacleType.BOX, - pose=PoseStamped(position=Vector3(), orientation=Quaternion()), # type: ignore[call-arg] + pose=PoseStamped( + header=Header(frame_id=""), pose=Pose(position=Point(), orientation=Quaternion()) + ), # type: ignore[call-arg] dimensions=(0.1, 0.2, 0.3), ) invalid_obstacles = [ @@ -942,11 +969,14 @@ def test_complete_update_rejects_invalid_obstacle_values( replace( valid, pose=PoseStamped( - position=[np.nan, 0.0, 0.0], - orientation=[0.0, 0.0, 0.0, 1.0], + header=Header(frame_id=""), + pose=Pose( + position=Point(x=np.nan, y=0.0, z=0.0), + orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0), + ), ), ), - "finite values", + "Position components must be finite", ), ] @@ -964,7 +994,9 @@ def test_complete_replacement_and_defensive_obstacle_snapshots( original = Obstacle( name="shape", obstacle_type=ObstacleType.BOX, - pose=PoseStamped(position=Vector3(), orientation=Quaternion()), # type: ignore[call-arg] + pose=PoseStamped( + header=Header(frame_id=""), pose=Pose(position=Point(), orientation=Quaternion()) + ), # type: ignore[call-arg] dimensions=(0.1, 0.2, 0.3), color=(1.0, 0.0, 0.0, 1.0), ) @@ -975,7 +1007,10 @@ def test_complete_replacement_and_defensive_obstacle_snapshots( replacement = Obstacle( name="shape", obstacle_type=ObstacleType.SPHERE, - pose=PoseStamped(position=Vector3(1, 2, 3), orientation=Quaternion()), # type: ignore[call-arg] + pose=PoseStamped( + header=Header(frame_id=""), + pose=Pose(position=Point(x=1, y=2, z=3), orientation=Quaternion()), + ), # type: ignore[call-arg] dimensions=(0.4,), color=(0.0, 1.0, 0.0, 0.5), ) @@ -988,9 +1023,9 @@ def test_complete_replacement_and_defensive_obstacle_snapshots( replacement.dimensions = (2.0,) stored.dimensions = (3.0,) - stored.pose.position.x = 99.0 + stored.pose.pose.position.x = 99.0 assert world.get_obstacles()[0].dimensions == (0.4,) - assert world.get_obstacles()[0].pose.position.x == pytest.approx(1.0) + assert world.get_obstacles()[0].pose.pose.position.x == pytest.approx(1.0) assert world.update_obstacle(replace(replacement, name="missing")) is False @@ -1003,7 +1038,9 @@ def test_collision_query_blocks_during_obstacle_replacement( obstacle = Obstacle( name="box", obstacle_type=ObstacleType.BOX, - pose=PoseStamped(position=Vector3(), orientation=Quaternion()), # type: ignore[call-arg] + pose=PoseStamped( + header=Header(frame_id=""), pose=Pose(position=Point(), orientation=Quaternion()) + ), # type: ignore[call-arg] dimensions=(0.1, 0.2, 0.3), ) world.add_obstacle(obstacle) @@ -1048,7 +1085,9 @@ def test_obstacle_replacement_blocks_during_collision_query( obstacle = Obstacle( name="box", obstacle_type=ObstacleType.BOX, - pose=PoseStamped(position=Vector3(), orientation=Quaternion()), # type: ignore[call-arg] + pose=PoseStamped( + header=Header(frame_id=""), pose=Pose(position=Point(), orientation=Quaternion()) + ), # type: ignore[call-arg] dimensions=(0.1, 0.2, 0.3), ) world.add_obstacle(obstacle) @@ -1093,7 +1132,9 @@ def test_native_update_failure_invalidates_world( obstacle = Obstacle( name="box", obstacle_type=ObstacleType.BOX, - pose=PoseStamped(position=Vector3(), orientation=Quaternion()), # type: ignore[call-arg] + pose=PoseStamped( + header=Header(frame_id=""), pose=Pose(position=Point(), orientation=Quaternion()) + ), # type: ignore[call-arg] dimensions=(0.1, 0.2, 0.3), ) world.add_obstacle(obstacle) @@ -1122,7 +1163,9 @@ def test_native_pose_update_failure_invalidates_world( obstacle = Obstacle( name="box", obstacle_type=ObstacleType.BOX, - pose=PoseStamped(position=Vector3(), orientation=Quaternion()), # type: ignore[call-arg] + pose=PoseStamped( + header=Header(frame_id=""), pose=Pose(position=Point(), orientation=Quaternion()) + ), # type: ignore[call-arg] dimensions=(0.1, 0.2, 0.3), ) world.add_obstacle(obstacle) @@ -1186,7 +1229,9 @@ def test_generic_planner_allows_update_between_collision_checks( obstacle = Obstacle( name="box", obstacle_type=ObstacleType.BOX, - pose=PoseStamped(position=Vector3(), orientation=Quaternion()), # type: ignore[call-arg] + pose=PoseStamped( + header=Header(frame_id=""), pose=Pose(position=Point(), orientation=Quaternion()) + ), # type: ignore[call-arg] dimensions=(0.1, 0.2, 0.3), ) world.add_obstacle(obstacle) @@ -1225,7 +1270,7 @@ def test_fk_jacobian_and_explicit_min_distance_unsupported( world.set_joint_state(ctx, JointState(name=["joint1", "joint2"], position=[0.25, 0.5])) pose = world.get_ee_pose(ctx) - assert pose.position.x == pytest.approx(0.75) + assert pose.pose.position.x == pytest.approx(0.75) assert world.get_jacobian(ctx).shape == (6, 2) with pytest.raises(NotImplementedError, match="get_min_distance"): world.get_min_distance(ctx) @@ -1273,14 +1318,14 @@ def fake_jacobian( ctx = world.get_live_context() world.set_joint_state( ctx, - JointState({"name": ["joint1", "joint2", "joint3"], "position": [1.0, 2.0, 3.0]}), + JointState(name=["joint1", "joint2", "joint3"], position=[1.0, 2.0, 3.0]), ) pose = world.get_group_ee_pose(ctx, "wrist") jacobian = world.get_group_jacobian(ctx, "wrist") assert fk_frames == ["tcp"] - assert pose.position.x == pytest.approx(6.0) + assert pose.pose.position.x == pytest.approx(6.0) np.testing.assert_allclose(jacobian, np.arange(18, dtype=np.float64).reshape(6, 3)[:, [2, 0]]) @@ -1588,7 +1633,9 @@ def test_native_planning_blocks_obstacle_replacement( obstacle = Obstacle( name="box", obstacle_type=ObstacleType.BOX, - pose=PoseStamped(position=Vector3(), orientation=Quaternion()), # type: ignore[call-arg] + pose=PoseStamped( + header=Header(frame_id=""), pose=Pose(position=Point(), orientation=Quaternion()) + ), # type: ignore[call-arg] dimensions=(0.1, 0.2, 0.3), ) world.add_obstacle(obstacle) @@ -1692,8 +1739,8 @@ def capture_scene_start( ) assert result.status == PlanningStatus.SUCCESS - assert result.path[0].position == pytest.approx(start.position) - assert observed_scene_start[:2] == pytest.approx(start.position) + assert list(result.path[0].position) == pytest.approx(list(start.position)) + assert observed_scene_start[:2] == pytest.approx(list(start.position)) def test_native_selected_planner_supports_non_overlapping_multi_group_selection( @@ -1719,7 +1766,7 @@ def test_native_selected_planner_supports_non_overlapping_multi_group_selection( assert result.status == PlanningStatus.SUCCESS assert result.path[-1].name == ["joint1", "joint2"] - assert result.path[-1].position == pytest.approx([0.1, 0.1]) + assert list(result.path[-1].position) == pytest.approx([0.1, 0.1]) def test_cartesian_planner_returns_timed_canonical_joint_states_and_options( @@ -1753,9 +1800,13 @@ def test_cartesian_planner_returns_timed_canonical_joint_states_and_options( JointState(name=list(selection.joint_names), position=[0.0, 0.0]), { "manipulator": _relative_target( - Transform( - translation=Vector3(0.1, 0.0, 0.0), - rotation=Quaternion.from_euler(Vector3(0.0, 0.0, np.pi / 2.0)), + TransformStamped( + header=Header(frame_id="world"), + transform=Transform( + translation=Vector3(x=0.1, y=0.0, z=0.0), + rotation=quaternion_from_euler(0.0, 0.0, np.pi / 2.0), + ), + child_frame_id="", ) ) }, @@ -1768,8 +1819,8 @@ def test_cartesian_planner_returns_timed_canonical_joint_states_and_options( assert result.status == PlanningStatus.SUCCESS assert result.timestamps == [0.0, 0.02, 0.04] assert [state.name for state in result.path] == [list(selection.joint_names)] * 3 - assert result.path[-1].position == pytest.approx([0.1, 0.1]) - assert result.path[1].velocity == pytest.approx([0.5, 0.5]) + assert list(result.path[-1].position) == pytest.approx([0.1, 0.1]) + assert list(result.path[1].velocity) == pytest.approx([0.5, 0.5]) planner = FakeCartesianPathPlanner.instances[-1] assert planner.options.speed_mode == FakeCartesianSpeedMode.TimeOptimal for field_name, expected in option_overrides.items(): @@ -1802,8 +1853,16 @@ def test_cartesian_zero_rotation_preserves_start_orientation( JointState(name=list(selection.joint_names), position=[0.0, 0.0]), { "manipulator": _relative_target( - Transform(translation=Vector3(0.05, 0.02, 0.0)), - Transform(translation=Vector3(0.1, 0.0, 0.0)), + TransformStamped( + header=Header(frame_id="world"), + transform=Transform(translation=Vector3(x=0.05, y=0.02, z=0.0)), + child_frame_id="", + ), + TransformStamped( + header=Header(frame_id="world"), + transform=Transform(translation=Vector3(x=0.1, y=0.0, z=0.0)), + child_frame_id="", + ), ) }, RoboPlanCartesianPathConfig(), @@ -1830,12 +1889,20 @@ def test_cartesian_uses_explicit_start_after_live_state_advances( world, selection, start, - {"manipulator": _relative_target(Transform(translation=Vector3(0.1, 0.0, 0.0)))}, + { + "manipulator": _relative_target( + TransformStamped( + header=Header(frame_id="world"), + transform=Transform(translation=Vector3(x=0.1, y=0.0, z=0.0)), + child_frame_id="", + ) + ) + }, RoboPlanCartesianPathConfig(), ) assert result.status == PlanningStatus.SUCCESS - assert result.path[0].position == pytest.approx(start.position) + assert list(result.path[0].position) == pytest.approx(list(start.position)) start_pose = FakeCartesianPathPlanner.instances[-1].paths[0].tforms[0][0] assert start_pose[0, 3] == pytest.approx(sum(start.position)) @@ -1858,7 +1925,15 @@ def test_cartesian_rejects_official_planner_failure( world, selection, JointState(name=list(selection.joint_names), position=[0.0, 0.0]), - {"manipulator": _relative_target(Transform(translation=Vector3(0.1, 0.0, 0.0)))}, + { + "manipulator": _relative_target( + TransformStamped( + header=Header(frame_id="world"), + transform=Transform(translation=Vector3(x=0.1, y=0.0, z=0.0)), + child_frame_id="", + ) + ) + }, RoboPlanCartesianPathConfig(), ) @@ -1910,7 +1985,15 @@ def collides_only_mid_edge(scene: FakeScene, q: np.ndarray) -> bool: world, selection, JointState(name=list(selection.joint_names), position=[0.0, 0.0]), - {"manipulator": _relative_target(Transform(translation=Vector3(0.05, 0.0, 0.0)))}, + { + "manipulator": _relative_target( + TransformStamped( + header=Header(frame_id="world"), + transform=Transform(translation=Vector3(x=0.05, y=0.0, z=0.0)), + child_frame_id="", + ) + ) + }, RoboPlanCartesianPathConfig(), ) @@ -2034,8 +2117,9 @@ def test_composed_model_fills_only_missing_acceleration_limits( def test_base_pose_is_written_to_composed_model( fake_roboplan: None, robot_config: RobotModelConfig ) -> None: - robot_config.base_pose = PoseStamped( # type: ignore[call-arg] - position=Vector3(1, 0, 0), orientation=Quaternion() + robot_config.base_pose = PoseStamped( + header=Header(frame_id=""), + pose=Pose(position=Point(x=1, y=0, z=0), orientation=Quaternion()), ) world = _make_world(fake_roboplan, robot_config) diff --git a/dimos/manipulation/test_roboplan_integration.py b/dimos/manipulation/test_roboplan_integration.py index 71ea6d94ab4..a029b17eeb4 100644 --- a/dimos/manipulation/test_roboplan_integration.py +++ b/dimos/manipulation/test_roboplan_integration.py @@ -18,6 +18,8 @@ from pathlib import Path from typing import Any +from dimos_generated.geometry_msgs.msg import Transform, TransformStamped +from dimos_generated.std_msgs.msg import Header import pytest pytest.importorskip("roboplan.cartesian_planning") @@ -26,6 +28,9 @@ "dimos.manipulation.planning.planners.roboplan_planner" ) +from dimos_generated.geometry_msgs.msg import TransformStamped, Vector3 +from dimos_generated.sensor_msgs.msg import JointState + from dimos.manipulation.planning.planners.roboplan_config import ( RoboPlanCartesianPathConfig, RoboPlanPlannerConfig, @@ -33,14 +38,11 @@ from dimos.manipulation.planning.spec.enums import PlanningStatus from dimos.manipulation.planning.spec.validation import prepare_robot_model from dimos.manipulation.planning.utils.kinematics_utils import compute_pose_error -from dimos.msgs.geometry_msgs.Transform import Transform -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.msgs.sensor_msgs.JointState import JointState +from dimos.msgs.geometry import pose_matrix from dimos.robot.manipulators.xarm.config import ( make_dual_xarm6_model_config, make_xarm6_model_config, ) -from dimos.utils.transform_utils import pose_to_matrix pytestmark = pytest.mark.self_hosted @@ -91,8 +93,12 @@ def test_real_roboplan_plans_fixed_orientation_cartesian_path( start, { group_id: ( - Transform.identity(), - Transform(translation=Vector3(0.005, 0.0, 0.0)), + TransformStamped(header=Header(frame_id="world"), child_frame_id=""), + TransformStamped( + header=Header(frame_id="world"), + transform=Transform(translation=Vector3(x=0.005, y=0.0, z=0.0)), + child_frame_id="", + ), ) }, RoboPlanCartesianPathConfig( @@ -108,9 +114,9 @@ def test_real_roboplan_plans_fixed_orientation_cartesian_path( with world.scratch_context() as ctx: planner._apply_selected_state(ctx, result.path[-1]) final_pose = world.get_group_ee_pose(ctx, group_id) - expected = pose_to_matrix(start_pose) + expected = pose_matrix(start_pose.pose) expected[0, 3] += 0.005 - position_error, orientation_error = compute_pose_error(pose_to_matrix(final_pose), expected) + position_error, orientation_error = compute_pose_error(pose_matrix(final_pose.pose), expected) assert position_error <= 0.005 assert orientation_error <= 0.01 @@ -142,13 +148,25 @@ def test_real_roboplan_synchronizes_different_length_dual_arm_targets( start, { left_group_id: ( - Transform.identity(), - Transform(translation=Vector3(0.0015, 0.001, 0.0)), - Transform(translation=Vector3(0.003, 0.0, 0.0)), + TransformStamped(header=Header(frame_id="world"), child_frame_id=""), + TransformStamped( + header=Header(frame_id="world"), + transform=Transform(translation=Vector3(x=0.0015, y=0.001, z=0.0)), + child_frame_id="", + ), + TransformStamped( + header=Header(frame_id="world"), + transform=Transform(translation=Vector3(x=0.003, y=0.0, z=0.0)), + child_frame_id="", + ), ), right_group_id: ( - Transform.identity(), - Transform(translation=Vector3(0.005, 0.0, 0.0)), + TransformStamped(header=Header(frame_id="world"), child_frame_id=""), + TransformStamped( + header=Header(frame_id="world"), + transform=Transform(translation=Vector3(x=0.005, y=0.0, z=0.0)), + child_frame_id="", + ), ), }, RoboPlanCartesianPathConfig(), diff --git a/dimos/manipulation/test_sdk.py b/dimos/manipulation/test_sdk.py index 7b31097f7d4..cc5b1bc51d5 100644 --- a/dimos/manipulation/test_sdk.py +++ b/dimos/manipulation/test_sdk.py @@ -15,6 +15,10 @@ import atexit from dataclasses import replace +from dimos_generated.geometry_msgs.msg import Point, Pose, PoseStamped, Quaternion +from dimos_generated.sensor_msgs.msg import JointState +from dimos_generated.std_msgs.msg import Header +from dimos_generated.trajectory_msgs.msg import JointTrajectory from IPython.core.completer import provisionalcompleter from IPython.core.interactiveshell import InteractiveShell import numpy as np @@ -38,9 +42,7 @@ ) from dimos.manipulation.planning.spec.models import GeneratedPlan from dimos.manipulation.sdk import Arm, MotionError -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.sensor_msgs.JointState import JointState -from dimos.msgs.trajectory_msgs.JointTrajectory import JointTrajectory +from dimos.msgs.time import header_now from dimos.porcelain.dimos import Dimos @@ -60,7 +62,11 @@ def rpc(mocker): "arm": PlanningGroupState( joints=JointState(name=["j0", "j1"], position=[0.1, 0.2]), end_effector_pose=PoseStamped( - frame_id="world", position=[0.4, 0.0, 0.3], orientation=[0.0, 1.0, 0.0, 0.0] + header=Header(frame_id="world"), + pose=Pose( + position=Point(x=0.4, y=0.0, z=0.3), + orientation=Quaternion(x=0.0, y=1.0, z=0.0, w=0.0), + ), ), gripper_position=0.5, joint_presets={ @@ -71,10 +77,22 @@ def rpc(mocker): }, ) proxy.plan_to_joints.return_value = PlanResult( - PlanStatus.SUCCEEDED, plan=GeneratedPlan(("arm",), JointTrajectory()) + PlanStatus.SUCCEEDED, + plan=GeneratedPlan( + ("arm",), + JointTrajectory( + header=header_now(), + ), + ), ) proxy.plan_to_poses.return_value = PlanResult( - PlanStatus.SUCCEEDED, plan=GeneratedPlan(("arm",), JointTrajectory()) + PlanStatus.SUCCEEDED, + plan=GeneratedPlan( + ("arm",), + JointTrajectory( + header=header_now(), + ), + ), ) proxy.execute.return_value = ExecutionResult(ExecutionStatus.COMPLETED) proxy.move_linear.return_value = MoveResult( @@ -173,7 +191,7 @@ def test_explicit_group_and_module_selection(app, rpc): def test_joints_are_fresh_arrays_in_declared_order(arm, rpc): state = rpc.get_state.return_value.groups["arm"] - state.joints.name[:] = ["j1", "j0"] + state.joints.name = ["j1", "j0"] positions = arm.joints() np.testing.assert_array_equal(positions, [0.2, 0.1]) @@ -226,9 +244,16 @@ def test_pose_preserves_current_orientation(arm, rpc): arm.move_pose([0.5, 0.1, 0.4], speed_scale=0.3, timeout=20.0) target = rpc.plan_to_poses.call_args.args[0]["arm"] - assert target.frame_id == "world" - np.testing.assert_array_equal(target.position.to_numpy(), [0.5, 0.1, 0.4]) - assert target.orientation.to_tuple() == (0.0, 1.0, 0.0, 0.0) + assert target.header.frame_id == "world" + np.testing.assert_array_equal( + [target.pose.position.x, target.pose.position.y, target.pose.position.z], [0.5, 0.1, 0.4] + ) + assert ( + target.pose.orientation.x, + target.pose.orientation.y, + target.pose.orientation.z, + target.pose.orientation.w, + ) == (0.0, 1.0, 0.0, 0.0) assert rpc.plan_to_poses.call_args.kwargs == {"speed_scale": 0.3} rpc.execute.assert_called_once_with( blocking=True, timeout=20.0, plan_id=rpc.plan_to_poses.return_value.plan.plan_id @@ -239,7 +264,12 @@ def test_explicit_orientation_needs_no_state_read(arm, rpc): arm.move_pose([0.4, 0.0, 0.3], orientation=(0.0, 0.0, 0.0, 1.0)) target = rpc.plan_to_poses.call_args.args[0]["arm"] - assert target.orientation.to_tuple() == (0.0, 0.0, 0.0, 1.0) + assert ( + target.pose.orientation.x, + target.pose.orientation.y, + target.pose.orientation.z, + target.pose.orientation.w, + ) == (0.0, 0.0, 0.0, 1.0) rpc.get_state.assert_not_called() diff --git a/dimos/manipulation/visualization/operator.py b/dimos/manipulation/visualization/operator.py index 55649ce6676..19d832bea25 100644 --- a/dimos/manipulation/visualization/operator.py +++ b/dimos/manipulation/visualization/operator.py @@ -17,15 +17,17 @@ from __future__ import annotations from collections.abc import Mapping, Sequence +import copy from dataclasses import dataclass, field import math from typing import TYPE_CHECKING +from dimos_generated.geometry_msgs.msg import PoseStamped +from dimos_generated.sensor_msgs.msg import JointState + from dimos.manipulation.planning.groups.models import PlanningGroup from dimos.manipulation.planning.planners.config import CartesianPathConfig from dimos.manipulation.planning.spec.models import GeneratedPlan, PlanningGroupID -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.sensor_msgs.JointState import JointState if TYPE_CHECKING: from dimos.manipulation.manipulation_module import ManipulationModule @@ -111,7 +113,7 @@ def set_motion_speed(self, speed_scale: float) -> bool: def get_init_joints(self) -> JointState | None: """Return the operator-authoritative initialization state.""" init = self._module.get_init_joints() - return None if init is None else JointState(init) + return None if init is None else copy.deepcopy(init) def evaluate_joint_target(self, request: JointTargetRequest) -> TargetEvaluationResult: """Validate and evaluate a canonical joint target.""" @@ -122,7 +124,7 @@ def evaluate_joint_target(self, request: JointTargetRequest) -> TargetEvaluation complete = self._complete_states(groups, request.target) if complete is None: return self._invalid(request.group_ids, "Incomplete model target state") - return self._evaluate_complete_target(groups, JointState(request.target), complete) + return self._evaluate_complete_target(groups, copy.deepcopy(request.target), complete) def evaluate_pose_target(self, request: PoseTargetRequest) -> TargetEvaluationResult: """Validate and evaluate explicit world-frame pose targets.""" @@ -132,7 +134,7 @@ def evaluate_pose_target(self, request: PoseTargetRequest) -> TargetEvaluationRe ik = self._module.inverse_kinematics( pose_targets=dict(request.pose_targets), auxiliary_group_ids=request.auxiliary_group_ids, - seed=JointState(request.seed) if request.seed is not None else None, + seed=copy.deepcopy(request.seed) if request.seed is not None else None, check_collision=True, ) if not ik.is_success() or ik.joint_state is None: @@ -155,12 +157,9 @@ def plan_to_joints(self, request: JointTargetRequest) -> GeneratedPlan | None: assert groups is not None targets = { group.id: JointState( - { - "name": list(group.joint_names), - "position": list( - request.target.position[offset : offset + len(group.joint_names)] - ), - } + name=list(group.joint_names), + header=request.target.header, + position=list(request.target.position)[offset : offset + len(group.joint_names)], ) for group, offset in self._group_offsets(groups) } @@ -260,9 +259,9 @@ def _validate_pose_request( group_ids, f"Planning group '{group.id}' has no tip_link" ) for group_id, pose in request.pose_targets.items(): - if pose.frame_id != "world": + if pose.header.frame_id != "world": return group_ids, self._invalid( - group_ids, f"Unsupported pose frame for '{group_id}': {pose.frame_id}" + group_ids, f"Unsupported pose frame for '{group_id}': {pose.header.frame_id}" ) if not self._pose_is_finite(pose): return group_ids, self._invalid( @@ -315,7 +314,7 @@ def _complete_states( if value is None: return None positions.append(value) - return JointState({"name": list(config.joint_names), "position": positions}) + return JointState(name=list(config.joint_names), position=positions) def _evaluate_complete_target( self, @@ -350,7 +349,7 @@ def _evaluate_complete_target( message="Target is collision-free" if valid else "Target is infeasible", collision_free=valid, group_ids=tuple(group.id for group in groups), - target_joints=JointState(target), + target_joints=copy.deepcopy(target), group_diagnostics=diagnostics, group_poses=poses, ) @@ -370,5 +369,6 @@ def _group_offsets(groups: Sequence[PlanningGroup]) -> tuple[tuple[PlanningGroup @staticmethod def _pose_is_finite(pose: PoseStamped) -> bool: - values = [*pose.position, *pose.orientation] + point, rotation = pose.pose.position, pose.pose.orientation + values = [point.x, point.y, point.z, rotation.x, rotation.y, rotation.z, rotation.w] return len(values) == 7 and all(math.isfinite(float(value)) for value in values) diff --git a/dimos/manipulation/visualization/test_factory.py b/dimos/manipulation/visualization/test_factory.py index bedfdcce871..d1b770e120e 100644 --- a/dimos/manipulation/visualization/test_factory.py +++ b/dimos/manipulation/visualization/test_factory.py @@ -18,6 +18,11 @@ from pathlib import Path from unittest.mock import MagicMock +from dimos_generated.dimos_msgs.msg import GraspCandidateArray +from dimos_generated.geometry_msgs.msg import Pose, PoseStamped +from dimos_generated.sensor_msgs.msg import JointState +from dimos_generated.std_msgs.msg import Header +from dimos_generated.trajectory_msgs.msg import JointTrajectory import numpy as np from numpy.typing import NDArray from pydantic import ValidationError @@ -43,10 +48,7 @@ ) from dimos.manipulation.visualization.factory import create_manipulation_visualization from dimos.manipulation.visualization.viser.config import ViserVisualizationConfig -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.manipulation_msgs.GraspCandidateArray import GraspCandidateArray -from dimos.msgs.sensor_msgs.JointState import JointState -from dimos.msgs.trajectory_msgs.JointTrajectory import JointTrajectory +from dimos.msgs.time import header_now from dimos.robot.assets.model import LoadedRobotModel, RobotModel @@ -97,7 +99,7 @@ def load_model(self, model: PreparedRobotModel) -> None: def get_model_config(self) -> RobotModelConfig: return RobotModelConfig( model=RobotModel.from_file(Path("fake.urdf")), - base_pose=PoseStamped(), + base_pose=PoseStamped(header=Header(frame_id=""), pose=Pose()), joint_names=["joint1"], planning_groups=[ PlanningGroupDefinition( @@ -160,7 +162,7 @@ def set_joint_state(self, ctx: object, joint_state: JointState) -> None: return None def get_joint_state(self, ctx: object) -> JointState: - return JointState({}) + return JointState() def is_collision_free(self, ctx: object) -> bool: return True @@ -180,7 +182,7 @@ def check_edge_collision_free( return True def get_ee_pose(self, ctx: object) -> PoseStamped: - return PoseStamped() + return PoseStamped(header=Header(frame_id=""), pose=Pose()) def get_link_pose(self, ctx: object, link_name: str) -> NDArray[np.float64]: return np.eye(4, dtype=np.float64) @@ -189,7 +191,7 @@ def get_jacobian(self, ctx: object) -> NDArray[np.float64]: return np.zeros((6, 0), dtype=np.float64) def get_group_ee_pose(self, ctx: object, group_id: str) -> PoseStamped: - return PoseStamped() + return PoseStamped(header=Header(frame_id=""), pose=Pose()) def get_group_jacobian(self, ctx: object, group_id: str) -> NDArray[np.float64]: return np.zeros((6, 0), dtype=np.float64) @@ -323,11 +325,11 @@ def test_create_visualization_meshcat_accepts_structural_world() -> None: PlanningSceneInfo(model=fake_world.get_prepared_model()), operator=object() ) frame = VisualizationStateFrame(joint_state=None) - trajectory = JointTrajectory(joint_names=["arm/j1"], points=[]) + trajectory = JointTrajectory(header=header_now(), joint_names=["arm/j1"], points=[]) obstacle = Obstacle( name="box", obstacle_type=ObstacleType.BOX, - pose=PoseStamped(), + pose=PoseStamped(header=Header(frame_id=""), pose=Pose()), dimensions=(1.0, 1.0, 1.0), ) visualization.initialize(session) @@ -394,7 +396,7 @@ def test_drake_meshcat_visualization_lifecycle_is_noop_without_meshcat() -> None obstacle = Obstacle( name="box", obstacle_type=ObstacleType.BOX, - pose=PoseStamped(), + pose=PoseStamped(header=Header(frame_id=""), pose=Pose()), dimensions=(1.0, 1.0, 1.0), ) world.add_vis_obstacle("box", obstacle) diff --git a/dimos/manipulation/visualization/test_operator.py b/dimos/manipulation/visualization/test_operator.py index 2943a237890..8e9cfbc483c 100644 --- a/dimos/manipulation/visualization/test_operator.py +++ b/dimos/manipulation/visualization/test_operator.py @@ -18,6 +18,11 @@ from types import SimpleNamespace from unittest.mock import MagicMock +from dimos_generated.geometry_msgs.msg import Pose, PoseStamped +from dimos_generated.sensor_msgs.msg import JointState +from dimos_generated.std_msgs.msg import Header +from dimos_generated.trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint + from dimos.manipulation.manipulation_spec import ( CommandResult, CommandStatus, @@ -37,10 +42,7 @@ ManipulationOperator, PoseTargetRequest, ) -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.sensor_msgs.JointState import JointState -from dimos.msgs.trajectory_msgs.JointTrajectory import JointTrajectory -from dimos.msgs.trajectory_msgs.TrajectoryPoint import TrajectoryPoint +from dimos.msgs.time import duration_from_seconds, header_now from dimos.robot.assets.model import RobotModel @@ -58,8 +60,11 @@ def _config() -> RobotModelConfig: def _plan() -> GeneratedPlan: trajectory = JointTrajectory( + header=header_now(), joint_names=["left/j1", "left/j2"], - points=[TrajectoryPoint(positions=[0.4, 0.5], time_from_start=1.0)], + points=[ + JointTrajectoryPoint(positions=[0.4, 0.5], time_from_start=duration_from_seconds(1.0)) + ], ) return GeneratedPlan( group_ids=("left_arm",), @@ -96,7 +101,9 @@ def _operator() -> tuple[ManipulationOperator, MagicMock, MagicMock]: name=config.joint_names, position=[0.0, 0.0, 0.0] ) monitor.is_state_valid.return_value = True - monitor.get_group_ee_pose.return_value = PoseStamped(frame_id="world") + monitor.get_group_ee_pose.return_value = PoseStamped( + header=Header(frame_id="world"), pose=Pose() + ) return ManipulationOperator(module, monitor), module, monitor @@ -138,7 +145,7 @@ def test_joint_evaluation_rejects_noncanonical_unknown_and_overlapping_selection def test_pose_evaluation_routes_group_id_and_canonical_seed() -> None: operator, module, _ = _operator() - pose = PoseStamped(frame_id="world") + pose = PoseStamped(header=Header(frame_id="world"), pose=Pose()) seed = JointState(name=["left/j1", "left/j2"], position=[0.0, 0.0]) result = operator.evaluate_pose_target(PoseTargetRequest({"left_arm": pose}, seed=seed)) assert result.success @@ -149,9 +156,11 @@ def test_pose_evaluation_routes_group_id_and_canonical_seed() -> None: def test_pose_evaluation_rejects_non_world_frame_and_ambiguous_local_seed() -> None: operator, _, _ = _operator() - bad_frame = PoseTargetRequest({"left_arm": PoseStamped(frame_id="camera")}) + bad_frame = PoseTargetRequest( + {"left_arm": PoseStamped(header=Header(frame_id="camera"), pose=Pose())} + ) bad_seed = PoseTargetRequest( - {"left_arm": PoseStamped(frame_id="world")}, + {"left_arm": PoseStamped(header=Header(frame_id="world"), pose=Pose())}, seed=JointState(name=["j1", "j2"], position=[0.0, 0.0]), ) assert not operator.evaluate_pose_target(bad_frame).success @@ -163,7 +172,9 @@ def test_planning_and_actions_return_exact_generated_plan() -> None: joint_request = JointTargetRequest( ("left_arm",), JointState(name=["left/j1", "left/j2"], position=[0.1, 0.2]) ) - pose_request = PoseTargetRequest({"left_arm": PoseStamped(frame_id="world")}) + pose_request = PoseTargetRequest( + {"left_arm": PoseStamped(header=Header(frame_id="world"), pose=Pose())} + ) assert ( operator.plan_to_joints(joint_request) is module.generate_plan_to_joint_targets.return_value ) @@ -176,7 +187,7 @@ def test_planning_and_actions_return_exact_generated_plan() -> None: def test_cartesian_planning_uses_current_group_pose() -> None: operator, module, monitor = _operator() - target = PoseStamped(frame_id="world") + target = PoseStamped(header=Header(frame_id="world"), pose=Pose()) config = RoboPlanCartesianPathConfig() result = operator.plan_cartesian(CartesianTargetRequest({"left_arm": target}, config)) assert result is module.generate_cartesian_plan.return_value diff --git a/dimos/manipulation/visualization/viser/gui.py b/dimos/manipulation/visualization/viser/gui.py index c1e04aa57bf..b977c3f83fc 100644 --- a/dimos/manipulation/visualization/viser/gui.py +++ b/dimos/manipulation/visualization/viser/gui.py @@ -15,10 +15,15 @@ from __future__ import annotations from collections.abc import Callable, Mapping, Sequence +import copy import math from threading import RLock from typing import Any, TypeAlias, cast +from dimos_generated.geometry_msgs.msg import Point, Pose, PoseStamped, Quaternion +from dimos_generated.sensor_msgs.msg import JointState +from dimos_generated.std_msgs.msg import Header + from dimos.manipulation.planning.groups.models import PlanningGroup from dimos.manipulation.planning.planners.roboplan_config import RoboPlanCartesianPathConfig from dimos.manipulation.planning.spec.config import RobotModelConfig @@ -51,9 +56,6 @@ TargetEvaluationWorker, TargetStatus, ) -from dimos.msgs.geometry_msgs.Pose import Pose -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.sensor_msgs.JointState import JointState from dimos.utils.logging_config import setup_logger from dimos.utils.trigonometry import angle_diff @@ -96,7 +98,7 @@ def group_display_name(group: PlanningGroup) -> str: def _copy_joint_state(state: JointState | None) -> JointState | None: - return None if state is None else JointState(state) + return None if state is None else copy.deepcopy(state) ROBOT_DISPLAY_LABELS = tuple(mode.value.title() for mode in RobotDisplayMode) @@ -204,10 +206,7 @@ def _current_target_for_group(self, group: PlanningGroup) -> dict[PlanningGroupI return {} return { group.id: JointState( - { - "name": list(group.joint_names), - "position": [values[name] for name in group.joint_names], - } + name=list(group.joint_names), position=[values[name] for name in group.joint_names] ) } @@ -234,21 +233,16 @@ def evaluate_joint_target_set( names.extend(str(name) for name in target.name) positions.extend(float(value) for value in target.position) return self.operator.evaluate_joint_target( - JointTargetRequest(tuple(group_ids), JointState({"name": names, "position": positions})) + JointTargetRequest(tuple(group_ids), JointState(name=names, position=positions)) ) def evaluate_pose_target_set( self, - pose_targets: Mapping[PlanningGroupID, Pose], + pose_targets: Mapping[PlanningGroupID, PoseStamped], auxiliary_group_ids: Sequence[PlanningGroupID] = (), seed: JointState | None = None, ) -> TargetEvaluationResult: - stamped = { - group_id: PoseStamped( - frame_id="world", position=pose.position, orientation=pose.orientation - ) - for group_id, pose in pose_targets.items() - } + stamped = dict(pose_targets) return self.operator.evaluate_pose_target( PoseTargetRequest(stamped, tuple(auxiliary_group_ids), _copy_joint_state(seed)) ) @@ -271,24 +265,17 @@ def plan_to_selected_joints( names.extend(str(name) for name in target.name) positions.extend(float(value) for value in target.position) plan = self.operator.plan_to_joints( - JointTargetRequest(tuple(group_ids), JointState({"name": names, "position": positions})) + JointTargetRequest(tuple(group_ids), JointState(name=names, position=positions)) ) self.state.plan_state.plan = plan return plan is not None def plan_cartesian( self, - pose_targets: Mapping[PlanningGroupID, Pose], + pose_targets: Mapping[PlanningGroupID, PoseStamped], auxiliary_group_ids: Sequence[PlanningGroupID], ) -> bool: - stamped = { - group_id: PoseStamped( - frame_id="world", - position=pose.position, - orientation=pose.orientation, - ) - for group_id, pose in pose_targets.items() - } + stamped = dict(pose_targets) plan = self.operator.plan_cartesian( CartesianTargetRequest( stamped, @@ -701,10 +688,8 @@ def _initialize_selected_group_targets(self) -> None: if any(str(name) not in values for name in group.joint_names): continue self.state.group_joint_targets[group_id] = JointState( - { - "name": list(group.joint_names), - "position": [float(values[str(name)]) for name in group.joint_names], - } + name=list(group.joint_names), + position=[float(values[str(name)]) for name in group.joint_names], ) if group.has_pose_target and group_id not in self.state.pose_targets: pose = self.get_group_ee_pose(group_id) @@ -735,11 +720,9 @@ def _refresh_target_joints_from_groups(self) -> None: if target is not None: names.extend(str(name) for name in target.name) positions.extend(float(value) for value in target.position) - self.state.target_joints = ( - JointState({"name": names, "position": positions}) if names else None - ) + self.state.target_joints = JointState(name=names, position=positions) if names else None - def _active_pose_targets(self) -> dict[PlanningGroupID, Pose]: + def _active_pose_targets(self) -> dict[PlanningGroupID, PoseStamped]: return { group_id: self.state.pose_targets[group_id] for group_id in self.state.selected_group_ids @@ -806,7 +789,7 @@ def _apply_preset(self, preset: str) -> None: ) return positions = [float(values[str(name)]) for name in group.joint_names] - targets[group_id] = JointState({"name": list(group.joint_names), "position": positions}) + targets[group_id] = JointState(name=list(group.joint_names), position=positions) slider_values.append((group_id, group.joint_names, positions)) self.state.group_joint_targets.update(targets) with self._joint_controls_lock: @@ -854,9 +837,7 @@ def _target_set_from_sliders(self) -> dict[PlanningGroupID, JointState] | None: self._set_error(f"Missing target slider for {group_id}/{joint_name}") return None positions.append(float(handle.value)) - targets[group_id] = JointState( - {"name": list(group.joint_names), "position": positions} - ) + targets[group_id] = JointState(name=list(group.joint_names), position=positions) return targets def _on_joint_slider_update(self, _group_id: PlanningGroupID, _joint_name: str) -> None: @@ -893,7 +874,7 @@ def _on_transform_update( joints=( None if self.state.target_joints is None - else JointState(self.state.target_joints) + else copy.deepcopy(self.state.target_joints) ), pose_targets=dict(self._active_pose_targets()), ) @@ -926,7 +907,7 @@ def _move_joint_target_visuals(self, targets: Mapping[PlanningGroupID, JointStat state = self._target_ghost_state(targets) if state is not None: config = self.get_model_config() - self.scene.set_target_joints(config.joint_names, state.position) + self.scene.set_target_joints(config.joint_names, list(state.position)) def _target_ghost_state( self, targets: Mapping[PlanningGroupID, JointState] @@ -944,10 +925,7 @@ def _target_ghost_state( if not all(name in values for name in config.joint_names): return None return JointState( - { - "name": list(config.joint_names), - "position": [values[name] for name in config.joint_names], - } + name=list(config.joint_names), position=[values[name] for name in config.joint_names] ) def _sync_target_ghost_visibility(self) -> None: @@ -992,12 +970,12 @@ def _apply_target_evaluation_result( ) self.state.error = "" if success and collision_free else self.state.feasibility.message if result.target_joints is not None: - self.state.target_joints = JointState(result.target_joints) + self.state.target_joints = copy.deepcopy(result.target_joints) self._split_target_joints_by_group(result.target_joints) self.state.group_poses = { str(group_id): pose for group_id, pose in result.group_poses.items() - if isinstance(pose, Pose) + if isinstance(pose, PoseStamped) } if request.source == "joints": self._sync_pose_targets_from_group_poses() @@ -1025,10 +1003,8 @@ def _split_target_joints_by_group(self, target_joints: JointState) -> None: if group is None or any(str(name) not in positions for name in group.joint_names): continue self.state.group_joint_targets[group_id] = JointState( - { - "name": list(group.joint_names), - "position": [positions[str(name)] for name in group.joint_names], - } + name=list(group.joint_names), + position=[positions[str(name)] for name in group.joint_names], ) def _sync_pose_targets_from_group_poses(self) -> None: @@ -1118,7 +1094,7 @@ def _submit_plan(self) -> None: selection_epoch = self.state.selection_epoch target_sequence_id = self.state.latest_sequence_id joint_targets: dict[PlanningGroupID, JointState] | None = None - pose_targets: dict[PlanningGroupID, Pose] = {} + pose_targets: dict[PlanningGroupID, PoseStamped] = {} auxiliary_group_ids: tuple[PlanningGroupID, ...] = () if planning_mode == PlanningMode.CARTESIAN_SPACE: pose_targets = self._active_pose_targets() @@ -1413,10 +1389,15 @@ def _set_visible(self, key: str, visible: bool) -> None: def _set_optional_handle_attr(handle: object, attr: str, value: object) -> None: setattr(handle, attr, value) - def _pose_from_transform_target(self, target: TransformControlsHandle) -> Pose | None: + def _pose_from_transform_target(self, target: TransformControlsHandle) -> PoseStamped | None: px, py, pz = (float(value) for value in target.position) qw, qx, qy, qz = (float(value) for value in target.wxyz) - return Pose({"position": [px, py, pz], "orientation": [qx, qy, qz, qw]}) + return PoseStamped( + header=Header(frame_id="world"), + pose=Pose( + position=Point(x=px, y=py, z=pz), orientation=Quaternion(x=qx, y=qy, z=qz, w=qw) + ), + ) def _feasibility_status( self, result: TargetEvaluationResult, success: bool, collision_free: bool diff --git a/dimos/manipulation/visualization/viser/scene.py b/dimos/manipulation/visualization/viser/scene.py index 5591d1a263a..ecab0f6c0ae 100644 --- a/dimos/manipulation/visualization/viser/scene.py +++ b/dimos/manipulation/visualization/viser/scene.py @@ -25,6 +25,9 @@ from typing import Any, Protocol, TypeAlias, cast import xml.etree.ElementTree as ET +from dimos_generated.dimos_msgs.msg import GraspCandidate, GraspCandidateArray +from dimos_generated.geometry_msgs.msg import PoseStamped +from dimos_generated.sensor_msgs.msg import JointState import numpy as np from numpy.typing import NDArray import trimesh @@ -43,11 +46,7 @@ VISER_INSTALL_HINT, VISER_URDF_INSTALL_HINT, ) -from dimos.msgs.geometry_msgs.Pose import Pose -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.manipulation_msgs.GraspCandidate import GraspCandidate -from dimos.msgs.manipulation_msgs.GraspCandidateArray import GraspCandidateArray -from dimos.msgs.sensor_msgs.JointState import JointState +from dimos.msgs.geometry import pose_matrix from dimos.robot.assets.model import LoadedRobotModel from dimos.utils.logging_config import setup_logger @@ -330,7 +329,7 @@ def show_grasp_proposals(self, candidates: GraspCandidateArray) -> None: if self._closed: return self.clear_grasp_proposals() - drawn = list(candidates.candidates[:GRASP_PROPOSAL_DRAW_LIMIT]) + drawn = list(candidates.candidates)[:GRASP_PROPOSAL_DRAW_LIMIT] if not drawn: return emphasis = min(GRASP_PROPOSAL_EMPHASIS_COUNT, len(drawn)) @@ -396,7 +395,7 @@ def _grasp_proposal_segments( ) for rank in range(start, stop): pose = candidates[rank].pose - rotation = np.asarray(pose.orientation.to_rotation_matrix(), dtype=np.float32) + rotation = np.asarray(pose_matrix(pose)[:3, :3], dtype=np.float32) origin = np.asarray( [pose.position.x, pose.position.y, pose.position.z], dtype=np.float32 ) @@ -448,12 +447,12 @@ def _obstacle_pose( ) -> tuple[tuple[float, float, float], tuple[float, float, float, float]]: pose = obstacle.pose return ( - (float(pose.position.x), float(pose.position.y), float(pose.position.z)), + (float(pose.pose.position.x), float(pose.pose.position.y), float(pose.pose.position.z)), ( - float(pose.orientation.w), - float(pose.orientation.x), - float(pose.orientation.y), - float(pose.orientation.z), + float(pose.pose.orientation.w), + float(pose.pose.orientation.x), + float(pose.pose.orientation.y), + float(pose.pose.orientation.z), ), ) @@ -661,14 +660,14 @@ def update_current_model(self, joint_state: JointState | None) -> None: return self._ensure_robot_urdfs(config) current = self._urdfs.get("current") - self.set_urdf_joints(current, config.joint_names, joint_state.position) + self.set_urdf_joints(current, config.joint_names, list(joint_state.position)) if self._target_tracks_current: - self._set_target_joints(config.joint_names, joint_state.position) + self._set_target_joints(config.joint_names, list(joint_state.position)) self._set_target_visibility(self._target_active) self.set_urdf_joints( self._collision_fallback_urdf, config.joint_names, - joint_state.position, + list(joint_state.position), ) def cancel_preview_animation(self) -> None: @@ -730,20 +729,20 @@ def _set_preview_ghost_joints( ghost = self._urdfs.get("preview") self.set_urdf_joints(ghost, joint_names, joints) - def set_target_pose(self, control_id: str, pose: Pose | None) -> None: + def set_target_pose(self, control_id: str, pose: PoseStamped | None) -> None: handle = self._handles.get(f"{control_id}:ee_control") if handle is None or pose is None: return handle.position = ( - float(pose.position.x), - float(pose.position.y), - float(pose.position.z), + float(pose.pose.position.x), + float(pose.pose.position.y), + float(pose.pose.position.z), ) handle.wxyz = ( - float(pose.orientation.w), - float(pose.orientation.x), - float(pose.orientation.y), - float(pose.orientation.z), + float(pose.pose.orientation.w), + float(pose.pose.orientation.x), + float(pose.pose.orientation.y), + float(pose.pose.orientation.z), ) def set_target_control_visual_state(self, control_id: str, feasible: bool) -> None: @@ -990,23 +989,21 @@ def _ensure_base_pose_frame(self, kind: str, config: RobotModelConfig) -> None: frame_name, show_axes=False, position=( - float(pose.position.x), - float(pose.position.y), - float(pose.position.z), + float(pose.pose.position.x), + float(pose.pose.position.y), + float(pose.pose.position.z), ), wxyz=( - float(pose.orientation.w), - float(pose.orientation.x), - float(pose.orientation.y), - float(pose.orientation.z), + float(pose.pose.orientation.w), + float(pose.pose.orientation.x), + float(pose.pose.orientation.y), + float(pose.pose.orientation.z), ), ) @staticmethod def _has_non_identity_base_pose(config: RobotModelConfig) -> bool: - pose = getattr(config, "base_pose", None) - if pose is None: - return False + pose = config.base_pose.pose return any( abs(value) > 1e-12 for value in ( diff --git a/dimos/manipulation/visualization/viser/state.py b/dimos/manipulation/visualization/viser/state.py index 90d43037dfe..f836b17fcc2 100644 --- a/dimos/manipulation/visualization/viser/state.py +++ b/dimos/manipulation/visualization/viser/state.py @@ -21,10 +21,11 @@ import threading from typing import Literal +from dimos_generated.geometry_msgs.msg import PoseStamped +from dimos_generated.sensor_msgs.msg import JointState + from dimos.manipulation.planning.spec.models import GeneratedPlan, PlanningGroupID from dimos.manipulation.visualization.operator import TargetEvaluationResult -from dimos.msgs.geometry_msgs.Pose import Pose -from dimos.msgs.sensor_msgs.JointState import JointState from dimos.utils.logging_config import setup_logger logger = setup_logger() @@ -107,17 +108,17 @@ class PanelState: selected_group_ids: tuple[PlanningGroupID, ...] = () planning_mode: PlanningMode = PlanningMode.JOINT_SPACE selection_epoch: int = 0 - pose_targets: dict[PlanningGroupID, Pose] = field(default_factory=dict) + pose_targets: dict[PlanningGroupID, PoseStamped] = field(default_factory=dict) group_joint_targets: dict[PlanningGroupID, JointState] = field(default_factory=dict) target_joints: JointState | None = None - group_poses: dict[PlanningGroupID, Pose] = field(default_factory=dict) + group_poses: dict[PlanningGroupID, PoseStamped] = field(default_factory=dict) runtime: PanelRuntime = PanelRuntime.STOPPED backend_status: BackendConnectionStatus = BackendConnectionStatus.DISCONNECTED target_status: TargetStatus = TargetStatus.EMPTY action_status: ActionStatus = ActionStatus.IDLE manipulation_state: str = "DISCONNECTED" current_joints: list[float] | None = None - cartesian_target: Pose | None = None + cartesian_target: PoseStamped | None = None feasibility: FeasibilityState = field(default_factory=FeasibilityState) latest_sequence_id: int = 0 plan_state: PanelPlanState = field(default_factory=PanelPlanState) @@ -207,10 +208,10 @@ class TargetEvaluationRequest: source: PreviewSource selection_epoch: int = 0 group_ids: tuple[PlanningGroupID, ...] = () - pose: Pose | None = None + pose: PoseStamped | None = None joints: JointState | None = None auxiliary_group_ids: tuple[PlanningGroupID, ...] = () - pose_targets: dict[PlanningGroupID, Pose] = field(default_factory=dict) + pose_targets: dict[PlanningGroupID, PoseStamped] = field(default_factory=dict) joint_targets: dict[PlanningGroupID, JointState] = field(default_factory=dict) diff --git a/dimos/manipulation/visualization/viser/test_gui.py b/dimos/manipulation/visualization/viser/test_gui.py index d7b8b3ebcad..b89c7e42583 100644 --- a/dimos/manipulation/visualization/viser/test_gui.py +++ b/dimos/manipulation/visualization/viser/test_gui.py @@ -23,8 +23,14 @@ import pytest +from dimos.msgs.time import duration_from_seconds, header_now + pytest.importorskip("viser", reason="Viser optional dependency is not installed") +from dimos_generated.dimos_msgs.msg import TrajectoryStatus +from dimos_generated.sensor_msgs.msg import JointState +from dimos_generated.trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint + from dimos.control.tasks.trajectory_task.trajectory_task import ( TrajectoryExecutionResult, TrajectoryExecutionStatus, @@ -61,10 +67,7 @@ TargetEvaluationWorker, TargetStatus, ) -from dimos.msgs.sensor_msgs.JointState import JointState -from dimos.msgs.trajectory_msgs.JointTrajectory import JointTrajectory -from dimos.msgs.trajectory_msgs.TrajectoryPoint import TrajectoryPoint -from dimos.msgs.trajectory_msgs.TrajectoryStatus import TrajectoryState, TrajectoryStatus +from dimos.msgs.trajectory import TrajectoryState from dimos.robot.assets.model import LoadedRobotModel, PlanarBaseDefinition, RobotModel @@ -264,10 +267,15 @@ def executable_gui(monkeypatch, mocker): plan=GeneratedPlan( group_ids=gui.state.selected_group_ids, trajectory=JointTrajectory( + header=header_now(), joint_names=["arm/j0"], points=[ - TrajectoryPoint(positions=[0.0], time_from_start=0.0), - TrajectoryPoint(positions=[1.0], time_from_start=1.0), + JointTrajectoryPoint( + positions=[0.0], time_from_start=duration_from_seconds(0.0) + ), + JointTrajectoryPoint( + positions=[1.0], time_from_start=duration_from_seconds(1.0) + ), ], ), path=[JointState(name=["arm/j0"], position=[value]) for value in (0.0, 1.0)], @@ -353,7 +361,7 @@ def test_gui_completion_enables_next_plan_without_cancel(executable_gui, module_ def invoke(task, method, args=None): if method == "execute": return TrajectoryExecutionResult(TrajectoryExecutionStatus.ACCEPTED) - return TrajectoryStatus(state=reported["state"]) + return TrajectoryStatus(header=header_now(), state=reported["state"]) mocker.patch.object(module._control_coordinator, "task_invoke", side_effect=invoke) cancel = mocker.spy(module, "cancel") @@ -606,8 +614,10 @@ def test_gui_preview_enters_previewing_before_worker_runs( gui.state.plan_state.target_sequence_id = gui.state.latest_sequence_id gui.state.plan_state.plan = GeneratedPlan( group_ids=gui.state.selected_group_ids, - trajectory=JointTrajectory(), - path=[JointState({"name": [], "position": []})], + trajectory=JointTrajectory( + header=header_now(), + ), + path=[JointState(name=[], position=[])], ) assert gui.state.can_execute() is True @@ -643,8 +653,10 @@ def test_gui_selection_change_clears_invalidated_preview( gui.state.plan_state.target_sequence_id = gui.state.latest_sequence_id gui.state.plan_state.plan = GeneratedPlan( group_ids=gui.state.selected_group_ids, - trajectory=JointTrajectory(), - path=[JointState({"name": [], "position": []})], + trajectory=JointTrajectory( + header=header_now(), + ), + path=[JointState(name=[], position=[])], ) gui._submit_preview() @@ -679,8 +691,10 @@ def test_gui_selection_change_ignores_invalidated_preview_error( gui.state.plan_state.target_sequence_id = gui.state.latest_sequence_id gui.state.plan_state.plan = GeneratedPlan( group_ids=gui.state.selected_group_ids, - trajectory=JointTrajectory(), - path=[JointState({"name": [], "position": []})], + trajectory=JointTrajectory( + header=header_now(), + ), + path=[JointState(name=[], position=[])], ) gui._submit_preview() diff --git a/dimos/manipulation/visualization/viser/test_scene_grasp_proposals.py b/dimos/manipulation/visualization/viser/test_scene_grasp_proposals.py index 88ec1369799..f81f7de9131 100644 --- a/dimos/manipulation/visualization/viser/test_scene_grasp_proposals.py +++ b/dimos/manipulation/visualization/viser/test_scene_grasp_proposals.py @@ -17,22 +17,22 @@ import inspect from unittest.mock import MagicMock +from dimos_generated.geometry_msgs.msg import Point import numpy as np import pytest pytest.importorskip("viser", reason="Viser optional dependency is not installed") +from dimos_generated.dimos_msgs.msg import GraspCandidate, GraspCandidateArray +from dimos_generated.geometry_msgs.msg import Pose, Quaternion +from dimos_generated.std_msgs.msg import Header + from dimos.manipulation.visualization.viser.scene import ( GRASP_PROPOSAL_DRAW_LIMIT, GRASP_PROPOSAL_EMPHASIS_COUNT, ViserManipulationScene, ) -from dimos.msgs.geometry_msgs.Pose import Pose -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.msgs.manipulation_msgs.GraspCandidate import GraspCandidate -from dimos.msgs.manipulation_msgs.GraspCandidateArray import GraspCandidateArray -from dimos.msgs.std_msgs.Header import Header +from dimos.msgs.time import time_from_seconds def _scene() -> ViserManipulationScene: @@ -41,11 +41,14 @@ def _scene() -> ViserManipulationScene: def _proposals(count: int) -> GraspCandidateArray: return GraspCandidateArray( - Header(1.0, "world"), - [ + header=Header(stamp=time_from_seconds(1.0), frame_id="world"), + candidates=[ GraspCandidate( - Pose(Vector3(0.1 * index, 0.0, 0.2), Quaternion(0.0, 0.0, 0.0, 1.0)), - 1.0 - 0.01 * index, + pose=Pose( + position=Point(x=0.1 * index, y=0.0, z=0.2), + orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0), + ), + score=1.0 - 0.01 * index, ) for index in range(count) ], diff --git a/dimos/manipulation/visualization/viser/test_scene_obstacles.py b/dimos/manipulation/visualization/viser/test_scene_obstacles.py index fd88770c5d9..02bda7e1916 100644 --- a/dimos/manipulation/visualization/viser/test_scene_obstacles.py +++ b/dimos/manipulation/visualization/viser/test_scene_obstacles.py @@ -16,15 +16,18 @@ from unittest.mock import MagicMock +from dimos_generated.geometry_msgs.msg import Pose +from dimos_generated.std_msgs.msg import Header import numpy as np import pytest pytest.importorskip("viser", reason="Viser optional dependency is not installed") +from dimos_generated.geometry_msgs.msg import PoseStamped + from dimos.manipulation.planning.spec.enums import ObstacleType from dimos.manipulation.planning.spec.models import Obstacle from dimos.manipulation.visualization.viser.scene import ViserManipulationScene -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped def _scene() -> ViserManipulationScene: @@ -35,7 +38,7 @@ def _octree(points: np.ndarray, resolution: float = 0.025) -> Obstacle: return Obstacle( name="mapping/voxel-map", obstacle_type=ObstacleType.OCTREE, - pose=PoseStamped(frame_id="world"), + pose=PoseStamped(header=Header(frame_id="world"), pose=Pose()), points=points, octree_resolution=resolution, ) diff --git a/dimos/manipulation/visualization/viser/test_viser_visualization.py b/dimos/manipulation/visualization/viser/test_viser_visualization.py index 30d96f2b385..e7f57cdd812 100644 --- a/dimos/manipulation/visualization/viser/test_viser_visualization.py +++ b/dimos/manipulation/visualization/viser/test_viser_visualization.py @@ -19,8 +19,13 @@ import pytest +from dimos.msgs.time import duration_from_seconds, header_now + pytest.importorskip("viser", reason="Viser optional dependency is not installed") +from dimos_generated.sensor_msgs.msg import JointState +from dimos_generated.trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint + from dimos.manipulation.planning.groups.models import PlanningGroupDefinition from dimos.manipulation.planning.spec.config import RobotModelConfig from dimos.manipulation.planning.spec.models import ( @@ -34,9 +39,6 @@ scaled_frame_delays, ) from dimos.manipulation.visualization.viser.visualizer import ViserManipulationVisualizer -from dimos.msgs.sensor_msgs.JointState import JointState -from dimos.msgs.trajectory_msgs.JointTrajectory import JointTrajectory -from dimos.msgs.trajectory_msgs.TrajectoryPoint import TrajectoryPoint from dimos.robot.assets.model import RobotModel @@ -69,8 +71,9 @@ def test_visualizer_builds_full_model_preview_from_selected_canonical_joints() - visualizer._model_config = _model() visualizer._current_state = JointState(name=["left/j1", "right/j1"], position=[0.1, 0.2]) trajectory = JointTrajectory( + header=header_now(), joint_names=["right/j1"], - points=[TrajectoryPoint(positions=[0.8], time_from_start=1.0)], + points=[JointTrajectoryPoint(positions=[0.8], time_from_start=duration_from_seconds(1.0))], ) preview = visualizer._raw_preview_animation(trajectory) assert preview == PreviewAnimation(("left/j1", "right/j1"), (PreviewFrame(1.0, (0.1, 0.8)),)) @@ -82,8 +85,13 @@ def test_visualizer_rejects_unknown_or_duplicate_trajectory_joints() -> None: visualizer._current_state = JointState(name=["left/j1", "right/j1"], position=[0.1, 0.2]) for names in (["unknown"], ["left/j1", "left/j1"]): trajectory = JointTrajectory( + header=header_now(), joint_names=names, - points=[TrajectoryPoint(positions=[0.0] * len(names), time_from_start=1.0)], + points=[ + JointTrajectoryPoint( + positions=[0.0] * len(names), time_from_start=duration_from_seconds(1.0) + ) + ], ) assert visualizer._raw_preview_animation(trajectory) is None diff --git a/dimos/manipulation/visualization/viser/test_visualizer_lifecycle.py b/dimos/manipulation/visualization/viser/test_visualizer_lifecycle.py index 44631f6bcfb..4d12c4409c6 100644 --- a/dimos/manipulation/visualization/viser/test_visualizer_lifecycle.py +++ b/dimos/manipulation/visualization/viser/test_visualizer_lifecycle.py @@ -17,10 +17,18 @@ from pathlib import Path from types import SimpleNamespace +from dimos_generated.geometry_msgs.msg import Pose +from dimos_generated.std_msgs.msg import Header import pytest +from dimos.msgs.time import header_now + pytest.importorskip("viser", reason="Viser optional dependency is not installed") +from dimos_generated.geometry_msgs.msg import PoseStamped +from dimos_generated.sensor_msgs.msg import JointState +from dimos_generated.trajectory_msgs.msg import JointTrajectory + from dimos.manipulation.planning.groups.models import PlanningGroupDefinition from dimos.manipulation.planning.spec.config import RobotModelConfig from dimos.manipulation.planning.spec.joint_space import JointSpace @@ -38,9 +46,6 @@ from dimos.manipulation.visualization.viser.runtime import ViserRuntime from dimos.manipulation.visualization.viser.scene import RobotDisplayMode, ViserManipulationScene from dimos.manipulation.visualization.viser.visualizer import ViserManipulationVisualizer -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.sensor_msgs.JointState import JointState -from dimos.msgs.trajectory_msgs.JointTrajectory import JointTrajectory from dimos.robot.assets.model import LoadedRobotModel, RobotModel @@ -86,7 +91,7 @@ def stop(self) -> None: def fake_robot_config(name: str) -> RobotModelConfig: return RobotModelConfig( model=RobotModel.from_file(Path(f"{name}.urdf")), - base_pose=PoseStamped(), + base_pose=PoseStamped(header=Header(frame_id=""), pose=Pose()), joint_names=[], planning_groups=[ PlanningGroupDefinition( @@ -424,7 +429,7 @@ def test_visualizer_publish_preview_and_close_paths( monkeypatch: pytest.MonkeyPatch, ) -> None: calls: list[tuple[str, str]] = [] - current = JointState({"name": ["joint1"], "position": [0.5]}) + current = JointState(name=["joint1"], position=[0.5]) class FakeRuntime: url = "http://localhost:8095" @@ -476,7 +481,9 @@ def close(self) -> None: visualizer.cancel_preview_animation() visualizer.update_state(VisualizationStateFrame(current)) visualizer.cancel_preview_animation() - visualizer.animate_trajectory(JointTrajectory(joint_names=["arm/joint1"]), duration=1.5) + visualizer.animate_trajectory( + JointTrajectory(header=header_now(), joint_names=["arm/joint1"]), duration=1.5 + ) visualizer.close() visualizer.update_state(VisualizationStateFrame(current)) @@ -541,7 +548,7 @@ def __init__( config = fake_robot_config("arm") config.model = RobotModel.from_file(fixed_world_root) - config.base_pose.position.x = 1.0 + config.base_pose.pose.position.x = 1.0 def prepare(description: object, **kwargs: object) -> object: prepared.append(kwargs) @@ -595,7 +602,7 @@ def test_selected_display_mode_survives_primary_recreation_and_joint_updates( scene.register_model(prepared) current = scene._urdfs["current"] - scene.update_current_model(JointState({"name": ["joint1"], "position": [0.75]})) + scene.update_current_model(JointState(name=["joint1"], position=[0.75])) assert current is not old_current assert scene.robot_display_mode == mode diff --git a/dimos/manipulation/visualization/viser/visualizer.py b/dimos/manipulation/visualization/viser/visualizer.py index bfe364bf040..3fc8b0f1643 100644 --- a/dimos/manipulation/visualization/viser/visualizer.py +++ b/dimos/manipulation/visualization/viser/visualizer.py @@ -15,8 +15,13 @@ from __future__ import annotations from contextlib import suppress +import copy from typing import TYPE_CHECKING +from dimos_generated.dimos_msgs.msg import GraspCandidateArray +from dimos_generated.sensor_msgs.msg import JointState +from dimos_generated.trajectory_msgs.msg import JointTrajectory + from dimos.manipulation.visualization.viser.animation import ( PreviewAnimation, PreviewFrame, @@ -30,9 +35,8 @@ ) from dimos.manipulation.visualization.viser.scene import ViserManipulationScene from dimos.manipulation.visualization.viser.theme import apply_dimos_theme -from dimos.msgs.manipulation_msgs.GraspCandidateArray import GraspCandidateArray -from dimos.msgs.sensor_msgs.JointState import JointState -from dimos.msgs.trajectory_msgs.JointTrajectory import JointTrajectory +from dimos.msgs.time import to_seconds +from dimos.msgs.trajectory import trajectory_duration from dimos.utils.logging_config import setup_logger try: @@ -47,6 +51,8 @@ raise ModuleNotFoundError(VISER_URDF_INSTALL_HINT) from e if TYPE_CHECKING: + from dimos_generated.geometry_msgs.msg import PoseStamped + from dimos.manipulation.planning.spec.config import RobotModelConfig from dimos.manipulation.planning.spec.models import ( Obstacle, @@ -54,7 +60,6 @@ VisualizationSession, VisualizationStateFrame, ) - from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped logger = setup_logger() @@ -233,7 +238,7 @@ def update_state(self, frame: VisualizationStateFrame) -> None: if self._scene is None: return if frame.joint_state is not None: - self._current_state = JointState(frame.joint_state) + self._current_state = copy.deepcopy(frame.joint_state) self._scene.update_current_model(frame.joint_state) if self._gui is not None: self._gui.refresh() @@ -249,7 +254,10 @@ def animate_trajectory( preview = self._raw_preview_animation(trajectory) if preview is not None: self._scene.animate_preview( - preview, duration if duration is not None else max(float(trajectory.duration), 0.0) + preview, + duration + if duration is not None + else max(float(trajectory_duration(trajectory)), 0.0), ) def cancel_preview_animation(self) -> None: @@ -280,7 +288,7 @@ def _raw_preview_animation(self, trajectory: JointTrajectory) -> PreviewAnimatio for point in trajectory.points: selected = {name: float(point.positions[index]) for name, index in indices.items()} positions = [float(selected.get(name, baseline[name])) for name in config.joint_names] - frames.append(PreviewFrame(float(point.time_from_start), tuple(positions))) + frames.append(PreviewFrame(float(to_seconds(point.time_from_start)), tuple(positions))) return PreviewAnimation(tuple(config.joint_names), tuple(frames)) if frames else None @staticmethod diff --git a/dimos/mapping/cli/map.py b/dimos/mapping/cli/map.py index 6fca8a6a2e9..23cd92fb024 100644 --- a/dimos/mapping/cli/map.py +++ b/dimos/mapping/cli/map.py @@ -23,12 +23,12 @@ import typer if TYPE_CHECKING: + from dimos_generated.geometry_msgs.msg import TransformStamped + from dimos_generated.sensor_msgs.msg import Image, PointCloud2 + from dimos.mapping.loop_closure.pgo import PoseGraph from dimos.memory.stream import Stream from dimos.memory.type.observation import Observation - from dimos.msgs.geometry_msgs.Transform import Transform - from dimos.msgs.sensor_msgs.Image import Image - from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 PATH_THICKNESS = 0.01 # Pin pattern (from dimos/memory/vis/space/rerun.py): thin vertical line @@ -109,7 +109,7 @@ def _accumulate( block_count: int, device: str, graph: PoseGraph | None = None, - register: Callable[[Observation[Any]], Transform | None] | None = None, + register: Callable[[Observation[Any]], TransformStamped | None] | None = None, carve_columns: bool = False, progress_cb: Callable[[Observation[Any]], None] | None = None, ) -> PointCloud2 | None: @@ -122,18 +122,24 @@ def _accumulate( Returns the final ``PointCloud2`` (or ``None`` if the input was empty). Disposal of the underlying ``VoxelGrid`` is handled by ``VoxelMapTransformer``. """ + from dimos_generated.geometry_msgs.msg import TransformStamped + from dimos_generated.std_msgs.msg import Header + import numpy as np + from dimos.mapping.voxels.module import VoxelMapTransformer + from dimos.msgs.geometry import transform_from_matrix, transform_matrix + from dimos.msgs.pointcloud import transform_cloud def prepared() -> Iterable[Observation[PointCloud2]]: for obs in obs_iter: if progress_cb is not None: progress_cb(obs) - if len(obs.data) == 0: + if obs.data.width * obs.data.height == 0: continue # sensor->world via `register`, unless the clouds are already # world-registered. graph adds the PGO correction on top # (correction ∘ tf), applied after the registration. - tf: Transform | None = None + tf: TransformStamped | None = None if register is not None: tf = register(obs) if tf is None: @@ -141,9 +147,14 @@ def prepared() -> Iterable[Observation[PointCloud2]]: if graph is not None: if obs.pose_tuple is None: continue - correction = graph.correction_at(obs.ts) - tf = correction if tf is None else correction + tf - yield obs if tf is None else obs.derive(data=obs.data.transform(tf)) + correction = transform_matrix(graph.correction_at(obs.ts).transform) + matrix = np.eye(4) if tf is None else transform_matrix(tf.transform) + tf = TransformStamped( + header=Header(stamp=obs.data.header.stamp, frame_id="world_corrected"), + child_frame_id=obs.data.header.frame_id, + transform=transform_from_matrix(correction @ matrix), + ) + yield obs if tf is None else obs.derive(data=transform_cloud(obs.data, tf)) vmt = VoxelMapTransformer( emit_every=0, # batch mode: emit once on exhaustion @@ -158,12 +169,26 @@ def prepared() -> Iterable[Observation[PointCloud2]]: def _denoise(cloud: PointCloud2 | None) -> PointCloud2 | None: """Statistical outlier removal via o3d; drops sparse floaters, keeps colors.""" - from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 - - if cloud is None or len(cloud.pointcloud.points) < 20: + if cloud is None or cloud.width * cloud.height < 20: return cloud - clean, _ = cloud.pointcloud_tensor.remove_statistical_outliers(nb_neighbors=20, std_ratio=2.0) - return PointCloud2(pointcloud=clean, frame_id=cloud.frame_id, ts=cloud.ts) + import numpy as np + import open3d as o3d + + from dimos.msgs.pointcloud import ( + pointcloud_from_xyz, + pointcloud_from_xyz_rgb, + pointcloud_to_open3d, + ) + + if {field.name for field in cloud.fields} - {"x", "y", "z", "rgb"}: + raise ValueError("map denoising supports only XYZ/RGB point fields") + tensor = o3d.t.geometry.PointCloud.from_legacy(pointcloud_to_open3d(cloud)) + clean, _ = tensor.remove_statistical_outliers(nb_neighbors=20, std_ratio=2.0) + points = clean.point.positions.numpy() + if "colors" in clean.point: + colors = (np.clip(clean.point.colors.numpy(), 0.0, 1.0) * 255).astype(np.uint8) + return pointcloud_from_xyz_rgb(points, colors, header=cloud.header) + return pointcloud_from_xyz(points, header=cloud.header) def _log_reconstruction( @@ -180,17 +205,19 @@ def _log_reconstruction( bottom_cutoff: float | None = None, ) -> None: """Log maps, paths, the PGO graph, and markers to the active rerun recording.""" + from dimos_generated.geometry_msgs.msg import Point, Pose import rerun as rr import rerun.blueprint as rrb from dimos.memory.vis.color import Color - from dimos.msgs.geometry_msgs.Transform import Transform + from dimos.msgs.geometry import pose_from_matrix, pose_matrix, transform_matrix + from dimos.visualization.rerun.message_helpers import cloud_archetype rr.send_blueprint(rrb.Blueprint(rrb.Spatial3DView(origin="world"))) if global_map is not None: rr.log( "world/raw_map/pointcloud", - global_map.to_rerun(voxel_size=voxel / 2, bottom_cutoff=bottom_cutoff), + cloud_archetype(global_map, ui_radius=voxel / 2, bottom_cutoff=bottom_cutoff), static=True, ) if path: @@ -202,13 +229,13 @@ def _log_reconstruction( if pgo_map is not None: rr.log( "world/pgo_map/pointcloud", - pgo_map.to_rerun(voxel_size=voxel / 2, bottom_cutoff=bottom_cutoff), + cloud_archetype(pgo_map, ui_radius=voxel / 2, bottom_cutoff=bottom_cutoff), static=True, ) if full_pgo_map is not None: rr.log( "world/full_pgo_map/pointcloud", - full_pgo_map.to_rerun(voxel_size=voxel / 2, bottom_cutoff=bottom_cutoff), + cloud_archetype(full_pgo_map, ui_radius=voxel / 2, bottom_cutoff=bottom_cutoff), static=True, ) if pgo_path: @@ -225,8 +252,16 @@ def _log_reconstruction( if graph is not None and graph.loops: loop_strips = [ [ - (lc.source.translation.x, lc.source.translation.y, lc.source.translation.z), - (lc.target.translation.x, lc.target.translation.y, lc.target.translation.z), + ( + lc.source.transform.translation.x, + lc.source.transform.translation.y, + lc.source.transform.translation.z, + ), + ( + lc.target.transform.translation.x, + lc.target.transform.translation.y, + lc.target.transform.translation.z, + ), ] for lc in graph.loops ] @@ -269,23 +304,23 @@ def _log_reconstruction( pgo_centers: list[tuple[float, float, float]] = [] pgo_quats: list[tuple[float, float, float, float]] = [] for d in marker_dets: - raw_tf = Transform( - translation=d.data.center, - rotation=d.data.orientation, - frame_id="world", - child_frame_id=f"marker_{d.data.marker_id}", - ts=d.ts, + center = d.data.center + raw_pose = Pose( + position=Point(x=center.x, y=center.y, z=center.z), + orientation=d.data.orientation, + ) + corrected = pose_from_matrix( + transform_matrix(graph.correction_at(d.ts).transform) @ pose_matrix(raw_pose) ) - corrected = graph.correct(raw_tf) pgo_centers.append( - (corrected.translation.x, corrected.translation.y, corrected.translation.z) + (corrected.position.x, corrected.position.y, corrected.position.z) ) pgo_quats.append( ( - corrected.rotation.x, - corrected.rotation.y, - corrected.rotation.z, - corrected.rotation.w, + corrected.orientation.x, + corrected.orientation.y, + corrected.orientation.z, + corrected.orientation.w, ) ) _log_markers( @@ -328,7 +363,7 @@ def main( export: bool = typer.Option( False, "--export", - help="Export PGO map to ./.pc2.lcm in cwd (implies --pgo)", + help="Export PGO map to ./.pc2.cdr in cwd (implies --pgo)", ), full_pgo: bool = typer.Option( False, @@ -418,17 +453,19 @@ def main( ), ) -> None: """Rebuild a voxel map from a recorded SQLite dataset, write a .rrd, and open it in rerun.""" + from dimos_generated.sensor_msgs.msg import Image, PointCloud2 + from dimos_generated.std_msgs.msg import Header import rerun as rr from dimos.mapping.loop_closure.pgo import PGO from dimos.memory.cli.dataset import open_store, resolve_dataset from dimos.memory.transform import QualityWindow, SpeedLimit from dimos.memory.utils.progress import progress - from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo - from dimos.msgs.sensor_msgs.Image import Image - from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 + from dimos.msgs.camera_info import camera_info_from_yaml + from dimos.msgs.image import image_sharpness from dimos.perception.fiducial.marker_transformer import DetectMarkers - from dimos.robot.unitree.go2.connection import BASE_TO_OPTICAL, _camera_info_static + from dimos.robot.unitree.go2.camera_calibration import front_camera_calibration + from dimos.robot.unitree.go2.connection import BASE_TO_OPTICAL from dimos.visualization.rerun.init import rerun_init db_path = resolve_dataset(dataset) @@ -454,7 +491,7 @@ def main( tf_buf = StreamTF.from_store(store) # Streams are homogeneous: read the cloud frame from the first observation. first_obs = next(iter(lidar), None) - cloud_frame: str | None = first_obs.data.frame_id if first_obs is not None else None + cloud_frame: str | None = first_obs.data.header.frame_id if first_obs is not None else None world = frame if world is None and first_obs is not None and cloud_frame is not None: @@ -473,7 +510,7 @@ def main( # Registration: sensor-frame clouds get a per-frame tf lookup lifting them # into the world frame (frames with no tf answer are dropped); clouds # already stamped with the world frame accumulate verbatim (register=None). - register: Callable[[Observation[Any]], Transform | None] | None = None + register: Callable[[Observation[Any]], TransformStamped | None] | None = None if first_obs is not None and cloud_frame is not None and cloud_frame != world: # Fail fast when registration is impossible: probe the first cloud's # timestamp (unbounded tolerance — "possible at all", not "in range"). @@ -490,8 +527,10 @@ def main( print(f"registering clouds {world!r} ← {cloud_frame!r} via tf") buf = tf_buf - def _register(obs: Observation[Any]) -> Transform | None: - return buf.get(world, obs.data.frame_id, time_point=obs.ts, time_tolerance=tf_tolerance) + def _register(obs: Observation[Any]) -> TransformStamped | None: + return buf.get( + world, obs.data.header.frame_id, time_point=obs.ts, time_tolerance=tf_tolerance + ) register = _register elif cloud_frame is not None: @@ -504,11 +543,21 @@ def _position(obs: Observation[Any]) -> tuple[float, float, float] | None: tf = register(obs) if tf is None: return None - return (tf.translation.x, tf.translation.y, tf.translation.z) + return ( + tf.transform.translation.x, + tf.transform.translation.y, + tf.transform.translation.z, + ) pose = obs.pose # Reject placeholder poses: zero translation OR uninitialized rotation. # Same condition as pgo_keyframes so dedup and PGO see the same frames. - if pose is not None and not (pose.position.is_zero() or pose.orientation.is_zero()): + if ( + pose is not None + and any((pose.position.x, pose.position.y, pose.position.z)) + and any( + (pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w) + ) + ): return (pose.position.x, pose.position.y, pose.position.z) return None @@ -553,7 +602,11 @@ def _position(obs: Observation[Any]) -> tuple[float, float, float] | None: graph = lidar.tap(bar).transform(PGO()).last().data pgo_path = [ - (kf.optimized.translation.x, kf.optimized.translation.y, kf.optimized.translation.z) + ( + kf.optimized.transform.translation.x, + kf.optimized.transform.translation.y, + kf.optimized.transform.translation.z, + ) for kf in graph.keyframes ] @@ -619,7 +672,11 @@ def _position(obs: Observation[Any]) -> tuple[float, float, float] | None: ) print(f"re-posing color_image from {image_pose!r} + camera optical mount") color_image = pose_fill(color_image, src_pose, tolerance=0.1, mount=BASE_TO_OPTICAL) - cam_info = CameraInfo.from_yaml(str(camera_info)) if camera_info else _camera_info_static() + cam_info = ( + camera_info_from_yaml(camera_info, header=Header(frame_id="camera_optical")) + if camera_info + else front_camera_calibration() + ) xf = DetectMarkers( camera_info=cam_info, marker_length_m=marker_size, @@ -631,7 +688,7 @@ def _position(obs: Observation[Any]) -> tuple[float, float, float] | None: # than the limits. Defaults match replay_marker.py so positions agree. with progress(n_images, "detecting markers") as bar: pipeline: Stream[Image] = color_image.tap(bar).transform( - QualityWindow(lambda img: img.sharpness, window=marker_quality_window) + QualityWindow(image_sharpness, window=marker_quality_window) ) if marker_max_speed > 0: pipeline = pipeline.transform( @@ -677,14 +734,14 @@ def _position(obs: Observation[Any]) -> tuple[float, float, float] | None: subprocess.Popen(["rerun", str(out)]) if export and pgo_map is not None: - out_path = Path.cwd() / f"{db_path.stem}.pc2.lcm" + out_path = Path.cwd() / f"{db_path.stem}.pc2.cdr" print(f"exporting PGO twopass map to {out_path}...") - out_path.write_bytes(pgo_map.lcm_encode()) + out_path.write_bytes(pgo_map.encode()) print(f"wrote {out_path}") print() print("load back with:") - print(" from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2") - print(f' pcd = PointCloud2.lcm_decode(open("{out_path.name}", "rb").read())') + print(" from dimos_generated.sensor_msgs.msg import PointCloud2") + print(f' pcd = PointCloud2.decode(open("{out_path.name}", "rb").read())') if __name__ == "__main__": diff --git a/dimos/mapping/cli/pose_fill.py b/dimos/mapping/cli/pose_fill.py index 38465ff59fb..d7fb3f1cb98 100644 --- a/dimos/mapping/cli/pose_fill.py +++ b/dimos/mapping/cli/pose_fill.py @@ -37,10 +37,11 @@ # Heavy dimos imports are deferred (TYPE_CHECKING / inside functions) so that # `dimos map --help` stays fast. See test_cli_startup.py. if TYPE_CHECKING: + from dimos_generated.geometry_msgs.msg import TransformStamped + from dimos.memory.backend import Backend from dimos.memory.stream import Stream from dimos.memory.type.observation import Observation - from dimos.msgs.geometry_msgs.Transform import Transform def pose_fill( @@ -48,7 +49,7 @@ def pose_fill( pose_stream: Stream[Any], *, tolerance: float = 0.1, - mount: Transform | None = None, + mount: TransformStamped | None = None, ) -> Stream[Any]: """Re-pose each observation in *stream* from the nearest entry in *pose_stream*. @@ -64,10 +65,10 @@ def pose_fill( camera_optical`` so a base-frame odometry source yields an optical-frame image pose. """ + from dimos_generated.geometry_msgs.msg import Point, Pose, Quaternion + from dimos.memory.type.observation import _to_tuple - from dimos.msgs.geometry_msgs.Quaternion import Quaternion - from dimos.msgs.geometry_msgs.Transform import Transform - from dimos.msgs.geometry_msgs.Vector3 import Vector3 + from dimos.msgs.geometry import pose_from_matrix, pose_matrix, transform_matrix def _fill(pair_obs: Observation[Any]) -> Observation[Any]: primary, secondary = cast( @@ -76,8 +77,13 @@ def _fill(pair_obs: Observation[Any]) -> Observation[Any]: if mount is None: return primary.with_pose(secondary.data) x, y, z, qx, qy, qz, qw = cast("tuple[float, ...]", _to_tuple(secondary.data)) - world_base = Transform(translation=Vector3(x, y, z), rotation=Quaternion(qx, qy, qz, qw)) - return primary.with_pose(world_base + mount) + world_base = Pose( + position=Point(x=x, y=y, z=z), + orientation=Quaternion(x=qx, y=qy, z=qz, w=qw), + ) + return primary.with_pose( + pose_from_matrix(pose_matrix(world_base) @ transform_matrix(mount.transform)) + ) return stream.align(pose_stream, tolerance=tolerance).map(_fill) diff --git a/dimos/mapping/cli/replay.py b/dimos/mapping/cli/replay.py index 9f30e89cfd2..6a93394cbe8 100644 --- a/dimos/mapping/cli/replay.py +++ b/dimos/mapping/cli/replay.py @@ -40,8 +40,10 @@ from dimos.memory.utils.progress import progress if TYPE_CHECKING: + from dimos_generated.sensor_msgs.msg import PointCloud2 + from dimos.memory.stream import Stream - from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 + TIMELINE = "ts" @@ -64,6 +66,8 @@ def _log_clouds( """ import rerun as rr + from dimos.visualization.rerun.message_helpers import cloud_archetype + n = total if total is not None else stream.count() with progress(n, label) as bar: for obs in stream: @@ -71,7 +75,9 @@ def _log_clouds( rr.set_time(TIMELINE, timestamp=obs.ts) rr.log( entity, - obs.data.to_rerun(voxel_size=voxel, mode=point_mode, bottom_cutoff=bottom_cutoff), + cloud_archetype( + obs.data, voxel_size=voxel, mode=point_mode, bottom_cutoff=bottom_cutoff + ), ) @@ -174,22 +180,27 @@ def main( ), ) -> None: """Dump a recording to .rrd (lidar clouds + camera frames) and open it in rerun.""" + from dimos_generated.geometry_msgs.msg import PoseStamped + from dimos_generated.nav_msgs.msg import Odometry + from dimos_generated.sensor_msgs.msg import Image, PointCloud2 import rerun as rr from dimos.mapping.voxels.module import VoxelMapTransformer from dimos.memory.cli.dataset import open_store, resolve_dataset, stream_payload_types from dimos.memory.transform import throttle - from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped - from dimos.msgs.nav_msgs.Odometry import Odometry - from dimos.msgs.sensor_msgs.Image import Image - from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2, register_colormap_annotation - from dimos.robot.unitree.go2.connection import _camera_info_static + from dimos.robot.unitree.go2.camera_calibration import front_camera_calibration + from dimos.visualization.rerun.message_helpers import ( + camera_pinhole, + cloud_archetype, + image_archetype, + register_colormap_annotation, + ) src_path = resolve_dataset(dataset) store = open_store(src_path) if out is None: out = Path.cwd() / f"{src_path.stem}.rrd" - cam_info = _camera_info_static() + cam_info = front_camera_calibration() with store: # Resolve which streams to voxelize: all PointCloud2 streams, or the @@ -205,7 +216,7 @@ def main( # Static pinhole on the camera entity; per-frame Transform3D goes on the # same entity. Image is the child so it projects through the pinhole. - pinhole = cam_info.to_rerun() + pinhole = camera_pinhole(cam_info) assert not isinstance(pinhole, list) rr.log("world/camera", pinhole, static=True) @@ -268,8 +279,11 @@ def clipped(name: str, ptype: type[Any]) -> Stream[Any]: ).last() rr.log( f"world/{name}_map", - final.data.to_rerun( - voxel_size=voxel / 4, mode=point_mode, bottom_cutoff=bottom_cutoff + cloud_archetype( + final.data, + voxel_size=voxel / 4, + mode=point_mode, + bottom_cutoff=bottom_cutoff, ), static=True, ) @@ -339,7 +353,7 @@ def clipped(name: str, ptype: type[Any]) -> Stream[Any]: translation=[x, y, z], quaternion=rr.Quaternion(xyzw=[qx, qy, qz, qw]) ), ) - rr.log("world/camera/image", img_obs.data.to_rerun()) + rr.log("world/camera/image", image_archetype(img_obs.data)) print(f"wrote {out}") if no_gui: diff --git a/dimos/mapping/cli/replay_marker.py b/dimos/mapping/cli/replay_marker.py index d2e72a2bc47..3525ea7c69f 100644 --- a/dimos/mapping/cli/replay_marker.py +++ b/dimos/mapping/cli/replay_marker.py @@ -34,6 +34,7 @@ if TYPE_CHECKING: from dimos.memory.stream import Stream + TIMELINE = "ts" @@ -62,28 +63,29 @@ def main( ), ) -> None: """Dump an AprilTag detection replay to .rrd and open it in rerun.""" + from dimos_generated.sensor_msgs.msg import Image, PointCloud2 import rerun as rr from dimos.memory.cli.dataset import open_store, resolve_dataset from dimos.memory.transform import QualityWindow, SpeedLimit from dimos.memory.vis.color import Color - from dimos.msgs.sensor_msgs.Image import Image - from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 + from dimos.msgs.image import image_sharpness from dimos.perception.fiducial.marker_transformer import DetectMarkers - from dimos.robot.unitree.go2.connection import _camera_info_static + from dimos.robot.unitree.go2.camera_calibration import front_camera_calibration + from dimos.visualization.rerun.message_helpers import camera_pinhole, image_archetype db_path = resolve_dataset(dataset) store = open_store(db_path) if out is None: out = Path.cwd() / f"{db_path.stem}.rrd" - cam_info = _camera_info_static() + cam_info = front_camera_calibration() rr.init("dimos markers", recording_id=db_path.stem) rr.save(str(out)) # Static pinhole on the camera entity; per-frame Transform3D goes on the # same entity. Image is the child so it projects through the pinhole. - pinhole = cam_info.to_rerun() + pinhole = camera_pinhole(cam_info) assert not isinstance(pinhole, list) rr.log("world/camera", pinhole, static=True) @@ -116,7 +118,7 @@ def main( translation=[x, y, z], quaternion=rr.Quaternion(xyzw=[qx, qy, qz, qw]) ), ) - rr.log("world/camera/image", img_obs.data.to_rerun()) + rr.log("world/camera/image", image_archetype(img_obs.data)) # Clear any prior detection box at this ts; pass 3 will overwrite # this with the actual detection iff one fires for this frame. rr.log( @@ -129,7 +131,7 @@ def main( # Pass 3: marker detections (filtered same way as `dimos map`). xf = DetectMarkers(camera_info=cam_info, marker_length_m=marker_size) pipeline: Stream[Any] = color_image.transform( - QualityWindow(lambda img: img.sharpness, window=quality_window) + QualityWindow(image_sharpness, window=quality_window) ) if marker_max_speed > 0: pipeline = pipeline.transform( @@ -195,7 +197,7 @@ def main( smoothing_window=smoothing_window, ) pipeline_tracked: Stream[Any] = color_image.transform( - QualityWindow(lambda img: img.sharpness, window=quality_window) + QualityWindow(image_sharpness, window=quality_window) ) if marker_max_speed > 0: pipeline_tracked = pipeline_tracked.transform( diff --git a/dimos/mapping/cli/test_cli.py b/dimos/mapping/cli/test_cli.py index 53674915c6d..12fe2a9ae95 100644 --- a/dimos/mapping/cli/test_cli.py +++ b/dimos/mapping/cli/test_cli.py @@ -151,8 +151,9 @@ def test_pose_fill_snippet(dataset: str, tmp_path: Path) -> None: assert res.returncode == 0, res.stderr assert out.exists() + from dimos_generated.sensor_msgs.msg import PointCloud2 + from dimos.memory.store.sqlite import SqliteStore - from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 store = SqliteStore(path=str(out), must_exist=True) try: @@ -223,21 +224,3 @@ def test_global_snippet(dataset: str, tmp_path: Path) -> None: ) assert res.returncode == 0, res.stderr assert out.exists() and out.stat().st_size > 0 - - -def test_view_pc2(tmp_path: Path) -> None: - import numpy as np - - from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 - - pc2 = tmp_path / "map.pc2.lcm" - cloud = PointCloud2.from_numpy( - np.random.default_rng(0).random((256, 3)), frame_id="world", timestamp=0.0 - ) - pc2.write_bytes(cloud.lcm_encode()) - - out = tmp_path / "view.rrd" - res = _run("view", str(pc2), "--out", str(out)) - assert res.returncode == 0, res.stderr - assert "256 points" in res.stdout - assert out.exists() and out.stat().st_size > 0 diff --git a/dimos/mapping/cli/test_pgo_accumulate.py b/dimos/mapping/cli/test_pgo_accumulate.py new file mode 100644 index 00000000000..0c25f3e57bf --- /dev/null +++ b/dimos/mapping/cli/test_pgo_accumulate.py @@ -0,0 +1,48 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from dimos_generated.geometry_msgs.msg import Transform, TransformStamped, Vector3 +from dimos_generated.std_msgs.msg import Header +import numpy as np + +from dimos.mapping.cli.map import _accumulate +from dimos.mapping.loop_closure.pgo import PoseGraph +from dimos.memory.store.memory import MemoryStore +from dimos.msgs.pointcloud import pointcloud_from_xyz, pointcloud_xyz +from dimos.msgs.time import time_from_nanoseconds + + +def test_pgo_accumulate_applies_generated_correction_before_mapping(mocker): + graph = mocker.Mock(spec=PoseGraph) + graph.correction_at.return_value = TransformStamped( + header=Header(frame_id="world_corrected"), + child_frame_id="world", + transform=Transform(translation=Vector3(x=2.0)), + ) + mapper = mocker.Mock(side_effect=lambda observations: observations) + mocker.patch("dimos.mapping.voxels.module.VoxelMapTransformer", return_value=mapper) + stamp = time_from_nanoseconds(42_123_456_789) + cloud = pointcloud_from_xyz( + np.array([[1.0, 2.0, 3.0]]), header=Header(stamp=stamp, frame_id="world") + ) + with MemoryStore() as store: + stream = store.stream("cloud", type(cloud)) + stream.append(cloud, ts=42.0, pose=(0.0, 0.0, 0.0)) + result = _accumulate(stream, voxel=0.1, block_count=10, device="CPU:0", graph=graph) + assert result is not None + np.testing.assert_array_equal(pointcloud_xyz(result), [[3.0, 2.0, 3.0]]) + assert result.header.frame_id == "world_corrected" + assert result.header.stamp.sec == 42 + assert result.header.stamp.nanosec == 123_456_789 + graph.correction_at.assert_called_once_with(42.0) diff --git a/dimos/mapping/cli/test_pose_fill.py b/dimos/mapping/cli/test_pose_fill.py index 2953d9e13e6..4db1cab4c57 100644 --- a/dimos/mapping/cli/test_pose_fill.py +++ b/dimos/mapping/cli/test_pose_fill.py @@ -20,20 +20,29 @@ from __future__ import annotations +import math + +from dimos_generated.geometry_msgs.msg import ( + Point, + Pose, + Quaternion, + Transform, + TransformStamped, + Vector3, +) +import pytest + from dimos.mapping.cli.pose_fill import pose_fill from dimos.memory.store.memory import MemoryStore -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Transform import Transform -from dimos.msgs.geometry_msgs.Vector3 import Vector3 def test_pose_fill_attaches_nearest_pose() -> None: """Each target obs gets the nearest pose-source pose; payload is preserved.""" with MemoryStore() as store: target = store.stream("image", str) - poses = store.stream("odom", Transform) + poses = store.stream("odom", Pose) target.append("img0", ts=0.0) - poses.append(Transform(translation=Vector3(1.0, 0.0, 0.0)), ts=0.001) + poses.append(Pose(position=Point(x=1.0), orientation=Quaternion(w=1)), ts=0.001) out = pose_fill(target, poses, tolerance=0.05).to_list() @@ -47,11 +56,13 @@ def test_pose_fill_mount_composes_static_child_transform() -> None: """`mount` composes ``world_base + mount`` onto each attached pose.""" with MemoryStore() as store: target = store.stream("image", str) - poses = store.stream("odom", Transform) + poses = store.stream("odom", Pose) target.append("img0", ts=0.0) # Base pose 1m forward in x; mount offsets 1m in y (identity rotations). - poses.append(Transform(translation=Vector3(1.0, 0.0, 0.0)), ts=0.001) - mount = Transform(translation=Vector3(0.0, 1.0, 0.0), rotation=Quaternion()) + poses.append(Pose(position=Point(x=1.0), orientation=Quaternion(w=1)), ts=0.001) + mount = TransformStamped( + transform=Transform(translation=Vector3(y=1), rotation=Quaternion(w=1)) + ) out = pose_fill(target, poses, tolerance=0.05, mount=mount).to_list() @@ -59,3 +70,35 @@ def test_pose_fill_mount_composes_static_child_transform() -> None: assert out[0].pose_tuple is not None # Identity rotations → composed translation is the component-wise sum. assert out[0].pose_tuple[:3] == (1.0, 1.0, 0.0) + + +def test_pose_fill_rotates_mount_offset_in_base_axes() -> None: + """A base yaw rotates the local camera offset before the world translation.""" + with MemoryStore() as store: + target = store.stream("image", str) + poses = store.stream("odom", Pose) + target.append("camera frame", ts=5.0) + poses.append( + Pose( + position=Point(x=2), + orientation=Quaternion(z=math.sqrt(0.5), w=math.sqrt(0.5)), + ), + ts=5.001, + ) + mount = TransformStamped( + transform=Transform(translation=Vector3(x=1), rotation=Quaternion(w=1)) + ) + result = pose_fill(target, poses, tolerance=0.05, mount=mount).to_list() + assert len(result) == 1 + assert result[0].data == "camera frame" + assert result[0].ts == 5.0 + assert result[0].pose_tuple == pytest.approx((2, 1, 0, 0, 0, math.sqrt(0.5), math.sqrt(0.5))) + + +def test_pose_fill_drops_unmatched_frame_without_loading_payload() -> None: + with MemoryStore() as store: + target = store.stream("image", str) + poses = store.stream("odom", Pose) + target.append("unmatched", ts=1.0) + poses.append(Pose(position=Point(x=1), orientation=Quaternion(w=1)), ts=3.0) + assert pose_fill(target, poses, tolerance=0.05).to_list() == [] diff --git a/dimos/mapping/cli/test_view.py b/dimos/mapping/cli/test_view.py new file mode 100644 index 00000000000..d7ddb137033 --- /dev/null +++ b/dimos/mapping/cli/test_view.py @@ -0,0 +1,40 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +# Copyright 2026 Dimensional Inc. +# SPDX-License-Identifier: Apache-2.0 + +from pathlib import Path + +from dimos_generated.std_msgs.msg import Header +import numpy as np +import rerun as rr + +from dimos.mapping.cli.view import main +from dimos.msgs.pointcloud import pointcloud_from_xyz + + +def test_view_cdr_cloud_writes_offline_recording(tmp_path: Path, capsys): + pc2 = tmp_path / "map.pc2.cdr" + cloud = pointcloud_from_xyz( + np.random.default_rng(0).random((256, 3)), header=Header(frame_id="world") + ) + pc2.write_bytes(cloud.encode()) + out = tmp_path / "view.rrd" + try: + main(pc2, voxel=0.05, bottom_cutoff=None, out=out) + finally: + rr.disconnect() + assert "256 points in 'world'" in capsys.readouterr().out + assert out.exists() and out.stat().st_size > 0 diff --git a/dimos/mapping/cli/view.py b/dimos/mapping/cli/view.py index a76eccc173d..60c330ad25d 100644 --- a/dimos/mapping/cli/view.py +++ b/dimos/mapping/cli/view.py @@ -12,7 +12,7 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""`dimos map view`: open a `.pc2.lcm` cloud - `dimos map global --export` - in rerun.""" +"""`dimos map view`: open a `.pc2.cdr` cloud - `dimos map global --export` - in rerun.""" from __future__ import annotations @@ -22,7 +22,7 @@ def main( - path: Path = typer.Argument(..., help="Point cloud to view (.pc2.lcm)"), + path: Path = typer.Argument(..., help="Point cloud to view (.pc2.cdr)"), voxel: float = typer.Option(0.05, "--voxel", help="Rendered point size (m)"), bottom_cutoff: float | None = typer.Option( None, @@ -33,14 +33,15 @@ def main( None, "--out", help="Write a .rrd instead of opening the viewer" ), ) -> None: - """Load an LCM-encoded PointCloud2 and display it in rerun.""" + """Load a CDR-encoded PointCloud2 and display it in rerun.""" + from dimos_generated.sensor_msgs.msg import PointCloud2 import rerun as rr - from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 from dimos.visualization.rerun.init import rerun_init + from dimos.visualization.rerun.message_helpers import cloud_archetype - cloud = PointCloud2.lcm_decode(path.read_bytes()) - print(f"{path}: {len(cloud.pointcloud.points)} points in {cloud.frame_id!r}") + cloud = PointCloud2.decode(path.read_bytes()) + print(f"{path}: {cloud.width * cloud.height} points in {cloud.header.frame_id!r}") rerun_init("dimos map view") if out is not None: @@ -49,7 +50,7 @@ def main( rr.spawn() rr.log( f"world/{path.name.split('.')[0]}/pointcloud", - cloud.to_rerun(voxel_size=voxel / 2, bottom_cutoff=bottom_cutoff), + cloud_archetype(cloud, ui_radius=voxel / 2, bottom_cutoff=bottom_cutoff), static=True, ) if out is not None: diff --git a/dimos/mapping/costmapper.py b/dimos/mapping/costmapper.py index b7fa2b6878f..98a39d46694 100644 --- a/dimos/mapping/costmapper.py +++ b/dimos/mapping/costmapper.py @@ -15,6 +15,8 @@ from dataclasses import asdict import time +from dimos_generated.nav_msgs.msg import OccupancyGrid +from dimos_generated.sensor_msgs.msg import PointCloud2 import numpy as np from pydantic import Field from reactivex import combine_latest, operators as ops @@ -27,8 +29,8 @@ HeightCostConfig, OccupancyConfig, ) -from dimos.msgs.nav_msgs.OccupancyGrid import OccupancyGrid -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 +from dimos.msgs.geometry import pose_matrix +from dimos.msgs.occupancy import occupancy_view from dimos.utils.logging_config import setup_logger logger = setup_logger() @@ -92,20 +94,24 @@ def _calculate_costmap(self, msg: PointCloud2) -> OccupancyGrid: def _apply_initial_safe_radius(self, grid: OccupancyGrid) -> None: radius_meters = self.config.initial_safe_radius_meters - if radius_meters <= 0 or grid.grid.size == 0: + if radius_meters <= 0: + return + cells = occupancy_view(grid).copy() + if cells.size == 0: return - resolution = grid.resolution - origin_x = grid.origin.position.x - origin_y = grid.origin.position.y - - rows, columns = np.ogrid[: grid.grid.shape[0], : grid.grid.shape[1]] - cell_world_x = columns * resolution + origin_x - cell_world_y = rows * resolution + origin_y + resolution = grid.info.resolution + matrix = pose_matrix(grid.info.origin) + rows, columns = np.ogrid[: cells.shape[0], : cells.shape[1]] + local_x = columns * resolution + local_y = rows * resolution + cell_world_x = matrix[0, 0] * local_x + matrix[0, 1] * local_y + matrix[0, 3] + cell_world_y = matrix[1, 0] * local_x + matrix[1, 1] * local_y + matrix[1, 3] distance_squared_meters = cell_world_x**2 + cell_world_y**2 # Half-cell tolerance: a cell counts as inside if any part of it overlaps # the disc. Avoids floating-point boundary flakiness from radius/resolution. effective_radius_meters = radius_meters + resolution * 0.5 safe_mask = distance_squared_meters <= effective_radius_meters**2 - grid.grid[safe_mask] = 0 + cells[safe_mask] = 0 + grid.data = cells.ravel() diff --git a/dimos/mapping/dim_slam/dim_slam.py b/dimos/mapping/dim_slam/dim_slam.py index f17926d7ad1..0cd97571b19 100644 --- a/dimos/mapping/dim_slam/dim_slam.py +++ b/dimos/mapping/dim_slam/dim_slam.py @@ -19,16 +19,13 @@ from typing import Literal +from dimos_generated.nav_msgs.msg import Odometry +from dimos_generated.sensor_msgs.msg import CameraInfo, Image, Imu, PointCloud2 +from dimos_generated.tf2_msgs.msg import TFMessage from pydantic import BaseModel, Field, model_validator from dimos.core.native_module import NativeModule, NativeModuleConfig from dimos.core.stream import IO, In, Out -from dimos.msgs.nav_msgs.Odometry import Odometry -from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo -from dimos.msgs.sensor_msgs.Image import Image -from dimos.msgs.sensor_msgs.Imu import Imu -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 -from dimos.msgs.tf2_msgs.TFMessage import TFMessage from dimos.utils.logging_config import setup_logger from dimos.utils.nvidia_env import driver_env, sdk_variant diff --git a/dimos/mapping/dim_slam/rust/Cargo.lock b/dimos/mapping/dim_slam/rust/Cargo.lock index 16109040106..1592f5d3a7e 100644 --- a/dimos/mapping/dim_slam/rust/Cargo.lock +++ b/dimos/mapping/dim_slam/rust/Cargo.lock @@ -220,6 +220,20 @@ name = "bytemuck" version = "1.25.2" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "95832e849adfb21180ccb6826a99da14e5d266ae5c2e668e1602cf234f153797" +dependencies = [ + "bytemuck_derive", +] + +[[package]] +name = "bytemuck_derive" +version = "1.12.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "6a1f896587b6f2c069c73d2f0913e2d590c3990285cd2f0b6aa02b786b4c679c" +dependencies = [ + "proc-macro2", + "quote", + "syn 3.0.4", +] [[package]] name = "byteorder" @@ -556,8 +570,8 @@ version = "0.1.0" dependencies = [ "cu_vslam_rs", "dim_slam", + "dimos-generated-messages", "dimos-module", - "lcm-msgs", "serde", "tokio", "tracing", @@ -576,9 +590,17 @@ dependencies = [ ] [[package]] -name = "dimos-lcm" +name = "dimos-generated-messages" +version = "0.1.0" +dependencies = [ + "re_cdr", + "serde", + "serde-big-array", +] + +[[package]] +name = "dimos-lcm-transport" version = "0.1.0" -source = "git+https://github.com/dimensionalOS/dimos-lcm.git?branch=rust-codegen#04d78e8622500244123ba9cefa4c51b4cb454549" dependencies = [ "byteorder", "socket2 0.5.10", @@ -589,9 +611,9 @@ dependencies = [ name = "dimos-module" version = "0.1.0" dependencies = [ - "dimos-lcm", + "dimos-generated-messages", + "dimos-lcm-transport", "dimos-module-macros", - "lcm-msgs", "nalgebra", "rayon", "serde", @@ -1385,14 +1407,6 @@ version = "1.5.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "bbd2bcb4c963f2ddae06a2efc7e9f3591312473c50c6685e1f298068316e66fe" -[[package]] -name = "lcm-msgs" -version = "0.1.0" -source = "git+https://github.com/dimensionalOS/dimos-lcm.git?branch=rust-codegen#04d78e8622500244123ba9cefa4c51b4cb454549" -dependencies = [ - "byteorder", -] - [[package]] name = "libc" version = "0.2.189" @@ -2167,6 +2181,18 @@ dependencies = [ "yasna", ] +[[package]] +name = "re_cdr" +version = "0.1.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "bba10c21433b597481f366b978cdf2617e56785b38cb0dcc3833c6b8ad7e061a" +dependencies = [ + "bytemuck", + "byteorder", + "serde", + "thiserror", +] + [[package]] name = "redox_syscall" version = "0.5.18" @@ -2523,6 +2549,15 @@ dependencies = [ "serde_derive", ] +[[package]] +name = "serde-big-array" +version = "0.5.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "11fc7cc2c76d73e0f27ee52abbd64eec84d46f370c88371120433196934e4b7f" +dependencies = [ + "serde", +] + [[package]] name = "serde_core" version = "1.0.229" diff --git a/dimos/mapping/dim_slam/rust/Cargo.toml b/dimos/mapping/dim_slam/rust/Cargo.toml index 5af8d516d21..2807007898c 100644 --- a/dimos/mapping/dim_slam/rust/Cargo.toml +++ b/dimos/mapping/dim_slam/rust/Cargo.toml @@ -16,7 +16,7 @@ dim_slam = { git = "https://github.com/dimensionalOS/dimSLAM", tag = "v0.4.1" } # and build.rs needs it for the rpath. cu_vslam_rs = "0.2.0" dimos-module = { path = "../../../../native/rust/dimos-module" } -lcm-msgs = { git = "https://github.com/dimensionalOS/dimos-lcm.git", branch = "rust-codegen" } +dimos-generated-messages = { path = "../../../message_codegen" } tokio = { version = "1", features = ["rt-multi-thread", "macros"] } tracing = "0.1" # native_config expands to ::serde and ::validator derives, so both must be direct deps. diff --git a/dimos/mapping/dim_slam/rust/flake.nix b/dimos/mapping/dim_slam/rust/flake.nix index 7b44713e85d..fe2288ee502 100644 --- a/dimos/mapping/dim_slam/rust/flake.nix +++ b/dimos/mapping/dim_slam/rust/flake.nix @@ -39,6 +39,9 @@ mkdir -p $out/native/rust cp -r ${dimos-repo}/native/rust/dimos-module $out/native/rust/dimos-module cp -r ${dimos-repo}/native/rust/dimos-module-macros $out/native/rust/dimos-module-macros + cp -r ${dimos-repo}/native/rust/dimos-lcm-transport $out/native/rust/dimos-lcm-transport + mkdir -p $out/dimos/message_codegen + cp -r ${dimos-repo}/dimos/message_codegen/. $out/dimos/message_codegen/ ''; generatedCargoNix = crate2nix.tools.${system}.generatedCargoNix { @@ -52,6 +55,7 @@ inherit pkgs; buildRustCrateForPkgs = cratePkgs: cratePkgs.buildRustCrate.override { defaultCrateOverrides = cratePkgs.defaultCrateOverrides // { + dimos-generated-messages = _: { nativeBuildInputs = [cratePkgs.python3]; }; # cu_vslam_rs's build.rs compiles its shim against this SDK. cu_vslam_rs = _: { CUVSLAM_SDK_DIR = sdkPackage; }; # buildRustCrate names DEP_ vars after the crate, cargo after the diff --git a/dimos/mapping/dim_slam/rust/src/main.rs b/dimos/mapping/dim_slam/rust/src/main.rs index 23b8318f935..aa938075c39 100644 --- a/dimos/mapping/dim_slam/rust/src/main.rs +++ b/dimos/mapping/dim_slam/rust/src/main.rs @@ -17,9 +17,10 @@ mod msg_convert; use dim_slam::{ CameraConfig, DimSlamCore, DimSlamCoreConfig, ImuConfig, InitialStds, SourceConfig, }; +use dimos_generated_messages::nav_msgs::msg::Odometry; +use dimos_generated_messages::sensor_msgs::msg::{CameraInfo, Image, Imu, PointCloud2}; +use dimos_module::cdr; use dimos_module::{native_config, run_with_transport, Input, Module, Output, Tf}; -use lcm_msgs::nav_msgs::Odometry; -use lcm_msgs::sensor_msgs::{CameraInfo, Image, Imu, PointCloud2}; use msg_convert::{ tf_lookup, to_camera_model, to_estimate, to_image_frame, to_imu_sample, to_odometry_msg, to_point_cloud2, to_transform, @@ -114,25 +115,25 @@ struct DimSlamConfig { #[derive(Module)] #[module(setup = init, teardown = report)] struct DimSlam { - #[input(decode = CameraInfo::decode)] + #[input(decode = cdr::decode)] camera_info: Input, - #[input(decode = Image::decode)] + #[input(decode = cdr::decode)] image: Input, /// rgbd only; unconnected otherwise. - #[input(decode = Image::decode)] + #[input(decode = cdr::decode)] depth_image: Input, /// Only needed when depth has to be reprojected onto the rig camera. - #[input(decode = CameraInfo::decode)] + #[input(decode = cdr::decode)] depth_camera_info: Input, - #[input(decode = Imu::decode)] + #[input(decode = cdr::decode)] imu: Input, /// External odometry sources, told apart by the transform each message carries. - #[input(decode = Odometry::decode)] + #[input(decode = cdr::decode)] odom_sources: Input, - #[output(encode = Odometry::encode)] + #[output(encode = cdr::encode)] odometry: Output, /// rgbd only: range-gated depth points, in the depth frame. - #[output(encode = PointCloud2::encode)] + #[output(encode = cdr::encode)] depth_cloud: Output, #[tf] tf: Tf, @@ -199,26 +200,47 @@ impl DimSlam { } async fn handle_camera_info(&mut self, info: CameraInfo) { + let info = match to_camera_model(info) { + Ok(value) => value, + Err(error) => { + tracing::warn!(%error, "invalid info message"); + return; + } + }; let Self { slam, tf, .. } = self; slam.as_mut() .expect("setup ran") - .handle_camera_info(to_camera_model(info), &tf_lookup(tf)); + .handle_camera_info(info, &tf_lookup(tf)); } async fn handle_image(&mut self, img: Image) { + let img = match to_image_frame(img) { + Ok(value) => value, + Err(error) => { + tracing::warn!(%error, "invalid img message"); + return; + } + }; let Self { slam, tf, .. } = self; slam.as_mut() .expect("setup ran") - .handle_image(to_image_frame(img), &tf_lookup(tf)); + .handle_image(img, &tf_lookup(tf)); self.publish().await; } async fn handle_depth_image(&mut self, img: Image) { + let img = match to_image_frame(img) { + Ok(value) => value, + Err(error) => { + tracing::warn!(%error, "invalid img message"); + return; + } + }; let Self { slam, tf, .. } = self; let cloud = slam .as_mut() .expect("setup ran") - .handle_depth_image(to_image_frame(img), &tf_lookup(tf)); + .handle_depth_image(img, &tf_lookup(tf)); if let Some(cloud) = cloud { self.depth_cloud .publish(&to_point_cloud2(&cloud)) @@ -229,10 +251,17 @@ impl DimSlam { } async fn handle_depth_camera_info(&mut self, info: CameraInfo) { + let info = match to_camera_model(info) { + Ok(value) => value, + Err(error) => { + tracing::warn!(%error, "invalid info message"); + return; + } + }; self.slam .as_mut() .expect("setup ran") - .handle_depth_camera_info(to_camera_model(info)); + .handle_depth_camera_info(info); } async fn handle_imu(&mut self, msg: Imu) { diff --git a/dimos/mapping/dim_slam/rust/src/msg_convert.rs b/dimos/mapping/dim_slam/rust/src/msg_convert.rs index e0e7d8289c0..2f4b15883fc 100644 --- a/dimos/mapping/dim_slam/rust/src/msg_convert.rs +++ b/dimos/mapping/dim_slam/rust/src/msg_convert.rs @@ -12,33 +12,33 @@ // See the License for the specific language governing permissions and // limitations under the License. // -// LCM messages in and out of the dim_slam library's plain types. +// Generated ROS2 messages in and out of the dim_slam library's plain types. use dim_slam::nalgebra::{Isometry3, Matrix6, Quaternion, Translation3, UnitQuaternion, Vector3}; use dim_slam::{CameraModel, ImageFrame, ImuSample, OdometryEstimate, PointCloud, Twist}; +use dimos_generated_messages::builtin_interfaces::msg::Time; +use dimos_generated_messages::geometry_msgs::msg as geometry_msgs; +use dimos_generated_messages::nav_msgs::msg::Odometry; +use dimos_generated_messages::sensor_msgs::msg::{CameraInfo, Image, Imu, PointCloud2, PointField}; +use dimos_generated_messages::std_msgs::msg::Header; use dimos_module::{Tf, Transform}; -use lcm_msgs::geometry_msgs; -use lcm_msgs::nav_msgs::Odometry; -use lcm_msgs::sensor_msgs::{CameraInfo, Image, Imu, PointCloud2, PointField}; -use lcm_msgs::std_msgs::{Header, Time}; const NS_PER_SEC: i64 = 1_000_000_000; -const BYTES_PER_POINT: i32 = 12; +const BYTES_PER_POINT: u32 = 12; fn stamp_to_ns(header: &Header) -> i64 { - header.stamp.sec as i64 * NS_PER_SEC + header.stamp.nsec as i64 + header.stamp.sec as i64 * NS_PER_SEC + header.stamp.nanosec as i64 } fn to_stamp(timestamp_ns: i64) -> Time { Time { - sec: (timestamp_ns / NS_PER_SEC) as i32, - nsec: (timestamp_ns % NS_PER_SEC) as i32, + sec: timestamp_ns.div_euclid(NS_PER_SEC) as i32, + nanosec: timestamp_ns.rem_euclid(NS_PER_SEC) as u32, } } fn to_header(timestamp_ns: i64, frame_id: &str) -> Header { Header { - seq: 0, stamp: to_stamp(timestamp_ns), frame_id: frame_id.to_string(), } @@ -74,27 +74,69 @@ pub fn tf_lookup(tf: &Tf) -> impl Fn(&str, &str) -> Option> + '_ } } -pub fn to_image_frame(img: Image) -> ImageFrame { - ImageFrame { +pub fn to_image_frame(mut img: Image) -> Result { + let (scalar_bytes, channels) = match img.encoding.as_str() { + "mono8" => (1usize, 1), + "rgb8" | "bgr8" => (1, 3), + "mono16" | "16UC1" => (2, 1), + "32FC1" => (4, 1), + _ => return Err(format!("unsupported image encoding {}", img.encoding)), + }; + let row_bytes = img.width as usize * scalar_bytes * channels; + let step = img.step as usize; + if step < row_bytes || img.data.len() != step * img.height as usize { + return Err("image dimensions/stride do not match data".into()); + } + if img.is_bigendian != 0 && scalar_bytes > 1 && step > 0 { + for row in img.data.chunks_exact_mut(step) { + for value in row[..row_bytes].chunks_exact_mut(scalar_bytes) { + value.reverse(); + } + } + } + if img.encoding == "bgr8" && step > 0 { + for row in img.data.chunks_exact_mut(step) { + for pixel in row[..row_bytes].as_chunks_mut::<3>().0 { + pixel.swap(0, 2); + } + } + img.encoding = "rgb8".into(); + } + Ok(ImageFrame { timestamp_ns: stamp_to_ns(&img.header), frame_id: img.header.frame_id, - width: img.width, - height: img.height, + width: img + .width + .try_into() + .map_err(|_| "image width exceeds backend range")?, + height: img + .height + .try_into() + .map_err(|_| "image height exceeds backend range")?, encoding: img.encoding, - step: img.step, + step: img + .step + .try_into() + .map_err(|_| "image step exceeds backend range")?, data: img.data, - } + }) } -pub fn to_camera_model(info: CameraInfo) -> CameraModel { - CameraModel { +pub fn to_camera_model(info: CameraInfo) -> Result { + Ok(CameraModel { timestamp_ns: stamp_to_ns(&info.header), frame_id: info.header.frame_id, - width: info.width, - height: info.height, - distortion: info.D, - intrinsics: info.K, - } + width: info + .width + .try_into() + .map_err(|_| "camera width exceeds backend range")?, + height: info + .height + .try_into() + .map_err(|_| "camera height exceeds backend range")?, + distortion: info.d, + intrinsics: info.k, + }) } pub fn to_imu_sample(msg: &Imu) -> ImuSample { @@ -172,17 +214,17 @@ pub fn to_transform(estimate: &OdometryEstimate) -> Transform { ) } -fn xyz_field(name: &str, offset: i32) -> PointField { +fn xyz_field(name: &str, offset: u32) -> PointField { PointField { name: name.to_string(), offset, - datatype: PointField::FLOAT32 as u8, + datatype: PointField::FLOAT32, count: 1, } } pub fn to_point_cloud2(cloud: &PointCloud) -> PointCloud2 { - let width = cloud.points.len() as i32; + let width = cloud.points.len() as u32; let mut data = Vec::with_capacity(cloud.points.len() * BYTES_PER_POINT as usize); for point in &cloud.points { for coordinate in point { @@ -206,6 +248,67 @@ pub fn to_point_cloud2(cloud: &PointCloud) -> PointCloud2 { mod tests { use super::*; + #[test] + fn image_adapter_normalizes_depth_byte_order_and_preserves_row_padding() { + let image = Image { + width: 1, + height: 2, + step: 4, + encoding: "16UC1".into(), + is_bigendian: 1, + data: vec![0x12, 0x34, 99, 98, 0x56, 0x78, 97, 96], + ..Default::default() + }; + let frame = to_image_frame(image).unwrap(); + assert_eq!(frame.data, [0x34, 0x12, 99, 98, 0x78, 0x56, 97, 96]); + assert_eq!((frame.width, frame.height, frame.step), (1, 2, 4)); + } + + #[test] + fn image_adapter_normalizes_bgr_for_the_backend() { + let frame = to_image_frame(Image { + width: 1, + height: 1, + step: 4, + encoding: "bgr8".into(), + data: vec![1, 2, 3, 99], + ..Default::default() + }) + .unwrap(); + assert_eq!(frame.encoding, "rgb8"); + assert_eq!(frame.data, [3, 2, 1, 99]); + } + + #[test] + fn image_adapter_rejects_invalid_layout_and_backend_dimension_overflow() { + assert!(to_image_frame(Image { + width: 1, + height: 1, + step: 1, + encoding: "rgb8".into(), + data: vec![1], + ..Default::default() + }) + .is_err()); + assert!(to_camera_model(CameraInfo { + width: u32::MAX, + ..Default::default() + }) + .is_err()); + assert!(to_image_frame(Image { + encoding: "16UC1".into(), + is_bigendian: 1, + ..Default::default() + }) + .is_ok()); + } + + #[test] + fn negative_source_stamp_is_normalized() { + let stamp = to_stamp(-250000000); + assert_eq!((stamp.sec, stamp.nanosec), (-1, 750000000)); + } + #[test] fn stamp_round_trips_through_ns() { let header = Header { diff --git a/dimos/mapping/loop_closure/eval.py b/dimos/mapping/loop_closure/eval.py index 571f94635e1..9242a97530e 100644 --- a/dimos/mapping/loop_closure/eval.py +++ b/dimos/mapping/loop_closure/eval.py @@ -31,6 +31,8 @@ import time from typing import Any +from dimos_generated.sensor_msgs.msg import Image +import numpy as np import typer from dimos.mapping.loop_closure.pgo import PGO @@ -38,10 +40,10 @@ from dimos.memory.stream import Stream from dimos.memory.transform import QualityWindow, SpeedLimit from dimos.memory.type.observation import Observation -from dimos.msgs.geometry_msgs.Transform import Transform -from dimos.msgs.sensor_msgs.Image import Image +from dimos.msgs.geometry import transform_matrix +from dimos.msgs.image import image_sharpness from dimos.perception.fiducial.marker_transformer import DetectMarkers -from dimos.robot.unitree.go2.connection import _camera_info_static +from dimos.robot.unitree.go2.camera_calibration import front_camera_calibration from dimos.utils.data import get_data DEFAULT_DATASETS = [f"hk_village{i}" for i in range(1, 7)] @@ -68,7 +70,7 @@ def _eval_recording( ) -> tuple[float, float]: """Returns (pgo_time_s, spread_m) for one recording.""" db_path = get_data(f"{name}.db") - cam_info = _camera_info_static() + cam_info = front_camera_calibration() store = SqliteStore(path=str(db_path)) with store: @@ -86,7 +88,7 @@ def _eval_recording( smoothing_window=marker_smoothing, ) pipeline: Stream[Image] = color_image.transform( - QualityWindow(lambda img: img.sharpness, window=marker_quality_window) + QualityWindow(image_sharpness, window=marker_quality_window) ) if marker_max_speed > 0: pipeline = pipeline.transform( @@ -106,16 +108,13 @@ def _eval_recording( # PGO-correct each track's pose; group by marker_id. by_marker: dict[int, list[tuple[float, float, float]]] = {} for d in tracks: - raw_tf = Transform( - translation=d.data.center, - rotation=d.data.orientation, - frame_id="world", - child_frame_id=f"marker_{d.data.marker_id}", - ts=d.ts, + center = d.data.center + corrected = transform_matrix(graph.correction_at(d.ts).transform) @ np.array( + [center.x, center.y, center.z, 1.0] + ) + by_marker.setdefault(d.data.marker_id, []).append( + (float(corrected[0]), float(corrected[1]), float(corrected[2])) ) - corrected = graph.correct(raw_tf) - t = corrected.translation - by_marker.setdefault(d.data.marker_id, []).append((t.x, t.y, t.z)) spread = sum(_pairwise_sum(v) for v in by_marker.values()) return pgo_time, spread diff --git a/dimos/mapping/loop_closure/pgo.py b/dimos/mapping/loop_closure/pgo.py index 273f0ef6b9e..3a5137cb098 100644 --- a/dimos/mapping/loop_closure/pgo.py +++ b/dimos/mapping/loop_closure/pgo.py @@ -48,15 +48,31 @@ from dataclasses import dataclass from typing import TYPE_CHECKING, Any, Literal, TypedDict, TypeVar, cast +from dimos_generated.geometry_msgs.msg import PoseStamped, Transform, TransformStamped, Vector3 +from dimos_generated.sensor_msgs.msg import PointCloud2 +from dimos_generated.std_msgs.msg import Header import numpy as np from scipy.spatial.transform import Rotation, Slerp from dimos.memory.transform import Transformer from dimos.memory.type.observation import Observation -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Transform import Transform -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 +from dimos.msgs.geometry import ( + compose_transforms, + inverse_transform, + pose_from_matrix, + pose_matrix, + quaternion_from_matrix, + transform_from_matrix, + transform_matrix, +) +from dimos.msgs.pointcloud import ( + concatenate_clouds, + pointcloud_from_xyz, + pointcloud_to_open3d, + transform_cloud, + voxel_downsample_cloud, +) +from dimos.msgs.time import time_from_seconds, to_seconds from dimos.protocol.service.spec import BaseConfig from dimos.utils.logging_config import setup_logger @@ -136,8 +152,8 @@ class Keyframe: """ ts: float - local: Transform - optimized: Transform + local: TransformStamped + optimized: TransformStamped @dataclass(frozen=True) @@ -150,8 +166,8 @@ class LoopClosure: tighter match. """ - source: Transform - target: Transform + source: TransformStamped + target: TransformStamped score: float @@ -167,16 +183,17 @@ class PoseGraph(Transformer[Any, Any]): keyframes: tuple[Keyframe, ...] = () loops: tuple[LoopClosure, ...] = () - def correct(self, pose: Transform) -> Transform: - """Map a raw-world pose into the drift-corrected frame at ``pose.ts``.""" - # Transform.__add__ composes: (T_corr + T_raw) applies T_corr after T_raw. - return self.correction_at(pose.ts) + pose + def correct(self, pose: TransformStamped) -> TransformStamped: + """Map a raw-world stamped transform into the drift-corrected frame.""" + result = compose_transforms(self.correction_at(to_seconds(pose.header.stamp)), pose) + result.header.stamp = pose.header.stamp + return result - def correction_at(self, ts: float) -> Transform: + def correction_at(self, ts: float) -> TransformStamped: """The raw ``world_corrected <- world_raw`` transform at ``ts``. Useful when applying the correction to non-pose data (e.g. point - clouds): ``pointcloud.transform(graph.correction_at(obs.ts))``. + clouds): ``transform_cloud(pointcloud, graph.correction_at(obs.ts))``. """ return self._interp()(ts) @@ -187,23 +204,39 @@ def __call__(self, upstream: Iterator[Observation[Any]]) -> Iterator[Observation if ps is None: yield obs continue - raw_tf = Transform.from_pose(FRAME_BODY, ps) - yield obs.derive(data=obs.data, pose=self.correct(raw_tf)) + correction = self.correction_at(obs.ts) + corrected = PoseStamped( + header=Header(stamp=ps.header.stamp, frame_id=FRAME_WORLD_CORRECTED), + pose=pose_from_matrix( + transform_matrix(correction.transform) @ pose_matrix(ps.pose) + ), + ) + yield obs.derive(data=obs.data, pose=corrected) - def _interp(self) -> Callable[[float], Transform]: + def _interp(self) -> Callable[[float], TransformStamped]: """Lazy slerp/lerp drift-correction lookup keyed by ts.""" cached = self.__dict__.get("_interp_cache") if cached is not None: - return cast("Callable[[float], Transform]", cached) + return cast("Callable[[float], TransformStamped]", cached) if not self.keyframes: raise ValueError("PoseGraph has no keyframes") # Per-keyframe drift: world_corrected <- body <- world_raw. ts_list = [kf.ts for kf in self.keyframes] - drifts = [(kf.optimized + kf.local.inverse()) for kf in self.keyframes] - quat_list = [tf.rotation.to_numpy() for tf in drifts] - t_list = [tf.translation.to_numpy() for tf in drifts] + drifts = [ + compose_transforms(kf.optimized, inverse_transform(kf.local)) for kf in self.keyframes + ] + quat_list = [ + [ + tf.transform.rotation.x, + tf.transform.rotation.y, + tf.transform.rotation.z, + tf.transform.rotation.w, + ] + for tf in drifts + ] + t_list = [transform_matrix(tf.transform)[:3, 3] for tf in drifts] # Slerp needs ≥2 keyframes; pad a len==1 list with a near-duplicate. if len(ts_list) == 1: @@ -215,7 +248,7 @@ def _interp(self) -> Callable[[float], Transform]: t_stack = np.stack(t_list) slerp = Slerp(ts_arr, Rotation.from_quat(np.stack(quat_list))) - def interp(ts: float) -> Transform: + def interp(ts: float) -> TransformStamped: ts_clip = float(np.clip(ts, ts_arr[0], ts_arr[-1])) R = slerp([ts_clip])[0].as_matrix() idx = int(np.searchsorted(ts_arr, ts_clip)) @@ -227,12 +260,13 @@ def interp(ts: float) -> Transform: t_lo, t_hi = ts_arr[idx - 1], ts_arr[idx] alpha = (ts_clip - t_lo) / (t_hi - t_lo) if t_hi > t_lo else 0.0 t = (1 - alpha) * t_stack[idx - 1] + alpha * t_stack[idx] - return Transform( - translation=Vector3(t), - rotation=Quaternion.from_rotation_matrix(R), - frame_id=FRAME_WORLD_CORRECTED, + return TransformStamped( + header=Header(frame_id=FRAME_WORLD_CORRECTED, stamp=time_from_seconds(float(ts))), child_frame_id=FRAME_WORLD_RAW, - ts=float(ts), + transform=Transform( + translation=Vector3(x=t[0], y=t[1], z=t[2]), + rotation=quaternion_from_matrix(R), + ), ) # frozen=True blocks plain attribute writes; use object.__setattr__. @@ -273,7 +307,12 @@ def __call__( continue # Placeholder filter: zero translation OR uninitialized (all-zero) # quaternion. Identity rotation (qw=1) is valid and stays. - if pose.position.is_zero() or pose.orientation.is_zero(): + if np.allclose([pose.position.x, pose.position.y, pose.position.z], 0.0) or ( + pose.orientation.x == 0.0 + and pose.orientation.y == 0.0 + and pose.orientation.z == 0.0 + and pose.orientation.w == 0.0 + ): continue pgo.process(_obs_to_pose3(obs), obs.ts, obs.data) @@ -358,20 +397,23 @@ def process( ts: float, world_cloud: PointCloud2, ) -> None: - if len(world_cloud) == 0: + if world_cloud.width * world_cloud.height == 0: return if not self._is_keyframe(local_pose): return # Unregister: lift world-frame scan back into body frame using its # odom pose, so PGO can re-project it via the optimized pose later. - body_cloud = world_cloud.transform( - _pose3_to_transform( - local_pose.inverse(), - ts=ts, - frame_id=FRAME_BODY, - child_frame_id=FRAME_WORLD_RAW, - ) - ).voxel_downsample(self._cfg.submap_resolution) + body_cloud = voxel_downsample_cloud( + transform_cloud( + world_cloud, + TransformStamped( + header=Header(frame_id=FRAME_BODY, stamp=world_cloud.header.stamp), + child_frame_id=world_cloud.header.frame_id, + transform=transform_from_matrix(local_pose.inverse().matrix()), + ), + ), + self._cfg.submap_resolution, + ) self._add_keyframe(local_pose, ts, body_cloud) self._search_for_loops() self._smooth_and_update() @@ -493,26 +535,24 @@ def _get_submap(self, idx: int, half_range: int) -> PointCloud2: lo = max(0, idx - half_range) hi = min(len(self._key_poses) - 1, idx + half_range) if lo > hi: - return PointCloud2() - cloud = self._key_poses[lo].body_cloud.transform( - _pose3_to_transform( - self._key_poses[lo].optimized, - ts=self._key_poses[lo].timestamp, - frame_id=FRAME_WORLD_CORRECTED, - child_frame_id=FRAME_BODY, + return pointcloud_from_xyz( + np.empty((0, 3)), header=Header(frame_id=FRAME_WORLD_CORRECTED) ) - ) - for i in range(lo + 1, hi + 1): - kp = self._key_poses[i] - cloud = cloud + kp.body_cloud.transform( - _pose3_to_transform( - kp.optimized, - ts=kp.timestamp, - frame_id=FRAME_WORLD_CORRECTED, + + def registered(kp: _KeyPose) -> PointCloud2: + return transform_cloud( + kp.body_cloud, + TransformStamped( + header=Header(frame_id=FRAME_WORLD_CORRECTED, stamp=kp.body_cloud.header.stamp), child_frame_id=FRAME_BODY, - ) + transform=transform_from_matrix(kp.optimized.matrix()), + ), ) - return cloud.voxel_downsample(self._cfg.submap_resolution) + + cloud = registered(self._key_poses[lo]) + for i in range(lo + 1, hi + 1): + cloud = concatenate_clouds(cloud, registered(self._key_poses[i])) + return voxel_downsample_cloud(cloud, self._cfg.submap_resolution) def _search_for_loops(self) -> None: if len(self._key_poses) < self._cfg.min_keyframes_for_loop_search: @@ -636,23 +676,24 @@ def _pose3_to_transform( ts: float, frame_id: str, child_frame_id: str, -) -> Transform: +) -> TransformStamped: """PGO-internal: build a Transform from a Pose3.""" t = np.asarray(pose.translation()) - return Transform( - translation=Vector3(float(t[0]), float(t[1]), float(t[2])), - rotation=Quaternion.from_rotation_matrix(pose.rotation().matrix()), - frame_id=frame_id, + return TransformStamped( + header=Header(frame_id=frame_id, stamp=time_from_seconds(ts)), child_frame_id=child_frame_id, - ts=ts, + transform=Transform( + translation=Vector3(x=float(t[0]), y=float(t[1]), z=float(t[2])), + rotation=quaternion_from_matrix(pose.rotation().matrix()), + ), ) -def _transform_to_pose3(tf: Transform) -> gtsam.Pose3: +def _transform_to_pose3(tf: TransformStamped) -> gtsam.Pose3: """PGO-internal: build a Pose3 from a Transform.""" import gtsam # type: ignore[import-not-found,import-untyped] - return gtsam.Pose3(tf.to_matrix()) + return gtsam.Pose3(transform_matrix(tf.transform)) def _icp( @@ -662,8 +703,8 @@ def _icp( max_dist: float = 1.0, tol: float = 1e-6, min_inliers: int = 10, - init: Transform | None = None, -) -> tuple[Transform, float]: + init: TransformStamped | None = None, +) -> tuple[TransformStamped, float]: """Point-to-plane ICP using Open3D's tensor pipeline. Returns ``(tf, fitness)`` where ``fitness`` is mean squared inlier @@ -674,11 +715,11 @@ def _icp( import open3d as o3d # type: ignore[import-untyped] import open3d.core as o3c # type: ignore[import-untyped] - if len(source) < min_inliers or len(target) < min_inliers: - return Transform.identity(), float("inf") + if source.width * source.height < min_inliers or target.width * target.height < min_inliers: + return TransformStamped(), float("inf") - src_pcd = source.pointcloud_tensor - tgt_pcd = target.pointcloud_tensor + src_pcd = o3d.t.geometry.PointCloud.from_legacy(pointcloud_to_open3d(source)) + tgt_pcd = o3d.t.geometry.PointCloud.from_legacy(pointcloud_to_open3d(target)) # Normals on the target enable point-to-plane ICP — converges tighter # than point-to-point on indoor scenes (walls give unambiguous normals @@ -687,7 +728,7 @@ def _icp( device = src_pcd.device init_T = o3c.Tensor( - init.to_matrix() if init is not None else np.eye(4), + transform_matrix(init.transform) if init is not None else np.eye(4), dtype=o3c.float64, device=device, ) @@ -709,9 +750,12 @@ def _icp( ) if float(result.fitness) == 0.0: - return Transform.identity(), float("inf") + return TransformStamped(), float("inf") - # Frames intentionally unlabeled: caller only reads .to_matrix(). - tf = Transform.from_matrix(result.transformation.numpy(), ts=source.ts) + # Frames intentionally unlabeled: caller only reads the transform matrix. + tf = TransformStamped( + header=Header(stamp=source.header.stamp), + transform=transform_from_matrix(result.transformation.numpy()), + ) rmse = float(result.inlier_rmse) return tf, rmse * rmse diff --git a/dimos/mapping/loop_closure/pgo_auto.py b/dimos/mapping/loop_closure/pgo_auto.py index 1a5fe756d9f..9e172c1f6f2 100644 --- a/dimos/mapping/loop_closure/pgo_auto.py +++ b/dimos/mapping/loop_closure/pgo_auto.py @@ -18,14 +18,14 @@ estimate of where it is in the world slowly diverges from ground truth as small per-frame errors accumulate. PGO ("pose graph optimization") detects revisited places via loop closure and pulls the trajectory back into -self-consistency. The result is a stream of *corrections*: a Transform per +self-consistency. The result is a stream of *corrections*: a TransformStamped per keyframe that maps every drifted ("raw") pose to its corrected one. Pipeline: lidar: Stream[PointCloud2] # pose-stamped scans -> pgo_keyframes(...) -> Stream[Keyframe] # one per kf - -> keyframes_to_corrections(...) -> Stream[Transform] # world_corrected <- world_raw + -> keyframes_to_corrections(...) -> Stream[TransformStamped] # world_corrected <- world_raw -> apply_corrections(any_stream, corrections) -> Stream[T] # obs.pose shuffled `pgo_keyframes` runs ISAM2 + ICP loop closure across the input, emitting @@ -35,11 +35,11 @@ optimized poses + ICP fitness) — useful for visualizing the pose graph. `keyframes_to_corrections` is pure arithmetic: per keyframe, the drift -correction is `optimized @ local^-1` (Transform composition). The resulting +correction is `optimized @ local^-1` (TransformStamped composition). The resulting stream has one entry per keyframe — sparse in time. `make_interpolator` materializes the sparse correction stream into a -ts -> Transform function: SLERP for rotation, linear lerp for translation, +ts -> TransformStamped function: SLERP for rotation, linear lerp for translation, clipping out-of-range queries to the endpoints. `apply_corrections` shuffles `obs.pose` on any stream using the interpolated @@ -58,16 +58,30 @@ from dataclasses import dataclass from typing import TYPE_CHECKING, Any, TypedDict, TypeVar +from dimos_generated.geometry_msgs.msg import Transform, TransformStamped, Vector3 +from dimos_generated.sensor_msgs.msg import PointCloud2 +from dimos_generated.std_msgs.msg import Header import numpy as np from scipy.spatial.transform import Rotation from dimos.memory.store.memory import MemoryStore from dimos.memory.stream import Stream from dimos.memory.type.observation import Observation -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Transform import Transform -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 +from dimos.msgs.geometry import ( + compose_transforms, + inverse_transform, + quaternion_from_matrix, + transform_from_pose, + transform_matrix, +) +from dimos.msgs.pointcloud import ( + concatenate_clouds, + pointcloud_from_xyz, + pointcloud_to_open3d, + transform_cloud, + voxel_downsample_cloud, +) +from dimos.msgs.time import time_from_seconds, to_seconds from dimos.protocol.service.spec import BaseConfig from dimos.utils.logging_config import setup_logger @@ -142,8 +156,8 @@ class Keyframe: """ ts: float - local: Transform - optimized: Transform + local: TransformStamped + optimized: TransformStamped @dataclass(frozen=True) @@ -156,8 +170,8 @@ class LoopClosure: tighter match. """ - source: Transform - target: Transform + source: TransformStamped + target: TransformStamped score: float @@ -199,27 +213,26 @@ def pgo_keyframes( return out -def keyframes_to_corrections(keyframes: Stream[Keyframe]) -> Stream[Transform]: - """Per-keyframe drift correction as Transform(world_corrected <- world_raw).""" +def keyframes_to_corrections(keyframes: Stream[Keyframe]) -> Stream[TransformStamped]: + """Per-keyframe drift correction as TransformStamped(world_corrected <- world_raw).""" mem = MemoryStore() - out: Stream[Transform] = mem.stream("corrections", Transform) + out: Stream[TransformStamped] = mem.stream("corrections", TransformStamped) for obs in keyframes: kf = obs.data # world_corrected <- body <- world_raw composes to world_corrected <- world_raw. - drift = kf.optimized + kf.local.inverse() + drift = compose_transforms(kf.optimized, inverse_transform(kf.local)) out.append(drift, ts=kf.ts) return out -def make_interpolator(corrections: Stream[Transform]) -> Callable[[float], Transform]: - """Materialize corrections once; return a fast ts -> Transform lookup.""" +def make_interpolator(corrections: Stream[TransformStamped]) -> Callable[[float], TransformStamped]: + """Materialize corrections once; return a fast ts -> TransformStamped lookup.""" ts_list: list[float] = [] R_list: list[np.ndarray] = [] t_list: list[np.ndarray] = [] for obs in corrections: R, t = _r_t_from_transform(obs.data) - # obs.ts is authoritative; obs.data.ts can be mutated by Transform's - # ts=0.0 -> time.time() fallback in its constructor. + # Observation time is the correction lookup key. ts_list.append(obs.ts) R_list.append(R) t_list.append(t) @@ -238,7 +251,7 @@ def make_interpolator(corrections: Stream[Transform]) -> Callable[[float], Trans R_stack = np.stack(R_list) t_stack = np.stack(t_list) - def interp(ts: float) -> Transform: + def interp(ts: float) -> TransformStamped: ts_clip = float(np.clip(ts, ts_arr[0], ts_arr[-1])) idx = int(np.searchsorted(ts_arr, ts_clip)) if idx == 0: @@ -261,7 +274,7 @@ def interp(ts: float) -> Transform: def apply_corrections( stream: Stream[T], - corrections: Stream[Transform], + corrections: Stream[TransformStamped], ) -> Stream[T]: """Shuffle obs.pose on `stream` by the interpolated correction at each obs.ts. @@ -275,10 +288,12 @@ def xf(upstream: Iterator[Observation[T]]) -> Iterator[Observation[T]]: if obs.pose is None: yield obs continue - raw_tf = Transform.from_pose(FRAME_BODY, obs.pose_stamped) - # Transform.__add__ composes: (T_corr + T_raw) applies T_corr after T_raw. - # Observation normalizes Transform back to 7-tuple via __post_init__. - corrected = interp(obs.ts) + raw_tf + pose = obs.pose_stamped + assert pose is not None + pose.header.frame_id = FRAME_WORLD_RAW + raw_tf = transform_from_pose(pose, child_frame_id=FRAME_BODY) + # Observation normalizes the generated transform to its storage pose tuple. + corrected = compose_transforms(interp(obs.ts), raw_tf) yield obs.derive(data=obs.data, pose=corrected) return stream.transform(xf) @@ -297,19 +312,19 @@ def __init__( self, keyframes: tuple[Keyframe, ...], loops: tuple[LoopClosure, ...], - correct: Callable[[float], Transform], + correct: Callable[[float], TransformStamped], ) -> None: self.keyframes = keyframes self.loops = loops self._correct = correct - def correction_at(self, ts: float) -> Transform: + def correction_at(self, ts: float) -> TransformStamped: """Drift correction (world_corrected <- world_raw) at `ts`.""" return self._correct(ts) - def correct(self, pose: Transform) -> Transform: + def correct(self, pose: TransformStamped) -> TransformStamped: """Lift a world_raw pose into world_corrected.""" - return self._correct(pose.ts) + pose + return compose_transforms(self._correct(to_seconds(pose.header.stamp)), pose) class PGO: @@ -333,27 +348,28 @@ def __call__( last_ts = keyframes[-1].ts else: # No valid keyframes (e.g. pose-less input): correction is identity. - correct = lambda ts: Transform( # noqa: E731 - frame_id=FRAME_WORLD_CORRECTED, child_frame_id=FRAME_WORLD_RAW, ts=ts - ) + correct = lambda ts: _transform_from_r_t(np.eye(3), np.zeros(3), ts=ts) # noqa: E731 last_ts = 0.0 yield Observation(ts=last_ts, _data=PoseGraph(keyframes, tuple(loops), correct)) -def _r_t_from_transform(tf: Transform) -> tuple[np.ndarray, np.ndarray]: - q = tf.rotation +def _r_t_from_transform(tf: TransformStamped) -> tuple[np.ndarray, np.ndarray]: + q = tf.transform.rotation R = Rotation.from_quat([q.x, q.y, q.z, q.w]).as_matrix() - t = np.array([tf.translation.x, tf.translation.y, tf.translation.z]) + t = np.array( + [tf.transform.translation.x, tf.transform.translation.y, tf.transform.translation.z] + ) return R, t -def _transform_from_r_t(R: np.ndarray, t: np.ndarray, *, ts: float) -> Transform: - return Transform( - translation=Vector3(float(t[0]), float(t[1]), float(t[2])), - rotation=Quaternion.from_rotation_matrix(R), - frame_id=FRAME_WORLD_CORRECTED, +def _transform_from_r_t(R: np.ndarray, t: np.ndarray, *, ts: float) -> TransformStamped: + return TransformStamped( + header=Header(frame_id=FRAME_WORLD_CORRECTED, stamp=time_from_seconds(ts)), child_frame_id=FRAME_WORLD_RAW, - ts=ts, + transform=Transform( + translation=Vector3(x=float(t[0]), y=float(t[1]), z=float(t[2])), + rotation=quaternion_from_matrix(R), + ), ) @@ -419,15 +435,24 @@ def process( ts: float, world_cloud: PointCloud2, ) -> None: - if len(world_cloud) == 0: + if world_cloud.width * world_cloud.height == 0: return if not self._is_keyframe(local_pose): return # Unregister: lift world-frame scan back into body frame using its # odom pose, so PGO can re-project it via the optimized pose later. - body_cloud = world_cloud.transform( - _pose3_to_transform(local_pose.inverse(), ts=ts) - ).voxel_downsample(self._cfg.submap_resolution) + body_cloud = voxel_downsample_cloud( + transform_cloud( + world_cloud, + _pose3_to_transform( + local_pose.inverse(), + ts=ts, + frame_id=FRAME_BODY, + child_frame_id=world_cloud.header.frame_id, + ), + ), + self._cfg.submap_resolution, + ) self._add_keyframe(local_pose, ts, body_cloud) self._search_for_loops() self._smooth_and_update() @@ -558,16 +583,25 @@ def _get_submap(self, idx: int, half_range: int) -> PointCloud2: lo = max(0, idx - half_range) hi = min(len(self._key_poses) - 1, idx + half_range) if lo > hi: - return PointCloud2() - cloud = self._key_poses[lo].body_cloud.transform( - _pose3_to_transform(self._key_poses[lo].optimized, ts=self._key_poses[lo].timestamp) - ) - for i in range(lo + 1, hi + 1): - kp = self._key_poses[i] - cloud = cloud + kp.body_cloud.transform( - _pose3_to_transform(kp.optimized, ts=kp.timestamp) + return pointcloud_from_xyz( + np.empty((0, 3)), header=Header(frame_id=FRAME_WORLD_CORRECTED) ) - return cloud.voxel_downsample(self._cfg.submap_resolution) + + def registered(kp: _KeyPose) -> PointCloud2: + return transform_cloud( + kp.body_cloud, + _pose3_to_transform( + kp.optimized, + ts=kp.timestamp, + frame_id=FRAME_WORLD_CORRECTED, + child_frame_id=FRAME_BODY, + ), + ) + + cloud = registered(self._key_poses[lo]) + for i in range(lo + 1, hi + 1): + cloud = concatenate_clouds(cloud, registered(self._key_poses[i])) + return voxel_downsample_cloud(cloud, self._cfg.submap_resolution) def _search_for_loops( self, @@ -668,7 +702,7 @@ def _collect(local_idxs: set[int], time_thresh: float) -> list[tuple[float, int] # already and extra loops add noise. Early-exit on a very tight # match to save redundant ICP runs in the common case. source = self._get_submap(cur_idx, source_half_range) - accepted: list[tuple[int, Transform, float]] = [] + accepted: list[tuple[int, TransformStamped, float]] = [] k = ( candidates_per_iter if candidates_per_iter is not None @@ -771,23 +805,24 @@ def _pose3_to_transform( ts: float, frame_id: str = "", child_frame_id: str = "", -) -> Transform: - """PGO-internal: build a Transform from a Pose3.""" +) -> TransformStamped: + """PGO-internal: build a TransformStamped from a Pose3.""" t = np.asarray(pose.translation()) - return Transform( - translation=Vector3(float(t[0]), float(t[1]), float(t[2])), - rotation=Quaternion.from_rotation_matrix(pose.rotation().matrix()), - frame_id=frame_id, + return TransformStamped( + header=Header(frame_id=frame_id, stamp=time_from_seconds(ts)), child_frame_id=child_frame_id, - ts=ts, + transform=Transform( + translation=Vector3(x=float(t[0]), y=float(t[1]), z=float(t[2])), + rotation=quaternion_from_matrix(pose.rotation().matrix()), + ), ) -def _transform_to_pose3(tf: Transform) -> gtsam.Pose3: - """PGO-internal: build a Pose3 from a Transform.""" +def _transform_to_pose3(tf: TransformStamped) -> gtsam.Pose3: + """PGO-internal: build a Pose3 from a TransformStamped.""" import gtsam # type: ignore[import-not-found,import-untyped] - return gtsam.Pose3(tf.to_matrix()) + return gtsam.Pose3(transform_matrix(tf.transform)) def _icp( @@ -797,8 +832,8 @@ def _icp( max_dist: float = 1.0, tol: float = 1e-6, min_inliers: int = 10, - init: Transform | None = None, -) -> tuple[Transform, float]: + init: TransformStamped | None = None, +) -> tuple[TransformStamped, float]: """Point-to-plane ICP using Open3D's tensor pipeline. Returns ``(tf, fitness)`` where ``fitness`` is mean squared inlier @@ -809,11 +844,13 @@ def _icp( import open3d as o3d # type: ignore[import-untyped] import open3d.core as o3c # type: ignore[import-untyped] - if len(source) < min_inliers or len(target) < min_inliers: - return Transform.identity(), float("inf") + if source.width * source.height < min_inliers or target.width * target.height < min_inliers: + return _transform_from_r_t( + np.eye(3), np.zeros(3), ts=to_seconds(source.header.stamp) + ), float("inf") - src_pcd = source.pointcloud_tensor - tgt_pcd = target.pointcloud_tensor + src_pcd = o3d.t.geometry.PointCloud.from_legacy(pointcloud_to_open3d(source)) + tgt_pcd = o3d.t.geometry.PointCloud.from_legacy(pointcloud_to_open3d(target)) # Normals on the target enable point-to-plane ICP — converges tighter # than point-to-point on indoor scenes (walls give unambiguous normals @@ -822,7 +859,7 @@ def _icp( device = src_pcd.device init_T = o3c.Tensor( - init.to_matrix() if init is not None else np.eye(4), + transform_matrix(init.transform) if init is not None else np.eye(4), dtype=o3c.float64, device=device, ) @@ -845,11 +882,17 @@ def _icp( ), ) except RuntimeError: - return Transform.identity(), float("inf") + return _transform_from_r_t( + np.eye(3), np.zeros(3), ts=to_seconds(source.header.stamp) + ), float("inf") if float(result.fitness) == 0.0: - return Transform.identity(), float("inf") + return _transform_from_r_t( + np.eye(3), np.zeros(3), ts=to_seconds(source.header.stamp) + ), float("inf") T_mat = result.transformation.numpy() rmse = float(result.inlier_rmse) - return _transform_from_r_t(T_mat[:3, :3], T_mat[:3, 3], ts=source.ts), rmse * rmse + return _transform_from_r_t( + T_mat[:3, :3], T_mat[:3, 3], ts=to_seconds(source.header.stamp) + ), rmse * rmse diff --git a/dimos/mapping/loop_closure/test_pgo.py b/dimos/mapping/loop_closure/test_pgo.py index 36277883f6d..c1bf3e5302a 100644 --- a/dimos/mapping/loop_closure/test_pgo.py +++ b/dimos/mapping/loop_closure/test_pgo.py @@ -14,6 +14,17 @@ from __future__ import annotations +from dimos_generated.geometry_msgs.msg import ( + Point, + Pose, + PoseStamped, + Quaternion, + Transform, + TransformStamped, + Vector3, +) +from dimos_generated.sensor_msgs.msg import PointCloud2 +from dimos_generated.std_msgs.msg import Header import numpy as np import pytest from scipy.spatial.transform import Rotation @@ -28,10 +39,30 @@ ) from dimos.memory.store.memory import MemoryStore from dimos.memory.stream import Stream -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Transform import Transform -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 +from dimos.msgs.geometry import quaternion_from_matrix, transform_matrix +from dimos.msgs.pointcloud import pointcloud_from_xyz +from dimos.msgs.time import time_from_seconds, to_seconds + + +def _vector(*values): + values = values[0] if len(values) == 1 else values + return Vector3(x=float(values[0]), y=float(values[1]), z=float(values[2])) + + +def _quaternion(x, y, z, w): + return Quaternion(x=x, y=y, z=z, w=w) + + +def _stamped_transform(translation=None, rotation=None, ts=0.0, frame_id="", child_frame_id=""): + return TransformStamped( + header=Header(frame_id=frame_id, stamp=time_from_seconds(ts)), + child_frame_id=child_frame_id, + transform=Transform( + translation=translation if translation is not None else Vector3(), + rotation=rotation if rotation is not None else Quaternion(w=1.0), + ), + ) + # TODO(PY311): drop — the mapping extra excludes gtsam-extended where it has no # wheels (py3.10 Linux), see pyproject. @@ -75,9 +106,9 @@ def test_observation_normalizes_transform_pose(self) -> None: """Constructing/deriving with pose=Transform should coerce to 7-tuple.""" from dimos.memory.type.observation import Observation - tf = Transform( - translation=Vector3(1.5, -2.0, 0.7), - rotation=Quaternion(0.1, 0.2, 0.3, 0.927), + tf = _stamped_transform( + translation=_vector(1.5, -2.0, 0.7), + rotation=_quaternion(0.1, 0.2, 0.3, 0.927), ts=1.0, ) obs: Observation[int] = Observation(id=0, ts=1.0, pose=tf, _data=0) @@ -91,9 +122,11 @@ def test_observation_normalizes_transform_pose(self) -> None: def test_observation_normalizes_posestamped(self) -> None: from dimos.memory.type.observation import Observation - from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped - ps = PoseStamped(ts=1.0, position=(1.0, 2.0, 3.0), orientation=(0.0, 0.0, 0.0, 1.0)) + ps = PoseStamped( + header=Header(stamp=time_from_seconds(1.0)), + pose=Pose(position=Point(x=1.0, y=2.0, z=3.0), orientation=Quaternion(w=1.0)), + ) obs: Observation[int] = Observation(id=0, ts=1.0, pose=ps, _data=0) assert obs.pose_tuple == (1.0, 2.0, 3.0, 0.0, 0.0, 0.0, 1.0) @@ -103,7 +136,7 @@ def test_obs_to_pose3_roundtrip(self) -> None: rng = np.random.default_rng(4) R = _random_R(rng) t = rng.uniform(-3, 3, size=3) - tf = Transform(translation=Vector3(t), rotation=Quaternion.from_rotation_matrix(R), ts=1.0) + tf = _stamped_transform(translation=_vector(t), rotation=quaternion_from_matrix(R), ts=1.0) obs: Observation[int] = Observation(id=0, ts=1.0, pose=tf, _data=0) p = _obs_to_pose3(obs) np.testing.assert_allclose(p.rotation().matrix(), R, atol=1e-9) @@ -117,8 +150,8 @@ def test_pose3_to_transform(self) -> None: t = rng.uniform(-3, 3, size=3) p = gtsam.Pose3(gtsam.Rot3(R), gtsam.Point3(t)) tf = _pose3_to_transform(p, ts=7.89, frame_id="world", child_frame_id="body") - np.testing.assert_allclose(tf.rotation.to_rotation_matrix(), R, atol=1e-10) - np.testing.assert_allclose(tf.translation.to_numpy(), t, atol=1e-10) + np.testing.assert_allclose(transform_matrix(tf.transform)[:3, :3], R, atol=1e-10) + np.testing.assert_allclose(transform_matrix(tf.transform)[:3, 3], t, atol=1e-10) def test_pose3_to_transform_with_frames(self) -> None: import gtsam @@ -128,10 +161,10 @@ def test_pose3_to_transform_with_frames(self) -> None: t = rng.uniform(-3, 3, size=3) p = gtsam.Pose3(gtsam.Rot3(R), gtsam.Point3(t)) tf = _pose3_to_transform(p, ts=1.0, frame_id="world_corrected", child_frame_id="body") - assert tf.frame_id == "world_corrected" + assert tf.header.frame_id == "world_corrected" assert tf.child_frame_id == "body" - np.testing.assert_allclose(tf.rotation.to_rotation_matrix(), R, atol=1e-10) - np.testing.assert_allclose(tf.translation.to_numpy(), t, atol=1e-10) + np.testing.assert_allclose(transform_matrix(tf.transform)[:3, :3], R, atol=1e-10) + np.testing.assert_allclose(transform_matrix(tf.transform)[:3, 3], t, atol=1e-10) def _make_lidar_stream(n_frames: int = 12, points_per_frame: int = 500) -> Stream[PointCloud2]: @@ -152,7 +185,10 @@ def _make_lidar_stream(n_frames: int = 12, points_per_frame: int = 500) -> Strea body = rng.uniform(-1, 1, size=(points_per_frame, 3)).astype(np.float32) world = (R_world @ body.T).T + np.array([i, 0, 0], dtype=np.float32) lidar.append( - PointCloud2.from_numpy(world.astype(np.float32)), + pointcloud_from_xyz( + world.astype(np.float32), + header=Header(frame_id="world_raw", stamp=time_from_seconds(float(i))), + ), ts=float(i), pose=(float(i), 0.0, 0.0, qx, qy, qz, qw), ) @@ -182,19 +218,31 @@ def test_apply_identity_corrections_preserves_poses(self) -> None: assert a == pytest.approx(b, abs=1e-6) -def _graph_with_drift_at(drifts: list[Transform]) -> PoseGraph: +def _graph_with_drift_at(drifts: list[TransformStamped]) -> PoseGraph: """PoseGraph whose drift correction equals each ``drifts[i]`` at ``drifts[i].ts``. Trick: drift = optimized + local^-1. With local=identity, drift==optimized. """ - identity = Vector3(0.0, 0.0, 0.0) - identity_rot = Quaternion(0.0, 0.0, 0.0, 1.0) + identity = _vector(0.0, 0.0, 0.0) + identity_rot = _quaternion(0.0, 0.0, 0.0, 1.0) return PoseGraph( keyframes=tuple( Keyframe( - ts=d.ts, - local=Transform(translation=identity, rotation=identity_rot, ts=d.ts), - optimized=Transform(translation=d.translation, rotation=d.rotation, ts=d.ts), + ts=to_seconds(d.header.stamp), + local=_stamped_transform( + translation=identity, + rotation=identity_rot, + ts=to_seconds(d.header.stamp), + frame_id="world_raw", + child_frame_id="body", + ), + optimized=_stamped_transform( + translation=d.transform.translation, + rotation=d.transform.rotation, + ts=to_seconds(d.header.stamp), + frame_id="world_corrected", + child_frame_id="body", + ), ) for d in drifts ) @@ -208,34 +256,36 @@ def test_empty_raises(self) -> None: def test_single_keyframe_returns_constant(self) -> None: R = Rotation.from_euler("z", np.pi / 4).as_matrix() - only = Transform( - translation=Vector3(1.0, 2.0, 3.0), - rotation=Quaternion.from_rotation_matrix(R), + only = _stamped_transform( + translation=_vector(1.0, 2.0, 3.0), + rotation=quaternion_from_matrix(R), ts=10.0, ) graph = _graph_with_drift_at([only]) for query_ts in (0.0, 10.0, 100.0): out = graph.correction_at(query_ts) - assert out.translation.x == pytest.approx(1.0, abs=1e-10) - assert out.translation.y == pytest.approx(2.0, abs=1e-10) - assert out.translation.z == pytest.approx(3.0, abs=1e-10) + assert out.transform.translation.x == pytest.approx(1.0, abs=1e-10) + assert out.transform.translation.y == pytest.approx(2.0, abs=1e-10) + assert out.transform.translation.z == pytest.approx(3.0, abs=1e-10) def test_out_of_range_clips_to_endpoints(self) -> None: - # Transform's ctor maps ts=0.0 -> time.time(); use ts>0 for determinism. - a = Transform(translation=Vector3(0.0, 0.0, 0.0), ts=1.0) - b = Transform(translation=Vector3(10.0, 0.0, 0.0), ts=11.0) + # Positive fixture stamps make the clipping interval explicit. + a = _stamped_transform(translation=_vector(0.0, 0.0, 0.0), ts=1.0) + b = _stamped_transform(translation=_vector(10.0, 0.0, 0.0), ts=11.0) graph = _graph_with_drift_at([a, b]) # Below range -> clipped to a - assert graph.correction_at(-5.0).translation.x == pytest.approx(0.0, abs=1e-10) + assert graph.correction_at(-5.0).transform.translation.x == pytest.approx(0.0, abs=1e-10) # Above range -> clipped to b - assert graph.correction_at(100.0).translation.x == pytest.approx(10.0, abs=1e-10) + assert graph.correction_at(100.0).transform.translation.x == pytest.approx(10.0, abs=1e-10) # In-range midpoint - assert graph.correction_at(6.0).translation.x == pytest.approx(5.0, abs=1e-10) + assert graph.correction_at(6.0).transform.translation.x == pytest.approx(5.0, abs=1e-10) def test_frozen(self) -> None: graph = PoseGraph() with pytest.raises(Exception): - graph.keyframes = (Keyframe(ts=0, local=Transform(), optimized=Transform()),) # type: ignore[misc] + graph.keyframes = ( + Keyframe(ts=0, local=_stamped_transform(), optimized=_stamped_transform()), + ) # type: ignore[misc] class TestApplyAsTransformer: @@ -247,14 +297,14 @@ def test_pure_translation_shifts_poses(self) -> None: lidar: Stream[PointCloud2] = mem.stream("lidar", PointCloud2) for i in range(3): lidar.append( - PointCloud2.from_numpy(np.zeros((1, 3), dtype=np.float32)), + pointcloud_from_xyz(np.zeros((1, 3), dtype=np.float32), header=Header()), ts=float(i + 1), pose=(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0), ) graph = _graph_with_drift_at( [ - Transform(translation=Vector3(5.0, 0.0, 0.0), ts=1.0), - Transform(translation=Vector3(5.0, 0.0, 0.0), ts=3.0), + _stamped_transform(translation=_vector(5.0, 0.0, 0.0), ts=1.0), + _stamped_transform(translation=_vector(5.0, 0.0, 0.0), ts=3.0), ] ) for obs in lidar.transform(graph): @@ -268,14 +318,14 @@ def test_passes_through_pose_none(self) -> None: mem = MemoryStore() lidar: Stream[PointCloud2] = mem.stream("lidar", PointCloud2) lidar.append( - PointCloud2.from_numpy(np.zeros((1, 3), dtype=np.float32)), + pointcloud_from_xyz(np.zeros((1, 3), dtype=np.float32), header=Header()), ts=1.0, pose=None, ) graph = _graph_with_drift_at( [ - Transform(translation=Vector3(5.0, 0.0, 0.0), ts=1.0), - Transform(translation=Vector3(5.0, 0.0, 0.0), ts=2.0), + _stamped_transform(translation=_vector(5.0, 0.0, 0.0), ts=1.0), + _stamped_transform(translation=_vector(5.0, 0.0, 0.0), ts=2.0), ] ) for obs in lidar.transform(graph): @@ -284,13 +334,13 @@ def test_passes_through_pose_none(self) -> None: class TestKeyframeType: def test_keyframe_is_frozen(self) -> None: - identity = Transform( - translation=Vector3(0.0, 0.0, 0.0), - rotation=Quaternion(0.0, 0.0, 0.0, 1.0), + identity = _stamped_transform( + translation=_vector(0.0, 0.0, 0.0), + rotation=_quaternion(0.0, 0.0, 0.0, 1.0), ts=1.0, ) kf = Keyframe(ts=1.0, local=identity, optimized=identity) with pytest.raises(Exception): kf.ts = 2.0 # type: ignore[misc] - assert isinstance(kf.local, Transform) - assert isinstance(kf.optimized, Transform) + assert isinstance(kf.local, TransformStamped) + assert isinstance(kf.optimized, TransformStamped) diff --git a/dimos/mapping/loop_closure/test_pgo_auto.py b/dimos/mapping/loop_closure/test_pgo_auto.py new file mode 100644 index 00000000000..77eba4dbe4f --- /dev/null +++ b/dimos/mapping/loop_closure/test_pgo_auto.py @@ -0,0 +1,105 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from dimos_generated.geometry_msgs.msg import Transform, TransformStamped, Vector3 +from dimos_generated.std_msgs.msg import Header +import numpy as np +import pytest + +from dimos.mapping.loop_closure.pgo_auto import ( + FRAME_BODY, + FRAME_WORLD_CORRECTED, + FRAME_WORLD_RAW, + PGO, + Keyframe, + apply_corrections, + keyframes_to_corrections, + make_interpolator, +) +from dimos.memory.store.memory import MemoryStore +from dimos.memory.type.observation import Observation +from dimos.msgs.geometry import quaternion_from_euler +from dimos.msgs.pointcloud import pointcloud_from_xyz +from dimos.msgs.time import time_from_seconds, to_seconds + + +def _pose(x: float, frame: str, ts: float) -> TransformStamped: + return TransformStamped( + header=Header(frame_id=frame, stamp=time_from_seconds(ts)), + child_frame_id=FRAME_BODY, + transform=Transform(translation=Vector3(x=x), rotation=quaternion_from_euler(0, 0, 0)), + ) + + +def test_generated_corrections_preserve_frames_time_and_nearest_neighbor() -> None: + with MemoryStore() as store: + keyframes = store.stream("keyframes", Keyframe) + keyframes.append( + Keyframe(0.0, _pose(1, FRAME_WORLD_RAW, 0), _pose(3, FRAME_WORLD_CORRECTED, 0)), ts=0 + ) + keyframes.append( + Keyframe(2.0, _pose(2, FRAME_WORLD_RAW, 2), _pose(6, FRAME_WORLD_CORRECTED, 2)), ts=2 + ) + corrections = keyframes_to_corrections(keyframes) + lookup = make_interpolator(corrections) + for ts, expected in [(-1, 2), (0.9, 2), (1.1, 4), (3, 4)]: + correction = TransformStamped.decode(lookup(ts).encode()) + assert correction.transform.translation.x == expected + assert correction.header.frame_id == FRAME_WORLD_CORRECTED + assert correction.child_frame_id == FRAME_WORLD_RAW + assert to_seconds(correction.header.stamp) == ts + stream = store.stream("values", str) + stream.append("payload", ts=0, pose=(1, 0, 0, 0, 0, 0, 1)) + stream.append("no pose", ts=2) + output = list(apply_corrections(stream, corrections)) + assert output[0].data == "payload" + assert output[0].pose_tuple == (3, 0, 0, 0, 0, 0, 1) + assert output[0].ts == 0 + assert output[1].pose is None + assert stream.first().pose_tuple == (1, 0, 0, 0, 0, 0, 1) + + +def test_empty_pgo_has_generated_identity_correction() -> None: + pytest.importorskip("gtsam") + graph = next(PGO()(iter(()))).data + assert not graph.keyframes and not graph.loops + correction = graph.correction_at(0) + assert to_seconds(correction.header.stamp) == 0 + assert correction.transform.rotation.w == 1 + result = graph.correct(_pose(5, FRAME_WORLD_RAW, 0)) + assert result.transform.translation.x == 5 + assert result.header.frame_id == FRAME_WORLD_CORRECTED + + +def test_pgo_consumes_generated_cloud_without_mutating_input() -> None: + pytest.importorskip("gtsam") + cloud = pointcloud_from_xyz( + np.array([[1.0, 0, 0], [1, 1, 0], [1, 0, 1]]), + header=Header(frame_id=FRAME_WORLD_RAW, stamp=time_from_seconds(12.25)), + ) + before = cloud.encode() + quaternion = quaternion_from_euler(0, 0, 0.1) + observation = Observation( + ts=12.25, + _data=cloud, + pose=(1, 0, 0, quaternion.x, quaternion.y, quaternion.z, quaternion.w), + ) + graph = next(PGO()(iter([observation]))).data + assert len(graph.keyframes) == 1 + assert graph.keyframes[0].ts == 12.25 + assert graph.keyframes[0].local.header.frame_id == FRAME_WORLD_RAW + correction = graph.correction_at(12.25) + assert correction.transform.translation.x == pytest.approx(0, abs=1e-6) + assert correction.transform.rotation.w == pytest.approx(1) + assert cloud.encode() == before diff --git a/dimos/mapping/loop_closure/test_pgo_generated.py b/dimos/mapping/loop_closure/test_pgo_generated.py new file mode 100644 index 00000000000..cca13fefc75 --- /dev/null +++ b/dimos/mapping/loop_closure/test_pgo_generated.py @@ -0,0 +1,143 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import math +import sys +from types import SimpleNamespace +from unittest.mock import MagicMock + +from dimos_generated.geometry_msgs.msg import Quaternion, Transform, TransformStamped, Vector3 +from dimos_generated.std_msgs.msg import Header +import numpy as np +import pytest + +from dimos.mapping.loop_closure.pgo import Keyframe, PGOConfig, PoseGraph, _KeyPose, _PGOState +from dimos.memory.type.observation import Observation +from dimos.msgs.pointcloud import pointcloud_from_xyz, pointcloud_xyz +from dimos.msgs.time import time_from_nanoseconds, time_from_seconds + + +def test_generated_observation_pose_correction_applies_rotation_after_translation(): + local = TransformStamped( + header=Header(frame_id="world_raw", stamp=time_from_seconds(1.0)), child_frame_id="body" + ) + optimized = TransformStamped( + header=Header(frame_id="world_corrected", stamp=time_from_seconds(1.0)), + child_frame_id="body", + transform=Transform( + translation=Vector3(x=5.0), + rotation=Quaternion(z=math.sqrt(0.5), w=math.sqrt(0.5)), + ), + ) + graph = PoseGraph(keyframes=(Keyframe(ts=1.0, local=local, optimized=optimized),)) + source = Observation(id=1, ts=1.0, _data="payload", pose=(1.0, 2.0, 0.0, 0.0, 0.0, 0.0, 1.0)) + corrected = next(graph(iter([source]))) + assert corrected.data == "payload" and corrected.ts == source.ts + np.testing.assert_allclose( + corrected.pose_tuple, (3.0, 1.0, 0.0, 0.0, 0.0, math.sqrt(0.5), math.sqrt(0.5)), atol=1e-9 + ) + assert source.pose_tuple == (1.0, 2.0, 0.0, 0.0, 0.0, 0.0, 1.0) + + +def test_pose_graph_retains_poseless_observations_without_constructing_correction(): + source = Observation(id=1, ts=1.0, _data="payload") + assert list(PoseGraph()(iter([source]))) == [source] + + +def test_pgo_submap_places_generated_body_clouds_before_merging(monkeypatch): + class MatrixPose: + def __init__(self, x): + self.value = np.eye(4) + self.value[0, 3] = x + + def matrix(self): + return self.value + + solver = SimpleNamespace( + **{ + name: MagicMock(name=name) + for name in ( + "Pose3", + "ISAM2Params", + "ISAM2", + "NonlinearFactorGraph", + "Values", + ) + } + ) + monkeypatch.setitem(sys.modules, "gtsam", solver) + state = _PGOState(PGOConfig(submap_resolution=0.2)) + solver.ISAM2Params.return_value.setRelinearizeThreshold.assert_called_once_with(0.01) + assert solver.ISAM2Params.return_value.relinearizeSkip == 1 + clouds = [ + pointcloud_from_xyz( + np.array([[1.0, 0.0, 0.0]]), header=Header(frame_id="body", stamp=time_from_seconds(ts)) + ) + for ts in (1.0, 2.0) + ] + state._key_poses = [ + _KeyPose(local=MatrixPose(0), optimized=MatrixPose(offset), timestamp=ts, body_cloud=cloud) + for cloud, offset, ts in zip(clouds, (2.0, 4.0), (1.0, 2.0), strict=True) + ] + before = [cloud.encode() for cloud in clouds] + result = state._get_submap(0, 1) + np.testing.assert_allclose(pointcloud_xyz(result), [[3.0, 0.0, 0.0], [5.0, 0.0, 0.0]]) + assert result.header.frame_id == "world_corrected" + assert result.header.stamp == time_from_seconds(2.0) + assert [cloud.encode() for cloud in clouds] == before + + +@pytest.mark.parametrize( + "query, expected_x", [(-5.0, 0.0), (1.0, 0.0), (6.0, 5.0), (11.0, 10.0), (100.0, 10.0)] +) +def test_generated_correction_interpolates_and_clips_without_solver(query, expected_x): + keyframes = [] + for ts, x in ((1.0, 0.0), (11.0, 10.0)): + local = TransformStamped( + header=Header(frame_id="world_raw", stamp=time_from_seconds(ts)), child_frame_id="body" + ) + optimized = TransformStamped( + header=Header(frame_id="world_corrected", stamp=time_from_seconds(ts)), + child_frame_id="body", + transform=Transform(translation=Vector3(x=x)), + ) + keyframes.append(Keyframe(ts=ts, local=local, optimized=optimized)) + graph = PoseGraph(keyframes=tuple(keyframes)) + value = graph.correction_at(query) + assert value.header.frame_id == "world_corrected" and value.child_frame_id == "world_raw" + assert value.header.stamp == time_from_seconds(query) + assert value.transform.translation.x == pytest.approx(expected_x) + assert TransformStamped.decode(value.encode()) == value + + +def test_generated_correct_preserves_exact_source_stamp_and_rejects_bad_frame(): + local = TransformStamped(header=Header(frame_id="world_raw"), child_frame_id="body") + optimized = TransformStamped( + header=Header(frame_id="world_corrected"), + child_frame_id="body", + transform=Transform(translation=Vector3(x=5.0)), + ) + graph = PoseGraph(keyframes=(Keyframe(ts=1.0, local=local, optimized=optimized),)) + source = TransformStamped( + header=Header(frame_id="world_raw", stamp=time_from_nanoseconds(1700000000123456789)), + child_frame_id="camera", + transform=Transform(translation=Vector3(x=2.0)), + ) + result = graph.correct(source) + assert result.header.stamp == source.header.stamp + assert result.header.frame_id == "world_corrected" and result.child_frame_id == "camera" + assert result.transform.translation.x == 7.0 and source.transform.translation.x == 2.0 + source.header.frame_id = "unrelated" + with pytest.raises(ValueError, match="Cannot compose frames"): + graph.correct(source) diff --git a/dimos/mapping/loop_closure/utils/markers_rrd.py b/dimos/mapping/loop_closure/utils/markers_rrd.py index 5b8a27b1074..9ee0ea2a68e 100644 --- a/dimos/mapping/loop_closure/utils/markers_rrd.py +++ b/dimos/mapping/loop_closure/utils/markers_rrd.py @@ -30,17 +30,21 @@ from pathlib import Path +from dimos_generated.sensor_msgs.msg import Image, PointCloud2 import typer from dimos.memory.store.sqlite import SqliteStore from dimos.memory.stream import Stream from dimos.memory.transform import QualityWindow, SpeedLimit from dimos.memory.vis.color import Color -from dimos.msgs.sensor_msgs.Image import Image -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 +from dimos.msgs.image import image_sharpness from dimos.perception.fiducial.marker_transformer import DetectMarkers -from dimos.robot.unitree.go2.connection import _camera_info_static +from dimos.robot.unitree.go2.camera_calibration import front_camera_calibration from dimos.utils.data import resolve_named_path +from dimos.visualization.rerun.message_helpers import ( + camera_pinhole, + image_archetype, +) TIMELINE = "ts" @@ -65,14 +69,14 @@ def main( import rerun as rr db_path = resolve_named_path(dataset, ".db") - cam_info = _camera_info_static() + cam_info = front_camera_calibration() rr.init("dimos markers", recording_id=db_path.stem) rr.save(str(out)) # Static pinhole on the camera entity; per-frame Transform3D goes on the # same entity. Image is the child so it projects through the pinhole. - pinhole = cam_info.to_rerun() + pinhole = camera_pinhole(cam_info) assert not isinstance(pinhole, list) rr.log("world/camera", pinhole, static=True) @@ -106,7 +110,7 @@ def main( translation=[x, y, z], quaternion=rr.Quaternion(xyzw=[qx, qy, qz, qw]) ), ) - rr.log("world/camera/image", img_obs.data.to_rerun()) + rr.log("world/camera/image", image_archetype(img_obs.data)) # Clear any prior detection box at this ts; pass 3 will overwrite # this with the actual detection iff one fires for this frame. rr.log( @@ -119,7 +123,7 @@ def main( # Pass 3: marker detections, filtered the same way as `dimos map`. xf = DetectMarkers(camera_info=cam_info, marker_length_m=marker_size) pipeline: Stream[Image] = color_image.transform( - QualityWindow(lambda img: img.sharpness, window=quality_window) + QualityWindow(image_sharpness, window=quality_window) ) if marker_max_speed > 0: pipeline = pipeline.transform( @@ -185,7 +189,7 @@ def main( smoothing_window=smoothing_window, ) pipeline_tracked: Stream[Image] = color_image.transform( - QualityWindow(lambda img: img.sharpness, window=quality_window) + QualityWindow(image_sharpness, window=quality_window) ) if marker_max_speed > 0: pipeline_tracked = pipeline_tracked.transform( diff --git a/dimos/mapping/occupancy/conftest.py b/dimos/mapping/occupancy/conftest.py index f20dc1310b1..0847c3199cd 100644 --- a/dimos/mapping/occupancy/conftest.py +++ b/dimos/mapping/occupancy/conftest.py @@ -12,17 +12,27 @@ # See the License for the specific language governing permissions and # limitations under the License. +from dimos_generated.geometry_msgs.msg import Pose, Quaternion +from dimos_generated.nav_msgs.msg import MapMetaData, OccupancyGrid import numpy as np import pytest from dimos.mapping.occupancy.gradient import gradient -from dimos.msgs.nav_msgs.OccupancyGrid import OccupancyGrid from dimos.utils.data import get_data @pytest.fixture def occupancy() -> OccupancyGrid: - return OccupancyGrid(np.load(get_data("occupancy_simple.npy"))) + cells = np.load(get_data("occupancy_simple.npy")) + return OccupancyGrid( + info=MapMetaData( + width=cells.shape[1], + height=cells.shape[0], + resolution=0.05, + origin=Pose(orientation=Quaternion(w=1)), + ), + data=cells.astype(np.int8).ravel(), + ) @pytest.fixture diff --git a/dimos/mapping/occupancy/extrude_occupancy.py b/dimos/mapping/occupancy/extrude_occupancy.py index 799319cbf6d..bf6f8d59954 100644 --- a/dimos/mapping/occupancy/extrude_occupancy.py +++ b/dimos/mapping/occupancy/extrude_occupancy.py @@ -13,10 +13,11 @@ # limitations under the License. +from dimos_generated.nav_msgs.msg import OccupancyGrid import numpy as np from numpy.typing import NDArray -from dimos.msgs.nav_msgs.OccupancyGrid import CostValues, OccupancyGrid +from dimos.msgs.occupancy import occupancy_view # Rectangle type: (x, y, width, height) Rect = tuple[int, int, int, int] @@ -36,12 +37,10 @@ def identify_convex_shapes(occupancy_grid: OccupancyGrid) -> list[Rect]: Returns: List of rectangles as (x, y, width, height) tuples in grid coords. """ - grid = occupancy_grid.grid + grid = occupancy_view(occupancy_grid) # Create binary mask of occupied cells (treat UNKNOWN as OCCUPIED) - occupied_mask = ((grid == CostValues.OCCUPIED) | (grid == CostValues.UNKNOWN)).astype( - np.uint8 - ) * 255 + occupied_mask = ((grid == 100) | (grid == -1)).astype(np.uint8) * 255 return _decompose_to_rectangles(occupied_mask) @@ -184,9 +183,9 @@ def generate_mujoco_scene( # Get rectangles from the occupancy grid rectangles = identify_convex_shapes(occupancy_grid) - resolution = occupancy_grid.resolution - origin_x = occupancy_grid.origin.position.x - origin_y = occupancy_grid.origin.position.y + resolution = occupancy_grid.info.resolution + origin_x = occupancy_grid.info.origin.position.x + origin_y = occupancy_grid.info.origin.position.y # Build XML xml_lines = [ @@ -208,7 +207,7 @@ def generate_mujoco_scene( # Add each rectangle as a box geom for i, (gx, gy, gw, gh) in enumerate(rectangles): # Convert grid coordinates to world coordinates - # Grid origin is top-left, world origin is at occupancy_grid.origin + # Grid origin is top-left, world origin is at occupancy_grid.info.origin # gx, gy are in grid cells, need to convert to meters world_x = origin_x + (gx + gw / 2) * resolution world_y = origin_y + (gy + gh / 2) * resolution diff --git a/dimos/mapping/occupancy/gradient.py b/dimos/mapping/occupancy/gradient.py index 802b52a9faf..0478058b1c5 100644 --- a/dimos/mapping/occupancy/gradient.py +++ b/dimos/mapping/occupancy/gradient.py @@ -14,10 +14,11 @@ from typing import TYPE_CHECKING, Literal, TypeAlias, cast +from dimos_generated.nav_msgs.msg import OccupancyGrid import numpy as np from scipy import ndimage -from dimos.msgs.nav_msgs.OccupancyGrid import CostValues, OccupancyGrid +from dimos.msgs.occupancy import occupancy_view if TYPE_CHECKING: from numpy.typing import NDArray @@ -49,19 +50,19 @@ def gradient( """ # Remember which cells are unknown - unknown_mask = occupancy_grid.grid == CostValues.UNKNOWN + unknown_mask = occupancy_view(occupancy_grid) == -1 # Create binary obstacle map # Consider cells >= threshold as obstacles (1), everything else as free (0) # Unknown cells are not considered obstacles for distance calculation - obstacle_map = (occupancy_grid.grid >= obstacle_threshold).astype(np.float32) + obstacle_map = (occupancy_view(occupancy_grid) >= obstacle_threshold).astype(np.float32) # Compute distance transform (distance to nearest obstacle in cells) # Unknown cells are treated as if they don't exist for distance calculation distance_cells = cast("NDArray[np.float64]", ndimage.distance_transform_edt(1 - obstacle_map)) # Convert to meters and clip to max distance - distance_meters = np.clip(distance_cells * occupancy_grid.resolution, 0, max_distance) + distance_meters = np.clip(distance_cells * occupancy_grid.info.resolution, 0, max_distance) # Invert and scale to 0-100 range # Far from obstacles (max_distance) -> 0 @@ -69,21 +70,17 @@ def gradient( gradient_values = (1 - distance_meters / max_distance) * 100 # Ensure obstacles are exactly 100 - gradient_values[obstacle_map > 0] = CostValues.OCCUPIED + gradient_values[obstacle_map > 0] = 100 # Convert to int8 for OccupancyGrid gradient_data = gradient_values.astype(np.int8) # Preserve unknown cells as unknown (don't apply gradient to them) - gradient_data[unknown_mask] = CostValues.UNKNOWN + gradient_data[unknown_mask] = -1 # Create new OccupancyGrid with gradient gradient_grid = OccupancyGrid( - grid=gradient_data, - resolution=occupancy_grid.resolution, - origin=occupancy_grid.origin, - frame_id=occupancy_grid.frame_id, - ts=occupancy_grid.ts, + header=occupancy_grid.header, info=occupancy_grid.info, data=gradient_data.ravel() ) return gradient_grid @@ -122,22 +119,18 @@ def voronoi_gradient( - 100: At obstacles """ # Remember which cells are unknown - unknown_mask = occupancy_grid.grid == CostValues.UNKNOWN + unknown_mask = occupancy_view(occupancy_grid) == -1 # Create binary obstacle map - obstacle_map = (occupancy_grid.grid >= obstacle_threshold).astype(np.float32) + obstacle_map = (occupancy_view(occupancy_grid) >= obstacle_threshold).astype(np.float32) # Check if there are any obstacles if not np.any(obstacle_map): # No obstacles - everything is free - gradient_data = np.zeros_like(occupancy_grid.grid, dtype=np.int8) - gradient_data[unknown_mask] = CostValues.UNKNOWN + gradient_data = np.zeros_like(occupancy_view(occupancy_grid), dtype=np.int8) + gradient_data[unknown_mask] = -1 return OccupancyGrid( - grid=gradient_data, - resolution=occupancy_grid.resolution, - origin=occupancy_grid.origin, - frame_id=occupancy_grid.frame_id, - ts=occupancy_grid.ts, + header=occupancy_grid.header, info=occupancy_grid.info, data=gradient_data.ravel() ) # Label connected obstacle regions (clusters) @@ -189,22 +182,18 @@ def voronoi_gradient( gradient_values = cost_ratio * 99 # Ensure obstacles are exactly 100 - gradient_values[obstacle_map > 0] = CostValues.OCCUPIED + gradient_values[obstacle_map > 0] = 100 # Apply max_distance clipping - cells beyond max_distance from obstacles get cost 0 - max_distance_cells = max_distance / occupancy_grid.resolution + max_distance_cells = max_distance / occupancy_grid.info.resolution gradient_values[distance_cells > max_distance_cells] = 0 # Convert to int8 gradient_data = gradient_values.astype(np.int8) # Preserve unknown cells - gradient_data[unknown_mask] = CostValues.UNKNOWN + gradient_data[unknown_mask] = -1 return OccupancyGrid( - grid=gradient_data, - resolution=occupancy_grid.resolution, - origin=occupancy_grid.origin, - frame_id=occupancy_grid.frame_id, - ts=occupancy_grid.ts, + header=occupancy_grid.header, info=occupancy_grid.info, data=gradient_data.ravel() ) diff --git a/dimos/mapping/occupancy/inflation.py b/dimos/mapping/occupancy/inflation.py index 610157ddf62..f7264039912 100644 --- a/dimos/mapping/occupancy/inflation.py +++ b/dimos/mapping/occupancy/inflation.py @@ -12,10 +12,14 @@ # See the License for the specific language governing permissions and # limitations under the License. +from copy import deepcopy +import math + +from dimos_generated.nav_msgs.msg import OccupancyGrid import numpy as np from scipy import ndimage -from dimos.msgs.nav_msgs.OccupancyGrid import CostValues, OccupancyGrid +from dimos.msgs.occupancy import occupancy_extent, occupancy_view def simple_inflate(occupancy_grid: OccupancyGrid, radius: float) -> OccupancyGrid: @@ -25,29 +29,28 @@ def simple_inflate(occupancy_grid: OccupancyGrid, radius: float) -> OccupancyGri Returns: New OccupancyGrid with inflated obstacles """ + occupancy_extent(occupancy_grid) + if not math.isfinite(radius) or radius < 0: + raise ValueError("inflation radius must be finite and nonnegative") + # Convert radius to grid cells - cell_radius = int(np.ceil(radius / occupancy_grid.resolution)) + cell_radius = int(np.ceil(radius / occupancy_grid.info.resolution)) # Get grid as numpy array - grid_array = occupancy_grid.grid + grid_array = occupancy_view(occupancy_grid) # Create circular kernel for binary inflation y, x = np.ogrid[-cell_radius : cell_radius + 1, -cell_radius : cell_radius + 1] kernel = (x**2 + y**2 <= cell_radius**2).astype(np.uint8) # Find occupied cells - occupied_mask = grid_array >= CostValues.OCCUPIED + occupied_mask = grid_array >= 100 # Binary inflation inflated = ndimage.binary_dilation(occupied_mask, structure=kernel) result_grid = grid_array.copy() - result_grid[inflated] = CostValues.OCCUPIED - - # Create new OccupancyGrid with inflated data using numpy constructor - return OccupancyGrid( - grid=result_grid, - resolution=occupancy_grid.resolution, - origin=occupancy_grid.origin, - frame_id=occupancy_grid.frame_id, - ts=occupancy_grid.ts, - ) + result_grid[inflated] = 100 + + result = deepcopy(occupancy_grid) + result.data = result_grid.ravel() + return result diff --git a/dimos/mapping/occupancy/operations.py b/dimos/mapping/occupancy/operations.py index 8609ac634b3..3771bcae436 100644 --- a/dimos/mapping/occupancy/operations.py +++ b/dimos/mapping/occupancy/operations.py @@ -12,10 +12,11 @@ # See the License for the specific language governing permissions and # limitations under the License. +from dimos_generated.nav_msgs.msg import OccupancyGrid import numpy as np from scipy import ndimage -from dimos.msgs.nav_msgs.OccupancyGrid import CostValues, OccupancyGrid +from dimos.msgs.occupancy import occupancy_view def smooth_occupied( @@ -33,8 +34,8 @@ def smooth_occupied( Returns: New OccupancyGrid with smoothed occupied zones """ - grid_array = occupancy_grid.grid - occupied_mask = grid_array >= CostValues.OCCUPIED + grid_array = occupancy_view(occupancy_grid) + occupied_mask = grid_array >= 100 # Count occupied neighbors for each cell (8-connectivity). kernel = np.array([[1, 1, 1], [1, 0, 1], [1, 1, 1]], dtype=np.uint8) @@ -47,14 +48,10 @@ def smooth_occupied( unsupported = occupied_mask & (neighbor_count < min_neighbors) result_grid = grid_array.copy() - result_grid[unsupported] = CostValues.FREE + result_grid[unsupported] = 0 return OccupancyGrid( - grid=result_grid, - resolution=occupancy_grid.resolution, - origin=occupancy_grid.origin, - frame_id=occupancy_grid.frame_id, - ts=occupancy_grid.ts, + header=occupancy_grid.header, info=occupancy_grid.info, data=result_grid.ravel() ) @@ -70,19 +67,13 @@ def overlay_occupied(base: OccupancyGrid, overlay: OccupancyGrid) -> OccupancyGr Returns: New OccupancyGrid with combined occupied zones """ - if base.grid.shape != overlay.grid.shape: + if occupancy_view(base).shape != occupancy_view(overlay).shape: raise ValueError( - f"Grid shapes must match: base {base.grid.shape} vs overlay {overlay.grid.shape}" + f"Grid shapes must match: base {occupancy_view(base).shape} vs overlay {occupancy_view(overlay).shape}" ) - result_grid = base.grid.copy() - overlay_occupied_mask = overlay.grid >= CostValues.OCCUPIED - result_grid[overlay_occupied_mask] = CostValues.OCCUPIED + result_grid = occupancy_view(base).copy() + overlay_occupied_mask = occupancy_view(overlay) >= 100 + result_grid[overlay_occupied_mask] = 100 - return OccupancyGrid( - grid=result_grid, - resolution=base.resolution, - origin=base.origin, - frame_id=base.frame_id, - ts=base.ts, - ) + return OccupancyGrid(header=base.header, info=base.info, data=result_grid.ravel()) diff --git a/dimos/mapping/occupancy/path_map.py b/dimos/mapping/occupancy/path_map.py index 69a1f937385..6f5a241c213 100644 --- a/dimos/mapping/occupancy/path_map.py +++ b/dimos/mapping/occupancy/path_map.py @@ -12,11 +12,12 @@ # See the License for the specific language governing permissions and # limitations under the License. +from dimos_generated.nav_msgs.msg import OccupancyGrid + from dimos.mapping.occupancy.gradient import GradientStrategy, gradient, voronoi_gradient from dimos.mapping.occupancy.inflation import simple_inflate from dimos.mapping.occupancy.operations import overlay_occupied, smooth_occupied from dimos.mapping.occupancy.types import NavigationStrategy -from dimos.msgs.nav_msgs.OccupancyGrid import OccupancyGrid def make_navigation_map( diff --git a/dimos/mapping/occupancy/path_mask.py b/dimos/mapping/occupancy/path_mask.py index b71372bbf62..3fb8a4bb4e0 100644 --- a/dimos/mapping/occupancy/path_mask.py +++ b/dimos/mapping/occupancy/path_mask.py @@ -12,11 +12,13 @@ # See the License for the specific language governing permissions and # limitations under the License. +from __future__ import annotations + +from dimos_generated.nav_msgs.msg import OccupancyGrid, Path import numpy as np from numpy.typing import NDArray -from dimos.msgs.nav_msgs.OccupancyGrid import CostValues, OccupancyGrid -from dimos.msgs.nav_msgs.Path import Path +from dimos.msgs.occupancy import occupancy_view, world_to_grid def make_path_mask( @@ -44,9 +46,9 @@ def make_path_mask( """ import cv2 - mask = np.zeros((occupancy_grid.height, occupancy_grid.width), dtype=np.uint8) + mask = np.zeros((occupancy_grid.info.height, occupancy_grid.info.width), dtype=np.uint8) - line_width_pixels = max(1, int(robot_width / occupancy_grid.resolution)) + line_width_pixels = max(1, int(robot_width / occupancy_grid.info.resolution)) poses = path.poses if len(poses) < pose_index + 2: @@ -55,8 +57,8 @@ def make_path_mask( # Draw lines between consecutive points cumulative_length = 0.0 for i in range(pose_index, len(poses) - 1): - pos1 = poses[i].position - pos2 = poses[i + 1].position + pos1 = poses[i].pose.position + pos2 = poses[i + 1].pose.position segment_length = np.sqrt( (pos2.x - pos1.x) ** 2 + (pos2.y - pos1.y) ** 2 + (pos2.z - pos1.z) ** 2 @@ -67,22 +69,22 @@ def make_path_mask( cumulative_length += segment_length - grid_pt1 = occupancy_grid.world_to_grid(pos1) - grid_pt2 = occupancy_grid.world_to_grid(pos2) + grid_pt1 = world_to_grid(occupancy_grid, pos1) + grid_pt2 = world_to_grid(occupancy_grid, pos2) - pt1 = (round(grid_pt1.x), round(grid_pt1.y)) - pt2 = (round(grid_pt2.x), round(grid_pt2.y)) + pt1 = (round(grid_pt1[0]), round(grid_pt1[1])) + pt2 = (round(grid_pt2[0]), round(grid_pt2[1])) cv2.line(mask, pt1, pt2, (255.0,), thickness=line_width_pixels) - bool_mask = mask.astype(np.bool_) + bool_mask: NDArray[np.bool_] = mask.astype(np.bool_) total_points = np.sum(bool_mask) if total_points == 0: return bool_mask - occupied_mask = occupancy_grid.grid >= CostValues.OCCUPIED + occupied_mask = occupancy_view(occupancy_grid) >= 100 occupied_in_path = bool_mask & occupied_mask occupied_count = np.sum(occupied_in_path) diff --git a/dimos/mapping/occupancy/path_resampling.py b/dimos/mapping/occupancy/path_resampling.py index 1f50fad7cdf..277600e4420 100644 --- a/dimos/mapping/occupancy/path_resampling.py +++ b/dimos/mapping/occupancy/path_resampling.py @@ -15,16 +15,13 @@ import math +from dimos_generated.geometry_msgs.msg import Point, Pose, PoseStamped, Quaternion +from dimos_generated.nav_msgs.msg import Path import numpy as np from scipy.ndimage import uniform_filter1d -from dimos.msgs.geometry_msgs.Pose import Pose -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.msgs.nav_msgs.Path import Path +from dimos.msgs.geometry import quaternion_from_euler from dimos.utils.logging_config import setup_logger -from dimos.utils.transform_utils import euler_to_quaternion logger = setup_logger() @@ -48,27 +45,30 @@ def _add_orientations_to_path(path: Path, goal_orientation: Quaternion) -> None: next_pose = path.poses[i + 1] # Calculate direction to next point - dx = next_pose.position.x - current_pose.position.x - dy = next_pose.position.y - current_pose.position.y + dx = next_pose.pose.position.x - current_pose.pose.position.x + dy = next_pose.pose.position.y - current_pose.pose.position.y # Calculate yaw angle yaw = math.atan2(dy, dx) # Convert to quaternion (roll=0, pitch=0, yaw) - orientation = euler_to_quaternion(Vector3(0, 0, yaw)) - current_pose.orientation = orientation + orientation = quaternion_from_euler(0, 0, yaw) + current_pose.pose.orientation = orientation + path.poses[i] = current_pose # Set last pose orientation - identity_quat = Quaternion(0, 0, 0, 1) + identity_quat = Quaternion(w=1) + last_pose = path.poses[-1] if goal_orientation != identity_quat: # Use the provided goal orientation if it's not the identity - path.poses[-1].orientation = goal_orientation + last_pose.pose.orientation = goal_orientation elif len(path.poses) > 1: # Use the previous pose's orientation - path.poses[-1].orientation = path.poses[-2].orientation + last_pose.pose.orientation = path.poses[-2].pose.orientation else: # Single pose with identity goal orientation - path.poses[-1].orientation = identity_quat + last_pose.pose.orientation = identity_quat + path.poses[-1] = last_pose # TODO: replace goal_pose with just goal_orientation @@ -82,7 +82,7 @@ def simple_resample_path(path: Path, goal_pose: Pose, spacing: float) -> Path: Returns: A new Path with resampled poses """ - if len(path) < 2 or spacing <= 0: + if len(path.poses) < 2 or spacing <= 0: return path resampled = [] @@ -95,8 +95,8 @@ def simple_resample_path(path: Path, goal_pose: Pose, spacing: float) -> Path: prev = path.poses[i - 1] # Calculate segment distance - dx = current.x - prev.x - dy = current.y - prev.y + dx = current.pose.position.x - prev.pose.position.x + dy = current.pose.position.y - prev.pose.position.y segment_length = (dx**2 + dy**2) ** 0.5 if segment_length < 1e-10: @@ -114,12 +114,11 @@ def simple_resample_path(path: Path, goal_pose: Pose, spacing: float) -> Path: break # Create new pose - new_x = prev.x + dir_x * dist_along - new_y = prev.y + dir_y * dist_along + new_x = prev.pose.position.x + dir_x * dist_along + new_y = prev.pose.position.y + dir_y * dist_along new_pose = PoseStamped( - frame_id=path.frame_id, - position=[new_x, new_y, 0.0], - orientation=prev.orientation, # Keep same orientation + header=path.header, + pose=Pose(position=Point(x=new_x, y=new_y), orientation=prev.pose.orientation), ) resampled.append(new_pose) @@ -133,10 +132,18 @@ def simple_resample_path(path: Path, goal_pose: Pose, spacing: float) -> Path: # Add last pose if not already there if len(path.poses) > 1: last = path.poses[-1] - if not resampled or (resampled[-1].x != last.x or resampled[-1].y != last.y): + if ( + math.hypot( + resampled[-1].pose.position.x - last.pose.position.x, + resampled[-1].pose.position.y - last.pose.position.y, + ) + <= 1e-10 + ): + resampled[-1] = last + else: resampled.append(last) - ret = Path(frame_id=path.frame_id, poses=resampled) + ret = Path(header=path.header, poses=resampled) _add_orientations_to_path(ret, goal_pose.orientation) @@ -165,21 +172,20 @@ def smooth_resample_path( """ if len(path.poses) == 1: - p = path.poses[0].position + p = path.poses[0].pose.position o = goal_pose.orientation new_pose = PoseStamped( - frame_id=path.frame_id, - position=[p.x, p.y, p.z], - orientation=[o.x, o.y, o.z, o.w], + header=path.header, + pose=Pose(position=p, orientation=o), ) - return Path(frame_id=path.frame_id, poses=[new_pose]) + return Path(header=path.header, poses=[new_pose]) - if len(path) < 2 or spacing <= 0: + if len(path.poses) < 2 or spacing <= 0: return path # Extract x, y coordinates from path - xs = np.array([p.x for p in path.poses]) - ys = np.array([p.y for p in path.poses]) + xs = np.array([p.pose.position.x for p in path.poses]) + ys = np.array([p.pose.position.y for p in path.poses]) # Remove duplicate consecutive points diffs = np.sqrt(np.diff(xs) ** 2 + np.diff(ys) ** 2) @@ -246,13 +252,15 @@ def smooth_resample_path( resampled = [] for i in range(len(sampled_x)): new_pose = PoseStamped( - frame_id=path.frame_id, - position=[float(sampled_x[i]), float(sampled_y[i]), 0.0], - orientation=Quaternion(0, 0, 0, 1), + header=path.header, + pose=Pose( + position=Point(x=float(sampled_x[i]), y=float(sampled_y[i])), + orientation=Quaternion(w=1), + ), ) resampled.append(new_pose) - ret = Path(frame_id=path.frame_id, poses=resampled) + ret = Path(header=path.header, poses=resampled) _add_orientations_to_path(ret, goal_pose.orientation) diff --git a/dimos/mapping/occupancy/test_generated_visualizations.py b/dimos/mapping/occupancy/test_generated_visualizations.py new file mode 100644 index 00000000000..a96e03144b8 --- /dev/null +++ b/dimos/mapping/occupancy/test_generated_visualizations.py @@ -0,0 +1,86 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +# Copyright 2026 Dimensional Inc. +# SPDX-License-Identifier: Apache-2.0 + +import math + +from dimos_generated.builtin_interfaces.msg import Time +from dimos_generated.geometry_msgs.msg import Point, Pose, PoseStamped, Quaternion +from dimos_generated.nav_msgs.msg import MapMetaData, OccupancyGrid, Path +from dimos_generated.sensor_msgs.msg import Image +from dimos_generated.std_msgs.msg import Header +import numpy as np +import pytest + +from dimos.mapping.occupancy.visualizations import Palette, visualize_occupancy_grid +from dimos.mapping.occupancy.visualize_path import visualize_path +from dimos.msgs.geometry import quaternion_from_euler +from dimos.msgs.image import image_view + + +@pytest.mark.parametrize("palette", ["rainbow", "turbo"]) +def test_generated_image_preserves_header_and_colors(palette: Palette) -> None: + grid = OccupancyGrid( + header=Header(stamp=Time(sec=1, nanosec=999), frame_id="map"), + info=MapMetaData(width=3, height=1, resolution=1), + data=[-1, 0, 100], + ) + image = Image.decode(visualize_occupancy_grid(grid, palette).encode()) + assert image.header == grid.header + assert image.encoding == "bgr8" + assert image_view(image).shape == (1, 3, 3) + np.testing.assert_array_equal( + image_view(image)[0, 0], [0, 0, 0] if palette == "rainbow" else [28, 24, 34] + ) + assert not np.array_equal(image_view(image)[0, 1], image_view(image)[0, 2]) + + +@pytest.mark.parametrize("footprint", [False, True]) +def test_path_rendering_is_relative_to_rotated_grid(footprint: bool) -> None: + grid = OccupancyGrid( + info=MapMetaData( + width=10, height=10, resolution=1, origin=Pose(orientation=Quaternion(w=1)) + ), + data=[0] * 100, + ) + path = Path( + poses=[ + PoseStamped(pose=Pose(position=Point(x=x, y=3), orientation=Quaternion(w=1))) + for x in [2, 4, 6] + ] + ) + expected = ( + visualize_path(grid, path, 1, 2) + if footprint + else visualize_occupancy_grid(grid, "rainbow", path) + ) + grid.info.origin = Pose( + position=Point(x=20, y=10), orientation=quaternion_from_euler(0, 0, math.pi / 2) + ) + path.poses = [ + PoseStamped( + pose=Pose( + position=Point(x=17, y=10 + x), orientation=quaternion_from_euler(0, 0, math.pi / 2) + ) + ) + for x in [2, 4, 6] + ] + actual = ( + visualize_path(grid, path, 1, 2) + if footprint + else visualize_occupancy_grid(grid, "rainbow", path) + ) + np.testing.assert_array_equal(image_view(actual), image_view(expected)) diff --git a/dimos/mapping/occupancy/test_gradient.py b/dimos/mapping/occupancy/test_gradient.py index a097873aae7..2a328c2141e 100644 --- a/dimos/mapping/occupancy/test_gradient.py +++ b/dimos/mapping/occupancy/test_gradient.py @@ -12,18 +12,19 @@ # See the License for the specific language governing permissions and # limitations under the License. +import cv2 import numpy as np import pytest from dimos.mapping.occupancy.gradient import gradient, voronoi_gradient from dimos.mapping.occupancy.visualizations import visualize_occupancy_grid -from dimos.msgs.sensor_msgs.Image import Image +from dimos.msgs.image import image_view from dimos.utils.data import get_data @pytest.mark.parametrize("method", ["simple", "voronoi"]) def test_gradient(occupancy, method) -> None: - expected = Image.from_file(get_data(f"gradient_{method}.png")) + expected = cv2.imread(str(get_data(f"gradient_{method}.png")), cv2.IMREAD_COLOR) match method: case "simple": @@ -34,4 +35,4 @@ def test_gradient(occupancy, method) -> None: raise ValueError(f"Unknown resampling method: {method}") actual = visualize_occupancy_grid(og, "rainbow") - np.testing.assert_array_equal(actual.data, expected.data) + np.testing.assert_array_equal(image_view(actual), expected) diff --git a/dimos/mapping/occupancy/test_inflation.py b/dimos/mapping/occupancy/test_inflation.py index a30ad413b16..03a502f05d7 100644 --- a/dimos/mapping/occupancy/test_inflation.py +++ b/dimos/mapping/occupancy/test_inflation.py @@ -19,6 +19,7 @@ from dimos.mapping.occupancy.inflation import simple_inflate from dimos.mapping.occupancy.visualizations import visualize_occupancy_grid +from dimos.msgs.image import image_view from dimos.utils.data import get_data @@ -28,4 +29,4 @@ def test_inflation(occupancy) -> None: og = simple_inflate(occupancy, 0.2) result = visualize_occupancy_grid(og, "rainbow") - np.testing.assert_array_equal(result.data, expected) + np.testing.assert_array_equal(image_view(result), expected) diff --git a/dimos/mapping/occupancy/test_operations.py b/dimos/mapping/occupancy/test_operations.py index 89332d0bdd5..56a0b91f5c0 100644 --- a/dimos/mapping/occupancy/test_operations.py +++ b/dimos/mapping/occupancy/test_operations.py @@ -14,11 +14,15 @@ # limitations under the License. +from copy import deepcopy + import cv2 import numpy as np from dimos.mapping.occupancy.operations import overlay_occupied, smooth_occupied from dimos.mapping.occupancy.visualizations import visualize_occupancy_grid +from dimos.msgs.image import image_view +from dimos.msgs.occupancy import occupancy_view from dimos.utils.data import get_data @@ -27,14 +31,16 @@ def test_smooth_occupied(occupancy) -> None: result = visualize_occupancy_grid(smooth_occupied(occupancy), "rainbow") - np.testing.assert_array_equal(result.data, expected) + np.testing.assert_array_equal(image_view(result), expected) def test_overlay_occupied(occupancy) -> None: expected = cv2.imread(get_data("overlay_occupied.png"), cv2.IMREAD_COLOR) - overlay = occupancy.copy() - overlay.grid[50:100, 50:100] = 100 + overlay = deepcopy(occupancy) + cells = occupancy_view(overlay).copy() + cells[50:100, 50:100] = 100 + overlay.data = cells.ravel() result = visualize_occupancy_grid(overlay_occupied(occupancy, overlay), "rainbow") - np.testing.assert_array_equal(result.data, expected) + np.testing.assert_array_equal(image_view(result), expected) diff --git a/dimos/mapping/occupancy/test_path_map.py b/dimos/mapping/occupancy/test_path_map.py index 8928e1ab923..46e03ea9955 100644 --- a/dimos/mapping/occupancy/test_path_map.py +++ b/dimos/mapping/occupancy/test_path_map.py @@ -20,6 +20,7 @@ from dimos.mapping.occupancy.path_map import make_navigation_map from dimos.mapping.occupancy.visualizations import visualize_occupancy_grid +from dimos.msgs.image import image_view from dimos.utils.data import get_data @@ -31,4 +32,4 @@ def test_make_navigation_map(occupancy, strategy) -> None: og = make_navigation_map(occupancy, robot_width, strategy=strategy, gradient_strategy="voronoi") result = visualize_occupancy_grid(og, "rainbow") - np.testing.assert_array_equal(result.data, expected) + np.testing.assert_array_equal(image_view(result), expected) diff --git a/dimos/mapping/occupancy/test_path_mask.py b/dimos/mapping/occupancy/test_path_mask.py index f566af2a23e..f39df5a58d8 100644 --- a/dimos/mapping/occupancy/test_path_mask.py +++ b/dimos/mapping/occupancy/test_path_mask.py @@ -13,15 +13,15 @@ # limitations under the License. +import cv2 +from dimos_generated.geometry_msgs.msg import Point, Pose, Quaternion import numpy as np import pytest from dimos.mapping.occupancy.path_mask import make_path_mask from dimos.mapping.occupancy.path_resampling import smooth_resample_path from dimos.mapping.occupancy.visualizations import visualize_occupancy_grid -from dimos.msgs.geometry_msgs.Pose import Pose -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.msgs.sensor_msgs.Image import Image +from dimos.msgs.image import image_view from dimos.navigation.replanning_a_star.min_cost_astar import min_cost_astar from dimos.utils.data import get_data @@ -34,15 +34,16 @@ ], ) def test_make_path_mask(occupancy_gradient, pose_index, max_length, expected_image) -> None: - start = Vector3(4.0, 2.0, 0) - goal_pose = Pose(6.15, 10.0, 0, 0, 0, 0, 1) - expected = Image.from_file(get_data(expected_image)) + start = Point(x=4, y=2) + goal_pose = Pose(position=Point(x=6.15, y=10), orientation=Quaternion(w=1)) + expected = cv2.imread(str(get_data(expected_image)), cv2.IMREAD_COLOR) path = min_cost_astar(occupancy_gradient, goal_pose.position, start, use_cpp=False) path = smooth_resample_path(path, goal_pose, 0.1) robot_width = 0.4 path_mask = make_path_mask(occupancy_gradient, path, robot_width, pose_index, max_length) actual = visualize_occupancy_grid(occupancy_gradient, "rainbow") - actual.data[path_mask] = [0, 100, 0] + pixels = image_view(actual).copy() + pixels[path_mask] = [0, 100, 0] - np.testing.assert_array_equal(actual.data, expected.data) + np.testing.assert_array_equal(pixels, expected) diff --git a/dimos/mapping/occupancy/test_path_resampling.py b/dimos/mapping/occupancy/test_path_resampling.py index aeda7d11ad5..24e1af231ef 100644 --- a/dimos/mapping/occupancy/test_path_resampling.py +++ b/dimos/mapping/occupancy/test_path_resampling.py @@ -12,30 +12,42 @@ # See the License for the specific language governing permissions and # limitations under the License. +import cv2 +from dimos_generated.geometry_msgs.msg import Point, Pose, Quaternion +from dimos_generated.nav_msgs.msg import MapMetaData, OccupancyGrid import numpy as np import pytest from dimos.mapping.occupancy.gradient import gradient from dimos.mapping.occupancy.path_resampling import simple_resample_path, smooth_resample_path from dimos.mapping.occupancy.visualize_path import visualize_path -from dimos.msgs.geometry_msgs.Pose import Pose -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.msgs.nav_msgs.OccupancyGrid import OccupancyGrid -from dimos.msgs.sensor_msgs.Image import Image +from dimos.msgs.image import image_view from dimos.navigation.replanning_a_star.min_cost_astar import min_cost_astar from dimos.utils.data import get_data +def _grid(cells, resolution=0.05): + return OccupancyGrid( + info=MapMetaData( + width=cells.shape[1], + height=cells.shape[0], + resolution=resolution, + origin=Pose(orientation=Quaternion(w=1)), + ), + data=cells.astype(np.int8).ravel(), + ) + + @pytest.fixture def costmap() -> OccupancyGrid: - return gradient(OccupancyGrid(np.load(get_data("occupancy_simple.npy"))), max_distance=1.5) + return gradient(_grid(np.load(get_data("occupancy_simple.npy"))), max_distance=1.5) @pytest.mark.parametrize("method", ["simple", "smooth"]) def test_resample_path(costmap, method) -> None: - start = Vector3(4.0, 2.0, 0) - goal_pose = Pose(6.15, 10.0, 0, 0, 0, 0, 1) - expected = Image.from_file(get_data(f"resample_path_{method}.png")) + start = Point(x=4, y=2) + goal_pose = Pose(position=Point(x=6.15, y=10), orientation=Quaternion(w=1)) + expected = cv2.imread(str(get_data(f"resample_path_{method}.png")), cv2.IMREAD_COLOR) path = min_cost_astar(costmap, goal_pose.position, start, use_cpp=False) match method: @@ -47,4 +59,4 @@ def test_resample_path(costmap, method) -> None: raise ValueError(f"Unknown resampling method: {method}") actual = visualize_path(costmap, resampled, 0.2, 0.4) - np.testing.assert_array_equal(actual.data, expected.data) + np.testing.assert_array_equal(image_view(actual), expected) diff --git a/dimos/mapping/occupancy/test_visualizations.py b/dimos/mapping/occupancy/test_visualizations.py index 17b2629e802..e0066c86df7 100644 --- a/dimos/mapping/occupancy/test_visualizations.py +++ b/dimos/mapping/occupancy/test_visualizations.py @@ -19,6 +19,7 @@ import pytest from dimos.mapping.occupancy.visualizations import visualize_occupancy_grid +from dimos.msgs.image import image_view from dimos.utils.data import get_data @@ -28,4 +29,4 @@ def test_visualize_occupancy_grid(occupancy_gradient, palette) -> None: result = visualize_occupancy_grid(occupancy_gradient, palette) - np.testing.assert_array_equal(result.data, expected) + np.testing.assert_array_equal(image_view(result), expected) diff --git a/dimos/mapping/occupancy/visualizations.py b/dimos/mapping/occupancy/visualizations.py index 560707ccae4..dcb2a75f574 100644 --- a/dimos/mapping/occupancy/visualizations.py +++ b/dimos/mapping/occupancy/visualizations.py @@ -12,16 +12,20 @@ # See the License for the specific language governing permissions and # limitations under the License. +from __future__ import annotations + from functools import lru_cache from typing import Literal, TypeAlias +from dimos_generated.nav_msgs.msg import OccupancyGrid, Path +from dimos_generated.sensor_msgs.msg import Image import matplotlib.pyplot as plt import numpy as np from numpy.typing import NDArray -from dimos.msgs.nav_msgs.OccupancyGrid import OccupancyGrid -from dimos.msgs.nav_msgs.Path import Path -from dimos.msgs.sensor_msgs.Image import Image, ImageFormat +from dimos.msgs.geometry import pose_matrix +from dimos.msgs.image import image_from_array +from dimos.msgs.occupancy import occupancy_extent, occupancy_view Palette: TypeAlias = Literal["rainbow", "turbo"] @@ -31,33 +35,29 @@ def visualize_occupancy_grid( ) -> Image: match palette: case "rainbow": - bgr_image = rainbow_image(occupancy_grid.grid) + bgr_image = rainbow_image(occupancy_view(occupancy_grid)) case "turbo": - bgr_image = turbo_image(occupancy_grid.grid) + bgr_image = turbo_image(occupancy_view(occupancy_grid)) case _: raise NotImplementedError() if path is not None and len(path.poses) > 0: _draw_path(occupancy_grid, bgr_image, path) - return Image( - data=bgr_image, - format=ImageFormat.BGR, - frame_id=occupancy_grid.frame_id, - ts=occupancy_grid.ts, - ) + return image_from_array(bgr_image, encoding="bgr8", header=occupancy_grid.header) def _draw_path(occupancy_grid: OccupancyGrid, bgr_image: NDArray[np.uint8], path: Path) -> None: import cv2 + occupancy_extent(occupancy_grid) + world_to_grid = np.linalg.inv(pose_matrix(occupancy_grid.info.origin)) points = [] for pose in path.poses: - grid_coord = occupancy_grid.world_to_grid([pose.x, pose.y, pose.z]) - pixel_x = int(grid_coord.x) - pixel_y = int(grid_coord.y) - - if 0 <= pixel_x < occupancy_grid.width and 0 <= pixel_y < occupancy_grid.height: + position = pose.pose.position + local = world_to_grid @ np.array([position.x, position.y, position.z, 1.0]) + pixel_x, pixel_y = np.floor(local[:2] / occupancy_grid.info.resolution).astype(int) + if 0 <= pixel_x < occupancy_grid.info.width and 0 <= pixel_y < occupancy_grid.info.height: points.append((pixel_x, pixel_y)) if len(points) > 1: @@ -140,7 +140,7 @@ def _interpolate_turbo(t: float) -> tuple[int, int, int]: def generate_rgba_texture( - grid: OccupancyGrid, + grid: NDArray[np.int8], colormap: str | None = None, opacity: float = 1.0, cost_range: tuple[int, int] | None = None, @@ -149,7 +149,7 @@ def generate_rgba_texture( """Generate RGBA texture for an occupancy grid. Args: - grid: OccupancyGrid to render. + grid: NDArray[np.int8] to render. colormap: Optional matplotlib colormap name. opacity: Blend factor (0.0 to 1.0). Blends towards background color. cost_range: Optional (min, max) cost range. Cells outside range use background. @@ -166,18 +166,18 @@ def generate_rgba_texture( bg_rgb = np.array([0, 0, 0], dtype=np.float32) if cost_range is not None: - in_range_mask = (grid.grid >= cost_range[0]) & (grid.grid <= cost_range[1]) + in_range_mask = (grid >= cost_range[0]) & (grid <= cost_range[1]) else: in_range_mask = None if colormap is not None: cmap = plt.get_cmap(colormap) - grid_float = grid.grid.astype(np.float32) + grid_float = grid.astype(np.float32) - vis = np.zeros((grid.height, grid.width, 4), dtype=np.uint8) + vis = np.zeros((*grid.shape, 4), dtype=np.uint8) - free_mask = grid.grid == 0 - occupied_mask = grid.grid > 0 + free_mask = grid == 0 + occupied_mask = grid > 0 if np.any(free_mask): fg = np.array(cmap(0.0)[:3]) * 255 @@ -193,21 +193,21 @@ def generate_rgba_texture( vis[occupied_mask, :3] = blended.astype(np.uint8) vis[occupied_mask, 3] = 255 - unknown_mask = grid.grid == -1 + unknown_mask = grid == -1 vis[unknown_mask] = 0 if in_range_mask is not None: - out_of_range = ~in_range_mask & (grid.grid != -1) + out_of_range = ~in_range_mask & (grid != -1) vis[out_of_range, :3] = bg_rgb.astype(np.uint8) vis[out_of_range, 3] = 255 return vis # Default: Foxglove-style coloring - vis = np.zeros((grid.height, grid.width, 4), dtype=np.uint8) + vis = np.zeros((*grid.shape, 4), dtype=np.uint8) - free_mask = grid.grid == 0 - occupied_mask = grid.grid > 0 + free_mask = grid == 0 + occupied_mask = grid > 0 fg_free = np.array([72, 73, 129], dtype=np.float32) blended_free = fg_free * opacity + bg_rgb * (1 - opacity) @@ -215,18 +215,18 @@ def generate_rgba_texture( vis[free_mask, 3] = 255 if np.any(occupied_mask): - costs = grid.grid[occupied_mask].astype(np.float32) + costs = grid[occupied_mask].astype(np.float32) factor = (1 - costs / 100).clip(0, 1) fg_occ = np.column_stack([72 * factor, 73 * factor, 129 * factor]) blended_occ = fg_occ * opacity + bg_rgb * (1 - opacity) vis[occupied_mask, :3] = blended_occ.astype(np.uint8) vis[occupied_mask, 3] = 255 - unknown_mask = grid.grid == -1 + unknown_mask = grid == -1 vis[unknown_mask] = 0 if in_range_mask is not None: - out_of_range = ~in_range_mask & (grid.grid != -1) + out_of_range = ~in_range_mask & (grid != -1) vis[out_of_range, :3] = bg_rgb.astype(np.uint8) vis[out_of_range, 3] = 255 diff --git a/dimos/mapping/occupancy/visualize_path.py b/dimos/mapping/occupancy/visualize_path.py index 4d3de0f0a0e..10467b3ba6b 100644 --- a/dimos/mapping/occupancy/visualize_path.py +++ b/dimos/mapping/occupancy/visualize_path.py @@ -12,12 +12,16 @@ # See the License for the specific language governing permissions and # limitations under the License. +from __future__ import annotations + +from dimos_generated.nav_msgs.msg import OccupancyGrid, Path +from dimos_generated.sensor_msgs.msg import Image import numpy as np from dimos.mapping.occupancy.visualizations import visualize_occupancy_grid -from dimos.msgs.nav_msgs.OccupancyGrid import OccupancyGrid -from dimos.msgs.nav_msgs.Path import Path -from dimos.msgs.sensor_msgs.Image import Image, ImageFormat +from dimos.msgs.geometry import pose_matrix +from dimos.msgs.image import image_from_array, image_view +from dimos.msgs.occupancy import occupancy_extent def visualize_path( @@ -31,7 +35,7 @@ def visualize_path( import cv2 image = visualize_occupancy_grid(occupancy_grid, "rainbow") - bgr = image.data + bgr = image_view(image) bgr = cv2.resize( bgr, @@ -40,19 +44,21 @@ def visualize_path( ) # Convert robot dimensions from meters to grid cells, then to scaled pixels - resolution = occupancy_grid.resolution + occupancy_extent(occupancy_grid) + resolution = occupancy_grid.info.resolution + world_to_grid = np.linalg.inv(pose_matrix(occupancy_grid.info.origin)) robot_width_px = int((robot_width / resolution) * scale) robot_length_px = int((robot_length / resolution) * scale) # Draw robot rectangle at each path point for pose in path.poses: # Convert world coordinates to grid coordinates - grid_coord = occupancy_grid.world_to_grid([pose.x, pose.y, pose.z]) - cx = int(grid_coord.x * scale) - cy = int(grid_coord.y * scale) + local = world_to_grid @ pose_matrix(pose.pose) + cx = int(local[0, 3] / resolution * scale) + cy = int(local[1, 3] / resolution * scale) - # Get yaw angle from pose orientation - yaw = pose.yaw + # Get heading relative to the grid's origin orientation. + yaw = np.arctan2(local[1, 0], local[0, 0]) # Define rectangle corners centered at origin (length along x, width along y) half_length = robot_length_px / 2 @@ -81,9 +87,4 @@ def visualize_path( pts = rotated_corners.astype(np.int32).reshape((-1, 1, 2)) cv2.polylines(bgr, [pts], isClosed=True, color=(0, 0, 0), thickness=thickness) - return Image( - data=bgr, - format=ImageFormat.BGR, - frame_id=occupancy_grid.frame_id, - ts=occupancy_grid.ts, - ) + return image_from_array(bgr, encoding="bgr8", header=occupancy_grid.header) diff --git a/dimos/mapping/odometry_hist.py b/dimos/mapping/odometry_hist.py index f368a7fed1b..eb2594007ae 100644 --- a/dimos/mapping/odometry_hist.py +++ b/dimos/mapping/odometry_hist.py @@ -15,21 +15,25 @@ from __future__ import annotations from collections import deque +from copy import deepcopy import math from typing import Any +from dimos_generated.geometry_msgs.msg import PoseStamped +from dimos_generated.nav_msgs.msg import Odometry, Path from pydantic import Field from dimos.core.module import Module, ModuleConfig from dimos.core.stream import In, Out -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.nav_msgs.Odometry import Odometry -from dimos.msgs.nav_msgs.Path import Path +from dimos.msgs.time import to_nanoseconds def path_at_true_height(path: Path) -> Any: """The default z lift clears a costmap a bare odometry demo has none of.""" - return path.to_rerun(z_offset=0.0, radii=0.02) + import rerun as rr + + points = [[p.pose.position.x, p.pose.position.y, p.pose.position.z] for p in path.poses] + return rr.LineStrips3D([points] if points else [], colors=[(0, 255, 128)], radii=0.02) class OdometryHistConfig(ModuleConfig): @@ -51,36 +55,37 @@ def __init__(self, **kwargs: Any) -> None: super().__init__(**kwargs) self._poses: deque[PoseStamped] = deque(maxlen=self.config.max_poses) # None, not 0.0: a recording stamped from zero would suppress the first path. - self._last_publish_ts: float | None = None + self._last_publish_ns: int | None = None self._unpublished = False async def handle_odometry(self, msg: Odometry) -> None: - position = msg.pose.position + position = msg.pose.pose.position point = (position.x, position.y, position.z) - frame_id = self.config.frame_id or msg.frame_id + frame_id = self.config.frame_id or msg.header.frame_id previous = self._poses[-1] if self._poses else None if ( previous is None - or math.dist((previous.x, previous.y, previous.z), point) >= self.config.min_step_meters - ): - orientation = msg.pose.orientation - self._poses.append( - PoseStamped( - ts=msg.ts, - frame_id=frame_id, - position=list(point), - orientation=[orientation.x, orientation.y, orientation.z, orientation.w], - ) + or math.dist( + (previous.pose.position.x, previous.pose.position.y, previous.pose.position.z), + point, ) + >= self.config.min_step_meters + ): + header = deepcopy(msg.header) + header.frame_id = frame_id + self._poses.append(PoseStamped(header=header, pose=msg.pose.pose)) self._unpublished = True if not self._unpublished: return - if self._last_publish_ts is not None: - elapsed = msg.ts - self._last_publish_ts + stamp_ns = to_nanoseconds(msg.header.stamp) + if self._last_publish_ns is not None: + elapsed = stamp_ns - self._last_publish_ns # A replay restart moves the stamp backwards; publish rather than wait it out. - if 0.0 <= elapsed < self.config.min_publish_interval_seconds: + if 0 <= elapsed < round(self.config.min_publish_interval_seconds * 1_000_000_000): return - self._last_publish_ts = msg.ts + self._last_publish_ns = stamp_ns self._unpublished = False - self.odom_hist.publish(Path(ts=msg.ts, frame_id=frame_id, poses=list(self._poses))) + header = deepcopy(msg.header) + header.frame_id = frame_id + self.odom_hist.publish(Path(header=header, poses=list(self._poses))) diff --git a/dimos/mapping/osm/current_location_map.py b/dimos/mapping/osm/current_location_map.py index d573370a068..7e47d0473b8 100644 --- a/dimos/mapping/osm/current_location_map.py +++ b/dimos/mapping/osm/current_location_map.py @@ -12,12 +12,16 @@ # See the License for the specific language governing permissions and # limitations under the License. +from dataclasses import replace + +import numpy as np from PIL import Image as PILImage, ImageDraw from dimos.mapping.models import LatLon from dimos.mapping.osm.osm import MapImage, get_osm_map from dimos.mapping.osm.query import query_for_one_position, query_for_one_position_and_context from dimos.models.vl.base import VlModel +from dimos.msgs.image import image_from_array, image_to_rgb from dimos.utils.logging_config import setup_logger logger = setup_logger() @@ -70,11 +74,9 @@ def _fetch_new_map(self) -> None: self._map_image = get_osm_map(self._position, self._zoom_level, self._n_tiles) # type: ignore[arg-type] # Add position marker - import numpy as np - assert self._map_image is not None assert self._position is not None - pil_image = PILImage.fromarray(self._map_image.image.data) + pil_image = PILImage.fromarray(image_to_rgb(self._map_image.image)) draw = ImageDraw.Draw(pil_image) x, y = self._map_image.latlon_to_pixel(self._position) radius = 20 @@ -85,7 +87,12 @@ def _fetch_new_map(self) -> None: width=3, ) - self._map_image.image.data[:] = np.array(pil_image) + self._map_image = replace( + self._map_image, + image=image_from_array( + np.asarray(pil_image), encoding="rgb8", header=self._map_image.image.header + ), + ) def _position_is_too_far_off_center(self) -> bool: x, y = self._map_image.latlon_to_pixel(self._position) # type: ignore[arg-type, union-attr] @@ -108,6 +115,6 @@ def save_current_map_image(self, filepath: str = "osm_debug_map.png") -> str: self._get_current_map() # type: ignore[no-untyped-call] if self._map_image is not None: - self._map_image.image.save(filepath) + PILImage.fromarray(image_to_rgb(self._map_image.image)).save(filepath) logger.info(f"Saved OSM map image to {filepath}") return filepath diff --git a/dimos/mapping/osm/osm.py b/dimos/mapping/osm/osm.py index 505da875a8b..51350b98295 100644 --- a/dimos/mapping/osm/osm.py +++ b/dimos/mapping/osm/osm.py @@ -17,12 +17,13 @@ import io import math +from dimos_generated.sensor_msgs.msg import Image import numpy as np from PIL import Image as PILImage import requests from dimos.mapping.models import ImageCoord, LatLon -from dimos.msgs.sensor_msgs.Image import Image, ImageFormat +from dimos.msgs.image import image_from_array @dataclass(frozen=True) @@ -176,7 +177,7 @@ def get_osm_map(position: LatLon, zoom_level: int = 18, n_tiles: int = 4) -> Map raise ValueError("Failed to download all tiles for the requested map.") return MapImage( - image=Image.from_numpy(np.array(output_img), format=ImageFormat.RGB), + image=image_from_array(np.array(output_img), encoding="rgb8"), position=position, zoom_level=zoom_level, n_tiles=n_tiles, diff --git a/dimos/mapping/osm/test_current_location_map.py b/dimos/mapping/osm/test_current_location_map.py new file mode 100644 index 00000000000..05e35896c24 --- /dev/null +++ b/dimos/mapping/osm/test_current_location_map.py @@ -0,0 +1,56 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from pathlib import Path +from unittest.mock import MagicMock + +from dimos_generated.builtin_interfaces.msg import Time +from dimos_generated.std_msgs.msg import Header +import numpy as np +from PIL import Image as PILImage +import pytest + +from dimos.mapping.models import LatLon +from dimos.mapping.osm import current_location_map +from dimos.mapping.osm.current_location_map import CurrentLocationMap +from dimos.mapping.osm.osm import MapImage +from dimos.models.vl.base import VlModel +from dimos.msgs.image import image_from_array, image_to_rgb + + +def test_position_marker_preserves_source_stamp_color_and_cached_tile( + monkeypatch: pytest.MonkeyPatch, tmp_path: Path +) -> None: + position = LatLon(lat=0, lon=0) + pixels = np.full((256, 256, 3), [0, 10, 30], dtype=np.uint8) + source = image_from_array( + pixels, encoding="bgr8", header=Header(frame_id="map", stamp=Time(sec=123, nanosec=456)) + ) + tile = MapImage(source, position, 0, 1) + monkeypatch.setattr(current_location_map, "get_osm_map", lambda *_args: tile) + current = CurrentLocationMap(MagicMock(spec=VlModel)) + current.update_position(position) + current._fetch_new_map() + assert current._map_image is not None + image = current._map_image.image + assert image.header == source.header + assert image.encoding == "rgb8" + x, y = tile.latlon_to_pixel(position) + np.testing.assert_array_equal(image_to_rgb(image)[y, x], [255, 0, 0]) + np.testing.assert_array_equal(image_to_rgb(image)[0, 0], [30, 10, 0]) + np.testing.assert_array_equal(image_to_rgb(source)[y, x], [30, 10, 0]) + output = tmp_path / "map.png" + assert current.save_current_map_image(str(output)) == str(output) + with PILImage.open(output) as saved: + np.testing.assert_array_equal(np.asarray(saved), image_to_rgb(image)) diff --git a/dimos/mapping/pointclouds/demo.py b/dimos/mapping/pointclouds/demo.py index 387a32a22af..78daaf0db15 100644 --- a/dimos/mapping/pointclouds/demo.py +++ b/dimos/mapping/pointclouds/demo.py @@ -17,6 +17,9 @@ from typing import TYPE_CHECKING +from dimos_generated.nav_msgs.msg import OccupancyGrid +from dimos_generated.std_msgs.msg import Header +import numpy as np import typer from dimos.mapping.occupancy.gradient import gradient @@ -27,8 +30,8 @@ read_pointcloud, visualize, ) -from dimos.msgs.nav_msgs.OccupancyGrid import OccupancyGrid -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 +from dimos.msgs.image import image_to_bgr +from dimos.msgs.pointcloud import pointcloud_from_xyz from dimos.utils.data import get_data if TYPE_CHECKING: @@ -46,7 +49,7 @@ def _get_occupancy_grid() -> OccupancyGrid: min_height = 0.15 max_height = 0.6 occupancygrid = simple_occupancy( - PointCloud2(_get_sum_map()), + pointcloud_from_xyz(np.asarray(_get_sum_map().points), header=Header(frame_id="world")), resolution=resolution, min_height=min_height, max_height=max_height, @@ -57,15 +60,15 @@ def _get_occupancy_grid() -> OccupancyGrid: def _show_occupancy_grid(og: OccupancyGrid) -> None: import cv2 - cost_map = visualize_occupancy_grid(og, "turbo").to_opencv() - cost_map = cv2.flip(cost_map, 0) + cost_map = image_to_bgr(visualize_occupancy_grid(og, "turbo")) + flipped = cv2.flip(cost_map, 0) # Resize to make the image larger (scale by 4x) - height, width = cost_map.shape[:2] - cost_map = cv2.resize(cost_map, (width * 4, height * 4), interpolation=cv2.INTER_NEAREST) + height, width = flipped.shape[:2] + resized = cv2.resize(flipped, (width * 4, height * 4), interpolation=cv2.INTER_NEAREST) cv2.namedWindow("Occupancy Grid", cv2.WINDOW_NORMAL) - cv2.imshow("Occupancy Grid", cost_map) + cv2.imshow("Occupancy Grid", resized) cv2.waitKey(0) cv2.destroyAllWindows() diff --git a/dimos/mapping/pointclouds/occupancy.py b/dimos/mapping/pointclouds/occupancy.py index 2884cacec39..1389cad7c9f 100644 --- a/dimos/mapping/pointclouds/occupancy.py +++ b/dimos/mapping/pointclouds/occupancy.py @@ -14,15 +14,19 @@ from __future__ import annotations +from copy import deepcopy from dataclasses import dataclass +import math from typing import TYPE_CHECKING, Any, Protocol, TypeVar +from dimos_generated.geometry_msgs.msg import Pose, Quaternion +from dimos_generated.nav_msgs.msg import MapMetaData, OccupancyGrid +from dimos_generated.sensor_msgs.msg import PointCloud2 from numba import njit, prange # type: ignore[import-untyped] import numpy as np from scipy import ndimage -from dimos.msgs.geometry_msgs.Pose import Pose -from dimos.msgs.nav_msgs.OccupancyGrid import OccupancyGrid +from dimos.msgs.pointcloud import pointcloud_xyz if TYPE_CHECKING: from numpy.typing import NDArray @@ -99,8 +103,6 @@ def _simple_occupancy_kernel( if TYPE_CHECKING: from collections.abc import Callable - from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 - @dataclass(frozen=True) class OccupancyConfig: @@ -109,6 +111,31 @@ class OccupancyConfig: resolution: float = 0.05 frame_id: str | None = None + def __post_init__(self) -> None: + if not math.isfinite(self.resolution) or self.resolution <= 0: + raise ValueError("occupancy resolution must be finite and positive") + + +def _grid_message( + cloud: PointCloud2, + config: OccupancyConfig, + cells: NDArray[np.int8], + origin: Pose | None = None, +) -> OccupancyGrid: + header = deepcopy(cloud.header) + if config.frame_id is not None: + header.frame_id = config.frame_id + return OccupancyGrid( + header=header, + info=MapMetaData( + resolution=config.resolution, + width=cells.shape[1], + height=cells.shape[0], + origin=origin if origin is not None else Pose(orientation=Quaternion(w=1.0)), + ), + data=cells.ravel(), + ) + ConfigT = TypeVar("ConfigT", bound=OccupancyConfig, covariant=True) @@ -156,17 +183,11 @@ def height_cost_occupancy(cloud: PointCloud2, **kwargs: Any) -> OccupancyGrid: OccupancyGrid with costs 0-100 based on terrain slope, -1 for unknown """ cfg = HeightCostConfig(**kwargs) - points, _ = cloud.as_numpy() - points = points.astype(np.float64) # Upcast to avoid float32 rounding - ts = cloud.ts if hasattr(cloud, "ts") and cloud.ts is not None else 0.0 + points = pointcloud_xyz(cloud) + points = points[np.isfinite(points).all(axis=1)] if len(points) == 0: - return OccupancyGrid( - width=1, - height=1, - resolution=cfg.resolution, - frame_id=cfg.frame_id or cloud.frame_id, - ) + return _grid_message(cloud, cfg, np.full((1, 1), -1, dtype=np.int8)) # Find bounds of the point cloud in X-Y plane (use all points) min_x = np.min(points[:, 0]) @@ -275,13 +296,7 @@ def height_cost_occupancy(cloud: PointCloud2, **kwargs: Any) -> OccupancyGrid: else: cost = np.full((height, width), -1, dtype=np.int8) - return OccupancyGrid( - grid=cost, - resolution=cfg.resolution, - origin=origin, - frame_id=cfg.frame_id or cloud.frame_id, - ts=ts, - ) + return _grid_message(cloud, cfg, cost, origin) @dataclass(frozen=True) @@ -306,16 +321,11 @@ def general_occupancy(cloud: PointCloud2, **kwargs: Any) -> OccupancyGrid: OccupancyGrid with occupied cells where points were projected """ cfg = GeneralOccupancyConfig(**kwargs) - points, _ = cloud.as_numpy() - points = points.astype(np.float64) # Upcast to avoid float32 rounding + points = pointcloud_xyz(cloud) + points = points[np.isfinite(points).all(axis=1)] if len(points) == 0: - return OccupancyGrid( - width=1, - height=1, - resolution=cfg.resolution, - frame_id=cfg.frame_id or cloud.frame_id, - ) + return _grid_message(cloud, cfg, np.full((1, 1), -1, dtype=np.int8)) # Filter points by height for obstacles obstacle_mask = (points[:, 2] >= cfg.min_height) & (points[:, 2] <= cfg.max_height) @@ -399,15 +409,8 @@ def general_occupancy(cloud: PointCloud2, **kwargs: Any) -> OccupancyGrid: # Create and return OccupancyGrid # Get timestamp from cloud if available - ts = cloud.ts if hasattr(cloud, "ts") and cloud.ts is not None else 0.0 - return OccupancyGrid( - grid=grid, - resolution=cfg.resolution, - origin=origin, - frame_id=cfg.frame_id or cloud.frame_id, - ts=ts, - ) + return _grid_message(cloud, cfg, grid, origin) @dataclass(frozen=True) @@ -436,16 +439,11 @@ def simple_occupancy(cloud: PointCloud2, **kwargs: Any) -> OccupancyGrid: OccupancyGrid with occupied/free cells """ cfg = SimpleOccupancyConfig(**kwargs) - points, _ = cloud.as_numpy() - points = points.astype(np.float64) # Upcast to avoid float32 rounding + points = pointcloud_xyz(cloud) + points = points[np.isfinite(points).all(axis=1)] if len(points) == 0: - return OccupancyGrid( - width=1, - height=1, - resolution=cfg.resolution, - frame_id=cfg.frame_id or cloud.frame_id, - ) + return _grid_message(cloud, cfg, np.full((1, 1), -1, dtype=np.int8)) # Find bounds of the point cloud in X-Y plane min_x = float(np.min(points[:, 0])) - 1.0 @@ -480,15 +478,7 @@ def simple_occupancy(cloud: PointCloud2, **kwargs: Any) -> OccupancyGrid: cfg.max_height, ) - ts = cloud.ts if hasattr(cloud, "ts") and cloud.ts is not None else 0.0 - - return OccupancyGrid( - grid=grid, - resolution=cfg.resolution, - origin=origin, - frame_id=cfg.frame_id or cloud.frame_id, - ts=ts, - ) + return _grid_message(cloud, cfg, grid, origin) # Populate algorithm registry diff --git a/dimos/mapping/pointclouds/test_generated_occupancy.py b/dimos/mapping/pointclouds/test_generated_occupancy.py new file mode 100644 index 00000000000..4920e8b7db5 --- /dev/null +++ b/dimos/mapping/pointclouds/test_generated_occupancy.py @@ -0,0 +1,96 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +# Copyright 2026 Dimensional Inc. +# SPDX-License-Identifier: Apache-2.0 + +from collections.abc import Callable + +from dimos_generated.builtin_interfaces.msg import Time +from dimos_generated.nav_msgs.msg import OccupancyGrid +from dimos_generated.sensor_msgs.msg import PointCloud2 +from dimos_generated.std_msgs.msg import Header +import numpy as np +from numpy.typing import NDArray +import pytest + +from dimos.mapping.occupancy.inflation import simple_inflate +from dimos.mapping.pointclouds.occupancy import ( + general_occupancy, + height_cost_occupancy, + simple_occupancy, +) +from dimos.msgs.occupancy import occupancy_view +from dimos.msgs.pointcloud import pointcloud_from_xyz + + +@pytest.mark.parametrize("algorithm", [general_occupancy, height_cost_occupancy, simple_occupancy]) +def test_generated_cloud_to_grid(algorithm: Callable[..., OccupancyGrid]) -> None: + header = Header(stamp=Time(sec=1700000000, nanosec=123456789), frame_id="map") + xy = np.arange(0, 2, 0.1) + x, y = np.meshgrid(xy, xy) + points = np.column_stack((x.ravel(), y.ravel(), np.zeros(x.size))) + cloud = pointcloud_from_xyz(points, header=header) + decoded = PointCloud2.decode(cloud.encode()) + grid = algorithm(decoded, resolution=0.1) + restored = OccupancyGrid.decode(grid.encode()) + assert restored.header == header + cells = occupancy_view(restored) + assert cells.shape == (39, 39) + assert np.any(cells == 0) + assert np.any(cells == -1) + assert np.all((cells >= -1) & (cells <= 100)) + override = algorithm(decoded, resolution=0.1, frame_id="world") + assert override.header.frame_id == "world" + assert override.header.stamp == header.stamp + assert decoded.header == header + + +@pytest.mark.parametrize("algorithm", [general_occupancy, height_cost_occupancy, simple_occupancy]) +@pytest.mark.parametrize("points", [np.empty((0, 3)), np.array([[np.nan, 0, 0]])]) +def test_empty_or_missing_points_remain_unknown( + algorithm: Callable[..., OccupancyGrid], + points: NDArray[np.float64], +) -> None: + cloud = pointcloud_from_xyz(points, header=Header(stamp=Time(nanosec=123), frame_id="map")) + grid = algorithm(cloud) + assert grid.header == cloud.header + assert occupancy_view(grid).tolist() == [[-1]] + assert grid.info.origin.orientation.w == 1 + + +@pytest.mark.parametrize("resolution", [0, -1, float("nan"), float("inf")]) +def test_invalid_resolution(resolution: float) -> None: + cloud = pointcloud_from_xyz(np.zeros((1, 3)), header=Header()) + with pytest.raises(ValueError, match="resolution"): + general_occupancy(cloud, resolution=resolution) + + +def test_inflation_preserves_metadata_and_source() -> None: + cloud = pointcloud_from_xyz( + np.array([[0, 0, 1.0]]), header=Header(stamp=Time(nanosec=789), frame_id="map") + ) + grid = general_occupancy(cloud, resolution=0.1) + original = occupancy_view(grid).copy() + inflated = simple_inflate(grid, 0.2) + assert inflated.header == grid.header + assert inflated.info == grid.info + assert np.count_nonzero(occupancy_view(inflated) == 100) == 13 + np.testing.assert_array_equal(occupancy_view(grid), original) + inflated.header.frame_id = "changed" + assert grid.header.frame_id == "map" + assert simple_inflate(grid, 0) == grid + for radius in [-1, float("nan"), float("inf")]: + with pytest.raises(ValueError, match="radius"): + simple_inflate(grid, radius) diff --git a/dimos/mapping/pointclouds/test_occupancy.py b/dimos/mapping/pointclouds/test_occupancy.py index 1ad3ef684d6..4f067da2e11 100644 --- a/dimos/mapping/pointclouds/test_occupancy.py +++ b/dimos/mapping/pointclouds/test_occupancy.py @@ -13,21 +13,26 @@ # See the License for the specific language governing permissions and # limitations under the License. +from pathlib import Path + import cv2 +from dimos_generated.nav_msgs.msg import OccupancyGrid +from dimos_generated.std_msgs.msg import Header import numpy as np from open3d.geometry import PointCloud import pytest from dimos.core.transport import LCMTransport +from dimos.e2e_tests.cdr_replay_fixture import write_go2_cdr_replay from dimos.mapping.occupancy.visualizations import visualize_occupancy_grid from dimos.mapping.pointclouds.occupancy import ( height_cost_occupancy, simple_occupancy, ) from dimos.mapping.pointclouds.util import read_pointcloud -from dimos.msgs.nav_msgs.OccupancyGrid import OccupancyGrid -from dimos.msgs.sensor_msgs.Image import Image -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 +from dimos.msgs.image import image_from_file, image_view +from dimos.msgs.occupancy import occupancy_view +from dimos.msgs.pointcloud import pointcloud_from_xyz from dimos.utils.data import get_data from dimos.utils.testing.moment import OutputMoment from dimos.utils.testing.test_moment import Go2Moment @@ -53,12 +58,12 @@ def big_office() -> PointCloud: ) def test_occupancy(apartment: PointCloud, occupancy_fn, output_name: str) -> None: expected_image = cv2.imread(str(get_data(output_name)), cv2.IMREAD_GRAYSCALE) - cloud = PointCloud2.from_numpy(np.asarray(apartment.points), frame_id="map") + cloud = pointcloud_from_xyz(np.asarray(apartment.points), header=Header(frame_id="map")) occupancy_grid = occupancy_fn(cloud) # Convert grid from -1..100 to 0..101 for PNG - computed_image = (occupancy_grid.grid + 1).astype(np.uint8) + computed_image = (occupancy_view(occupancy_grid) + 1).astype(np.uint8) np.testing.assert_array_equal(computed_image, expected_image) @@ -71,14 +76,13 @@ def test_occupancy(apartment: PointCloud, occupancy_fn, output_name: str) -> Non ], ) def test_occupancy2(big_office, occupancy_fn, output_name): - expected_image = Image.from_file(get_data(output_name)) - cloud = PointCloud2.from_numpy(np.asarray(big_office.points), frame_id="") + expected_image = image_from_file(get_data(output_name)) + cloud = pointcloud_from_xyz(np.asarray(big_office.points), header=Header()) occupancy_grid = occupancy_fn(cloud) actual = visualize_occupancy_grid(occupancy_grid, "rainbow") - actual.ts = expected_image.ts - np.testing.assert_array_equal(actual, expected_image) + np.testing.assert_array_equal(image_view(actual), image_view(expected_image)) class HeightCostMoment(Go2Moment): @@ -86,8 +90,10 @@ class HeightCostMoment(Go2Moment): @pytest.fixture -def height_cost_moment(): - moment = HeightCostMoment() +def height_cost_moment(tmp_path: Path): + recording = tmp_path / "go2-cdr.db" + write_go2_cdr_replay(recording, duration_s=3) + moment = HeightCostMoment(recording) def get_moment(ts: float, publish: bool = True) -> HeightCostMoment: moment.seek(ts) @@ -103,28 +109,29 @@ def get_moment(ts: float, publish: bool = True) -> HeightCostMoment: moment.publish() return moment - yield get_moment - - moment.stop() + try: + yield get_moment + finally: + moment.stop() def test_height_cost_occupancy_from_lidar(height_cost_moment) -> None: - """Test height_cost_occupancy with real lidar data.""" + """Test height_cost_occupancy with a deterministic CDR lidar recording.""" moment = height_cost_moment(1.0) costmap = moment.costmap.value assert costmap is not None # Basic sanity checks - assert costmap.grid is not None - assert costmap.width > 0 - assert costmap.height > 0 + assert occupancy_view(costmap) is not None + assert costmap.info.width > 0 + assert costmap.info.height > 0 # Costs should be in range -1 to 100 (-1 = unknown) - assert costmap.grid.min() >= -1 - assert costmap.grid.max() <= 100 + assert occupancy_view(costmap).min() >= -1 + assert occupancy_view(costmap).max() <= 100 # Check we have some unknown, some known - known_mask = costmap.grid >= 0 + known_mask = occupancy_view(costmap) >= 0 assert known_mask.sum() > 0, "Expected some known cells" assert (~known_mask).sum() > 0, "Expected some unknown cells" diff --git a/dimos/mapping/pointclouds/tool_occupancy_speed.py b/dimos/mapping/pointclouds/tool_occupancy_speed.py index 2ae1ca66cf3..66389ba968b 100644 --- a/dimos/mapping/pointclouds/tool_occupancy_speed.py +++ b/dimos/mapping/pointclouds/tool_occupancy_speed.py @@ -14,13 +14,14 @@ import pickle +from dimos_generated.sensor_msgs.msg import PointCloud2 + from dimos.mapping.voxels.grid import VoxelGrid -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 from dimos.utils.data import get_data_dir from dimos.utils.testing.replay import TimedSensorReplay -def test_build_map(): +def test_build_map() -> None: grid = VoxelGrid() replay: TimedSensorReplay[PointCloud2] = TimedSensorReplay("go2_bigoffice/lidar") diff --git a/dimos/mapping/ray_tracing/module.py b/dimos/mapping/ray_tracing/module.py index d18e042f841..0656d9d6239 100644 --- a/dimos/mapping/ray_tracing/module.py +++ b/dimos/mapping/ray_tracing/module.py @@ -17,12 +17,13 @@ from typing import TYPE_CHECKING +from dimos_generated.geometry_msgs.msg import PoseStamped +from dimos_generated.sensor_msgs.msg import PointCloud2 +from dimos_generated.tf2_msgs.msg import TFMessage + from dimos.constants import DIMOS_PROJECT_ROOT from dimos.core.native_module import NativeModule, NativeModuleConfig from dimos.core.stream import In, Out -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 -from dimos.msgs.tf2_msgs.TFMessage import TFMessage from dimos.spec import mapping # Max stamp gap between a cloud and the transform used to register it (s). diff --git a/dimos/mapping/ray_tracing/rust/Cargo.toml b/dimos/mapping/ray_tracing/rust/Cargo.toml index 501f8c50282..03e807a332f 100644 --- a/dimos/mapping/ray_tracing/rust/Cargo.toml +++ b/dimos/mapping/ray_tracing/rust/Cargo.toml @@ -21,19 +21,19 @@ python = "dimos.mapping.ray_tracing.module:RayTracingVoxelMap" threads = 2 [package.metadata.dimos.module.ray_tracing.inputs] -lidar = "sensor_msgs.PointCloud2" -voxel_clear_mask = "sensor_msgs.PointCloud2" -tf = "tf2_msgs.TFMessage" +lidar = "sensor_msgs/msg/PointCloud2" +voxel_clear_mask = "sensor_msgs/msg/PointCloud2" +tf = "tf2_msgs/msg/TFMessage" [package.metadata.dimos.module.ray_tracing.outputs] -global_map = "sensor_msgs.PointCloud2" -local_map = "sensor_msgs.PointCloud2" -local_map_fine = "sensor_msgs.PointCloud2" -region_bounds = "geometry_msgs.PoseStamped" +global_map = "sensor_msgs/msg/PointCloud2" +local_map = "sensor_msgs/msg/PointCloud2" +local_map_fine = "sensor_msgs/msg/PointCloud2" +region_bounds = "geometry_msgs/msg/PoseStamped" [dependencies] dimos-module = { path = "../../../../native/rust/dimos-module" } -lcm-msgs = { git = "https://github.com/dimensionalOS/dimos-lcm.git", branch = "rust-codegen" } +dimos-generated-messages = { path = "../../../message_codegen" } tokio = { version = "1", features = ["rt-multi-thread", "macros", "signal"] } serde = { version = "1", features = ["derive"] } ahash = "0.8" diff --git a/dimos/mapping/ray_tracing/rust/src/module.rs b/dimos/mapping/ray_tracing/rust/src/module.rs index 9b64e43e51c..5b3bf84f06c 100644 --- a/dimos/mapping/ray_tracing/rust/src/module.rs +++ b/dimos/mapping/ray_tracing/rust/src/module.rs @@ -16,38 +16,43 @@ use std::time::Duration; use crate::mapper::{Mapper, Pose}; use crate::voxel_ray_tracer::Config; +use dimos_generated_messages::builtin_interfaces::msg::Time; +use dimos_generated_messages::geometry_msgs::msg::{ + Point, Pose as PoseMsg, PoseStamped, Quaternion, +}; +use dimos_generated_messages::sensor_msgs::msg::{PointCloud2, PointField}; +use dimos_generated_messages::std_msgs::msg::Header; +use dimos_module::cdr; +use dimos_module::pointcloud::xyz_points as extract_xyz; use dimos_module::{error_throttled, warn_throttled, Input, Module, Output, Tf}; -use lcm_msgs::geometry_msgs::{Point, Pose as PoseMsg, PoseStamped, Quaternion}; -use lcm_msgs::sensor_msgs::{PointCloud2, PointField}; -use lcm_msgs::std_msgs::{Header, Time}; use tracing::warn; #[derive(Module)] #[module(name = "ray_tracing", setup = init_mapper)] pub struct RayTracingVoxelMap { - #[input(decode = PointCloud2::decode, handler = on_lidar)] + #[input(decode = cdr::decode, handler = on_lidar)] lidar: Input, // World-frame points a sensor knows to be empty. Their voxels are deleted // outright, reaching space ray tracing cannot clear. - #[input(decode = PointCloud2::decode, handler = on_voxel_clear_mask)] + #[input(decode = cdr::decode, handler = on_voxel_clear_mask)] voxel_clear_mask: Input, #[tf] tf: Tf, - #[output(encode = PointCloud2::encode)] + #[output(encode = cdr::encode)] global_map: Output, - #[output(encode = PointCloud2::encode)] + #[output(encode = cdr::encode)] local_map: Output, - #[output(encode = PointCloud2::encode)] + #[output(encode = cdr::encode)] local_map_fine: Output, // Cylinder bounds of the local map. Position is the center, orientation holds // radius, z_min, z_max. Stamped like local_map so consumers pair them. - #[output(encode = PoseStamped::encode)] + #[output(encode = cdr::encode)] region_bounds: Output, #[config] @@ -134,7 +139,6 @@ impl RayTracingVoxelMap { if let Some(c) = region { let bounds_msg = PoseStamped { header: Header { - seq: 0, stamp: stamp.clone(), frame_id: out_frame_id.to_string(), }, @@ -230,76 +234,10 @@ impl RayTracingVoxelMap { const TF_WAIT_TIMEOUT: Duration = Duration::from_millis(50); fn time_secs(t: &Time) -> f64 { - t.sec as f64 + t.nsec as f64 * 1e-9 + t.sec as f64 + t.nanosec as f64 * 1e-9 } -struct ExtractError(&'static str); -impl std::fmt::Display for ExtractError { - fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result { - f.write_str(self.0) - } -} - -fn extract_xyz(msg: &PointCloud2) -> Result, ExtractError> { - let mut x_off: Option = None; - let mut y_off: Option = None; - let mut z_off: Option = None; - for f in &msg.fields { - if f.datatype != PointField::FLOAT32 as u8 { - continue; - } - match f.name.as_str() { - "x" => x_off = Some(f.offset as usize), - "y" => y_off = Some(f.offset as usize), - "z" => z_off = Some(f.offset as usize), - _ => {} - } - } - let xo = x_off.ok_or(ExtractError("missing float32 x field"))?; - let yo = y_off.ok_or(ExtractError("missing float32 y field"))?; - let zo = z_off.ok_or(ExtractError("missing float32 z field"))?; - - let n = (msg.width as usize) * (msg.height as usize); - let step = msg.point_step as usize; - if step == 0 { - return Err(ExtractError("point_step is 0")); - } - if msg.data.len() < n * step { - return Err(ExtractError( - "data buffer shorter than width*height*point_step", - )); - } - if xo + 4 > step || yo + 4 > step || zo + 4 > step { - return Err(ExtractError( - "xyz field offsets do not fit within point_step", - )); - } - if msg.is_bigendian { - return Err(ExtractError("big-endian point data not supported")); - } - - let mut out = Vec::with_capacity(n); - for i in 0..n { - let base = i * step; - let x = read_f32_le(&msg.data, base + xo); - let y = read_f32_le(&msg.data, base + yo); - let z = read_f32_le(&msg.data, base + zo); - if x.is_finite() && y.is_finite() && z.is_finite() { - out.push((x, y, z)); - } - } - Ok(out) -} - -#[inline] -fn read_f32_le(buf: &[u8], off: usize) -> f32 { - let bytes: [u8; 4] = buf[off..off + 4] - .try_into() - .expect("bounds checked by caller"); - f32::from_le_bytes(bytes) -} - -fn write_point(data: &mut Vec, n: &mut i32, x: f32, y: f32, z: f32) { +fn write_point(data: &mut Vec, n: &mut u32, x: f32, y: f32, z: f32) { data.extend_from_slice(&x.to_le_bytes()); data.extend_from_slice(&y.to_le_bytes()); data.extend_from_slice(&z.to_le_bytes()); @@ -307,16 +245,15 @@ fn write_point(data: &mut Vec, n: &mut i32, x: f32, y: f32, z: f32) { *n += 1; } -fn make_cloud(data: Vec, n: i32, frame_id: &str, stamp: Time) -> PointCloud2 { - let make_field = |name: &str, off: i32| PointField { +fn make_cloud(data: Vec, n: u32, frame_id: &str, stamp: Time) -> PointCloud2 { + let make_field = |name: &str, off: u32| PointField { name: name.into(), offset: off, - datatype: PointField::FLOAT32 as u8, + datatype: PointField::FLOAT32, count: 1, }; PointCloud2 { header: Header { - seq: 0, stamp, frame_id: frame_id.into(), }, @@ -336,10 +273,10 @@ fn make_cloud(data: Vec, n: i32, frame_id: &str, stamp: Time) -> PointCloud2 } } -/// Pack flat (x, y, z) triples into an LCM cloud message. +/// Pack flat (x, y, z) triples into a generated PointCloud2 message. fn points_to_cloud(points: &[f32], frame_id: &str, stamp: Time) -> PointCloud2 { let mut data = Vec::with_capacity((points.len() / 3) * 16); - let mut n: i32 = 0; + let mut n: u32 = 0; for p in points.as_chunks::<3>().0 { write_point(&mut data, &mut n, p[0], p[1], p[2]); } diff --git a/dimos/mapping/ray_tracing/test_transformer.py b/dimos/mapping/ray_tracing/test_transformer.py index 12cb2b0b98f..97a597e91f3 100644 --- a/dimos/mapping/ray_tracing/test_transformer.py +++ b/dimos/mapping/ray_tracing/test_transformer.py @@ -20,10 +20,13 @@ pytest.importorskip("dimos_voxel_ray_tracing") +from dimos_generated.sensor_msgs.msg import PointCloud2 +from dimos_generated.std_msgs.msg import Header + from dimos.mapping.ray_tracing.transformer import RayTraceMap from dimos.mapping.ray_tracing.voxel_map import VoxelRayMapper from dimos.memory.type.observation import Observation -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 +from dimos.msgs.pointcloud import pointcloud_from_xyz def _default_margin() -> float: @@ -38,7 +41,7 @@ def _obs( id=0, ts=ts, pose=pose, - _data=PointCloud2.from_numpy(points), + _data=pointcloud_from_xyz(points, header=Header(frame_id="lidar")), ) @@ -58,7 +61,9 @@ def test_emit_every_n_yields_on_cadence_and_flushes_remainder() -> None: def test_poseless_obs_are_skipped() -> None: points = _cube() - poseless = Observation(id=1, ts=0.0, pose=None, _data=PointCloud2.from_numpy(points)) + poseless = Observation( + id=1, ts=0.0, pose=None, _data=pointcloud_from_xyz(points, header=Header(frame_id="lidar")) + ) posed = _obs(points, ts=1.0, pose=(0.0, 0.0, 0.0)) results = list(RayTraceMap()(iter([poseless, posed]))) @@ -111,7 +116,7 @@ def test_registers_sensor_frame_cloud_by_pose() -> None: id=0, ts=1.0, pose=(5.0, 0.0, 2.0, 0.0, s, 0.0, s), - _data=PointCloud2.from_numpy(point), + _data=pointcloud_from_xyz(point, header=Header(frame_id="lidar")), ) [emitted] = list(rtm(iter([obs]))) diff --git a/dimos/mapping/ray_tracing/transformer.py b/dimos/mapping/ray_tracing/transformer.py index 835b464c893..70b01b309df 100644 --- a/dimos/mapping/ray_tracing/transformer.py +++ b/dimos/mapping/ray_tracing/transformer.py @@ -16,9 +16,13 @@ from typing import TYPE_CHECKING, Any +from dimos_generated.sensor_msgs.msg import PointCloud2 +from dimos_generated.std_msgs.msg import Header +import numpy as np + from dimos.mapping.ray_tracing.voxel_map import VoxelRayMapper from dimos.memory.transform import Transformer -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 +from dimos.msgs.pointcloud import pointcloud_from_xyz, pointcloud_xyz from dimos.utils.logging_config import setup_logger if TYPE_CHECKING: @@ -56,16 +60,13 @@ def _make_obs( last_obs: Observation[PointCloud2], count: int, ) -> Observation[PointCloud2]: - import open3d as o3d # type: ignore[import-untyped] - import open3d.core as o3c # type: ignore[import-untyped] - tags = {**last_obs.tags, "frame_count": count} cx, cy, radius, z_min, z_max = mapper.take_local_bounds() positions = mapper.local_map((cx, cy, 0.0), radius, z_min, z_max) tags["region_bounds"] = (cx, cy, radius, z_min, z_max) - pcd = o3d.t.geometry.PointCloud() - pcd.point["positions"] = o3c.Tensor.from_numpy(positions) - cloud = PointCloud2(pointcloud=pcd, frame_id="world", ts=last_obs.ts) + cloud = pointcloud_from_xyz( + positions, header=Header(frame_id="world", stamp=last_obs.data.header.stamp) + ) return last_obs.derive(data=cloud, tags=tags) def __call__( @@ -88,7 +89,9 @@ def __call__( logger.warning("RayTraceMap: obs %s has no pose. dropped a cloud", obs.id) continue x, y, z, qx, qy, qz, qw = obs.pose_tuple - mapper.add_frame(obs.data.points_f32(), (x, y, z), (qx, qy, qz, qw)) + mapper.add_frame( + pointcloud_xyz(obs.data).astype(np.float32), (x, y, z), (qx, qy, qz, qw) + ) last_obs = obs count += 1 diff --git a/dimos/mapping/ray_tracing/utils/raytrace_rrd.py b/dimos/mapping/ray_tracing/utils/raytrace_rrd.py index 03ab86700d9..477490a230b 100644 --- a/dimos/mapping/ray_tracing/utils/raytrace_rrd.py +++ b/dimos/mapping/ray_tracing/utils/raytrace_rrd.py @@ -26,6 +26,7 @@ from pathlib import Path +from dimos_generated.sensor_msgs.msg import PointCloud2 import numpy as np from numpy.typing import NDArray import typer @@ -35,7 +36,7 @@ from dimos.memory.store.sqlite import SqliteStore from dimos.memory.tf import StreamTF from dimos.memory.vis.utils import DEFAULT_RENDER_VOXEL, default_render_voxel -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 +from dimos.msgs.pointcloud import pointcloud_xyz from dimos.utils.data import resolve_named_path TIMELINE = "ts" @@ -204,17 +205,26 @@ def main( for obs in lidar: t = tf.get( world_frame, - obs.data.frame_id, + obs.data.header.frame_id, time_point=obs.ts, time_tolerance=TF_MATCH_TOLERANCE_S, ) if t is None: dropped += 1 continue - x, y, z = float(t.translation.x), float(t.translation.y), float(t.translation.z) - qx, qy, qz, qw = t.rotation.x, t.rotation.y, t.rotation.z, t.rotation.w + x, y, z = ( + float(t.transform.translation.x), + float(t.transform.translation.y), + float(t.transform.translation.z), + ) + qx, qy, qz, qw = ( + t.transform.rotation.x, + t.transform.rotation.y, + t.transform.rotation.z, + t.transform.rotation.w, + ) # Sensor-frame cloud: the mapper registers it by the tf pose. - raw = obs.data.points_f32() + raw = pointcloud_xyz(obs.data).astype(np.float32) for mapper in mappers.values(): mapper.add_frame(raw, (x, y, z), (qx, qy, qz, qw)) count += 1 @@ -254,7 +264,7 @@ def main( flip = np.sum(vectors * (robot - origins), axis=1) < 0 vectors = np.where(flip[:, None], -vectors, vectors) if min_eigs is None: - lengths = np.full(len(origins), normal_scale, np.float32) + lengths: NDArray[np.float32] = np.full(len(origins), normal_scale, np.float32) else: lengths = normal_scale * _planarity_scale(min_eigs[keep]) rr.log( diff --git a/dimos/mapping/relocalization/blueprints.py b/dimos/mapping/relocalization/blueprints.py index 4aa8d39e2d3..2d7e78f084b 100644 --- a/dimos/mapping/relocalization/blueprints.py +++ b/dimos/mapping/relocalization/blueprints.py @@ -29,6 +29,9 @@ from typing import Any +from dimos_generated.sensor_msgs.msg import PointCloud2 +from dimos_generated.tf2_msgs.msg import TFMessage + from dimos.core.coordination.blueprints import autoconnect from dimos.core.core import rpc from dimos.core.module import Module, ModuleConfig @@ -37,10 +40,9 @@ from dimos.mapping.relocalization.lidar.module import LocalMapRelocalization from dimos.memory.replay import ReplayStream from dimos.memory.store.sqlite import SqliteStore -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 -from dimos.msgs.tf2_msgs.TFMessage import TFMessage from dimos.utils.data import resolve_named_path from dimos.utils.logging_config import setup_logger +from dimos.visualization.rerun.message_helpers import cloud_archetype from dimos.visualization.vis_module import vis_module logger = setup_logger() @@ -87,9 +89,9 @@ def start(self) -> None: self.register_disposable(tf.observable().subscribe(self.tf.publish)) -def _fine_points(cloud: Any) -> Any: +def _fine_points(cloud: PointCloud2) -> Any: """The premap is millimetre-scale; draw it at that size, not the 5 cm default.""" - return cloud.to_rerun(voxel_size=0.0015) + return cloud_archetype(cloud, voxel_size=0.0015) # Off until asked for. diff --git a/dimos/mapping/relocalization/go2/module.py b/dimos/mapping/relocalization/go2/module.py index 0bec9eae10d..9dfcc3921b9 100644 --- a/dimos/mapping/relocalization/go2/module.py +++ b/dimos/mapping/relocalization/go2/module.py @@ -14,14 +14,15 @@ """Go2 relocalization: lidar relocalization plus the premap merged into the live map.""" +from dimos_generated.geometry_msgs.msg import TransformStamped +from dimos_generated.sensor_msgs.msg import PointCloud2 from reactivex import combine_latest from dimos.core.core import rpc from dimos.core.stream import In, Out from dimos.mapping.relocalization.lidar.module import LidarConfig, LidarWindowRelocalization from dimos.mapping.voxels.grid import VoxelGrid -from dimos.msgs.geometry_msgs.Transform import Transform -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 +from dimos.msgs.pointcloud import concatenate_clouds, transform_cloud from dimos.utils.reactive import backpressure @@ -51,12 +52,14 @@ def start(self) -> None: ).subscribe(self._on_merge_input) ) - def _on_merge_input(self, pair: tuple[PointCloud2, Transform]) -> None: + def _on_merge_input(self, pair: tuple[PointCloud2, TransformStamped]) -> None: local, tf = pair assert self.premap is not None - premap_in_world = self.premap.transform(tf) + premap_in_world = transform_cloud(self.premap, tf) if self.config.use_carving: - grid = VoxelGrid(carve_columns=True, frame_id=local.frame_id, show_startup_log=False) + grid = VoxelGrid( + carve_columns=True, frame_id=local.header.frame_id, show_startup_log=False + ) try: grid.add_frame(premap_in_world) grid.add_frame(local) @@ -64,4 +67,4 @@ def _on_merge_input(self, pair: tuple[PointCloud2, Transform]) -> None: finally: grid.dispose() else: - self.merged_map.publish(local + premap_in_world) + self.merged_map.publish(concatenate_clouds(local, premap_in_world)) diff --git a/dimos/mapping/relocalization/lidar/module.py b/dimos/mapping/relocalization/lidar/module.py index 0afb1761be8..5122e2bc2b0 100644 --- a/dimos/mapping/relocalization/lidar/module.py +++ b/dimos/mapping/relocalization/lidar/module.py @@ -16,10 +16,10 @@ from collections import deque from functools import reduce -import operator import time from typing import Any +from dimos_generated.sensor_msgs.msg import PointCloud2 from reactivex import Observable, operators as ops from dimos.core.core import rpc @@ -30,7 +30,7 @@ RelocalizeConfig, ) from dimos.mapping.relocalization.module import Config, RelocalizationModule -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 +from dimos.msgs.pointcloud import concatenate_clouds, pointcloud_to_open3d from dimos.utils.logging_config import setup_logger from dimos.utils.reactive import backpressure @@ -51,7 +51,7 @@ def window( ops.do_action(held.append), ops.throttle_first(interval), ops.filter(lambda _: len(held) >= cfg.min_frames), - ops.map(lambda _: reduce(operator.add, held)), + ops.map(lambda _: reduce(concatenate_clouds, held)), ) @@ -91,7 +91,9 @@ def start(self) -> None: return # Downsampling the premap and computing its normals and FPFH is the # pipeline's dominant cost, so it is a startup cost, not a per-fix one. - self._relocalizer = LidarRelocalizer(self.premap.pointcloud, self.config.relocalize) + self._relocalizer = LidarRelocalizer( + pointcloud_to_open3d(self.premap), self.config.relocalize + ) self.register_disposable( backpressure( self.clouds().pipe( @@ -108,20 +110,20 @@ def _maybe_log_skip(self, msg: PointCloud2) -> None: now = time.monotonic() if now - self._last_skip_log > 5.0: logger.warning( - f"relocalize skipped: n_pts={len(msg)} " + f"relocalize skipped: n_pts={msg.width * msg.height} " f"< min_local_points={self.config.min_local_points}" ) self._last_skip_log = now def _has_enough_points(self, msg: PointCloud2) -> bool: - return len(msg) >= self.config.min_local_points + return msg.width * msg.height >= self.config.min_local_points def _relocalize(self, msg: PointCloud2) -> None: assert self._relocalizer is not None t0 = time.monotonic() try: tf = self._relocalizer.relocalize( - msg.pointcloud, self.config.world_frame, self.config.map_frame + pointcloud_to_open3d(msg), self.config.world_frame, self.config.map_frame ) except Exception: logger.exception("relocalize() failed") @@ -129,11 +131,11 @@ def _relocalize(self, msg: PointCloud2) -> None: dt = time.monotonic() - t0 if tf is None: logger.info( - f"relocalize lidar: refused after {dt:.1f}s n_pts={len(msg)} " + f"relocalize lidar: refused after {dt:.1f}s n_pts={msg.width * msg.height} " f"(below fitness_threshold={self.config.relocalize.fitness_threshold})" ) return - logger.info(f"relocalize lidar: time_cost={dt:.1f}s n_pts={len(msg)}") + logger.info(f"relocalize lidar: time_cost={dt:.1f}s n_pts={msg.width * msg.height}") self.submit(tf, "lidar") diff --git a/dimos/mapping/relocalization/lidar/relocalize.py b/dimos/mapping/relocalization/lidar/relocalize.py index e7638c83fc1..f5c3760c0da 100644 --- a/dimos/mapping/relocalization/lidar/relocalize.py +++ b/dimos/mapping/relocalization/lidar/relocalize.py @@ -37,9 +37,11 @@ from typing import TYPE_CHECKING, NamedTuple +from dimos_generated.geometry_msgs.msg import TransformStamped +from dimos_generated.std_msgs.msg import Header import numpy as np -from dimos.msgs.geometry_msgs.Transform import Transform +from dimos.msgs.geometry import inverse_transform, transform_from_matrix from dimos.protocol.service.spec import BaseConfig from dimos.utils.logging_config import setup_logger @@ -249,7 +251,7 @@ def refine(coarse_T: np.ndarray) -> RegistrationResult: def relocalize( self, local_map: PointCloud, world_frame: str, map_frame: str - ) -> Transform | None: + ) -> TransformStamped | None: """The ``world_frame -> map_frame`` transform, or ``None`` when nothing was good enough. Ready to publish: stamped with the frames the TF tree expects, and @@ -263,7 +265,9 @@ def relocalize( logger.info(f"align: fitness={result.fitness:.3f} rmse={result.inlier_rmse:.3f}") if result.fitness < self.config.fitness_threshold: return None - placement = Transform.from_matrix( - np.asarray(result.transformation), frame_id=map_frame, child_frame_id=world_frame + placement = TransformStamped( + header=Header(frame_id=map_frame), + child_frame_id=world_frame, + transform=transform_from_matrix(np.asarray(result.transformation)), ) - return placement.inverse() + return inverse_transform(placement) diff --git a/dimos/mapping/relocalization/lidar/tune.py b/dimos/mapping/relocalization/lidar/tune.py index 8eb07f86994..00365275387 100644 --- a/dimos/mapping/relocalization/lidar/tune.py +++ b/dimos/mapping/relocalization/lidar/tune.py @@ -39,6 +39,8 @@ import time from typing import TYPE_CHECKING, Any, NamedTuple +from dimos_generated.sensor_msgs.msg import PointCloud2 +from dimos_generated.std_msgs.msg import Header import numpy as np import typer @@ -48,7 +50,8 @@ LidarRelocalizer, RelocalizeConfig, ) -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 +from dimos.msgs.pointcloud import pointcloud_from_xyz, pointcloud_to_open3d, pointcloud_xyz +from dimos.msgs.time import time_from_seconds from dimos.utils.data import get_data if TYPE_CHECKING: @@ -189,7 +192,7 @@ class Dataset(NamedTuple): # so probes come from before that. "go2-sf-area1": Dataset( recording="recording_go2_mid360_2026-05-29_4-45pm-PST_corrected.db", - premap="recording_go2_mid360_2026-05-29_4-45pm-PST_corrected.pc2.lcm", + premap="recording_go2_mid360_2026-05-29_4-45pm-PST_corrected.pc2.cdr", # Runs the full stretch before the premap's own scans: 0-250 s is # ground the loop revisits after 400 s and the premap therefore # holds, while 250-400 s is a one-time excursion it never saw. @@ -282,7 +285,7 @@ def fixtures(name: str) -> tuple[PointCloud2, Any]: from dimos.memory.store.sqlite import SqliteStore ds = DATASETS[name] - premap = PointCloud2.lcm_decode(get_data(ds.premap).read_bytes()) + premap = PointCloud2.decode(get_data(ds.premap).read_bytes()) store = SqliteStore(path=str(get_data(ds.recording)), must_exist=True) store.start() return premap, store @@ -292,13 +295,13 @@ def fixtures(name: str) -> tuple[PointCloud2, Any]: def premap_index(name: str, voxel: float = 0.2) -> Any: """The premap thinned for nearest-neighbour queries, built once.""" premap, _ = fixtures(name) - return premap.pointcloud.voxel_down_sample(voxel) + return pointcloud_to_open3d(premap).voxel_down_sample(voxel) def premap_distance(name: str, points: np.ndarray) -> np.ndarray: """Per-point distance from ``points`` to the nearest premap point.""" - cloud = PointCloud2.from_numpy(points, frame_id="world", timestamp=0.0) - return np.asarray(cloud.pointcloud.compute_point_cloud_distance(premap_index(name))) + cloud = pointcloud_from_xyz(points, header=Header(frame_id="world")) + return np.asarray(pointcloud_to_open3d(cloud).compute_point_cloud_distance(premap_index(name))) def accumulate( @@ -336,7 +339,7 @@ def accumulate( if obs.pose_tuple is None: continue x, y, z = obs.pose_tuple[:3] - mapper.add_frame_world(obs.data.points_f32(), (x, y, z)) + mapper.add_frame_world(pointcloud_xyz(obs.data).astype(np.float32), (x, y, z)) used += 1 last_ts = obs.ts if used == n_frames: @@ -344,7 +347,9 @@ def accumulate( if used < n_frames: raise ValueError(f"only {used} of {n_frames} scans had a pose from {start:.0f}s") - return PointCloud2.from_numpy(mapper.global_map(), frame_id="world", timestamp=last_ts) + return pointcloud_from_xyz( + mapper.global_map(), header=Header(frame_id="world", stamp=time_from_seconds(last_ts)) + ) @lru_cache(maxsize=512) @@ -385,7 +390,7 @@ def coverage(name: str, cloud: PointCloud2) -> float: whether the aligner answered it: a probe standing where the premap has no points cannot be placed by any parameter setting. """ - distances = premap_distance(name, np.asarray(cloud.pointcloud.points)) + distances = premap_distance(name, pointcloud_xyz(cloud)) return float((distances < COVERAGE_TOL_M).mean()) @@ -466,7 +471,7 @@ def identity_error(T: np.ndarray) -> tuple[float, float, float]: def applied(T: np.ndarray, cloud: PointCloud2) -> np.ndarray: """``cloud``'s points where ``T`` puts them.""" - points: np.ndarray = np.asarray(cloud.pointcloud.points) @ T[:3, :3].T + T[:3, 3] + points: np.ndarray = pointcloud_xyz(cloud) @ T[:3, :3].T + T[:3, 3] return points @@ -494,7 +499,7 @@ def run_probes( premap, _ = fixtures(name) # The premap's preprocessing depends on the config but not the probe, so # it is paid once per trial rather than once per attempt. - relocalizer = LidarRelocalizer(premap.pointcloud, config) + relocalizer = LidarRelocalizer(pointcloud_to_open3d(premap), config) rate = scan_rate(name) probes: list[Probe] = [] @@ -511,7 +516,7 @@ def run_probes( frames = min(max(int(elapsed * rate), min_frames), max_frames) cloud = local_map(name, frames, start, voxel) t0, c0 = time.monotonic(), time.process_time() - result = relocalizer.align(cloud.pointcloud) + result = relocalizer.align(pointcloud_to_open3d(cloud)) dt = time.monotonic() - t0 elapsed += dt wall += dt @@ -524,13 +529,13 @@ def run_probes( break T = np.asarray(result.transformation) - truth_points = np.asarray(cloud.pointcloud.points) + truth_points = pointcloud_xyz(cloud) moved = applied(T, cloud) translation, rotation, tilt = identity_error(T) probes.append( Probe( start=start, - n_points=len(cloud), + n_points=cloud.width * cloud.height, coverage=place_coverage(name, start, voxel), translation_m=translation, rotation_deg=rotation, @@ -613,6 +618,7 @@ def view(name: str, probes: list[Probe], out: str | None) -> None: import rerun as rr from dimos.visualization.rerun.init import rerun_init + from dimos.visualization.rerun.message_helpers import cloud_archetype premap, _ = fixtures(name) rerun_init("dimos relocalize eval") @@ -620,22 +626,20 @@ def view(name: str, probes: list[Probe], out: str | None) -> None: rr.save(out) else: rr.spawn() - rr.log("world/premap/pointcloud", premap.to_rerun(voxel_size=0.05), static=True) + rr.log("world/premap/pointcloud", cloud_archetype(premap, ui_radius=0.05), static=True) for probe in probes: - aligned = PointCloud2.from_numpy( - applied(probe.transform, probe.cloud), - frame_id=probe.cloud.frame_id, - timestamp=probe.cloud.ts, + aligned = pointcloud_from_xyz( + applied(probe.transform, probe.cloud), header=probe.cloud.header ) entity = f"world/probe_{probe.start:04.0f}s" rr.log( f"{entity}/truth", - probe.cloud.to_rerun(voxel_size=0.05, colors=[76, 220, 41]), + rr.Points3D(pointcloud_xyz(probe.cloud), radii=0.05, colors=[76, 220, 41]), static=True, ) rr.log( f"{entity}/aligned", - aligned.to_rerun(voxel_size=0.05, colors=[231, 76, 60]), + rr.Points3D(pointcloud_xyz(aligned), radii=0.05, colors=[231, 76, 60]), static=True, ) if out is not None: @@ -679,7 +683,7 @@ def _register( DatasetOpt = typer.Option(DEFAULT_DATASET, "--dataset", "-d", help="Registered dataset name") RecordingOpt = typer.Option(None, "--recording", help="Recording, resolved through get_data") -PremapOpt = typer.Option(None, "--premap", help="Premap .pc2.lcm, resolved through get_data") +PremapOpt = typer.Option(None, "--premap", help="Premap .pc2.cdr, resolved through get_data") LidarOpt = typer.Option(None, "--lidar", help="Lidar stream in the recording") FromOpt = typer.Option(None, "--from", help="Earliest second probes may come from") ToOpt = typer.Option(None, "--to", help="Latest second probes may come from") @@ -725,7 +729,7 @@ def run( console = Console() table = _table( - f"{name} premap {len(pre):,} pts preset={preset} " + f"{name} premap {pre.width * pre.height:,} pts preset={preset} " f"cutoff={config.fitness_threshold} " f"frames {config.min_frames}-{config.max_frames}", *PROBE_COLUMNS, diff --git a/dimos/mapping/relocalization/module.py b/dimos/mapping/relocalization/module.py index 09af2b1fc88..fcc7710da4f 100644 --- a/dimos/mapping/relocalization/module.py +++ b/dimos/mapping/relocalization/module.py @@ -39,29 +39,34 @@ from __future__ import annotations +from copy import deepcopy +import time from typing import Any +from dimos_generated.geometry_msgs.msg import TransformStamped +from dimos_generated.sensor_msgs.msg import PointCloud2 +from dimos_generated.tf2_msgs.msg import TFMessage import reactivex as rx from reactivex import Observable, Subject, operators as ops from dimos.core.core import rpc from dimos.core.module import Module, ModuleConfig from dimos.core.stream import Out -from dimos.msgs.geometry_msgs.Transform import Transform -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 -from dimos.msgs.tf2_msgs.TFMessage import TFMessage +from dimos.msgs.time import time_from_seconds from dimos.utils.data import get_data from dimos.utils.logging_config import setup_logger logger = setup_logger() -MAP_SUFFIX = ".pc2.lcm" +MAP_SUFFIX = ".pc2.cdr" -def fix_stream(fixes: Observable[Transform], interval: float) -> Observable[Transform]: +def fix_stream( + fixes: Observable[TransformStamped], interval: float +) -> Observable[TransformStamped]: """Every accepted fix as it lands, then again every ``interval`` s (once only if <= 0).""" - def now_and_again(fix: Transform) -> Observable[Transform]: + def now_and_again(fix: TransformStamped) -> Observable[TransformStamped]: again = rx.interval(interval).pipe(ops.map(lambda _: fix)) if interval > 0 else rx.empty() return rx.concat(rx.of(fix), again) @@ -70,7 +75,7 @@ def now_and_again(fix: Transform) -> Observable[Transform]: class Config(ModuleConfig): # Premap stem or path, e.g. `--map-file=go2_hongkong_office_twopass_map`; - # `.pc2.lcm` is appended if absent. Without one the module runs but never + # `.pc2.cdr` is appended if absent. Without one the module runs but never # attempts a fix. map_file: str | None = None # What the live fixed frame is called, whatever the odometry is in @@ -97,16 +102,14 @@ class RelocalizationModule(Module): def __init__(self, **kwargs: Any) -> None: super().__init__(**kwargs) - self.fixes: Subject[Transform] = Subject() + self.fixes: Subject[TransformStamped] = Subject() self.premap: PointCloud2 | None = None @rpc def start(self) -> None: super().start() self.register_disposable( - fix_stream(self.fixes, self.config.tf_interval).subscribe( - lambda tf: self.tf.publish(TFMessage(tf.now())) - ) + fix_stream(self.fixes, self.config.tf_interval).subscribe(self._publish_tf) ) if not self.config.map_file: logger.info("Relocalization module disabled (no map_file configured)") @@ -114,13 +117,18 @@ def start(self) -> None: self._load_premap(self.config.map_file) logger.info(f"Relocalization module started: map_file={self.config.map_file!r}") + def _publish_tf(self, tf: TransformStamped) -> None: + published = deepcopy(tf) + published.header.stamp = time_from_seconds(time.time()) + self.tf.publish(TFMessage(transforms=[published])) + def _load_premap(self, map_file: str) -> None: # get_data, so a premap that is only in LFS is pulled and decompressed # rather than reported missing. name = map_file if map_file.endswith(MAP_SUFFIX) else map_file + MAP_SUFFIX - premap = PointCloud2.lcm_decode(get_data(name).read_bytes()) + premap = PointCloud2.decode(get_data(name).read_bytes()) - premap.frame_id = self.config.map_frame + premap.header.frame_id = self.config.map_frame self.premap = premap self.register_disposable( @@ -138,14 +146,16 @@ def keep_relocalizing(self) -> bool: """Whether to keep attempting. Implementations gate their input on this.""" return not (self._placed and self.config.relocalize_once) - def submit(self, tf: Transform, source: str = "") -> None: + def submit(self, tf: TransformStamped, source: str = "") -> None: """Publish a ``world_frame -> map`` fix the implementation already decided to believe.""" world, map_frame = self.config.world_frame, self.config.map_frame - assert (tf.frame_id, tf.child_frame_id) == (world, map_frame), ( + assert (tf.header.frame_id, tf.child_frame_id) == (world, map_frame), ( f"relocalize {source}: expected {world!r} -> {map_frame!r}, " - f"got {tf.frame_id!r} -> {tf.child_frame_id!r}" + f"got {tf.header.frame_id!r} -> {tf.child_frame_id!r}" + ) + logger.info( + f"relocalize {source}: TF {world!r} -> {map_frame!r} t={tf.transform.translation}" ) - logger.info(f"relocalize {source}: TF {world!r} -> {map_frame!r} t={tf.translation}") self.fixes.on_next(tf) if not self._placed and self.config.relocalize_once: logger.info(f"relocalize {source}: placed, no further attempts (relocalize_once)") diff --git a/dimos/mapping/relocalization/test_module.py b/dimos/mapping/relocalization/test_module.py index df09411de8e..34ecea46e7f 100644 --- a/dimos/mapping/relocalization/test_module.py +++ b/dimos/mapping/relocalization/test_module.py @@ -15,6 +15,9 @@ from types import SimpleNamespace from typing import get_type_hints +from dimos_generated.geometry_msgs.msg import TransformStamped +from dimos_generated.std_msgs.msg import Header +from dimos_generated.vision_msgs.msg import Detection3DArray import numpy as np import pytest from scipy.spatial.transform import Rotation @@ -22,9 +25,8 @@ from dimos.core.stream import In from dimos.mapping.relocalization.lidar.module import LidarWindowRelocalization from dimos.mapping.relocalization.module import RelocalizationModule -from dimos.msgs.geometry_msgs.Transform import Transform -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 -from dimos.msgs.vision_msgs.Detection3DArray import Detection3DArray +from dimos.msgs.geometry import inverse_transform, transform_from_matrix, transform_matrix +from dimos.msgs.pointcloud import pointcloud_from_xyz, pointcloud_xyz @pytest.fixture @@ -53,19 +55,33 @@ def test_submit_publishes_and_checks_frames(module): """submit does not second-guess the fix; the implementation already decided.""" m = module() got = fixes(m) - tf = Transform.from_matrix(np.eye(4), frame_id="world", child_frame_id="map") + tf = TransformStamped( + header=Header(frame_id="world"), + child_frame_id="map", + transform=transform_from_matrix(np.eye(4)), + ) m.submit(tf, "x") m.submit(tf, "x") assert got == [tf, tf] # A strategy handing over the placement instead of the frame transform is # caught here rather than publishing a backwards TF. with pytest.raises(AssertionError): - m.submit(Transform.from_matrix(np.eye(4), frame_id="map", child_frame_id="world")) + m.submit( + TransformStamped( + header=Header(frame_id="map"), + child_frame_id="world", + transform=transform_from_matrix(np.eye(4)), + ) + ) def test_relocalize_once_stops_after_the_first_fix(module): """What the flag does, either way. Which one is the default is a policy call.""" - tf = Transform.from_matrix(np.eye(4), frame_id="world", child_frame_id="map") + tf = TransformStamped( + header=Header(frame_id="world"), + child_frame_id="map", + transform=transform_from_matrix(np.eye(4)), + ) once = module(relocalize_once=True) assert once.keep_relocalizing() and not once.placed @@ -86,7 +102,11 @@ def test_the_fix_goes_out_the_moment_it_is_accepted(interval): fixes, sent = Subject(), [] disposable = fix_stream(fixes, interval=interval).subscribe(sent.append) - tf = Transform.from_matrix(np.eye(4), frame_id="world", child_frame_id="map") + tf = TransformStamped( + header=Header(frame_id="world"), + child_frame_id="map", + transform=transform_from_matrix(np.eye(4)), + ) fixes.on_next(tf) assert sent == [tf] disposable.dispose() @@ -94,9 +114,9 @@ def test_the_fix_goes_out_the_moment_it_is_accepted(interval): def test_premap_defines_the_map_frame_and_waits_for_a_fix(module, tmp_path): """Loading is the base's: every strategy reads a premap and publishes it, once placed.""" - path = tmp_path / "somewhere.pc2.lcm" + path = tmp_path / "somewhere.pc2.cdr" path.write_bytes( - PointCloud2.from_numpy(np.zeros((5, 3), dtype=np.float32), timestamp=0.0).lcm_encode() + pointcloud_from_xyz(np.zeros((5, 3), dtype=np.float32), header=Header()).encode() ) m = module() published, disposables = [], [] @@ -106,11 +126,17 @@ def test_premap_defines_the_map_frame_and_waits_for_a_fix(module, tmp_path): m.register_disposable = disposables.append m._load_premap(str(path)) - assert m.premap is not None and len(m.premap) == 5 - assert m.premap.frame_id == "map" + assert m.premap is not None and m.premap.width * m.premap.height == 5 + assert m.premap.header.frame_id == "map" assert len(disposables) == 1 # the gated publish assert published == [] # ... which stays silent until a fix lands - m.submit(Transform.from_matrix(np.eye(4), frame_id="world", child_frame_id="map")) + m.submit( + TransformStamped( + header=Header(frame_id="world"), + child_frame_id="map", + transform=transform_from_matrix(np.eye(4)), + ) + ) assert published == [m.premap] # republish_loaded_map=0: once, on that fix disposables[0].dispose() @@ -135,8 +161,8 @@ def relocalizer(threshold): # Accepted: open3d places the live cloud in the map, the TF tree wants the # other direction, and relocalize() is what turns one into the other. tf = relocalizer(0.3).relocalize(None, "world", "map") - assert (tf.frame_id, tf.child_frame_id) == ("world", "map") - np.testing.assert_allclose(tf.to_matrix(), np.linalg.inv(placement), atol=1e-9) + assert (tf.header.frame_id, tf.child_frame_id) == ("world", "map") + np.testing.assert_allclose(transform_matrix(tf.transform), np.linalg.inv(placement), atol=1e-9) def test_no_config_without_naming_a_rig(): @@ -166,22 +192,28 @@ def test_the_match_runs_on_a_window_of_the_last_scans(): window(scans, cfg, interval=0.001).subscribe(matched.append) for i in range(5): - scans.on_next(PointCloud2.from_numpy(np.full((4, 3), i, dtype=np.float32), timestamp=0.0)) + scans.on_next(pointcloud_from_xyz(np.full((4, 3), i, dtype=np.float32), header=Header())) time.sleep(0.01) # clear the throttle, so every scan gets its attempt # The first scan is below min_frames; from then on the window is full. - assert [len(c) for c in matched] == [8, 12, 12, 12] + assert [c.width * c.height for c in matched] == [8, 12, 12, 12] # ... and holds the *last* three scans, not the first. - assert set(matched[-1].points_f32()[:, 0]) == {2.0, 3.0, 4.0} + assert set(pointcloud_xyz(matched[-1])[:, 0]) == {2.0, 3.0, 4.0} def test_from_matrix_inverse_matches_linalg_inv(): T = np.eye(4) T[:3, :3] = Rotation.from_euler("xyz", [0.1, -0.2, 1.3]).as_matrix() T[:3, 3] = [1.5, -2.0, 0.3] - tf = Transform.from_matrix(T, frame_id="map", child_frame_id="world").inverse() - assert (tf.frame_id, tf.child_frame_id) == ("world", "map") - np.testing.assert_allclose(tf.to_matrix(), np.linalg.inv(T), atol=1e-9) + tf = inverse_transform( + TransformStamped( + header=Header(frame_id="map"), + child_frame_id="world", + transform=transform_from_matrix(T), + ) + ) + assert (tf.header.frame_id, tf.child_frame_id) == ("world", "map") + np.testing.assert_allclose(transform_matrix(tf.transform), np.linalg.inv(T), atol=1e-9) def test_dual_strategy_merges_ports(): diff --git a/dimos/mapping/test_generated_costmapper.py b/dimos/mapping/test_generated_costmapper.py new file mode 100644 index 00000000000..1b918edf3c9 --- /dev/null +++ b/dimos/mapping/test_generated_costmapper.py @@ -0,0 +1,59 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import math + +from dimos_generated.builtin_interfaces.msg import Time +from dimos_generated.geometry_msgs.msg import Point, Pose +from dimos_generated.nav_msgs.msg import MapMetaData, OccupancyGrid +from dimos_generated.std_msgs.msg import Header +import numpy as np + +from dimos.mapping.costmapper import CostMapper +from dimos.mapping.pointclouds.occupancy import GeneralOccupancyConfig +from dimos.msgs.geometry import quaternion_from_euler +from dimos.msgs.occupancy import occupancy_view +from dimos.msgs.pointcloud import pointcloud_from_xyz + + +def test_cost_mapper_generates_cdr_grid_with_exact_header(request): + mapper = CostMapper(algo="general", config=GeneralOccupancyConfig(resolution=0.25)) + request.addfinalizer(mapper.stop) + header = Header(stamp=Time(sec=1700000000, nanosec=123456789), frame_id="map") + cloud = pointcloud_from_xyz(np.array([[0, 0, 0], [1, 1, 1]]), header=header) + grid = mapper._calculate_costmap(cloud) + decoded = OccupancyGrid.decode(grid.encode()) + assert decoded.header == header + assert np.any(occupancy_view(decoded) == 100) + assert np.any(occupancy_view(decoded) == 0) + + +def test_initial_safe_radius_respects_rotated_origin_and_replaces_readonly_data(request): + mapper = CostMapper(initial_safe_radius_meters=0.1) + request.addfinalizer(mapper.stop) + grid = OccupancyGrid( + info=MapMetaData( + width=3, + height=3, + resolution=1, + origin=Pose( + position=Point(x=1, y=-1), orientation=quaternion_from_euler(0, 0, math.pi / 2) + ), + ), + data=[100] * 9, + ) + mapper._apply_initial_safe_radius(grid) + expected = np.full((3, 3), 100, dtype=np.int8) + expected[1, 1] = 0 + np.testing.assert_array_equal(occupancy_view(grid), expected) diff --git a/dimos/mapping/test_generated_odometry_hist.py b/dimos/mapping/test_generated_odometry_hist.py new file mode 100644 index 00000000000..61cdae3fe40 --- /dev/null +++ b/dimos/mapping/test_generated_odometry_hist.py @@ -0,0 +1,63 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from dimos_generated.builtin_interfaces.msg import Time +from dimos_generated.geometry_msgs.msg import Point, Pose, Quaternion +from dimos_generated.nav_msgs.msg import Odometry, Path +from dimos_generated.std_msgs.msg import Header +import pytest + +from dimos.mapping.odometry_hist import OdometryHist, path_at_true_height + + +def odometry(x: float, nanosec: int, sec: int = 1700000000) -> Odometry: + message = Odometry(header=Header(stamp=Time(sec=sec, nanosec=nanosec), frame_id="odom")) + message.pose.pose = Pose(position=Point(x=x, y=2, z=3), orientation=Quaternion(w=1)) + return Odometry.decode(message.encode()) + + +@pytest.mark.asyncio +async def test_history_retains_exact_stamp_and_one_nanosecond_interval(request): + module = OdometryHist(min_publish_interval_seconds=1e-9) + request.addfinalizer(module.stop) + paths = [] + module.odom_hist.subscribe(lambda path: paths.append(Path.decode(path.encode()))) + source = odometry(1, 123456789) + await module.handle_odometry(source) + source.pose.pose.position.x = 100 + await module.handle_odometry(odometry(2, 123456790)) + assert len(paths) == 2 + assert paths[-1].header.stamp.nanosec == 123456790 + assert [p.header.stamp.nanosec for p in paths[-1].poses] == [123456789, 123456790] + assert paths[-1].poses[0].pose.position.x == 1 + assert paths[0].poses[0].pose.position.x == 1 + archetype = path_at_true_height(paths[-1]) + assert archetype.strips.as_arrow_array().to_pylist() == [[[1, 2, 3], [2, 2, 3]]] + + +@pytest.mark.asyncio +async def test_history_throttles_bounds_and_restarts_at_zero(request): + module = OdometryHist(max_poses=2, frame_id="world", min_publish_interval_seconds=0.1) + request.addfinalizer(module.stop) + paths = [] + module.odom_hist.subscribe(paths.append) + await module.handle_odometry(odometry(1, 0)) + await module.handle_odometry(odometry(2, 50000000)) + assert len(paths) == 1 + await module.handle_odometry(odometry(3, 100000000)) + assert len(paths) == 2 and len(paths[-1].poses) == 2 + assert paths[-1].header.frame_id == "world" + assert all(p.header.frame_id == "world" for p in paths[-1].poses) + await module.handle_odometry(odometry(4, 0, sec=0)) + assert len(paths) == 3 and paths[-1].header.stamp == Time() diff --git a/dimos/mapping/test_generated_voxels.py b/dimos/mapping/test_generated_voxels.py new file mode 100644 index 00000000000..6e3dc1e7722 --- /dev/null +++ b/dimos/mapping/test_generated_voxels.py @@ -0,0 +1,76 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from dimos_generated.builtin_interfaces.msg import Time +from dimos_generated.std_msgs.msg import Header +import numpy as np +import open3d.core as o3c +import pytest + +from dimos.mapping.voxels.grid import VoxelGrid +from dimos.mapping.voxels.impl.o3d import O3dVoxels +from dimos.mapping.voxels.impl.packed import PackedVoxels +from dimos.msgs.pointcloud import pointcloud_from_xyz, pointcloud_xyz + + +@pytest.mark.parametrize("backend", ["packed", "open3d"]) +@pytest.mark.parametrize("carve", [False, True]) +def test_generated_cloud_union_and_column_carving(backend, carve): + grid = ( + PackedVoxels(0.5, carve) + if backend == "packed" + else O3dVoxels(0.5, 1000, carve, o3c.Device("CPU:0")) + ) + try: + for points in [[[0.1, 0.1, 0.1], [0.1, 0.1, 1.1], [1.1, 1.1, 0.1]], [[0.1, 0.1, 0.6]]]: + grid.add_frame(pointcloud_from_xyz(np.array(points), header=Header(frame_id="map"))) + actual = {tuple(point) for point in grid.points()} + expected = {(0.25, 0.25, 0.75), (1.25, 1.25, 0.25)} + if not carve: + expected |= {(0.25, 0.25, 0.25), (0.25, 0.25, 1.25)} + assert actual == expected + grid.add_frame( + pointcloud_from_xyz(np.array([[np.nan, 0, 0], [0, np.inf, 0]]), header=Header()) + ) + assert {tuple(point) for point in grid.points()} == expected + finally: + grid.dispose() + + +def test_voxel_output_retains_exact_latest_stamp_and_invalidates_cache(): + grid = VoxelGrid(voxel_size=0.5, device="CPU:0", frame_id="map", show_startup_log=False) + try: + empty = grid.get_global_pointcloud2() + assert empty.width == 0 and empty.header.stamp == Time() + header = Header(stamp=Time(sec=1700000000, nanosec=123456789), frame_id="map") + source = pointcloud_from_xyz(np.array([[0.1, 0.2, 0.3]]), header=header) + grid.add_frame(source) + source.header.stamp.nanosec = 1 + first = grid.get_global_pointcloud2() + assert first.header.stamp == header.stamp + np.testing.assert_array_equal(pointcloud_xyz(first), [[0.25, 0.25, 0.25]]) + assert grid.get_global_pointcloud2() is first + header.stamp.nanosec += 1 + grid.add_frame(pointcloud_from_xyz(np.array([[1.1, 1.1, 1.1]]), header=header)) + second = grid.get_global_pointcloud2() + assert second is not first + assert second.header == header and second.width == 2 + assert first.width == 1 + grid.add_frame(pointcloud_from_xyz(np.empty((0, 3)), header=Header(frame_id="map"))) + assert grid.get_global_pointcloud2().header.stamp == Time() + assert grid.get_global_pointcloud2().width == 2 + finally: + grid.dispose() + with pytest.raises(RuntimeError, match="disposed"): + grid.get_global_pointcloud2() diff --git a/dimos/mapping/test_voxels.py b/dimos/mapping/test_voxels.py index 662a791b41c..26963134604 100644 --- a/dimos/mapping/test_voxels.py +++ b/dimos/mapping/test_voxels.py @@ -12,131 +12,76 @@ # See the License for the specific language governing permissions and # limitations under the License. -from collections.abc import Callable, Generator +"""Deterministic CDR replacement for the retired typed-pickle lidar fixture.""" +from collections.abc import Generator + +from dimos_generated.sensor_msgs.msg import PointCloud2 +from dimos_generated.std_msgs.msg import Header import numpy as np import pytest -from dimos.core.transport import LCMTransport from dimos.mapping.voxels.grid import VoxelGrid -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 -from dimos.utils.data import get_data -from dimos.utils.testing.legacy_pickle import LegacyPickleStore -from dimos.utils.testing.moment import OutputMoment -from dimos.utils.testing.test_moment import Go2Moment - -pytestmark = pytest.mark.self_hosted +from dimos.msgs.pointcloud import pointcloud_from_xyz, pointcloud_xyz @pytest.fixture def grid() -> Generator[VoxelGrid, None, None]: - g = VoxelGrid() - yield g - g.dispose() - - -class Go2MapperMoment(Go2Moment): - global_map: OutputMoment[PointCloud2] = OutputMoment(LCMTransport("/global_map", PointCloud2)) - - -MomentFactory = Callable[[float, bool], Go2MapperMoment] - - -@pytest.fixture -def moment() -> Generator[MomentFactory, None, None]: - instances: list[Go2MapperMoment] = [] - - def get_moment(ts: float, publish: bool = True) -> Go2MapperMoment: - m = Go2MapperMoment() - m.seek(ts) - if publish: - m.publish() - instances.append(m) - return m - - yield get_moment - for m in instances: - m.stop() - - -@pytest.fixture -def moment1(moment: MomentFactory) -> Go2MapperMoment: - return moment(10, False) + value = VoxelGrid(device="CPU:0", show_startup_log=False) + try: + yield value + finally: + value.dispose() @pytest.fixture -def moment2(moment: MomentFactory) -> Go2MapperMoment: - return moment(85, False) +def lidar_frame() -> PointCloud2: + axis = np.arange(20) * 0.05 + 0.025 + points = np.stack(np.meshgrid(axis, axis, axis, indexing="ij"), axis=-1).reshape(-1, 3) + message = pointcloud_from_xyz(points, header=Header(frame_id="world")) + return PointCloud2.decode(message.encode()) def test_ingest_a_few(grid: VoxelGrid) -> None: - data_dir = get_data("unitree_go2_office_walk2") - lidar_store = LegacyPickleStore(f"{data_dir}/lidar") - - for i in [1, 4, 8]: - frame = lidar_store.find_closest_seek(i) - assert frame is not None - print("add", frame) - grid.add_frame(frame) - - assert len(grid.get_global_pointcloud2()) == 30136 - - -@pytest.mark.parametrize( - "voxel_size, expected_points", - [ - (0.5, 277), - (0.1, 7290), - (0.05, 28199), - ], -) -def test_roundtrip(moment1: Go2MapperMoment, voxel_size: float, expected_points: int) -> None: - lidar_frame = moment1.lidar.value - assert lidar_frame is not None - - grid = VoxelGrid(voxel_size=voxel_size) + for offset in (0.0, 1.0, 2.0): + points = np.array([[offset + 0.025, 0.025, 0.025], [offset + 0.075, 0.025, 0.025]]) + frame = pointcloud_from_xyz(points, header=Header(frame_id="world")) + grid.add_frame(PointCloud2.decode(frame.encode())) + assert grid.get_global_pointcloud2().width == 6 + np.testing.assert_allclose( + np.sort(pointcloud_xyz(grid.get_global_pointcloud2())[:, 0]), + [0.025, 0.075, 1.025, 1.075, 2.025, 2.075], + atol=1e-6, + ) + + +@pytest.mark.parametrize("voxel_size,expected_points", [(0.5, 8), (0.1, 1000), (0.05, 8000)]) +def test_roundtrip(lidar_frame: PointCloud2, voxel_size: float, expected_points: int) -> None: + grid = VoxelGrid(voxel_size=voxel_size, device="CPU:0", show_startup_log=False) + try: + grid.add_frame(lidar_frame) + first = grid.get_global_pointcloud2() + assert first.width == expected_points + if voxel_size == 0.05: + assert first.width == lidar_frame.width + np.testing.assert_allclose( + np.sort(pointcloud_xyz(first), axis=0), + np.sort(pointcloud_xyz(lidar_frame), axis=0), + atol=1e-6, + ) + grid.add_frame(PointCloud2.decode(first.encode())) + assert grid.get_global_pointcloud2().width == expected_points + np.testing.assert_array_equal( + pointcloud_xyz(grid.get_global_pointcloud2()), pointcloud_xyz(first) + ) + finally: + grid.dispose() + + +def test_roundtrip_range_preserved(grid: VoxelGrid, lidar_frame: PointCloud2) -> None: + inputs = pointcloud_xyz(lidar_frame) grid.add_frame(lidar_frame) - - global1 = grid.get_global_pointcloud2() - assert len(global1) == expected_points - - # loseless roundtrip - if voxel_size == 0.05: - assert len(global1) == len(lidar_frame) - # TODO: we want __eq__ on PointCloud2 - should actually compare - # all points in both frames - - grid.add_frame(global1) - # no new information, no global map change - assert len(grid.get_global_pointcloud2()) == len(global1) - - moment1.publish() - grid.dispose() - - -def test_roundtrip_range_preserved(grid: VoxelGrid) -> None: - """Test that input coordinate ranges are preserved in output.""" - data_dir = get_data("unitree_go2_office_walk2") - lidar_store = LegacyPickleStore(f"{data_dir}/lidar") - - frame = lidar_store.find_closest_seek(1.0) - assert frame is not None - input_pts = np.asarray(frame.pointcloud.points) - - grid.add_frame(frame) - - out_pcd = grid.get_global_pointcloud().to_legacy() - out_pts = np.asarray(out_pcd.points) - - voxel_size = grid._voxel_size - tolerance = voxel_size # Allow one voxel of difference at boundaries - - # TODO: we want __eq__ on PointCloud2 - should actually compare - # all points in both frames - - for axis, name in enumerate(["X", "Y", "Z"]): - in_min, in_max = input_pts[:, axis].min(), input_pts[:, axis].max() - out_min, out_max = out_pts[:, axis].min(), out_pts[:, axis].max() - - assert abs(in_min - out_min) < tolerance, f"{name} min mismatch: in={in_min}, out={out_min}" - assert abs(in_max - out_max) < tolerance, f"{name} max mismatch: in={in_max}, out={out_max}" + outputs = np.asarray(grid.get_global_pointcloud().to_legacy().points) + for axis in range(3): + assert abs(inputs[:, axis].min() - outputs[:, axis].min()) < grid._voxel_size + assert abs(inputs[:, axis].max() - outputs[:, axis].max()) < grid._voxel_size diff --git a/dimos/mapping/tool_voxels.py b/dimos/mapping/tool_voxels.py index 84fd48c729a..4a360fdcf11 100644 --- a/dimos/mapping/tool_voxels.py +++ b/dimos/mapping/tool_voxels.py @@ -13,14 +13,16 @@ # limitations under the License. from collections.abc import Callable, Generator +import os import time +from dimos_generated.sensor_msgs.msg import PointCloud2 import numpy as np import pytest from dimos.core.transport import LCMTransport from dimos.mapping.voxels.grid import VoxelGrid -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 +from dimos.msgs.pointcloud import pointcloud_xyz from dimos.utils.testing.moment import OutputMoment from dimos.utils.testing.test_moment import Go2Moment @@ -44,7 +46,7 @@ def moment() -> Generator[MomentFactory, None, None]: instances: list[Go2MapperMoment] = [] def get_moment(ts: float, publish: bool = True) -> Go2MapperMoment: - m = Go2MapperMoment() + m = Go2MapperMoment(os.environ["DIMOS_CDR_REPLAY"]) m.seek(ts) if publish: m.publish() @@ -87,8 +89,8 @@ def test_carving(grid: VoxelGrid, moment1: Go2MapperMoment, moment2: Go2MapperMo count_carving = grid.size() voxel_size = grid._voxel_size - pts1 = np.asarray(lidar_frame1.pointcloud.points) - pts2 = np.asarray(lidar_frame2.pointcloud.points) + pts1 = pointcloud_xyz(lidar_frame1) + pts2 = pointcloud_xyz(lidar_frame2) combined_vox = np.floor(np.vstack([pts1, pts2]) / voxel_size).astype(np.int64) count_additive = np.unique(combined_vox, axis=0).shape[0] diff --git a/dimos/mapping/voxels/grid.py b/dimos/mapping/voxels/grid.py index 00b5d345d27..298455e18c1 100644 --- a/dimos/mapping/voxels/grid.py +++ b/dimos/mapping/voxels/grid.py @@ -14,12 +14,15 @@ from __future__ import annotations -import time +from copy import deepcopy from typing import TYPE_CHECKING +from dimos_generated.sensor_msgs.msg import PointCloud2 +from dimos_generated.std_msgs.msg import Header + from dimos.mapping.voxels.impl.o3d import O3dVoxels from dimos.mapping.voxels.impl.packed import PackedVoxels -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 +from dimos.msgs.pointcloud import pointcloud_from_xyz from dimos.utils.decorators.decorators import simple_mcache from dimos.utils.logging_config import setup_logger @@ -50,22 +53,27 @@ def __init__( frame_id: str = "world", show_startup_log: bool = True, ) -> None: - import open3d.core as o3c # type: ignore[import-untyped] - self._voxel_size = voxel_size self._frame_id = frame_id - use_cuda = device.startswith("CUDA") and o3c.cuda.is_available() - self._impl: O3dVoxels | PackedVoxels = ( - O3dVoxels(voxel_size, block_count, carve_columns, o3c.Device(device)) - if use_cuda - else PackedVoxels(voxel_size, carve_columns) - ) + use_cuda = False + self._impl: O3dVoxels | PackedVoxels + if device.startswith("CUDA"): + import open3d.core as o3c # type: ignore[import-untyped] + + use_cuda = o3c.cuda.is_available() + self._impl = ( + O3dVoxels(voxel_size, block_count, carve_columns, o3c.Device(device)) + if use_cuda + else PackedVoxels(voxel_size, carve_columns) + ) + else: + self._impl = PackedVoxels(voxel_size, carve_columns) if show_startup_log: logger.info(f"VoxelGrid using device: {device if use_cuda else 'CPU:0 (packed-numpy)'}") - self._latest_frame_ts: float = 0.0 + self._latest_header = Header(frame_id=frame_id) self._disposed = False def _check_disposed(self) -> None: @@ -74,21 +82,16 @@ def _check_disposed(self) -> None: def add_frame(self, frame: PointCloud2) -> None: self._check_disposed() - if frame.ts is not None: - self._latest_frame_ts = frame.ts - self._impl.add_frame(frame) + self._latest_header = deepcopy(frame.header) + self._latest_header.frame_id = self._frame_id self.get_global_pointcloud.invalidate_cache(self) self.get_global_pointcloud2.invalidate_cache(self) @simple_mcache def get_global_pointcloud2(self) -> PointCloud2: self._check_disposed() - return PointCloud2( - ensure_legacy_pcd(self.get_global_pointcloud()), - frame_id=self._frame_id, - ts=self._latest_frame_ts if self._latest_frame_ts else time.time(), - ) + return pointcloud_from_xyz(self._impl.points(), header=self._latest_header) @simple_mcache def get_global_pointcloud(self) -> o3d.t.geometry.PointCloud: @@ -116,18 +119,3 @@ def dispose(self) -> None: self.get_global_pointcloud.invalidate_cache(self) # type: ignore[attr-defined] self.get_global_pointcloud2.invalidate_cache(self) # type: ignore[attr-defined] self._impl.dispose() - - -def ensure_legacy_pcd( - pcd_any: o3d.t.geometry.PointCloud | o3d.geometry.PointCloud, -) -> o3d.geometry.PointCloud: - import open3d as o3d # type: ignore[import-untyped] - - if isinstance(pcd_any, o3d.geometry.PointCloud): - return pcd_any - - assert isinstance(pcd_any, o3d.t.geometry.PointCloud), ( - "Input must be a legacy PointCloud or a tensor PointCloud" - ) - - return pcd_any.to_legacy() diff --git a/dimos/mapping/voxels/impl/o3d.py b/dimos/mapping/voxels/impl/o3d.py index 82e80982b97..9fd3e48572d 100644 --- a/dimos/mapping/voxels/impl/o3d.py +++ b/dimos/mapping/voxels/impl/o3d.py @@ -16,33 +16,17 @@ from typing import TYPE_CHECKING +from dimos_generated.sensor_msgs.msg import PointCloud2 import numpy as np +from numpy.typing import NDArray -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 +from dimos.msgs.pointcloud import pointcloud_xyz if TYPE_CHECKING: import open3d as o3d # type: ignore[import-untyped] import open3d.core as o3c # type: ignore[import-untyped] -def ensure_tensor_pcd( - pcd_any: o3d.t.geometry.PointCloud | o3d.geometry.PointCloud, - device: o3c.Device, -) -> o3d.t.geometry.PointCloud: - """Convert legacy / cuda.pybind point clouds into o3d.t.geometry.PointCloud on `device`.""" - import open3d as o3d # type: ignore[import-untyped] - import open3d.core as o3c # type: ignore[import-untyped] - - if isinstance(pcd_any, o3d.t.geometry.PointCloud): - return pcd_any.to(device) - - assert isinstance(pcd_any, o3d.geometry.PointCloud), ( - "Input must be a legacy PointCloud or a tensor PointCloud" - ) - - return o3d.t.geometry.PointCloud.from_legacy(pcd_any, o3c.float32, device) - - class O3dVoxels: """CUDA voxel store: Open3D VoxelBlockGrid hashmap.""" @@ -70,11 +54,12 @@ def __init__( def add_frame(self, frame: PointCloud2) -> None: import open3d.core as o3c # type: ignore[import-untyped] - pcd = ensure_tensor_pcd(frame.pointcloud, self._dev) - if pcd.is_empty(): + values = pointcloud_xyz(frame).astype(np.float32) + values = values[np.isfinite(values).all(axis=1)] + if not len(values): return - pts = pcd.point["positions"].to(self._dev, o3c.float32) + pts = o3c.Tensor(values, device=self._dev, dtype=o3c.float32) vox = (pts / self._voxel_size).floor().to(self._key_dtype) keys_Nx3 = vox.contiguous() @@ -127,7 +112,7 @@ def _carve_and_insert(self, new_keys: o3c.Tensor) -> None: self._hashmap.activate(new_keys) - def points(self) -> np.ndarray: + def points(self) -> NDArray[np.float32]: """Voxel centers, (N, 3) float32.""" import open3d.core as o3c # type: ignore[import-untyped] diff --git a/dimos/mapping/voxels/impl/packed.py b/dimos/mapping/voxels/impl/packed.py index e84580da37b..764c5e05e5d 100644 --- a/dimos/mapping/voxels/impl/packed.py +++ b/dimos/mapping/voxels/impl/packed.py @@ -14,10 +14,12 @@ from __future__ import annotations +from dimos_generated.sensor_msgs.msg import PointCloud2 import numpy as np +from numpy.typing import NDArray from dimos.mapping.voxels.keys import FIELD_BITS as _BITS, FIELD_MASK as _MASK, KEY_OFFSET as _BIAS -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 +from dimos.msgs.pointcloud import pointcloud_xyz class PackedVoxels: @@ -31,10 +33,11 @@ class PackedVoxels: def __init__(self, voxel_size: float, carve_columns: bool) -> None: self._voxel_size = voxel_size self._carve_columns = carve_columns - self._keys = np.empty(0, dtype=np.int64) + self._keys: NDArray[np.int64] = np.empty(0, dtype=np.int64) def add_frame(self, frame: PointCloud2) -> None: - pts = frame.points_f32() + pts = pointcloud_xyz(frame).astype(np.float32) + pts = pts[np.isfinite(pts).all(axis=1)] if not len(pts): return vox = np.floor(pts / np.float32(self._voxel_size)).astype(np.int64) @@ -63,7 +66,7 @@ def add_frame(self, frame: PointCloud2) -> None: pos, new = pos[fresh], new[fresh] self._keys = np.insert(keys, pos, new) - def points(self) -> np.ndarray: + def points(self) -> NDArray[np.float32]: """Voxel centers, (N, 3) float32.""" k = self._keys vox = np.empty((len(k), 3), dtype=np.float32) diff --git a/dimos/mapping/voxels/module.py b/dimos/mapping/voxels/module.py index b273889a4c0..b67ebffb419 100644 --- a/dimos/mapping/voxels/module.py +++ b/dimos/mapping/voxels/module.py @@ -16,6 +16,8 @@ from typing import TYPE_CHECKING, Any +from dimos_generated.sensor_msgs.msg import PointCloud2 + from dimos.core.core import rpc from dimos.core.module import ModuleConfig from dimos.core.stream import In, Out @@ -23,7 +25,6 @@ from dimos.memory.module import StreamModule from dimos.memory.stream import Stream from dimos.memory.transform import Transformer -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 if TYPE_CHECKING: from collections.abc import Iterator diff --git a/dimos/mapping/voxels/test_grid.py b/dimos/mapping/voxels/test_grid.py new file mode 100644 index 00000000000..365661d437f --- /dev/null +++ b/dimos/mapping/voxels/test_grid.py @@ -0,0 +1,48 @@ +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from dimos_generated.builtin_interfaces.msg import Time +from dimos_generated.std_msgs.msg import Header +import numpy as np +import pytest + +from dimos.mapping.voxels.grid import VoxelGrid +from dimos.msgs.pointcloud import pointcloud_from_xyz, pointcloud_xyz + + +def test_cpu_accumulation_preserves_latest_exact_stamp_and_output_frame() -> None: + first = pointcloud_from_xyz( + np.array([[0.25, 0.25, 0.25]], dtype=np.float32), + header=Header(frame_id="lidar", stamp=Time(sec=1, nanosec=987654321)), + ) + second = pointcloud_from_xyz( + np.array([[1.25, 1.25, 1.25]], dtype=np.float32), + header=Header(frame_id="lidar", stamp=Time(sec=2, nanosec=123456789)), + ) + grid = VoxelGrid(voxel_size=0.5, device="CPU:0", carve_columns=False, frame_id="map") + try: + grid.add_frame(first) + grid.add_frame(second) + result = grid.get_global_pointcloud2() + decoded = type(result).decode(result.encode()) + assert decoded.header.frame_id == "map" + assert decoded.header.stamp == second.header.stamp + assert first.header.frame_id == second.header.frame_id == "lidar" + np.testing.assert_allclose( + pointcloud_xyz(decoded), [[0.25, 0.25, 0.25], [1.25, 1.25, 1.25]] + ) + finally: + grid.dispose() + with pytest.raises(RuntimeError, match="disposed"): + grid.get_global_pointcloud2() diff --git a/dimos/memory/architecture.md b/dimos/memory/architecture.md index 1e8e6316a17..5e3ce102149 100644 --- a/dimos/memory/architecture.md +++ b/dimos/memory/architecture.md @@ -17,7 +17,7 @@ Store → Stream → [filters / transforms / terminals] → Stream → [filters Supporting Systems: - BlobStore — separates large payloads from metadata. FileBlobStore (files on disk) and SqliteBlobStore (blob table per stream). Supports lazy loading. -- Codecs — codec_for() auto-selects: JpegCodec for images (TurboJPEG, ~10-20x compression), LcmCodec for DimOS messages, PickleCodec fallback. +- Codecs — codec_for() selects CdrCodec for generated messages, including raw Image and explicit CompressedImage values. Python objects use PickleCodec. SQLite can wrap CDR in LZ4; MCAP uses chunk compression. - Transformers — Transformer[T,R] ABC wrapping iterator-to-iterator. EmbedImages/EmbedText enrich observations with embeddings. QualityWindow keeps best per time window. - Backpressure Buffers — KeepLast, Bounded, DropNew, Unbounded — bridge push/pull for live mode. diff --git a/dimos/memory/cli/dataset.py b/dimos/memory/cli/dataset.py index a9e16648585..932a756d599 100644 --- a/dimos/memory/cli/dataset.py +++ b/dimos/memory/cli/dataset.py @@ -17,7 +17,7 @@ One entry point for every CLI that opens a recording. :func:`open_dataset` resolves a dataset name/path (bare names look up the cwd / repo ``data/`` dir) and picks the store by file extension: ``.db`` -> SqliteStore, ``.mcap`` -> -Go2McapStore. Use :func:`open_store` when the path is already resolved. +McapStore. Use :func:`open_store` when the path is already resolved. """ from __future__ import annotations @@ -35,9 +35,9 @@ def open_store(path: str | Path) -> Store: """Open an already-resolved dataset *path*, dispatching on its extension.""" s = str(path) if s.endswith(".mcap"): - from dimos.robot.unitree.go2.dds.store import Go2McapStore # lazy: robot-layer codecs + from dimos.memory.store.mcap import McapStore - return Go2McapStore(path=s) + return McapStore(path=s) if s.endswith(".db"): from dimos.memory.store.sqlite import SqliteStore diff --git a/dimos/memory/cli/render.py b/dimos/memory/cli/render.py index 1b6729f8023..e60664b6456 100644 --- a/dimos/memory/cli/render.py +++ b/dimos/memory/cli/render.py @@ -15,8 +15,8 @@ """Render any memory store into rerun. Generic: walks the store's streams and logs every observation whose payload -implements ``to_rerun()`` (the :class:`RerunConvertible` convention). Streams -whose payload has no ``to_rerun`` are skipped. Each stream becomes an entity +has a generated-message viewer helper or an explicit application ``to_rerun()`` +adapter. Unsupported payloads are skipped. Each stream becomes an entity path; observations share one ``time`` timeline (relative to the store's earliest observation, so streams stay aligned). CameraInfo streams are the exception: logged once as a Pinhole on their matching image entity (see @@ -25,16 +25,56 @@ from __future__ import annotations +from copy import deepcopy from pathlib import Path import shutil import subprocess from typing import TYPE_CHECKING, Any if TYPE_CHECKING: + from dimos_generated.sensor_msgs.msg import CameraInfo + from dimos.memory.store.base import Store from dimos.memory.stream import Stream from dimos.memory.type.observation import Observation - from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo + + +def _render_message(data: Any) -> Any: + """Render generated standard messages, or an application's explicit adapter.""" + from dimos_generated.geometry_msgs.msg import PointStamped, PoseStamped + from dimos_generated.nav_msgs.msg import OccupancyGrid, Odometry, Path + from dimos_generated.sensor_msgs.msg import CameraInfo, CompressedImage, Image, PointCloud2 + from dimos_generated.tf2_msgs.msg import TFMessage + from dimos_generated.vision_msgs.msg import Detection3DArray + + from dimos.visualization.rerun.message_helpers import ( + camera_pinhole, + cloud_archetype, + detection_boxes, + image_archetype, + navigation_archetype, + occupancy_mesh, + tf_archetypes, + ) + + if isinstance(data, CameraInfo): + return camera_pinhole(data) + if isinstance(data, (Image, CompressedImage)): + return image_archetype(data) + if isinstance(data, PointCloud2): + return cloud_archetype(data) + if isinstance(data, TFMessage): + return tf_archetypes(data) + if isinstance(data, (PointStamped, PoseStamped, Odometry, Path)): + return navigation_archetype(data) + if isinstance(data, OccupancyGrid): + return occupancy_mesh(data) + if isinstance(data, Detection3DArray): + return detection_boxes(data) + adapter = getattr(data, "to_rerun", None) + if callable(adapter): + return adapter() + raise TypeError(f"no viewer adapter for {type(data).__name__}") def _pair_camera_infos( @@ -46,27 +86,30 @@ def _pair_camera_infos( has one of each; the image's frame_id wins as the pinhole's parent frame. Returns (image stream -> (CameraInfo, parent frame), paired stream names). """ - from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo - from dimos.msgs.sensor_msgs.Image import Image + from dimos_generated.sensor_msgs.msg import CameraInfo, CompressedImage, Image infos = [(n, f.data) for n, _, f in renderable if isinstance(f.data, CameraInfo)] - images = [(n, f.data) for n, _, f in renderable if isinstance(f.data, Image)] + images = [(n, f.data) for n, _, f in renderable if isinstance(f.data, (Image, CompressedImage))] pinholes: dict[str, tuple[CameraInfo, str]] = {} paired: set[str] = set() for info_name, info in infos: - targets = [(n, img) for n, img in images if img.frame_id and img.frame_id == info.frame_id] + targets = [ + (n, img) + for n, img in images + if img.header.frame_id and img.header.frame_id == info.header.frame_id + ] if not targets and len(infos) == 1 and len(images) == 1: targets = [images[0]] print( - f" {info_name}: frame_id {info.frame_id!r} matches no image stream, " + f" {info_name}: frame_id {info.header.frame_id!r} matches no image stream, " f"pairing with {targets[0][0]!r} as the only image stream" ) if not targets: - print(f" {info_name}: no image stream matches frame_id {info.frame_id!r}") + print(f" {info_name}: no image stream matches frame_id {info.header.frame_id!r}") continue for target, img in targets: - pinholes[target] = (info, img.frame_id or info.frame_id) + pinholes[target] = (info, img.header.frame_id or info.header.frame_id) paired.add(info_name) return pinholes, paired @@ -81,7 +124,7 @@ def _frame_first_seen( first tf message overall (which may be an unrelated or static edge). Scans each tf stream only until every frame is found. """ - from dimos.msgs.tf2_msgs.TFMessage import TFMessage + from dimos_generated.tf2_msgs.msg import TFMessage seen: dict[str, float] = {} for _, stream, first in renderable: @@ -124,11 +167,12 @@ def render_store( (``/``) — except a stream whose name matches ``root``'s last segment, which stays at ```` itself. Returns the ``.rrd`` path. """ + from dimos_generated.tf2_msgs.msg import TFMessage import rerun as rr from dimos.memory.utils.progress import progress - from dimos.msgs.tf2_msgs.TFMessage import TFMessage from dimos.visualization.rerun.init import rerun_init + from dimos.visualization.rerun.message_helpers import camera_pinhole if out is None: src = getattr(store.config, "path", None) or "store" @@ -151,13 +195,10 @@ def entity(name: str) -> str: except LookupError: continue data = first.data - if not hasattr(data, "to_rerun"): - print(f" skip {name}: {type(data).__name__} has no to_rerun()") - continue try: - data.to_rerun() + _render_message(data) except Exception as e: - print(f" skip {name}: to_rerun() failed ({e})") + print(f" skip {name}: viewer conversion failed ({e})") continue renderable.append((name, stream, first)) @@ -180,7 +221,9 @@ def entity(name: str) -> str: rr.save(out) for image_name, (info, frame) in pinholes.items(): - pinhole = info.to_rerun_pinhole(optical_frame=frame) + calibration = deepcopy(info) + calibration.header.frame_id = frame + pinhole = camera_pinhole(calibration) at = frame_at.get(frame) if not has_tf: rr.log(entity(image_name), pinhole, static=True) @@ -205,7 +248,7 @@ def entity(name: str) -> str: report(obs) continue rr.set_time("time", duration=obs.ts - t0) - data = obs.data.to_rerun() + data = _render_message(obs.data) path = entity(name) if isinstance(data, list): # RerunMulti: [(subpath, archetype), ...] for sub, arch in data: diff --git a/dimos/memory/cli/test_render.py b/dimos/memory/cli/test_render.py index acb0be178b4..1b3f4540ecc 100644 --- a/dimos/memory/cli/test_render.py +++ b/dimos/memory/cli/test_render.py @@ -15,16 +15,17 @@ from pathlib import Path from types import SimpleNamespace +from dimos_generated.geometry_msgs.msg import Quaternion, Transform, TransformStamped, Vector3 +from dimos_generated.sensor_msgs.msg import CameraInfo, Image +from dimos_generated.std_msgs.msg import Header +from dimos_generated.tf2_msgs.msg import TFMessage import numpy as np from dimos.memory.cli.render import _frame_first_seen, _pair_camera_infos, render_store from dimos.memory.store.memory import MemoryStore -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Transform import Transform -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo -from dimos.msgs.sensor_msgs.Image import Image -from dimos.msgs.tf2_msgs.TFMessage import TFMessage +from dimos.msgs.camera_info import camera_info_from_intrinsics +from dimos.msgs.image import image_from_array +from dimos.msgs.time import time_from_seconds def _entry(name: str, data: object) -> tuple: # type: ignore[type-arg] @@ -32,13 +33,21 @@ def _entry(name: str, data: object) -> tuple: # type: ignore[type-arg] def _info(frame_id: str) -> CameraInfo: - return CameraInfo.from_intrinsics( - fx=500.0, fy=500.0, cx=320.0, cy=240.0, width=640, height=480, frame_id=frame_id + return camera_info_from_intrinsics( + fx=500.0, + fy=500.0, + cx=320.0, + cy=240.0, + width=640, + height=480, + header=Header(frame_id=frame_id), ) def _image(frame_id: str) -> Image: - return Image(data=np.zeros((4, 4, 3), dtype=np.uint8), frame_id=frame_id) + return image_from_array( + np.zeros((4, 4, 3), dtype=np.uint8), encoding="bgr8", header=Header(frame_id=frame_id) + ) def test_pairs_by_frame_id() -> None: @@ -50,7 +59,7 @@ def test_pairs_by_frame_id() -> None: pinholes, paired = _pair_camera_infos(renderable) assert set(pinholes) == {"color_image"} info, frame = pinholes["color_image"] - assert info.frame_id == "camera_optical" + assert info.header.frame_id == "camera_optical" assert frame == "camera_optical" assert paired == {"camera_info"} @@ -98,13 +107,13 @@ def test_no_infos() -> None: def _tf(child: str, ts: float) -> TFMessage: return TFMessage( - Transform( - translation=Vector3(1.0, 0.0, 0.0), - rotation=Quaternion(0.0, 0.0, 0.0, 1.0), - frame_id="world", - child_frame_id=child, - ts=ts, - ) + transforms=[ + TransformStamped( + header=Header(frame_id="world", stamp=time_from_seconds(ts)), + child_frame_id=child, + transform=Transform(translation=Vector3(x=1), rotation=Quaternion(w=1)), + ) + ] ) diff --git a/dimos/memory/codecs/README.md b/dimos/memory/codecs/README.md index 719369f29a7..921608a3f41 100644 --- a/dimos/memory/codecs/README.md +++ b/dimos/memory/codecs/README.md @@ -1,57 +1,35 @@ -# codecs +# Storage codecs -Encode/decode payloads for persistent storage. Codecs convert typed Python objects to `bytes` and back, used by backends that store observation data as blobs. +Codecs convert Python values to bytes for blob-backed stores such as SQLite. +Generated messages use the same CDR bytes as typed transports. Raw images retain +their pixels and encoding; lossy compression is an explicit conversion to a +`sensor_msgs/msg/CompressedImage` value before storage. -## Protocol +| Codec | Values | Stored representation | +|-------|--------|-----------------------| +| `CdrCodec` | Generated messages | Encapsulated CDR, with no private wrapper | +| `Lz4Codec` | Values accepted by its inner codec | LZ4 frame around the inner bytes | +| `PickleCodec` | Python objects | Python pickle; separate from typed message storage | ```python -class Codec(Protocol[T]): - def encode(self, value: T) -> bytes: ... - def decode(self, data: bytes) -> T: ... -``` - -## Built-in codecs - -| Codec | Type | Notes | -|-------|------|-------| -| `PickleCodec` | Any Python object | Fallback. Uses `HIGHEST_PROTOCOL`. | -| `JpegCodec` | `Image` | Lossy compression via TurboJPEG. ~10-20x smaller. Preserves `frame_id` in header. | -| `LcmCodec` | `DimosMsg` subclasses | Uses `lcm_encode()`/`lcm_decode()`. Zero-copy for LCM message types. | - -## Auto-selection - -`codec_for(payload_type)` picks the right codec: - -```python -from dimos.memory.codecs import codec_for - -codec_for(Image) # → JpegCodec(quality=50) -codec_for(SomeLcmMsg) # → LcmCodec(SomeLcmMsg) (if has lcm_encode/lcm_decode) -codec_for(dict) # → PickleCodec() (fallback) -codec_for(None) # → PickleCodec() -``` - -## Writing a new codec - -1. Create `dimos/memory/codecs/mycodec.py`: - -```python -class MyCodec: - def encode(self, value: MyType) -> bytes: - ... +from dimos_generated.sensor_msgs.msg import Image, CompressedImage +from dimos.memory.codecs.base import codec_for - def decode(self, data: bytes) -> MyType: - ... +codec_for(Image) # CdrCodec(Image), lossless +codec_for(CompressedImage) # CdrCodec(CompressedImage) +codec_for(dict) # PickleCodec() ``` -2. Add a branch in `codec_for()` in `base.py` to auto-select it for the relevant type. - -3. Add a test case to `test_codecs.py` — the grid fixture makes this easy: +SQLite persists the codec ID and Python payload type so reopening a stream uses +the same codec. Use `codec="lz4+cdr"` for lossless blob compression. Old `lcm`, +`lz4+lcm`, and `jpeg` storage codec IDs are rejected; no legacy decoder is retained. -```python -@pytest.fixture(params=[..., ("mycodec", MyCodec(), sample_value)]) -def codec_case(request): - ... -``` +MCAP uses `cdr` channels with embedded `ros2msg` definitions and chunk compression. +The reader resolves installed message types by qualified schema name. Unknown +schemas remain inspectable as raw bytes; payload-module metadata never triggers +an arbitrary import. Compressed images remain CompressedImage values when read. +Multiple channels on one topic must agree on schema and timestamp policy. -No base class needed — `Codec` is a protocol. Just implement `encode` and `decode`. +A custom codec only needs `encode(value) -> bytes` and `decode(bytes) -> value`. +Implement the `Codec` protocol and supply the instance explicitly to a stream. +Adding a generated message does not require a new storage codec. diff --git a/dimos/memory/codecs/base.py b/dimos/memory/codecs/base.py index 7b77609b36f..fbfdad2fa27 100644 --- a/dimos/memory/codecs/base.py +++ b/dimos/memory/codecs/base.py @@ -17,6 +17,10 @@ import importlib from typing import Any, Protocol, TypeVar, runtime_checkable +from dimos.memory.codecs.cdr import CdrCodec +from dimos.memory.codecs.lz4 import Lz4Codec +from dimos.memory.codecs.pickle import PickleCodec + T = TypeVar("T") @@ -30,24 +34,15 @@ def decode(self, data: bytes) -> T: ... def codec_for(payload_type: type[Any] | None = None) -> Codec[Any]: """Auto-select codec based on payload type.""" - from dimos.memory.codecs.pickle import PickleCodec - - if payload_type is not None: - from dimos.msgs.sensor_msgs.Image import Image - - if issubclass(payload_type, Image): - from dimos.memory.codecs.jpeg import JpegCodec - - return JpegCodec() - if hasattr(payload_type, "lcm_encode") and hasattr(payload_type, "lcm_decode"): - from dimos.memory.codecs.lcm import LcmCodec - - return LcmCodec(payload_type) + if payload_type is not None and all( + hasattr(payload_type, name) for name in ("encode", "decode", "msg_name", "schema") + ): + return CdrCodec(payload_type) return PickleCodec() def codec_id(codec: Codec[Any]) -> str: - """Derive a string ID from a codec instance, e.g. ``'lz4+lcm'``. + """Derive a string ID from a codec instance, e.g. ``'lz4+cdr'``. Walks the ``_inner`` chain for wrapper codecs, joining with ``+``. Uses the naming convention ``FooCodec`` → ``'foo'``. @@ -62,7 +57,7 @@ def codec_id(codec: Codec[Any]) -> str: def codec_from_id(codec_id_str: str, payload_module: str) -> Codec[Any]: - """Reconstruct a codec chain from its string ID (e.g. ``'lz4+lcm'``). + """Reconstruct a codec chain from its string ID (e.g. ``'lz4+cdr'``). Builds inside-out: the rightmost segment is the innermost (base) codec. """ @@ -92,21 +87,19 @@ def resolve_payload_type(payload_module: str) -> type[Any]: def _make_one(name: str, payload_module: str, inner: Codec[Any] | None = None) -> Codec[Any]: """Instantiate a single codec by its short name.""" if name == "lz4": - from dimos.memory.codecs.lz4 import Lz4Codec - if inner is None: raise ValueError("lz4 is a wrapper codec — must have an inner codec") return Lz4Codec(inner) - if name == "jpeg": - from dimos.memory.codecs.jpeg import JpegCodec - - return JpegCodec() - if name == "lcm": - from dimos.memory.codecs.lcm import LcmCodec - - return LcmCodec(resolve_payload_type(payload_module)) + if name == "cdr": + return CdrCodec(resolve_payload_type(payload_module)) if name == "pickle": - from dimos.memory.codecs.pickle import PickleCodec - return PickleCodec() + if name in {"lcm", "jpeg"}: + raise ValueError( + f"Legacy recording codec {name!r} is unsupported after the CDR cutover. " + "Create a new CDR recording with the installed generated message package; " + "historical recordings must be exported with their original compatible checkout. " + "No legacy payload fallback or in-place conversion is provided. " + "See docs/development/messages.md." + ) raise ValueError(f"Unknown codec: {name!r}") diff --git a/dimos/memory/codecs/lcm.py b/dimos/memory/codecs/cdr.py similarity index 67% rename from dimos/memory/codecs/lcm.py rename to dimos/memory/codecs/cdr.py index 9dd010aa3d2..51d127d983a 100644 --- a/dimos/memory/codecs/lcm.py +++ b/dimos/memory/codecs/cdr.py @@ -20,10 +20,14 @@ from dimos.msgs.protocol import DimosMsg -class LcmCodec: - """Codec for DimosMsg types — uses lcm_encode/lcm_decode.""" +class CdrCodec: + """Store a generated message as its transport-neutral CDR bytes.""" def __init__(self, msg_type: type[DimosMsg]) -> None: + if not all( + hasattr(msg_type, member) for member in ("encode", "decode", "msg_name", "schema") + ): + raise TypeError(f"{msg_type!r} is not a generated CDR message type") self._msg_type = msg_type @property @@ -32,7 +36,9 @@ def payload_type(self) -> type[DimosMsg]: return self._msg_type def encode(self, value: DimosMsg) -> bytes: - return value.lcm_encode() + if not isinstance(value, self._msg_type): + raise TypeError(f"Expected {self._msg_type.msg_name}, got {type(value).__name__}") + return value.encode() def decode(self, data: bytes) -> DimosMsg: - return self._msg_type.lcm_decode(data) + return self._msg_type.decode(data) diff --git a/dimos/memory/codecs/jpeg.py b/dimos/memory/codecs/jpeg.py deleted file mode 100644 index 8f1bf8625bd..00000000000 --- a/dimos/memory/codecs/jpeg.py +++ /dev/null @@ -1,39 +0,0 @@ -# Copyright 2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from __future__ import annotations - -from dimos.msgs.sensor_msgs.Image import Image - - -class JpegCodec: - """Codec for Image types — JPEG-compressed inside an LCM Image envelope. - - Uses ``Image.lcm_jpeg_encode/decode`` which preserves ``ts``, ``frame_id``, - and all LCM header fields. Pixel data is lossy-compressed via TurboJPEG. - """ - - def __init__(self, quality: int = 50) -> None: - self._quality = quality - - @property - def payload_type(self) -> type[Image]: - """Message type decoded by this storage codec.""" - return Image - - def encode(self, value: Image) -> bytes: - return value.lcm_jpeg_encode(quality=self._quality) - - def decode(self, data: bytes) -> Image: - return Image.lcm_jpeg_decode(data) diff --git a/dimos/memory/codecs/test_codecs.py b/dimos/memory/codecs/test_codecs.py index 6c647bf110c..0b642e1a067 100644 --- a/dimos/memory/codecs/test_codecs.py +++ b/dimos/memory/codecs/test_codecs.py @@ -12,207 +12,112 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""Grid tests for Codec implementations. - -Runs roundtrip encode→decode tests across every codec, verifying data preservation. -""" - -from __future__ import annotations - -from dataclasses import dataclass -from typing import TYPE_CHECKING, Any +"""Storage uses generated CDR without altering message fields or image pixels.""" +from dimos_generated.builtin_interfaces.msg import Time +from dimos_generated.geometry_msgs.msg import Point, Pose, PoseStamped +from dimos_generated.sensor_msgs.msg import CompressedImage, Image +from dimos_generated.std_msgs.msg import Header +import numpy as np import pytest -from dimos.memory.codecs.base import Codec, codec_for -from dimos.memory.codecs.jpeg import JpegCodec -from dimos.memory.codecs.lcm import LcmCodec +from dimos.memory.codecs.base import codec_for, codec_from_id, codec_id +from dimos.memory.codecs.cdr import CdrCodec +from dimos.memory.codecs.lz4 import Lz4Codec from dimos.memory.codecs.pickle import PickleCodec -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.sensor_msgs.Image import Image, ImageFormat - -if TYPE_CHECKING: - from collections.abc import Callable - - from dimos.msgs.protocol import DimosMsg - - -@dataclass -class Case: - name: str - codec: Codec[Any] - values: list[Any] - eq: Callable[[Any, Any], bool] | None = None # custom equality: (original, decoded) -> bool - - -def _lcm_values() -> list[DimosMsg]: - from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped - from dimos.msgs.geometry_msgs.Quaternion import Quaternion - from dimos.msgs.geometry_msgs.Vector3 import Vector3 - - return [ - PoseStamped( - ts=1.0, - frame_id="map", - position=Vector3(1.0, 2.0, 3.0), - orientation=Quaternion(0.0, 0.0, 0.0, 1.0), - ), - PoseStamped(ts=0.5, frame_id="odom"), - ] - - -def _pickle_case() -> Case: - from dimos.memory.codecs.pickle import PickleCodec - - return Case( - name="pickle", - codec=PickleCodec(), - values=[42, "hello", b"raw bytes", {"key": "value"}], - ) - - -def _lcm_case() -> Case: - from dimos.memory.codecs.lcm import LcmCodec - from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped - - return Case( - name="lcm", - codec=LcmCodec(PoseStamped), - values=_lcm_values(), - ) - - -def _lz4_pickle_case() -> Case: - from dimos.memory.codecs.lz4 import Lz4Codec - from dimos.memory.codecs.pickle import PickleCodec - - return Case( - name="lz4+pickle", - codec=Lz4Codec(PickleCodec()), - values=[42, "hello", b"raw bytes", {"key": "value"}, list(range(1000))], +from dimos.memory.store.sqlite import SqliteStore +from dimos.memory.type.observation import Observation +from dimos.msgs.geometry import transform_from_pose +from dimos.msgs.image import image_from_array, image_to_jpeg, image_view + + +@pytest.fixture(params=["pose", "image", "depth", "compressed"]) +def message(request): + header = Header(frame_id="sensor", stamp=Time(sec=1700000000, nanosec=123456789)) + if request.param == "pose": + return PoseStamped(header=header, pose=Pose(position=Point(x=1.25, y=-2.5, z=3.75))) + if request.param == "depth": + return image_from_array( + np.array([[0, 1, 65535]], dtype=np.uint16), encoding="16UC1", header=header + ) + image = image_from_array( + np.full((8, 8, 3), [20, 80, 140], dtype=np.uint8), encoding="rgb8", header=header ) - - -def _lz4_lcm_case() -> Case: - from dimos.memory.codecs.lcm import LcmCodec - from dimos.memory.codecs.lz4 import Lz4Codec - from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped - - return Case( - name="lz4+lcm", - codec=Lz4Codec(LcmCodec(PoseStamped)), - values=_lcm_values(), - ) - - -def _jpeg_eq(original: Any, decoded: Any) -> bool: - """JPEG is lossy and normalizes to RGB — check shape, frame_id, RGB tag, and color closeness. - - Compares against ``original.to_rgb()`` because the codec normalizes everything to RGB on - the wire (so a BGR-tagged input comes back RGB-tagged with channels swapped accordingly). - """ - import numpy as np - - if decoded.data.shape != original.data.shape: - return False - if decoded.frame_id != original.frame_id: - return False - if decoded.format != ImageFormat.RGB: - return False - expected = original.to_rgb().data - return bool(np.mean(np.abs(decoded.data.astype(float) - expected.astype(float))) < 5) - - -def _turbojpeg_available() -> bool: - try: - from turbojpeg import TurboJPEG - - TurboJPEG() # fail fast if native lib is missing - except (ImportError, RuntimeError): - return False - return True - - -def _jpeg_case() -> Case | None: - if not _turbojpeg_available(): - return None - - import numpy as np - - # smooth gradients survive lossy jpeg within the eq tolerance - frames = [] - for shift in (0, 90, 180): - arr = np.zeros((48, 64, 3), np.uint8) - arr[..., 0] = np.linspace(0, 255, 64, dtype=np.uint8) - arr[..., 1] = np.linspace(0, 255, 48, dtype=np.uint8)[:, None] - arr[..., 2] = shift - frames.append(Image(data=arr, format=ImageFormat.RGB, frame_id="cam", ts=1.0)) - - return Case( - name="jpeg", - codec=JpegCodec(quality=95), - values=frames, - eq=_jpeg_eq, - ) - - -_case_factories = { - "pickle": _pickle_case, - "lcm": _lcm_case, - "lz4+pickle": _lz4_pickle_case, - "lz4+lcm": _lz4_lcm_case, - "jpeg": _jpeg_case, -} - -case_params: list[Any] = ["pickle", "lcm", "lz4+pickle", "lz4+lcm"] -if _turbojpeg_available(): - case_params.append("jpeg") - - -@pytest.fixture -def case(request: pytest.FixtureRequest) -> Case: - resolved = _case_factories[request.param]() - if resolved is None: - pytest.skip(f"no usable data for the {request.param} case") - return resolved - - -@pytest.mark.parametrize("case", case_params, indirect=True) -class TestCodecRoundtrip: - """Every codec must perfectly roundtrip its values.""" - - def test_roundtrip_preserves_value(self, case: Case) -> None: - eq = case.eq or (lambda a, b: a == b) - for value in case.values: - encoded = case.codec.encode(value) - assert isinstance(encoded, bytes) - decoded = case.codec.decode(encoded) - assert eq(value, decoded), f"Roundtrip failed for {value!r}: got {decoded!r}" - - def test_encode_returns_nonempty_bytes(self, case: Case) -> None: - for value in case.values: - encoded = case.codec.encode(value) - assert len(encoded) > 0, f"Empty encoding for {value!r}" - - def test_different_values_produce_different_bytes(self, case: Case) -> None: - encodings = [case.codec.encode(v) for v in case.values] - assert len(set(encodings)) > 1, "All values encoded to identical bytes" - - -class TestCodecFor: - """codec_for() auto-selects the right codec.""" - - def test_none_returns_pickle(self) -> None: - assert isinstance(codec_for(None), PickleCodec) - - def test_unknown_type_returns_pickle(self) -> None: - assert isinstance(codec_for(dict), PickleCodec) - - def test_lcm_type_returns_lcm(self) -> None: - assert isinstance(codec_for(PoseStamped), LcmCodec) - - def test_image_type_returns_jpeg(self) -> None: - pytest.importorskip("turbojpeg") - from dimos.memory.codecs.jpeg import JpegCodec - - assert isinstance(codec_for(Image), JpegCodec) + if request.param == "compressed": + return CompressedImage( + header=header, format="rgb8; jpeg compressed bgr8", data=image_to_jpeg(image) + ) + return image + + +@pytest.mark.parametrize("wrapped", [False, True]) +def test_codec_roundtrip_and_reconstruction_preserve_generated_value(message, wrapped): + codec = Lz4Codec(CdrCodec(type(message))) if wrapped else codec_for(type(message)) + name = codec_id(codec) + assert name == ("lz4+cdr" if wrapped else "cdr") + restored = codec_from_id(name, f"{type(message).__module__}.{type(message).__qualname__}") + encoded = codec.encode(message) + decoded = restored.decode(encoded) + assert decoded == message + assert decoded.header.stamp.nanosec == 123456789 + if not wrapped: + assert encoded == message.encode() + if isinstance(message, Image): + np.testing.assert_array_equal(image_view(decoded), image_view(message)) + + +@pytest.mark.parametrize("wrapped", [False, True]) +def test_sqlite_reopen_uses_cdr_and_preserves_exact_message(message, wrapped, tmp_path): + path = tmp_path / "messages.db" + with SqliteStore(path=str(path)) as store: + stream = store.stream("samples", type(message), codec="lz4+cdr" if wrapped else "cdr") + stream.append(message, ts=12.5) + with SqliteStore(path=str(path), must_exist=True) as store: + observation = store.stream("samples").first() + assert observation.ts == 12.5 + assert observation.data == message + assert observation.data.encode() == message.encode() + + +@pytest.mark.parametrize("identifier", ["lcm", "lz4+lcm", "jpeg"]) +def test_obsolete_storage_codecs_are_rejected(identifier): + with pytest.raises(ValueError, match="Legacy recording codec.*unsupported after the CDR"): + codec_from_id(identifier, "builtins.dict") + + +def test_cdr_requires_a_generated_message_type(): + with pytest.raises(TypeError, match="not a generated CDR"): + codec_from_id("cdr", "builtins.dict") + + +def test_cdr_malformed_payload_raises(): + with pytest.raises((ValueError, RuntimeError)): + CdrCodec(PoseStamped).decode(b"bad") + + +@pytest.mark.parametrize("value", [42, "hello", b"raw bytes", {"key": "value"}]) +def test_python_objects_keep_the_explicit_python_storage_path(value): + codec = codec_for(type(value)) + assert isinstance(codec, PickleCodec) + assert codec.decode(codec.encode(value)) == value + + +@pytest.mark.parametrize("stamped_transform", [False, True]) +def test_observation_pose_metadata_accepts_nested_generated_values(stamped_transform): + pose = PoseStamped(header=Header(frame_id="world"), pose=Pose(position=Point(x=1, y=2, z=3))) + value = transform_from_pose(pose, child_frame_id="base") if stamped_transform else pose + observation = Observation(id=0, ts=-0.5, pose=value, _data="payload") + assert observation.pose_tuple == (1, 2, 3, 0, 0, 0, 1) + assert observation.pose == pose.pose + assert observation.pose_stamped.pose == pose.pose + assert observation.pose_stamped.header.stamp == Time(sec=-1, nanosec=500000000) + + +def test_cdr_rejects_a_message_of_the_wrong_type(): + with pytest.raises(TypeError, match="Expected geometry_msgs/msg/PoseStamped"): + CdrCodec(PoseStamped).encode(Image()) + + +def test_unknown_storage_codec_is_not_misdiagnosed_as_historical_recording(): + with pytest.raises(ValueError, match="Unknown codec: 'misspelled'"): + codec_from_id("misspelled", "builtins.dict") diff --git a/dimos/memory/demo_data.py b/dimos/memory/demo_data.py new file mode 100644 index 00000000000..d1abd2a84f6 --- /dev/null +++ b/dimos/memory/demo_data.py @@ -0,0 +1,65 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Small deterministic CDR recordings for offline documentation examples.""" + +import math +from pathlib import Path + +from dimos_generated.geometry_msgs.msg import Point, Pose, PoseStamped, Quaternion +from dimos_generated.sensor_msgs.msg import Image, PointCloud2 +from dimos_generated.std_msgs.msg import Header +import numpy as np + +from dimos.memory.codecs.cdr import CdrCodec +from dimos.memory.codecs.lz4 import Lz4Codec +from dimos.memory.store.sqlite import SqliteStore +from dimos.msgs.image import image_from_array +from dimos.msgs.pointcloud import pointcloud_from_xyz +from dimos.msgs.time import time_from_nanoseconds + + +def write_demo_recording(path: Path, *, samples: int = 720) -> SqliteStore: + """Create a new image/pose/cloud recording at 4 Hz without downloads or models. + + Refuse existing files so examples cannot replace a user's recording. + Images have both exact source stamps and observation poses. Storage uses + LZ4 around generated CDR, retaining complete type/schema metadata. + """ + if path.exists(): + raise FileExistsError(path) + if samples <= 0: + raise ValueError("samples must be positive") + store = SqliteStore(path=path) + images = store.stream("color_image", Image, codec=Lz4Codec(CdrCodec(Image))) + poses = store.stream("odom", PoseStamped, codec=Lz4Codec(CdrCodec(PoseStamped))) + clouds = store.stream("lidar", PointCloud2, codec=Lz4Codec(CdrCodec(PointCloud2))) + x, y = np.meshgrid(np.linspace(-2.0, 2.0, 16), np.linspace(-2.0, 2.0, 16)) + points = np.column_stack((x.ravel(), y.ravel(), np.zeros(x.size))) + for index in range(samples): + stamp = time_from_nanoseconds(1_700_000_000_123_456_789 + index * 250_000_000) + header = Header(stamp=stamp, frame_id="world") + angle = index / 80.0 + pose = Pose( + position=Point(x=math.cos(angle), y=math.sin(angle)), + orientation=Quaternion(w=1.0), + ) + pixels = np.full((48, 64, 3), 40 + index % 180, dtype=np.uint8) + pixels[:, 16:32, 1] = 230 + image = image_from_array(pixels, encoding="rgb8", header=header) + ts = 1_700_000_000.1234567 + index / 4.0 + images.append(image, ts=ts, pose=pose) + poses.append(PoseStamped(header=header, pose=pose), ts=ts, pose=pose) + clouds.append(pointcloud_from_xyz(points, header=header), ts=ts, pose=pose) + return store diff --git a/dimos/memory/module.py b/dimos/memory/module.py index 4293b561cfc..a36dadf0cdf 100644 --- a/dimos/memory/module.py +++ b/dimos/memory/module.py @@ -16,7 +16,6 @@ from collections.abc import Awaitable, Callable from datetime import datetime -import enum import inspect import os from pathlib import Path @@ -24,6 +23,9 @@ import time from typing import TYPE_CHECKING, Any, Generic, TypeVar, cast +from dimos_generated.geometry_msgs.msg import PoseStamped +from dimos_generated.sensor_msgs.msg import Image +from dimos_generated.tf2_msgs.msg import TFMessage from pydantic import Field, field_validator from reactivex import operators as ops from reactivex.disposable import Disposable @@ -34,23 +36,23 @@ from dimos.core.module import Module, ModuleConfig from dimos.core.stream import In from dimos.memory.embed import EmbedImages +from dimos.memory.recording_policy import OnExisting from dimos.memory.store.null import NullStore from dimos.memory.store.sqlite import SqliteStore from dimos.memory.stream import Stream from dimos.memory.transform import QualityWindow from dimos.memory.type.observation import EmbeddedObservation, Observation from dimos.models.embedding.base import EmbeddingModel -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.sensor_msgs.Image import Image -from dimos.msgs.tf2_msgs.TFMessage import TFMessage +from dimos.msgs.geometry import pose_from_transform +from dimos.msgs.time import to_seconds from dimos.utils.data import backup_file from dimos.utils.logging_config import setup_logger if TYPE_CHECKING: + from dimos_generated.geometry_msgs.msg import Pose from reactivex.abc import DisposableBase from dimos.core.stream import Out - from dimos.msgs.geometry_msgs.Pose import Pose logger = setup_logger() @@ -268,13 +270,6 @@ def _similarity(obs: Observation[Any]) -> float: return best.pose_stamped -class OnExisting(str, enum.Enum): - OVERWRITE = "overwrite" - ERROR = "error" - BACKUP = "backup" - APPEND = "append" - - class RecorderConfig(MemoryModuleConfig): on_existing: OnExisting = OnExisting.BACKUP backup_keep_last: int = Field(default=10, ge=0) @@ -289,7 +284,7 @@ class RecorderConfig(MemoryModuleConfig): # read the active remappings from inside the module (AFAIK), so this config # arg does the per-stream rename directly. stream_remapping: dict[str, str] = Field(default_factory=dict) - # ex: {"depth_image_1": "lz4+lcm"} for lossless depth recording + # ex: {"depth_image_1": "lz4+cdr"} for lossless depth recording stream_codecs: dict[str, str] = Field(default_factory=dict) # Port names that inherently have no pose to anchor (command streams, etc.). poseless_streams: list[str] = Field(default_factory=list) @@ -401,7 +396,7 @@ def _data_ports(self) -> dict[str, In[Any]]: def _port_to_stream(self, name: str, input_topic: In[Any], stream: Stream[Any]) -> None: """Append each message from *input_topic* to *stream*, attaching world pose via tf. - Stamped messages use their own ``.frame_id`` and ``.ts``; unstamped + Stamped messages use their own ``header.frame_id`` and ``header.stamp``; unstamped messages (or ones whose frame isn't in the tf graph, e.g. a payload already in world coords) fall back to ``config.default_frame_id`` — so every observation gets a robot-pose anchor when tf is publishing. @@ -442,7 +437,8 @@ def _prepare_streams(self) -> None: def _resolve_ts(self, name: str, msg: Any) -> float: """Timestamp to record *msg* at. Override to re-base onto another clock.""" - return getattr(msg, "ts", None) or time.time() + header = getattr(msg, "header", None) + return to_seconds(header.stamp) if header is not None else time.time() async def _resolve_pose(self, name: str, msg: Any, ts: float) -> Pose | None: """Pose to anchor *msg* with. Dispatches to the stream's (async) @@ -453,11 +449,12 @@ async def _resolve_pose(self, name: str, msg: Any, ts: float) -> Pose | None: return cast("Pose | None", await setter(msg)) if self._tf is None: return None - frame_id = getattr(msg, "frame_id", None) or self.config.default_frame_id + header = getattr(msg, "header", None) + frame_id = (header.frame_id if header is not None else "") or self.config.default_frame_id transform = self._tf.get( self.config.root_frame, frame_id, time_point=ts, time_tolerance=self.config.tf_tolerance ) - return transform.to_pose() if transform is not None else None + return pose_from_transform(transform).pose if transform is not None else None def _collect_pose_setters(self) -> dict[str, PoseSetter]: """Map stream name -> bound ``@pose_setter_for`` method.""" @@ -478,7 +475,11 @@ def _record_tf(self) -> None: def on_tf(msg: TFMessage) -> None: try: for transform in msg.transforms: - tf_stream.append(TFMessage(transform), ts=transform.ts, pose=None) + tf_stream.append( + TFMessage(transforms=[transform]), + ts=to_seconds(transform.header.stamp), + pose=None, + ) except sqlite3.ProgrammingError: # A late LCM callback raced teardown and hit the closed store. pass diff --git a/dimos/msgs/vision_msgs/BoundingBox2DArray.py b/dimos/memory/recording_policy.py similarity index 67% rename from dimos/msgs/vision_msgs/BoundingBox2DArray.py rename to dimos/memory/recording_policy.py index f376de6372a..388a5b5e81f 100644 --- a/dimos/msgs/vision_msgs/BoundingBox2DArray.py +++ b/dimos/memory/recording_policy.py @@ -1,4 +1,4 @@ -# Copyright 2025-2026 Dimensional Inc. +# Copyright 2026 Dimensional Inc. # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. @@ -12,10 +12,13 @@ # See the License for the specific language governing permissions and # limitations under the License. -from dimos_lcm.vision_msgs.BoundingBox2DArray import ( - BoundingBox2DArray as LCMBoundingBox2DArray, -) +"""File-existence policy shared by Python and native recorders.""" +import enum -class BoundingBox2DArray(LCMBoundingBox2DArray): # type: ignore[misc] - msg_name = "vision_msgs.BoundingBox2DArray" + +class OnExisting(str, enum.Enum): + OVERWRITE = "overwrite" + ERROR = "error" + BACKUP = "backup" + APPEND = "append" diff --git a/dimos/memory/replay_module.py b/dimos/memory/replay_module.py index 04d98351d5b..bda26989f3b 100644 --- a/dimos/memory/replay_module.py +++ b/dimos/memory/replay_module.py @@ -73,21 +73,12 @@ def dataset_path(name: str, *, explicit: bool = True) -> str: return "" -def _wire_type(t: type) -> type: - """The class that owns *t*'s ``msg_name``: the type a port must declare so a consumer - of the base message shares its topic (unitree ``Odometry`` -> ``PoseStamped``).""" - for cls in t.__mro__: - if "msg_name" in vars(cls): - return cls - return t - - def stream_types_of(dataset: str) -> dict[str, type]: """Stream name -> port type for every stream in *dataset*.""" store = open_dataset(dataset) store.start() try: - return {n: _wire_type(t) for n, t in stream_payload_types(store).items()} + return stream_payload_types(store) finally: store.stop() @@ -232,10 +223,9 @@ def recorded_rerun_config(dataset: str) -> dict[str, Any]: def rerun_layout(stream_types: dict[str, type]) -> Any: """A Rerun blueprint showing every recorded stream: one 3D world view, one 2D view per image.""" + from dimos_generated.sensor_msgs.msg import Image import rerun.blueprint as rrb - from dimos.msgs.sensor_msgs.Image import Image - images = [ rrb.Spatial2DView(origin=f"world/{n}", name=n) for n, t in stream_types.items() diff --git a/dimos/memory/store/mcap.py b/dimos/memory/store/mcap.py index 4cb75ec0a40..a1eb3820420 100644 --- a/dimos/memory/store/mcap.py +++ b/dimos/memory/store/mcap.py @@ -14,8 +14,8 @@ """Read-only memory store backed by an mcap file. -Generic and robot-independent. JPEG channels decode automatically because their -payload type is fixed. Other formats use a caller-supplied ``codecs`` map (wire +Generic and robot-independent. CDR channels resolve installed message types by +their ROS schema name. Other formats use a caller-supplied ``codecs`` map (wire topic -> codec), while ``streams`` may map friendly stream names to topics. See ``dimos.robot.unitree.go2.dds.store.Go2McapStore`` for the Go2 DDS wiring. @@ -32,12 +32,13 @@ from dimos.memory.backend import Backend from dimos.memory.codecs.base import codec_for -from dimos.memory.codecs.jpeg import JpegCodec +from dimos.memory.codecs.cdr import CdrCodec from dimos.memory.notifier.subject import SubjectNotifier from dimos.memory.observationstore.base import ObservationStore, ObservationStoreConfig from dimos.memory.store.base import Store, StoreConfig from dimos.memory.type.filter import StreamQuery from dimos.memory.type.observation import Observation +from dimos.msgs.helpers import resolve_msg_type @runtime_checkable @@ -187,14 +188,37 @@ def __init__( # _BYTES_CODEC — reachable but undecoded. _raw maps their stream name to the # source schema so summary() can flag them [raw bytes: ]. self._raw: dict[str, str | None] = {} # raw stream name -> source schema + topic_signatures: dict[str, tuple[Any, ...]] = {} if summary is not None and summary.statistics is not None: for cid, ch in summary.channels.items(): count = summary.statistics.channel_message_counts.get(cid, 0) name = name_of.get(ch.topic) or _slug(ch.topic) - if ch.topic not in self._codecs and ch.message_encoding == "jpeg": - self._codecs[ch.topic] = JpegCodec() + schema = summary.schemas.get(ch.schema_id) + signature = ( + ch.message_encoding, + (schema.name, schema.encoding, schema.data) if schema else None, + ch.metadata.get("dimos.observation_time"), + ) + if ch.topic in topic_signatures and topic_signatures[ch.topic] != signature: + raise ValueError( + f"Topic {ch.topic!r} has conflicting channel schemas or timing policies" + ) + if name in self._stream_topic and self._stream_topic[name] != ch.topic: + raise ValueError( + f"Topics {self._stream_topic[name]!r} and {ch.topic!r} map to stream {name!r}" + ) + topic_signatures[ch.topic] = signature + if ( + ch.topic not in self._codecs + and ch.message_encoding == "cdr" + and schema is not None + and schema.encoding == "ros2msg" + ): + message_type = resolve_msg_type(schema.name) + if message_type is not None: + self._codecs[ch.topic] = CdrCodec(message_type) self._stream_topic[name] = ch.topic - self._available[name] = count + self._available[name] = self._available.get(name, 0) + count self._observation_uses_publish_time[name] = ( ch.metadata.get("dimos.observation_time") == "publish_time" ) diff --git a/dimos/memory/store/test_mcap.py b/dimos/memory/store/test_mcap.py index e6e3496e61b..788682ed21c 100644 --- a/dimos/memory/store/test_mcap.py +++ b/dimos/memory/store/test_mcap.py @@ -12,166 +12,131 @@ # See the License for the specific language governing permissions and # limitations under the License. -from __future__ import annotations - -from pathlib import Path - +from dimos_generated.builtin_interfaces.msg import Time +from dimos_generated.geometry_msgs.msg import Vector3 +from dimos_generated.sensor_msgs.msg import CompressedImage, Imu +from dimos_generated.std_msgs.msg import Header +from mcap.writer import Writer import numpy as np import pytest -from dimos.memory.codecs.jpeg import JpegCodec -from dimos.memory.codecs.lcm import LcmCodec -from dimos.memory.codecs.lz4 import Lz4Codec +from dimos.memory.cli.dataset import open_store +from dimos.memory.codecs.cdr import CdrCodec from dimos.memory.store.mcap import McapStore -from dimos.memory.type.observation import Observation -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.msgs.sensor_msgs.Image import Image, ImageFormat -from dimos.msgs.sensor_msgs.Imu import Imu +from dimos.msgs.image import image_from_array, image_to_jpeg +from dimos.protocol.cdr_mcap import CdrMcapWriter -mcap_writer = pytest.importorskip("mcap.writer", reason="mcap not installed") - -def test_lcm_channel_decodes_with_explicit_codec(tmp_path: Path) -> None: +@pytest.mark.parametrize("explicit", [False, True]) +def test_cdr_channel_decodes_and_orders_by_declared_source_time(tmp_path, explicit): path = tmp_path / "recording.mcap" expected = Imu( - ts=12.5, - frame_id="imu_link", - angular_velocity=Vector3(1.0, 2.0, 3.0), + header=Header(stamp=Time(sec=12, nanosec=500000000), frame_id="imu_link"), + angular_velocity=Vector3(x=1, y=2, z=3), ) + earlier = Imu(header=Header(stamp=Time(sec=11, nanosec=500000000), frame_id="earlier")) with path.open("wb") as output: - writer = mcap_writer.Writer(output) - writer.start(profile="dimos", library="test") - channel_id = writer.register_channel( - topic="imu", - message_encoding="lcm", - schema_id=0, - metadata={ - "dimos.payload_type": "dimos.msgs.sensor_msgs.Imu.Imu", - "dimos.observation_time": "publish_time", - }, + writer = Writer(output) + writer.start(profile="ros2", library="test") + schema = writer.register_schema( + name=Imu.msg_name, encoding="ros2msg", data=Imu.schema.encode() ) - writer.add_message( - channel_id=channel_id, - log_time=13_000_000_000, - publish_time=12_500_000_000, - data=expected.lcm_encode(), - ) - writer.add_message( - channel_id=channel_id, - log_time=14_000_000_000, - publish_time=11_500_000_000, - data=Imu(ts=11.5, frame_id="earlier").lcm_encode(), + channel = writer.register_channel( + topic="imu", + message_encoding="cdr", + schema_id=schema, + metadata={"dimos.observation_time": "publish_time"}, ) + for index, (message, stamp) in enumerate([(expected, 12500000000), (earlier, 11500000000)]): + writer.add_message( + channel_id=channel, + log_time=13000000000 + index * 1000000000, + publish_time=stamp, + data=message.encode(), + ) writer.finish() - - with McapStore(path=str(path), codecs={"imu": LcmCodec(Imu)}) as store: + with McapStore(path=str(path), codecs={"imu": CdrCodec(Imu)} if explicit else None) as store: assert store.list_streams() == ["imu"] - observation: Observation[Imu] = store.stream("imu").order_by("ts").first() - assert observation.ts == 11.5 - assert observation.data.frame_id == "earlier" - - latest_observation: Observation[Imu] = store.stream("imu").order_by("ts", desc=True).first() - assert latest_observation.ts == 12.5 - assert latest_observation.data.lcm_encode() == expected.lcm_encode() - - -def test_wrapped_codec_decodes_with_explicit_codec(tmp_path: Path) -> None: - path = tmp_path / "recording.mcap" - expected = Imu( - ts=12.5, - frame_id="imu_link", - angular_velocity=Vector3(1.0, 2.0, 3.0), + observations = list(store.stream("imu").order_by("ts")) + assert [observation.ts for observation in observations] == [11.5, 12.5] + assert [observation.data for observation in observations] == [earlier, expected] + assert store.stream("imu").order_by("ts", desc=True).first().data == expected + + +def test_standard_compressed_image_uses_cdr_and_generic_dataset_dispatch(tmp_path): + path = tmp_path / "camera.mcap" + image = image_from_array( + np.full((8, 8, 3), [20, 80, 140], dtype=np.uint8), + encoding="rgb8", + header=Header(frame_id="camera", stamp=Time(sec=12, nanosec=123456789)), ) - codec = Lz4Codec(LcmCodec(Imu)) - with path.open("wb") as output: - writer = mcap_writer.Writer(output) - writer.start(profile="dimos", library="test") - channel_id = writer.register_channel( - topic="imu", - message_encoding="lz4+lcm", - schema_id=0, - metadata={ - "dimos.payload_type": "dimos.msgs.sensor_msgs.Imu.Imu", - "dimos.observation_time": "publish_time", - }, - ) - writer.add_message( - channel_id=channel_id, - log_time=13_000_000_000, - publish_time=12_500_000_000, - data=codec.encode(expected), - ) - writer.finish() - - with McapStore(path=str(path), codecs={"imu": codec}) as store: - observation: Observation[Imu] = store.stream("imu").first() - assert observation.ts == 12.5 - assert observation.data.lcm_encode() == expected.lcm_encode() - - -def test_self_describing_jpeg_channel_decodes_without_a_codec_registry(tmp_path: Path) -> None: - path = tmp_path / "recording.mcap" - expected = Image( - data=np.full((8, 8, 3), [20, 80, 140], dtype=np.uint8), - format=ImageFormat.RGB, - frame_id="camera", - ts=12.5, + expected = CompressedImage( + header=image.header, format="rgb8; jpeg compressed bgr8", data=image_to_jpeg(image) ) - codec = JpegCodec() - with path.open("wb") as output: - writer = mcap_writer.Writer(output) - writer.start(profile="dimos", library="test") - channel_id = writer.register_channel( - topic="color_image", - message_encoding="jpeg", - schema_id=0, - metadata={ - "dimos.payload_type": "dimos.msgs.sensor_msgs.Image.Image", - "dimos.observation_time": "publish_time", - }, + with CdrMcapWriter(path) as writer: + writer.write( + "color_image", + expected.encode(), + schema_name=expected.msg_name, + schema=expected.schema, + log_time_ns=13000000000, + publish_time_ns=12123456789, ) - writer.add_message( - channel_id=channel_id, - log_time=13_000_000_000, - publish_time=12_500_000_000, - data=codec.encode(expected), - ) - writer.finish() - - with McapStore(path=str(path)) as store: - observation: Observation[Image] = store.stream("color_image").first() - decoded = observation.data - assert observation.ts == 12.5 - assert decoded.frame_id == "camera" - assert decoded.format is ImageFormat.RGB - assert decoded.data.shape == expected.data.shape - assert np.mean(np.abs(decoded.data.astype(float) - expected.data.astype(float))) < 5 + with open_store(path) as store: + observation = store.stream("color_image").first() + assert observation.ts == 13.0 + assert observation.data == expected + assert observation.data.header.stamp.nanosec == 123456789 + assert observation.data.format == "rgb8; jpeg compressed bgr8" -def test_lcm_metadata_does_not_import_payload_module( - tmp_path: Path, monkeypatch: pytest.MonkeyPatch -) -> None: +def test_unknown_schema_metadata_never_imports_a_payload_module(tmp_path, monkeypatch): path = tmp_path / "untrusted.mcap" with path.open("wb") as output: - writer = mcap_writer.Writer(output) - writer.start(profile="dimos", library="test") - channel_id = writer.register_channel( + writer = Writer(output) + writer.start(profile="ros2", library="test") + schema = writer.register_schema( + name="untrusted/msg/Payload", encoding="ros2msg", data=b"uint8 value\n" + ) + channel = writer.register_channel( topic="untrusted", - message_encoding="lcm", - schema_id=0, + message_encoding="cdr", + schema_id=schema, metadata={"dimos.payload_type": "untrusted_module.Payload"}, ) - writer.add_message( - channel_id=channel_id, - log_time=1, - publish_time=1, - data=b"raw payload", - ) + writer.add_message(channel_id=channel, log_time=1, publish_time=1, data=b"raw payload") writer.finish() - def fail_import(name: str) -> None: + def fail_import(name): raise AssertionError(f"artifact metadata imported {name!r}") - monkeypatch.setattr("dimos.memory.codecs.base.importlib.import_module", fail_import) + monkeypatch.setattr("dimos.memory.codecs.base.resolve_payload_type", fail_import) with McapStore(path=str(path)) as store: assert store.stream("untrusted").first().data == b"raw payload" + assert "untrusted/msg/Payload" in store.summary() + + +@pytest.mark.parametrize("second_type", [Imu, CompressedImage]) +def test_duplicate_topic_channels_count_together_or_reject_conflicting_schemas( + tmp_path, second_type +): + path = tmp_path / "channels.mcap" + with path.open("wb") as output: + writer = Writer(output) + writer.start(profile="ros2") + for cls in [Imu, second_type]: + schema = writer.register_schema( + name=cls.msg_name, encoding="ros2msg", data=cls.schema.encode() + ) + channel = writer.register_channel( + topic="sensor", message_encoding="cdr", schema_id=schema + ) + writer.add_message(channel_id=channel, log_time=1, publish_time=1, data=cls().encode()) + writer.finish() + if second_type is Imu: + with McapStore(path=str(path)) as store: + assert store.stream("sensor").count() == 2 + assert [observation.data for observation in store.stream("sensor")] == [Imu(), Imu()] + else: + with pytest.raises(ValueError, match="conflicting channel schemas"): + McapStore(path=str(path)) diff --git a/dimos/memory/stream.py b/dimos/memory/stream.py index c5b41782444..aaf9a6ed12a 100644 --- a/dimos/memory/stream.py +++ b/dimos/memory/stream.py @@ -25,6 +25,9 @@ else: from typing_extensions import TypeVar +from dimos_generated.geometry_msgs.msg import PoseStamped, TransformStamped, Vector3 +from dimos_generated.nav_msgs.msg import Odometry + from dimos.core.resource import CompositeResource from dimos.memory.backend import Backend from dimos.memory.buffer import BackpressureBuffer, KeepLast @@ -41,7 +44,6 @@ TimeRangeFilter, ) from dimos.memory.type.observation import EmbeddedObservation, Observation -from dimos.msgs.geometry_msgs.Vector3 import Vector3 from dimos.utils.human import human_bytes from dimos.utils.logging_config import setup_logger @@ -242,11 +244,22 @@ def at_relative(self, t: float, tolerance: float = 1.0) -> Stream[T, O]: return self.at(t0 + t, tolerance=tolerance) def near(self, pose: Any, radius: float) -> Stream[T, O]: - # Accept Pose/PoseStamped (any object with `.position`), Vector3, - # numpy arrays, or (x, y, z) tuples — Vector3() handles the rest. + if isinstance(pose, PoseStamped): + pose = pose.pose + elif isinstance(pose, Odometry): + pose = pose.pose.pose + elif isinstance(pose, TransformStamped): + pose = pose.transform.translation if hasattr(pose, "position"): pose = pose.position - return self._with_filter(NearFilter(Vector3(pose), radius)) + if all(hasattr(pose, coordinate) for coordinate in ("x", "y", "z")): + xyz = (pose.x, pose.y, pose.z) + else: + xyz = tuple(pose) + if len(xyz) != 3: + raise ValueError("near requires exactly three position coordinates") + center = Vector3(x=float(xyz[0]), y=float(xyz[1]), z=float(xyz[2])) + return self._with_filter(NearFilter(center, radius)) def tags(self, **tags: Any) -> Stream[T, O]: return self._with_filter(TagsFilter(tags)) diff --git a/dimos/memory/tap.py b/dimos/memory/tap.py index b0726edd6ef..cca4684077a 100644 --- a/dimos/memory/tap.py +++ b/dimos/memory/tap.py @@ -33,9 +33,12 @@ import time from typing import TYPE_CHECKING, Any +from dimos_generated.std_msgs.msg import Header + from dimos.constants import RECORDINGS_DIR from dimos.core.global_config import global_config from dimos.memory.store.sqlite import SqliteStore +from dimos.msgs.time import to_seconds from dimos.utils.logging_config import setup_logger if TYPE_CHECKING: @@ -105,14 +108,18 @@ def tap( """Subscribe *transport* and record into stream *name*; returns the unsubscribe.""" if not matching(self._topics, [name]): return None - if not hasattr(stream_type, "lcm_encode"): + if not all( + hasattr(stream_type, member) for member in ("encode", "decode", "msg_name", "schema") + ): logger.info("--record: %s (%s) is not a dimos message type, skipped", name, stream_type) return None stream: Stream[Any] = self.store.stream(name, stream_type) def on_msg(msg: Any) -> None: try: - self._queue.put_nowait((stream, msg, getattr(msg, "ts", None) or time.time())) + header = getattr(msg, "header", None) + timestamp = to_seconds(header.stamp) if isinstance(header, Header) else time.time() + self._queue.put_nowait((stream, msg, timestamp)) except queue.Full: self.dropped += 1 if self.dropped % 1000 == 1: diff --git a/dimos/memory/test_replay_module.py b/dimos/memory/test_replay_module.py index bcfff4c96d1..356a335672a 100644 --- a/dimos/memory/test_replay_module.py +++ b/dimos/memory/test_replay_module.py @@ -16,12 +16,14 @@ import time from typing import Any +from dimos_generated.geometry_msgs.msg import PoseStamped +from dimos_generated.std_msgs.msg import Header import pytest from dimos.core.stream import Out from dimos.memory.replay_module import replay_module from dimos.memory.store.sqlite import SqliteStore -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped +from dimos.msgs.time import time_from_seconds, to_seconds @pytest.fixture @@ -32,8 +34,8 @@ def recording(tmp_path: Path) -> str: odom = store.stream("odom", PoseStamped) goal = store.stream("goal", PoseStamped) for ts in (1.0, 1.1, 1.2): - odom.append(PoseStamped(ts=ts), ts=ts) - goal.append(PoseStamped(ts=1.05), ts=1.05) + odom.append(PoseStamped(header=Header(stamp=time_from_seconds(ts))), ts=ts) + goal.append(PoseStamped(header=Header(stamp=time_from_seconds(1.05))), ts=1.05) store.stop() return str(path) @@ -52,8 +54,8 @@ def test_module_publishes_recorded_messages(recording: str) -> None: cls = replay_module(recording) module = cls(dataset=recording) got: dict[str, list[float]] = {"odom": [], "goal": []} - module.outputs["odom"].subscribe(lambda m: got["odom"].append(m.ts)) - module.outputs["goal"].subscribe(lambda m: got["goal"].append(m.ts)) + module.outputs["odom"].subscribe(lambda m: got["odom"].append(to_seconds(m.header.stamp))) + module.outputs["goal"].subscribe(lambda m: got["goal"].append(to_seconds(m.header.stamp))) module.start() deadline = time.time() + 5 while time.time() < deadline and len(got["odom"]) < 3: @@ -91,8 +93,12 @@ def test_stream_named_like_a_module_attribute_is_skipped( path = tmp_path / "memory.db" store = SqliteStore(path=str(path)) store.start() - store.stream("start", PoseStamped).append(PoseStamped(ts=1.0), ts=1.0) - store.stream("odom", PoseStamped).append(PoseStamped(ts=1.0), ts=1.0) + store.stream("start", PoseStamped).append( + PoseStamped(header=Header(stamp=time_from_seconds(1.0))), ts=1.0 + ) + store.stream("odom", PoseStamped).append( + PoseStamped(header=Header(stamp=time_from_seconds(1.0))), ts=1.0 + ) store.stop() assert list(replay_module(str(path)).__annotations__) == ["odom"] module = replay_module("")(dataset=str(path)) @@ -101,19 +107,6 @@ def test_stream_named_like_a_module_attribute_is_skipped( assert [c.args[1] for c in log.warning.call_args_list] == ["start", "start"] -def test_subclass_payload_gets_the_base_port_type(tmp_path: Path) -> None: - """A recorded unitree Odometry is a PoseStamped on the wire; the port must say so or a - PoseStamped consumer of the same name lands on a different topic.""" - from dimos.robot.unitree.type.odometry import Odometry - - path = tmp_path / "memory.db" - store = SqliteStore(path=str(path)) - store.start() - store.stream("odom", Odometry).append(Odometry(ts=1.0), ts=1.0) - store.stop() - assert replay_module(str(path)).__annotations__ == {"odom": Out[PoseStamped]} - - def test_single_timestamp_stream_is_republished_and_not_the_anchor(tmp_path: Path) -> None: """camera_info stamped at import sits 39 s before the run; the run must start at odom.""" path = tmp_path / "memory.db" @@ -121,8 +114,10 @@ def test_single_timestamp_stream_is_republished_and_not_the_anchor(tmp_path: Pat store.start() info = store.stream("camera_info", PoseStamped) for _ in range(3): - info.append(PoseStamped(ts=1.0), ts=1.0) - store.stream("odom", PoseStamped).append(PoseStamped(ts=40.0), ts=40.0) + info.append(PoseStamped(header=Header(stamp=time_from_seconds(1.0))), ts=1.0) + store.stream("odom", PoseStamped).append( + PoseStamped(header=Header(stamp=time_from_seconds(40.0))), ts=40.0 + ) store.stop() module = replay_module(str(path))(dataset=str(path)) got: dict[str, int] = {"camera_info": 0, "odom": 0} @@ -160,8 +155,8 @@ def slow(self: Any, obs: Any) -> Any: monkeypatch.setattr(replay_mod.ReplayStream, "_decode", slow) module = replay_module(recording)(dataset=recording) got: dict[str, list[float]] = {"odom": [], "goal": []} - module.outputs["odom"].subscribe(lambda m: got["odom"].append(m.ts)) - module.outputs["goal"].subscribe(lambda m: got["goal"].append(m.ts)) + module.outputs["odom"].subscribe(lambda m: got["odom"].append(to_seconds(m.header.stamp))) + module.outputs["goal"].subscribe(lambda m: got["goal"].append(to_seconds(m.header.stamp))) module.start() deadline = time.time() + 5 while time.time() < deadline and (len(got["odom"]) < 3 or not got["goal"]): diff --git a/dimos/memory/test_tap.py b/dimos/memory/test_tap.py index f408e5a7eab..e25c279cc6e 100644 --- a/dimos/memory/test_tap.py +++ b/dimos/memory/test_tap.py @@ -16,6 +16,8 @@ from pathlib import Path from typing import Any +from dimos_generated.geometry_msgs.msg import PoseStamped, Twist +from dimos_generated.std_msgs.msg import Header import pytest from pytest_mock import MockerFixture @@ -24,7 +26,7 @@ from dimos.memory import tap from dimos.memory.store.sqlite import SqliteStore from dimos.memory.tap import TransportRecorder -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped +from dimos.msgs.time import time_from_seconds class _Transport(Transport[Any]): @@ -57,12 +59,12 @@ def test_taps_matching_dimos_streams(tmp_path: Path) -> None: unsub = rec.tap("odom", PoseStamped, odom) assert rec.tap("goal", PoseStamped, goal) is None # filtered out assert rec.tap("lidar", dict, raw) is None # not a dimos message type - odom.publish(PoseStamped(ts=1.0)) - odom.publish(PoseStamped(ts=2.0)) - goal.publish(PoseStamped(ts=3.0)) + odom.publish(PoseStamped(header=Header(stamp=time_from_seconds(1.0)))) + odom.publish(PoseStamped(header=Header(stamp=time_from_seconds(2.0)))) + goal.publish(PoseStamped(header=Header(stamp=time_from_seconds(3.0)))) assert unsub is not None unsub() - odom.publish(PoseStamped(ts=4.0)) + odom.publish(PoseStamped(header=Header(stamp=time_from_seconds(4.0)))) rec.close() store.stop() @@ -83,9 +85,9 @@ def test_full_queue_drops_and_counts(tmp_path: Path, monkeypatch: pytest.MonkeyP rec._writer.join() odom = _Transport() rec.tap("odom", PoseStamped, odom) - odom.publish(PoseStamped(ts=1.0)) - odom.publish(PoseStamped(ts=2.0)) - odom.publish(PoseStamped(ts=3.0)) + odom.publish(PoseStamped(header=Header(stamp=time_from_seconds(1.0)))) + odom.publish(PoseStamped(header=Header(stamp=time_from_seconds(2.0)))) + odom.publish(PoseStamped(header=Header(stamp=time_from_seconds(3.0)))) assert rec.dropped == 2 assert len(warnings) == 1 store.stop() @@ -99,7 +101,7 @@ def test_append_failure_does_not_kill_writer(tmp_path: Path) -> None: odom = _Transport() rec.tap("odom", PoseStamped, odom) odom.publish("not a PoseStamped") # append raises TypeError inside the writer - odom.publish(PoseStamped(ts=2.0)) + odom.publish(PoseStamped(header=Header(stamp=time_from_seconds(2.0)))) rec.close() # returns: the writer is still draining store.stop() @@ -145,3 +147,29 @@ def test_recording_delegates_to_rust_session( make_plan.assert_called_once_with(transports) create_session.assert_called_once_with("plan") session.stop.assert_called_once_with() + + +def test_zero_source_stamp_and_unstamped_arrival_time_are_distinct(tmp_path, monkeypatch): + monkeypatch.setattr(tap.time, "time", lambda: 123.5) + path = tmp_path / "time.db" + pose_transport, twist_transport = _Transport(), _Transport() + with SqliteStore(path=str(path)) as store: + recorder = TransportRecorder(store) + subscriptions = [] + try: + subscriptions.append(recorder.tap("pose", PoseStamped, pose_transport)) + subscriptions.append(recorder.tap("twist", Twist, twist_transport)) + pose_transport.publish(PoseStamped()) + twist_transport.publish(Twist()) + finally: + for unsubscribe in subscriptions: + assert unsubscribe is not None + unsubscribe() + recorder.close() + with SqliteStore(path=str(path), must_exist=True) as store: + pose = store.stream("pose").first() + twist = store.stream("twist").first() + assert pose.ts == 0.0 + assert pose.data == PoseStamped() + assert twist.ts == 123.5 + assert twist.data == Twist() diff --git a/dimos/memory/test_voxel_map.py b/dimos/memory/test_voxel_map.py index ae9adc3bfcf..f5c60a20876 100644 --- a/dimos/memory/test_voxel_map.py +++ b/dimos/memory/test_voxel_map.py @@ -14,15 +14,24 @@ from __future__ import annotations +from dimos_generated.sensor_msgs.msg import PointCloud2 +from dimos_generated.std_msgs.msg import Header import numpy as np from dimos.mapping.voxels.module import VoxelMapTransformer from dimos.memory.type.observation import Observation -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 +from dimos.msgs.pointcloud import pointcloud_from_xyz +from dimos.msgs.time import time_from_seconds def _make_obs(obs_id: int, points: np.ndarray, ts: float = 0.0) -> Observation[PointCloud2]: - return Observation(id=obs_id, ts=ts, _data=PointCloud2.from_numpy(points)) + return Observation( + id=obs_id, + ts=ts, + _data=pointcloud_from_xyz( + points, header=Header(frame_id="world", stamp=time_from_seconds(ts)) + ), + ) def _unit_cube_points(n: int = 100) -> np.ndarray: @@ -36,18 +45,21 @@ def test_accumulate_two_frames() -> None: obs1 = _make_obs(0, pts, ts=1.0) obs2 = _make_obs(1, pts + 10.0, ts=2.0) # offset by 10m, no overlap - xf = VoxelMapTransformer(voxel_size=0.5, carve_columns=False) + xf = VoxelMapTransformer(voxel_size=0.5, carve_columns=False, device="CPU:0") results = list(xf(iter([obs1, obs2]))) assert len(results) == 2 # emit_every=1 default global_map = results[-1].data # last result has the full accumulated map - single_results = list(VoxelMapTransformer(voxel_size=0.5)(iter([obs1]))) - assert len(global_map) > len(single_results[0].data) + single_results = list(VoxelMapTransformer(voxel_size=0.5, device="CPU:0")(iter([obs1]))) + assert ( + global_map.width * global_map.height + > single_results[0].data.width * single_results[0].data.height + ) def test_empty_stream() -> None: - xf = VoxelMapTransformer(voxel_size=0.5) + xf = VoxelMapTransformer(voxel_size=0.5, device="CPU:0") assert list(xf(iter([]))) == [] diff --git a/dimos/memory/tf.py b/dimos/memory/tf.py index 53e720a9125..036a16b6c22 100644 --- a/dimos/memory/tf.py +++ b/dimos/memory/tf.py @@ -19,12 +19,14 @@ import math from typing import TYPE_CHECKING, Any, cast +from dimos_generated.tf2_msgs.msg import TFMessage + from dimos.memory.stream import Stream -from dimos.msgs.tf2_msgs.TFMessage import TFMessage from dimos.protocol.tf.tf import MultiTBuffer if TYPE_CHECKING: - from dimos.msgs.geometry_msgs.Transform import Transform + from dimos_generated.geometry_msgs.msg import TransformStamped + from dimos.protocol.tf.tf import TFLookup @@ -58,7 +60,7 @@ def from_store(cls, store: Any, stream: str = "tf") -> StreamTF | None: recorded = tf_stream(store, stream) return None if recorded is None else cls(recorded) - def publish(self, *args: Transform) -> None: + def publish(self, *args: TransformStamped) -> None: raise NotImplementedError("StreamTF is a read-only replay service.") def _load(self, lo: float, hi: float) -> None: @@ -86,7 +88,7 @@ def get( *, forward_tolerance: float = 0.0, warn: bool = True, - ) -> Transform | None: + ) -> TransformStamped | None: tp = time_point if tp is None: last = next(iter(self.stream.order_by("ts", desc=True).limit(1)), None) diff --git a/dimos/memory/tool_e2e.py b/dimos/memory/tool_e2e.py index a3d3ffa6767..42a8f5e80f8 100644 --- a/dimos/memory/tool_e2e.py +++ b/dimos/memory/tool_e2e.py @@ -12,46 +12,69 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""E2E test: import legacy pickle replays into memory SqliteStore.""" +"""E2E test: copy a deterministic CDR recording into a fresh SQLite store.""" from __future__ import annotations import bisect from typing import TYPE_CHECKING, Any +from dimos_generated.geometry_msgs.msg import Point, Pose, PoseStamped +from dimos_generated.sensor_msgs.msg import Image, PointCloud2 +from dimos_generated.std_msgs.msg import Header +import numpy as np import pytest from dimos.memory.embed import EmbedImages from dimos.memory.store.sqlite import SqliteStore +from dimos.memory.stream import Stream from dimos.memory.transform import QualityWindow from dimos.models.embedding.clip import CLIPModel -from dimos.msgs.sensor_msgs.Image import Image -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 -from dimos.robot.unitree.type.odometry import Odometry -from dimos.utils.data import get_data_dir -from dimos.utils.testing.legacy_pickle import LegacyPickleStore +from dimos.msgs.image import image_brightness, image_from_array, image_sharpness +from dimos.msgs.pointcloud import pointcloud_from_xyz +from dimos.msgs.time import time_from_nanoseconds if TYPE_CHECKING: from collections.abc import Iterator -DB_PATH = get_data_dir() / "go2_short.db" + +@pytest.fixture(scope="module") +def source(tmp_path_factory: pytest.TempPathFactory) -> Iterator[SqliteStore]: + path = tmp_path_factory.mktemp("cdr-source") / "recording.db" + with SqliteStore(path=str(path)) as store: + video = store.stream("color_image", Image) + lidar = store.stream("lidar", PointCloud2) + odom = store.stream("odom", PoseStamped) + for index in range(1024): + ns = 1_700_000_000_123_456_789 + index * 25_000_000 + header = Header(stamp=time_from_nanoseconds(ns), frame_id="world") + pose = PoseStamped(header=header, pose=Pose(position=Point(x=index / 100))) + pixels = np.full((8, 8, 3), 64 + index % 128, dtype=np.uint8) + pixels[:, index % 8] = (255, 0, 127) + video.append(image_from_array(pixels, encoding="rgb8", header=header), ts=ns / 1e9) + lidar.append( + pointcloud_from_xyz(np.array([[1, 0, 0], [1, 1, 0], [2, 0, 0]]), header=header), + ts=ns / 1e9, + ) + odom.append(pose, ts=ns / 1e9) + yield store @pytest.fixture(scope="module") -def session() -> Iterator[SqliteStore]: - store = SqliteStore(path=str(DB_PATH)) - with store: +def session(tmp_path_factory: pytest.TempPathFactory) -> Iterator[SqliteStore]: + path = tmp_path_factory.mktemp("cdr-output") / "recording.db" + with SqliteStore(path=str(path)) as store: yield store - store.stop() class PoseIndex: """Preloaded odom data with O(log n) closest-timestamp lookup.""" - def __init__(self, replay: LegacyPickleStore[Any]) -> None: + def __init__(self, replay: Stream[Any]) -> None: self._timestamps: list[float] = [] self._data: list[Any] = [] - for ts, data in replay.iterate_ts(): + for obs in replay: + ts, data = obs.ts, obs.data self._timestamps.append(ts) self._data.append(data) @@ -73,22 +96,22 @@ def __iter__(self) -> Iterator[tuple[float, Any]]: @pytest.fixture(scope="module") -def video_replay() -> LegacyPickleStore[Image]: - return LegacyPickleStore("go2_short/video") +def video_replay(source: SqliteStore) -> Stream[Image]: + return source.stream("color_image", Image) @pytest.fixture(scope="module") -def odom_index() -> PoseIndex: - return PoseIndex(LegacyPickleStore("go2_short/odom")) +def odom_index(source: SqliteStore) -> PoseIndex: + return PoseIndex(source.stream("odom", PoseStamped)) @pytest.fixture(scope="module") -def lidar_replay() -> LegacyPickleStore[PointCloud2]: - return LegacyPickleStore("go2_short/lidar") +def lidar_replay(source: SqliteStore) -> Stream[PointCloud2]: + return source.stream("lidar", PointCloud2) class TestImportReplay: - """Import legacy pickle replay data into a memory SqliteStore. + """Copy generated CDR streams into a fresh memory SqliteStore. Lidar/odom are trimmed to start at video's first ts. The MemoryReplayAdapter scheduler anchors each stream to its own first_ts on subscribe, so aligning @@ -99,10 +122,10 @@ def test_import_odom( self, session: SqliteStore, odom_index: PoseIndex, - video_replay: LegacyPickleStore[Any], + video_replay: Stream[Any], ) -> None: - threshold = video_replay.first_timestamp() - with session.stream("odom", Odometry) as odom: + threshold = video_replay.first().ts + with session.stream("odom", PoseStamped) as odom: count = 0 skipped = 0 for ts, data in odom_index: @@ -119,12 +142,13 @@ def test_import_odom( def test_import_video( self, session: SqliteStore, - video_replay: LegacyPickleStore[Any], + video_replay: Stream[Any], odom_index: PoseIndex, ) -> None: with session.stream("color_image", Image) as video: count = 0 - for ts, frame in video_replay.iterate_ts(): + for obs in video_replay: + ts, frame = obs.ts, obs.data pose = odom_index.find_closest(ts) video.append(frame, ts=ts, pose=pose) count += 1 @@ -137,16 +161,17 @@ def test_import_video( def test_import_lidar( self, session: SqliteStore, - lidar_replay: LegacyPickleStore[Any], + lidar_replay: Stream[Any], odom_index: PoseIndex, - video_replay: LegacyPickleStore[Any], + video_replay: Stream[Any], ) -> None: - threshold = video_replay.first_timestamp() - lidar = session.stream("lidar", PointCloud2, codec="lz4+lcm") + threshold = video_replay.first().ts + lidar = session.stream("lidar", PointCloud2, codec="lz4+cdr") count = 0 skipped = 0 - for ts, frame in lidar_replay.iterate_ts(): + for obs in lidar_replay: + ts, frame = obs.ts, obs.data if ts < threshold: skipped += 1 continue @@ -173,9 +198,9 @@ def test_embed_and_save(self, session: SqliteStore, clip: CLIPModel) -> None: # Downsample to 2Hz, then embed pipeline = ( - video.filter(lambda obs: obs.data.brightness > 0.1) + video.filter(lambda obs: image_brightness(obs.data) > 0.1) .tap(print) - .transform(QualityWindow(lambda img: img.sharpness, window=0.5)) + .transform(QualityWindow(image_sharpness, window=0.5)) .transform(EmbedImages(clip)) .save(embedded) ) @@ -209,7 +234,7 @@ def test_query_imported_data(self, session: SqliteStore) -> None: class TestE2EQuery: - """Query operations against real robot replay data.""" + """Query operations against a deterministic generated CDR recording.""" def test_list_streams(self, session: SqliteStore) -> None: streams = session.list_streams() diff --git a/dimos/memory/tool_visualizer.py b/dimos/memory/tool_visualizer.py index 399abb71216..174955e153d 100644 --- a/dimos/memory/tool_visualizer.py +++ b/dimos/memory/tool_visualizer.py @@ -19,6 +19,7 @@ import pickle from typing import TYPE_CHECKING +from dimos_generated.sensor_msgs.msg import Image import pytest from dimos.memory.store.sqlite import SqliteStore @@ -26,8 +27,9 @@ from dimos.models.embedding.clip import CLIPModel from dimos.models.vl.florence import Florence2Model from dimos.models.vl.moondream import MoondreamVlModel -from dimos.msgs.geometry_msgs.Transform import Transform -from dimos.msgs.sensor_msgs.Image import Image +from dimos.msgs.geometry import inverse_transform, transform_from_pose +from dimos.msgs.image import image_sharpness +from dimos.msgs.time import to_seconds from dimos.perception.detection.type.detection3d.pointcloud import Detection3DPC from dimos.robot.unitree.go2.connection import GO2Connection from dimos.utils.data import get_data, get_data_dir @@ -113,11 +115,10 @@ def test_detect_objects(self, store: SqliteStore, clip: CLIPModel) -> None: embedded = store.streams.color_image_embedded lidar = store.streams.lidar - for obs in embedded.search(clip.embed_text("bottle"), k=1).map( - lambda obs: obs.derive(data=vlm.query_detections(obs.data, "bottle")) - ): + for obs in embedded.search(clip.embed_text("bottle"), k=1): + detections = vlm.query_detections(obs.data, "bottle") print(f"ts={obs.ts:.2f} sim={obs.similarity:.3f} pose={obs.pose}") - for det in obs.data.detections: + for det in detections.detections: print(det) print( lidar.at(obs.ts).first().data @@ -140,7 +141,7 @@ def test_agent_visual_description_passive(self, store: SqliteStore) -> None: pipeline = ( store.streams.color_image.limit(200) .transform( - QualityWindow(lambda img: img.sharpness, window=5.0) + QualityWindow(image_sharpness, window=5.0) # we are batch processing images here, # so we can use the more efficient batch captioning API # (instead of using .map() and calling caption() for each image, @@ -167,20 +168,26 @@ def test_detect_objects_smart(self, store: SqliteStore, clip: CLIPModel) -> None # find a location in the world with highest semantic similarity to a bottle bottle_pos = embedded.search(clip.embed_text("bottle"), k=1).first().pose_stamped + if bottle_pos is None: + raise ValueError("The semantic hotspot has no pose for spatial detection") for obs in ( store.streams.color_image # find all frames within 60 seconds of the semantic hotspot - .at(bottle_pos.ts, tolerance=60.0) + .at(to_seconds(bottle_pos.header.stamp), tolerance=60.0) # filter the frames within 1m radius near the semantic hotspot .near(bottle_pos, radius=1.0) # select highest quality frames from these results (based on sharpness) - .transform(QualityWindow(lambda img: img.sharpness, window=1.0)) + .transform(QualityWindow(image_sharpness, window=1.0)) # run detection on these frames to find bottles .map(lambda obs: obs.derive(data=vlm.query_detections(obs.data, "bottle"))) ): print(f"ts={obs.ts:.2f} pose={obs.pose_stamped}") + image_pose = obs.pose_stamped + if image_pose is None: + continue + # find the lidar frame captured closest in time to an image lidar_frame = lidar.at(obs.ts).first().data @@ -192,10 +199,11 @@ def test_detect_objects_smart(self, store: SqliteStore, clip: CLIPModel) -> None det, lidar_frame, camera_info=GO2Connection.camera_info_static, - world_to_optical_transform=Transform( - ts=obs.ts, - translation=obs.pose_stamped.position, - rotation=obs.pose_stamped.orientation, - ).inverse(), + world_to_optical_transform=inverse_transform( + transform_from_pose( + image_pose, + child_frame_id=GO2Connection.camera_info_static.header.frame_id, + ) + ), ) print(det3d) diff --git a/dimos/memory/tool_voxel_map.py b/dimos/memory/tool_voxel_map.py index 90b992be103..c1bdc8edf2e 100644 --- a/dimos/memory/tool_voxel_map.py +++ b/dimos/memory/tool_voxel_map.py @@ -17,11 +17,11 @@ import time from typing import TYPE_CHECKING +from dimos_generated.sensor_msgs.msg import PointCloud2 import pytest from dimos.mapping.voxels.module import VoxelMapTransformer from dimos.memory.store.sqlite import SqliteStore -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 from dimos.utils.data import get_data if TYPE_CHECKING: @@ -51,11 +51,11 @@ def test_build_global_map(store: SqliteStore) -> None: frame_count = result.tags["frame_count"] assert frame_count == n_frames - assert len(global_map) > 0 + assert global_map.width * global_map.height > 0 print( lidar.summary(), - f"\n{frame_count} frames -> {len(global_map)} voxels" + f"\n{frame_count} frames -> {global_map.width * global_map.height} voxels" f"\n transform: {t_transform:.2f}s ({t_transform / frame_count * 1000:.1f}ms/frame)" f"\n total wall: {t_total:.2f}s", ) diff --git a/dimos/memory/transform.py b/dimos/memory/transform.py index 2f23ec7859c..abcef5fe5b2 100644 --- a/dimos/memory/transform.py +++ b/dimos/memory/transform.py @@ -22,6 +22,7 @@ from typing import TYPE_CHECKING, Any, Generic, Literal, TypeVar, cast from dimos.memory.utils.formatting import FilterRepr +from dimos.msgs.geometry import point_distance, quaternion_angle if TYPE_CHECKING: from collections.abc import Callable, Iterator, Sequence @@ -183,7 +184,7 @@ def _speed(upstream: Iterator[Observation[Any]]) -> Iterator[Observation[float]] pp = prev.pose if prev is not None else None if prev is not None and p is not None and pp is not None: dt = obs.ts - prev.ts - v = (p.position - pp.position).length() / dt if dt > 0 else 0.0 + v = point_distance(p.position, pp.position) / dt if dt > 0 else 0.0 yield obs.derive(data=v) prev = obs @@ -231,10 +232,10 @@ def __call__(self, upstream: Iterator[Observation[T]]) -> Iterator[Observation[T if prev is not None and pp is not None: dt = obs.ts - prev.ts if dt > 0: - v = (p.position - pp.position).length() / dt + v = point_distance(p.position, pp.position) / dt ok = v <= max_mps if ok and max_rps is not None: - ok = (p.orientation.angle_to(pp.orientation) / dt) <= max_rps + ok = (quaternion_angle(p.orientation, pp.orientation) / dt) <= max_rps if ok: yield obs prev = obs diff --git a/dimos/memory/type/filter.py b/dimos/memory/type/filter.py index 37229571f5c..01ce60d2da6 100644 --- a/dimos/memory/type/filter.py +++ b/dimos/memory/type/filter.py @@ -19,7 +19,7 @@ from itertools import islice from typing import TYPE_CHECKING, Any -from dimos.msgs.geometry_msgs.Vector3 import Vector3 +from dimos_generated.geometry_msgs.msg import Vector3 if TYPE_CHECKING: from collections.abc import Callable, Iterator @@ -84,8 +84,13 @@ def matches(self, obs: Observation[Any]) -> bool: p = obs.pose_tuple if p is None: return False - delta = self.position - Vector3(p[0], p[1], p[2]) - return delta.length_squared() <= self.radius * self.radius + distance_squared = sum( + (value - center) ** 2 + for value, center in zip( + p[:3], (self.position.x, self.position.y, self.position.z), strict=True + ) + ) + return distance_squared <= self.radius * self.radius @dataclass(frozen=True) diff --git a/dimos/memory/type/observation.py b/dimos/memory/type/observation.py index 3a42b4c80d0..ee3808f94ab 100644 --- a/dimos/memory/type/observation.py +++ b/dimos/memory/type/observation.py @@ -19,8 +19,10 @@ import threading from typing import TYPE_CHECKING, Any, Generic, TypeVar, cast -from dimos.msgs.geometry_msgs.Pose import Pose -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped +from dimos_generated.geometry_msgs.msg import Point, Pose, PoseStamped, Quaternion, TransformStamped +from dimos_generated.std_msgs.msg import Header + +from dimos.msgs.time import time_from_seconds if sys.version_info >= (3, 11): from typing import Self @@ -65,9 +67,8 @@ def _to_tuple(p: Any) -> PoseTuple | None: Accepts: `None`, a 3- or 7-tuple, or any object with `translation`+`rotation` (Transform) or `position`+`orientation` - (Pose / PoseStamped) attributes. 3-tuples are padded with the identity - quaternion. Duck-typed to avoid importing `dimos.msgs.geometry_msgs` - at module load. + (Pose) attributes, or generated PoseStamped/TransformStamped values. + 3-tuples are padded with the identity quaternion. """ if p is None: return None @@ -78,6 +79,10 @@ def _to_tuple(p: Any) -> PoseTuple | None: x, y, z = p return (float(x), float(y), float(z), 0.0, 0.0, 0.0, 1.0) raise TypeError(f"Pose tuple must have length 3 or 7, got {len(p)}") + if isinstance(p, PoseStamped): + p = p.pose + elif isinstance(p, TransformStamped): + p = p.transform # Use ``is not None`` rather than ``or`` — a Vector3 at the origin is # falsy but valid, and falling through to ``position`` on a Transform # would crash. @@ -156,17 +161,21 @@ def pose(self) -> Pose | None: if self.pose_tuple is None: return None - return Pose(*self.pose_tuple) + x, y, z, qx, qy, qz, qw = self.pose_tuple + return Pose(position=Point(x=x, y=y, z=z), orientation=Quaternion(x=qx, y=qy, z=qz, w=qw)) @property def pose_stamped(self) -> PoseStamped | None: """Typed :class:`PoseStamped` (or None) carrying this observation's ts.""" if self.pose_tuple is None: return None - from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped - x, y, z, qx, qy, qz, qw = self.pose_tuple - return PoseStamped(ts=self.ts, position=(x, y, z), orientation=(qx, qy, qz, qw)) + return PoseStamped( + header=Header(stamp=time_from_seconds(self.ts)), + pose=Pose( + position=Point(x=x, y=y, z=z), orientation=Quaternion(x=qx, y=qy, z=qz, w=qw) + ), + ) @property def data(self) -> T: diff --git a/dimos/memory/type/test_generated_query.py b/dimos/memory/type/test_generated_query.py new file mode 100644 index 00000000000..50a57d8a547 --- /dev/null +++ b/dimos/memory/type/test_generated_query.py @@ -0,0 +1,54 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +# Copyright 2026 Dimensional Inc. +# SPDX-License-Identifier: Apache-2.0 + +from dimos_generated.geometry_msgs.msg import ( + Point, + Pose, + PoseStamped, + PoseWithCovariance, + Transform, + TransformStamped, + Vector3, +) +from dimos_generated.nav_msgs.msg import Odometry +import numpy as np +import pytest + +from dimos.memory.store.memory import MemoryStore + + +@pytest.mark.parametrize( + "center", + [ + Point(x=1), + Vector3(x=1), + (1, 0, 0), + np.array([1, 0, 0]), + Pose(position=Point(x=1)), + PoseStamped(pose=Pose(position=Point(x=1))), + TransformStamped(transform=Transform(translation=Vector3(x=1))), + Odometry(pose=PoseWithCovariance(pose=Pose(position=Point(x=1)))), + ], +) +def test_near_accepts_generated_nested_positions_and_retains_radius_boundary(center): + with MemoryStore() as store: + stream = store.stream("values", str) + stream.append("center", ts=1, pose=(1, 0, 0)) + stream.append("boundary", ts=2, pose=(2, 0, 0)) + stream.append("outside", ts=3, pose=(2.001, 0, 0)) + stream.append("poseless", ts=4) + assert [obs.data for obs in stream.near(center, radius=1)] == ["center", "boundary"] diff --git a/dimos/memory/vis/space/elements.py b/dimos/memory/vis/space/elements.py index 2c4f052f639..7b7d84f8ecf 100644 --- a/dimos/memory/vis/space/elements.py +++ b/dimos/memory/vis/space/elements.py @@ -32,16 +32,16 @@ ColorLike = Union[str, Color, DeferredColor] if TYPE_CHECKING: + from dimos_generated.geometry_msgs.msg import ( + Point as GeoPoint, + Pose as GeoPose, + PoseStamped, + Vector3, + ) + from dimos_generated.nav_msgs.msg import OccupancyGrid, Path + from dimos_generated.sensor_msgs.msg import CameraInfo, Image, PointCloud2 + from dimos.memory.type.observation import Observation - from dimos.msgs.geometry_msgs.Point import Point as GeoPoint - from dimos.msgs.geometry_msgs.Pose import Pose as GeoPose - from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped - from dimos.msgs.geometry_msgs.Vector3 import Vector3 - from dimos.msgs.nav_msgs.OccupancyGrid import OccupancyGrid - from dimos.msgs.nav_msgs.Path import Path - from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo - from dimos.msgs.sensor_msgs.Image import Image - from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 @dataclass @@ -83,7 +83,7 @@ class Point: Rerun: rr.Points3D """ - msg: GeoPoint | GeoPose + msg: GeoPoint | GeoPose | PoseStamped color: ColorLike = "#e74c3c" radius: float = 0.05 label: str | None = None diff --git a/dimos/memory/vis/space/geometry.py b/dimos/memory/vis/space/geometry.py new file mode 100644 index 00000000000..9db4a82378c --- /dev/null +++ b/dimos/memory/vis/space/geometry.py @@ -0,0 +1,32 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Geometry access shared by SVG and Rerun scene renderers.""" + +from dimos_generated.geometry_msgs.msg import Point, Pose, PoseStamped + +from dimos.msgs.geometry import yaw + + +def message_position(message: Point | Pose | PoseStamped) -> Point: + if isinstance(message, PoseStamped): + return message.pose.position + if isinstance(message, Pose): + return message.position + return message + + +def message_yaw(message: Pose | PoseStamped) -> float: + pose = message.pose if isinstance(message, PoseStamped) else message + return yaw(pose.orientation) diff --git a/dimos/memory/vis/space/rerun.py b/dimos/memory/vis/space/rerun.py index 0242774dc3d..643b6f363c7 100644 --- a/dimos/memory/vis/space/rerun.py +++ b/dimos/memory/vis/space/rerun.py @@ -19,14 +19,26 @@ import math from typing import TYPE_CHECKING, Any +from dimos_generated.geometry_msgs.msg import Quaternion, Transform, TransformStamped, Vector3 +from dimos_generated.nav_msgs.msg import OccupancyGrid +from dimos_generated.sensor_msgs.msg import Image, PointCloud2 +from dimos_generated.std_msgs.msg import Header +import numpy as np + +from dimos.mapping.occupancy.visualizations import generate_rgba_texture from dimos.memory.type.observation import Observation from dimos.memory.vis.color import Color from dimos.memory.vis.space.elements import Arrow, Box3D, Camera, Point, Polyline, Pose, Text -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Transform import Transform -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.msgs.nav_msgs.OccupancyGrid import OccupancyGrid -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 +from dimos.memory.vis.space.geometry import message_position, message_yaw +from dimos.msgs.geometry import ( + compose_transforms, + pose_matrix, + transform_from_pose, + transform_matrix, +) +from dimos.msgs.occupancy import occupancy_extent, occupancy_view +from dimos.msgs.pointcloud import pointcloud_rgb, pointcloud_xyz +from dimos.visualization.rerun.message_helpers import camera_pinhole, image_archetype if TYPE_CHECKING: from dimos.memory.vis.space.space import Space @@ -40,16 +52,13 @@ def _rgba(el: Any) -> tuple[int, int, int, int]: # base_link → camera_optical extrinsics (applied at render time for image observations) -_BASE_TO_OPTICAL = Transform( - translation=Vector3(0.3, 0.0, 0.0), - rotation=Quaternion(0.0, 0.0, 0.0, 1.0), - frame_id="base_link", - child_frame_id="camera_link", -) + Transform( - translation=Vector3(0.0, 0.0, 0.0), - rotation=Quaternion(-0.5, 0.5, -0.5, 0.5), - frame_id="camera_link", +_BASE_TO_OPTICAL = TransformStamped( + header=Header(frame_id="base_link"), child_frame_id="camera_optical", + transform=Transform( + translation=Vector3(x=0.3), + rotation=Quaternion(x=-0.5, y=0.5, z=-0.5, w=0.5), + ), ) @@ -127,17 +136,44 @@ def render(space: Space, app_id: str = "space", spawn: bool = True) -> None: # Log elements if grids: for i, el in enumerate(grids): - rr.log(f"scene/map/{i}", el.to_rerun(), static=True) + cells = occupancy_view(el) + if cells.size == 0: + continue + width, height = occupancy_extent(el) + matrix = pose_matrix(el.info.origin) + corners = np.array([[0, 0, 0], [width, 0, 0], [width, height, 0], [0, height, 0]]) + vertices = corners @ matrix[:3, :3].T + matrix[:3, 3] + rr.log( + f"scene/map/{i}", + rr.Mesh3D( + vertex_positions=vertices, + triangle_indices=[[0, 1, 2], [0, 2, 3]], + vertex_texcoords=[[0, 1], [1, 1], [1, 0], [0, 0]], + albedo_texture=np.ascontiguousarray(np.flipud(generate_rgba_texture(cells))), + ), + static=True, + ) if pointclouds: for i, el in enumerate(pointclouds): - rr.log(f"scene/pointcloud/{i}", el.to_rerun(), static=True) + rr.log( + f"scene/pointcloud/{i}", + rr.Points3D(pointcloud_xyz(el), colors=pointcloud_rgb(el)), + static=True, + ) if points: rr.log( "scene/points", rr.Points3D( - positions=[[p.msg.x, p.msg.y, p.msg.z] for p in points], + positions=[ + [ + message_position(p.msg).x, + message_position(p.msg).y, + message_position(p.msg).z, + ] + for p in points + ], colors=[_rgba(p) for p in points], radii=[max(p.radius, 0.05) for p in points], labels=[p.label or "" for p in points] if any(p.label for p in points) else None, @@ -149,7 +185,9 @@ def render(space: Space, app_id: str = "space", spawn: bool = True) -> None: rr.log( "scene/poses", rr.Points3D( - positions=[[p.msg.x, p.msg.y, 0] for p in poses], + positions=[ + [message_position(p.msg).x, message_position(p.msg).y, 0] for p in poses + ], colors=[_rgba(p) for p in poses], radii=[p.size * 0.3 for p in poses], labels=[p.label or "" for p in poses] if any(p.label for p in poses) else None, @@ -159,9 +197,14 @@ def render(space: Space, app_id: str = "space", spawn: bool = True) -> None: rr.log( "scene/poses/headings", rr.Arrows3D( - origins=[[p.msg.x, p.msg.y, 0] for p in poses], + origins=[[message_position(p.msg).x, message_position(p.msg).y, 0] for p in poses], vectors=[ - [math.cos(p.msg.yaw) * p.size, math.sin(p.msg.yaw) * p.size, 0] for p in poses + [ + math.cos(message_yaw(p.msg)) * p.size, + math.sin(message_yaw(p.msg)) * p.size, + 0, + ] + for p in poses ], colors=[_rgba(p) for p in poses], ), @@ -172,9 +215,13 @@ def render(space: Space, app_id: str = "space", spawn: bool = True) -> None: rr.log( "scene/arrows", rr.Arrows3D( - origins=[[a.msg.x, a.msg.y, 0] for a in arrows], + origins=[[message_position(a.msg).x, message_position(a.msg).y, 0] for a in arrows], vectors=[ - [math.cos(a.msg.yaw) * a.length, math.sin(a.msg.yaw) * a.length, 0] + [ + math.cos(message_yaw(a.msg)) * a.length, + math.sin(message_yaw(a.msg)) * a.length, + 0, + ] for a in arrows ], colors=[_rgba(a) for a in arrows], @@ -186,7 +233,7 @@ def render(space: Space, app_id: str = "space", spawn: bool = True) -> None: rr.log( "scene/boxes", rr.Boxes3D( - centers=[[b.center.x, b.center.y, 0] for b in boxes], + centers=[[b.center.position.x, b.center.position.y, 0] for b in boxes], half_sizes=[[b.size.x / 2, b.size.y / 2, b.size.z / 2] for b in boxes], colors=[_rgba(b) for b in boxes], labels=[b.label or "" for b in boxes] if any(b.label for b in boxes) else None, @@ -198,7 +245,7 @@ def render(space: Space, app_id: str = "space", spawn: bool = True) -> None: rr.log( f"scene/polylines/{i}", rr.LineStrips3D( - strips=[[[p.x, p.y, 0] for p in el.msg.poses]], + strips=[[[p.pose.position.x, p.pose.position.y, 0] for p in el.msg.poses]], colors=[_rgba(el)], radii=[el.width / 2], ), @@ -219,13 +266,14 @@ def render(space: Space, app_id: str = "space", spawn: bool = True) -> None: for i, el in enumerate(cameras): path = f"scene/cameras/{i}" - rr.log(path, el.pose.to_rerun(), static=True) + matrix = pose_matrix(el.pose.pose) + rr.log(path, rr.Transform3D(translation=matrix[:3, 3], mat3x3=matrix[:3, :3]), static=True) if el.camera_info: - pinhole = el.camera_info.to_rerun() + pinhole = camera_pinhole(el.camera_info) assert not isinstance(pinhole, list) rr.log(path, pinhole, static=True) elif el.image: - h, w = el.image.shape[:2] + h, w = el.image.height, el.image.width focal = max(w, h) rr.log( path, @@ -233,7 +281,7 @@ def render(space: Space, app_id: str = "space", spawn: bool = True) -> None: static=True, ) if el.image: - rr.log(f"{path}/image", el.image.to_rerun(), static=True) + rr.log(f"{path}/image", image_archetype(el.image), static=True) for i, obs in enumerate(observations): path = f"scene/observations/{i}" @@ -244,9 +292,14 @@ def render(space: Space, app_id: str = "space", spawn: bool = True) -> None: img = _as_image(data) if img is not None: # Apply base→optical extrinsics for camera frustum rendering - world_T_optical = Transform.from_pose("world", ps) + _BASE_TO_OPTICAL - rr.log(path, world_T_optical.to_pose().to_rerun(), static=True) - h, w = img.shape[:2] + world_T_optical = compose_transforms( + transform_from_pose(ps, child_frame_id="base_link"), _BASE_TO_OPTICAL + ) + matrix = transform_matrix(world_T_optical.transform) + rr.log( + path, rr.Transform3D(translation=matrix[:3, 3], mat3x3=matrix[:3, :3]), static=True + ) + h, w = img.height, img.width focal = max(w, h) rr.log( path, @@ -258,15 +311,22 @@ def render(space: Space, app_id: str = "space", spawn: bool = True) -> None: ), static=True, ) - rr.log(f"{path}/image", img.to_rerun(), static=True) + rr.log(f"{path}/image", image_archetype(img), static=True) elif isinstance(data, PointCloud2): - rr.log(path, ps.to_rerun(), static=True) - rr.log(f"{path}/pointcloud", data.to_rerun(), static=True) + matrix = pose_matrix(ps.pose) + rr.log( + path, rr.Transform3D(translation=matrix[:3, 3], mat3x3=matrix[:3, :3]), static=True + ) + rr.log( + f"{path}/pointcloud", + rr.Points3D(pointcloud_xyz(data), colors=pointcloud_rgb(data)), + static=True, + ) elif isinstance(data, (int, float)): rr.log( path, rr.Points3D( - positions=[[ps.x, ps.y, 0]], + positions=[[ps.pose.position.x, ps.pose.position.y, 0]], labels=[str(data)], radii=[0.025], ), @@ -286,7 +346,7 @@ def render(space: Space, app_id: str = "space", spawn: bool = True) -> None: if line: lines.append(line) label = "\n".join(lines) - x, y = ps.x, ps.y + x, y = ps.pose.position.x, ps.pose.position.y # Pin: line from ground up, label at the tip rr.log( f"{path}/pin", @@ -309,19 +369,18 @@ def render(space: Space, app_id: str = "space", spawn: bool = True) -> None: else: rr.log( path, - rr.Points3D(positions=[[ps.x, ps.y, 0]], radii=[0.05]), + rr.Points3D(positions=[[ps.pose.position.x, ps.pose.position.y, 0]], radii=[0.05]), static=True, ) for i, obs in enumerate(panels): img = _as_image(obs.data) if img is not None: - rr.log(f"scene/panels/{i}", img.to_rerun(), static=True) + rr.log(f"scene/panels/{i}", image_archetype(img), static=True) def _as_image(data: Any) -> Any | None: """Return an Image if data is an Image or ImageDetections, else None.""" - from dimos.msgs.sensor_msgs.Image import Image from dimos.perception.detection.type.imageDetections import ImageDetections if isinstance(data, Image): diff --git a/dimos/memory/vis/space/space.py b/dimos/memory/vis/space/space.py index 65afe7d0efc..864da7169a2 100644 --- a/dimos/memory/vis/space/space.py +++ b/dimos/memory/vis/space/space.py @@ -23,6 +23,11 @@ from typing import Any +from dimos_generated.geometry_msgs.msg import Point as GeoPoint, Pose as GeoPose, PoseStamped +from dimos_generated.nav_msgs.msg import OccupancyGrid, Path as NavPath +from dimos_generated.sensor_msgs.msg import PointCloud2 +from dimos_generated.vision_msgs.msg import Detection3D + from dimos.memory.type.observation import EmbeddedObservation, Observation from dimos.memory.vis.color import ColorRange, resolve_deferred from dimos.memory.vis.space.elements import ( @@ -35,14 +40,7 @@ SpaceElement, Text, ) -from dimos.msgs.geometry_msgs.Point import Point as GeoPoint -from dimos.msgs.geometry_msgs.Pose import Pose as GeoPose -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.nav_msgs.OccupancyGrid import OccupancyGrid -from dimos.msgs.nav_msgs.Path import Path as NavPath from dimos.msgs.protocol import DimosMsg -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 -from dimos.msgs.vision_msgs.Detection3D import Detection3D def _autocolor_value(item: Any) -> float | None: diff --git a/dimos/memory/vis/space/svg.py b/dimos/memory/vis/space/svg.py index aae639e5346..ee024a7f0ec 100644 --- a/dimos/memory/vis/space/svg.py +++ b/dimos/memory/vis/space/svg.py @@ -28,10 +28,14 @@ from pathlib import Path from typing import TYPE_CHECKING +from dimos_generated.nav_msgs.msg import OccupancyGrid +from dimos_generated.sensor_msgs.msg import PointCloud2 import numpy as np from PIL import Image as PILImage +from dimos.mapping.occupancy.inflation import simple_inflate from dimos.mapping.occupancy.visualizations import generate_rgba_texture +from dimos.mapping.pointclouds.occupancy import height_cost_occupancy from dimos.memory.type.observation import Observation from dimos.memory.vis.color import Color from dimos.memory.vis.space.elements import ( @@ -44,8 +48,9 @@ SpaceElement, Text, ) -from dimos.msgs.nav_msgs.OccupancyGrid import OccupancyGrid -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 +from dimos.memory.vis.space.geometry import message_position, message_yaw +from dimos.msgs.geometry import pose_matrix +from dimos.msgs.occupancy import occupancy_extent, occupancy_view if TYPE_CHECKING: from dimos.memory.vis.space.space import Space @@ -95,7 +100,8 @@ def _style(el: object) -> tuple[str, float]: def _render_point(el: Point, b: Bounds) -> str: - x, y = el.msg.x, _y(el.msg.y) + position = message_position(el.msg) + x, y = position.x, _y(position.y) r = el.radius b.include(x - r, y - r) b.include(x + r, y + r) @@ -110,8 +116,9 @@ def _render_point(el: Point, b: Bounds) -> str: def _render_arrow(el: Arrow, b: Bounds) -> str: - x, y = el.msg.x, _y(el.msg.y) - yaw = el.msg.yaw + position = message_position(el.msg) + x, y = position.x, _y(position.y) + yaw = message_yaw(el.msg) length = el.length half_base = length * 0.4 @@ -139,7 +146,8 @@ def _render_pose(el: Pose, b: Bounds) -> str: arrow = Arrow(msg=el.msg, length=el.size, color=el.color, opacity=el.opacity) parts = [_render_arrow(arrow, b)] if el.label: - x, y = el.msg.x, _y(el.msg.y) + position = message_position(el.msg) + x, y = position.x, _y(position.y) fill, alpha = _style(el) parts.append( f' str: def _render_polyline(el: Polyline, b: Bounds) -> str: pts = [] for p in el.msg.poses: - x, y = p.x, _y(p.y) + x, y = p.pose.position.x, _y(p.pose.position.y) b.include(x, y) pts.append(f"{x:.4f},{y:.4f}") stroke, alpha = _style(el) @@ -163,7 +171,7 @@ def _render_polyline(el: Polyline, b: Bounds) -> str: def _render_box3d(el: Box3D, b: Bounds) -> str: - cx, cy = el.center.x, el.center.y + cx, cy = el.center.position.x, el.center.position.y hw, hh = el.size.x / 2, el.size.y / 2 # Top-left in world → SVG x = cx - hw @@ -188,12 +196,13 @@ def _render_box3d(el: Box3D, b: Bounds) -> str: def _render_camera(el: Camera, b: Bounds) -> str: - x, y = el.pose.x, _y(el.pose.y) - yaw = el.pose.yaw + position = message_position(el.pose) + x, y = position.x, _y(position.y) + yaw = message_yaw(el.pose) stroke, alpha = _style(el) - if el.camera_info and el.camera_info.K[4] > 0: - fy = el.camera_info.K[4] + if el.camera_info and el.camera_info.k[4] > 0: + fy = el.camera_info.k[4] fov_y = 2 * math.atan(el.camera_info.height / (2 * fy)) fov_half = fov_y / 2 wedge_len = 0.8 @@ -239,35 +248,29 @@ def _render_text(el: Text, b: Bounds) -> str: def _render_occupancy_grid(el: OccupancyGrid, b: Bounds) -> str: - if el.grid.size == 0: + cells = occupancy_view(el) + if cells.size == 0: return "" - - rgba = np.flipud(generate_rgba_texture(el)) + rgba = np.flipud(generate_rgba_texture(cells)) img = PILImage.fromarray(rgba, "RGBA") buf = io.BytesIO() img.save(buf, format="PNG") b64 = base64.b64encode(buf.getvalue()).decode("ascii") - - ox, oy = el.origin.x, el.origin.y - world_w = el.width * el.resolution - world_h = el.height * el.resolution - - # SVG top-left: world top-left with Y-flip - sx = ox - sy = _y(oy + world_h) - - b.include(sx, sy) - b.include(sx + world_w, sy + world_h) - + matrix = pose_matrix(el.info.origin) + width, height = occupancy_extent(el) + # Image row zero is the top edge after flipud; project the ROS grid plane into SVG XY. + a, c = matrix[0, 0], -matrix[0, 1] + d, e = -matrix[1, 0], matrix[1, 1] + tx, ty = matrix[0, 3] + matrix[0, 1] * height, -matrix[1, 3] - matrix[1, 1] * height + for x, y in ((0, 0), (width, 0), (width, height), (0, height)): + b.include(a * x + c * y + tx, d * x + e * y + ty) return ( - f'' ) -# Dispatch + top-level render - - def _render_element(el: SpaceElement, b: Bounds) -> str: if isinstance(el, Point): return _render_point(el, b) @@ -286,9 +289,6 @@ def _render_element(el: SpaceElement, b: Bounds) -> str: elif isinstance(el, OccupancyGrid): return _render_occupancy_grid(el, b) elif isinstance(el, PointCloud2): - from dimos.mapping.occupancy.inflation import simple_inflate - from dimos.mapping.pointclouds.occupancy import height_cost_occupancy - return _render_occupancy_grid(simple_inflate(height_cost_occupancy(el), 0.05), b) elif isinstance(el, Observation): ps = el.pose_stamped diff --git a/dimos/memory/vis/space/test_generated_geometry.py b/dimos/memory/vis/space/test_generated_geometry.py new file mode 100644 index 00000000000..ac0a9f36027 --- /dev/null +++ b/dimos/memory/vis/space/test_generated_geometry.py @@ -0,0 +1,155 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Generated geometry reaches both scene renderers without message convenience methods.""" + +import math +import xml.etree.ElementTree as ET + +from dimos_generated.geometry_msgs.msg import Point as GeoPoint, Pose as GeoPose, PoseStamped +from dimos_generated.nav_msgs.msg import MapMetaData, OccupancyGrid, Path +from dimos_generated.sensor_msgs.msg import PointCloud2 +from dimos_generated.std_msgs.msg import Header +import numpy as np +import pytest +import rerun as rr + +from dimos.memory.vis.space.elements import Arrow, Camera, Point, Polyline, Pose +from dimos.memory.vis.space.rerun import render +from dimos.memory.vis.space.space import Space +from dimos.msgs.camera_info import camera_info_from_intrinsics +from dimos.msgs.geometry import quaternion_from_euler +from dimos.msgs.image import image_from_array +from dimos.msgs.pointcloud import pointcloud_from_xyz + + +def test_rerun_logs_generated_pose_point_and_path(mocker): + stamped = PoseStamped( + pose=GeoPose( + position=GeoPoint(x=1, y=2, z=3), orientation=quaternion_from_euler(0, 0, math.pi / 2) + ) + ) + path = Path(poses=[stamped, PoseStamped(pose=GeoPose(position=GeoPoint(x=4, y=5)))]) + space = Space() + space.add(Point(stamped)).add(Pose(stamped)).add(Arrow(stamped)) + space.add(Polyline(path)).add(Camera(stamped)) + mocker.patch("dimos.visualization.rerun.init.rerun_init") + log = mocker.spy(rr, "log") + + with rr.RecordingStream("generated-geometry-test") as recording: + memory = recording.memory_recording() + try: + render(space, spawn=False) + assert memory.num_msgs() > 0 + finally: + recording.disconnect() + + logged = {call.args[0]: call.args[1] for call in log.call_args_list} + assert logged["scene/points"].positions.as_arrow_array().to_pylist() == [[1, 2, 3]] + assert logged["scene/poses/headings"].vectors.as_arrow_array().to_pylist()[0] == pytest.approx( + [0, 0.3, 0], abs=1e-7 + ) + assert logged["scene/polylines/0"].strips.as_arrow_array().to_pylist() == [ + [[1, 2, 0], [4, 5, 0]] + ] + assert logged["scene/cameras/0"].translation.as_arrow_array().to_pylist() == [[1, 2, 3]] + + +def test_both_renderers_apply_rotated_occupancy_origin(mocker): + grid = OccupancyGrid( + info=MapMetaData( + width=2, + height=1, + resolution=1.0, + origin=GeoPose( + position=GeoPoint(x=2, y=3), orientation=quaternion_from_euler(0, 0, math.pi / 2) + ), + ), + data=[0, 100], + ) + space = Space().base_map(OccupancyGrid.decode(grid.encode())) + svg = ET.fromstring(space.to_svg()) + image = svg.find(".//{http://www.w3.org/2000/svg}image") + assert image is not None + transform = image.attrib["transform"].removeprefix("matrix(").removesuffix(")") + assert [float(value) for value in transform.split()] == pytest.approx([0, -1, 1, 0, 1, -3]) + mocker.patch("dimos.visualization.rerun.init.rerun_init") + log = mocker.spy(rr, "log") + with rr.RecordingStream("generated-grid-test") as recording: + memory = recording.memory_recording() + try: + render(space, spawn=False) + assert memory.num_msgs() > 0 + finally: + recording.disconnect() + meshes = [call.args[1] for call in log.call_args_list if call.args[0] == "scene/map/0"] + assert len(meshes) == 1 + np.testing.assert_allclose( + meshes[0].vertex_positions.as_arrow_array().to_pylist(), + [[2, 3, 0], [2, 5, 0], [1, 5, 0], [1, 3, 0]], + atol=1e-7, + ) + + +def test_generated_cloud_renders_in_svg_and_rerun(mocker): + cloud = pointcloud_from_xyz( + np.array([[0.0, 0.0, 0.0], [0.2, 0.3, 1.0], [1.0, 1.0, 0.0]]), + header=Header(frame_id="map"), + ) + space = Space().add(PointCloud2.decode(cloud.encode())) + svg = ET.fromstring(space.to_svg()) + assert svg.find(".//{http://www.w3.org/2000/svg}image") is not None + mocker.patch("dimos.visualization.rerun.init.rerun_init") + log = mocker.spy(rr, "log") + with rr.RecordingStream("generated-cloud-test") as recording: + memory = recording.memory_recording() + try: + render(space, spawn=False) + assert memory.num_msgs() > 0 + finally: + recording.disconnect() + logged = {call.args[0]: call.args[1] for call in log.call_args_list} + np.testing.assert_allclose( + logged["scene/pointcloud/0"].positions.as_arrow_array().to_pylist(), + [[0, 0, 0], [0.2, 0.3, 1], [1, 1, 0]], + ) + + +def test_rerun_camera_uses_generated_calibration_and_image_helpers(mocker): + calibration = camera_info_from_intrinsics(20, 30, 2, 2, 4, 4, header=Header(frame_id="optical")) + image = image_from_array( + np.zeros((4, 4, 3), dtype=np.uint8), encoding="rgb8", header=calibration.header + ) + pose = PoseStamped( + pose=GeoPose(position=GeoPoint(x=2), orientation=quaternion_from_euler(0, 0, 0)) + ) + space = Space().add(Camera(pose=pose, image=image, camera_info=calibration)) + original = (calibration.encode(), image.encode()) + mocker.patch("dimos.visualization.rerun.init.rerun_init") + log = mocker.spy(rr, "log") + with rr.RecordingStream("generated-camera-test") as recording: + recording.memory_recording() + try: + render(space, spawn=False) + finally: + recording.disconnect() + values = [call.args[1] for call in log.call_args_list if call.args[0] == "scene/cameras/0"] + assert any(isinstance(value, rr.Pinhole) for value in values) + pinhole = next(value for value in values if isinstance(value, rr.Pinhole)) + assert pinhole.parent_frame.as_arrow_array().to_pylist() == ["tf#/optical"] + assert any( + call.args[0] == "scene/cameras/0/image" and isinstance(call.args[1], rr.EncodedImage) + for call in log.call_args_list + ) + assert (calibration.encode(), image.encode()) == original diff --git a/dimos/memory/vis/space/test_space.py b/dimos/memory/vis/space/test_space.py index 8f09e0e21ea..cc289827a93 100644 --- a/dimos/memory/vis/space/test_space.py +++ b/dimos/memory/vis/space/test_space.py @@ -14,59 +14,71 @@ """Tests for Space builder and element types.""" +from dimos_generated.geometry_msgs.msg import ( + Point as GeoPoint, + Pose as GeoPose, + PoseStamped, + Quaternion, + Vector3, +) +from dimos_generated.nav_msgs.msg import MapMetaData, OccupancyGrid, Path as Path +from dimos_generated.vision_msgs.msg import Detection3D import numpy as np import pytest from dimos.memory.type.observation import EmbeddedObservation, Observation +from dimos.memory.vis import color from dimos.memory.vis.space.elements import Arrow, Box3D, Camera, Point, Polyline, Pose, Text from dimos.memory.vis.space.space import Space -from dimos.msgs.geometry_msgs.Point import Point as GeoPoint -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.nav_msgs.OccupancyGrid import OccupancyGrid -from dimos.msgs.nav_msgs.Path import Path as Path -from dimos.msgs.sensor_msgs.Image import Image -from dimos.msgs.vision_msgs.Detection3D import Detection3D +from dimos.msgs.image import image_from_array class TestElementTypes: """Element types wrap msgs with rendering intent + style.""" def test_pose_wraps_posestamped(self): - ps = PoseStamped(3.2, 1.5, 0.0) + ps = PoseStamped( + pose=GeoPose(position=GeoPoint(x=3.2, y=1.5, z=0.0), orientation=Quaternion()) + ) p = Pose(ps, color="red", label="fridge") assert p.msg is ps assert p.color == "red" assert p.label == "fridge" def test_arrow_wraps_posestamped(self): - ps = PoseStamped(1, 2, 0, 0, 0, 0.1, 1) + ps = PoseStamped( + pose=GeoPose( + position=GeoPoint(x=1, y=2, z=0), orientation=Quaternion(x=0, y=0, z=0.1, w=1) + ) + ) a = Arrow(ps, color="orange", length=0.8) assert a.msg is ps assert a.length == 0.8 def test_point_wraps_geopoint(self): - gp = GeoPoint(7.1, 4.3, 0) + gp = GeoPoint(x=7.1, y=4.3, z=0) p = Point(gp, color="green", label="bottle") assert p.msg is gp assert p.msg.x == pytest.approx(7.1) def test_point_wraps_posestamped(self): - ps = PoseStamped(3, 1, 0) + ps = PoseStamped(pose=GeoPose(position=GeoPoint(x=3, y=1, z=0), orientation=Quaternion())) p = Point(ps, radius=0.5) - assert p.msg.x == pytest.approx(3.0) + assert p.msg.pose.position.x == pytest.approx(3.0) def test_box3d_from_center_size(self): - from dimos.msgs.geometry_msgs.Pose import Pose as GeoPose - from dimos.msgs.geometry_msgs.Vector3 import Vector3 - - b = Box3D(center=GeoPose(5, 3, 0), size=Vector3(2, 1, 0.5), label="table") - assert b.center.x == pytest.approx(5.0) + b = Box3D( + center=GeoPose(position=GeoPoint(x=5, y=3, z=0), orientation=Quaternion()), + size=Vector3(x=2, y=1, z=0.5), + label="table", + ) + assert b.center.position.x == pytest.approx(5.0) assert b.size.x == pytest.approx(2.0) assert b.label == "table" def test_camera_with_image(self): - ps = PoseStamped(1, 2, 0) - img = Image(np.zeros((480, 640, 3), dtype=np.uint8)) + ps = PoseStamped(pose=GeoPose(position=GeoPoint(x=1, y=2, z=0), orientation=Quaternion())) + img = image_from_array(np.zeros((480, 640, 3), dtype=np.uint8), encoding="rgb8") c = Camera(pose=ps, image=img, color="purple") assert c.pose is ps assert c.image is img @@ -83,7 +95,7 @@ class TestSpaceExplicitElements: def test_add_pose(self): s = Space() - ps = PoseStamped(3, 1, 0) + ps = PoseStamped(pose=GeoPose(position=GeoPoint(x=3, y=1, z=0), orientation=Quaternion())) pose = Pose(ps, color="red") s.add(pose) assert len(s) == 1 @@ -91,15 +103,15 @@ def test_add_pose(self): def test_add_multiple_types(self): s = Space() - ps = PoseStamped(3, 1, 0) + ps = PoseStamped(pose=GeoPose(position=GeoPoint(x=3, y=1, z=0), orientation=Quaternion())) s.add(Pose(ps, color="red")) s.add(Arrow(ps, color="orange")) - s.add(Point(GeoPoint(1, 2, 0), label="x")) + s.add(Point(GeoPoint(x=1, y=2, z=0), label="x")) s.add(Text((0, 0, 0), "hello")) assert len(s) == 4 def test_chaining(self): - ps = PoseStamped(1, 1, 0) + ps = PoseStamped(pose=GeoPose(position=GeoPoint(x=1, y=1, z=0), orientation=Quaternion())) s = Space().add(Pose(ps)).add(Arrow(ps)).add(Text((0, 0, 0), "hi")) assert len(s) == 3 @@ -109,7 +121,9 @@ class TestSpaceAutoWrap: def test_posestamped_becomes_pose(self): s = Space() - ps = PoseStamped(3.2, 1.5, 0) + ps = PoseStamped( + pose=GeoPose(position=GeoPoint(x=3.2, y=1.5, z=0), orientation=Quaternion()) + ) s.add(ps, color="blue", label="auto") assert len(s) == 1 el = s.elements[0] @@ -120,7 +134,7 @@ def test_posestamped_becomes_pose(self): def test_geopoint_becomes_point(self): s = Space() - gp = GeoPoint(7, 4, 0) + gp = GeoPoint(x=7, y=4, z=0) s.add(gp, color="yellow") el = s.elements[0] assert isinstance(el, Point) @@ -129,7 +143,14 @@ def test_geopoint_becomes_point(self): def test_path_becomes_polyline(self): s = Space() - p = Path(poses=[PoseStamped(i, 0, 0) for i in range(3)]) + p = Path( + poses=[ + PoseStamped( + pose=GeoPose(position=GeoPoint(x=i, y=0, z=0), orientation=Quaternion()) + ) + for i in range(3) + ] + ) s.add(p, color="blue", width=0.1) el = s.elements[0] assert isinstance(el, Polyline) @@ -169,7 +190,7 @@ class TestSpaceObservations: """Space.add() smart dispatch for Observation types.""" def test_image_observation_stored_as_observation(self): - img = Image(np.zeros((480, 640, 3), dtype=np.uint8)) + img = image_from_array(np.zeros((480, 640, 3), dtype=np.uint8), encoding="rgb8") obs = Observation(id=1, ts=1.0, pose=(3, 1, 0, 0, 0, 0, 1), _data=img) s = Space() @@ -188,15 +209,18 @@ def test_non_image_observation_stored_as_observation(self): assert el.data == "some_data" def test_posestamped_observation_stored_as_observation(self): - from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped as PS - - obs = Observation(id=3, ts=3.0, pose=(1, 2, 0, 0, 0, 0, 1), _data=PS(5, 2, 0)) + obs = Observation( + id=3, + ts=3.0, + pose=(1, 2, 0, 0, 0, 0, 1), + _data=PoseStamped(pose=GeoPose(position=GeoPoint(x=5, y=2))), + ) s = Space() s.add(obs) el = s.elements[0] assert isinstance(el, Observation) - assert el.data.x == pytest.approx(5.0) + assert el.data.pose.position.x == pytest.approx(5.0) def test_embedded_observation_stored_as_arrow(self): obs = EmbeddedObservation( @@ -224,7 +248,10 @@ def test_base_map(self): assert isinstance(s.elements[0], OccupancyGrid) def test_add_list_of_msgs(self): - poses = [PoseStamped(i, 0, 0) for i in range(3)] + poses = [ + PoseStamped(pose=GeoPose(position=GeoPoint(x=i, y=0, z=0), orientation=Quaternion())) + for i in range(3) + ] s = Space() s.add(poses, color="red") assert len(s) == 3 @@ -239,7 +266,7 @@ def test_repr_empty(self): def test_repr_with_elements(self): s = Space() - ps = PoseStamped(0, 0, 0) + ps = PoseStamped(pose=GeoPose(position=GeoPoint(x=0, y=0, z=0), orientation=Quaternion())) s.add(Pose(ps)) s.add(Pose(ps)) s.add(Arrow(ps)) @@ -255,29 +282,40 @@ def test_empty_space(self): assert svg.endswith("") def test_point_renders_circle(self): - from dimos.memory.vis import color - s = Space() - s.add(Point(GeoPoint(3, 4, 0), color="red", label="hi")) + s.add(Point(GeoPoint(x=3, y=4, z=0), color="red", label="hi")) svg = s.to_svg() assert "/` output describes the native Python fields, keyword-only +constructors, sequence operations, and NumPy views. For a local generated build, +point `MYPYPATH` at `typing/`. Wheels install the same interfaces as a PEP 561 +`-stubs` package, so callers get type checking without source generation +or runtime introspection. Fixed-array interfaces omit resizing methods. + +DimOS-owned definitions live under `schemas/dimos_msgs/msg/`. They use the same +generation and packaging path as the pinned standard definitions. In particular, +weighted line segments have explicit endpoints and weights; they do not reuse +Path poses or quaternion fields. Stamped custom messages use standard Header, +and trajectory durations use builtin_interfaces/Duration. Numeric convenience +operations belong in helpers such as `dimos.msgs.time`, outside generated types. + Python and C++ use Fast CDR 2.4.0; Python bindings use pybind11 3.0.1. Rust uses `re_cdr` 0.1.0 and `serde-big-array` 0.5.1. The current generator explicitly rejects `wstring` because the selected Rust backend has no matching wide-string Serde diff --git a/dimos/message_codegen/__init__.py b/dimos/message_codegen/__init__.py deleted file mode 100644 index e69de29bb2d..00000000000 diff --git a/dimos/message_codegen/_vendor/__init__.py b/dimos/message_codegen/_vendor/__init__.py deleted file mode 100644 index e69de29bb2d..00000000000 diff --git a/dimos/message_codegen/_vendor/rosidl_parser.py b/dimos/message_codegen/_vendor/rosidl_parser.py index cdf64bc3ae6..2063a08b75a 100644 --- a/dimos/message_codegen/_vendor/rosidl_parser.py +++ b/dimos/message_codegen/_vendor/rosidl_parser.py @@ -907,7 +907,7 @@ def parse_action_string(pkg_name, action_name, action_string): pkg_name, action_name + ACTION_RESULT_SUFFIX, result_string) feedback_message = parse_message_string( pkg_name, action_name + ACTION_FEEDBACK_SUFFIX, feedback_string) - # --------------------------------------------------------------------------------------------- + # Parse action definitions. return ActionSpecification( pkg_name, action_name, goal_message, result_message, feedback_message) diff --git a/dimos/message_codegen/backend.py b/dimos/message_codegen/backend.py new file mode 100644 index 00000000000..272df7210c3 --- /dev/null +++ b/dimos/message_codegen/backend.py @@ -0,0 +1,146 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""PEP 517/660 backend for standalone message projects, not the DimOS runtime.""" + +from __future__ import annotations + +from collections.abc import Iterator +from contextlib import contextmanager +import gzip +import os +from pathlib import Path +import tarfile +from typing import Any + +from setuptools import build_meta + +from .distribution import write_distribution +from .project import Project, prepare + + +@contextmanager +def python_project() -> Iterator[None]: + project = Project.load(Path.cwd()) + output = prepare(project) + dependencies = project.dependencies() + write_distribution( + output, + project.module, + (), + project.version, + dependencies=dependencies, + shared=True, + source_output=output, + ) + previous = Path.cwd() + old_prefix = os.environ.get("CMAKE_PREFIX_PATH") + prefixes = [str(dep.root) for dep in dependencies] + if old_prefix: + prefixes.append(old_prefix) + os.environ["CMAKE_PREFIX_PATH"] = os.pathsep.join(prefixes) + try: + os.chdir(output / "python") + yield + finally: + os.chdir(previous) + if old_prefix is None: + os.environ.pop("CMAKE_PREFIX_PATH", None) + else: + os.environ["CMAKE_PREFIX_PATH"] = old_prefix + + +def get_requires_for_build_wheel(config_settings: dict[str, Any] | None = None) -> list[str]: + return [ + "setuptools>=70", + "wheel", + "pybind11==3.0.1", + "cmake>=3.20", + *Project.load(Path.cwd()).requirements(), + ] + + +get_requires_for_build_editable = get_requires_for_build_wheel + + +def get_requires_for_build_sdist(config_settings: dict[str, Any] | None = None) -> list[str]: + return [] + + +def build_wheel( + wheel_directory: str, + config_settings: dict[str, Any] | None = None, + metadata_directory: str | None = None, +) -> str: + destination = str(Path(wheel_directory).resolve()) + metadata = str(Path(metadata_directory).resolve()) if metadata_directory else None + with python_project(): + return str(build_meta.build_wheel(destination, config_settings, metadata)) + + +def build_editable( + wheel_directory: str, + config_settings: dict[str, Any] | None = None, + metadata_directory: str | None = None, +) -> str: + destination = str(Path(wheel_directory).resolve()) + metadata = str(Path(metadata_directory).resolve()) if metadata_directory else None + with python_project(): + return str(build_meta.build_editable(destination, config_settings, metadata)) + + +def prepare_metadata_for_build_wheel( + metadata_directory: str, config_settings: dict[str, Any] | None = None +) -> str: + destination = str(Path(metadata_directory).resolve()) + with python_project(): + return str(build_meta.prepare_metadata_for_build_wheel(destination, config_settings)) + + +prepare_metadata_for_build_editable = prepare_metadata_for_build_wheel + + +def build_sdist(sdist_directory: str, config_settings: dict[str, Any] | None = None) -> str: + project = Project.load(Path.cwd()) + if any("path" in spec for spec in project.dependency_specs.values()): + raise ValueError( + "Source distributions require installed, version-pinned dependencies; remove development dependency paths" + ) + name = f"{project.name.replace('-', '_')}-{project.version}" + destination = Path(sdist_directory).resolve() + destination.mkdir(parents=True, exist_ok=True) + paths = [project.root / "pyproject.toml", *sorted(project.source.rglob("*.msg"))] + if not project.internal and len(paths) == 1: + raise ValueError("Cannot package a project with no message definitions") + paths.extend( + path for path in (project.root / "README.md", project.root / "LICENSE") if path.is_file() + ) + epoch = int(os.environ.get("SOURCE_DATE_EPOCH", "0")) + with ( + (destination / f"{name}.tar.gz").open("wb") as raw, + gzip.GzipFile(filename="", mode="wb", fileobj=raw, mtime=epoch) as compressed, + tarfile.open(fileobj=compressed, mode="w") as archive, + ): + for path in paths: + if path.is_symlink() or not path.resolve().is_relative_to(project.root): + raise ValueError(f"Source file escapes the project: {path}") + info = archive.gettarinfo( + str(path), f"{name}/{path.relative_to(project.root).as_posix()}" + ) + info.uid = info.gid = 0 + info.uname = info.gname = "" + info.mtime = epoch + with path.open("rb") as stream: + archive.addfile(info, stream) + return f"{name}.tar.gz" diff --git a/dimos/message_codegen/build.py b/dimos/message_codegen/build.py new file mode 100644 index 00000000000..40874c6aff5 --- /dev/null +++ b/dimos/message_codegen/build.py @@ -0,0 +1,163 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Language build frontends over the shared message-project generation plan.""" + +from __future__ import annotations + +from collections.abc import Iterator +from contextlib import contextmanager +import json +import os +from pathlib import Path +import re +import shutil +import subprocess +import sys +import tarfile + +from . import backend +from .project import LANGUAGES, Project, prepare + + +@contextmanager +def project_directory(root: Path) -> Iterator[None]: + previous = Path.cwd() + try: + os.chdir(root) + yield + finally: + os.chdir(previous) + + +def local_cargo_dependency(text: str, module: str) -> str: + """Relocate one versioned crate reference inside a local source bundle.""" + crate_name = module.replace("_", "-") + "-messages" + + def replace(match: re.Match[str]) -> str: + declaration = re.sub(r', path = "[^"]*"', "", match.group(1)) + return declaration + ", path = " + json.dumps("../" + module) + " }" + + return re.sub( + r"(?m)^(" + re.escape(crate_name) + r" = \{[^}]*?)(?:, path = \"[^\"]*\")? \}$", + replace, + text, + ) + + +def build_project( + project: Project, + languages: tuple[str, ...] | None = None, + *, + install: bool = False, + offline: bool = False, +) -> dict[str, str]: + selected = languages or project.languages + if not selected or any(language not in LANGUAGES for language in selected): + raise ValueError("Select python, cpp and/or rust") + if install and "python" in selected and sys.prefix == sys.base_prefix: + raise ValueError( + "Python --install requires an active virtual environment; no system install is performed" + ) + for tool in ({"cmake", "c++"} if set(selected) & {"python", "cpp"} else set()) | ( + {"cargo"} if "rust" in selected else set() + ): + if shutil.which(tool) is None: + raise ValueError( + f"Missing build tool {tool}; prepare the language toolchain before building" + ) + output = prepare(project) + dist = project.root / "dist" + dist.mkdir(exist_ok=True) + state = output / "artifacts.json" + artifacts = json.loads(state.read_text()) if state.is_file() else {} + artifacts.update( + { + "schemas": str(output / "schemas.json"), + "ownership": str(output / "message-package.json"), + } + ) + dependencies = project.dependencies() + prefixes = [str(dep.root) for dep in dependencies] + prefixes.extend(os.environ.get("CMAKE_PREFIX_PATH", "").split(os.pathsep)) + if "python" in selected: + with project_directory(project.root): + wheel = backend.build_wheel(str(dist)) + backend.build_sdist(str(dist)) + artifacts["python_wheel"] = str(dist / wheel) + if install: + subprocess.run( + [sys.executable, "-m", "pip", "install", "--no-deps", str(dist / wheel)], check=True + ) + if "cpp" in selected: + subprocess.run( + [ + "cmake", + "-S", + str(output / "cpp"), + "-B", + str(output / "cmake"), + "-DDIMOS_BUILD_PYTHON=OFF", + "-DCMAKE_PREFIX_PATH=" + ";".join(prefixes), + "-DCMAKE_INSTALL_PREFIX=" + str(output / "install"), + ], + check=True, + ) + subprocess.run(["cmake", "--build", str(output / "cmake"), "--parallel", "2"], check=True) + subprocess.run(["cmake", "--install", str(output / "cmake")], check=True) + archive = dist / f"{project.name}-{project.version}-cmake.tar.gz" + with tarfile.open(archive, "w:gz") as tar: + tar.add(output / "install", arcname=".") + artifacts["cmake_prefix"] = str(output / "install") + artifacts["cmake_archive"] = str(archive) + if "rust" in selected: + # A relocatable local source bundle works before any registry publication. + bundle = output / "cargo-packages" + lock_path = bundle / project.module / "Cargo.lock" + previous_lock = lock_path.read_bytes() if lock_path.is_file() else None + if bundle.exists(): + shutil.rmtree(bundle) + crates = [(project.module, output), *((dep.module, dep.root) for dep in dependencies)] + for module, root in crates: + target = bundle / module + shutil.copytree( + root / "rust", target, ignore=shutil.ignore_patterns("target", "Cargo.lock") + ) + manifest = target / "Cargo.toml" + text = manifest.read_text() + for dep in dependencies: + text = local_cargo_dependency(text, dep.module) + manifest.write_text(text) + cargo_manifest = bundle / project.module / "Cargo.toml" + if previous_lock is not None: + lock_path.write_bytes(previous_lock) + subprocess.run( + [ + "cargo", + "build", + "--manifest-path", + str(cargo_manifest), + *(["--offline"] if offline else []), + *(["--locked"] if previous_lock is not None else []), + ], + env={**os.environ, "CARGO_TARGET_DIR": str(output / "cargo-target")}, + check=True, + ) + archive = dist / f"{project.name}-{project.version}-cargo.tar.gz" + with tarfile.open(archive, "w:gz") as tar: + tar.add(bundle, arcname=".") + artifacts["cargo_manifest"] = str(cargo_manifest) + artifacts["cargo_archive"] = str(archive) + (output / "artifacts.json").write_text(json.dumps(artifacts, indent=2, sort_keys=True) + "\n") + return artifacts diff --git a/dimos/message_codegen/build.rs b/dimos/message_codegen/build.rs new file mode 100644 index 00000000000..bf5cff3fdd5 --- /dev/null +++ b/dimos/message_codegen/build.rs @@ -0,0 +1,45 @@ +// Copyright 2026 Dimensional Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// Copyright 2026 Dimensional Inc. +// SPDX-License-Identifier: Apache-2.0 + +use std::{env, path::PathBuf, process::Command}; + +fn main() { + let root = PathBuf::from(env::var_os("CARGO_MANIFEST_DIR").unwrap()); + for input in [ + "build_rust.py", + "definitions.py", + "providers.py", + "rust.py", + "_vendor", + "schemas", + "templates/codec.rs", + ] { + println!("cargo:rerun-if-changed={input}"); + } + println!("cargo:rerun-if-env-changed=DIMOS_CODEGEN_PYTHON"); + let python = env::var_os("DIMOS_CODEGEN_PYTHON").unwrap_or_else(|| "python3".into()); + let status = Command::new(python) + .arg("-I") + .arg("-B") + .arg(root.join("build_rust.py")) + .arg(env::var_os("OUT_DIR").unwrap()) + .status() + .expect( + "message generation requires Python 3.10+; set DIMOS_CODEGEN_PYTHON to its executable", + ); + assert!(status.success(), "ROS-free Rust message generation failed"); +} diff --git a/dimos/message_codegen/build_rust.py b/dimos/message_codegen/build_rust.py new file mode 100644 index 00000000000..5d1af76e54e --- /dev/null +++ b/dimos/message_codegen/build_rust.py @@ -0,0 +1,48 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Cargo entry point using only this source package and Python's standard library.""" + +import importlib +import importlib.machinery +import importlib.util +from pathlib import Path +import shutil +import sys +from types import ModuleType + + +def main() -> None: + root = Path(__file__).resolve().parent + # Cargo packages do not preserve the repository's parent directory names. + # Bind the bundled codegen package explicitly, without importing installed DimOS. + parent = ModuleType("dimos") + parent.__path__ = [] + sys.modules["dimos"] = parent + spec = importlib.machinery.ModuleSpec("dimos.message_codegen", loader=None, is_package=True) + spec.submodule_search_locations = [str(root)] + package = importlib.util.module_from_spec(spec) + sys.modules[spec.name] = package + definitions_module = importlib.import_module("dimos.message_codegen.definitions") + rust_module = importlib.import_module("dimos.message_codegen.rust") + definitions = definitions_module.Definitions([]) + messages = definitions.resolve() + output = Path(sys.argv[1]) + output.mkdir(parents=True, exist_ok=True) + (output / "messages.rs").write_text(rust_module.generate(messages, definitions)) + shutil.copyfile(root / "templates/codec.rs", output / "codec.rs") + + +if __name__ == "__main__": + main() diff --git a/dimos/message_codegen/builtin_provider.py b/dimos/message_codegen/builtin_provider.py index 02ac387693d..f93f946a47d 100644 --- a/dimos/message_codegen/builtin_provider.py +++ b/dimos/message_codegen/builtin_provider.py @@ -18,7 +18,7 @@ from pathlib import Path from typing import Any -from dimos.message_codegen.definitions import Definitions +from .definitions import Definitions def schema_root() -> Path: diff --git a/dimos/message_codegen/cpp.py b/dimos/message_codegen/cpp.py index 6cac6108dfe..2b2e06e5a51 100644 --- a/dimos/message_codegen/cpp.py +++ b/dimos/message_codegen/cpp.py @@ -22,7 +22,7 @@ import math from typing import Any -from dimos.message_codegen.definitions import FieldType, Message +from .definitions import FieldType, Message PRIMITIVES = { "bool": "bool", @@ -202,7 +202,7 @@ def validation(message: Message) -> list[str]: return lines -def generate(messages: tuple[Message, ...]) -> str: +def generate(messages: tuple[Message, ...], imports: tuple[str, ...] = ()) -> str: guards = { message.name: "DIMOS_MESSAGE_" + sha256( @@ -232,6 +232,7 @@ def generate(messages: tuple[Message, ...]) -> str: "#include ", '#include "dimos_cdr.hpp"', ] + lines.extend(f"#include <{module}/messages.hpp>" for module in imports) for message in messages: guard = guards[message.name] + "_TYPE" lines.extend([f"#ifndef {guard}", f"#define {guard}"]) @@ -253,12 +254,23 @@ def generate(messages: tuple[Message, ...]) -> str: else: default = "{" + literal(field.default, field.type.name) + "}" lines.append(f"{type_name(field.type)} {identifier(field.name)}{default};") + equal = ( + " && ".join( + f"this->{identifier(field.name)} == other.{identifier(field.name)}" + for field in message.fields + ) + or "true" + ) + other = " other" if message.fields else "" + lines.append(f"bool operator==(const {name}&{other}) const {{ return {equal}; }}") + lines.append(f"bool operator!=(const {name}& other) const {{ return !(*this == other); }}") lines.append("void validate() const {") lines.extend(validation(message)) lines.extend(["}", f'static constexpr const char* msg_name = "{message.name}";', "};", "}"]) lines.append("#endif") lines.append("namespace eprosima::fastcdr {") + lines.extend(f"#include <{module}/messages.hpp>" for module in imports) for message in messages: guard = guards[message.name] + "_CODEC" lines.extend([f"#ifndef {guard}", f"#define {guard}"]) diff --git a/dimos/message_codegen/definitions.py b/dimos/message_codegen/definitions.py index e2f8db261f4..7a862464997 100644 --- a/dimos/message_codegen/definitions.py +++ b/dimos/message_codegen/definitions.py @@ -23,12 +23,12 @@ from pathlib import Path from typing import Any -from dimos.message_codegen._vendor.rosidl_parser import ( +from ._vendor.rosidl_parser import ( InvalidSpecification, InvalidValue, parse_message_string, ) -from dimos.message_codegen.providers import schema_roots +from .providers import schema_roots BUNDLED_SCHEMAS = Path(__file__).with_name("schemas") diff --git a/dimos/message_codegen/distribution.py b/dimos/message_codegen/distribution.py index e8b506a20bd..dc4497b4ff1 100644 --- a/dimos/message_codegen/distribution.py +++ b/dimos/message_codegen/distribution.py @@ -18,12 +18,25 @@ import json from pathlib import Path +import re import shutil +from .ownership import Dependency + def write_distribution( - output: Path, module: str, names: tuple[str, ...], version: str = "0.1.0" + output: Path, + module: str, + names: tuple[str, ...], + version: str = "0.1.0", + *, + dependencies: tuple[Dependency, ...] = (), + shared: bool = False, + source_output: Path | None = None, ) -> None: + manifest = json.loads((output / "message-package.json").read_text()) + names = tuple(manifest["owned"]) + shared = shared or manifest["shared"] project = output / "python" support = module + "_schemas" package = project / support @@ -33,12 +46,27 @@ def write_distribution( shutil.rmtree(package / "schemas") shutil.copytree(output / "schemas", package / "schemas") shutil.copyfile(output / "schemas.json", package / "schemas.json") + resources = package / "package" + if resources.exists(): + shutil.rmtree(resources) + resources.mkdir() + for directory in ("schemas", "include", "lib", "rust"): + shutil.copytree( + output / directory, + resources / directory, + ignore=shutil.ignore_patterns("target", "Cargo.lock"), + ) + cargo_manifest = resources / "rust" / "Cargo.toml" + cargo_manifest.write_text(re.sub(r', path = "[^"\n]*"', "", cargo_manifest.read_text())) + shutil.copyfile(output / "message-package.json", resources / "message-package.json") + shutil.copyfile(output / "schemas.json", resources / "schemas.json") (package / "provider.py").write_text( "from importlib import import_module\nimport json\nfrom pathlib import Path\n\n" "def schema_root():\n return Path(__file__).with_name('schemas')\n\n" + "def package_root():\n return Path(__file__).with_name('package')\n\n" "def message_types():\n" f" extension = import_module({module!r})\n" - " names = json.loads(Path(__file__).with_name('schemas.json').read_text())\n" + " names = json.loads((package_root() / 'message-package.json').read_text())['owned']\n" " return {name: getattr(getattr(extension, name.split('/')[0]).msg, name.split('/')[-1]) for name in names}\n" ) toolkit = project / "_codegen" / "dimos" / "message_codegen" @@ -46,32 +74,51 @@ def write_distribution( Path(__file__).parent, toolkit, dirs_exist_ok=True, - ignore=shutil.ignore_patterns("__pycache__", "test_*.py"), + ignore=shutil.ignore_patterns( + "__pycache__", "test_*.py", "build", "dist", "*.egg-info", "target" + ), ) # A namespace directory loses to an installed regular `dimos` package, # even with _codegen first on sys.path. Build with the bundled generator. (toolkit.parent / "__init__.py").write_text("") (project / "pyproject.toml").write_text( - '[build-system]\nrequires = ["setuptools>=70", "wheel", "pybind11==3.0.1"]\n' - 'build-backend = "setuptools.build_meta"\n' + "[build-system]\nrequires = " + + json.dumps( + [ + "setuptools>=70", + "wheel", + "pybind11==3.0.1", + "cmake>=3.20", + *[dep.module.replace("_", "-") + "==" + dep.version for dep in dependencies], + ] + ) + + "\n" + + 'build-backend = "setuptools.build_meta"\n' ) (project / "setup.py").write_text( - "from pathlib import Path\nimport sys\nfrom setuptools import setup\n" + "from pathlib import Path\nimport os\nimport sys\nfrom setuptools import setup\n" "sys.path.insert(0, str(Path(__file__).parent / '_codegen'))\n" "from dimos.message_codegen.generate import generate\n" + "from dimos.message_codegen.ownership import Dependency\n" + "from importlib import import_module\n" "from dimos.message_codegen.native_build import MessageExtension, MessageBuildExt\n" "root = Path(__file__).parent\n" - f"generate([root / {support!r} / 'schemas'], root / 'build/messages', {list(names)!r}, {module!r}, version={version!r})\n" - f"setup(name={module.replace('_', '-')!r}, version={version!r}, packages=[{support!r}],\n" - f" package_data={{{support!r}: ['schemas.json', 'schemas/**/*']}},\n" + f"dependencies = tuple(Dependency.load(Path(import_module(name + '_schemas').__file__).parent / 'package') for name in {[dep.module for dep in dependencies]!r})\n" + + "os.environ['CMAKE_PREFIX_PATH'] = os.pathsep.join([str(dep.root) for dep in dependencies] + [os.environ.get('CMAKE_PREFIX_PATH', '')])\n" + + ( + f"generate([root / {support!r} / 'schemas'], root / 'build/messages', {list(names)!r}, {module!r}, version={version!r}, dependencies=dependencies, shared={shared!r})\nmessage_source = root / 'build/messages/cpp'\n" + if source_output is None + else f"message_source = Path({str(source_output / 'cpp')!r})\n" + ) + + f"setup(name={module.replace('_', '-')!r}, version={version!r}, packages=[{support!r}],\n" + f" package_data={{{support!r}: ['schemas.json', 'schemas/**/*', 'package/**/*']}},\n" + f" install_requires={[dep.module.replace(chr(95), chr(45)) + '==' + dep.version for dep in dependencies]!r},\n" f" entry_points={{'dimos.messages': [{(module + '=' + support + '.provider')!r}]}},\n" - f" ext_modules=[MessageExtension({module!r}, root / 'build/messages/cpp')],\n" + f" ext_modules=[MessageExtension({module!r}, message_source)],\n" " cmdclass={'build_ext': MessageBuildExt})\n" ) (project / "MANIFEST.in").write_text( f"graft _codegen\ngraft {support}\ninclude pyproject.toml setup.py\n" "global-exclude __pycache__ *.pyc\n" ) - (project / "message-package.json").write_text( - json.dumps({"module": module, "types": names}, indent=2) + "\n" - ) + shutil.copyfile(output / "message-package.json", project / "message-package.json") diff --git a/dimos/message_codegen/generate.py b/dimos/message_codegen/generate.py index c1acf694ab9..5d26775e4e3 100644 --- a/dimos/message_codegen/generate.py +++ b/dimos/message_codegen/generate.py @@ -19,13 +19,15 @@ import argparse import json import keyword +import os from pathlib import Path import re import shutil -from dimos.message_codegen import cpp, python, rust -from dimos.message_codegen.definitions import Definitions -from dimos.message_codegen.distribution import write_distribution +from . import cpp, python, rust, stubs +from .definitions import Definitions +from .distribution import write_distribution +from .ownership import ABI, Dependency, resolve_owners, schema_hash TEMPLATES = Path(__file__).with_name("templates") @@ -38,13 +40,17 @@ def generate( *, installed: bool = False, version: str = "0.1.0", + dependencies: tuple[Dependency, ...] = (), + shared: bool = False, ) -> tuple[str, ...]: """Emit all three native language outputs, resolving dependencies first.""" if not re.fullmatch(r"[a-z][a-z0-9_]*", module) or keyword.iskeyword(module): raise ValueError(f"Invalid Python extension module name: {module}") if not re.fullmatch(r"[0-9]+\.[0-9]+\.[0-9]+", version): raise ValueError("Package version must have the form major.minor.patch") - definitions = Definitions(roots, installed=installed) + definitions = Definitions( + roots + [dep.root / "schemas" for dep in dependencies], installed=installed + ) messages = definitions.resolve(names) for message in messages: for field in message.fields: @@ -52,30 +58,67 @@ def generate( raise ValueError( f"{message.source}: wstring is not yet supported by the Rust CDR backend" ) + owners = resolve_owners(messages, definitions, dependencies) + owned = tuple(message for message in messages if message.name not in owners) + imports = tuple(sorted({owner.module for owner in owners.values()})) output.mkdir(parents=True, exist_ok=True) + typing_root = output / "typing" / module + if typing_root.exists(): + shutil.rmtree(typing_root) + stub_files = stubs.generate(owned, module) + for name, content in stub_files.items(): + for imported_name, owner in owners.items(): + content = content.replace( + f"{module}.{imported_name.replace(chr(47), chr(46))}", + f"{owner.module}.{imported_name.replace(chr(47), chr(46))}", + ) + if name.endswith(".pyi") and content: + content = "".join(f"import {dep}\n" for dep in imports) + content + path = typing_root / name + path.parent.mkdir(parents=True, exist_ok=True) + path.write_text(content) native = output / "cpp" native.mkdir(exist_ok=True) - (native / "messages.hpp").write_text(cpp.generate(messages)) + (native / "messages.hpp").write_text(cpp.generate(owned, imports)) shutil.copyfile(TEMPLATES / "dimos_cdr.hpp", native / "dimos_cdr.hpp") shutil.copyfile(TEMPLATES / "dimos_python.hpp", native / "dimos_python.hpp") - bindings = python.generate(messages, definitions, module) + bindings = python.generate( + owned, + definitions, + module, + imports=imports, + shared=shared, + version=version, + imported={name: owner.module for name, owner in owners.items()}, + dependency_versions={dep.module: dep.version for dep in dependencies}, + ) for name, content in bindings.items(): (native / name).write_text(content) + for stale in native.glob("bind_*.cpp"): + if stale.name not in bindings: + stale.unlink() sources = " ".join(sorted(bindings)) + dependency_cmake = "".join( + f"find_package({dep.module} {dep.version} EXACT CONFIG REQUIRED)\n" for dep in dependencies + ) + dependency_targets = " ".join(f"{name}::messages" for name in imports) (native / "CMakeLists.txt").write_text( "cmake_minimum_required(VERSION 3.20)\n" f"project({module} VERSION {version} LANGUAGES CXX)\n" "find_package(fastcdr 2.4.0 EXACT REQUIRED)\n" "include(GNUInstallDirs)\n" - f"add_library({module}_messages INTERFACE)\n" + + dependency_cmake + + f"add_library({module}_messages INTERFACE)\n" f"set_target_properties({module}_messages PROPERTIES EXPORT_NAME messages)\n" f"target_compile_features({module}_messages INTERFACE cxx_std_17)\n" f"target_include_directories({module}_messages INTERFACE $ $)\n" - f"target_link_libraries({module}_messages INTERFACE fastcdr)\n" + f"target_link_libraries({module}_messages INTERFACE fastcdr {dependency_targets})\n" f"install(TARGETS {module}_messages EXPORT {module}Targets)\n" f"install(FILES messages.hpp dimos_cdr.hpp DESTINATION include/{module})\n" f"install(EXPORT {module}Targets NAMESPACE {module}:: DESTINATION lib/cmake/{module})\n" - f"install(FILES {module}Config.cmake DESTINATION lib/cmake/{module})\n" + "include(CMakePackageConfigHelpers)\n" + f"write_basic_package_version_file(${{CMAKE_CURRENT_BINARY_DIR}}/{module}ConfigVersion.cmake VERSION {version} COMPATIBILITY ExactVersion)\n" + f"install(FILES {module}Config.cmake ${{CMAKE_CURRENT_BINARY_DIR}}/{module}ConfigVersion.cmake DESTINATION lib/cmake/{module})\n" f"install(DIRECTORY ../schemas DESTINATION share/{module})\n" 'option(DIMOS_BUILD_PYTHON "Build Python bindings" ON)\n' "if(DIMOS_BUILD_PYTHON)\n" @@ -84,23 +127,38 @@ def generate( "find_package(pybind11 3.0.1 EXACT REQUIRED)\n" f"pybind11_add_module({module} NO_EXTRAS {sources})\n" f"target_compile_features({module} PRIVATE cxx_std_17)\n" - f"target_link_libraries({module} PRIVATE fastcdr)\n" + f"target_link_libraries({module} PRIVATE fastcdr {dependency_targets})\n" "endif()\n" ) (native / f"{module}Config.cmake").write_text( "include(CMakeFindDependencyMacro)\nfind_dependency(fastcdr 2.4.0 EXACT)\n" - f'include("${{CMAKE_CURRENT_LIST_DIR}}/{module}Targets.cmake")\n' + + "".join( + f"find_dependency({dep.module} {dep.version} EXACT CONFIG)\n" for dep in dependencies + ) + + f'include("${{CMAKE_CURRENT_LIST_DIR}}/{module}Targets.cmake")\n' ) crate = output / "rust" (crate / "src").mkdir(parents=True, exist_ok=True) - (crate / "src" / "lib.rs").write_text(rust.generate(messages, definitions)) - shutil.copyfile(TEMPLATES / "codec.rs", crate / "src" / "codec.rs") + (crate / "src" / "lib.rs").write_text( + rust.generate(owned, definitions, {name: owner.crate for name, owner in owners.items()}) + ) + codec_owner = ( + (dependencies[0].codec_owner or dependencies[0].module) if dependencies else module + ) + if dependencies: + (crate / "src" / "codec.rs").write_text(f"pub use {codec_owner}_messages::codec::*;\n") + else: + shutil.copyfile(TEMPLATES / "codec.rs", crate / "src" / "codec.rs") (crate / "Cargo.toml").write_text( f'[package]\nname = "{module.replace("_", "-")}-messages"\nversion = "{version}"\nedition = "2024"\n' 'license = "Apache-2.0"\ndescription = "Native CDR messages generated from ROS2 definitions"\n' 'include = ["src/*.rs", "schemas.json", "schemas/**", "Cargo.toml"]\n' '[workspace]\n[dependencies]\nserde = { version = "1.0", features = ["derive"] }\n' 'serde-big-array = "=0.5.1"\nre_cdr = "=0.1.0"\n' + + "".join( + f'{dep.module.replace("_", "-")}-messages = {{ version = "={dep.version}", path = {json.dumps(os.path.relpath(dep.root / "rust", crate))} }}\n' + for dep in dependencies + ) ) metadata = {message.name: definitions.schema(message.name) for message in messages} (output / "schemas.json").write_text(json.dumps(metadata, indent=2, sort_keys=True) + "\n") @@ -119,6 +177,44 @@ def generate( if (crate / "schemas").exists(): shutil.rmtree(crate / "schemas") shutil.copytree(schema_root, crate / "schemas", dirs_exist_ok=True) + (output / "message-package.json").write_text( + json.dumps( + { + "format": 1, + "abi": ABI, + "module": module, + "version": version, + "shared": shared, + "codec_owner": codec_owner, + "dependencies": {dep.module: dep.version for dep in dependencies}, + "owned": [message.name for message in owned], + "schemas": {name: schema_hash(schema) for name, schema in metadata.items()}, + }, + indent=2, + sort_keys=True, + ) + + "\n" + ) + # A relocatable header-only CMake package also travels inside message wheels. + include = output / "include" / module + include.mkdir(parents=True, exist_ok=True) + for filename in ("messages.hpp", "dimos_cdr.hpp"): + shutil.copyfile(native / filename, include / filename) + config = output / "lib" / "cmake" / module + config.mkdir(parents=True, exist_ok=True) + (config / f"{module}Config.cmake").write_text( + "include(CMakeFindDependencyMacro)\nfind_dependency(fastcdr 2.4.0 EXACT)\n" + + "".join( + f"find_dependency({dep.module} {dep.version} EXACT CONFIG)\n" for dep in dependencies + ) + + f"if(NOT TARGET {module}::messages)\nadd_library({module}::messages INTERFACE IMPORTED)\n" + + f'set_target_properties({module}::messages PROPERTIES INTERFACE_COMPILE_FEATURES "cxx_std_17" INTERFACE_INCLUDE_DIRECTORIES "${{CMAKE_CURRENT_LIST_DIR}}/../../../include" INTERFACE_LINK_LIBRARIES "fastcdr;{";".join(f"{name}::messages" for name in imports)}")\nendif()\n' + ) + (config / f"{module}ConfigVersion.cmake").write_text( + f'set(PACKAGE_VERSION "{version}")\n' + "if(PACKAGE_FIND_VERSION VERSION_EQUAL PACKAGE_VERSION)\n" + "set(PACKAGE_VERSION_EXACT TRUE)\nset(PACKAGE_VERSION_COMPATIBLE TRUE)\nendif()\n" + ) return tuple(message.name for message in messages) diff --git a/dimos/message_codegen/native_build.py b/dimos/message_codegen/native_build.py index c374215bb75..3a3644f1c73 100644 --- a/dimos/message_codegen/native_build.py +++ b/dimos/message_codegen/native_build.py @@ -18,6 +18,7 @@ import os from pathlib import Path +import shutil import subprocess import sys from typing import Any @@ -62,3 +63,7 @@ def build_extension(self, extension: Any) -> None: check=True, ) subprocess.run(["cmake", "--build", str(build), "--parallel", "2"], check=True) + stubs = destination / f"{extension.name}-stubs" + if stubs.exists(): + shutil.rmtree(stubs) + shutil.copytree(extension.cmake_source_dir.parent / "typing" / extension.name, stubs) diff --git a/dimos/message_codegen/ownership.py b/dimos/message_codegen/ownership.py new file mode 100644 index 00000000000..3c78bd2c887 --- /dev/null +++ b/dimos/message_codegen/ownership.py @@ -0,0 +1,97 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Explicit ownership contracts for separately compiled message packages.""" + +from __future__ import annotations + +from dataclasses import dataclass +from hashlib import sha256 +import json +from pathlib import Path +import re + +from .definitions import Definitions, Message + +ABI = "dimos-cdr-pybind11-3.0.1-fastcdr-2.4.0-v1" + + +@dataclass(frozen=True) +class Dependency: + module: str + version: str + root: Path + owned: tuple[str, ...] + schemas: dict[str, str] + codec_owner: str = "" + + @property + def crate(self) -> str: + return self.module + "_messages" + + @classmethod + def load(cls, root: Path) -> Dependency: + data = json.loads((root / "message-package.json").read_text()) + if data.get("format") != 1 or data.get("abi") != ABI or not data.get("shared"): + raise ValueError(f"Incompatible message package ABI: {root}") + module, version = data["module"], data["version"] + if not re.fullmatch(r"[a-z][a-z0-9_]*", module): + raise ValueError(f"Invalid dependency module: {module}") + if not re.fullmatch(r"[0-9]+\.[0-9]+\.[0-9]+", version): + raise ValueError(f"Invalid dependency version: {version}") + return cls( + module, + version, + root.resolve(), + tuple(data["owned"]), + data["schemas"], + data.get("codec_owner", module), + ) + + +def schema_hash(schema: str) -> str: + return sha256(schema.encode()).hexdigest() + + +def resolve_owners( + messages: tuple[Message, ...], definitions: Definitions, dependencies: tuple[Dependency, ...] +) -> dict[str, Dependency]: + owners: dict[str, Dependency] = {} + modules: dict[str, str] = {} + for dependency in dependencies: + previous = modules.setdefault(dependency.module, dependency.version) + if previous != dependency.version: + raise ValueError(f"Conflicting package versions for {dependency.module}") + for name in dependency.owned: + if name in owners: + raise ValueError(f"Multiple owners for message {name}") + owners[name] = dependency + codec_owners = {dep.codec_owner or dep.module for dep in dependencies} + if len(codec_owners) > 1: + raise ValueError(f"Incompatible Rust codec owners: {sorted(codec_owners)}") + if codec_owners - modules.keys(): + raise ValueError(f"Missing codec owner package: {sorted(codec_owners - modules.keys())}") + for dependency in dependencies: + missing = set(dependency.schemas) - owners.keys() + if missing: + raise ValueError( + f"Missing transitive message owners for {dependency.module}: {sorted(missing)}" + ) + for message in messages: + owner = owners.get(message.name) + if owner is not None and owner.schemas.get(message.name) != schema_hash( + definitions.schema(message.name) + ): + raise ValueError(f"Dependency schema mismatch: {message.name} from {owner.module}") + return {message.name: owners[message.name] for message in messages if message.name in owners} diff --git a/dimos/message_codegen/project.py b/dimos/message_codegen/project.py new file mode 100644 index 00000000000..0b3dabf36dc --- /dev/null +++ b/dimos/message_codegen/project.py @@ -0,0 +1,242 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Project configuration and deterministic generation shared by every build frontend.""" + +from __future__ import annotations + +from dataclasses import dataclass +from hashlib import sha256 +from importlib.util import find_spec +import json +from pathlib import Path +import re +import shutil +import sys +from typing import Any + +if sys.version_info >= (3, 11): + import tomllib +else: + import tomli as tomllib + +from .definitions import Definitions, parse_message +from .generate import generate +from .ownership import ABI, Dependency + +BUILD_VERSION = "0.1.0" +LANGUAGES = frozenset(("python", "cpp", "rust")) + + +@dataclass(frozen=True) +class Project: + root: Path + name: str + version: str + module: str + source: Path + languages: tuple[str, ...] + dependency_specs: dict[str, dict[str, str]] + internal: bool = False + + @classmethod + def load(cls, root: Path) -> Project: + root = root.resolve() + data = tomllib.loads((root / "pyproject.toml").read_text()) + project = data.get("project", {}) + config = data.get("tool", {}).get("dimos", {}).get("messages", {}) + name, version = project.get("name"), project.get("version") + if not isinstance(name, str) or not re.fullmatch(r"[a-z][a-z0-9_-]*", name): + raise ValueError("[project].name must be a lowercase package name") + if not isinstance(version, str) or not re.fullmatch(r"[0-9]+\.[0-9]+\.[0-9]+", version): + raise ValueError("[project].version must be an explicit major.minor.patch version") + allowed = {"source-dir", "python-module", "languages", "dependencies", "internal"} + unknown = config.keys() - allowed + if unknown: + raise ValueError(f"Unknown message configuration: {sorted(unknown)}") + module = config.get("python-module", name.replace("-", "_")) + if not isinstance(module, str) or not re.fullmatch(r"[a-z][a-z0-9_]*", module): + raise ValueError("Invalid python-module") + if module != name.replace("-", "_"): + raise ValueError("python-module must match the normalized project name") + source = (root / config.get("source-dir", "interfaces")).resolve() + if not source.is_relative_to(root): + raise ValueError("Message source-dir must be inside the project") + languages = config.get("languages", ["python"]) + if ( + not isinstance(languages, list) + or not languages + or any(value not in LANGUAGES for value in languages) + ): + raise ValueError("languages must select python, cpp and/or rust") + internal = config.get("internal", False) + if not isinstance(internal, bool): + raise ValueError("internal must be a boolean") + specs = config.get( + "dependencies", {} if internal else {"dimos_generated": {"version": BUILD_VERSION}} + ) + if not isinstance(specs, dict): + raise ValueError("dependencies must map Python modules to exact versions") + for dep_module, spec in specs.items(): + if not re.fullmatch(r"[a-z][a-z0-9_]*", dep_module) or not isinstance(spec, dict): + raise ValueError("Invalid message dependency") + if spec.keys() - {"version", "path"} or not re.fullmatch( + r"[0-9]+\.[0-9]+\.[0-9]+", str(spec.get("version", "")) + ): + raise ValueError(f"Dependency {dep_module} requires an exact version") + if "path" in spec and not isinstance(spec["path"], str): + raise ValueError(f"Dependency path must be a string: {dep_module}") + return cls( + root, name, version, module, source, tuple(dict.fromkeys(languages)), specs, internal + ) + + @property + def output(self) -> Path: + return self.root / "build" / "dimos" + + def requirements(self) -> list[str]: + return [ + f"{module.replace('_', '-')}=={spec['version']}" + for module, spec in sorted(self.dependency_specs.items()) + if "path" not in spec + ] + + def dependencies(self) -> tuple[Dependency, ...]: + result: dict[str, Dependency] = {} + pending = dict(self.dependency_specs) + while pending: + module = sorted(pending)[0] + spec = pending.pop(module) + if module in result: + if result[module].version != spec["version"]: + raise ValueError(f"Conflicting package versions for {module}") + continue + if "path" in spec: + root = (self.root / spec["path"]).resolve() + else: + found = find_spec(module + "_schemas") + if found is None or found.origin is None: + raise ValueError( + f"Install message dependency {module.replace('_', '-')}=={spec['version']} before building (or supply it to pip's build environment)" + ) + root = Path(found.origin).parent / "package" + dep = Dependency.load(root) + if dep.module != module or dep.version != spec["version"]: + raise ValueError( + f"Dependency identity/version mismatch: {module}=={spec['version']}" + ) + if module == self.module: + raise ValueError("A message package cannot depend on itself") + result[module] = dep + metadata = json.loads((root / "message-package.json").read_text()) + for child, version in sorted(metadata.get("dependencies", {}).items()): + expected = {"version": version} + existing = pending.get(child, self.dependency_specs.get(child, expected)) + if existing["version"] != version: + raise ValueError(f"Conflicting package versions for {child}") + pending[child] = existing + return tuple(result[module] for module in sorted(result)) + + +def prepare(project: Project) -> Path: + """Generate a complete owned package atomically; never invoke compilers or networks.""" + dependencies = project.dependencies() + paths = sorted(project.source.rglob("*.msg")) if project.source.is_dir() else [] + if not paths and not project.internal: + raise ValueError(f"No messages found under {project.source}; expected package/msg/Type.msg") + names = [parse_message(path).name for path in paths] + roots = [project.source] if paths else [] + definitions = Definitions(roots + [dep.root / "schemas" for dep in dependencies]) + resolved = definitions.resolve(names if not project.internal else None) + imported = {name for dep in dependencies for name in dep.owned} + unexpected = {message.name for message in resolved} - set(names) - imported + if unexpected and not project.internal: + raise ValueError(f"Missing dependency owners: {sorted(unexpected)}") + if set(names) & imported: + raise ValueError( + f"Local definitions duplicate dependency ownership: {sorted(set(names) & imported)}" + ) + toolkit = Path(__file__).parent + toolkit_hash = sha256() + toolkit_paths = ( + list(toolkit.glob("*.py")) + + list((toolkit / "_vendor").glob("*.py")) + + list((toolkit / "templates").glob("*")) + ) + for path in sorted(toolkit_paths): + if ( + path.is_file() + and (path.suffix in {".py", ".hpp", ".rs"}) + and "__pycache__" not in path.parts + ): + toolkit_hash.update(path.relative_to(toolkit).as_posix().encode()) + toolkit_hash.update(path.read_bytes()) + inputs: dict[str, Any] = { + "abi": ABI, + "toolkit": toolkit_hash.hexdigest(), + "module": project.module, + "version": project.version, + "names": names, + "internal": project.internal, + "schemas": {message.name: definitions.schema(message.name) for message in resolved}, + "dependencies": [ + { + "module": dep.module, + "version": dep.version, + "schemas": dep.schemas, + "root": str(dep.root), + } + for dep in dependencies + ], + } + digest = sha256(json.dumps(inputs, sort_keys=True).encode()).hexdigest() + output = project.output + state = output / "generation.json" + if state.is_file(): + previous = json.loads(state.read_text()) + if previous.get("digest") == digest and all( + (output / path).is_file() + and sha256((output / path).read_bytes()).hexdigest() == expected + for path, expected in previous.get("files", {}).items() + ): + return output + staging = output.with_name("dimos-generating") + if staging.exists(): + shutil.rmtree(staging) + generate( + roots, + staging, + names if not project.internal else None, + project.module, + version=project.version, + dependencies=dependencies, + shared=True, + ) + (staging / "toolchain.cmake").write_text( + 'list(PREPEND CMAKE_PREFIX_PATH "${CMAKE_CURRENT_LIST_DIR}"' + + "".join(" " + json.dumps(str(dep.root)) for dep in dependencies) + + ")\n" + ) + files = { + path.relative_to(staging).as_posix(): sha256(path.read_bytes()).hexdigest() + for path in sorted(staging.rglob("*")) + if path.is_file() + } + (staging / "generation.json").write_text( + json.dumps({"digest": digest, "files": files}, indent=2, sort_keys=True) + "\n" + ) + if output.exists(): + shutil.rmtree(output) + staging.rename(output) + return output diff --git a/dimos/message_codegen/pyproject.toml b/dimos/message_codegen/pyproject.toml new file mode 100644 index 00000000000..a3da3a374ef --- /dev/null +++ b/dimos/message_codegen/pyproject.toml @@ -0,0 +1,18 @@ +[build-system] +requires = ["setuptools>=70", "wheel"] +build-backend = "setuptools.build_meta" + +[project] +name = "dimos-message-build" +version = "0.1.0" +description = "Standalone DimOS message build backend" +requires-python = ">=3.10" +dependencies = ["setuptools>=70", "wheel", "pybind11==3.0.1", "tomli>=2; python_version < '3.11'"] + +[tool.setuptools] +packages = ["dimos_message_build", "dimos_message_build._vendor"] +package-dir = {"dimos_message_build" = "."} +include-package-data = false + +[tool.setuptools.package-data] +"dimos_message_build" = ["schemas/**/*", "templates/*", "sources.json"] diff --git a/dimos/message_codegen/python.py b/dimos/message_codegen/python.py index 4a653aa64e3..9fc32d24827 100644 --- a/dimos/message_codegen/python.py +++ b/dimos/message_codegen/python.py @@ -18,8 +18,9 @@ import json -from dimos.message_codegen import cpp -from dimos.message_codegen.definitions import Definitions, Message +from . import cpp +from .definitions import Definitions, Message +from .ownership import ABI def string_literal(value: str) -> str: @@ -30,7 +31,15 @@ def string_literal(value: str) -> str: def generate( - messages: tuple[Message, ...], definitions: Definitions, module: str + messages: tuple[Message, ...], + definitions: Definitions, + module: str, + *, + imports: tuple[str, ...] = (), + shared: bool = False, + version: str = "0.1.0", + imported: dict[str, str] | None = None, + dependency_versions: dict[str, str] | None = None, ) -> dict[str, str]: sources: dict[str, str] = {} containers = sorted( @@ -49,6 +58,7 @@ def generate( lines = [ "// Generated from ROS2 .msg definitions. Do not edit.", "#include ", + "#include ", "#include ", '#include "messages.hpp"', '#include "dimos_python.hpp"', @@ -62,7 +72,8 @@ def generate( "{", f'auto package = py::hasattr(root, "{message.package}") ? root.attr("{message.package}").cast() : root.def_submodule("{message.package}");', 'auto module = py::hasattr(package, "msg") ? package.attr("msg").cast() : package.def_submodule("msg");', - f'auto cls = py::class_<{native}>(module, "{message.short_name}", py::dynamic_attr(), py::module_local());', + f'auto cls = py::class_<{native}>(module, "{message.short_name}", py::dynamic_attr(), py::module_local({str(not shared).lower()}));', + "cls.def(py::self == py::self).def(py::self != py::self);", f"cls.def(py::init([](py::kwargs kwargs) {{ {native} value{{}};", "for (auto item : kwargs) { auto key = py::cast(item.first);", ] @@ -114,6 +125,16 @@ def generate( ) lines.append("}") sources[function + ".cpp"] = "\n".join(lines) + "\n" + checks = [] + for dependency, expected in sorted((dependency_versions or {}).items()): + checks.append( + f'{{ auto dep = pybind11::module_::import("{dependency}"); if (pybind11::str(dep.attr("__dimos_version__")).cast() != {json.dumps(expected)} || pybind11::str(dep.attr("__dimos_abi__")).cast() != {json.dumps(ABI)}) throw pybind11::import_error("Incompatible message dependency: {dependency}"); }}' + ) + for name, dependency in sorted((imported or {}).items()): + package, _, short = name.split("/") + checks.append( + f'if (pybind11::str(pybind11::module_::import("{dependency}").attr("{package}").attr("msg").attr("{short}").attr("schema")).cast() != {string_literal(definitions.schema(name))}) throw pybind11::import_error("Dependency schema mismatch: {name}");' + ) sources["bindings.cpp"] = ( "\n".join( [ @@ -121,6 +142,10 @@ def generate( "#include ", *(f"void {function}(pybind11::module_&);" for function in functions), f"PYBIND11_MODULE({module}, root) {{", + *(f'pybind11::module_::import("{dependency}");' for dependency in imports), + *checks, + f'root.attr("__dimos_version__") = {json.dumps(version)};', + f'root.attr("__dimos_abi__") = {json.dumps(ABI)};', *(f"{function}(root);" for function in functions), "}", ] diff --git a/dimos/message_codegen/registry.py b/dimos/message_codegen/registry.py index b1f3b87b4ab..4c07bf65eab 100644 --- a/dimos/message_codegen/registry.py +++ b/dimos/message_codegen/registry.py @@ -16,8 +16,8 @@ from typing import Any -from dimos.message_codegen.definitions import Definitions -from dimos.message_codegen.providers import providers, schema_roots +from .definitions import Definitions +from .providers import providers, schema_roots def message_types() -> dict[str, Any]: diff --git a/dimos/message_codegen/rust.py b/dimos/message_codegen/rust.py index 7c237d3861b..ba7b2e9562b 100644 --- a/dimos/message_codegen/rust.py +++ b/dimos/message_codegen/rust.py @@ -22,7 +22,7 @@ import re from typing import Any -from dimos.message_codegen.definitions import Definitions, FieldType, Message +from .definitions import Definitions, FieldType, Message PRIMITIVES = { "bool": "bool", @@ -135,13 +135,24 @@ def literal(value: Any, type_: FieldType) -> str: return str(value) -def generate(messages: tuple[Message, ...], definitions: Definitions) -> str: +def generate( + messages: tuple[Message, ...], definitions: Definitions, imports: dict[str, str] | None = None +) -> str: + imports = imports or {} packages: dict[str, list[Message]] = defaultdict(list) for message in messages: packages[message.package].append(message) lines = ["// Generated from ROS2 .msg definitions. Do not edit.", "pub mod codec;"] + for name in imports: + packages.setdefault(name.split("/")[0], []) for package, contents in sorted(packages.items()): lines.append(f"pub mod {identifier(package)} {{ pub mod msg {{") + for qualified, owner in sorted(imports.items()): + imported_package, _, imported_name = qualified.split("/") + if imported_package == package: + lines.append( + f"pub use {owner}::{identifier(package)}::msg::{identifier(imported_name)};" + ) for message in contents: name = identifier(message.short_name) lines.extend( @@ -159,7 +170,12 @@ def generate(messages: tuple[Message, ...], definitions: Definitions) -> str: if not message.fields: lines.extend(["#[serde(default)]", "_unused: u8,"]) lines.extend( - ["}", f"impl ::std::default::Default for {name} {{ fn default() -> Self {{ Self {{"] + [ + "}", + "// Explicit defaults also support .msg defaults and arrays longer than 32.", + "#[allow(clippy::derivable_impls)]", + f"impl ::std::default::Default for {name} {{ fn default() -> Self {{ Self {{", + ] ) for field in message.fields: if field.default is not None: @@ -193,7 +209,11 @@ def generate(messages: tuple[Message, ...], definitions: Definitions) -> str: f'if {value}.len() > {field.type.string_bound} {{ return Err("{field.name} exceeds string bound".into()); }}' ) if field.type.nested: - checks.append(f"crate::codec::Message::validate(&{value})?;") + field_owner = imports.get(field.type.name) + trait = ( + f"{field_owner}::codec::Message" if field_owner else "crate::codec::Message" + ) + checks.append(f"{trait}::validate(&{value})?;") if checks and field.type.is_array: lines.append( f"for item in &{member} {{ {' '.join(checks).replace('&item', 'item')} }}" diff --git a/dimos/message_codegen/schemas/dimos_msgs/LICENSE b/dimos/message_codegen/schemas/dimos_msgs/LICENSE new file mode 100644 index 00000000000..5e2927e3adb --- /dev/null +++ b/dimos/message_codegen/schemas/dimos_msgs/LICENSE @@ -0,0 +1,17 @@ + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + Copyright 2025 Dimensional Inc. + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. diff --git a/dimos/message_codegen/schemas/dimos_msgs/msg/BoundingBox2DArray.msg b/dimos/message_codegen/schemas/dimos_msgs/msg/BoundingBox2DArray.msg new file mode 100644 index 00000000000..7360bdbaec4 --- /dev/null +++ b/dimos/message_codegen/schemas/dimos_msgs/msg/BoundingBox2DArray.msg @@ -0,0 +1,5 @@ +# Copyright 2026 Dimensional Inc. +# SPDX-License-Identifier: Apache-2.0 + +std_msgs/Header header +vision_msgs/BoundingBox2D[] boxes diff --git a/dimos/message_codegen/schemas/dimos_msgs/msg/BoundingBox3DArray.msg b/dimos/message_codegen/schemas/dimos_msgs/msg/BoundingBox3DArray.msg new file mode 100644 index 00000000000..3bb3649298e --- /dev/null +++ b/dimos/message_codegen/schemas/dimos_msgs/msg/BoundingBox3DArray.msg @@ -0,0 +1,5 @@ +# Copyright 2026 Dimensional Inc. +# SPDX-License-Identifier: Apache-2.0 + +std_msgs/Header header +vision_msgs/BoundingBox3D[] boxes diff --git a/dimos/message_codegen/schemas/dimos_msgs/msg/EntityMarker.msg b/dimos/message_codegen/schemas/dimos_msgs/msg/EntityMarker.msg new file mode 100644 index 00000000000..c4eb6dc7625 --- /dev/null +++ b/dimos/message_codegen/schemas/dimos_msgs/msg/EntityMarker.msg @@ -0,0 +1,8 @@ +# Copyright 2026 Dimensional Inc. +# SPDX-License-Identifier: Apache-2.0 + +# A labeled world-space entity; rendering belongs to viewer adapters. +string entity_id +string label +string entity_type +geometry_msgs/Point position diff --git a/dimos/message_codegen/schemas/dimos_msgs/msg/EntityMarkers.msg b/dimos/message_codegen/schemas/dimos_msgs/msg/EntityMarkers.msg new file mode 100644 index 00000000000..a4ebd89ced5 --- /dev/null +++ b/dimos/message_codegen/schemas/dimos_msgs/msg/EntityMarkers.msg @@ -0,0 +1,5 @@ +# Copyright 2026 Dimensional Inc. +# SPDX-License-Identifier: Apache-2.0 + +std_msgs/Header header +dimos_msgs/EntityMarker[] markers diff --git a/dimos/message_codegen/schemas/dimos_msgs/msg/GraspCandidate.msg b/dimos/message_codegen/schemas/dimos_msgs/msg/GraspCandidate.msg new file mode 100644 index 00000000000..480cb94c313 --- /dev/null +++ b/dimos/message_codegen/schemas/dimos_msgs/msg/GraspCandidate.msg @@ -0,0 +1,6 @@ +# Copyright 2026 Dimensional Inc. +# SPDX-License-Identifier: Apache-2.0 + +# Target TCP pose and generator-local ranking score. +geometry_msgs/Pose pose +float64 score diff --git a/dimos/message_codegen/schemas/dimos_msgs/msg/GraspCandidateArray.msg b/dimos/message_codegen/schemas/dimos_msgs/msg/GraspCandidateArray.msg new file mode 100644 index 00000000000..5d0beb21198 --- /dev/null +++ b/dimos/message_codegen/schemas/dimos_msgs/msg/GraspCandidateArray.msg @@ -0,0 +1,6 @@ +# Copyright 2026 Dimensional Inc. +# SPDX-License-Identifier: Apache-2.0 + +# Candidates share the input cloud's frame and timestamp. +std_msgs/Header header +GraspCandidate[] candidates diff --git a/dimos/message_codegen/schemas/dimos_msgs/msg/ImuInfo.msg b/dimos/message_codegen/schemas/dimos_msgs/msg/ImuInfo.msg new file mode 100644 index 00000000000..8a2ed049200 --- /dev/null +++ b/dimos/message_codegen/schemas/dimos_msgs/msg/ImuInfo.msg @@ -0,0 +1,15 @@ +# Copyright 2026 Dimensional Inc. +# SPDX-License-Identifier: Apache-2.0 + +# Continuous-time IMU noise model. Extrinsics are supplied separately through TF. +std_msgs/Header header +# rad/s/sqrt(Hz) +float64 gyro_noise_density +# rad/s^2/sqrt(Hz) +float64 gyro_random_walk +# m/s^2/sqrt(Hz) +float64 accel_noise_density +# m/s^3/sqrt(Hz) +float64 accel_random_walk +# Delivered sample rate in Hz. +float64 frequency diff --git a/dimos/message_codegen/schemas/dimos_msgs/msg/JointCommand.msg b/dimos/message_codegen/schemas/dimos_msgs/msg/JointCommand.msg new file mode 100644 index 00000000000..8f5d1b68447 --- /dev/null +++ b/dimos/message_codegen/schemas/dimos_msgs/msg/JointCommand.msg @@ -0,0 +1,6 @@ +# Copyright 2026 Dimensional Inc. +# SPDX-License-Identifier: Apache-2.0 + +# Joint position targets in radians, ordered by the controller's joint list. +std_msgs/Header header +float64[] positions diff --git a/dimos/message_codegen/schemas/dimos_msgs/msg/LineSegment3D.msg b/dimos/message_codegen/schemas/dimos_msgs/msg/LineSegment3D.msg new file mode 100644 index 00000000000..f026c5eef45 --- /dev/null +++ b/dimos/message_codegen/schemas/dimos_msgs/msg/LineSegment3D.msg @@ -0,0 +1,6 @@ +# Copyright 2026 Dimensional Inc. +# SPDX-License-Identifier: Apache-2.0 + +geometry_msgs/Point start +geometry_msgs/Point end +float64 weight 1.0 diff --git a/dimos/message_codegen/schemas/dimos_msgs/msg/LineSegments3D.msg b/dimos/message_codegen/schemas/dimos_msgs/msg/LineSegments3D.msg new file mode 100644 index 00000000000..92947baed42 --- /dev/null +++ b/dimos/message_codegen/schemas/dimos_msgs/msg/LineSegments3D.msg @@ -0,0 +1,6 @@ +# Copyright 2026 Dimensional Inc. +# SPDX-License-Identifier: Apache-2.0 + +# Weighted segments in a common coordinate frame. +std_msgs/Header header +LineSegment3D[] segments diff --git a/dimos/message_codegen/schemas/dimos_msgs/msg/MotorCommandArray.msg b/dimos/message_codegen/schemas/dimos_msgs/msg/MotorCommandArray.msg new file mode 100644 index 00000000000..e98e7ae9e8c --- /dev/null +++ b/dimos/message_codegen/schemas/dimos_msgs/msg/MotorCommandArray.msg @@ -0,0 +1,11 @@ +# Copyright 2026 Dimensional Inc. +# SPDX-License-Identifier: Apache-2.0 + +# Per-joint hybrid commands. All arrays must have the same length. +# q: rad; dq: rad/s; tau: N*m. Gains follow the actuator controller's units. +std_msgs/Header header +float64[] q +float64[] dq +float64[] kp +float64[] kd +float64[] tau diff --git a/dimos/message_codegen/schemas/dimos_msgs/msg/RobotState.msg b/dimos/message_codegen/schemas/dimos_msgs/msg/RobotState.msg new file mode 100644 index 00000000000..36b399f85c8 --- /dev/null +++ b/dimos/message_codegen/schemas/dimos_msgs/msg/RobotState.msg @@ -0,0 +1,15 @@ +# Copyright 2026 Dimensional Inc. +# SPDX-License-Identifier: Apache-2.0 + +# Controller feedback. TCP values retain the driver's x/y/z/roll/pitch/yaw convention. +std_msgs/Header header +int32 state +int32 mode +int32 error_code +int32 warn_code +int32 cmdnum +int32 mt_brake +int32 mt_able +float64[] tcp_pose +float64[] tcp_offset +float64[] joints diff --git a/dimos/message_codegen/schemas/dimos_msgs/msg/TrajectoryStatus.msg b/dimos/message_codegen/schemas/dimos_msgs/msg/TrajectoryStatus.msg new file mode 100644 index 00000000000..4baa51c0431 --- /dev/null +++ b/dimos/message_codegen/schemas/dimos_msgs/msg/TrajectoryStatus.msg @@ -0,0 +1,15 @@ +# Copyright 2026 Dimensional Inc. +# SPDX-License-Identifier: Apache-2.0 + +std_msgs/Header header +uint8 IDLE=0 +uint8 EXECUTING=1 +uint8 COMPLETED=2 +uint8 ABORTED=3 +uint8 FAULT=4 +uint8 state +# Fraction completed, in [0, 1]. +float64 progress +builtin_interfaces/Duration time_elapsed +builtin_interfaces/Duration time_remaining +string error diff --git a/dimos/message_codegen/schemas/dimos_msgs/msg/VideoStats.msg b/dimos/message_codegen/schemas/dimos_msgs/msg/VideoStats.msg new file mode 100644 index 00000000000..3472623a7a7 --- /dev/null +++ b/dimos/message_codegen/schemas/dimos_msgs/msg/VideoStats.msg @@ -0,0 +1,11 @@ +std_msgs/Header header +float64 fps +float64 kbps +uint32 width +uint32 height +float64 loss_pct +float64 jitter_buffer_ms +float64 decode_ms +uint64 frames_dropped +uint64 freezes +float64 e2e_latency_ms diff --git a/dimos/message_codegen/src/lib.rs b/dimos/message_codegen/src/lib.rs new file mode 100644 index 00000000000..65ad2af6781 --- /dev/null +++ b/dimos/message_codegen/src/lib.rs @@ -0,0 +1,21 @@ +// Copyright 2026 Dimensional Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// Copyright 2026 Dimensional Inc. +// SPDX-License-Identifier: Apache-2.0 + +//! Standard and DimOS messages generated from the bundled, pinned ROS2 definitions. +//! Python is needed only while building this source crate; runtime codecs are native Rust. + +include!(concat!(env!("OUT_DIR"), "/messages.rs")); diff --git a/dimos/message_codegen/stubs.py b/dimos/message_codegen/stubs.py new file mode 100644 index 00000000000..189731b8aef --- /dev/null +++ b/dimos/message_codegen/stubs.py @@ -0,0 +1,148 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Emit static Python interfaces from the same definitions as the native bindings.""" + +from collections import defaultdict + +from . import cpp +from .definitions import FieldType, Message + +NUMPY_TYPES = { + "byte": "uint8", + "char": "uint8", + "int8": "int8", + "uint8": "uint8", + "int16": "int16", + "uint16": "uint16", + "int32": "int32", + "uint32": "uint32", + "int64": "int64", + "uint64": "uint64", + "float32": "float32", + "float64": "float64", +} + + +def scalar_type(name: str, module: str) -> str: + if "/" in name: + return f"{module}.{name.replace('/', '.')}" + if name in ("string", "wstring"): + return "str" + if name == "bool": + return "bool" + return "float" if name.startswith("float") else "int" + + +def input_type(field: FieldType, module: str) -> str: + scalar = scalar_type(field.name, module) + if not field.is_array: + return scalar + result = f"Iterable[{scalar}]" + if field.name in NUMPY_TYPES: + result += " | NDArray[Any]" + return result + + +def generate(messages: tuple[Message, ...], module: str) -> dict[str, str]: + containers = { + cpp.type_name(field.type): field.type + for message in messages + for field in message.fields + if field.type.is_array + } + names = {native: f"_Sequence{index}" for index, native in enumerate(sorted(containers))} + packages: dict[str, list[Message]] = defaultdict(list) + for message in messages: + packages[message.package].append(message) + imports = [ + "from collections.abc import Iterable, Iterator", + "from typing import Any, ClassVar", + "import numpy as np", + "from numpy.typing import NDArray", + f"import {module}", + *[f"import {module}.{package}.msg" for package in sorted(packages)], + ] + root = imports + [f"from . import {package} as {package}" for package in sorted(packages)] + for native in sorted(containers): + field = containers[native] + scalar = scalar_type(field.name, module) + root.extend( + [ + f"class {names[native]}:", + " def __len__(self) -> int: ...", + f" def __getitem__(self, index: int) -> {scalar}: ...", + f" def __setitem__(self, index: int, value: {scalar}) -> None: ...", + f" def __iter__(self) -> Iterator[{scalar}]: ...", + ] + ) + if field.sequence: + root.extend( + [ + f" def append(self, value: {scalar}) -> None: ...", + f" def extend(self, values: {input_type(field, module)}) -> None: ...", + " def clear(self) -> None: ...", + ] + ) + if field.name in NUMPY_TYPES: + array = f"NDArray[np.{NUMPY_TYPES[field.name]}]" + root.extend( + [ + f" def view(self) -> {array}: ...", + f" def copy(self) -> {array}: ...", + ] + ) + output = {"__init__.pyi": "\n".join(root) + "\n", "py.typed": ""} + for package, types in sorted(packages.items()): + output[f"{package}/__init__.pyi"] = "from . import msg as msg\n" + lines = imports.copy() + for message in types: + lines.extend( + [ + f"class {message.short_name}:", + " msg_name: ClassVar[str]", + " schema: ClassVar[str]", + ] + ) + for constant in message.constants: + lines.append(f" {constant.name}: ClassVar[{scalar_type(constant.type, module)}]") + arguments = ", ".join( + f"{field.name}: {input_type(field.type, module)} = ..." for field in message.fields + ) + args = f", *, {arguments}" if arguments else "" + lines.append(f" def __init__(self{args}) -> None: ...") + for message_field in message.fields: + if message_field.type.is_array: + sequence = names[cpp.type_name(message_field.type)] + lines.extend( + [ + " @property", + f" def {message_field.name}(self) -> {module}.{sequence}: ...", + f" @{message_field.name}.setter", + f" def {message_field.name}(self, value: {input_type(message_field.type, module)}) -> None: ...", + ] + ) + else: + lines.append( + f" {message_field.name}: {scalar_type(message_field.type.name, module)}" + ) + lines.extend( + [ + " def encode(self, little_endian: bool = True) -> bytes: ...", + " @staticmethod", + f" def decode(data: bytes) -> {message.short_name}: ...", + ] + ) + output[f"{package}/msg/__init__.pyi"] = "\n".join(lines) + "\n" + return output diff --git a/dimos/message_codegen/templates/codec.rs b/dimos/message_codegen/templates/codec.rs index 7507889426f..4e538b7f508 100644 --- a/dimos/message_codegen/templates/codec.rs +++ b/dimos/message_codegen/templates/codec.rs @@ -12,6 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. +use std::io; + use re_cdr::{BigEndian, LittleEndian}; use serde::{de::DeserializeOwned, Serialize}; @@ -21,36 +23,36 @@ pub trait Message: Serialize + DeserializeOwned { fn validate(&self) -> Result<(), String>; - fn encode(&self) -> Result, String> { + fn encode(&self) -> io::Result> { self.encode_endian(true) } - fn encode_endian(&self, little_endian: bool) -> Result, String> { - self.validate()?; + fn encode_endian(&self, little_endian: bool) -> io::Result> { + self.validate().map_err(|error| io::Error::new(io::ErrorKind::InvalidInput, error))?; let body = if little_endian { re_cdr::to_vec::<_, LittleEndian>(self) } else { re_cdr::to_vec::<_, BigEndian>(self) - }.map_err(|error| error.to_string())?; + }.map_err(|error| io::Error::new(io::ErrorKind::InvalidInput, error.to_string()))?; let mut bytes = Vec::with_capacity(4 + body.len()); bytes.extend_from_slice(&[0, u8::from(little_endian), 0, 0]); bytes.extend(body); Ok(bytes) } - fn decode(bytes: &[u8]) -> Result { + fn decode(bytes: &[u8]) -> io::Result { if bytes.len() < 4 || bytes[0] != 0 || bytes[1] > 1 || bytes[2] != 0 || bytes[3] != 0 { - return Err("Expected plain CDR/XCDR1 encapsulation".into()); + return Err(io::Error::new(io::ErrorKind::InvalidData, "Expected plain CDR/XCDR1 encapsulation")); } let (value, consumed): (Self, usize) = if bytes[1] == 1 { re_cdr::from_bytes::(&bytes[4..]) } else { re_cdr::from_bytes::(&bytes[4..]) - }.map_err(|error| error.to_string())?; + }.map_err(|error| io::Error::new(io::ErrorKind::InvalidData, error.to_string()))?; if consumed != bytes.len() - 4 { - return Err("Trailing bytes after CDR message".into()); + return Err(io::Error::new(io::ErrorKind::InvalidData, "Trailing bytes after CDR message")); } - value.validate()?; + value.validate().map_err(|error| io::Error::new(io::ErrorKind::InvalidData, error))?; Ok(value) } } diff --git a/dimos/message_codegen/test_builtin_artifacts.py b/dimos/message_codegen/test_builtin_artifacts.py new file mode 100644 index 00000000000..0111f9e8825 --- /dev/null +++ b/dimos/message_codegen/test_builtin_artifacts.py @@ -0,0 +1,72 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import json +import os +from pathlib import Path +import subprocess +import tarfile + +import pytest + +from dimos.message_codegen.definitions import Definitions +from dimos.message_codegen.project import Project + + +def test_builtin_native_artifact_version_and_relocated_consumer(tmp_path): + location = os.environ.get("DIMOS_BUILTIN_PACKAGE") + fastcdr = os.environ.get("DIMOS_FASTCDR_PREFIX") + if not location or not fastcdr: + pytest.skip("Set DIMOS_BUILTIN_PACKAGE and DIMOS_FASTCDR_PREFIX for artifact acceptance") + output = Path(location).resolve() + root = Path(__file__).resolve().parents[2] + version = Project.load(root / "packages/dimos-generated").version + metadata = json.loads((output / "message-package.json").read_text()) + assert metadata["version"] == version + assert metadata["shared"] + assert set(metadata["owned"]) == {message.name for message in Definitions([]).resolve()} + assert (output / "dist" / f"dimos-generated-messages-{version}.crate").is_file() + prefix = tmp_path / "prefix" + prefix.mkdir() + with tarfile.open(output / "dist" / f"dimos-messages-cmake-{version}.tar.gz") as archive: + archive.extractall(prefix, filter="data") + source = tmp_path / "app" + source.mkdir() + (source / "CMakeLists.txt").write_text(f"""cmake_minimum_required(VERSION 3.20) +project(external_builtin LANGUAGES CXX) +find_package(dimos_generated {version} EXACT CONFIG REQUIRED) +add_executable(check main.cpp) +target_link_libraries(check PRIVATE dimos_generated::messages) +""") + (source / "main.cpp").write_text("""#include +int main() { + std_msgs::msg::Header h; h.frame_id = "outside-checkout"; h.stamp.sec = 17; + auto result = dimos::cdr::decode(dimos::cdr::encode(h)); + return result.frame_id == h.frame_id && result.stamp.sec == 17 ? 0 : 1; +} +""") + build = tmp_path / "build" + subprocess.run( + [ + "cmake", + "-S", + str(source), + "-B", + str(build), + f"-DCMAKE_PREFIX_PATH={prefix};{Path(fastcdr).resolve()}", + ], + check=True, + ) + subprocess.run(["cmake", "--build", str(build), "--parallel", "2"], check=True) + subprocess.run([str(build / "check")], check=True) diff --git a/dimos/message_codegen/test_documented_project.py b/dimos/message_codegen/test_documented_project.py new file mode 100644 index 00000000000..29cdd3d4cd8 --- /dev/null +++ b/dimos/message_codegen/test_documented_project.py @@ -0,0 +1,35 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from pathlib import Path +import re + + +def test_documented_sources_match_compiled_example(): + root = Path(__file__).resolve().parents[2] + document = (root / "docs/development/messages.md").read_text() + blocks = re.findall(r"\n```[^\n]*\n(.*?)```", document, re.S) + assert len(blocks) >= 8 + for name, snippet in blocks: + source = (root / name).read_text() + if name.endswith((".py", ".cpp", ".rs")): + lines = source.splitlines() + while lines and (lines[0].startswith(("#", "//")) or not lines[0].strip()): + lines.pop(0) + if lines and lines[0].startswith(chr(34) * 3): + lines.pop(0) + while lines and not lines[0].strip(): + lines.pop(0) + source = "\n".join(lines) + "\n" + assert snippet == source, name diff --git a/dimos/message_codegen/test_native.py b/dimos/message_codegen/test_native.py index e1c2da6a49e..42e681e6204 100644 --- a/dimos/message_codegen/test_native.py +++ b/dimos/message_codegen/test_native.py @@ -79,6 +79,19 @@ def test_sequence_and_nested_edits_survive_serialization(): assert decoded.reading.temperature == 37.5 +def test_value_equality_compares_nested_fields_and_arrays(): + first = Telemetry(hops=[1, 2], payload=b"pixels") + second = Telemetry.decode(first.encode()) + + assert first == second + second.reading.temperature += 1 + assert first != second + second.reading.temperature = first.reading.temperature + second.hops[1] = 3 + assert first != second + assert first != Image() + + def test_numpy_views_are_readonly_and_share_native_storage(): value = Image(data=np.arange(16, dtype=np.uint8)) first = value.data.view() diff --git a/dimos/message_codegen/test_ownership.py b/dimos/message_codegen/test_ownership.py new file mode 100644 index 00000000000..e3fe6d57298 --- /dev/null +++ b/dimos/message_codegen/test_ownership.py @@ -0,0 +1,94 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from dataclasses import replace +import json + +import pytest + +from dimos.message_codegen.generate import generate +from dimos.message_codegen.ownership import Dependency + + +def packages(tmp_path): + base = tmp_path / "base" + generate([], base, ["std_msgs/msg/Header"], "base_messages", shared=True) + dependency = Dependency.load(base) + source = tmp_path / "interfaces/custom_msgs/msg/Reading.msg" + source.parent.mkdir(parents=True) + source.write_text("std_msgs/Header header\nfloat64 value\n") + return dependency, source.parent.parent.parent + + +def test_external_package_owns_only_custom_type_and_preserves_schema_closure(tmp_path): + dependency, interfaces = packages(tmp_path) + output = tmp_path / "custom" + generate( + [interfaces], + output, + ["custom_msgs/msg/Reading"], + "custom_messages", + dependencies=(dependency,), + shared=True, + ) + manifest = json.loads((output / "message-package.json").read_text()) + assert manifest["owned"] == ["custom_msgs/msg/Reading"] + assert set(manifest["schemas"]) == { + "custom_msgs/msg/Reading", + "std_msgs/msg/Header", + "builtin_interfaces/msg/Time", + } + assert ( + "MSG: std_msgs/Header" + in json.loads((output / "schemas.json").read_text())["custom_msgs/msg/Reading"] + ) + assert "struct Header" not in (output / "cpp/messages.hpp").read_text() + assert "pub struct Header" not in (output / "rust/src/lib.rs").read_text() + + +def test_dependency_schema_mismatch_fails_before_creating_output(tmp_path): + dependency, interfaces = packages(tmp_path) + changed = replace(dependency, schemas={}) + output = tmp_path / "custom" + with pytest.raises(ValueError, match="Dependency schema mismatch"): + generate([interfaces], output, ["custom_msgs/msg/Reading"], dependencies=(changed,)) + assert not output.exists() + + +def test_duplicate_owner_and_conflicting_package_versions_are_rejected(tmp_path): + dependency, interfaces = packages(tmp_path) + with pytest.raises(ValueError, match="Multiple owners"): + generate( + [interfaces], + tmp_path / "out", + ["custom_msgs/msg/Reading"], + dependencies=(dependency, dependency), + ) + with pytest.raises(ValueError, match="Conflicting package versions"): + generate( + [interfaces], + tmp_path / "out", + ["custom_msgs/msg/Reading"], + dependencies=(dependency, replace(dependency, version="2.0.0")), + ) + + +def test_incompatible_dependency_abi_is_rejected(tmp_path): + dependency, _ = packages(tmp_path) + manifest = dependency.root / "message-package.json" + data = json.loads(manifest.read_text()) + data["abi"] = "unknown" + manifest.write_text(json.dumps(data)) + with pytest.raises(ValueError, match="Incompatible message package ABI"): + Dependency.load(dependency.root) diff --git a/dimos/message_codegen/test_ownership_native.py b/dimos/message_codegen/test_ownership_native.py new file mode 100644 index 00000000000..d56c2ff4044 --- /dev/null +++ b/dimos/message_codegen/test_ownership_native.py @@ -0,0 +1,209 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import json +import os +from pathlib import Path +import subprocess +import sys + +from mcap.reader import make_reader +import pytest +from rosbags.typesys import Stores, get_types_from_msg, get_typestore + +from dimos.message_codegen.generate import generate +from dimos.message_codegen.ownership import Dependency +from dimos.protocol.cdr_mcap import CdrMcapWriter + + +def test_separate_packages_share_types_and_exchange_cdr(tmp_path): + prefix = os.environ.get("DIMOS_FASTCDR_PREFIX") + if not prefix: + pytest.skip("Set DIMOS_FASTCDR_PREFIX to run the compiled ownership contract") + source = tmp_path / "interfaces/custom_msgs/msg/Reading.msg" + source.parent.mkdir(parents=True) + source.write_text("std_msgs/Header header\nfloat64 value\n") + base, custom = tmp_path / "base", tmp_path / "custom" + generate([], base, ["std_msgs/msg/Header"], "ownership_base", shared=True) + generate( + [source.parents[2]], + custom, + ["custom_msgs/msg/Reading"], + "ownership_custom", + dependencies=(Dependency.load(base),), + shared=True, + ) + install = tmp_path / "install" + cmake_prefix = f"{Path(prefix).resolve()};{install}" + for package in (base, custom): + subprocess.run( + [ + "cmake", + "-S", + str(package / "cpp"), + "-B", + str(package / "build"), + "-DCMAKE_BUILD_TYPE=Release", + f"-DCMAKE_PREFIX_PATH={cmake_prefix}", + f"-DCMAKE_INSTALL_PREFIX={install}", + f"-DPython_EXECUTABLE={sys.executable}", + ], + check=True, + ) + subprocess.run(["cmake", "--build", str(package / "build"), "-j", "2"], check=True) + subprocess.run(["cmake", "--install", str(package / "build")], check=True) + env = { + **os.environ, + "PYTHONPATH": os.pathsep.join([str(base / "build"), str(custom / "build")]), + } + python = """ +from ownership_base.std_msgs.msg import Header +from ownership_custom.custom_msgs.msg import Reading +from pathlib import Path +h = Header(frame_id='map') +h.stamp.sec = 17 +h.stamp.nanosec = 123456789 +msg = Reading(header=h, value=20.5) +assert type(msg.header) is Header +msg.header = h +assert type(Reading.decode(msg.encode()).header) is Header +Path('input.cdr').write_bytes(msg.encode()) +""" + subprocess.run([sys.executable, "-c", python], env=env, cwd=tmp_path, check=True) + consumer = tmp_path / "consumer" + consumer.mkdir() + (consumer / "CMakeLists.txt").write_text("""cmake_minimum_required(VERSION 3.20) +project(ownership_consumer LANGUAGES CXX) +find_package(ownership_custom CONFIG REQUIRED) +add_executable(consumer main.cpp) +target_link_libraries(consumer PRIVATE ownership_custom::messages) +""") + (consumer / "main.cpp").write_text("""#include +#include +#include +#include +#include +static_assert(std::is_same_v); +int main() { + std::ifstream in("input.cdr", std::ios::binary); + std::vector bytes((std::istreambuf_iterator(in)), {}); + auto msg = dimos::cdr::decode(bytes); + std_msgs::msg::Header header = msg.header; msg.header = header; + msg.value += 1; + auto out = dimos::cdr::encode(msg); + std::ofstream file("cpp.cdr", std::ios::binary); + file.write(reinterpret_cast(out.data()), out.size()); + return file ? 0 : 1; +} +""") + subprocess.run( + [ + "cmake", + "-S", + str(consumer), + "-B", + str(consumer / "build"), + f"-DCMAKE_PREFIX_PATH={cmake_prefix}", + ], + check=True, + ) + subprocess.run(["cmake", "--build", str(consumer / "build"), "-j", "2"], check=True) + subprocess.run([str(consumer / "build/consumer")], cwd=tmp_path, check=True) + (custom / "rust/src/bin").mkdir() + ( + custom / "rust/src/bin/check.rs" + ).write_text("""use ownership_custom_messages::{codec::Message, custom_msgs::msg::Reading}; +use ownership_base_messages::std_msgs::msg::Header; +fn main() { + let mut msg = Reading::decode(&std::fs::read("cpp.cdr").unwrap()).unwrap(); + let header: Header = msg.header; msg.header = header; + msg.value += 1.0; + std::fs::write("rust.cdr", ownership_base_messages::codec::Message::encode(&msg).unwrap()).unwrap(); +} +""") + subprocess.run( + [ + "cargo", + "run", + "--offline", + "--manifest-path", + str(custom / "rust/Cargo.toml"), + "--bin", + "check", + ], + cwd=tmp_path, + check=True, + ) + subprocess.run( + [ + sys.executable, + "-c", + """ +from ownership_base.std_msgs.msg import Header +from ownership_custom.custom_msgs.msg import Reading +from pathlib import Path +from rosbags.typesys import Stores, get_typestore, get_types_from_msg +msg = Reading.decode(Path('rust.cdr').read_bytes()) +assert type(msg.header) is Header +assert (msg.value, msg.header.frame_id, msg.header.stamp.nanosec) == (22.5, 'map', 123456789) +reference = get_typestore(Stores.ROS2_JAZZY) +reference.register(get_types_from_msg(Reading.schema, Reading.msg_name)) +assert reference.deserialize_cdr(msg.encode(), Reading.msg_name).value == 22.5 +""", + ], + cwd=tmp_path, + env=env, + check=True, + ) + for mutation in ( + "base.__dimos_version__ = '99.0.0'", + "base.std_msgs.msg.Header.schema = 'wrong'", + ): + result = subprocess.run( + [ + sys.executable, + "-c", + f"import ownership_base as base; {mutation}; import ownership_custom", + ], + env=env, + capture_output=True, + text=True, + ) + assert result.returncode != 0 + assert "ImportError" in result.stderr + + # Playback has only the recording: imported dependency schemas must be embedded. + name = "custom_msgs/msg/Reading" + schema = json.loads((custom / "schemas.json").read_text())[name] + recording = tmp_path / "custom.mcap" + with CdrMcapWriter(recording) as writer: + writer.write( + "/reading", + (tmp_path / "rust.cdr").read_bytes(), + schema_name=name, + schema=schema, + log_time_ns=1, + ) + with recording.open("rb") as stream: + [(stored, channel, message)] = list(make_reader(stream).iter_messages()) + assert channel.message_encoding == "cdr" + assert stored.encoding == "ros2msg" + reference = get_typestore(Stores.EMPTY) + reference.register(get_types_from_msg(stored.data.decode(), stored.name)) + decoded = reference.deserialize_cdr(message.data, stored.name) + assert (decoded.value, decoded.header.frame_id, decoded.header.stamp.nanosec) == ( + 22.5, + "map", + 123456789, + ) diff --git a/dimos/message_codegen/test_packages.py b/dimos/message_codegen/test_packages.py index 87a45d868e2..a5367451518 100644 --- a/dimos/message_codegen/test_packages.py +++ b/dimos/message_codegen/test_packages.py @@ -12,6 +12,7 @@ # See the License for the specific language governing permissions and # limitations under the License. +from pathlib import Path import subprocess import sys from types import ModuleType @@ -111,3 +112,15 @@ def test_registry_rejects_conflicting_installed_definitions(tmp_path, monkeypatc with pytest.raises(ValueError, match="Conflicting definition geometry_msgs/msg/Point"): registry.message_types() + + +def test_cargo_generator_runs_as_isolated_namespace_package(tmp_path): + source = Path(__file__).parent / "build_rust.py" + output = tmp_path / "rust" + subprocess.run( + [sys.executable, "-I", "-B", str(source), str(output)], + check=True, + cwd=tmp_path, + ) + assert "struct Image" in (output / "messages.rs").read_text() + assert (output / "codec.rs").read_bytes() == (source.parent / "templates/codec.rs").read_bytes() diff --git a/dimos/message_codegen/test_project.py b/dimos/message_codegen/test_project.py new file mode 100644 index 00000000000..ca6c3469ffc --- /dev/null +++ b/dimos/message_codegen/test_project.py @@ -0,0 +1,167 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import json +import tarfile +from types import SimpleNamespace + +import pytest + +from dimos.message_codegen.backend import build_sdist, get_requires_for_build_wheel +from dimos.message_codegen.generate import generate +import dimos.message_codegen.project as project_module +from dimos.message_codegen.project import Project, prepare + + +def project(tmp_path, definition="float64 value\n"): + (tmp_path / "pyproject.toml").write_text("""[project] +name = "example-messages" +version = "1.2.3" +[tool.dimos.messages] +dependencies = {} +""") + source = tmp_path / "interfaces/example_msgs/msg/Reading.msg" + source.parent.mkdir(parents=True) + source.write_text(definition) + return Project.load(tmp_path), source + + +def test_defaults_and_cache_reuse_without_touching_generated_files(tmp_path): + config, _ = project(tmp_path) + output = prepare(config) + header = output / "cpp/messages.hpp" + stamp = header.stat().st_mtime_ns + assert config.module == "example_messages" + assert config.languages == ("python",) + assert prepare(config) == output + assert header.stat().st_mtime_ns == stamp + + +def test_schema_edits_deletion_and_rename_remove_stale_outputs(tmp_path): + config, source = project(tmp_path) + old = source.with_name("Old.msg") + old.write_text("uint32 number\n") + output = prepare(config) + assert (output / "schemas/example_msgs/msg/Old.msg").is_file() + old.unlink() + source.rename(source.with_name("Renamed.msg")) + prepare(config) + assert not (output / "schemas/example_msgs/msg/Old.msg").exists() + assert not (output / "schemas/example_msgs/msg/Reading.msg").exists() + assert (output / "schemas/example_msgs/msg/Renamed.msg").is_file() + source.with_name("Renamed.msg").write_text("int32 replacement\n") + prepare(config) + assert "replacement" in (output / "cpp/messages.hpp").read_text() + + +def test_missing_or_modified_generated_file_is_repaired(tmp_path): + config, _ = project(tmp_path) + output = prepare(config) + header = output / "cpp/messages.hpp" + original = header.read_bytes() + header.write_text("corrupt") + prepare(config) + assert header.read_bytes() == original + header.unlink() + prepare(config) + assert header.read_bytes() == original + + +def test_missing_standard_type_owner_is_not_silently_regenerated(tmp_path): + config, _ = project(tmp_path, "std_msgs/Header header\n") + with pytest.raises(ValueError, match="Missing dependency owners"): + prepare(config) + assert not config.output.exists() + + +def test_invalid_input_keeps_last_successful_generation(tmp_path): + config, source = project(tmp_path) + output = prepare(config) + state = (output / "generation.json").read_bytes() + source.write_text("unknown_msgs/Absent value\n") + with pytest.raises(ValueError, match="unresolved"): + prepare(config) + assert (output / "generation.json").read_bytes() == state + + +def test_config_rejects_typo_instead_of_ignoring_it(tmp_path): + config, _ = project(tmp_path) + with (tmp_path / "pyproject.toml").open("a") as stream: + stream.write('langauges = ["rust"]\n') + with pytest.raises(ValueError, match="Unknown message configuration"): + Project.load(tmp_path) + + +def test_python_backend_never_requests_native_sdk_or_rust(tmp_path, monkeypatch): + project(tmp_path) + monkeypatch.chdir(tmp_path) + assert get_requires_for_build_wheel() == [ + "setuptools>=70", + "wheel", + "pybind11==3.0.1", + "cmake>=3.20", + ] + name = build_sdist(str(tmp_path / "dist")) + with tarfile.open(tmp_path / "dist" / name) as archive: + assert sorted(archive.getnames()) == [ + "example_messages-1.2.3/interfaces/example_msgs/msg/Reading.msg", + "example_messages-1.2.3/pyproject.toml", + ] + assert not (tmp_path / "build").exists() + + +def test_version_change_invalidates_generated_metadata(tmp_path): + config, _ = project(tmp_path) + output = prepare(config) + path = tmp_path / "pyproject.toml" + path.write_text(path.read_text().replace('"1.2.3"', '"1.2.4"')) + prepare(Project.load(tmp_path)) + assert json.loads((output / "message-package.json").read_text())["version"] == "1.2.4" + + +def test_source_archive_is_reproducible(tmp_path, monkeypatch): + project(tmp_path) + monkeypatch.chdir(tmp_path) + first = build_sdist(str(tmp_path / "first")) + second = build_sdist(str(tmp_path / "second")) + assert (tmp_path / "first" / first).read_bytes() == (tmp_path / "second" / second).read_bytes() + + +def test_module_override_cannot_change_distribution_identity(tmp_path): + project(tmp_path) + with (tmp_path / "pyproject.toml").open("a") as stream: + stream.write('python-module = "unrelated"\n') + with pytest.raises(ValueError, match="normalized project name"): + Project.load(tmp_path) + + +def test_transitive_dependencies_are_discovered_and_versions_checked(tmp_path, monkeypatch): + config, _ = project(tmp_path) + base = tmp_path / "base_schemas/package" + generate([], base, ["std_msgs/msg/Header"], "base", version="1.0.0", shared=True) + middle = tmp_path / "middle_schemas/package" + middle.mkdir(parents=True) + metadata = json.loads((base / "message-package.json").read_text()) + metadata.update(module="middle", owned=[], dependencies={"base": "1.0.0"}, codec_owner="base") + (middle / "message-package.json").write_text(json.dumps(metadata)) + monkeypatch.setattr( + project_module, + "find_spec", + lambda name: SimpleNamespace(origin=str(tmp_path / name / "__init__.py")), + ) + config.dependency_specs["middle"] = {"version": "1.0.0"} + assert [dep.module for dep in config.dependencies()] == ["base", "middle"] + config.dependency_specs["base"] = {"version": "2.0.0"} + with pytest.raises(ValueError, match="mismatch|Conflicting package versions"): + config.dependencies() diff --git a/dimos/message_codegen/test_project_install.py b/dimos/message_codegen/test_project_install.py new file mode 100644 index 00000000000..74a38369722 --- /dev/null +++ b/dimos/message_codegen/test_project_install.py @@ -0,0 +1,111 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import os +from pathlib import Path +import subprocess +import sys +import tarfile + +import pytest + + +def test_clean_source_wheel_sdist_and_editable_install(tmp_path): + wheelhouse = os.environ.get("DIMOS_MESSAGE_WHEELHOUSE") + prefix = os.environ.get("DIMOS_FASTCDR_PREFIX") + if not wheelhouse or not prefix: + pytest.skip( + "Set DIMOS_MESSAGE_WHEELHOUSE and DIMOS_FASTCDR_PREFIX for isolated install acceptance" + ) + wheelhouse = str(Path(wheelhouse).resolve()) + environment = {key: value for key, value in os.environ.items() if key != "PYTHONPATH"} + environment.update( + PIP_NO_INDEX="1", PIP_FIND_LINKS=wheelhouse, CMAKE_PREFIX_PATH=str(Path(prefix).resolve()) + ) + venv = tmp_path / "venv" + subprocess.run([sys.executable, "-m", "venv", str(venv)], check=True) + python = str(venv / "bin/python") + app = tmp_path / "app" + definition = app / "interfaces/story_msgs/msg/DeviceReading.msg" + definition.parent.mkdir(parents=True) + definition.write_text("std_msgs/Header header\nfloat64 value\n") + (app / "pyproject.toml").write_text("""[build-system] +requires = ["dimos-message-build==0.1.0"] +build-backend = "dimos_message_build.backend" +[project] +name = "story-messages" +version = "0.1.0" +[tool.dimos.messages] +languages = ["python", "cpp", "rust"] +""") + + def pip(*args): + subprocess.run([python, "-m", "pip", *args], cwd=tmp_path, env=environment, check=True) + + def verify(code): + subprocess.run([python, "-I", "-c", code], cwd=tmp_path, env=environment, check=True) + + pip("install", str(app)) + verify(""" +from importlib.metadata import distributions +from dimos_generated.std_msgs.msg import Header +from story_messages.story_msgs.msg import DeviceReading +message = DeviceReading(header=Header(frame_id='sensor'), value=20.5) +assert type(message.header) is Header +assert DeviceReading.decode(message.encode()).value == 20.5 +assert 'dimos' not in {dist.metadata['Name'] for dist in distributions()} +""") + assert not list((app / "build").rglob("target")) + # Produce a wheel, then prove its install needs no compiler or backend. + dist = tmp_path / "dist" + pip("wheel", "--no-deps", "--wheel-dir", str(dist), str(app)) + pip("uninstall", "-y", "story-messages") + wheel = next(dist.glob("story_messages-*.whl")) + old_path = environment["PATH"] + environment["PATH"] = str(venv / "bin") + pip("install", "--only-binary=:all:", str(wheel)) + verify( + "from story_messages.story_msgs.msg import DeviceReading; assert DeviceReading(value=7).value == 7" + ) + environment["PATH"] = old_path + # Source distribution builds outside the original project and has no generated output. + pip("install", "dimos-message-build==0.1.0") + subprocess.run( + [ + python, + "-c", + "from dimos_message_build.backend import build_sdist; build_sdist('../sdist')", + ], + cwd=app, + env=environment, + check=True, + ) + source = next((tmp_path / "sdist").glob("*.tar.gz")) + with tarfile.open(source) as archive: + assert not any("/build/" in name for name in archive.getnames()) + pip("install", "--force-reinstall", str(source)) + verify( + "from story_messages.story_msgs.msg import DeviceReading; assert DeviceReading(value=8).value == 8" + ) + pip("install", "-e", str(app)) + definition.write_text(definition.read_text() + 'string unit "C"\n') + pip("install", "-e", str(app)) + verify( + "from story_messages.story_msgs.msg import DeviceReading; assert DeviceReading().unit == 'C'" + ) + definition.rename(definition.with_name("RenamedReading.msg")) + pip("install", "-e", str(app)) + verify( + "import story_messages.story_msgs.msg as m; assert not hasattr(m, 'DeviceReading'); assert m.RenamedReading().unit == 'C'" + ) diff --git a/dimos/message_codegen/test_stubs.py b/dimos/message_codegen/test_stubs.py new file mode 100644 index 00000000000..c5db233028a --- /dev/null +++ b/dimos/message_codegen/test_stubs.py @@ -0,0 +1,65 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from pathlib import Path +import subprocess +import sys + +from dimos.message_codegen.generate import generate + + +def test_generated_interfaces_check_nested_fields_buffers_and_keyword_arguments( + tmp_path, monkeypatch +): + generate( + [Path("examples/message-codegen")], tmp_path / "generated", ["demo_msgs/msg/Telemetry"] + ) + monkeypatch.setenv("MYPYPATH", str(tmp_path / "generated/typing")) + source = tmp_path / "consumer.py" + source.write_text( + "from dimos_generated.demo_msgs.msg import Telemetry\n" + "from dimos_generated.geometry_msgs.msg import Point\n" + "value = Telemetry(position=Point(x=1.5), hops=[1, 2], payload=b'abc')\n" + "value.reading.temperature = 32.0\n" + "value.hops.append(3)\n" + "pixels = value.payload.view()\n" + "result: float = Telemetry.decode(value.encode()).position.x\n" + ) + command = [ + sys.executable, + "-m", + "mypy", + "--strict", + "--config-file=/dev/null", + "--cache-dir", + str(tmp_path / "cache"), + str(source), + ] + + valid = subprocess.run(command, capture_output=True, text=True, check=False) + + assert valid.returncode == 0, valid.stdout + valid.stderr + + source.write_text( + source.read_text() + "value.reading.temperature = 'hot'\n" + "value.axes.append(1.0)\n" + "Telemetry(unknown_field=1)\n" + ) + + invalid = subprocess.run(command, capture_output=True, text=True, check=False) + + assert invalid.returncode != 0 + assert "[assignment]" in invalid.stdout + assert "[attr-defined]" in invalid.stdout + assert "[call-arg]" in invalid.stdout diff --git a/dimos/models/embedding/base.py b/dimos/models/embedding/base.py index e3cad47a9ad..aa7c2d32452 100644 --- a/dimos/models/embedding/base.py +++ b/dimos/models/embedding/base.py @@ -26,7 +26,7 @@ from dimos.types.timestamped import Timestamped if TYPE_CHECKING: - from dimos.msgs.sensor_msgs.Image import Image + from dimos_generated.sensor_msgs.msg import Image class EmbeddingModelConfig(LocalModelConfig): @@ -52,7 +52,7 @@ class Embedding(Timestamped): def __init__(self, vector: torch.Tensor | np.ndarray, timestamp: float | None = None) -> None: self.vector = vector - if timestamp: + if timestamp is not None: self.timestamp = timestamp else: self.timestamp = time.time() diff --git a/dimos/models/embedding/clip.py b/dimos/models/embedding/clip.py index 87e7cefa5db..478cc5ee962 100644 --- a/dimos/models/embedding/clip.py +++ b/dimos/models/embedding/clip.py @@ -17,6 +17,7 @@ from functools import cached_property from typing import overload +from dimos_generated.sensor_msgs.msg import Image from PIL import Image as PILImage import torch import torch.nn.functional as functional @@ -24,7 +25,8 @@ from dimos.models.base import HuggingFaceModel from dimos.models.embedding.base import Embedding, EmbeddingModel, HuggingFaceEmbeddingModelConfig -from dimos.msgs.sensor_msgs.Image import Image +from dimos.msgs.image import image_to_rgb +from dimos.msgs.time import to_seconds class CLIPModelConfig(HuggingFaceEmbeddingModelConfig): @@ -57,7 +59,7 @@ def embed(self, *images: Image) -> Embedding | list[Embedding]: Returns embeddings as torch.Tensor on device for efficient GPU comparisons. """ # Convert to PIL images - pil_images = [PILImage.fromarray(img.to_rgb().data) for img in images] + pil_images = [PILImage.fromarray(image_to_rgb(img)) for img in images] # Process images with torch.inference_mode(): @@ -70,7 +72,7 @@ def embed(self, *images: Image) -> Embedding | list[Embedding]: # Create embeddings (keep as torch.Tensor on device) embeddings: list[Embedding] = [] for i, feat in enumerate(image_features): - timestamp = images[i].ts + timestamp = to_seconds(images[i].header.stamp) embeddings.append(Embedding(vector=feat, timestamp=timestamp)) return embeddings[0] if len(images) == 1 else embeddings diff --git a/dimos/models/embedding/dino.py b/dimos/models/embedding/dino.py index 54d500c693d..1cdb276f154 100644 --- a/dimos/models/embedding/dino.py +++ b/dimos/models/embedding/dino.py @@ -17,6 +17,7 @@ from functools import cached_property from typing import Any, overload +from dimos_generated.sensor_msgs.msg import Image from PIL import Image as PILImage import torch import torch.nn.functional as functional @@ -24,7 +25,8 @@ from dimos.models.base import HuggingFaceModel from dimos.models.embedding.base import Embedding, EmbeddingModel, HuggingFaceEmbeddingModelConfig -from dimos.msgs.sensor_msgs.Image import Image +from dimos.msgs.image import image_to_rgb +from dimos.msgs.time import to_seconds class DINOModelConfig(HuggingFaceEmbeddingModelConfig): @@ -51,7 +53,7 @@ def embed(self, image: Image, /) -> Embedding: ... @overload def embed(self, *images: Image) -> list[Embedding]: ... def embed(self, *images: Image) -> Embedding | list[Embedding]: - pil_images = [PILImage.fromarray(img.to_rgb().data) for img in images] + pil_images = [PILImage.fromarray(image_to_rgb(img)) for img in images] with torch.inference_mode(): inputs = self._processor(images=pil_images, return_tensors="pt").to(self.config.device) outputs = self._model(**inputs) @@ -68,7 +70,7 @@ def embed(self, *images: Image) -> Embedding | list[Embedding]: embeddings: list[Embedding] = [] for i, feat in enumerate(feats): - embeddings.append(Embedding(vector=feat, timestamp=images[i].ts)) + embeddings.append(Embedding(vector=feat, timestamp=to_seconds(images[i].header.stamp))) return embeddings[0] if len(images) == 1 else embeddings @overload diff --git a/dimos/models/embedding/mobileclip.py b/dimos/models/embedding/mobileclip.py index ff1f2efc5ad..fcc4855ce37 100644 --- a/dimos/models/embedding/mobileclip.py +++ b/dimos/models/embedding/mobileclip.py @@ -15,6 +15,7 @@ from functools import cached_property from typing import Any, overload +from dimos_generated.sensor_msgs.msg import Image import open_clip from PIL import Image as PILImage import torch @@ -22,7 +23,8 @@ from dimos.models.base import LocalModel from dimos.models.embedding.base import Embedding, EmbeddingModel, EmbeddingModelConfig -from dimos.msgs.sensor_msgs.Image import Image +from dimos.msgs.image import image_to_rgb +from dimos.msgs.time import to_seconds from dimos.utils.data import get_data @@ -66,7 +68,7 @@ def embed(self, *images: Image) -> Embedding | list[Embedding]: Returns embeddings as torch.Tensor on device for efficient GPU comparisons. """ # Convert to PIL images - pil_images = [PILImage.fromarray(img.to_opencv()) for img in images] + pil_images = [PILImage.fromarray(image_to_rgb(img)) for img in images] # Preprocess and batch with torch.inference_mode(): @@ -80,7 +82,7 @@ def embed(self, *images: Image) -> Embedding | list[Embedding]: # Create embeddings (keep as torch.Tensor on device) embeddings = [] for i, feat in enumerate(feats): - timestamp = images[i].ts + timestamp = to_seconds(images[i].header.stamp) embeddings.append(Embedding(vector=feat, timestamp=timestamp)) return embeddings[0] if len(images) == 1 else embeddings diff --git a/dimos/models/embedding/siglip.py b/dimos/models/embedding/siglip.py index 225c98a04f4..71dbd0a1e13 100644 --- a/dimos/models/embedding/siglip.py +++ b/dimos/models/embedding/siglip.py @@ -17,6 +17,7 @@ from functools import cached_property from typing import overload +from dimos_generated.sensor_msgs.msg import Image from PIL import Image as PILImage import torch import torch.nn.functional as functional @@ -24,7 +25,8 @@ from dimos.models.base import HuggingFaceModel from dimos.models.embedding.base import Embedding, EmbeddingModel, HuggingFaceEmbeddingModelConfig -from dimos.msgs.sensor_msgs.Image import Image +from dimos.msgs.image import image_to_rgb +from dimos.msgs.time import to_seconds class SigLIPModelConfig(HuggingFaceEmbeddingModelConfig): @@ -59,7 +61,7 @@ def embed(self, image: Image, /) -> Embedding: ... def embed(self, *images: Image) -> list[Embedding]: ... def embed(self, *images: Image) -> Embedding | list[Embedding]: """Embed one or more images into the shared image-text space.""" - pil_images = [PILImage.fromarray(img.to_rgb().data) for img in images] + pil_images = [PILImage.fromarray(image_to_rgb(img)) for img in images] with torch.inference_mode(): inputs = self._processor(images=pil_images, return_tensors="pt").to(self.config.device) @@ -68,7 +70,8 @@ def embed(self, *images: Image) -> Embedding | list[Embedding]: image_features = functional.normalize(image_features, dim=-1) embeddings = [ - Embedding(vector=feat, timestamp=images[i].ts) for i, feat in enumerate(image_features) + Embedding(vector=feat, timestamp=to_seconds(images[i].header.stamp)) + for i, feat in enumerate(image_features) ] return embeddings[0] if len(images) == 1 else embeddings diff --git a/dimos/models/embedding/test_embedding.py b/dimos/models/embedding/test_embedding.py index 9c7a0757073..aea5783fdee 100644 --- a/dimos/models/embedding/test_embedding.py +++ b/dimos/models/embedding/test_embedding.py @@ -15,13 +15,15 @@ import time from typing import Any +import numpy as np +from PIL import Image as PILImage import pytest import torch from dimos.models.embedding.clip import CLIPModel from dimos.models.embedding.mobileclip import MobileCLIPModel from dimos.models.embedding.treid import TorchReIDModel -from dimos.msgs.sensor_msgs.Image import Image +from dimos.msgs.image import image_from_array from dimos.utils.data import get_data @@ -38,7 +40,9 @@ @pytest.mark.skipif_in_ci def test_embedding_model(model_class: type, model_name: str, supports_text: bool) -> None: """Test embedding functionality across different model types.""" - image = Image.from_file(get_data("cafe.jpg")).to_rgb() + image = image_from_array( + np.asarray(PILImage.open(get_data("cafe.jpg")).convert("RGB")), encoding="rgb8" + ) print(f"\nTesting {model_name} embedding model") @@ -113,7 +117,9 @@ def test_embedding_model(model_class: type, model_name: str, supports_text: bool @pytest.mark.skipif_in_ci def test_text_image_retrieval(model_class: type, model_name: str) -> None: """Test text-to-image retrieval using embedding similarity.""" - image = Image.from_file(get_data("cafe.jpg")).to_rgb() + image = image_from_array( + np.asarray(PILImage.open(get_data("cafe.jpg")).convert("RGB")), encoding="rgb8" + ) print(f"\nTesting {model_name} text-image retrieval") @@ -146,7 +152,9 @@ def test_text_image_retrieval(model_class: type, model_name: str) -> None: @pytest.mark.skipif_in_ci def test_embedding_device_transfer() -> None: """Test embedding device transfer operations.""" - image = Image.from_file(get_data("cafe.jpg")).to_rgb() + image = image_from_array( + np.asarray(PILImage.open(get_data("cafe.jpg")).convert("RGB")), encoding="rgb8" + ) model = CLIPModel() embedding = model.embed(image) diff --git a/dimos/models/embedding/treid.py b/dimos/models/embedding/treid.py index ba648322e8d..9693f9054b9 100644 --- a/dimos/models/embedding/treid.py +++ b/dimos/models/embedding/treid.py @@ -19,13 +19,15 @@ from functools import cached_property +from dimos_generated.sensor_msgs.msg import Image import torch import torch.nn.functional as functional from torchreid import utils as torchreid_utils from dimos.models.base import LocalModel from dimos.models.embedding.base import Embedding, EmbeddingModel, EmbeddingModelConfig -from dimos.msgs.sensor_msgs.Image import Image +from dimos.msgs.image import image_to_bgr +from dimos.msgs.time import to_seconds from dimos.utils.data import get_data @@ -60,7 +62,7 @@ def embed(self, *images: Image) -> Embedding | list[Embedding]: Returns embeddings as torch.Tensor on device for efficient GPU comparisons. """ # Convert to numpy arrays - torchreid expects numpy arrays or file paths - np_images = [img.to_opencv() for img in images] + np_images = [image_to_bgr(img) for img in images] # Extract features with torch.inference_mode(): @@ -78,7 +80,7 @@ def embed(self, *images: Image) -> Embedding | list[Embedding]: # Create embeddings (keep as torch.Tensor on device) embeddings = [] for i, feat in enumerate(features_tensor): - timestamp = images[i].ts + timestamp = to_seconds(images[i].header.stamp) embeddings.append(Embedding(vector=feat, timestamp=timestamp)) return embeddings[0] if len(images) == 1 else embeddings diff --git a/dimos/models/qwen/video_query.py b/dimos/models/qwen/video_query.py index 35979addd43..fcf8c75b517 100644 --- a/dimos/models/qwen/video_query.py +++ b/dimos/models/qwen/video_query.py @@ -20,7 +20,7 @@ from dimos.models.qwen.bbox import BBox from dimos.models.vl.qwen import DEFAULT_QWEN_VL_MODEL, QwenVlModel -from dimos.msgs.sensor_msgs.Image import Image +from dimos.msgs.image import image_from_array def query_single_frame( @@ -51,7 +51,7 @@ def query_single_frame( model = QwenVlModel(model_name=model_name, api_key=api_key) # Wrap with the default BGR tag so Image.to_base64()'s to_bgr() is a no-op and the array # reaches Qwen's JPEG encoder unchanged, matching the prior cv2.imencode(".jpg", frame). - return model.query(Image.from_numpy(image), query) + return model.query(image_from_array(image, encoding="bgr8"), query) def get_bbox_from_qwen_frame(frame, object_name: str | None = None) -> BBox | None: # type: ignore[no-untyped-def] diff --git a/dimos/models/segmentation/edge_tam.py b/dimos/models/segmentation/edge_tam.py index ff4d3648f1a..3b6a1300eb9 100644 --- a/dimos/models/segmentation/edge_tam.py +++ b/dimos/models/segmentation/edge_tam.py @@ -20,6 +20,7 @@ import tempfile from typing import TYPE_CHECKING, Any, Protocol, TypedDict +from dimos_generated.sensor_msgs.msg import Image from hydra.utils import instantiate import numpy as np from numpy.typing import NDArray @@ -28,7 +29,7 @@ import torch from dimos.models.base import default_torch_device -from dimos.msgs.sensor_msgs.Image import Image +from dimos.msgs.image import image_to_bgr, image_to_jpeg from dimos.perception.detection.detectors.base import Detector from dimos.perception.detection.type.detection2d.bbox import Detection2DBBox from dimos.perception.detection.type.detection2d.imageDetections2D import ImageDetections2D @@ -167,7 +168,7 @@ def propose_all( ) self._mask_generators[points_per_side] = generator - rgb = cv2.cvtColor(image.to_opencv(), cv2.COLOR_BGR2RGB) + rgb = cv2.cvtColor(image_to_bgr(image), cv2.COLOR_BGR2RGB) amp = ( torch.autocast("cuda", dtype=torch.bfloat16) if self._predictor.device.type == "cuda" @@ -198,7 +199,7 @@ def segment( return ImageDetections2D(detections.image, []) image = detections.image - rgb = cv2.cvtColor(image.to_opencv(), cv2.COLOR_BGR2RGB) + rgb = cv2.cvtColor(image_to_bgr(image), cv2.COLOR_BGR2RGB) # bfloat16 autocast is CUDA-only; MPS runs in float32 amp = ( @@ -249,7 +250,7 @@ def _prepare_frame(self, image: Image) -> torch.Tensor: """Prepare frame for SAM2 (resize, normalize, convert to tensor).""" import cv2 - cv_image = image.to_opencv() + cv_image = image_to_bgr(image) rgb_image = cv2.cvtColor(cv_image, cv2.COLOR_BGR2RGB) pil_image = PILImage.fromarray(rgb_image) @@ -402,7 +403,7 @@ def stop(self) -> None: def _temp_dir_context(image: Image) -> Generator[str, None, None]: path = tempfile.mkdtemp() - image.save(f"{path}/00000.jpg") + Path(path, "00000.jpg").write_bytes(image_to_jpeg(image)) try: yield path diff --git a/dimos/models/segmentation/test_generated_edge_tam_images.py b/dimos/models/segmentation/test_generated_edge_tam_images.py new file mode 100644 index 00000000000..ac079411bb5 --- /dev/null +++ b/dimos/models/segmentation/test_generated_edge_tam_images.py @@ -0,0 +1,50 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from pathlib import Path +from types import SimpleNamespace + +import cv2 +import numpy as np +import torch + +from dimos.models.segmentation import edge_tam +from dimos.msgs.image import image_from_array +from dimos.perception.detection.type.detection2d.seg import Detection2DSeg + + +def test_generated_tracker_frame_and_initial_jpeg(monkeypatch): + monkeypatch.setattr( + edge_tam, "_build_model", lambda: SimpleNamespace(image_size=8, device="cpu") + ) + processor = edge_tam.EdgeTAMProcessor() + pixels = np.zeros((12, 16, 3), dtype=np.uint8) + pixels[:, :, 0] = 255 + image = image_from_array(pixels, encoding="rgb8") + frame = processor._prepare_frame(image) + assert frame.shape == (3, 8, 8) + torch.testing.assert_close( + frame[:, 0, 0], torch.tensor([(1 - 0.485) / 0.229, -0.456 / 0.224, -0.406 / 0.225]) + ) + with edge_tam._temp_dir_context(image) as directory: + path = Path(directory) + decoded = cv2.imread(str(path / "00000.jpg")) + assert decoded.shape == (12, 16, 3) + assert decoded[0, 0, 2] > 240 + assert not path.exists() + mask = torch.zeros((12, 16)) + mask[2:8, 3:10] = 1 + detection = Detection2DSeg.from_sam2_result(mask, 7, image) + assert detection.bbox == (3, 2, 9, 7) + assert detection.to_ros_detection2d().header == image.header diff --git a/dimos/models/segmentation/test_yoloe.py b/dimos/models/segmentation/test_yoloe.py index 5848f1aeca5..f576719b4c8 100644 --- a/dimos/models/segmentation/test_yoloe.py +++ b/dimos/models/segmentation/test_yoloe.py @@ -16,22 +16,24 @@ from typing import Any from unittest.mock import MagicMock +from dimos_generated.sensor_msgs.msg import Image +from dimos_generated.std_msgs.msg import Header import numpy as np from ultralytics.models.yolo.yoloe import YOLOEVPSegPredictor from dimos.models.segmentation.yoloe import YoloeBoxSegmenter -from dimos.msgs.sensor_msgs.Image import Image, ImageFormat +from dimos.msgs.image import image_from_array, image_to_bgr +from dimos.msgs.time import time_from_seconds, to_seconds from dimos.perception.detection.type.detection2d.bbox import Detection2DBBox from dimos.perception.detection.type.detection2d.imageDetections2D import ImageDetections2D from dimos.perception.detection.type.detection2d.seg import Detection2DSeg def _image() -> Image: - return Image( - data=np.zeros((32, 48, 3), dtype=np.uint8), - format=ImageFormat.BGR, - frame_id="camera", - ts=12.5, + return image_from_array( + np.zeros((32, 48, 3), dtype=np.uint8), + encoding="bgr8", + header=Header(frame_id="camera", stamp=time_from_seconds(12.5)), ) @@ -42,7 +44,7 @@ def _box(image: Image, *, bbox: tuple[float, float, float, float]) -> Detection2 class_id=7, confidence=0.73, name="white mug", - ts=image.ts, + ts=to_seconds(image.header.stamp), image=image, ) @@ -60,7 +62,7 @@ def _mask( class_id=prompt_id, confidence=0.11, name=f"prompt-{prompt_id}", - ts=image.ts, + ts=to_seconds(image.header.stamp), image=image, mask=np.full((32, 48), value, dtype=np.uint8), ) @@ -137,7 +139,7 @@ def test_visual_box_prompts_use_segmentation_predictor(mocker: Any) -> None: get_data.assert_called_once_with("models_yoloe") model_factory.assert_called_once_with(Path("/models/yoloe-11s-seg.pt")) kwargs = dict(model.predict.call_args.kwargs) - np.testing.assert_array_equal(kwargs.pop("source"), image.to_opencv()) + np.testing.assert_array_equal(kwargs.pop("source"), image_to_bgr(image)) visual_prompts = kwargs.pop("visual_prompts") np.testing.assert_array_equal( visual_prompts["bboxes"], np.asarray([detection.bbox], dtype=np.float64) diff --git a/dimos/models/segmentation/yoloe.py b/dimos/models/segmentation/yoloe.py index a13d0a9d009..b2be2676b24 100644 --- a/dimos/models/segmentation/yoloe.py +++ b/dimos/models/segmentation/yoloe.py @@ -19,11 +19,12 @@ from dataclasses import replace from typing import Protocol +from dimos_generated.sensor_msgs.msg import Image import numpy as np from numpy.typing import NDArray from ultralytics.models.yolo.yoloe import YOLOEVPSegPredictor -from dimos.msgs.sensor_msgs.Image import Image +from dimos.msgs.image import image_to_bgr from dimos.perception.detection.detectors.yoloe import Yoloe2DDetector, YoloePromptMode from dimos.perception.detection.type.detection2d.bbox import Bbox, Detection2DBBox from dimos.perception.detection.type.detection2d.imageDetections2D import ImageDetections2D @@ -58,7 +59,7 @@ def predict_image(self, image: Image) -> ImageDetections2D: raise RuntimeError("YOLO-E segmentation requires visual box prompts") with self._lock: results = self.model.predict( - source=image.to_opencv(), + source=image_to_bgr(image), device=self.device, conf=self.conf, iou=0.6, diff --git a/dimos/models/vl/README.md b/dimos/models/vl/README.md index 12fdbfbc1ba..488cee1ef19 100644 --- a/dimos/models/vl/README.md +++ b/dimos/models/vl/README.md @@ -13,14 +13,14 @@ returns absolute pixel coordinates, which rules out the `qwen3-vl-*` models. ```python from dimos.models.vl.qwen import QwenVlModel -from dimos.msgs.sensor_msgs.Image import Image +from dimos.msgs.image import image_from_file, image_view # Initialize the model (requires ALIBABA_API_KEY environment variable) model = QwenVlModel() -image = Image.from_file("path/to/your/image.jpg") +image = image_from_file("path/to/your/image.jpg") -response = model.query(image.data, "What do you see in this image?") +response = model.query(image_view(image), "What do you see in this image?") print(response) ``` @@ -48,10 +48,10 @@ The model supports four modes of operation: ```python from dimos.models.vl.moondream_hosted import MoondreamHostedVlModel -from dimos.msgs.sensor_msgs import Image +from dimos.msgs.image import image_from_file, image_view model = MoondreamHostedVlModel() -image = Image.from_file("path/to/image.jpg") +image = image_from_file("path/to/image.jpg") # 1. Caption print(f"Caption: {model.caption(image)}") diff --git a/dimos/models/vl/base.py b/dimos/models/vl/base.py index 55b616176dc..3f1d4bad18d 100644 --- a/dimos/models/vl/base.py +++ b/dimos/models/vl/base.py @@ -19,8 +19,11 @@ from typing import Any import warnings +from dimos_generated.sensor_msgs.msg import Image + from dimos.core.resource import Resource -from dimos.msgs.sensor_msgs.Image import Image +from dimos.msgs.image import image_from_array, image_view +from dimos.msgs.time import to_seconds from dimos.perception.detection.type.detection2d.bbox import Detection2DBBox from dimos.perception.detection.type.detection2d.imageDetections2D import ImageDetections2D from dimos.perception.detection.type.detection2d.point import Detection2DPoint @@ -116,7 +119,7 @@ def vlm_detection_to_detection2d( class_id=-1, confidence=1.0, name=name, - ts=image.ts, + ts=to_seconds(image.header.stamp), image=image, ) @@ -166,7 +169,7 @@ def vlm_point_to_detection2d_point( x=x, y=y, name=name, - ts=image.ts, + ts=to_seconds(image.header.stamp), image=image, track_id=track_id, ) @@ -196,9 +199,21 @@ def _prepare_image(self, image: Image) -> tuple[Image, float]: Returns: Tuple of (prepared_image, scale_factor). Scale factor is 1.0 if no resize. """ + import cv2 + if self.config.auto_resize is not None: max_w, max_h = self.config.auto_resize - return image.resize_to_fit(max_w, max_h) + if max_w <= 0 or max_h <= 0: + raise ValueError("auto_resize dimensions must be positive") + scale = min(1.0, max_w / image.width, max_h / image.height) + if scale == 1.0: + return image, scale + pixels = cv2.resize( + image_view(image), + (max(1, int(image.width * scale)), max(1, int(image.height * scale))), + interpolation=cv2.INTER_AREA, + ) + return image_from_array(pixels, encoding=image.encoding, header=image.header), scale return image, 1.0 @abstractmethod @@ -251,8 +266,13 @@ def caption(self, image: Image) -> str: def start(self) -> None: """Start the model by running a simple query (Resource interface).""" + import cv2 + try: - image = Image.from_file(get_data("cafe-smol.jpg")).to_rgb() + pixels = cv2.imread(str(get_data("cafe-smol.jpg"))) + if pixels is None: + raise ValueError("Could not read model warmup image") + image = image_from_array(pixels, encoding="bgr8") self.query(image, "What is this?") except Exception: pass diff --git a/dimos/models/vl/florence.py b/dimos/models/vl/florence.py index 8e964bb85f7..92ce9b2a353 100644 --- a/dimos/models/vl/florence.py +++ b/dimos/models/vl/florence.py @@ -15,13 +15,14 @@ from enum import Enum from functools import cached_property +from dimos_generated.sensor_msgs.msg import Image from PIL import Image as PILImage import torch from transformers import AutoModelForCausalLM, AutoProcessor from dimos.models.base import HuggingFaceModel from dimos.models.vl.base import Captioner -from dimos.msgs.sensor_msgs.Image import Image +from dimos.msgs.image import image_to_rgb class CaptionDetail(Enum): @@ -98,7 +99,7 @@ def caption(self, image: Image, detail: str | CaptionDetail | None = None) -> st task_prompt = CaptionDetail.from_str(detail).value # Convert to PIL - pil_image = PILImage.fromarray(image.to_rgb().data) + pil_image = PILImage.fromarray(image_to_rgb(image)) # Process inputs inputs = self._processor(text=task_prompt, images=pil_image, return_tensors="pt") @@ -137,7 +138,7 @@ def caption_batch(self, *images: Image) -> list[str]: task_prompt = self._task_prompt # Convert all to PIL - pil_images = [PILImage.fromarray(img.to_rgb().data) for img in images] + pil_images = [PILImage.fromarray(image_to_rgb(img)) for img in images] # Process batch inputs = self._processor( diff --git a/dimos/models/vl/moondream.py b/dimos/models/vl/moondream.py index e3cfe744ce8..0956d7844d1 100644 --- a/dimos/models/vl/moondream.py +++ b/dimos/models/vl/moondream.py @@ -16,6 +16,7 @@ from typing import Any import warnings +from dimos_generated.sensor_msgs.msg import Image import numpy as np from PIL import Image as PILImage import torch @@ -23,7 +24,8 @@ from dimos.models.base import HuggingFaceModel, HuggingFaceModelConfig from dimos.models.vl.base import VlModel, VlModelConfig -from dimos.msgs.sensor_msgs.Image import Image +from dimos.msgs.image import image_from_array, image_to_rgb +from dimos.msgs.time import to_seconds from dimos.perception.detection.type.detection2d.bbox import Detection2DBBox from dimos.perception.detection.type.detection2d.imageDetections2D import ImageDetections2D from dimos.perception.detection.type.detection2d.point import Detection2DPoint @@ -63,11 +65,11 @@ def _to_pil(self, image: Image | np.ndarray[Any, Any]) -> PILImage.Image: DeprecationWarning, stacklevel=2, ) - image = Image.from_numpy(image) + image = image_from_array(image, encoding="bgr8") image, _ = self._prepare_image(image) - rgb_image = image.to_rgb() - return PILImage.fromarray(rgb_image.data) + rgb_image = image_to_rgb(image) + return PILImage.fromarray(rgb_image) def query(self, image: Image | np.ndarray, query: str, **kwargs) -> str: # type: ignore[no-untyped-def] pil_image = self._to_pil(image) @@ -182,7 +184,7 @@ def query_detections( class_id=-1, # Moondream doesn't provide class IDs confidence=1.0, # Moondream doesn't provide confidence scores name=query, # Use the query as the object name - ts=image.ts, + ts=to_seconds(image.header.stamp), image=image, ) @@ -222,7 +224,7 @@ def query_points( x=x, y=y, name=query, - ts=image.ts, + ts=to_seconds(image.header.stamp), image=image, track_id=track_id, ) diff --git a/dimos/models/vl/moondream_hosted.py b/dimos/models/vl/moondream_hosted.py index 76e55451a1e..c03d50832a1 100644 --- a/dimos/models/vl/moondream_hosted.py +++ b/dimos/models/vl/moondream_hosted.py @@ -17,12 +17,14 @@ from typing import Any import warnings +from dimos_generated.sensor_msgs.msg import Image import moondream as md # type: ignore[import-untyped] import numpy as np from PIL import Image as PILImage from dimos.models.vl.base import VlModel, VlModelConfig -from dimos.msgs.sensor_msgs.Image import Image +from dimos.msgs.image import image_from_array, image_to_rgb +from dimos.msgs.time import to_seconds from dimos.perception.detection.type.detection2d.bbox import Detection2DBBox from dimos.perception.detection.type.detection2d.imageDetections2D import ImageDetections2D from dimos.perception.detection.type.detection2d.point import Detection2DPoint @@ -51,10 +53,10 @@ def _to_pil_image(self, image: Image | np.ndarray) -> PILImage.Image: DeprecationWarning, stacklevel=3, ) - image = Image.from_numpy(image) + image = image_from_array(image, encoding="bgr8") - rgb_image = image.to_rgb() - return PILImage.fromarray(rgb_image.data) + rgb_image = image_to_rgb(image) + return PILImage.fromarray(rgb_image) def query(self, image: Image | np.ndarray, query: str, **kwargs) -> str: # type: ignore[no-untyped-def] pil_image = self._to_pil_image(image) @@ -118,7 +120,7 @@ def query_detections( class_id=-1, confidence=1.0, name=query, - ts=image.ts, + ts=to_seconds(image.header.stamp), image=image, ) @@ -153,7 +155,7 @@ def query_points( x=x, y=y, name=query, - ts=image.ts, + ts=to_seconds(image.header.stamp), image=image, track_id=track_id, ) diff --git a/dimos/models/vl/openai.py b/dimos/models/vl/openai.py index 2303e20fb70..ae22e6a7a58 100644 --- a/dimos/models/vl/openai.py +++ b/dimos/models/vl/openai.py @@ -12,15 +12,16 @@ # See the License for the specific language governing permissions and # limitations under the License. +import base64 from functools import cached_property import os from typing import Any -import numpy as np +from dimos_generated.sensor_msgs.msg import Image from openai import OpenAI from dimos.models.vl.base import VlModel, VlModelConfig -from dimos.msgs.sensor_msgs.Image import Image +from dimos.msgs.image import image_to_jpeg from dimos.utils.logging_config import setup_logger logger = setup_logger() @@ -46,26 +47,15 @@ def _client(self) -> OpenAI: def query( self, - image: Image | np.ndarray, + image: Image, query: str, response_format: dict[str, Any] | None = None, **kwargs: Any, ) -> str: - if isinstance(image, np.ndarray): - import warnings - - warnings.warn( - "OpenAIVlModel.query should receive standard dimos Image type, not a numpy array", - DeprecationWarning, - stacklevel=2, - ) - - image = Image.from_numpy(image) - # Apply auto_resize if configured image, _ = self._prepare_image(image) - img_base64 = image.to_base64() + img_base64 = base64.b64encode(image_to_jpeg(image)).decode("ascii") api_kwargs: dict[str, Any] = { "model": self.config.model_name, @@ -105,7 +95,7 @@ def query_batch( { "type": "image_url", "image_url": { - "url": f"data:image/jpeg;base64,{self._prepare_image(img)[0].to_base64()}" + "url": f"data:image/jpeg;base64,{base64.b64encode(image_to_jpeg(self._prepare_image(img)[0])).decode('ascii')}" }, } for img in images diff --git a/dimos/models/vl/qwen.py b/dimos/models/vl/qwen.py index 84542b60c68..46a0ba2677f 100644 --- a/dimos/models/vl/qwen.py +++ b/dimos/models/vl/qwen.py @@ -12,15 +12,16 @@ # See the License for the specific language governing permissions and # limitations under the License. +import base64 from functools import cached_property import os from typing import Any -import numpy as np +from dimos_generated.sensor_msgs.msg import Image from openai import OpenAI from dimos.models.vl.base import VlModel, VlModelConfig -from dimos.msgs.sensor_msgs.Image import Image +from dimos.msgs.image import image_to_jpeg # Must be a model that returns ABSOLUTE pixel coordinates. The qwen3-vl-* models return # normalized 0-1000 coordinates and ignore prompts asking for pixels, which silently breaks @@ -51,22 +52,11 @@ def _client(self) -> OpenAI: api_key=api_key, ) - def query(self, image: Image | np.ndarray, query: str) -> str: # type: ignore[override] - if isinstance(image, np.ndarray): - import warnings - - warnings.warn( - "QwenVlModel.query should receive standard dimos Image type, not a numpy array", - DeprecationWarning, - stacklevel=2, - ) - - image = Image.from_numpy(image) - + def query(self, image: Image, query: str) -> str: # type: ignore[override] # Apply auto_resize if configured image, _ = self._prepare_image(image) - img_base64 = image.to_base64() + img_base64 = base64.b64encode(image_to_jpeg(image)).decode("ascii") response = self._client.chat.completions.create( model=self.config.model_name, @@ -76,7 +66,7 @@ def query(self, image: Image | np.ndarray, query: str) -> str: # type: ignore[o "content": [ { "type": "image_url", - "image_url": {"url": f"data:image/png;base64,{img_base64}"}, + "image_url": {"url": f"data:image/jpeg;base64,{img_base64}"}, }, {"type": "text", "text": query}, ], @@ -101,7 +91,7 @@ def query_batch( { "type": "image_url", "image_url": { - "url": f"data:image/png;base64,{self._prepare_image(img)[0].to_base64()}" + "url": f"data:image/jpeg;base64,{base64.b64encode(image_to_jpeg(self._prepare_image(img)[0])).decode('ascii')}" }, } for img in images diff --git a/dimos/models/vl/test_base.py b/dimos/models/vl/test_base.py index 92b8406f136..5d00e5395a0 100644 --- a/dimos/models/vl/test_base.py +++ b/dimos/models/vl/test_base.py @@ -17,7 +17,8 @@ import pytest from dimos.models.vl.qwen import QwenVlModel -from dimos.msgs.sensor_msgs.Image import Image +from dimos_generated.sensor_msgs.msg import Image +from dimos.msgs.image import image_from_file from dimos.perception.detection.type.detection2d.imageDetections2D import ImageDetections2D from dimos.utils.data import get_data @@ -44,7 +45,7 @@ def test_query_detections_mocked() -> None: """Test query_detections with mocked API response (no API key required).""" # Load test image - image = Image.from_file(get_data("cafe.jpg")) + image = image_from_file(get_data("cafe.jpg")) # Create model and mock the query method model = QwenVlModel() diff --git a/dimos/models/vl/test_captioner.py b/dimos/models/vl/test_captioner.py index 2d53192d4c2..5aa1d6bcf37 100644 --- a/dimos/models/vl/test_captioner.py +++ b/dimos/models/vl/test_captioner.py @@ -20,7 +20,8 @@ from dimos.models.vl.florence import Florence2Model from dimos.models.vl.moondream import MoondreamVlModel -from dimos.msgs.sensor_msgs.Image import Image +from dimos_generated.sensor_msgs.msg import Image +from dimos.msgs.image import image_from_file from dimos.utils.data import get_data @@ -38,7 +39,7 @@ def stop(self) -> None: ... @pytest.fixture(scope="module") def test_image() -> Image: - return Image.from_file(get_data("cafe.jpg")).to_rgb() + return image_from_file(get_data("cafe.jpg")) def generic_model_fixture(model_type: type[M]) -> Generator[M, None, None]: diff --git a/dimos/models/vl/test_generated_images.py b/dimos/models/vl/test_generated_images.py new file mode 100644 index 00000000000..01e560fb76a --- /dev/null +++ b/dimos/models/vl/test_generated_images.py @@ -0,0 +1,85 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import base64 +from unittest.mock import MagicMock + +import cv2 +from dimos_generated.builtin_interfaces.msg import Time +from dimos_generated.sensor_msgs.msg import Image +from dimos_generated.std_msgs.msg import Header +from dimos_generated.vision_msgs.msg import Detection2DArray +import numpy as np +import pytest + +from dimos.models.vl.base import VlModel +from dimos.models.vl.openai import OpenAIVlModel +from dimos.models.vl.qwen import QwenVlModel +from dimos.msgs.image import image_from_array + + +class StaticModel(VlModel): + def stop(self) -> None: + pass + + def query(self, image: Image, query: str, **kwargs) -> str: + return '[["target", 2, 4, 10, 12]]' + + +def test_generated_vl_resize_and_detection(): + image = image_from_array( + np.zeros((40, 60, 3), dtype=np.uint8), + encoding="rgb8", + header=Header(frame_id="camera", stamp=Time(sec=1700000000, nanosec=123456789)), + ) + model = StaticModel(auto_resize=(30, 20)) + resized, scale = model._prepare_image(image) + assert scale == 0.5 + assert (resized.width, resized.height) == (30, 20) + assert resized.header == image.header + detections = model.query_detections(image, "target") + wire = Detection2DArray.decode(detections.to_ros_detection2d_array().encode()) + assert len(wire.detections) == 1 + assert wire.header == image.header + assert wire.detections[0].results[0].hypothesis.class_id == "-1" + + +@pytest.mark.parametrize("model_type", [QwenVlModel, OpenAIVlModel]) +@pytest.mark.parametrize("batch", [False, True]) +def test_provider_encodes_generated_images_as_jpeg(model_type, batch): + pixels = np.zeros((40, 60, 3), dtype=np.uint8) + pixels[:, :, 0] = 255 + image = image_from_array(pixels, encoding="rgb8") + model = model_type(auto_resize=(30, 20)) + client = MagicMock() + client.chat.completions.create.return_value.choices[0].message.content = "target" + model.__dict__["_client"] = client + if batch: + assert model.query_batch([image, image], "find target") == ["target", "target"] + else: + assert model.query(image, "find target") == "target" + request = client.chat.completions.create.call_args.kwargs + content = request["messages"][0]["content"] + images = [item for item in content if item["type"] == "image_url"] + assert len(images) == (2 if batch else 1) + for item in images: + prefix, encoded = item["image_url"]["url"].split(",", 1) + assert prefix == "data:image/jpeg;base64" + jpeg = base64.b64decode(encoded) + assert jpeg[:2] == b"\xff\xd8" + decoded = cv2.imdecode(np.frombuffer(jpeg, dtype=np.uint8), cv2.IMREAD_COLOR) + assert decoded.shape == (20, 30, 3) + assert decoded[10, 15, 2] > 240 + assert decoded[10, 15, 0] < 10 + model.stop() diff --git a/dimos/models/vl/tool_base.py b/dimos/models/vl/tool_base.py index 0a6b5b80c62..2cd64850e00 100644 --- a/dimos/models/vl/tool_base.py +++ b/dimos/models/vl/tool_base.py @@ -17,7 +17,8 @@ from dimos.core.transport import LCMTransport from dimos.models.vl.moondream import MoondreamVlModel from dimos.models.vl.qwen import QwenVlModel -from dimos.msgs.sensor_msgs.Image import Image, ImageFormat +from dimos_generated.sensor_msgs.msg import Image +from dimos.msgs.image import image_from_file from dimos.perception.detection.type.detection2d.imageDetections2D import ImageDetections2D from dimos.utils.data import get_data @@ -26,7 +27,7 @@ def test_query_detections_real() -> None: """Test query_detections with real API calls (requires API key).""" # Load test image - image = Image.from_file(get_data("cafe.jpg")) + image = image_from_file(get_data("cafe.jpg")) # Initialize the model (will use real API) model = QwenVlModel() @@ -55,7 +56,7 @@ def test_query_detections_real() -> None: def test_query_points() -> None: """Test query_points with real API calls (requires API key).""" # Load test image - image = Image.from_file(get_data("cafe.jpg"), format=ImageFormat.RGB).to_rgb() + image = image_from_file(get_data("cafe.jpg")) # Initialize the model (will use real API) model = MoondreamVlModel() diff --git a/dimos/models/vl/tool_vlm.py b/dimos/models/vl/tool_vlm.py index 7bc88c13677..a8b99dd445a 100644 --- a/dimos/models/vl/tool_vlm.py +++ b/dimos/models/vl/tool_vlm.py @@ -12,13 +12,15 @@ # See the License for the specific language governing permissions and # limitations under the License. +import os import time from typing import TYPE_CHECKING import pytest from dimos.models.vl.moondream import MoondreamVlModel -from dimos.msgs.sensor_msgs.Image import Image +from dimos_generated.sensor_msgs.msg import Image +from dimos.utils.testing.replay import TimedSensorReplay if TYPE_CHECKING: from dimos.models.vl.base import VlModel @@ -32,11 +34,14 @@ ) def test_vlm_query_batch(model_class: "type[VlModel]", model_name: str) -> None: """Test query_batch optimization - multiple images, same query.""" - from dimos.utils.testing.legacy_pickle import LegacyPickleStore - - # Load 5 frames at 1-second intervals using LegacyPickleStore - replay = LegacyPickleStore[Image]("unitree_go2_office_walk2/video") - images = [replay.find_closest_seek(i).to_rgb() for i in range(0, 10, 2)] + # Explicit CDR recording input: .db/. + recording = os.environ["DIMOS_VLM_RECORDING"] + replay = TimedSensorReplay[Image](recording) + images = [] + for offset in range(0, 10, 2): + image = replay.find_closest_seek(offset) + assert isinstance(image, Image), f"No generated image at offset {offset}" + images.append(image) print(f"\nTesting {model_name} query_batch with {len(images)} images") diff --git a/dimos/msgs/camera_info.py b/dimos/msgs/camera_info.py new file mode 100644 index 00000000000..7977a275b22 --- /dev/null +++ b/dimos/msgs/camera_info.py @@ -0,0 +1,169 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Calibration construction and array access for generated ROS CameraInfo values.""" + +from copy import deepcopy +import math +from pathlib import Path +import re +from typing import Any, Literal + +from dimos_generated.builtin_interfaces.msg import Time +from dimos_generated.sensor_msgs.msg import CameraInfo +from dimos_generated.std_msgs.msg import Header +import numpy as np +from numpy.typing import NDArray +import yaml + + +def _dimensions(width: int, height: int) -> None: + if any( + isinstance(value, bool) or not isinstance(value, int) or value <= 0 + for value in (width, height) + ): + raise ValueError("Calibration width and height must be positive integers") + + +def camera_info_from_intrinsics( + fx: float, fy: float, cx: float, cy: float, width: int, height: int, *, header: Header +) -> CameraInfo: + """Construct an undistorted pinhole calibration with an explicit source header.""" + _dimensions(width, height) + if not all(math.isfinite(value) for value in (fx, fy, cx, cy)) or fx <= 0 or fy <= 0: + raise ValueError("Intrinsics must be finite with positive focal lengths") + return CameraInfo( + header=header, + width=width, + height=height, + distortion_model="plumb_bob", + d=[0.0] * 5, + k=[fx, 0, cx, 0, fy, cy, 0, 0, 1], + r=[1, 0, 0, 0, 1, 0, 0, 0, 1], + p=[fx, 0, cx, 0, 0, fy, cy, 0, 0, 0, 1, 0], + ) + + +def camera_info_from_fov( + fov_degrees: float, + width: int, + height: int, + *, + header: Header, + axis: Literal["horizontal", "vertical"] = "vertical", +) -> CameraInfo: + """Construct a centered pinhole model with square pixels from an axis's FOV.""" + _dimensions(width, height) + if not math.isfinite(fov_degrees) or not 0 < fov_degrees < 180: + raise ValueError("Field of view must be finite and between 0 and 180 degrees") + if axis not in ("horizontal", "vertical"): + raise ValueError("Field-of-view axis must be horizontal or vertical") + extent = width if axis == "horizontal" else height + focal = extent / (2 * math.tan(math.radians(fov_degrees) / 2)) + return camera_info_from_intrinsics( + focal, focal, width / 2, height / 2, width, height, header=header + ) + + +def _matrix(data: dict[str, Any], name: str, rows: int, columns: int | None) -> list[float]: + matrix = data[name] + if not isinstance(matrix, dict) or matrix.get("rows") != rows: + raise ValueError(f"{name} must declare {rows} rows") + actual_columns = matrix.get("cols") + if ( + not isinstance(actual_columns, int) + or actual_columns < 0 + or (columns is not None and actual_columns != columns) + ): + raise ValueError(f"{name} has invalid columns") + values = matrix.get("data") + if not isinstance(values, list) or len(values) != rows * actual_columns: + raise ValueError(f"{name} data length does not match its dimensions") + converted = [float(value) for value in values] + if not all(math.isfinite(value) for value in converted): + raise ValueError(f"{name} must contain finite values") + return converted + + +def camera_info_from_yaml(path: str | Path, *, header: Header) -> CameraInfo: + """Read a ROS calibration YAML file, validating declared matrix dimensions.""" + with Path(path).open() as source: + data = yaml.safe_load(source) + if not isinstance(data, dict): + raise ValueError("Calibration YAML must contain a mapping") + width, height = data["image_width"], data["image_height"] + _dimensions(width, height) + return CameraInfo( + header=header, + width=width, + height=height, + distortion_model=data["distortion_model"], + k=_matrix(data, "camera_matrix", 3, 3), + d=_matrix(data, "distortion_coefficients", 1, None), + r=_matrix(data, "rectification_matrix", 3, 3), + p=_matrix(data, "projection_matrix", 3, 4), + ) + + +def intrinsic_matrix(message: CameraInfo) -> NDArray[np.float64]: + """Copy the 3x3 intrinsic matrix into independently mutable NumPy storage.""" + return np.array(message.k, dtype=np.float64, copy=True).reshape(3, 3) + + +def camera_info_with_stamp(message: CameraInfo, stamp: Time) -> CameraInfo: + """Copy a calibration with an exact source stamp, without mutating prior publishes.""" + result = deepcopy(message) + result.header.stamp = deepcopy(stamp) + return result + + +class CalibrationProvider: + """Lazily load named YAML calibrations as plain generated CameraInfo values. + + Both SingleWebcam and single_webcam resolve single_webcam.yaml. This + helper lives outside generated messages; calibration files stay local. + """ + + def __init__(self, calibration_dir: str | Path, *, frame_id: str = "camera_optical") -> None: + self._calibration_dir = Path(calibration_dir) + self._frame_id = frame_id + self._cache: dict[str, CameraInfo] = {} + + @staticmethod + def _to_snake_case(name: str) -> str: + first = re.sub("(.)([A-Z][a-z]+)", r"\1_\2", name) + return re.sub("([a-z0-9])([A-Z])", r"\1_\2", first).lower() + + def __getattr__(self, name: str) -> CameraInfo: + if not re.fullmatch(r"[A-Za-z][A-Za-z0-9_]*", name): + raise AttributeError(f"No calibration file found for: {name}") + snake = self._to_snake_case(name) + for key in (name, snake): + if key in self._cache: + return self._cache[key] + for key in (name, snake): + path = self._calibration_dir / f"{key}.yaml" + if path.is_file(): + value = camera_info_from_yaml(path, header=Header(frame_id=self._frame_id)) + self._cache[name] = value + self._cache[snake] = value + return value + raise AttributeError(f"No calibration file found for: {name}") + + def __dir__(self) -> list[str]: + names: set[str] = set() + for path in self._calibration_dir.glob("*.yaml"): + names.add(path.stem) + names.add("".join(word.capitalize() for word in path.stem.split("_"))) + return sorted(names) diff --git a/dimos/msgs/foxglove_msgs/CompressedVideo.py b/dimos/msgs/foxglove_msgs/CompressedVideo.py deleted file mode 100644 index b13089c303b..00000000000 --- a/dimos/msgs/foxglove_msgs/CompressedVideo.py +++ /dev/null @@ -1,107 +0,0 @@ -# Copyright 2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -"""foxglove_msgs::msg::CompressedVideo — one encoded video packet. - -Just the encoded bytes + codec name. Inter-frame codecs (h264) can't be decoded -one packet at a time — feed the ordered stream through -:class:`~dimos.robot.unitree.go2.dds.video.H264Decoder` to get ``Image`` frames, -or let the rerun bridge log it as a ``VideoStream`` and decode in-viewer. -""" - -from __future__ import annotations - -import time -from typing import TYPE_CHECKING - -from dimos_lcm.foxglove_msgs import CompressedVideo as LCMCompressedVideo -import numpy as np - -from dimos.types.timestamped import Timestamped - -if TYPE_CHECKING: - from dimos.visualization.rerun.bridge import RerunData - - -class CompressedVideo(Timestamped): - """One encoded video packet (Annex-B for h264).""" - - msg_name = "foxglove_msgs.CompressedVideo" - - def __init__( - self, - data: np.ndarray | bytes, - format: str = "h264", - frame_id: str = "", - ts: float | None = None, - ) -> None: - """Initialize a CompressedVideo. - - Args: - data: The encoded packet, u8 - format: Codec name, e.g. "h264" - frame_id: Frame ID - ts: Timestamp in seconds - """ - self.data = data if isinstance(data, np.ndarray) else np.frombuffer(data, dtype=np.uint8) - self.format = format - self.frame_id = frame_id - self.ts = ts if ts is not None else time.time() - - def lcm_encode(self) -> bytes: - msg = LCMCompressedVideo() - msg.timestamp.sec = int(self.ts) - msg.timestamp.nanosec = int((self.ts - int(self.ts)) * 1e9) - msg.frame_id = self.frame_id - msg.data = self.data.tobytes() - msg.data_length = len(msg.data) - msg.format = self.format - return msg.lcm_encode() # type: ignore[no-any-return] - - @classmethod - def lcm_decode(cls, data: bytes) -> CompressedVideo: - msg = LCMCompressedVideo.lcm_decode(data) - return cls( - data=bytes(msg.data), - format=msg.format, - frame_id=msg.frame_id, - ts=msg.timestamp.sec + msg.timestamp.nanosec * 1e-9, - ) - - def to_rerun(self) -> RerunData: - """Log the encoded packet as a rerun ``VideoStream`` sample (viewer decodes). - - rerun decodes the stream in-viewer, so this stays per-packet and cheap — - no server-side decode, and the .rrd holds the compressed bytes. Iterate - from the start (or a keyframe) so the first sample the viewer sees is one. - """ - import rerun as rr - - codecs = { - "h264": rr.VideoCodec.H264, - "h265": rr.VideoCodec.H265, - "av1": rr.VideoCodec.AV1, - "vp8": rr.VideoCodec.VP8, - "vp9": rr.VideoCodec.VP9, - } - codec = codecs.get(self.format.lower()) - if codec is None: - raise ValueError(f"no rerun VideoCodec for format {self.format!r}") - return rr.VideoStream(codec, sample=self.data.tobytes()) - - def __repr__(self) -> str: - return ( - f"CompressedVideo(format='{self.format}', bytes={self.data.size}, " - f"frame_id='{self.frame_id}', ts={self.ts})" - ) diff --git a/dimos/msgs/foxglove_msgs/test_CompressedVideo.py b/dimos/msgs/foxglove_msgs/test_CompressedVideo.py index fcecd05a1cb..fa475e0c5b9 100644 --- a/dimos/msgs/foxglove_msgs/test_CompressedVideo.py +++ b/dimos/msgs/foxglove_msgs/test_CompressedVideo.py @@ -12,37 +12,44 @@ # See the License for the specific language governing permissions and # limitations under the License. +from dimos_generated.foxglove_msgs.msg import CompressedVideo import pytest import rerun as rr -from dimos.msgs.foxglove_msgs.CompressedVideo import CompressedVideo from dimos.msgs.helpers import resolve_msg_type +from dimos.msgs.time import time_from_nanoseconds, to_nanoseconds +from dimos.visualization.rerun.message_helpers import video_archetype PACKET = b"\x00\x00\x00\x01\x65payload" -def test_lcm_encode_decode() -> None: - original = CompressedVideo(data=PACKET, format="h264", frame_id="front_camera", ts=1234567890.5) - decoded = CompressedVideo.lcm_decode(original.lcm_encode()) +def test_cdr_encode_decode() -> None: + original = CompressedVideo( + data=PACKET, + format="h264", + frame_id="front_camera", + timestamp=time_from_nanoseconds(1234567890123456789), + ) + decoded = CompressedVideo.decode(original.encode()) - assert decoded.data.tobytes() == PACKET + assert bytes(decoded.data) == PACKET assert decoded.format == "h264" assert decoded.frame_id == "front_camera" - assert abs(decoded.ts - original.ts) < 1e-6 + assert to_nanoseconds(decoded.timestamp) == 1234567890123456789 -def test_resolves_over_the_raw_lcm_type() -> None: - """The rerun bridge decodes by type name — it must land on the to_rerun() class.""" - resolved = resolve_msg_type("foxglove_msgs.CompressedVideo") +def test_resolves_generated_canonical_type() -> None: + """Typed transports discover generated schema-bearing values.""" + resolved = resolve_msg_type("foxglove_msgs/msg/CompressedVideo") assert resolved is CompressedVideo def test_to_rerun_video_stream() -> None: - stream = CompressedVideo(data=PACKET, format="h264").to_rerun() + stream = video_archetype(CompressedVideo(data=PACKET, format="h264")) assert isinstance(stream, rr.VideoStream) assert bytes(stream.sample.as_arrow_array().to_pylist()[0]) == PACKET # type: ignore[union-attr] def test_to_rerun_unknown_codec() -> None: with pytest.raises(ValueError, match="mjpeg"): - CompressedVideo(data=PACKET, format="mjpeg").to_rerun() + video_archetype(CompressedVideo(data=PACKET, format="mjpeg")) diff --git a/dimos/msgs/geometry.py b/dimos/msgs/geometry.py new file mode 100644 index 00000000000..a5f007eb114 --- /dev/null +++ b/dimos/msgs/geometry.py @@ -0,0 +1,374 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Geometry operations on generated ROS value types.""" + +from collections.abc import Sequence +import math + +from dimos_generated.geometry_msgs.msg import ( + Point, + Pose, + PoseStamped, + Quaternion, + Transform, + TransformStamped, + Vector3, + Wrench, +) +from dimos_generated.nav_msgs.msg import Odometry +from dimos_generated.std_msgs.msg import Header +import numpy as np +from numpy.typing import NDArray +from scipy.spatial.transform import Rotation + + +def yaw(rotation: Quaternion) -> float: + """Return rotation about Z in radians (ROS XYZW quaternion convention).""" + return math.atan2( + 2 * (rotation.w * rotation.z + rotation.x * rotation.y), + 1 - 2 * (rotation.y * rotation.y + rotation.z * rotation.z), + ) + + +def transform_from_odometry(message: Odometry) -> TransformStamped: + """Copy the pose and declared frames into TF, preserving the exact source stamp.""" + position = message.pose.pose.position + return TransformStamped( + header=message.header, + child_frame_id=message.child_frame_id, + transform=Transform( + translation=Vector3(x=position.x, y=position.y, z=position.z), + rotation=message.pose.pose.orientation, + ), + ) + + +def quaternion_from_euler(roll: float, pitch: float, yaw: float) -> Quaternion: + """Convert fixed-axis XYZ angles in radians to a normalized ROS quaternion.""" + if not all(math.isfinite(value) for value in (roll, pitch, yaw)): + raise ValueError("Euler angles must be finite") + x, y, z, w = Rotation.from_euler("xyz", [roll, pitch, yaw]).as_quat() + return Quaternion(x=x, y=y, z=z, w=w) + + +def _rotation(quaternion: Quaternion) -> Rotation: + values = [quaternion.x, quaternion.y, quaternion.z, quaternion.w] + if not all(math.isfinite(value) for value in values): + raise ValueError("Quaternion components must be finite") + # SciPy rejects zero norm and normalizes non-unit input. + return Rotation.from_quat(values) + + +def _translation(transform: Transform) -> NDArray[np.float64]: + translation = transform.translation + values = np.array([translation.x, translation.y, translation.z], dtype=np.float64) + if not np.isfinite(values).all(): + raise ValueError("Translation components must be finite") + return values + + +def _transform(translation: NDArray[np.float64], rotation: Rotation) -> Transform: + x, y, z, w = rotation.as_quat() + return Transform( + translation=Vector3(x=translation[0], y=translation[1], z=translation[2]), + rotation=Quaternion(x=x, y=y, z=z, w=w), + ) + + +def transform_matrix(transform: Transform) -> NDArray[np.float64]: + """Copy a generated transform into a homogeneous 4-by-4 matrix.""" + matrix = np.eye(4, dtype=np.float64) + matrix[:3, :3] = _rotation(transform.rotation).as_matrix() + matrix[:3, 3] = _translation(transform) + return matrix + + +def compose_transforms(first: TransformStamped, second: TransformStamped) -> TransformStamped: + """Compose A←B and B←C, preserving A's exact source stamp in the A←C result. + + Inputs are copied into a new value. The shared frame must match; callers + querying a time-varying chain are responsible for choosing each edge's time. + """ + if first.child_frame_id != second.header.frame_id: + raise ValueError( + f"Cannot compose frames {first.child_frame_id!r} and {second.header.frame_id!r}" + ) + rotation = _rotation(first.transform.rotation) + translation = _translation(first.transform) + rotation.apply(_translation(second.transform)) + return TransformStamped( + header=first.header, + child_frame_id=second.child_frame_id, + transform=_transform(translation, rotation * _rotation(second.transform.rotation)), + ) + + +def inverse_transform(message: TransformStamped) -> TransformStamped: + """Return B←A from A←B, swapping frame names and preserving the exact stamp.""" + rotation = _rotation(message.transform.rotation).inv() + return TransformStamped( + header=Header(stamp=message.header.stamp, frame_id=message.child_frame_id), + child_frame_id=message.header.frame_id, + transform=_transform(-rotation.apply(_translation(message.transform)), rotation), + ) + + +def pose_from_transform(message: TransformStamped) -> PoseStamped: + """Copy the child frame's pose in the parent frame into a generated PoseStamped.""" + translation = message.transform.translation + return PoseStamped( + header=message.header, + pose=Pose( + position=Point(x=translation.x, y=translation.y, z=translation.z), + orientation=message.transform.rotation, + ), + ) + + +def transform_from_pose(message: PoseStamped, *, child_frame_id: str) -> TransformStamped: + """Copy a stamped pose into TF with an explicit child and unchanged source header.""" + if not isinstance(message, PoseStamped): + raise TypeError("Expected a generated PoseStamped") + position = message.pose.position + return TransformStamped( + header=message.header, + child_frame_id=child_frame_id, + transform=Transform( + translation=Vector3(x=position.x, y=position.y, z=position.z), + rotation=message.pose.orientation, + ), + ) + + +def pose_matrix(pose: Pose) -> NDArray[np.float64]: + """Copy a generated pose into a homogeneous matrix, rejecting invalid geometry.""" + values = np.array([pose.position.x, pose.position.y, pose.position.z], dtype=np.float64) + if not np.isfinite(values).all(): + raise ValueError("Position components must be finite") + result = np.eye(4, dtype=np.float64) + result[:3, :3] = _rotation(pose.orientation).as_matrix() + result[:3, 3] = values + return result + + +def quaternion_from_matrix(matrix: NDArray[np.float64]) -> Quaternion: + """Convert a finite proper 3-by-3 rotation matrix into a generated quaternion.""" + values = np.asarray(matrix, dtype=np.float64) + if values.shape != (3, 3) or not np.isfinite(values).all(): + raise ValueError("Rotation matrix must be finite and 3-by-3") + if not np.allclose(values.T @ values, np.eye(3), atol=1e-6, rtol=0.0) or not np.isclose( + np.linalg.det(values), 1.0, atol=1e-6, rtol=0.0 + ): + raise ValueError("Rotation matrix must be orthonormal with determinant +1") + x, y, z, w = Rotation.from_matrix(values).as_quat() + return Quaternion(x=x, y=y, z=z, w=w) + + +def pose_from_matrix(matrix: NDArray[np.float64]) -> Pose: + """Copy a finite rigid 4-by-4 transform into a generated pose.""" + values = np.asarray(matrix, dtype=np.float64) + if values.shape != (4, 4) or not np.isfinite(values).all(): + raise ValueError("Pose matrix must be finite and 4-by-4") + if not np.allclose(values[3], [0.0, 0.0, 0.0, 1.0], atol=1e-9, rtol=0.0): + raise ValueError("Pose matrix must have homogeneous last row [0, 0, 0, 1]") + return Pose( + position=Point(x=values[0, 3], y=values[1, 3], z=values[2, 3]), + orientation=quaternion_from_matrix(values[:3, :3]), + ) + + +def transform_from_matrix(matrix: NDArray[np.float64]) -> Transform: + """Copy a finite rigid homogeneous matrix into a generated transform.""" + pose = pose_from_matrix(matrix) + return Transform( + translation=Vector3(x=pose.position.x, y=pose.position.y, z=pose.position.z), + rotation=pose.orientation, + ) + + +def quaternion_euler(rotation: Quaternion) -> tuple[float, float, float]: + """Return fixed-axis XYZ Euler angles in radians for a generated quaternion.""" + roll, pitch, yaw = _rotation(rotation).as_euler("xyz") + return float(roll), float(pitch), float(yaw) + + +def translate_pose_local(pose: Pose, offset: Vector3) -> Pose: + """Copy a pose displaced by an offset expressed in its local axes.""" + matrix = pose_matrix(pose) + values = np.array([offset.x, offset.y, offset.z], dtype=np.float64) + if not np.isfinite(values).all(): + raise ValueError("Offset components must be finite") + delta = matrix[:3, :3] @ values + return Pose( + position=Point( + x=pose.position.x + delta[0], y=pose.position.y + delta[1], z=pose.position.z + delta[2] + ), + orientation=pose.orientation, + ) + + +def point_distance(first: Point, second: Point) -> float: + """Euclidean distance between generated position values.""" + return math.dist((first.x, first.y, first.z), (second.x, second.y, second.z)) + + +def quaternion_angle(first: Quaternion, second: Quaternion) -> float: + """Return the shortest angular separation in radians, normalizing inputs.""" + return float((_rotation(first).inv() * _rotation(second)).magnitude()) + + +def wrench_from_array(values: Sequence[float] | NDArray[np.float64]) -> Wrench: + """Copy force in N and torque in Nm from [fx, fy, fz, tx, ty, tz].""" + array = np.asarray(values, dtype=np.float64) + if array.shape != (6,): + raise ValueError("Expected 6 elements [fx, fy, fz, tx, ty, tz]") + return Wrench( + force=Vector3(x=array[0], y=array[1], z=array[2]), + torque=Vector3(x=array[3], y=array[4], z=array[5]), + ) + + +def wrench_array(message: Wrench) -> NDArray[np.float64]: + """Return an independent numeric force/torque array for arithmetic.""" + return np.array( + [ + message.force.x, + message.force.y, + message.force.z, + message.torque.x, + message.torque.y, + message.torque.z, + ], + dtype=np.float64, + ) + + +def vector_array(value: Vector3 | Point) -> NDArray[np.float64]: + """Copy a generated spatial vector/point into an independent XYZ array.""" + return np.array([value.x, value.y, value.z], dtype=np.float64) + + +def vector_from_array(values: Sequence[float] | NDArray[np.float64]) -> Vector3: + """Copy exactly three coordinates into a generated vector.""" + array = np.asarray(values, dtype=np.float64) + if array.shape != (3,): + raise ValueError("Expected 3 vector components") + return Vector3(x=array[0], y=array[1], z=array[2]) + + +def normalized_vector(value: Vector3) -> Vector3: + """Return a unit vector; zero length remains zero.""" + array = vector_array(value) + length = float(np.linalg.norm(array)) + return vector_from_array(array / length) if length else Vector3() + + +def quaternion_array(value: Quaternion) -> NDArray[np.float64]: + """Copy XYZW components without normalizing their declared values.""" + return np.array([value.x, value.y, value.z, value.w], dtype=np.float64) + + +def quaternion_from_array(values: Sequence[float] | NDArray[np.float64]) -> Quaternion: + """Copy exactly four XYZW components into a generated quaternion.""" + array = np.asarray(values, dtype=np.float64) + if array.shape != (4,): + raise ValueError("Quaternion requires exactly 4 components") + return Quaternion(x=array[0], y=array[1], z=array[2], w=array[3]) + + +def quaternion_product(first: Quaternion, second: Quaternion) -> Quaternion: + """Hamilton product, retaining non-unit magnitudes and multiplication order.""" + if not isinstance(first, Quaternion) or not isinstance(second, Quaternion): + raise TypeError("Cannot multiply Quaternion with a non-quaternion value") + a, b = quaternion_array(first), quaternion_array(second) + xyz = a[3] * b[:3] + b[3] * a[:3] + np.cross(a[:3], b[:3]) + return quaternion_from_array(np.append(xyz, a[3] * b[3] - np.dot(a[:3], b[:3]))) + + +def quaternion_conjugate(value: Quaternion) -> Quaternion: + """Negate the imaginary components, preserving the scalar component.""" + return Quaternion(x=-value.x, y=-value.y, z=-value.z, w=value.w) + + +def quaternion_inverse(value: Quaternion) -> Quaternion: + """Algebraic inverse, including non-unit quaternions.""" + array = quaternion_array(value) + squared = float(np.dot(array, array)) + if squared == 0: + raise ZeroDivisionError("Cannot invert zero quaternion") + return quaternion_from_array(quaternion_array(quaternion_conjugate(value)) / squared) + + +def normalized_quaternion(value: Quaternion) -> Quaternion: + """Return unit components without changing sign; reject zero norm.""" + array = quaternion_array(value) + length = float(np.linalg.norm(array)) + if length == 0: + raise ZeroDivisionError("Cannot normalize zero quaternion") + return quaternion_from_array(array / length) + + +def rotate_vector(rotation: Quaternion, value: Vector3) -> Vector3: + """Rotate an XYZ vector using a normalized spatial rotation.""" + return vector_from_array(_rotation(rotation).apply(vector_array(value))) + + +def point_from_array(values: Sequence[float] | NDArray[np.float64]) -> Point: + """Copy exactly three Cartesian coordinates into a generated point.""" + vector = vector_from_array(values) + return Point(x=vector.x, y=vector.y, z=vector.z) + + +def compose_poses(first: Pose, second: Pose) -> Pose: + """Compose parent/local poses without mutating inputs or normalizing stored quaternions.""" + offset = _rotation(first.orientation).apply(vector_array(second.position)) + return Pose( + position=point_from_array(vector_array(first.position) + offset), + orientation=quaternion_product(first.orientation, second.orientation), + ) + + +def compose_transform_values(first: Transform, second: Transform) -> Transform: + """Compose unframed spatial transforms in parent/local order.""" + if not isinstance(first, Transform) or not isinstance(second, Transform): + raise TypeError("Expected generated Transform values") + return Transform( + translation=vector_from_array( + vector_array(first.translation) + + _rotation(first.rotation).apply(vector_array(second.translation)) + ), + rotation=quaternion_product(first.rotation, second.rotation), + ) + + +def transform_pose_local(pose: Pose, transform: Transform) -> Pose: + """Apply an offset expressed in a pose's local axes.""" + return compose_poses( + pose, + Pose( + position=point_from_array(vector_array(transform.translation)), + orientation=transform.rotation, + ), + ) + + +def relative_pose_transform(first: Pose, second: Pose) -> Transform: + """Find the local transform taking the first pose to the second.""" + rotation = quaternion_inverse(first.orientation) + return Transform( + translation=vector_from_array( + _rotation(rotation).apply(vector_array(second.position) - vector_array(first.position)) + ), + rotation=quaternion_product(rotation, second.orientation), + ) diff --git a/dimos/msgs/geometry_msgs/Point.py b/dimos/msgs/geometry_msgs/Point.py deleted file mode 100644 index 84f260dc8a4..00000000000 --- a/dimos/msgs/geometry_msgs/Point.py +++ /dev/null @@ -1,33 +0,0 @@ -# Copyright 2025-2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from __future__ import annotations - -from dimos_lcm.geometry_msgs import Point as LCMPoint - - -class Point(LCMPoint): # type: ignore[misc] - """DimOS wrapper for geometry_msgs.Point (3D position). - - Inherits x/y/z from LCMPoint. Wire-identical to Vector3 but - semantically represents a position, not a direction/displacement. - """ - - msg_name = "geometry_msgs.Point" - - def __init__(self, x: float = 0.0, y: float = 0.0, z: float = 0.0) -> None: - super().__init__(float(x), float(y), float(z)) - - def __repr__(self) -> str: - return f"Point(x={self.x}, y={self.y}, z={self.z})" diff --git a/dimos/msgs/geometry_msgs/PointStamped.py b/dimos/msgs/geometry_msgs/PointStamped.py deleted file mode 100644 index 8d305d99493..00000000000 --- a/dimos/msgs/geometry_msgs/PointStamped.py +++ /dev/null @@ -1,102 +0,0 @@ -# Copyright 2025-2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from __future__ import annotations - -import time -from typing import TYPE_CHECKING, BinaryIO - -if TYPE_CHECKING: - from rerun._baseclasses import Archetype - - from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped - -from dimos_lcm.geometry_msgs import PointStamped as LCMPointStamped - -from dimos.msgs.geometry_msgs.Point import Point -from dimos.types.timestamped import Timestamped - - -class PointStamped(Point, Timestamped): - """A 3D point with timestamp and frame_id. - - Follows the same pattern as PoseStamped(Pose, Timestamped) and - TwistStamped(Twist, Timestamped). Inherits x/y/z from Point - (which inherits from LCMPoint). - """ - - msg_name = "geometry_msgs.PointStamped" - ts: float - frame_id: str - - def __init__( - self, - x: float = 0.0, - y: float = 0.0, - z: float = 0.0, - ts: float | None = None, - frame_id: str = "", - ) -> None: - self.frame_id = frame_id - self.ts = time.time() if ts is None else ts - super().__init__(float(x), float(y), float(z)) - - # -- LCM encode / decode -- - - def lcm_encode(self) -> bytes: - """Encode to LCM binary format.""" - lcm_msg = LCMPointStamped() - lcm_msg.point = self # Works because Point inherits from LCMPoint - [lcm_msg.header.stamp.sec, lcm_msg.header.stamp.nsec] = self.ros_timestamp() - lcm_msg.header.frame_id = self.frame_id - return lcm_msg.lcm_encode() # type: ignore[no-any-return] - - @classmethod - def lcm_decode(cls, data: bytes | BinaryIO) -> PointStamped: - """Decode from LCM binary format.""" - lcm_msg = LCMPointStamped.lcm_decode(data) - return cls( - x=lcm_msg.point.x, - y=lcm_msg.point.y, - z=lcm_msg.point.z, - ts=lcm_msg.header.stamp.sec + (lcm_msg.header.stamp.nsec / 1_000_000_000), - frame_id=lcm_msg.header.frame_id, - ) - - # -- Conversion methods -- - - def to_rerun(self) -> Archetype: - """Convert to rerun Points3D archetype for visualization.""" - import rerun as rr - - return rr.Points3D(positions=[[self.x, self.y, self.z]]) - - def to_pose_stamped(self) -> PoseStamped: - """Convert to PoseStamped with identity quaternion orientation.""" - from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped - - return PoseStamped( - ts=self.ts, - frame_id=self.frame_id, - position=[self.x, self.y, self.z], - orientation=[0.0, 0.0, 0.0, 1.0], - ) - - # -- String representations -- - - def __str__(self) -> str: - return f"PointStamped(point=[{self.x:.3f}, {self.y:.3f}, {self.z:.3f}], frame_id={self.frame_id!r})" - - def __repr__(self) -> str: - return f"PointStamped(x={self.x}, y={self.y}, z={self.z}, ts={self.ts}, frame_id={self.frame_id!r})" diff --git a/dimos/msgs/geometry_msgs/Pose.py b/dimos/msgs/geometry_msgs/Pose.py deleted file mode 100644 index 985a677c70f..00000000000 --- a/dimos/msgs/geometry_msgs/Pose.py +++ /dev/null @@ -1,265 +0,0 @@ -# Copyright 2025-2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from __future__ import annotations - -from typing import Any, TypeAlias, overload - -from dimos_lcm.geometry_msgs import ( - Pose as LCMPose, - Transform as LCMTransform, -) -from plum import dispatch - -from dimos.msgs.geometry_msgs.Quaternion import Quaternion, QuaternionConvertable -from dimos.msgs.geometry_msgs.Transform import Transform -from dimos.msgs.geometry_msgs.Vector3 import Vector3, VectorConvertable - -# Types that can be converted to/from Pose -PoseConvertable: TypeAlias = ( - tuple[VectorConvertable, QuaternionConvertable] - | LCMPose - | Vector3 - | dict[str, VectorConvertable | QuaternionConvertable] -) - - -def _is_position_orientation_pair(value: Any) -> bool: - """True for a 2-element (position, orientation) pair, false for a 2D position.""" - return ( - isinstance(value, tuple | list) - and len(value) == 2 - and not isinstance(value[0], int | float) - and not isinstance(value[1], int | float) - ) - - -class Pose(LCMPose): # type: ignore[misc] - position: Vector3 - orientation: Quaternion - msg_name = "geometry_msgs.Pose" - - @overload - def __init__(self) -> None: ... - - @overload - def __init__(self, x: int | float, y: int | float, z: int | float) -> None: ... - - @overload - def __init__( - self, - x: int | float, - y: int | float, - z: int | float, - qx: int | float, - qy: int | float, - qz: int | float, - qw: int | float, - ) -> None: ... - - @overload - def __init__( - self, - position: VectorConvertable | Vector3 | None = ..., - orientation: QuaternionConvertable | Quaternion | None = ..., - ) -> None: ... - - @overload - def __init__(self, value: PoseConvertable | Pose, /) -> None: ... - - def __init__(self, *args: Any, **kwargs: Any) -> None: - """Initialize a pose. - - Supported forms: - Pose() # origin, identity orientation - Pose(x, y, z) - Pose(x, y, z, qx, qy, qz, qw) - Pose(position) # any Vector3-convertable - Pose(position, orientation) - Pose(position=..., orientation=...) # keyword args - Pose((position, orientation)) # pair - Pose({"position": ..., "orientation": ...}) - Pose(other_pose) # copy constructor - Pose(lcm_pose) # from LCM message - """ - position: Any = None - orientation: Any = None - - if len(args) == 3: - position = args - elif len(args) == 7: - position, orientation = args[:3], args[3:] - elif len(args) == 2: - position, orientation = args - elif len(args) == 1: - value = args[0] - # Pose before LCMPose (it is a subclass); dict and pair before the - # generic "it is a position" fallback. - if isinstance(value, Pose): - position, orientation = value.position, value.orientation - elif isinstance(value, LCMPose): - position = (value.position.x, value.position.y, value.position.z) - orientation = ( - value.orientation.x, - value.orientation.y, - value.orientation.z, - value.orientation.w, - ) - elif isinstance(value, dict): - position, orientation = value["position"], value["orientation"] - elif _is_position_orientation_pair(value): - position, orientation = value - else: - position = value - elif args: - raise TypeError(f"Pose takes 1, 2, 3 or 7 positional arguments ({len(args)} given)") - - if kwargs: - position = kwargs.pop("position", position) - orientation = kwargs.pop("orientation", orientation) - if kwargs: - raise TypeError(f"Pose got unexpected keyword arguments {sorted(kwargs)}") - - self.position = Vector3(0.0, 0.0, 0.0) if position is None else Vector3(position) - self.orientation = ( - Quaternion(0.0, 0.0, 0.0, 1.0) if orientation is None else Quaternion(orientation) - ) - - @property - def x(self) -> float: - """X coordinate of position.""" - return self.position.x - - @property - def y(self) -> float: - """Y coordinate of position.""" - return self.position.y - - @property - def z(self) -> float: - """Z coordinate of position.""" - return self.position.z - - @property - def roll(self) -> float: - """Roll angle in radians.""" - return self.orientation.to_euler().roll - - @property - def pitch(self) -> float: - """Pitch angle in radians.""" - return self.orientation.to_euler().pitch - - @property - def yaw(self) -> float: - """Yaw angle in radians.""" - return self.orientation.to_euler().yaw - - def __repr__(self) -> str: - return f"Pose(position={self.position!r}, orientation={self.orientation!r})" - - def __str__(self) -> str: - return ( - f"Pose(pos=[{self.x:.3f}, {self.y:.3f}, {self.z:.3f}], " - f"euler=[{self.roll:.3f}, {self.pitch:.3f}, {self.yaw:.3f}]), " - f"quaternion=[{self.orientation}])" - ) - - def __eq__(self, other) -> bool: # type: ignore[no-untyped-def] - """Check if two poses are equal.""" - if not isinstance(other, Pose): - return False - return self.position == other.position and self.orientation == other.orientation - - def __matmul__(self, transform: LCMTransform | Transform) -> Pose: - return self + transform - - def __add__(self, other: Pose | PoseConvertable | LCMTransform | Transform) -> Pose: - """Compose two poses or apply a transform (transform composition). - - The operation self + other represents applying transformation 'other' - in the coordinate frame defined by 'self'. This is equivalent to: - - First apply transformation 'self' (from world to self's frame) - - Then apply transformation 'other' (from self's frame to other's frame) - - This matches ROS tf convention where: - T_world_to_other = T_world_to_self * T_self_to_other - - Args: - other: The pose or transform to compose with this one - - Returns: - A new Pose representing the composed transformation - - Example: - robot_pose = Pose(1, 0, 0) # Robot at (1,0,0) facing forward - object_in_robot = Pose(2, 0, 0) # Object 2m in front of robot - object_in_world = robot_pose + object_in_robot # Object at (3,0,0) in world - - # Or with a Transform: - transform = Transform() - transform.translation = Vector3(2, 0, 0) - transform.rotation = Quaternion(0, 0, 0, 1) - new_pose = pose + transform - """ - # Handle Transform objects - if isinstance(other, LCMTransform | Transform): - # Convert Transform to Pose using its translation and rotation - other_position = Vector3(other.translation) - other_orientation = Quaternion(other.rotation) - elif isinstance(other, Pose): - other_position = other.position - other_orientation = other.orientation - else: - # Convert to Pose if it's a convertible type - other_pose = Pose(other) - other_position = other_pose.position - other_orientation = other_pose.orientation - - # Compose orientations: self.orientation * other.orientation - new_orientation = self.orientation * other_orientation - - # Transform other's position by self's orientation, then add to self's position - rotated_position = self.orientation.rotate_vector(other_position) - new_position = self.position + rotated_position - - return Pose(new_position, new_orientation) - - def __sub__(self, other: Pose) -> Pose: - """Compute the delta pose: self - other. - - For position: simple subtraction. - For orientation: delta_quat = self.orientation * inverse(other.orientation) - - Returns: - A new Pose representing the delta transformation - """ - delta_position = self.position - other.position - delta_orientation = self.orientation * other.orientation.inverse() - return Pose(delta_position, delta_orientation) - - -@dispatch -def to_pose(value: Pose) -> Pose: - """Pass through Pose objects.""" - return value - - -@dispatch # type: ignore[no-redef] -def to_pose(value: PoseConvertable) -> Pose: - """Convert a pose-compatible value to a Pose object.""" - return Pose(value) - - -PoseLike: TypeAlias = PoseConvertable | Pose diff --git a/dimos/msgs/geometry_msgs/PoseArray.py b/dimos/msgs/geometry_msgs/PoseArray.py deleted file mode 100644 index e27f56b6bfd..00000000000 --- a/dimos/msgs/geometry_msgs/PoseArray.py +++ /dev/null @@ -1,97 +0,0 @@ -# Copyright 2025-2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -"""PoseArray message type for Dimos.""" - -from __future__ import annotations - -from typing import TYPE_CHECKING - -from dimos.msgs.std_msgs.Header import Header - -if TYPE_CHECKING: - from collections.abc import Iterator - - from dimos.msgs.geometry_msgs.Pose import Pose - - -class PoseArray: - """ - An array of poses with a header for reference frame and timestamp. - - This is commonly used for representing multiple candidate positions, - such as grasp poses, particle filter samples, or waypoints. - """ - - msg_name = "geometry_msgs.PoseArray" - - def __init__(self, header: Header | None = None, poses: list[Pose] | None = None) -> None: - """ - Initialize a PoseArray. - - Args: - header: Header with frame_id and timestamp - poses: List of Pose objects - """ - self.header = header if header is not None else Header() - self.poses = poses if poses is not None else [] - - def __repr__(self) -> str: - return f"PoseArray(header={self.header!r}, poses={len(self.poses)} poses)" - - def __str__(self) -> str: - return f"PoseArray(frame_id={self.header.frame_id}, num_poses={len(self.poses)})" - - def __len__(self) -> int: - """Return the number of poses in the array.""" - return len(self.poses) - - def __getitem__(self, index: int) -> Pose: - """Get pose at index.""" - return self.poses[index] - - def __iter__(self) -> Iterator[Pose]: - """Iterate over poses.""" - return iter(self.poses) - - def append(self, pose: Pose) -> None: - """Add a pose to the array.""" - self.poses.append(pose) - - def encode(self) -> bytes: - """ - Encode to bytes for LCM transmission. - - Note: This is a simple implementation. For production use, - consider using proper LCM encoding. - """ - import pickle - - return pickle.dumps({"header": self.header, "poses": self.poses}) - - @classmethod - def decode(cls, data: bytes) -> PoseArray: - """ - Decode from bytes. - - Args: - data: Pickled PoseArray data - - Returns: - Decoded PoseArray - """ - import pickle - - decoded = pickle.loads(data) - return cls(header=decoded["header"], poses=decoded["poses"]) diff --git a/dimos/msgs/geometry_msgs/PoseStamped.py b/dimos/msgs/geometry_msgs/PoseStamped.py deleted file mode 100644 index a78adadd19b..00000000000 --- a/dimos/msgs/geometry_msgs/PoseStamped.py +++ /dev/null @@ -1,155 +0,0 @@ -# Copyright 2025-2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from __future__ import annotations - -import math -import time -from typing import TYPE_CHECKING, Any, BinaryIO, TypeAlias - -if TYPE_CHECKING: - from rerun._baseclasses import Archetype - -from dimos_lcm.geometry_msgs import PoseStamped as LCMPoseStamped - -from dimos.msgs.geometry_msgs.Pose import Pose -from dimos.msgs.geometry_msgs.Quaternion import Quaternion, QuaternionConvertable -from dimos.msgs.geometry_msgs.Transform import Transform -from dimos.msgs.geometry_msgs.Vector3 import Vector3, VectorConvertable -from dimos.types.timestamped import Timestamped - -# Types that can be converted to/from Pose -PoseConvertable: TypeAlias = ( - tuple[VectorConvertable, QuaternionConvertable] - | LCMPoseStamped - | dict[str, VectorConvertable | QuaternionConvertable] -) - - -def sec_nsec(ts): # type: ignore[no-untyped-def] - s = int(ts) - return [s, int((ts - s) * 1_000_000_000)] - - -class PoseStamped(Pose, Timestamped): - msg_name = "geometry_msgs.PoseStamped" - ts: float - frame_id: str - - def __init__(self, *args: Any, **kwargs: Any) -> None: - """Initialize a stamped pose. - - Takes ``(ts, frame_id)`` positionally, plus every Pose keyword. Any other - positional form belongs to Pose, so ``PoseStamped(x, y, z)`` and - ``PoseStamped(x, y, z, qx, qy, qz, qw)`` work as they do on the base class. - """ - ts: Any - # A leading number or string is a timestamp; anything else is a Pose argument. - if args and len(args) < 3 and isinstance(args[0], int | float | str): - ts = args[0] - frame_id = args[1] if len(args) > 1 else kwargs.pop("frame_id", "") - pose_args: tuple[Any, ...] = () - else: - ts = kwargs.pop("ts", None) - frame_id = kwargs.pop("frame_id", "") - pose_args = args - - self.frame_id = frame_id - self.ts = time.time() if ts is None else ts - super().__init__(*pose_args, **kwargs) - - def lcm_encode(self) -> bytes: - lcm_mgs = LCMPoseStamped() - lcm_mgs.pose = self - [lcm_mgs.header.stamp.sec, lcm_mgs.header.stamp.nsec] = sec_nsec(self.ts) # type: ignore[no-untyped-call] - lcm_mgs.header.frame_id = self.frame_id - return lcm_mgs.lcm_encode() # type: ignore[no-any-return] - - @classmethod - def lcm_decode(cls, data: bytes | BinaryIO) -> PoseStamped: - lcm_msg = LCMPoseStamped.lcm_decode(data) - return cls( - ts=lcm_msg.header.stamp.sec + (lcm_msg.header.stamp.nsec / 1_000_000_000), - frame_id=lcm_msg.header.frame_id, - position=[lcm_msg.pose.position.x, lcm_msg.pose.position.y, lcm_msg.pose.position.z], - orientation=[ - lcm_msg.pose.orientation.x, - lcm_msg.pose.orientation.y, - lcm_msg.pose.orientation.z, - lcm_msg.pose.orientation.w, - ], - ) - - def __str__(self) -> str: - return ( - f"PoseStamped(pos=[{self.x:.3f}, {self.y:.3f}, {self.z:.3f}], " - f"euler=[{math.degrees(self.roll):.1f}, {math.degrees(self.pitch):.1f}, {math.degrees(self.yaw):.1f}])" - ) - - def to_rerun(self) -> Archetype: - """Convert to rerun Transform3D format. - - Returns a Transform3D that can be logged to Rerun to position - child entities in the transform hierarchy. - """ - import rerun as rr - - return rr.Transform3D( - translation=[self.x, self.y, self.z], - rotation=rr.Quaternion( - xyzw=[ - self.orientation.x, - self.orientation.y, - self.orientation.z, - self.orientation.w, - ] - ), - ) - - def to_rerun_arrow(self, length: float = 0.5): # type: ignore[no-untyped-def] - """Convert to rerun Arrows3D format for visualization.""" - import rerun as rr - - origin = [[self.x, self.y, self.z]] - forward = self.orientation.rotate_vector(Vector3(length, 0, 0)) - vector = [[forward.x, forward.y, forward.z]] - return rr.Arrows3D(origins=origin, vectors=vector) - - def new_transform_to(self, name: str) -> Transform: - return self.find_transform( - PoseStamped( - frame_id=name, - position=Vector3(0, 0, 0), - orientation=Quaternion(0, 0, 0, 1), # Identity quaternion - ) - ) - - def new_transform_from(self, name: str) -> Transform: - return self.new_transform_to(name).inverse() - - def find_transform(self, other: PoseStamped) -> Transform: - inv_orientation = self.orientation.conjugate() - - pos_diff = other.position - self.position - - local_translation = inv_orientation.rotate_vector(pos_diff) - - relative_rotation = inv_orientation * other.orientation - - return Transform( - child_frame_id=other.frame_id, - frame_id=self.frame_id, - translation=local_translation, - rotation=relative_rotation, - ) diff --git a/dimos/msgs/geometry_msgs/PoseWithCovariance.py b/dimos/msgs/geometry_msgs/PoseWithCovariance.py deleted file mode 100644 index f26a987bee1..00000000000 --- a/dimos/msgs/geometry_msgs/PoseWithCovariance.py +++ /dev/null @@ -1,201 +0,0 @@ -# Copyright 2025-2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from __future__ import annotations - -from typing import TYPE_CHECKING, Any, TypeAlias - -from dimos_lcm.geometry_msgs import ( - PoseWithCovariance as LCMPoseWithCovariance, -) -import numpy as np - -from dimos.msgs.geometry_msgs.Pose import Pose, PoseConvertable - -if TYPE_CHECKING: - from dimos.msgs.geometry_msgs.Quaternion import Quaternion - from dimos.msgs.geometry_msgs.Vector3 import Vector3 - -# Types that can be converted to/from PoseWithCovariance -PoseWithCovarianceConvertable: TypeAlias = ( - tuple[PoseConvertable, list[float] | np.ndarray] - | LCMPoseWithCovariance - | dict[str, PoseConvertable | list[float] | np.ndarray] -) - -COVARIANCE_SIZE = 36 - - -def _is_value_covariance_pair(value: Any) -> bool: - """True for a 2-element (value, covariance) pair, where covariance is 36 long.""" - if not isinstance(value, tuple | list) or len(value) != 2: - return False - covariance = value[1] - return isinstance(covariance, list | np.ndarray) and np.size(covariance) == COVARIANCE_SIZE - - -class PoseWithCovariance(LCMPoseWithCovariance): # type: ignore[misc] - pose: Pose - covariance: np.ndarray[tuple[int], np.dtype[np.floating[Any]]] - msg_name = "geometry_msgs.PoseWithCovariance" - - def __init__( - self, - pose: Pose | PoseConvertable | PoseWithCovarianceConvertable | None = None, - covariance: list[float] | np.ndarray | None = None, - ) -> None: - """Initialize a pose with covariance. - - Supported forms: - PoseWithCovariance() # origin, zero covariance - PoseWithCovariance(pose) - PoseWithCovariance(pose, covariance) - PoseWithCovariance(pose=..., covariance=...) - PoseWithCovariance((pose, covariance)) # pair - PoseWithCovariance({"pose": ..., "covariance": ...}) - PoseWithCovariance(other) # copy constructor - PoseWithCovariance(lcm_pose_with_cov) # from LCM message - """ - source: Any = pose - cov: Any = covariance - - # PoseWithCovariance before LCMPoseWithCovariance (it is a subclass). - if isinstance(source, PoseWithCovariance): - if cov is None: - cov = np.array(source.covariance).copy() - source = Pose(source.pose) # copy constructor: don't alias the source pose - elif isinstance(source, LCMPoseWithCovariance): - if cov is None: - cov = np.array(source.covariance) - source = source.pose - elif isinstance(source, dict) and "pose" in source: - cov = source.get("covariance", cov) - source = source["pose"] - elif _is_value_covariance_pair(source): - source, cov = source - - self.pose = source if isinstance(source, Pose) else Pose(source) - self.covariance = ( - np.zeros(COVARIANCE_SIZE) - if cov is None - else np.array(cov, dtype=float).reshape(COVARIANCE_SIZE) - ) - - def __getattribute__(self, name: str): # type: ignore[no-untyped-def] - """Override to ensure covariance is always returned as numpy array.""" - if name == "covariance": - cov = object.__getattribute__(self, "covariance") - if not isinstance(cov, np.ndarray): - return np.array(cov, dtype=float) - return cov - return super().__getattribute__(name) - - def __setattr__(self, name: str, value) -> None: # type: ignore[no-untyped-def] - """Override to ensure covariance is stored as numpy array.""" - if name == "covariance": - if not isinstance(value, np.ndarray): - value = np.array(value, dtype=float).reshape(36) - super().__setattr__(name, value) - - @property - def x(self) -> float: - """X coordinate of position.""" - return self.pose.x - - @property - def y(self) -> float: - """Y coordinate of position.""" - return self.pose.y - - @property - def z(self) -> float: - """Z coordinate of position.""" - return self.pose.z - - @property - def position(self) -> Vector3: - """Position vector.""" - return self.pose.position - - @property - def orientation(self) -> Quaternion: - """Orientation quaternion.""" - return self.pose.orientation - - @property - def roll(self) -> float: - """Roll angle in radians.""" - return self.pose.roll - - @property - def pitch(self) -> float: - """Pitch angle in radians.""" - return self.pose.pitch - - @property - def yaw(self) -> float: - """Yaw angle in radians.""" - return self.pose.yaw - - @property - def covariance_matrix(self) -> np.ndarray: - """Get covariance as 6x6 matrix.""" - return self.covariance.reshape(6, 6) - - @covariance_matrix.setter - def covariance_matrix(self, value: np.ndarray) -> None: - """Set covariance from 6x6 matrix.""" - self.covariance = np.array(value).reshape(36) - - def __repr__(self) -> str: - return f"PoseWithCovariance(pose={self.pose!r}, covariance=<{self.covariance.shape[0] if isinstance(self.covariance, np.ndarray) else len(self.covariance)} elements>)" - - def __str__(self) -> str: - return ( - f"PoseWithCovariance(pos=[{self.x:.3f}, {self.y:.3f}, {self.z:.3f}], " - f"euler=[{self.roll:.3f}, {self.pitch:.3f}, {self.yaw:.3f}], " - f"cov_trace={np.trace(self.covariance_matrix):.3f})" - ) - - def __eq__(self, other) -> bool: # type: ignore[no-untyped-def] - """Check if two PoseWithCovariance are equal.""" - if not isinstance(other, PoseWithCovariance): - return False - return self.pose == other.pose and np.allclose(self.covariance, other.covariance) - - def lcm_encode(self) -> bytes: - """Encode to LCM binary format.""" - lcm_msg = LCMPoseWithCovariance() - lcm_msg.pose = self.pose - # LCM expects list, not numpy array - if isinstance(self.covariance, np.ndarray): - lcm_msg.covariance = self.covariance.tolist() - else: - lcm_msg.covariance = list(self.covariance) - return lcm_msg.lcm_encode() # type: ignore[no-any-return] - - @classmethod - def lcm_decode(cls, data: bytes) -> PoseWithCovariance: - """Decode from LCM binary format.""" - lcm_msg = LCMPoseWithCovariance.lcm_decode(data) - pose = Pose( - position=[lcm_msg.pose.position.x, lcm_msg.pose.position.y, lcm_msg.pose.position.z], - orientation=[ - lcm_msg.pose.orientation.x, - lcm_msg.pose.orientation.y, - lcm_msg.pose.orientation.z, - lcm_msg.pose.orientation.w, - ], - ) - return cls(pose, lcm_msg.covariance) diff --git a/dimos/msgs/geometry_msgs/PoseWithCovarianceStamped.py b/dimos/msgs/geometry_msgs/PoseWithCovarianceStamped.py deleted file mode 100644 index 17f3f2b6d41..00000000000 --- a/dimos/msgs/geometry_msgs/PoseWithCovarianceStamped.py +++ /dev/null @@ -1,98 +0,0 @@ -# Copyright 2025-2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from __future__ import annotations - -import time -from typing import TypeAlias - -from dimos_lcm.geometry_msgs import ( - PoseWithCovarianceStamped as LCMPoseWithCovarianceStamped, -) -import numpy as np - -from dimos.msgs.geometry_msgs.Pose import Pose, PoseConvertable -from dimos.msgs.geometry_msgs.PoseWithCovariance import PoseWithCovariance -from dimos.types.timestamped import Timestamped - -# Types that can be converted to/from PoseWithCovarianceStamped -PoseWithCovarianceStampedConvertable: TypeAlias = ( - tuple[PoseConvertable, list[float] | np.ndarray] - | LCMPoseWithCovarianceStamped - | dict[str, PoseConvertable | list[float] | np.ndarray | float | str] -) - - -def sec_nsec(ts): # type: ignore[no-untyped-def] - s = int(ts) - return [s, int((ts - s) * 1_000_000_000)] - - -class PoseWithCovarianceStamped(PoseWithCovariance, Timestamped): - msg_name = "geometry_msgs.PoseWithCovarianceStamped" - ts: float - frame_id: str - - def __init__( - self, - ts: float | None = None, - frame_id: str = "", - pose: Pose | PoseConvertable | None = None, - covariance: list[float] | np.ndarray | None = None, - ) -> None: - """Initialize with timestamp, frame_id, pose and covariance.""" - self.frame_id = frame_id - self.ts = time.time() if ts is None else ts - super().__init__(pose, covariance) - - def lcm_encode(self) -> bytes: - lcm_msg = LCMPoseWithCovarianceStamped() - lcm_msg.pose.pose = self.pose - # LCM expects list, not numpy array - if isinstance(self.covariance, np.ndarray): - lcm_msg.pose.covariance = self.covariance.tolist() - else: - lcm_msg.pose.covariance = list(self.covariance) - [lcm_msg.header.stamp.sec, lcm_msg.header.stamp.nsec] = sec_nsec(self.ts) # type: ignore[no-untyped-call] - lcm_msg.header.frame_id = self.frame_id - return lcm_msg.lcm_encode() # type: ignore[no-any-return] - - @classmethod - def lcm_decode(cls, data: bytes) -> PoseWithCovarianceStamped: - lcm_msg = LCMPoseWithCovarianceStamped.lcm_decode(data) - return cls( - ts=lcm_msg.header.stamp.sec + (lcm_msg.header.stamp.nsec / 1_000_000_000), - frame_id=lcm_msg.header.frame_id, - pose=Pose( - position=[ - lcm_msg.pose.pose.position.x, - lcm_msg.pose.pose.position.y, - lcm_msg.pose.pose.position.z, - ], - orientation=[ - lcm_msg.pose.pose.orientation.x, - lcm_msg.pose.pose.orientation.y, - lcm_msg.pose.pose.orientation.z, - lcm_msg.pose.pose.orientation.w, - ], - ), - covariance=lcm_msg.pose.covariance, - ) - - def __str__(self) -> str: - return ( - f"PoseWithCovarianceStamped(pos=[{self.x:.3f}, {self.y:.3f}, {self.z:.3f}], " - f"euler=[{self.roll:.3f}, {self.pitch:.3f}, {self.yaw:.3f}], " - f"cov_trace={np.trace(self.covariance_matrix):.3f})" - ) diff --git a/dimos/msgs/geometry_msgs/Quaternion.py b/dimos/msgs/geometry_msgs/Quaternion.py deleted file mode 100644 index e782bc71142..00000000000 --- a/dimos/msgs/geometry_msgs/Quaternion.py +++ /dev/null @@ -1,325 +0,0 @@ -# Copyright 2025-2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from __future__ import annotations - -from collections.abc import Sequence -from io import BytesIO -import math -import struct -from typing import TYPE_CHECKING, Any, BinaryIO, TypeAlias, overload - -if TYPE_CHECKING: - import rerun as rr - -from dimos_lcm.geometry_msgs import Quaternion as LCMQuaternion -import numpy as np - -from dimos.msgs.geometry_msgs.Vector3 import Vector3 - -# Types that can be converted to/from Quaternion -QuaternionConvertable: TypeAlias = Sequence[int | float] | LCMQuaternion | np.ndarray - - -def _four_components(value: Any) -> tuple[float, float, float, float]: - """Unpack a length-4 sequence or array into (x, y, z, w).""" - if not isinstance(value, np.ndarray | Sequence): - raise TypeError(f"Cannot initialize Quaternion from {type(value)}") - size = value.size if isinstance(value, np.ndarray) else len(value) - if size != 4: - raise ValueError("Quaternion requires exactly 4 components [x, y, z, w]") - return (float(value[0]), float(value[1]), float(value[2]), float(value[3])) - - -class Quaternion(LCMQuaternion): # type: ignore[misc] - x: float = 0.0 - y: float = 0.0 - z: float = 0.0 - w: float = 1.0 - msg_name = "geometry_msgs.Quaternion" - - @classmethod - def lcm_decode(cls, data: bytes | BinaryIO): # type: ignore[no-untyped-def] - if not hasattr(data, "read"): - data = BytesIO(data) - if data.read(8) != cls._get_packed_fingerprint(): - raise ValueError("Decode error") - return cls._lcm_decode_one(data) # type: ignore[no-untyped-call] - - @classmethod - def _lcm_decode_one(cls, buf): # type: ignore[no-untyped-def] - return cls(struct.unpack(">dddd", buf.read(32))) - - @overload - def __init__(self) -> None: ... - - @overload - def __init__( - self, - x: int | float = ..., - y: int | float = ..., - z: int | float = ..., - w: int | float = ..., - ) -> None: ... - - @overload - def __init__(self, value: QuaternionConvertable | Quaternion, /) -> None: ... - - def __init__(self, *args: Any, **kwargs: Any) -> None: - """Initialize a quaternion. - - Supported forms: - Quaternion() # identity (0, 0, 0, 1) - Quaternion(x, y, z, w) - Quaternion(x=1, y=2, z=3, w=4) # keyword args - Quaternion([x, y, z, w]) # sequence - Quaternion(np.array([x, y, z, w])) # numpy array - Quaternion(other_quaternion) # copy constructor - Quaternion(lcm_quaternion) # from LCM message - """ - if len(args) == 4: - self.x = float(args[0]) - self.y = float(args[1]) - self.z = float(args[2]) - self.w = float(args[3]) - elif len(args) == 1: - value = args[0] - # Quaternion before LCMQuaternion (it is a subclass) and before the - # generic sequence branch (a Quaternion is indexable). - if isinstance(value, Quaternion): - self.x, self.y, self.z, self.w = value.x, value.y, value.z, value.w - elif isinstance(value, LCMQuaternion): - self.x, self.y, self.z, self.w = value.x, value.y, value.z, value.w - else: - self.x, self.y, self.z, self.w = _four_components(value) - elif args: - raise TypeError( - f"Quaternion takes 1 sequence or 4 components ({len(args)} positional given)" - ) - elif kwargs: - self.x = float(kwargs.pop("x", 0.0)) - self.y = float(kwargs.pop("y", 0.0)) - self.z = float(kwargs.pop("z", 0.0)) - self.w = float(kwargs.pop("w", 1.0)) - # else: no arguments — the class defaults already spell the identity. - - if kwargs: - raise TypeError(f"Quaternion got unexpected keyword arguments {sorted(kwargs)}") - - def to_tuple(self) -> tuple[float, float, float, float]: - """Tuple representation of the quaternion (x, y, z, w).""" - return (self.x, self.y, self.z, self.w) - - def to_list(self) -> list[float]: - """List representation of the quaternion (x, y, z, w).""" - return [self.x, self.y, self.z, self.w] - - def to_numpy(self) -> np.ndarray: - """Numpy array representation of the quaternion (x, y, z, w).""" - return np.array([self.x, self.y, self.z, self.w]) - - @property - def euler(self) -> Vector3: - return self.to_euler() - - @property - def radians(self) -> Vector3: - return self.to_euler() - - def to_radians(self) -> Vector3: - """Radians representation of the quaternion (x, y, z, w).""" - return self.to_euler() - - def to_rerun(self) -> rr.Quaternion: - import rerun as rr - - return rr.Quaternion(xyzw=[self.x, self.y, self.z, self.w]) - - @classmethod - def from_euler(cls, vector: Vector3) -> Quaternion: - """Convert Euler angles (roll, pitch, yaw) in radians to quaternion. - - Args: - vector: Vector3 containing (roll, pitch, yaw) in radians - - Returns: - Quaternion representation - """ - - # Calculate quaternion components - cy = np.cos(vector.yaw * 0.5) - sy = np.sin(vector.yaw * 0.5) - cp = np.cos(vector.pitch * 0.5) - sp = np.sin(vector.pitch * 0.5) - cr = np.cos(vector.roll * 0.5) - sr = np.sin(vector.roll * 0.5) - - w = cr * cp * cy + sr * sp * sy - x = sr * cp * cy - cr * sp * sy - y = cr * sp * cy + sr * cp * sy - z = cr * cp * sy - sr * sp * cy - - return cls(x, y, z, w) - - @classmethod - def from_rotation_matrix(cls, matrix: np.ndarray) -> Quaternion: - """Convert a 3x3 rotation matrix to quaternion. - - Args: - matrix: 3x3 rotation matrix (numpy array) - - Returns: - Quaternion representation - """ - from scipy.spatial.transform import ( - Rotation, # ~330ms: deferred to avoid startup cost - ) - - rotation = Rotation.from_matrix(matrix) - quat = rotation.as_quat() # Returns [x, y, z, w] - return cls(quat[0], quat[1], quat[2], quat[3]) - - def to_rotation_matrix(self) -> np.ndarray: - """Convert quaternion to a 3x3 rotation matrix.""" - from scipy.spatial.transform import ( - Rotation, # ~330ms: deferred to avoid startup cost - ) - - return np.asarray(Rotation.from_quat([self.x, self.y, self.z, self.w]).as_matrix()) - - def to_euler(self) -> Vector3: - """Convert quaternion to Euler angles (roll, pitch, yaw) in radians. - - Returns: - Vector3: Euler angles as (roll, pitch, yaw) in radians - """ - # Use scipy for accurate quaternion to euler conversion - from scipy.spatial.transform import ( - Rotation, # ~330ms: deferred to avoid startup cost - ) - - quat = [self.x, self.y, self.z, self.w] - rotation = Rotation.from_quat(quat) - euler_angles = rotation.as_euler("xyz") # roll, pitch, yaw - - return Vector3(euler_angles[0], euler_angles[1], euler_angles[2]) - - def __getitem__(self, idx: int) -> float: - """Allow indexing into quaternion components: 0=x, 1=y, 2=z, 3=w.""" - if idx == 0: - return self.x - elif idx == 1: - return self.y - elif idx == 2: - return self.z - elif idx == 3: - return self.w - else: - raise IndexError(f"Quaternion index {idx} out of range [0-3]") - - def __repr__(self) -> str: - return f"Quaternion({self.x:.6f}, {self.y:.6f}, {self.z:.6f}, {self.w:.6f})" - - def __str__(self) -> str: - return self.__repr__() - - def __eq__(self, other) -> bool: # type: ignore[no-untyped-def] - if not isinstance(other, Quaternion): - return False - return self.x == other.x and self.y == other.y and self.z == other.z and self.w == other.w - - def is_zero(self) -> bool: - """All components are zero — i.e. an uninitialized placeholder, not a valid rotation.""" - return self.x == 0.0 and self.y == 0.0 and self.z == 0.0 and self.w == 0.0 - - def __mul__(self, other: Quaternion) -> Quaternion: - """Multiply two quaternions (Hamilton product). - - The result represents the composition of rotations: - q1 * q2 represents rotating by q2 first, then by q1. - """ - if not isinstance(other, Quaternion): - raise TypeError(f"Cannot multiply Quaternion with {type(other)}") - - # Hamilton product formula - w = self.w * other.w - self.x * other.x - self.y * other.y - self.z * other.z - x = self.w * other.x + self.x * other.w + self.y * other.z - self.z * other.y - y = self.w * other.y - self.x * other.z + self.y * other.w + self.z * other.x - z = self.w * other.z + self.x * other.y - self.y * other.x + self.z * other.w - - return Quaternion(x, y, z, w) - - def dot(self, other: Quaternion) -> float: - return float(self.x * other.x + self.y * other.y + self.z * other.z + self.w * other.w) - - def angle_to(self, other: Quaternion) -> float: - """Smallest rotation angle (radians) between two unit quaternions. - - ``abs(self.dot(other))`` collapses the double-cover sign ambiguity; - the ``min(1.0, ...)`` clamps against numerical drift past 1. - """ - return 2.0 * math.acos(min(1.0, abs(self.dot(other)))) - - def conjugate(self) -> Quaternion: - """Return the conjugate of the quaternion. - - For unit quaternions, the conjugate represents the inverse rotation. - """ - return Quaternion(-self.x, -self.y, -self.z, self.w) - - def inverse(self) -> Quaternion: - """Return the inverse of the quaternion. - - For unit quaternions, this is equivalent to the conjugate. - For non-unit quaternions, this is conjugate / norm^2. - """ - norm_sq = self.x**2 + self.y**2 + self.z**2 + self.w**2 - if norm_sq == 0: - raise ZeroDivisionError("Cannot invert zero quaternion") - - # For unit quaternions (norm_sq ≈ 1), this simplifies to conjugate - if np.isclose(norm_sq, 1.0): - return self.conjugate() - - # For non-unit quaternions - conj = self.conjugate() - return Quaternion(conj.x / norm_sq, conj.y / norm_sq, conj.z / norm_sq, conj.w / norm_sq) - - def normalize(self) -> Quaternion: - """Return a normalized (unit) quaternion.""" - norm = np.sqrt(self.x**2 + self.y**2 + self.z**2 + self.w**2) - if norm == 0: - raise ZeroDivisionError("Cannot normalize zero quaternion") - return Quaternion(self.x / norm, self.y / norm, self.z / norm, self.w / norm) - - def rotate_vector(self, vector: Vector3) -> Vector3: - """Rotate a 3D vector by this quaternion. - - Args: - vector: The vector to rotate - - Returns: - The rotated vector - """ - # For unit quaternions, conjugate equals inverse, so we use conjugate for efficiency - # The rotation formula is: q * v * q^* where q^* is the conjugate - - # Convert vector to pure quaternion (w=0) - v_quat = Quaternion(vector.x, vector.y, vector.z, 0) - - # Apply rotation: q * v * q^* (conjugate for unit quaternions) - rotated = self * v_quat * self.conjugate() - - # Extract vector components - return Vector3(rotated.x, rotated.y, rotated.z) diff --git a/dimos/msgs/geometry_msgs/Transform.py b/dimos/msgs/geometry_msgs/Transform.py deleted file mode 100644 index 6499905da1a..00000000000 --- a/dimos/msgs/geometry_msgs/Transform.py +++ /dev/null @@ -1,329 +0,0 @@ -# Copyright 2025-2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from __future__ import annotations - -import time -from typing import TYPE_CHECKING, Any, BinaryIO - -if TYPE_CHECKING: - import numpy as np - import rerun as rr - - from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped - -from dimos_lcm.geometry_msgs import ( - Transform as LCMTransform, - TransformStamped as LCMTransformStamped, -) - -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.msgs.std_msgs.Header import Header -from dimos.types.timestamped import Timestamped - - -class Transform(Timestamped): - translation: Vector3 - rotation: Quaternion - ts: float - frame_id: str - child_frame_id: str - msg_name = "tf2_msgs.TFMessage" - - def __init__( # type: ignore[no-untyped-def] - self, - translation: Vector3 | None = None, - rotation: Quaternion | None = None, - frame_id: str = "world", - child_frame_id: str = "unset", - ts: float | None = None, - **kwargs, - ) -> None: - self.frame_id = frame_id - self.child_frame_id = child_frame_id - self.ts = time.time() if ts is None else ts - self.translation = translation if translation is not None else Vector3() - self.rotation = rotation if rotation is not None else Quaternion() - - def now(self) -> Transform: - """Return a copy of this Transform with the current timestamp.""" - return Transform( - translation=self.translation, - rotation=self.rotation, - frame_id=self.frame_id, - child_frame_id=self.child_frame_id, - ts=time.time(), - ) - - def __repr__(self) -> str: - return f"Transform(translation={self.translation!r}, rotation={self.rotation!r})" - - def __str__(self) -> str: - return f"{self.frame_id} -> {self.child_frame_id}\n Translation: {self.translation}\n Rotation: {self.rotation}" - - def __eq__(self, other) -> bool: # type: ignore[no-untyped-def] - """Check if two transforms are equal.""" - if not isinstance(other, Transform): - return False - return self.translation == other.translation and self.rotation == other.rotation - - @classmethod - def identity(cls) -> Transform: - """Create an identity transform.""" - return cls() - - def lcm_transform(self) -> LCMTransformStamped: - return LCMTransformStamped( - child_frame_id=self.child_frame_id, - header=Header(self.ts, self.frame_id), - transform=LCMTransform( - translation=self.translation, - rotation=self.rotation, - ), - ) - - def apply(self, other: Transform) -> Transform: - return self.__add__(other) - - def __add__(self, other: Transform) -> Transform: - """Compose two transforms (transform composition). - - The operation self + other represents applying transformation 'other' - in the coordinate frame defined by 'self'. This is equivalent to: - - First apply transformation 'self' (from frame A to frame B) - - Then apply transformation 'other' (from frame B to frame C) - - Args: - other: The transform to compose with this one - - Returns: - A new Transform representing the composed transformation - - Example: - t1 = Transform(Vector3(1, 0, 0), Quaternion(0, 0, 0, 1)) - t2 = Transform(Vector3(2, 0, 0), Quaternion(0, 0, 0, 1)) - t3 = t1 + t2 # Combined transform: translation (3, 0, 0) - """ - if not isinstance(other, Transform): - raise TypeError(f"Cannot add Transform and {type(other).__name__}") - - # Compose orientations: self.rotation * other.rotation - new_rotation = self.rotation * other.rotation - - # Transform other's translation by self's rotation, then add to self's translation - rotated_translation = self.rotation.rotate_vector(other.translation) - new_translation = self.translation + rotated_translation - - return Transform( - translation=new_translation, - rotation=new_rotation, - frame_id=self.frame_id, - child_frame_id=other.child_frame_id, - ts=self.ts, - ) - - def inverse(self) -> Transform: - """Compute the inverse transform. - - The inverse transform reverses the direction of the transformation. - If this transform goes from frame A to frame B, the inverse goes from B to A. - - Returns: - A new Transform representing the inverse transformation - """ - # Inverse rotation - inv_rotation = self.rotation.inverse() - - # Inverse translation: -R^(-1) * t - inv_translation = inv_rotation.rotate_vector(self.translation) - inv_translation = Vector3(-inv_translation.x, -inv_translation.y, -inv_translation.z) - - return Transform( - translation=inv_translation, - rotation=inv_rotation, - frame_id=self.child_frame_id, # Swap frame references - child_frame_id=self.frame_id, - ts=self.ts, - ) - - def __neg__(self) -> Transform: - """Unary minus operator returns the inverse transform.""" - return self.inverse() - - @classmethod - def from_pose(cls, frame_id: str, pose: Pose | PoseStamped) -> Transform: # type: ignore[name-defined] - """Create a Transform from a Pose or PoseStamped. - - Args: - pose: A Pose or PoseStamped object to convert - - Returns: - A Transform with the same translation and rotation as the pose - """ - # Import locally to avoid circular imports - from dimos.msgs.geometry_msgs.Pose import Pose - from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped - - # Handle both Pose and PoseStamped - if isinstance(pose, PoseStamped): - return cls( - translation=pose.position, - rotation=pose.orientation, - frame_id=pose.frame_id, - child_frame_id=frame_id, - ts=pose.ts, - ) - elif isinstance(pose, Pose): - return cls( - translation=pose.position, - rotation=pose.orientation, - child_frame_id=frame_id, - ) - else: - raise TypeError(f"Expected Pose or PoseStamped, got {type(pose).__name__}") - - def to_pose(self, **kwargs: Any) -> PoseStamped: - """Create a Transform from a Pose or PoseStamped. - - Args: - pose: A Pose or PoseStamped object to convert - - Returns: - A Transform with the same translation and rotation as the pose - """ - # Import locally to avoid circular imports - from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped as _PoseStamped - - # Handle both Pose and PoseStamped - fields: dict[str, Any] = { - "position": self.translation, - "orientation": self.rotation, - "frame_id": self.frame_id, - } - result: PoseStamped = _PoseStamped(**fields, **kwargs) - return result - - @classmethod - def from_matrix( - cls, - matrix: np.ndarray, - *, - ts: float | None = None, - frame_id: str = "world", - child_frame_id: str = "unset", - ) -> Transform: - """Build a Transform from a 4x4 homogeneous transformation matrix.""" - return cls( - translation=Vector3(matrix[:3, 3]), - rotation=Quaternion.from_rotation_matrix(matrix[:3, :3]), - frame_id=frame_id, - child_frame_id=child_frame_id, - ts=ts, - ) - - def to_matrix(self) -> np.ndarray: # type: ignore[name-defined] - """Convert Transform to a 4x4 transformation matrix. - - Returns a homogeneous transformation matrix that represents both - the rotation and translation of this transform. - - Returns: - np.ndarray: A 4x4 homogeneous transformation matrix - """ - import numpy as np - - # Extract quaternion components - x, y, z, w = self.rotation.x, self.rotation.y, self.rotation.z, self.rotation.w - - # Build rotation matrix from quaternion using standard formula - # This avoids numerical issues compared to converting to axis-angle first - rotation_matrix = np.array( - [ - [1 - 2 * (y * y + z * z), 2 * (x * y - z * w), 2 * (x * z + y * w)], - [2 * (x * y + z * w), 1 - 2 * (x * x + z * z), 2 * (y * z - x * w)], - [2 * (x * z - y * w), 2 * (y * z + x * w), 1 - 2 * (x * x + y * y)], - ] - ) - - # Build 4x4 homogeneous transformation matrix - matrix = np.eye(4) - matrix[:3, :3] = rotation_matrix - matrix[:3, 3] = [self.translation.x, self.translation.y, self.translation.z] - - return matrix - - def lcm_encode(self) -> bytes: - # we get a circular import otherwise - from dimos.msgs.tf2_msgs.TFMessage import TFMessage - - return TFMessage(self).lcm_encode() - - @classmethod - def lcm_decode(cls, data: bytes | BinaryIO) -> Transform: - """Decode from LCM TFMessage bytes.""" - from dimos_lcm.tf2_msgs import TFMessage as LCMTFMessage - - lcm_msg = LCMTFMessage.lcm_decode(data) - - if not lcm_msg.transforms: - raise ValueError("No transforms found in LCM message") - - # Get the first transform from the message - lcm_transform_stamped = lcm_msg.transforms[0] - - # Extract timestamp from header - ts = lcm_transform_stamped.header.stamp.sec + ( - lcm_transform_stamped.header.stamp.nsec / 1_000_000_000 - ) - - # Create and return Transform instance - return cls( - translation=Vector3( - lcm_transform_stamped.transform.translation.x, - lcm_transform_stamped.transform.translation.y, - lcm_transform_stamped.transform.translation.z, - ), - rotation=Quaternion( - lcm_transform_stamped.transform.rotation.x, - lcm_transform_stamped.transform.rotation.y, - lcm_transform_stamped.transform.rotation.z, - lcm_transform_stamped.transform.rotation.w, - ), - frame_id=lcm_transform_stamped.header.frame_id, - child_frame_id=lcm_transform_stamped.child_frame_id, - ts=ts, - ) - - def to_rerun(self, frameless: bool = False) -> rr.Transform3D: - """Convert to a rerun Transform3D. - - Args: - frameless: omit ``parent_frame``/``child_frame``. By default the - transform carries its frame IDs (resolved via rerun's tf-graph); - set this when positioning by entity-path hierarchy instead, where - the named frames would not compose. - """ - import rerun as rr - - translation = [self.translation.x, self.translation.y, self.translation.z] - rotation = self.rotation.to_rerun() - if frameless: - return rr.Transform3D(translation=translation, rotation=rotation) - return rr.Transform3D( - translation=translation, - rotation=rotation, - parent_frame="tf#/" + self.frame_id, - child_frame="tf#/" + self.child_frame_id, - ) diff --git a/dimos/msgs/geometry_msgs/Twist.py b/dimos/msgs/geometry_msgs/Twist.py deleted file mode 100644 index d08f61211f0..00000000000 --- a/dimos/msgs/geometry_msgs/Twist.py +++ /dev/null @@ -1,132 +0,0 @@ -# Copyright 2025-2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from __future__ import annotations - -from typing import Any, overload - -from dimos_lcm.geometry_msgs import Twist as LCMTwist - -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Vector3 import Vector3, VectorLike - - -class Twist(LCMTwist): # type: ignore[misc] - linear: Vector3 - angular: Vector3 - msg_name = "geometry_msgs.Twist" - - @overload - def __init__(self) -> None: ... - - @overload - def __init__( - self, - linear: VectorLike | None = ..., - angular: VectorLike | Quaternion | None = ..., - ) -> None: ... - - @overload - def __init__(self, twist: Twist | LCMTwist, /) -> None: ... - - def __init__(self, *args: Any, **kwargs: Any) -> None: - """Initialize a twist. - - Supported forms: - Twist() # zero twist - Twist(linear, angular) - Twist(linear=..., angular=...) # keyword args, either optional - Twist(linear, quaternion) # angular converted to euler - Twist(other_twist) # copy constructor - Twist(lcm_twist) # from LCM message - """ - linear: Any = None - angular: Any = None - - if len(args) == 2: - linear, angular = args - elif len(args) == 1: - value = args[0] - # Twist before LCMTwist (it is a subclass); anything else is a - # linear velocity, same as the `linear=` keyword. - if isinstance(value, Twist | LCMTwist): - linear, angular = value.linear, value.angular - else: - linear = value - elif args: - raise TypeError(f"Twist takes 1 or 2 positional arguments ({len(args)} given)") - - if kwargs: - linear = kwargs.pop("linear", linear) - angular = kwargs.pop("angular", angular) - if kwargs: - raise TypeError(f"Twist got unexpected keyword arguments {sorted(kwargs)}") - - self.linear = Vector3() if linear is None else Vector3(linear) - if angular is None: - self.angular = Vector3() - elif isinstance(angular, Quaternion): - self.angular = angular.to_euler() - else: - self.angular = Vector3(angular) - - def __repr__(self) -> str: - return f"Twist(linear={self.linear!r}, angular={self.angular!r})" - - def __str__(self) -> str: - return f"Twist:\n Linear: {self.linear}\n Angular: {self.angular}" - - def __eq__(self, other) -> bool: # type: ignore[no-untyped-def] - """Check if two twists are equal.""" - if not isinstance(other, Twist): - return False - return self.linear == other.linear and self.angular == other.angular - - @classmethod - def zero(cls) -> Twist: - """Create a zero twist (no motion).""" - return cls() - - def is_zero(self) -> bool: - """Check if this is a zero twist (no linear or angular velocity).""" - return self.linear.is_zero() and self.angular.is_zero() - - def __sub__(self, other: Twist) -> Twist: - """Component-wise subtraction: self - other.""" - if not isinstance(other, Twist): - return NotImplemented - return Twist( - linear=self.linear - other.linear, - angular=self.angular - other.angular, - ) - - def __add__(self, other: Twist) -> Twist: - """Component-wise addition: self + other.""" - if not isinstance(other, Twist): - return NotImplemented - return Twist( - linear=self.linear + other.linear, - angular=self.angular + other.angular, - ) - - def __bool__(self) -> bool: - """Boolean conversion for Twist. - - A Twist is considered False if it's a zero twist (no motion), - and True otherwise. - - Returns: - False if twist is zero, True otherwise - """ - return not self.is_zero() diff --git a/dimos/msgs/geometry_msgs/TwistStamped.py b/dimos/msgs/geometry_msgs/TwistStamped.py deleted file mode 100644 index 4377687e564..00000000000 --- a/dimos/msgs/geometry_msgs/TwistStamped.py +++ /dev/null @@ -1,85 +0,0 @@ -# Copyright 2025-2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from __future__ import annotations - -import time -from typing import Any, BinaryIO, TypeAlias - -from dimos_lcm.geometry_msgs import TwistStamped as LCMTwistStamped - -from dimos.msgs.geometry_msgs.Twist import Twist -from dimos.msgs.geometry_msgs.Vector3 import VectorConvertable -from dimos.types.timestamped import Timestamped - -# Types that can be converted to/from TwistStamped -TwistConvertable: TypeAlias = ( - tuple[VectorConvertable, VectorConvertable] | LCMTwistStamped | dict[str, VectorConvertable] -) - - -def sec_nsec(ts): # type: ignore[no-untyped-def] - s = int(ts) - return [s, int((ts - s) * 1_000_000_000)] - - -class TwistStamped(Twist, Timestamped): - msg_name = "geometry_msgs.TwistStamped" - ts: float - frame_id: str - - def __init__(self, *args: Any, **kwargs: Any) -> None: - """Initialize a stamped twist. - - Takes ``(ts, frame_id)`` positionally, plus every Twist keyword. Any other - positional form belongs to Twist, so ``TwistStamped(linear, angular)`` - works as it does on the base class. - """ - ts: Any - # A leading number or string is a timestamp; anything else is a Twist argument. - if args and isinstance(args[0], int | float | str): - ts = args[0] - frame_id = args[1] if len(args) > 1 else kwargs.pop("frame_id", "") - twist_args: tuple[Any, ...] = () - else: - ts = kwargs.pop("ts", None) - frame_id = kwargs.pop("frame_id", "") - twist_args = args - - self.frame_id = frame_id - self.ts = time.time() if ts is None else ts - super().__init__(*twist_args, **kwargs) - - def lcm_encode(self) -> bytes: - lcm_msg = LCMTwistStamped() - lcm_msg.twist = self - [lcm_msg.header.stamp.sec, lcm_msg.header.stamp.nsec] = sec_nsec(self.ts) # type: ignore[no-untyped-call] - lcm_msg.header.frame_id = self.frame_id - return lcm_msg.lcm_encode() # type: ignore[no-any-return] - - @classmethod - def lcm_decode(cls, data: bytes | BinaryIO) -> TwistStamped: - lcm_msg = LCMTwistStamped.lcm_decode(data) - return cls( - ts=lcm_msg.header.stamp.sec + (lcm_msg.header.stamp.nsec / 1_000_000_000), - frame_id=lcm_msg.header.frame_id, - linear=[lcm_msg.twist.linear.x, lcm_msg.twist.linear.y, lcm_msg.twist.linear.z], - angular=[lcm_msg.twist.angular.x, lcm_msg.twist.angular.y, lcm_msg.twist.angular.z], - ) - - def __str__(self) -> str: - return ( - f"TwistStamped(linear=[{self.linear.x:.3f}, {self.linear.y:.3f}, {self.linear.z:.3f}], " - f"angular=[{self.angular.x:.3f}, {self.angular.y:.3f}, {self.angular.z:.3f}])" - ) diff --git a/dimos/msgs/geometry_msgs/TwistWithCovariance.py b/dimos/msgs/geometry_msgs/TwistWithCovariance.py deleted file mode 100644 index 3786a013f54..00000000000 --- a/dimos/msgs/geometry_msgs/TwistWithCovariance.py +++ /dev/null @@ -1,181 +0,0 @@ -# Copyright 2025-2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from __future__ import annotations - -from typing import Any, TypeAlias - -from dimos_lcm.geometry_msgs import ( - TwistWithCovariance as LCMTwistWithCovariance, -) -import numpy as np - -from dimos.msgs.geometry_msgs.Twist import Twist -from dimos.msgs.geometry_msgs.Vector3 import Vector3, VectorConvertable - -# Types that can be converted to/from TwistWithCovariance -TwistWithCovarianceConvertable: TypeAlias = ( - tuple[Twist | tuple[VectorConvertable, VectorConvertable], list[float] | np.ndarray] - | LCMTwistWithCovariance - | dict[str, Twist | tuple[VectorConvertable, VectorConvertable] | list[float] | np.ndarray] -) - -COVARIANCE_SIZE = 36 - - -def _is_twist_covariance_pair(value: Any) -> bool: - """True for a 2-element (twist, covariance) pair, where covariance is 36 long.""" - if not isinstance(value, tuple | list) or len(value) != 2: - return False - covariance = value[1] - return isinstance(covariance, list | np.ndarray) and np.size(covariance) == COVARIANCE_SIZE - - -def _to_twist(value: Any) -> Twist: - """A Twist, or a (linear, angular) pair to build one from.""" - if isinstance(value, Twist): - return value - if value is None: - return Twist() - return Twist(value[0], value[1]) - - -class TwistWithCovariance(LCMTwistWithCovariance): # type: ignore[misc] - twist: Twist - covariance: np.ndarray[tuple[int], np.dtype[np.floating[Any]]] - msg_name = "geometry_msgs.TwistWithCovariance" - - def __init__( - self, - twist: Twist | TwistWithCovarianceConvertable | None = None, - covariance: list[float] | np.ndarray | None = None, - ) -> None: - """Initialize a twist with covariance. - - Supported forms: - TwistWithCovariance() # zero twist and covariance - TwistWithCovariance(twist) - TwistWithCovariance(twist, covariance) - TwistWithCovariance(twist=..., covariance=...) - TwistWithCovariance((linear, angular)) - TwistWithCovariance((twist, covariance)) # pair - TwistWithCovariance({"twist": ..., "covariance": ...}) - TwistWithCovariance(other) # copy constructor - TwistWithCovariance(lcm_twist_with_cov) # from LCM message - """ - source: Any = twist - cov: Any = covariance - - # TwistWithCovariance before LCMTwistWithCovariance (it is a subclass). - if isinstance(source, TwistWithCovariance): - if cov is None: - cov = np.array(source.covariance).copy() - source = Twist(source.twist) # copy constructor: don't alias the source twist - elif isinstance(source, LCMTwistWithCovariance): - if cov is None: - cov = np.array(source.covariance) - source = Twist(source.twist) - elif isinstance(source, dict) and "twist" in source: - cov = source.get("covariance", cov) - source = source["twist"] - elif _is_twist_covariance_pair(source): - source, cov = source - - self.twist = _to_twist(source) - self.covariance = ( - np.zeros(COVARIANCE_SIZE) - if cov is None - else np.array(cov, dtype=float).reshape(COVARIANCE_SIZE) - ) - - def __getattribute__(self, name: str): # type: ignore[no-untyped-def] - """Override to ensure covariance is always returned as numpy array.""" - if name == "covariance": - cov = object.__getattribute__(self, "covariance") - if not isinstance(cov, np.ndarray): - return np.array(cov, dtype=float) - return cov - return super().__getattribute__(name) - - def __setattr__(self, name: str, value) -> None: # type: ignore[no-untyped-def] - """Override to ensure covariance is stored as numpy array.""" - if name == "covariance": - if not isinstance(value, np.ndarray): - value = np.array(value, dtype=float).reshape(36) - super().__setattr__(name, value) - - @property - def linear(self) -> Vector3: - """Linear velocity vector.""" - return self.twist.linear - - @property - def angular(self) -> Vector3: - """Angular velocity vector.""" - return self.twist.angular - - @property - def covariance_matrix(self) -> np.ndarray: - """Get covariance as 6x6 matrix.""" - return self.covariance.reshape(6, 6) - - @covariance_matrix.setter - def covariance_matrix(self, value: np.ndarray) -> None: - """Set covariance from 6x6 matrix.""" - self.covariance = np.array(value).reshape(36) - - def __repr__(self) -> str: - return f"TwistWithCovariance(twist={self.twist!r}, covariance=<{self.covariance.shape[0] if isinstance(self.covariance, np.ndarray) else len(self.covariance)} elements>)" - - def __str__(self) -> str: - return ( - f"TwistWithCovariance(linear=[{self.linear.x:.3f}, {self.linear.y:.3f}, {self.linear.z:.3f}], " - f"angular=[{self.angular.x:.3f}, {self.angular.y:.3f}, {self.angular.z:.3f}], " - f"cov_trace={np.trace(self.covariance_matrix):.3f})" - ) - - def __eq__(self, other) -> bool: # type: ignore[no-untyped-def] - """Check if two TwistWithCovariance are equal.""" - if not isinstance(other, TwistWithCovariance): - return False - return self.twist == other.twist and np.allclose(self.covariance, other.covariance) - - def is_zero(self) -> bool: - """Check if this is a zero twist (no linear or angular velocity).""" - return self.twist.is_zero() - - def __bool__(self) -> bool: - """Boolean conversion - False if zero twist, True otherwise.""" - return not self.is_zero() - - def lcm_encode(self) -> bytes: - """Encode to LCM binary format.""" - lcm_msg = LCMTwistWithCovariance() - lcm_msg.twist = self.twist - # LCM expects list, not numpy array - if isinstance(self.covariance, np.ndarray): - lcm_msg.covariance = self.covariance.tolist() - else: - lcm_msg.covariance = list(self.covariance) - return lcm_msg.lcm_encode() # type: ignore[no-any-return] - - @classmethod - def lcm_decode(cls, data: bytes) -> TwistWithCovariance: - """Decode from LCM binary format.""" - lcm_msg = LCMTwistWithCovariance.lcm_decode(data) - twist = Twist( - linear=[lcm_msg.twist.linear.x, lcm_msg.twist.linear.y, lcm_msg.twist.linear.z], - angular=[lcm_msg.twist.angular.x, lcm_msg.twist.angular.y, lcm_msg.twist.angular.z], - ) - return cls(twist, lcm_msg.covariance) diff --git a/dimos/msgs/geometry_msgs/TwistWithCovarianceStamped.py b/dimos/msgs/geometry_msgs/TwistWithCovarianceStamped.py deleted file mode 100644 index a828fdaa836..00000000000 --- a/dimos/msgs/geometry_msgs/TwistWithCovarianceStamped.py +++ /dev/null @@ -1,106 +0,0 @@ -# Copyright 2025-2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from __future__ import annotations - -import time -from typing import TypeAlias - -from dimos_lcm.geometry_msgs import ( - TwistWithCovarianceStamped as LCMTwistWithCovarianceStamped, -) -import numpy as np - -from dimos.msgs.geometry_msgs.Twist import Twist -from dimos.msgs.geometry_msgs.TwistWithCovariance import TwistWithCovariance -from dimos.msgs.geometry_msgs.Vector3 import VectorConvertable -from dimos.types.timestamped import Timestamped - -# Types that can be converted to/from TwistWithCovarianceStamped -TwistWithCovarianceStampedConvertable: TypeAlias = ( - tuple[Twist | tuple[VectorConvertable, VectorConvertable], list[float] | np.ndarray] - | LCMTwistWithCovarianceStamped - | dict[ - str, - Twist - | tuple[VectorConvertable, VectorConvertable] - | list[float] - | np.ndarray - | float - | str, - ] -) - - -def sec_nsec(ts): # type: ignore[no-untyped-def] - s = int(ts) - return [s, int((ts - s) * 1_000_000_000)] - - -class TwistWithCovarianceStamped(TwistWithCovariance, Timestamped): - msg_name = "geometry_msgs.TwistWithCovarianceStamped" - ts: float - frame_id: str - - def __init__( - self, - ts: float | None = None, - frame_id: str = "", - twist: Twist | tuple[VectorConvertable, VectorConvertable] | None = None, - covariance: list[float] | np.ndarray | None = None, - ) -> None: - """Initialize with timestamp, frame_id, twist and covariance.""" - self.frame_id = frame_id - self.ts = time.time() if ts is None else ts - super().__init__(twist, covariance) - - def lcm_encode(self) -> bytes: - lcm_msg = LCMTwistWithCovarianceStamped() - lcm_msg.twist.twist = self.twist - # LCM expects list, not numpy array - if isinstance(self.covariance, np.ndarray): - lcm_msg.twist.covariance = self.covariance.tolist() - else: - lcm_msg.twist.covariance = list(self.covariance) - [lcm_msg.header.stamp.sec, lcm_msg.header.stamp.nsec] = sec_nsec(self.ts) # type: ignore[no-untyped-call] - lcm_msg.header.frame_id = self.frame_id - return lcm_msg.lcm_encode() # type: ignore[no-any-return] - - @classmethod - def lcm_decode(cls, data: bytes) -> TwistWithCovarianceStamped: - lcm_msg = LCMTwistWithCovarianceStamped.lcm_decode(data) - return cls( - ts=lcm_msg.header.stamp.sec + (lcm_msg.header.stamp.nsec / 1_000_000_000), - frame_id=lcm_msg.header.frame_id, - twist=Twist( - linear=[ - lcm_msg.twist.twist.linear.x, - lcm_msg.twist.twist.linear.y, - lcm_msg.twist.twist.linear.z, - ], - angular=[ - lcm_msg.twist.twist.angular.x, - lcm_msg.twist.twist.angular.y, - lcm_msg.twist.twist.angular.z, - ], - ), - covariance=lcm_msg.twist.covariance, - ) - - def __str__(self) -> str: - return ( - f"TwistWithCovarianceStamped(linear=[{self.linear.x:.3f}, {self.linear.y:.3f}, {self.linear.z:.3f}], " - f"angular=[{self.angular.x:.3f}, {self.angular.y:.3f}, {self.angular.z:.3f}], " - f"cov_trace={np.trace(self.covariance_matrix):.3f})" - ) diff --git a/dimos/msgs/geometry_msgs/Vector3.py b/dimos/msgs/geometry_msgs/Vector3.py deleted file mode 100644 index 99c98677182..00000000000 --- a/dimos/msgs/geometry_msgs/Vector3.py +++ /dev/null @@ -1,434 +0,0 @@ -# Copyright 2025-2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from __future__ import annotations - -from collections.abc import Sequence -from typing import Any, TypeAlias - -from dimos_lcm.geometry_msgs import Vector3 as LCMVector3 -import numpy as np - -# Types that can be converted to/from Vector -VectorConvertable: TypeAlias = Sequence[int | float] | LCMVector3 | np.ndarray - - -def _ensure_3d(data: np.ndarray) -> np.ndarray: - """Ensure the data array is exactly 3D by padding with zeros or raising an exception if too long.""" - if len(data) == 3: - return data - elif len(data) < 3: - padded = np.zeros(3, dtype=float) - padded[: len(data)] = data - return padded - else: - raise ValueError( - f"Vector3 cannot be initialized with more than 3 components. Got {len(data)} components." - ) - - -class Vector3(LCMVector3): # type: ignore[misc] - x: float = 0.0 - y: float = 0.0 - z: float = 0.0 - - def __init__(self, *args: Any, **kwargs: Any) -> None: - """Initialize a 3D vector. - - Supported forms: - Vector3() # zero vector - Vector3(x) # (x, 0, 0) - Vector3(x, y) # (x, y, 0) - Vector3(x, y, z) # (x, y, z) - Vector3(x=1, y=2, z=3) # keyword args - Vector3([x, y, z]) # sequence - Vector3(np.array([x, y, z])) # numpy array - Vector3(other_vector3) # copy constructor - Vector3(lcm_vector3) # from LCM message - """ - if kwargs and not args: - # Keyword arguments: Vector3(x=1, y=2, z=3) - self.x = float(kwargs.get("x", 0.0)) - self.y = float(kwargs.get("y", 0.0)) - self.z = float(kwargs.get("z", 0.0)) - elif not args: - # No arguments: zero vector - self.x = 0.0 - self.y = 0.0 - self.z = 0.0 - elif len(args) == 1: - arg = args[0] - if isinstance(arg, Vector3): - # Copy constructor - self.x = arg.x - self.y = arg.y - self.z = arg.z - elif isinstance(arg, LCMVector3): - # From LCM Vector3 - self.x = float(arg.x) - self.y = float(arg.y) - self.z = float(arg.z) - elif isinstance(arg, np.ndarray): - # From numpy array - data = _ensure_3d(np.array(arg, dtype=float)) - self.x = float(data[0]) - self.y = float(data[1]) - self.z = float(data[2]) - elif isinstance(arg, (list, tuple)): - # From sequence - data = _ensure_3d(np.array(arg, dtype=float)) - self.x = float(data[0]) - self.y = float(data[1]) - self.z = float(data[2]) - elif isinstance(arg, (int, float)): - # Single numeric value: (x, 0, 0) - self.x = float(arg) - self.y = 0.0 - self.z = 0.0 - else: - raise TypeError(f"Cannot initialize Vector3 from {type(arg)}") - elif len(args) == 2: - # Two numeric values: (x, y, 0) - self.x = float(args[0]) - self.y = float(args[1]) - self.z = 0.0 - elif len(args) == 3: - # Three numeric values: (x, y, z) - self.x = float(args[0]) - self.y = float(args[1]) - self.z = float(args[2]) - else: - raise TypeError(f"Vector3 takes at most 3 positional arguments ({len(args)} given)") - - @property - def as_tuple(self) -> tuple[float, float, float]: - return (self.x, self.y, self.z) - - @property - def yaw(self) -> float: - return self.z - - @property - def pitch(self) -> float: - return self.y - - @property - def roll(self) -> float: - return self.x - - @property - def data(self) -> np.ndarray: - """Get the underlying numpy array.""" - return np.array([self.x, self.y, self.z], dtype=float) - - def __getitem__(self, idx: int) -> float: - if idx == 0: - return self.x - elif idx == 1: - return self.y - elif idx == 2: - return self.z - else: - raise IndexError(f"Vector3 index {idx} out of range [0-2]") - - def __repr__(self) -> str: - return f"Vector({self.data})" - - def __str__(self) -> str: - def getArrow() -> str: - repr = ["←", "↖", "↑", "↗", "→", "↘", "↓", "↙"] - - if self.x == 0 and self.y == 0: - return "·" - - # Calculate angle in radians and convert to directional index - angle = np.arctan2(self.y, self.x) - # Map angle to 0-7 index (8 directions) with proper orientation - dir_index = int(((angle + np.pi) * 4 / np.pi) % 8) - # Get directional arrow symbol - return repr[dir_index] - - return f"{getArrow()} Vector {self.__repr__()}" - - def agent_encode(self) -> dict[str, float]: - """Encode the vector for agent communication.""" - return {"x": self.x, "y": self.y, "z": self.z} - - def serialize(self) -> dict[str, Any]: - """Serialize the vector to a tuple.""" - return {"type": "vector", "c": (self.x, self.y, self.z)} - - def __eq__(self, other: object) -> bool: - """Check if two vectors are equal using numpy's allclose for floating point comparison.""" - if not isinstance(other, Vector3): - return False - return bool(np.allclose([self.x, self.y, self.z], [other.x, other.y, other.z])) - - def __add__(self, other: VectorConvertable | Vector3) -> Vector3: - other_vector: Vector3 = to_vector(other) - return self.__class__( - self.x + other_vector.x, self.y + other_vector.y, self.z + other_vector.z - ) - - def __sub__(self, other: VectorConvertable | Vector3) -> Vector3: - other_vector = to_vector(other) - return self.__class__( - self.x - other_vector.x, self.y - other_vector.y, self.z - other_vector.z - ) - - def __mul__(self, scalar: float) -> Vector3: - return self.__class__(self.x * scalar, self.y * scalar, self.z * scalar) - - def __rmul__(self, scalar: float) -> Vector3: - return self.__mul__(scalar) - - def __truediv__(self, scalar: float) -> Vector3: - return self.__class__(self.x / scalar, self.y / scalar, self.z / scalar) - - def __neg__(self) -> Vector3: - return self.__class__(-self.x, -self.y, -self.z) - - def dot(self, other: VectorConvertable | Vector3) -> float: - """Compute dot product.""" - other_vector = to_vector(other) - return float(self.x * other_vector.x + self.y * other_vector.y + self.z * other_vector.z) - - def cross(self, other: VectorConvertable | Vector3) -> Vector3: - """Compute cross product (3D vectors only).""" - other_vector = to_vector(other) - return self.__class__( - self.y * other_vector.z - self.z * other_vector.y, - self.z * other_vector.x - self.x * other_vector.z, - self.x * other_vector.y - self.y * other_vector.x, - ) - - def magnitude(self) -> float: - """Alias for length().""" - return self.length() - - def length(self) -> float: - """Compute the Euclidean length (magnitude) of the vector.""" - return float(np.sqrt(self.x * self.x + self.y * self.y + self.z * self.z)) - - def length_squared(self) -> float: - """Compute the squared length of the vector (faster than length()).""" - return float(self.x * self.x + self.y * self.y + self.z * self.z) - - def normalize(self) -> Vector3: - """Return a normalized unit vector in the same direction.""" - length = self.length() - if length < 1e-10: # Avoid division by near-zero - return self.__class__(0.0, 0.0, 0.0) - return self.__class__(self.x / length, self.y / length, self.z / length) - - def to_2d(self) -> Vector3: - """Convert a vector to a 2D vector by taking only the x and y components (z=0).""" - return self.__class__(self.x, self.y, 0.0) - - def distance(self, other: VectorConvertable | Vector3) -> float: - """Compute Euclidean distance to another vector.""" - other_vector = to_vector(other) - dx = self.x - other_vector.x - dy = self.y - other_vector.y - dz = self.z - other_vector.z - return float(np.sqrt(dx * dx + dy * dy + dz * dz)) - - def distance_squared(self, other: VectorConvertable | Vector3) -> float: - """Compute squared Euclidean distance to another vector (faster than distance()).""" - other_vector = to_vector(other) - dx = self.x - other_vector.x - dy = self.y - other_vector.y - dz = self.z - other_vector.z - return float(dx * dx + dy * dy + dz * dz) - - def angle(self, other: VectorConvertable | Vector3) -> float: - """Compute the angle (in radians) between this vector and another.""" - other_vector = to_vector(other) - this_length = self.length() - other_length = other_vector.length() - - if this_length < 1e-10 or other_length < 1e-10: - return 0.0 - - cos_angle = np.clip( - self.dot(other_vector) / (this_length * other_length), - -1.0, - 1.0, - ) - return float(np.arccos(cos_angle)) - - def project(self, onto: VectorConvertable | Vector3) -> Vector3: - """Project this vector onto another vector.""" - onto_vector = to_vector(onto) - onto_length_sq = ( - onto_vector.x * onto_vector.x - + onto_vector.y * onto_vector.y - + onto_vector.z * onto_vector.z - ) - if onto_length_sq < 1e-10: - return self.__class__(0.0, 0.0, 0.0) - - scalar_projection = self.dot(onto_vector) / onto_length_sq - return self.__class__( - scalar_projection * onto_vector.x, - scalar_projection * onto_vector.y, - scalar_projection * onto_vector.z, - ) - - @classmethod - def zeros(cls) -> Vector3: - """Create a zero 3D vector.""" - return cls() - - @classmethod - def ones(cls) -> Vector3: - """Create a 3D vector of ones.""" - return cls(1.0, 1.0, 1.0) - - @classmethod - def unit_x(cls) -> Vector3: - """Create a unit vector in the x direction.""" - return cls(1.0, 0.0, 0.0) - - @classmethod - def unit_y(cls) -> Vector3: - """Create a unit vector in the y direction.""" - return cls(0.0, 1.0, 0.0) - - @classmethod - def unit_z(cls) -> Vector3: - """Create a unit vector in the z direction.""" - return cls(0.0, 0.0, 1.0) - - def to_list(self) -> list[float]: - """Convert the vector to a list.""" - return [self.x, self.y, self.z] - - def to_tuple(self) -> tuple[float, float, float]: - """Convert the vector to a tuple.""" - return (self.x, self.y, self.z) - - def to_numpy(self) -> np.ndarray: - """Convert the vector to a numpy array.""" - return np.array([self.x, self.y, self.z], dtype=float) - - def is_zero(self) -> bool: - """Check if this is a zero vector (all components are zero). - - Returns: - True if all components are zero, False otherwise - """ - return bool(np.allclose([self.x, self.y, self.z], 0.0)) - - @property - def quaternion(self) -> Quaternion: # type: ignore[name-defined] - return self.to_quaternion() - - def to_quaternion(self) -> Quaternion: # type: ignore[name-defined] - """Convert Vector3 representing Euler angles (roll, pitch, yaw) to a Quaternion. - - Assumes this Vector3 contains Euler angles in radians: - - x component: roll (rotation around x-axis) - - y component: pitch (rotation around y-axis) - - z component: yaw (rotation around z-axis) - - Returns: - Quaternion: The equivalent quaternion representation - """ - # Import here to avoid circular imports - from dimos.msgs.geometry_msgs.Quaternion import Quaternion - - # Extract Euler angles - roll = self.x - pitch = self.y - yaw = self.z - - # Convert Euler angles to quaternion using ZYX convention - # Source: https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles - - # Compute half angles - cy = np.cos(yaw * 0.5) - sy = np.sin(yaw * 0.5) - cp = np.cos(pitch * 0.5) - sp = np.sin(pitch * 0.5) - cr = np.cos(roll * 0.5) - sr = np.sin(roll * 0.5) - - # Compute quaternion components - w = cr * cp * cy + sr * sp * sy - x = sr * cp * cy - cr * sp * sy - y = cr * sp * cy + sr * cp * sy - z = cr * cp * sy - sr * sp * cy - - return Quaternion(x, y, z, w) - - def __bool__(self) -> bool: - """Boolean conversion for Vector. - - A Vector is considered False if it's a zero vector (all components are zero), - and True otherwise. - - Returns: - False if vector is zero, True otherwise - """ - return not self.is_zero() - - -def to_numpy(value: Vector3 | np.ndarray | Sequence[int | float]) -> np.ndarray: - """Convert a value to a numpy array.""" - if isinstance(value, Vector3): - return value.to_numpy() - elif isinstance(value, np.ndarray): - return value - else: - return np.array(value, dtype=float) - - -def to_vector(value: VectorConvertable | Vector3) -> Vector3: - """Convert a vector-compatible value to a Vector3 object.""" - if isinstance(value, Vector3): - return value - return Vector3(value) - - -def to_tuple(value: Vector3 | np.ndarray | Sequence[int | float]) -> tuple[float, ...]: - """Convert a value to a tuple.""" - if isinstance(value, Vector3): - return value.to_tuple() - elif isinstance(value, np.ndarray): - return tuple(value.tolist()) - elif isinstance(value, tuple): - return value - else: - return tuple(value) - - -def to_list(value: Vector3 | np.ndarray | Sequence[int | float]) -> list[float]: - """Convert a value to a list.""" - if isinstance(value, Vector3): - return value.to_list() - elif isinstance(value, np.ndarray): - result: list[float] = value.tolist() - return result - elif isinstance(value, list): - return value - else: - return list(value) - - -VectorLike: TypeAlias = VectorConvertable | Vector3 - - -def make_vector3(x: float, y: float, z: float) -> Vector3: - return Vector3(x, y, z) diff --git a/dimos/msgs/geometry_msgs/Wrench.py b/dimos/msgs/geometry_msgs/Wrench.py deleted file mode 100644 index 7b3a4427758..00000000000 --- a/dimos/msgs/geometry_msgs/Wrench.py +++ /dev/null @@ -1,121 +0,0 @@ -# Copyright 2025-2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from __future__ import annotations - -from collections.abc import Sequence -from typing import Any, overload - -from dimos_lcm.geometry_msgs import Wrench as LCMWrench -import numpy as np - -from dimos.msgs.geometry_msgs.Vector3 import Vector3, VectorLike - - -class Wrench(LCMWrench): # type: ignore[misc] - """Force (N) and torque (Nm) in 3D space.""" - - force: Vector3 - torque: Vector3 - msg_name = "geometry_msgs.Wrench" - - @overload - def __init__(self) -> None: ... - - @overload - def __init__( - self, - force: VectorLike | None = ..., - torque: VectorLike | None = ..., - ) -> None: ... - - @overload - def __init__(self, wrench: Wrench | LCMWrench, /) -> None: ... - - def __init__(self, *args: Any, **kwargs: Any) -> None: - """Initialize a wrench. - - Supported forms: - Wrench() # zero wrench - Wrench(force, torque) - Wrench(force=..., torque=...) # keyword args, either optional - Wrench(other_wrench) # copy constructor - Wrench(lcm_wrench) # from LCM message - """ - force: Any = None - torque: Any = None - - if len(args) == 2: - force, torque = args - elif len(args) == 1: - value = args[0] - # Wrench before LCMWrench (it is a subclass); anything else is a - # force, same as the `force=` keyword. - if isinstance(value, Wrench | LCMWrench): - force, torque = value.force, value.torque - else: - force = value - elif args: - raise TypeError(f"Wrench takes 1 or 2 positional arguments ({len(args)} given)") - - if kwargs: - force = kwargs.pop("force", force) - torque = kwargs.pop("torque", torque) - if kwargs: - raise TypeError(f"Wrench got unexpected keyword arguments {sorted(kwargs)}") - - self.force = Vector3() if force is None else Vector3(force) - self.torque = Vector3() if torque is None else Vector3(torque) - - @classmethod - def from_array(cls, ft: Sequence[float] | np.ndarray) -> Wrench: - """Build from a 6-element ``[fx, fy, fz, tx, ty, tz]`` sensor reading.""" - if len(ft) != 6: - raise ValueError(f"Expected 6 elements [fx, fy, fz, tx, ty, tz], got {len(ft)}") - return cls(force=ft[:3], torque=ft[3:]) - - def to_array(self) -> np.ndarray: - """``[fx, fy, fz, tx, ty, tz]``.""" - return np.concatenate([self.force.to_numpy(), self.torque.to_numpy()]) - - @classmethod - def zero(cls) -> Wrench: - return cls() - - def is_zero(self) -> bool: - return self.force.is_zero() and self.torque.is_zero() - - def __add__(self, other: Wrench) -> Wrench: - if not isinstance(other, Wrench): - return NotImplemented - return Wrench(force=self.force + other.force, torque=self.torque + other.torque) - - def __sub__(self, other: Wrench) -> Wrench: - if not isinstance(other, Wrench): - return NotImplemented - return Wrench(force=self.force - other.force, torque=self.torque - other.torque) - - def __eq__(self, other: object) -> bool: - if not isinstance(other, Wrench): - return False - return self.force == other.force and self.torque == other.torque - - def __bool__(self) -> bool: - return not self.is_zero() - - def __repr__(self) -> str: - return f"Wrench(force={self.force!r}, torque={self.torque!r})" - - def __str__(self) -> str: - return f"Wrench:\n Force: {self.force}\n Torque: {self.torque}" diff --git a/dimos/msgs/geometry_msgs/WrenchStamped.py b/dimos/msgs/geometry_msgs/WrenchStamped.py deleted file mode 100644 index 12e6e940072..00000000000 --- a/dimos/msgs/geometry_msgs/WrenchStamped.py +++ /dev/null @@ -1,97 +0,0 @@ -# Copyright 2025-2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from __future__ import annotations - -from collections.abc import Sequence -import time -from typing import Any, BinaryIO - -from dimos_lcm.geometry_msgs import WrenchStamped as LCMWrenchStamped -import numpy as np - -from dimos.msgs.geometry_msgs.Wrench import Wrench -from dimos.types.timestamped import Timestamped - - -class WrenchStamped(Wrench, Timestamped): - """A force/torque measurement with a timestamp and frame_id.""" - - msg_name = "geometry_msgs.WrenchStamped" - ts: float - frame_id: str - - def __init__(self, *args: Any, **kwargs: Any) -> None: - """Initialize a stamped wrench. - - Takes ``(ts, frame_id)`` positionally, plus every Wrench keyword. Any other - positional form belongs to Wrench, so ``WrenchStamped(force, torque)`` works - as it does on the base class. - """ - ts: Any - # A leading number or string is a timestamp; anything else is a Wrench argument. - if args and isinstance(args[0], int | float | str): - ts = args[0] - frame_id = args[1] if len(args) > 1 else kwargs.pop("frame_id", "") - wrench_args: tuple[Any, ...] = () - else: - ts = kwargs.pop("ts", None) - frame_id = kwargs.pop("frame_id", "") - wrench_args = args - - self.frame_id = frame_id - self.ts = time.time() if ts is None else ts - super().__init__(*wrench_args, **kwargs) - - @classmethod - def from_array( # type: ignore[override] - cls, - ft: Sequence[float] | np.ndarray, - frame_id: str = "ft_sensor", - ts: float | None = None, - ) -> WrenchStamped: - """Build from a 6-element ``[fx, fy, fz, tx, ty, tz]`` sensor reading.""" - if len(ft) != 6: - raise ValueError(f"Expected 6 elements [fx, fy, fz, tx, ty, tz], got {len(ft)}") - return cls(ts=ts, frame_id=frame_id, force=ft[:3], torque=ft[3:]) - - def lcm_encode(self) -> bytes: - lcm_msg = LCMWrenchStamped() - lcm_msg.wrench = self - [lcm_msg.header.stamp.sec, lcm_msg.header.stamp.nsec] = self.ros_timestamp() - lcm_msg.header.frame_id = self.frame_id - return lcm_msg.lcm_encode() # type: ignore[no-any-return] - - @classmethod - def lcm_decode(cls, data: bytes | BinaryIO) -> WrenchStamped: - lcm_msg = LCMWrenchStamped.lcm_decode(data) - return cls( - ts=lcm_msg.header.stamp.sec + (lcm_msg.header.stamp.nsec / 1_000_000_000), - frame_id=lcm_msg.header.frame_id, - force=[lcm_msg.wrench.force.x, lcm_msg.wrench.force.y, lcm_msg.wrench.force.z], - torque=[lcm_msg.wrench.torque.x, lcm_msg.wrench.torque.y, lcm_msg.wrench.torque.z], - ) - - def __repr__(self) -> str: - return ( - f"WrenchStamped(force={self.force!r}, torque={self.torque!r}, " - f"ts={self.ts}, frame_id={self.frame_id!r})" - ) - - def __str__(self) -> str: - return ( - f"WrenchStamped(force=[{self.force.x:.3f}, {self.force.y:.3f}, {self.force.z:.3f}], " - f"torque=[{self.torque.x:.3f}, {self.torque.y:.3f}, {self.torque.z:.3f}], " - f"frame_id={self.frame_id!r})" - ) diff --git a/dimos/msgs/geometry_msgs/test_PointStamped.py b/dimos/msgs/geometry_msgs/test_PointStamped.py index 727d9190ed8..e4a27f14b88 100644 --- a/dimos/msgs/geometry_msgs/test_PointStamped.py +++ b/dimos/msgs/geometry_msgs/test_PointStamped.py @@ -12,53 +12,49 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""Tests for geometry_msgs.PointStamped — msg -> lcm bytes -> msg roundtrip.""" +"""Generated point/stamp roundtrips and explicit pose construction.""" -import time +from dimos_generated.geometry_msgs.msg import Point, PointStamped, Pose, PoseStamped, Quaternion +from dimos_generated.std_msgs.msg import Header +from rosbags.typesys import Stores, get_typestore -from dimos_lcm.geometry_msgs import Point as LCMPoint +from dimos.msgs.time import time_from_nanoseconds, time_from_seconds, to_nanoseconds -from dimos.msgs.geometry_msgs.Point import Point -from dimos.msgs.geometry_msgs.PointStamped import PointStamped +def test_point_has_standard_cdr_schema() -> None: + point = Point(x=1, y=2, z=3) + decoded = get_typestore(Stores.ROS2_JAZZY).deserialize_cdr(point.encode(), Point.msg_name) + assert (decoded.x, decoded.y, decoded.z) == (1, 2, 3) -def test_point_inherits_lcm() -> None: - """Point wrapper inherits from LCMPoint.""" - assert isinstance(Point(1.0, 2.0, 3.0), LCMPoint) - -def test_lcm_encode_decode() -> None: - """Test encoding and decoding of PointStamped to/from binary LCM format.""" +def test_cdr_encode_decode() -> None: source = PointStamped( - x=1.5, - y=-2.5, - z=3.5, - ts=time.time(), - frame_id="/world/grid", + point=Point(x=1.5, y=-2.5, z=3.5), + header=Header(stamp=time_from_nanoseconds(1700000000123456789), frame_id="/world/grid"), ) - binary_msg = source.lcm_encode() - dest = PointStamped.lcm_decode(binary_msg) - + dest = PointStamped.decode(source.encode()) assert isinstance(dest, PointStamped) assert dest is not source - assert dest.x == source.x - assert dest.y == source.y - assert dest.z == source.z - assert abs(dest.ts - source.ts) < 1e-6 - assert dest.frame_id == source.frame_id - + assert dest.point.x == source.point.x + assert dest.point.y == source.point.y + assert dest.point.z == source.point.z + assert to_nanoseconds(dest.header.stamp) == to_nanoseconds(source.header.stamp) + assert dest.header.frame_id == source.header.frame_id -def test_to_pose_stamped() -> None: - """Test conversion to PoseStamped with identity orientation.""" - from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped - - pt = PointStamped(x=1.0, y=2.0, z=3.0, ts=500.0, frame_id="/map") - pose = pt.to_pose_stamped() +def test_explicit_pose_stamped_conversion() -> None: + point = PointStamped( + point=Point(x=1, y=2, z=3), header=Header(stamp=time_from_seconds(500), frame_id="/map") + ) + pose = PoseStamped( + header=point.header, pose=Pose(position=point.point, orientation=Quaternion(w=1)) + ) assert isinstance(pose, PoseStamped) - assert pose.x == 1.0 - assert pose.y == 2.0 - assert pose.z == 3.0 - assert pose.orientation.w == 1.0 - assert pose.ts == 500.0 - assert pose.frame_id == "/map" + assert pose.pose.position.x == 1 + assert pose.pose.position.y == 2 + assert pose.pose.position.z == 3 + assert pose.pose.orientation.w == 1 + assert to_nanoseconds(pose.header.stamp) == 500000000000 + assert pose.header.frame_id == "/map" + pose.pose.position.x = 9 + assert point.point.x == 1 diff --git a/dimos/msgs/geometry_msgs/test_Pose.py b/dimos/msgs/geometry_msgs/test_Pose.py index dac5ed6207d..9dd6bb12a88 100644 --- a/dimos/msgs/geometry_msgs/test_Pose.py +++ b/dimos/msgs/geometry_msgs/test_Pose.py @@ -14,112 +14,92 @@ import pickle -from dimos_lcm.geometry_msgs import Pose as LCMPose +from dimos_generated.geometry_msgs.msg import Point, Pose, Quaternion, Vector3 import numpy as np import pytest - -from dimos.msgs.geometry_msgs.Pose import Pose, to_pose -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Vector3 import Vector3 +from rosbags.typesys import Stores, get_typestore + +from dimos.msgs.geometry import ( + compose_poses, + point_from_array, + quaternion_euler, + quaternion_from_array, + quaternion_from_euler, + vector_array, + vector_from_array, +) def test_pose_default_init() -> None: """Test that default initialization creates a pose at origin with identity orientation.""" pose = Pose() - - # Position should be at origin assert pose.position.x == 0.0 assert pose.position.y == 0.0 assert pose.position.z == 0.0 - - # Orientation should be identity quaternion assert pose.orientation.x == 0.0 assert pose.orientation.y == 0.0 assert pose.orientation.z == 0.0 assert pose.orientation.w == 1.0 - - # Test convenience properties - assert pose.x == 0.0 - assert pose.y == 0.0 - assert pose.z == 0.0 + assert pose.position.x == 0.0 + assert pose.position.y == 0.0 + assert pose.position.z == 0.0 def test_pose_pose_init() -> None: """Test initialization with position coordinates only (identity orientation).""" - pose_data = Pose(1.0, 2.0, 3.0) - - pose = to_pose(pose_data) - - # Position should be as specified + pose_data = Pose(position=Point(x=1.0, y=2.0, z=3.0)) + pose = pose_data assert pose.position.x == 1.0 assert pose.position.y == 2.0 assert pose.position.z == 3.0 - - # Orientation should be identity quaternion assert pose.orientation.x == 0.0 assert pose.orientation.y == 0.0 assert pose.orientation.z == 0.0 assert pose.orientation.w == 1.0 - - # Test convenience properties - assert pose.x == 1.0 - assert pose.y == 2.0 - assert pose.z == 3.0 + assert pose.position.x == 1.0 + assert pose.position.y == 2.0 + assert pose.position.z == 3.0 def test_pose_position_init() -> None: """Test initialization with position coordinates only (identity orientation).""" - pose = Pose(1.0, 2.0, 3.0) - - # Position should be as specified + pose = Pose(position=Point(x=1.0, y=2.0, z=3.0)) assert pose.position.x == 1.0 assert pose.position.y == 2.0 assert pose.position.z == 3.0 - - # Orientation should be identity quaternion assert pose.orientation.x == 0.0 assert pose.orientation.y == 0.0 assert pose.orientation.z == 0.0 assert pose.orientation.w == 1.0 - - # Test convenience properties - assert pose.x == 1.0 - assert pose.y == 2.0 - assert pose.z == 3.0 + assert pose.position.x == 1.0 + assert pose.position.y == 2.0 + assert pose.position.z == 3.0 def test_pose_full_init() -> None: """Test initialization with position and orientation coordinates.""" - pose = Pose(1.0, 2.0, 3.0, 0.1, 0.2, 0.3, 0.9) - - # Position should be as specified + pose = Pose( + position=Point(x=1.0, y=2.0, z=3.0), orientation=Quaternion(x=0.1, y=0.2, z=0.3, w=0.9) + ) assert pose.position.x == 1.0 assert pose.position.y == 2.0 assert pose.position.z == 3.0 - - # Orientation should be as specified assert pose.orientation.x == 0.1 assert pose.orientation.y == 0.2 assert pose.orientation.z == 0.3 assert pose.orientation.w == 0.9 - - # Test convenience properties - assert pose.x == 1.0 - assert pose.y == 2.0 - assert pose.z == 3.0 + assert pose.position.x == 1.0 + assert pose.position.y == 2.0 + assert pose.position.z == 3.0 def test_pose_vector_position_init() -> None: """Test initialization with Vector3 position (identity orientation).""" - position = Vector3(4.0, 5.0, 6.0) - pose = Pose(position) - - # Position should match the vector + position = Vector3(x=4.0, y=5.0, z=6.0) + pose = Pose(position=point_from_array(vector_array(position))) assert pose.position.x == 4.0 assert pose.position.y == 5.0 assert pose.position.z == 6.0 - - # Orientation should be identity assert pose.orientation.x == 0.0 assert pose.orientation.y == 0.0 assert pose.orientation.z == 0.0 @@ -128,16 +108,12 @@ def test_pose_vector_position_init() -> None: def test_pose_vector_quaternion_init() -> None: """Test initialization with Vector3 position and Quaternion orientation.""" - position = Vector3(1.0, 2.0, 3.0) - orientation = Quaternion(0.1, 0.2, 0.3, 0.9) - pose = Pose(position, orientation) - - # Position should match the vector + position = Vector3(x=1.0, y=2.0, z=3.0) + orientation = Quaternion(x=0.1, y=0.2, z=0.3, w=0.9) + pose = Pose(position=point_from_array(vector_array(position)), orientation=orientation) assert pose.position.x == 1.0 assert pose.position.y == 2.0 assert pose.position.z == 3.0 - - # Orientation should match the quaternion assert pose.orientation.x == 0.1 assert pose.orientation.y == 0.2 assert pose.orientation.z == 0.3 @@ -148,14 +124,13 @@ def test_pose_list_init() -> None: """Test initialization with lists for position and orientation.""" position_list = [1.0, 2.0, 3.0] orientation_list = [0.1, 0.2, 0.3, 0.9] - pose = Pose(position_list, orientation_list) - - # Position should match the list + pose = Pose( + position=point_from_array(position_list), + orientation=quaternion_from_array(orientation_list), + ) assert pose.position.x == 1.0 assert pose.position.y == 2.0 assert pose.position.z == 3.0 - - # Orientation should match the list assert pose.orientation.x == 0.1 assert pose.orientation.y == 0.2 assert pose.orientation.z == 0.3 @@ -167,14 +142,12 @@ def test_pose_tuple_init() -> None: position = [1.0, 2.0, 3.0] orientation = [0.1, 0.2, 0.3, 0.9] pose_tuple = (position, orientation) - pose = Pose(pose_tuple) - - # Position should match + pose = Pose( + position=point_from_array(pose_tuple[0]), orientation=quaternion_from_array(pose_tuple[1]) + ) assert pose.position.x == 1.0 assert pose.position.y == 2.0 assert pose.position.z == 3.0 - - # Orientation should match assert pose.orientation.x == 0.1 assert pose.orientation.y == 0.2 assert pose.orientation.z == 0.3 @@ -184,14 +157,13 @@ def test_pose_tuple_init() -> None: def test_pose_dict_init() -> None: """Test initialization from a dictionary with 'position' and 'orientation' keys.""" pose_dict = {"position": [1.0, 2.0, 3.0], "orientation": [0.1, 0.2, 0.3, 0.9]} - pose = Pose(pose_dict) - - # Position should match + pose = Pose( + position=point_from_array(pose_dict["position"]), + orientation=quaternion_from_array(pose_dict["orientation"]), + ) assert pose.position.x == 1.0 assert pose.position.y == 2.0 assert pose.position.z == 3.0 - - # Orientation should match assert pose.orientation.x == 0.1 assert pose.orientation.y == 0.2 assert pose.orientation.z == 0.3 @@ -200,45 +172,35 @@ def test_pose_dict_init() -> None: def test_pose_copy_init() -> None: """Test initialization from another Pose (copy constructor).""" - original = Pose(1.0, 2.0, 3.0, 0.1, 0.2, 0.3, 0.9) - copy = Pose(original) - - # Position should match + original = Pose( + position=Point(x=1.0, y=2.0, z=3.0), orientation=Quaternion(x=0.1, y=0.2, z=0.3, w=0.9) + ) + copy = Pose.decode(original.encode()) assert copy.position.x == 1.0 assert copy.position.y == 2.0 assert copy.position.z == 3.0 - - # Orientation should match assert copy.orientation.x == 0.1 assert copy.orientation.y == 0.2 assert copy.orientation.z == 0.3 assert copy.orientation.w == 0.9 - - # Should be a copy, not the same object assert copy is not original assert copy == original -def test_pose_lcm_init() -> None: - """Test initialization from an LCM Pose.""" - # Create LCM pose - lcm_pose = LCMPose() - lcm_pose.position.x = 1.0 - lcm_pose.position.y = 2.0 - lcm_pose.position.z = 3.0 - lcm_pose.orientation.x = 0.1 - lcm_pose.orientation.y = 0.2 - lcm_pose.orientation.z = 0.3 - lcm_pose.orientation.w = 0.9 - - pose = Pose(lcm_pose) - - # Position should match +def test_pose_mutated_value_copy() -> None: + """Test initialization from a generated Pose.""" + source_pose = Pose() + source_pose.position.x = 1.0 + source_pose.position.y = 2.0 + source_pose.position.z = 3.0 + source_pose.orientation.x = 0.1 + source_pose.orientation.y = 0.2 + source_pose.orientation.z = 0.3 + source_pose.orientation.w = 0.9 + pose = Pose.decode(source_pose.encode()) assert pose.position.x == 1.0 assert pose.position.y == 2.0 assert pose.position.z == 3.0 - - # Orientation should match assert pose.orientation.x == 0.1 assert pose.orientation.y == 0.2 assert pose.orientation.z == 0.3 @@ -247,86 +209,63 @@ def test_pose_lcm_init() -> None: def test_pose_properties() -> None: """Test pose property access.""" - pose = Pose(1.0, 2.0, 3.0, 0.1, 0.2, 0.3, 0.9) - - # Test position properties - assert pose.x == 1.0 - assert pose.y == 2.0 - assert pose.z == 3.0 - - # Test orientation properties (through quaternion's to_euler method) - euler = pose.orientation.to_euler() - assert pose.roll == euler.x - assert pose.pitch == euler.y - assert pose.yaw == euler.z + pose = Pose( + position=Point(x=1.0, y=2.0, z=3.0), orientation=Quaternion(x=0.1, y=0.2, z=0.3, w=0.9) + ) + assert pose.position.x == 1.0 + assert pose.position.y == 2.0 + assert pose.position.z == 3.0 + euler = vector_from_array(quaternion_euler(pose.orientation)) + assert quaternion_euler(pose.orientation)[0] == euler.x + assert quaternion_euler(pose.orientation)[1] == euler.y + assert quaternion_euler(pose.orientation)[2] == euler.z def test_pose_euler_properties_identity() -> None: """Test pose Euler angle properties with identity orientation.""" - pose = Pose(1.0, 2.0, 3.0) # Identity orientation - - # Identity quaternion should give zero Euler angles - assert np.isclose(pose.roll, 0.0, atol=1e-10) - assert np.isclose(pose.pitch, 0.0, atol=1e-10) - assert np.isclose(pose.yaw, 0.0, atol=1e-10) - - # Euler property should also be zeros - assert np.isclose(pose.orientation.euler.x, 0.0, atol=1e-10) - assert np.isclose(pose.orientation.euler.y, 0.0, atol=1e-10) - assert np.isclose(pose.orientation.euler.z, 0.0, atol=1e-10) - - -def test_pose_repr() -> None: - """Test pose string representation.""" - pose = Pose(1.234, 2.567, 3.891, 0.1, 0.2, 0.3, 0.9) - - repr_str = repr(pose) - - # Should contain position and orientation info - assert "Pose" in repr_str - assert "position" in repr_str - assert "orientation" in repr_str - - # Should contain the actual values (approximately) - assert "1.234" in repr_str or "1.23" in repr_str - assert "2.567" in repr_str or "2.57" in repr_str - - -def test_pose_str() -> None: - """Test pose string formatting.""" - pose = Pose(1.234, 2.567, 3.891, 0.1, 0.2, 0.3, 0.9) - - str_repr = str(pose) - - # Should contain position coordinates - assert "1.234" in str_repr - assert "2.567" in str_repr - assert "3.891" in str_repr - - # Should contain Euler angles - assert "euler" in str_repr - - # Should be formatted with specified precision - assert str_repr.count("Pose") == 1 + pose = Pose(position=Point(x=1.0, y=2.0, z=3.0)) + assert np.isclose(quaternion_euler(pose.orientation)[0], 0.0, atol=1e-10) + assert np.isclose(quaternion_euler(pose.orientation)[1], 0.0, atol=1e-10) + assert np.isclose(quaternion_euler(pose.orientation)[2], 0.0, atol=1e-10) + assert np.isclose(vector_from_array(quaternion_euler(pose.orientation)).x, 0.0, atol=1e-10) + assert np.isclose(vector_from_array(quaternion_euler(pose.orientation)).y, 0.0, atol=1e-10) + assert np.isclose(vector_from_array(quaternion_euler(pose.orientation)).z, 0.0, atol=1e-10) + + +def test_pose_independent_cdr_fields() -> None: + source = Pose( + position=Point(x=1.234, y=2.567, z=3.891), + orientation=Quaternion(x=0.1, y=0.2, z=0.3, w=0.9), + ) + decoded = get_typestore(Stores.ROS2_JAZZY).deserialize_cdr(source.encode(), Pose.msg_name) + assert (decoded.position.x, decoded.position.y, decoded.position.z) == (1.234, 2.567, 3.891) + assert ( + decoded.orientation.x, + decoded.orientation.y, + decoded.orientation.z, + decoded.orientation.w, + ) == (0.1, 0.2, 0.3, 0.9) def test_pose_equality() -> None: """Test pose equality comparison.""" - pose1 = Pose(1.0, 2.0, 3.0, 0.1, 0.2, 0.3, 0.9) - pose2 = Pose(1.0, 2.0, 3.0, 0.1, 0.2, 0.3, 0.9) - pose3 = Pose(1.1, 2.0, 3.0, 0.1, 0.2, 0.3, 0.9) # Different position - pose4 = Pose(1.0, 2.0, 3.0, 0.11, 0.2, 0.3, 0.9) # Different orientation - - # Equal poses + pose1 = Pose( + position=Point(x=1.0, y=2.0, z=3.0), orientation=Quaternion(x=0.1, y=0.2, z=0.3, w=0.9) + ) + pose2 = Pose( + position=Point(x=1.0, y=2.0, z=3.0), orientation=Quaternion(x=0.1, y=0.2, z=0.3, w=0.9) + ) + pose3 = Pose( + position=Point(x=1.1, y=2.0, z=3.0), orientation=Quaternion(x=0.1, y=0.2, z=0.3, w=0.9) + ) + pose4 = Pose( + position=Point(x=1.0, y=2.0, z=3.0), orientation=Quaternion(x=0.11, y=0.2, z=0.3, w=0.9) + ) assert pose1 == pose2 assert pose2 == pose1 - - # Different poses assert pose1 != pose3 assert pose1 != pose4 assert pose3 != pose4 - - # Different types assert pose1 != "not a pose" assert pose1 != [1.0, 2.0, 3.0] assert pose1 is not None @@ -336,15 +275,13 @@ def test_pose_with_numpy_arrays() -> None: """Test pose initialization with numpy arrays.""" position_array = np.array([1.0, 2.0, 3.0]) orientation_array = np.array([0.1, 0.2, 0.3, 0.9]) - - pose = Pose(position_array, orientation_array) - - # Position should match + pose = Pose( + position=point_from_array(position_array), + orientation=quaternion_from_array(orientation_array), + ) assert pose.position.x == 1.0 assert pose.position.y == 2.0 assert pose.position.z == 3.0 - - # Orientation should match assert pose.orientation.x == 0.1 assert pose.orientation.y == 0.2 assert pose.orientation.z == 0.3 @@ -353,15 +290,13 @@ def test_pose_with_numpy_arrays() -> None: def test_pose_with_mixed_types() -> None: """Test pose initialization with mixed input types.""" - # Position as tuple, orientation as list - pose1 = Pose((1.0, 2.0, 3.0), [0.1, 0.2, 0.3, 0.9]) - - # Position as numpy array, orientation as Vector3/Quaternion + pose1 = Pose( + position=point_from_array((1.0, 2.0, 3.0)), + orientation=quaternion_from_array([0.1, 0.2, 0.3, 0.9]), + ) position = np.array([1.0, 2.0, 3.0]) - orientation = Quaternion(0.1, 0.2, 0.3, 0.9) - pose2 = Pose(position, orientation) - - # Both should result in the same pose + orientation = Quaternion(x=0.1, y=0.2, z=0.3, w=0.9) + pose2 = Pose(position=point_from_array(position), orientation=orientation) assert pose1.position.x == pose2.position.x assert pose1.position.y == pose2.position.y assert pose1.position.z == pose2.position.z @@ -371,24 +306,20 @@ def test_pose_with_mixed_types() -> None: assert pose1.orientation.w == pose2.orientation.w -def test_to_pose_passthrough() -> None: - """Test to_pose function with Pose input (passthrough).""" - original = Pose(1.0, 2.0, 3.0, 0.1, 0.2, 0.3, 0.9) - result = to_pose(original) - - # Should be the same object (passthrough) - assert result is original +def test_pose_nested_value_copy() -> None: + source = Pose(position=Point(x=1, y=2, z=3)) + copied = Pose(position=source.position, orientation=source.orientation) + copied.position.x = 10 + assert source.position == Point(x=1, y=2, z=3) + assert copied.position == Point(x=10, y=2, z=3) def test_to_pose_conversion() -> None: """Test to_pose function with convertible inputs.""" - # Note: The to_pose conversion function has type checking issues in the current implementation - # Test direct construction instead to verify the intended functionality - - # Test the intended functionality by creating poses directly pose_tuple = ([1.0, 2.0, 3.0], [0.1, 0.2, 0.3, 0.9]) - result1 = Pose(pose_tuple) - + result1 = Pose( + position=point_from_array(pose_tuple[0]), orientation=quaternion_from_array(pose_tuple[1]) + ) assert isinstance(result1, Pose) assert result1.position.x == 1.0 assert result1.position.y == 2.0 @@ -397,11 +328,11 @@ def test_to_pose_conversion() -> None: assert result1.orientation.y == 0.2 assert result1.orientation.z == 0.3 assert result1.orientation.w == 0.9 - - # Test with dictionary pose_dict = {"position": [1.0, 2.0, 3.0], "orientation": [0.1, 0.2, 0.3, 0.9]} - result2 = Pose(pose_dict) - + result2 = Pose( + position=point_from_array(pose_dict["position"]), + orientation=quaternion_from_array(pose_dict["orientation"]), + ) assert isinstance(result2, Pose) assert result2.position.x == 1.0 assert result2.position.y == 2.0 @@ -414,71 +345,56 @@ def test_to_pose_conversion() -> None: def test_pose_euler_roundtrip() -> None: """Test conversion from Euler angles to quaternion and back.""" - # Start with known Euler angles (small angles to avoid gimbal lock) roll = 0.1 pitch = 0.2 yaw = 0.3 - - # Create quaternion from Euler angles - euler_vector = Vector3(roll, pitch, yaw) - quaternion = euler_vector.to_quaternion() - - # Create pose with this quaternion - pose = Pose(Vector3(0, 0, 0), quaternion) - - # Convert back to Euler angles - result_euler = pose.orientation.euler - - # Should get back the original Euler angles (within tolerance) - assert np.isclose(result_euler.x, roll, atol=1e-6) - assert np.isclose(result_euler.y, pitch, atol=1e-6) - assert np.isclose(result_euler.z, yaw, atol=1e-6) + euler_vector = Vector3(x=roll, y=pitch, z=yaw) + quaternion = quaternion_from_euler(euler_vector.x, euler_vector.y, euler_vector.z) + pose = Pose( + position=point_from_array(vector_array(Vector3(x=0, y=0, z=0))), orientation=quaternion + ) + result_euler = vector_from_array(quaternion_euler(pose.orientation)) + assert np.isclose(result_euler.x, roll, atol=1e-06) + assert np.isclose(result_euler.y, pitch, atol=1e-06) + assert np.isclose(result_euler.z, yaw, atol=1e-06) def test_pose_zero_position() -> None: """Test pose with zero position vector.""" - # Use manual construction since Vector3.zeros has signature issues - pose = Pose(0.0, 0.0, 0.0) # Position at origin with identity orientation - - assert pose.x == 0.0 - assert pose.y == 0.0 - assert pose.z == 0.0 - assert np.isclose(pose.roll, 0.0, atol=1e-10) - assert np.isclose(pose.pitch, 0.0, atol=1e-10) - assert np.isclose(pose.yaw, 0.0, atol=1e-10) + pose = Pose(position=Point(x=0.0, y=0.0, z=0.0)) + assert pose.position.x == 0.0 + assert pose.position.y == 0.0 + assert pose.position.z == 0.0 + assert np.isclose(quaternion_euler(pose.orientation)[0], 0.0, atol=1e-10) + assert np.isclose(quaternion_euler(pose.orientation)[1], 0.0, atol=1e-10) + assert np.isclose(quaternion_euler(pose.orientation)[2], 0.0, atol=1e-10) def test_pose_unit_vectors() -> None: """Test pose with unit vector positions.""" - # Test unit x vector position - pose_x = Pose(Vector3.unit_x()) - assert pose_x.x == 1.0 - assert pose_x.y == 0.0 - assert pose_x.z == 0.0 - - # Test unit y vector position - pose_y = Pose(Vector3.unit_y()) - assert pose_y.x == 0.0 - assert pose_y.y == 1.0 - assert pose_y.z == 0.0 - - # Test unit z vector position - pose_z = Pose(Vector3.unit_z()) - assert pose_z.x == 0.0 - assert pose_z.y == 0.0 - assert pose_z.z == 1.0 + pose_x = Pose(position=point_from_array(vector_array(Vector3(x=1)))) + assert pose_x.position.x == 1.0 + assert pose_x.position.y == 0.0 + assert pose_x.position.z == 0.0 + pose_y = Pose(position=point_from_array(vector_array(Vector3(y=1)))) + assert pose_y.position.x == 0.0 + assert pose_y.position.y == 1.0 + assert pose_y.position.z == 0.0 + pose_z = Pose(position=point_from_array(vector_array(Vector3(z=1)))) + assert pose_z.position.x == 0.0 + assert pose_z.position.y == 0.0 + assert pose_z.position.z == 1.0 def test_pose_negative_coordinates() -> None: """Test pose with negative coordinates.""" - pose = Pose(-1.0, -2.0, -3.0, -0.1, -0.2, -0.3, 0.9) - - # Position should be negative - assert pose.x == -1.0 - assert pose.y == -2.0 - assert pose.z == -3.0 - - # Orientation should be as specified + pose = Pose( + position=Point(x=-1.0, y=-2.0, z=-3.0), + orientation=Quaternion(x=-0.1, y=-0.2, z=-0.3, w=0.9), + ) + assert pose.position.x == -1.0 + assert pose.position.y == -2.0 + assert pose.position.z == -3.0 assert pose.orientation.x == -0.1 assert pose.orientation.y == -0.2 assert pose.orientation.z == -0.3 @@ -488,13 +404,10 @@ def test_pose_negative_coordinates() -> None: def test_pose_large_coordinates() -> None: """Test pose with large coordinate values.""" large_value = 1000.0 - pose = Pose(large_value, large_value, large_value) - - assert pose.x == large_value - assert pose.y == large_value - assert pose.z == large_value - - # Orientation should still be identity + pose = Pose(position=Point(x=large_value, y=large_value, z=large_value)) + assert pose.position.x == large_value + assert pose.position.y == large_value + assert pose.position.z == large_value assert pose.orientation.x == 0.0 assert pose.orientation.y == 0.0 assert pose.orientation.z == 0.0 @@ -507,13 +420,10 @@ def test_pose_large_coordinates() -> None: ) def test_pose_parametrized_positions(x, y, z) -> None: """Parametrized test for various position values.""" - pose = Pose(x, y, z) - - assert pose.x == x - assert pose.y == y - assert pose.z == z - - # Should have identity orientation + pose = Pose(position=Point(x=x, y=y, z=z)) + assert pose.position.x == x + assert pose.position.y == y + assert pose.position.z == z assert pose.orientation.x == 0.0 assert pose.orientation.y == 0.0 assert pose.orientation.z == 0.0 @@ -523,53 +433,52 @@ def test_pose_parametrized_positions(x, y, z) -> None: @pytest.mark.parametrize( "qx,qy,qz,qw", [ - (0.0, 0.0, 0.0, 1.0), # Identity - (1.0, 0.0, 0.0, 0.0), # 180° around x - (0.0, 1.0, 0.0, 0.0), # 180° around y - (0.0, 0.0, 1.0, 0.0), # 180° around z - (0.5, 0.5, 0.5, 0.5), # Equal components + (0.0, 0.0, 0.0, 1.0), + (1.0, 0.0, 0.0, 0.0), + (0.0, 1.0, 0.0, 0.0), + (0.0, 0.0, 1.0, 0.0), + (0.5, 0.5, 0.5, 0.5), ], ) def test_pose_parametrized_orientations(qx, qy, qz, qw) -> None: """Parametrized test for various orientation values.""" - pose = Pose(0.0, 0.0, 0.0, qx, qy, qz, qw) - - # Position should be at origin - assert pose.x == 0.0 - assert pose.y == 0.0 - assert pose.z == 0.0 - - # Orientation should match + pose = Pose(position=Point(x=0.0, y=0.0, z=0.0), orientation=Quaternion(x=qx, y=qy, z=qz, w=qw)) + assert pose.position.x == 0.0 + assert pose.position.y == 0.0 + assert pose.position.z == 0.0 assert pose.orientation.x == qx assert pose.orientation.y == qy assert pose.orientation.z == qz assert pose.orientation.w == qw -def test_lcm_encode_decode() -> None: - """Test encoding and decoding of Pose to/from binary LCM format.""" +def test_cdr_encode_decode() -> None: + """Test encoding and decoding of Pose to/from binary CDR format.""" def encodepass() -> None: - pose_source = Pose(1.0, 2.0, 3.0, 0.1, 0.2, 0.3, 0.9) - binary_msg = pose_source.lcm_encode() - pose_dest = Pose.lcm_decode(binary_msg) + pose_source = Pose( + position=Point(x=1.0, y=2.0, z=3.0), orientation=Quaternion(x=0.1, y=0.2, z=0.3, w=0.9) + ) + binary_msg = pose_source.encode() + pose_dest = Pose.decode(binary_msg) assert isinstance(pose_dest, Pose) assert pose_dest is not pose_source assert pose_dest == pose_source - # Verify we get our custom types back - assert isinstance(pose_dest.position, Vector3) + assert isinstance(pose_dest.position, Point) assert isinstance(pose_dest.orientation, Quaternion) import timeit - print(f"{timeit.timeit(encodepass, number=1000)} ms per cycle") + timeit.timeit(encodepass, number=1000) def test_pickle_encode_decode() -> None: - """Test encoding and decoding of Pose to/from binary LCM format.""" + """Test encoding and decoding of Pose to/from binary CDR format.""" def encodepass() -> None: - pose_source = Pose(1.0, 2.0, 3.0, 0.1, 0.2, 0.3, 0.9) + pose_source = Pose( + position=Point(x=1.0, y=2.0, z=3.0), orientation=Quaternion(x=0.1, y=0.2, z=0.3, w=0.9) + ) binary_msg = pickle.dumps(pose_source) pose_dest = pickle.loads(binary_msg) assert isinstance(pose_dest, Pose) @@ -578,23 +487,17 @@ def encodepass() -> None: import timeit - print(f"{timeit.timeit(encodepass, number=1000)} ms per cycle") + timeit.timeit(encodepass, number=1000) def test_pose_addition_translation_only() -> None: """Test pose addition with translation only (identity rotations).""" - # Two poses with only translations - pose1 = Pose(1.0, 2.0, 3.0) # First translation - pose2 = Pose(4.0, 5.0, 6.0) # Second translation - - # Adding should combine translations - result = pose1 + pose2 - - assert result.position.x == 5.0 # 1 + 4 - assert result.position.y == 7.0 # 2 + 5 - assert result.position.z == 9.0 # 3 + 6 - - # Orientation should remain identity + pose1 = Pose(position=Point(x=1.0, y=2.0, z=3.0)) + pose2 = Pose(position=Point(x=4.0, y=5.0, z=6.0)) + result = compose_poses(pose1, pose2) + assert result.position.x == 5.0 + assert result.position.y == 7.0 + assert result.position.z == 9.0 assert result.orientation.x == 0.0 assert result.orientation.y == 0.0 assert result.orientation.z == 0.0 @@ -603,23 +506,16 @@ def test_pose_addition_translation_only() -> None: def test_pose_addition_with_rotation() -> None: """Test pose addition with rotation applied to translation.""" - # First pose: at origin, rotated 90 degrees around Z (yaw) - # 90 degree rotation quaternion around Z: (0, 0, sin(pi/4), cos(pi/4)) - angle = np.pi / 2 # 90 degrees - pose1 = Pose(0.0, 0.0, 0.0, 0.0, 0.0, np.sin(angle / 2), np.cos(angle / 2)) - - # Second pose: 1 unit forward (along X in its frame) - pose2 = Pose(1.0, 0.0, 0.0) - - # After rotation, the forward direction should be along Y - result = pose1 + pose2 - - # Position should be rotated + angle = np.pi / 2 + pose1 = Pose( + position=Point(x=0.0, y=0.0, z=0.0), + orientation=Quaternion(x=0.0, y=0.0, z=np.sin(angle / 2), w=np.cos(angle / 2)), + ) + pose2 = Pose(position=Point(x=1.0, y=0.0, z=0.0)) + result = compose_poses(pose1, pose2) assert np.isclose(result.position.x, 0.0, atol=1e-10) assert np.isclose(result.position.y, 1.0, atol=1e-10) assert np.isclose(result.position.z, 0.0, atol=1e-10) - - # Orientation should be same as pose1 (pose2 has identity rotation) assert np.isclose(result.orientation.x, 0.0, atol=1e-10) assert np.isclose(result.orientation.y, 0.0, atol=1e-10) assert np.isclose(result.orientation.z, np.sin(angle / 2), atol=1e-10) @@ -628,62 +524,45 @@ def test_pose_addition_with_rotation() -> None: def test_pose_addition_rotation_composition() -> None: """Test that rotations are properly composed.""" - # First pose: 45 degrees around Z - angle1 = np.pi / 4 # 45 degrees - pose1 = Pose(0.0, 0.0, 0.0, 0.0, 0.0, np.sin(angle1 / 2), np.cos(angle1 / 2)) - - # Second pose: another 45 degrees around Z - angle2 = np.pi / 4 # 45 degrees - pose2 = Pose(0.0, 0.0, 0.0, 0.0, 0.0, np.sin(angle2 / 2), np.cos(angle2 / 2)) - - # Result should be 90 degrees around Z - result = pose1 + pose2 - - # Check final angle is 90 degrees - expected_angle = angle1 + angle2 # 90 degrees + angle1 = np.pi / 4 + pose1 = Pose( + position=Point(x=0.0, y=0.0, z=0.0), + orientation=Quaternion(x=0.0, y=0.0, z=np.sin(angle1 / 2), w=np.cos(angle1 / 2)), + ) + angle2 = np.pi / 4 + pose2 = Pose( + position=Point(x=0.0, y=0.0, z=0.0), + orientation=Quaternion(x=0.0, y=0.0, z=np.sin(angle2 / 2), w=np.cos(angle2 / 2)), + ) + result = compose_poses(pose1, pose2) + expected_angle = angle1 + angle2 expected_qz = np.sin(expected_angle / 2) expected_qw = np.cos(expected_angle / 2) - assert np.isclose(result.orientation.z, expected_qz, atol=1e-10) assert np.isclose(result.orientation.w, expected_qw, atol=1e-10) def test_pose_addition_full_transform() -> None: """Test full pose composition with translation and rotation.""" - # Robot pose: at (2, 1, 0), facing 90 degrees left (positive yaw) - robot_yaw = np.pi / 2 # 90 degrees - robot_pose = Pose(2.0, 1.0, 0.0, 0.0, 0.0, np.sin(robot_yaw / 2), np.cos(robot_yaw / 2)) - - # Object in robot frame: 3 units forward, 1 unit right - object_in_robot = Pose(3.0, -1.0, 0.0) - - # Compose to get object in world frame - object_in_world = robot_pose + object_in_robot - - # Robot is facing left (90 degrees), so: - # - Robot's forward (X) is world's negative Y - # - Robot's right (negative Y) is world's X - # So object should be at: robot_pos + rotated_offset - # rotated_offset: (3, -1) rotated 90° CCW = (1, 3) - assert np.isclose(object_in_world.position.x, 3.0, atol=1e-10) # 2 + 1 - assert np.isclose(object_in_world.position.y, 4.0, atol=1e-10) # 1 + 3 + robot_yaw = np.pi / 2 + robot_pose = Pose( + position=Point(x=2.0, y=1.0, z=0.0), + orientation=Quaternion(x=0.0, y=0.0, z=np.sin(robot_yaw / 2), w=np.cos(robot_yaw / 2)), + ) + object_in_robot = Pose(position=Point(x=3.0, y=-1.0, z=0.0)) + object_in_world = compose_poses(robot_pose, object_in_robot) + assert np.isclose(object_in_world.position.x, 3.0, atol=1e-10) + assert np.isclose(object_in_world.position.y, 4.0, atol=1e-10) assert np.isclose(object_in_world.position.z, 0.0, atol=1e-10) - - # Orientation should match robot's orientation (object has no rotation) - assert np.isclose(object_in_world.yaw, robot_yaw, atol=1e-10) + assert np.isclose(quaternion_euler(object_in_world.orientation)[2], robot_yaw, atol=1e-10) def test_pose_addition_chain() -> None: """Test chaining multiple pose additions.""" - # Create a chain of transformations - pose1 = Pose(1.0, 0.0, 0.0) # Move 1 unit in X - pose2 = Pose(0.0, 1.0, 0.0) # Move 1 unit in Y (relative to pose1) - pose3 = Pose(0.0, 0.0, 1.0) # Move 1 unit in Z (relative to pose1+pose2) - - # Chain them together - result = pose1 + pose2 + pose3 - - # Should accumulate all translations + pose1 = Pose(position=Point(x=1.0, y=0.0, z=0.0)) + pose2 = Pose(position=Point(x=0.0, y=1.0, z=0.0)) + pose3 = Pose(position=Point(x=0.0, y=0.0, z=1.0)) + result = compose_poses(compose_poses(pose1, pose2), pose3) assert result.position.x == 1.0 assert result.position.y == 1.0 assert result.position.z == 1.0 @@ -691,18 +570,26 @@ def test_pose_addition_chain() -> None: def test_pose_addition_with_convertible() -> None: """Test pose addition with convertible types.""" - pose1 = Pose(1.0, 2.0, 3.0) - - # Add with tuple + pose1 = Pose(position=Point(x=1.0, y=2.0, z=3.0)) pose_tuple = ([4.0, 5.0, 6.0], [0.0, 0.0, 0.0, 1.0]) - result1 = pose1 + pose_tuple + result1 = compose_poses( + pose1, + Pose( + position=point_from_array(pose_tuple[0]), + orientation=quaternion_from_array(pose_tuple[1]), + ), + ) assert result1.position.x == 5.0 assert result1.position.y == 7.0 assert result1.position.z == 9.0 - - # Add with dict pose_dict = {"position": [1.0, 0.0, 0.0], "orientation": [0.0, 0.0, 0.0, 1.0]} - result2 = pose1 + pose_dict + result2 = compose_poses( + pose1, + Pose( + position=point_from_array(pose_dict["position"]), + orientation=quaternion_from_array(pose_dict["orientation"]), + ), + ) assert result2.position.x == 2.0 assert result2.position.y == 2.0 assert result2.position.z == 3.0 @@ -710,12 +597,11 @@ def test_pose_addition_with_convertible() -> None: def test_pose_identity_addition() -> None: """Test that adding identity pose leaves pose unchanged.""" - pose = Pose(1.0, 2.0, 3.0, 0.1, 0.2, 0.3, 0.9) - identity = Pose() # Identity pose at origin - - result = pose + identity - - # Should be unchanged + pose = Pose( + position=Point(x=1.0, y=2.0, z=3.0), orientation=Quaternion(x=0.1, y=0.2, z=0.3, w=0.9) + ) + identity = Pose() + result = compose_poses(pose, identity) assert result.position.x == pose.position.x assert result.position.y == pose.position.y assert result.position.z == pose.position.z @@ -727,23 +613,15 @@ def test_pose_identity_addition() -> None: def test_pose_addition_3d_rotation() -> None: """Test pose addition with 3D rotations.""" - # First pose: rotated around X axis (roll) - roll = np.pi / 4 # 45 degrees - pose1 = Pose(1.0, 0.0, 0.0, np.sin(roll / 2), 0.0, 0.0, np.cos(roll / 2)) - - # Second pose: movement along Y and Z in local frame - pose2 = Pose(0.0, 1.0, 1.0) - - # Compose transformations - result = pose1 + pose2 - - # The Y and Z movement should be rotated around X - # After 45° rotation around X: - # - Local Y -> world Y * cos(45°) - Z * sin(45°) - # - Local Z -> world Y * sin(45°) + Z * cos(45°) + roll = np.pi / 4 + pose1 = Pose( + position=Point(x=1.0, y=0.0, z=0.0), + orientation=Quaternion(x=np.sin(roll / 2), y=0.0, z=0.0, w=np.cos(roll / 2)), + ) + pose2 = Pose(position=Point(x=0.0, y=1.0, z=1.0)) + result = compose_poses(pose1, pose2) cos45 = np.cos(roll) sin45 = np.sin(roll) - - assert np.isclose(result.position.x, 1.0, atol=1e-10) # X unchanged + assert np.isclose(result.position.x, 1.0, atol=1e-10) assert np.isclose(result.position.y, cos45 - sin45, atol=1e-10) assert np.isclose(result.position.z, sin45 + cos45, atol=1e-10) diff --git a/dimos/msgs/geometry_msgs/test_PoseStamped.py b/dimos/msgs/geometry_msgs/test_PoseStamped.py index a486f333031..d43b2981eaa 100644 --- a/dimos/msgs/geometry_msgs/test_PoseStamped.py +++ b/dimos/msgs/geometry_msgs/test_PoseStamped.py @@ -13,43 +13,52 @@ # limitations under the License. import pickle -import time -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped +from dimos_generated.geometry_msgs.msg import Point, Pose, PoseStamped, Quaternion +from dimos_generated.std_msgs.msg import Header +from rosbags.typesys import Stores, get_typestore +from dimos.msgs.time import time_from_nanoseconds -def test_lcm_encode_decode() -> None: - """Test encoding and decoding of Pose to/from binary LCM format.""" - pose_source = PoseStamped( - ts=time.time(), - position=(1.0, 2.0, 3.0), - orientation=(0.1, 0.2, 0.3, 0.9), +def test_cdr_encode_decode() -> None: + source = PoseStamped( + header=Header(stamp=time_from_nanoseconds(1234567890123456789)), + pose=Pose( + position=Point(x=1, y=2, z=3), orientation=Quaternion(x=0.1, y=0.2, z=0.3, w=0.9) + ), ) - binary_msg = pose_source.lcm_encode() - pose_dest = PoseStamped.lcm_decode(binary_msg) - - assert isinstance(pose_dest, PoseStamped) - assert pose_dest is not pose_source - - print(pose_source.position) - print(pose_source.orientation) - - print(pose_dest.position) - print(pose_dest.orientation) - assert pose_dest == pose_source + binary = source.encode() + decoded = PoseStamped.decode(binary) + assert isinstance(decoded, PoseStamped) + assert decoded is not source + assert decoded.encode() == binary + independent = get_typestore(Stores.ROS2_JAZZY).deserialize_cdr(binary, source.msg_name) + assert (independent.header.stamp.sec, independent.header.stamp.nanosec) == ( + 1234567890, + 123456789, + ) + assert ( + independent.pose.position.x, + independent.pose.position.y, + independent.pose.position.z, + ) == (1, 2, 3) + assert ( + independent.pose.orientation.x, + independent.pose.orientation.y, + independent.pose.orientation.z, + independent.pose.orientation.w, + ) == (0.1, 0.2, 0.3, 0.9) def test_pickle_encode_decode() -> None: - """Test encoding and decoding of PoseStamped to/from binary LCM format.""" - - pose_source = PoseStamped( - ts=time.time(), - position=(1.0, 2.0, 3.0), - orientation=(0.1, 0.2, 0.3, 0.9), + source = PoseStamped( + header=Header(stamp=time_from_nanoseconds(1234567890123456789)), + pose=Pose( + position=Point(x=1, y=2, z=3), orientation=Quaternion(x=0.1, y=0.2, z=0.3, w=0.9) + ), ) - binary_msg = pickle.dumps(pose_source) - pose_dest = pickle.loads(binary_msg) - assert isinstance(pose_dest, PoseStamped) - assert pose_dest is not pose_source - assert pose_dest == pose_source + decoded = pickle.loads(pickle.dumps(source)) + assert isinstance(decoded, PoseStamped) + assert decoded is not source + assert decoded.encode() == source.encode() diff --git a/dimos/msgs/geometry_msgs/test_PoseWithCovariance.py b/dimos/msgs/geometry_msgs/test_PoseWithCovariance.py index f6936db9f7a..fa52d25c80a 100644 --- a/dimos/msgs/geometry_msgs/test_PoseWithCovariance.py +++ b/dimos/msgs/geometry_msgs/test_PoseWithCovariance.py @@ -12,311 +12,111 @@ # See the License for the specific language governing permissions and # limitations under the License. -from dimos_lcm.geometry_msgs import PoseWithCovariance as LCMPoseWithCovariance -import numpy as np -import pytest - -from dimos.msgs.geometry_msgs.Pose import Pose -from dimos.msgs.geometry_msgs.PoseWithCovariance import PoseWithCovariance - - -def test_pose_with_covariance_default_init() -> None: - """Test that default initialization creates a pose at origin with zero covariance.""" - pose_cov = PoseWithCovariance() - - # Pose should be at origin with identity orientation - assert pose_cov.pose.position.x == 0.0 - assert pose_cov.pose.position.y == 0.0 - assert pose_cov.pose.position.z == 0.0 - assert pose_cov.pose.orientation.x == 0.0 - assert pose_cov.pose.orientation.y == 0.0 - assert pose_cov.pose.orientation.z == 0.0 - assert pose_cov.pose.orientation.w == 1.0 - - # Covariance should be all zeros - assert np.all(pose_cov.covariance == 0.0) - assert pose_cov.covariance.shape == (36,) - - -def test_pose_with_covariance_pose_init() -> None: - """Test initialization with a Pose object.""" - pose = Pose(1.0, 2.0, 3.0, 0.1, 0.2, 0.3, 0.9) - pose_cov = PoseWithCovariance(pose) - - # Pose should match - assert pose_cov.pose.position.x == 1.0 - assert pose_cov.pose.position.y == 2.0 - assert pose_cov.pose.position.z == 3.0 - assert pose_cov.pose.orientation.x == 0.1 - assert pose_cov.pose.orientation.y == 0.2 - assert pose_cov.pose.orientation.z == 0.3 - assert pose_cov.pose.orientation.w == 0.9 - - # Covariance should be zeros by default - assert np.all(pose_cov.covariance == 0.0) - - -def test_pose_with_covariance_pose_and_covariance_init() -> None: - """Test initialization with pose and covariance.""" - pose = Pose(1.0, 2.0, 3.0) - covariance = np.arange(36, dtype=float) - pose_cov = PoseWithCovariance(pose, covariance) - - # Pose should match - assert pose_cov.pose.position.x == 1.0 - assert pose_cov.pose.position.y == 2.0 - assert pose_cov.pose.position.z == 3.0 - - # Covariance should match - assert np.array_equal(pose_cov.covariance, covariance) - - -def test_pose_with_covariance_list_covariance() -> None: - """Test initialization with covariance as a list.""" - pose = Pose(1.0, 2.0, 3.0) - covariance_list = list(range(36)) - pose_cov = PoseWithCovariance(pose, covariance_list) - - # Covariance should be converted to numpy array - assert isinstance(pose_cov.covariance, np.ndarray) - assert np.array_equal(pose_cov.covariance, np.array(covariance_list)) - - -def test_pose_with_covariance_copy_init() -> None: - """Test copy constructor.""" - pose = Pose(1.0, 2.0, 3.0, 0.1, 0.2, 0.3, 0.9) - covariance = np.arange(36, dtype=float) - original = PoseWithCovariance(pose, covariance) - copy = PoseWithCovariance(original) - - # Should be equal but not the same object - assert copy == original - assert copy is not original - assert copy.pose is not original.pose - assert copy.covariance is not original.covariance - - # Modify original to ensure they're independent - original.covariance[0] = 999.0 - assert copy.covariance[0] != 999.0 - - -def test_pose_with_covariance_lcm_init() -> None: - """Test initialization from LCM message.""" - lcm_msg = LCMPoseWithCovariance() - lcm_msg.pose.position.x = 1.0 - lcm_msg.pose.position.y = 2.0 - lcm_msg.pose.position.z = 3.0 - lcm_msg.pose.orientation.x = 0.1 - lcm_msg.pose.orientation.y = 0.2 - lcm_msg.pose.orientation.z = 0.3 - lcm_msg.pose.orientation.w = 0.9 - lcm_msg.covariance = list(range(36)) - - pose_cov = PoseWithCovariance(lcm_msg) - - # Pose should match - assert pose_cov.pose.position.x == 1.0 - assert pose_cov.pose.position.y == 2.0 - assert pose_cov.pose.position.z == 3.0 - assert pose_cov.pose.orientation.x == 0.1 - assert pose_cov.pose.orientation.y == 0.2 - assert pose_cov.pose.orientation.z == 0.3 - assert pose_cov.pose.orientation.w == 0.9 - - # Covariance should match - assert np.array_equal(pose_cov.covariance, np.arange(36)) - - -def test_pose_with_covariance_dict_init() -> None: - """Test initialization from dictionary.""" - pose_dict = {"pose": Pose(1.0, 2.0, 3.0), "covariance": list(range(36))} - pose_cov = PoseWithCovariance(pose_dict) - - assert pose_cov.pose.position.x == 1.0 - assert pose_cov.pose.position.y == 2.0 - assert pose_cov.pose.position.z == 3.0 - assert np.array_equal(pose_cov.covariance, np.arange(36)) - - -def test_pose_with_covariance_dict_init_no_covariance() -> None: - """Test initialization from dictionary without covariance.""" - pose_dict = {"pose": Pose(1.0, 2.0, 3.0)} - pose_cov = PoseWithCovariance(pose_dict) - - assert pose_cov.pose.position.x == 1.0 - assert np.all(pose_cov.covariance == 0.0) - - -def test_pose_with_covariance_tuple_init() -> None: - """Test initialization from tuple.""" - pose = Pose(1.0, 2.0, 3.0) - covariance = np.arange(36, dtype=float) - pose_tuple = (pose, covariance) - pose_cov = PoseWithCovariance(pose_tuple) - - assert pose_cov.pose.position.x == 1.0 - assert pose_cov.pose.position.y == 2.0 - assert pose_cov.pose.position.z == 3.0 - assert np.array_equal(pose_cov.covariance, covariance) - - -def test_pose_with_covariance_properties() -> None: - """Test convenience properties.""" - pose = Pose(1.0, 2.0, 3.0, 0.1, 0.2, 0.3, 0.9) - pose_cov = PoseWithCovariance(pose) - - # Position properties - assert pose_cov.x == 1.0 - assert pose_cov.y == 2.0 - assert pose_cov.z == 3.0 - assert pose_cov.position.x == 1.0 - assert pose_cov.position.y == 2.0 - assert pose_cov.position.z == 3.0 - - # Orientation properties - assert pose_cov.orientation.x == 0.1 - assert pose_cov.orientation.y == 0.2 - assert pose_cov.orientation.z == 0.3 - assert pose_cov.orientation.w == 0.9 - - # Euler angle properties - assert pose_cov.roll == pose.roll - assert pose_cov.pitch == pose.pitch - assert pose_cov.yaw == pose.yaw - - -def test_pose_with_covariance_matrix_property() -> None: - """Test covariance matrix property.""" - pose = Pose() - covariance_array = np.arange(36, dtype=float) - pose_cov = PoseWithCovariance(pose, covariance_array) - - # Get as matrix - cov_matrix = pose_cov.covariance_matrix - assert cov_matrix.shape == (6, 6) - assert cov_matrix[0, 0] == 0.0 - assert cov_matrix[5, 5] == 35.0 - - # Set from matrix - new_matrix = np.eye(6) * 2.0 - pose_cov.covariance_matrix = new_matrix - assert np.array_equal(pose_cov.covariance[:6], [2.0, 0.0, 0.0, 0.0, 0.0, 0.0]) - - -def test_pose_with_covariance_repr() -> None: - """Test string representation.""" - pose = Pose(1.234, 2.567, 3.891) - pose_cov = PoseWithCovariance(pose) - - repr_str = repr(pose_cov) - assert "PoseWithCovariance" in repr_str - assert "pose=" in repr_str - assert "covariance=" in repr_str - assert "36 elements" in repr_str - - -def test_pose_with_covariance_str() -> None: - """Test string formatting.""" - pose = Pose(1.234, 2.567, 3.891) - covariance = np.eye(6).flatten() - pose_cov = PoseWithCovariance(pose, covariance) - - str_repr = str(pose_cov) - assert "PoseWithCovariance" in str_repr - assert "1.234" in str_repr - assert "2.567" in str_repr - assert "3.891" in str_repr - assert "cov_trace" in str_repr - assert "6.000" in str_repr # Trace of identity matrix is 6 - - -def test_pose_with_covariance_equality() -> None: - """Test equality comparison.""" - pose1 = Pose(1.0, 2.0, 3.0) - cov1 = np.arange(36, dtype=float) - pose_cov1 = PoseWithCovariance(pose1, cov1) - - pose2 = Pose(1.0, 2.0, 3.0) - cov2 = np.arange(36, dtype=float) - pose_cov2 = PoseWithCovariance(pose2, cov2) - - # Equal - assert pose_cov1 == pose_cov2 - - # Different pose - pose3 = Pose(1.1, 2.0, 3.0) - pose_cov3 = PoseWithCovariance(pose3, cov1) - assert pose_cov1 != pose_cov3 - - # Different covariance - cov3 = np.arange(36, dtype=float) + 1 - pose_cov4 = PoseWithCovariance(pose1, cov3) - assert pose_cov1 != pose_cov4 - - # Different type - assert pose_cov1 != "not a pose" - assert pose_cov1 is not None - - -def test_pose_with_covariance_lcm_encode_decode() -> None: - """Test LCM encoding and decoding.""" - pose = Pose(1.0, 2.0, 3.0, 0.1, 0.2, 0.3, 0.9) - covariance = np.arange(36, dtype=float) - source = PoseWithCovariance(pose, covariance) - - # Encode and decode - binary_msg = source.lcm_encode() - decoded = PoseWithCovariance.lcm_decode(binary_msg) - - # Should be equal - assert decoded == source - assert isinstance(decoded, PoseWithCovariance) - assert isinstance(decoded.pose, Pose) - assert isinstance(decoded.covariance, np.ndarray) - - -def test_pose_with_covariance_zero_covariance() -> None: - """Test with zero covariance matrix.""" - pose = Pose(1.0, 2.0, 3.0) - pose_cov = PoseWithCovariance(pose) - - assert np.all(pose_cov.covariance == 0.0) - assert np.trace(pose_cov.covariance_matrix) == 0.0 - - -def test_pose_with_covariance_diagonal_covariance() -> None: - """Test with diagonal covariance matrix.""" - pose = Pose() - covariance = np.zeros(36) - # Set diagonal elements - for i in range(6): - covariance[i * 6 + i] = i + 1 - - pose_cov = PoseWithCovariance(pose, covariance) - - cov_matrix = pose_cov.covariance_matrix - assert np.trace(cov_matrix) == sum(range(1, 7)) # 1+2+3+4+5+6 = 21 - - # Check diagonal elements - for i in range(6): - assert cov_matrix[i, i] == i + 1 - - # Check off-diagonal elements are zero - for i in range(6): - for j in range(6): - if i != j: - assert cov_matrix[i, j] == 0.0 +"""Generated covariance values, with explicit NumPy matrix operations. +Legacy polymorphic positional constructors and presentation methods are retired. +""" +from dimos_generated.geometry_msgs.msg import Point, Pose, PoseWithCovariance, Quaternion +import numpy as np +import pytest +from rosbags.typesys import Stores, get_typestore + + +def test_default_fields_and_covariance() -> None: + source = PoseWithCovariance() + value = source.pose + np.testing.assert_array_equal( + [ + value.position.x, + value.position.y, + value.position.z, + value.orientation.x, + value.orientation.y, + value.orientation.z, + value.orientation.w, + ], + [0, 0, 0, 0, 0, 0, 1], + ) + assert np.asarray(source.covariance).shape == (36,) + np.testing.assert_array_equal(source.covariance, np.zeros(36)) + + +@pytest.mark.parametrize("as_list", [False, True]) @pytest.mark.parametrize( - "x,y,z", - [(0.0, 0.0, 0.0), (1.0, 2.0, 3.0), (-1.0, -2.0, -3.0), (100.0, -100.0, 0.0)], + "covariance", + [np.zeros(36), np.arange(36, dtype=float), np.eye(6).ravel(), np.diag(np.arange(1, 7)).ravel()], ) -def test_pose_with_covariance_parametrized_positions(x, y, z) -> None: - """Parametrized test for various position values.""" - pose = Pose(x, y, z) - pose_cov = PoseWithCovariance(pose) - - assert pose_cov.x == x - assert pose_cov.y == y - assert pose_cov.z == z +def test_explicit_construction_and_independent_cdr(as_list: bool, covariance: np.ndarray) -> None: + source = PoseWithCovariance( + pose=Pose( + position=Point(x=1, y=2, z=3), orientation=Quaternion(x=0.1, y=0.2, z=0.3, w=0.9) + ), + covariance=covariance.tolist() if as_list else covariance, + ) + decoded = PoseWithCovariance.decode(source.encode()) + independent = get_typestore(Stores.ROS2_JAZZY).deserialize_cdr( + source.encode(), PoseWithCovariance.msg_name + ) + for result in (source, decoded, independent): + value = result.pose + np.testing.assert_array_equal( + [ + value.position.x, + value.position.y, + value.position.z, + value.orientation.x, + value.orientation.y, + value.orientation.z, + value.orientation.w, + ], + [1, 2, 3, 0.1, 0.2, 0.3, 0.9], + ) + np.testing.assert_array_equal(result.covariance, covariance) + matrix = np.asarray(result.covariance).reshape(6, 6) + for row in range(6): + for col in range(6): + assert matrix[row, col] == covariance[row * 6 + col] + assert np.trace(matrix) == np.trace(covariance.reshape(6, 6)) + + +def test_copy_equality_and_independent_storage() -> None: + original = PoseWithCovariance( + pose=Pose( + position=Point(x=1, y=2, z=3), orientation=Quaternion(x=0.1, y=0.2, z=0.3, w=0.9) + ), + covariance=np.arange(36, dtype=float), + ) + copied = PoseWithCovariance.decode(original.encode()) + assert copied == original + assert copied is not original + assert copied.pose is not original.pose + assert copied.covariance is not original.covariance + original.covariance[0] = 999 + assert copied.covariance[0] == 0 + assert copied != original + assert copied != "not a message" + assert copied is not None + + +def test_matrix_assignment() -> None: + source = PoseWithCovariance(covariance=np.arange(36, dtype=float)) + matrix = np.asarray(source.covariance).reshape(6, 6) + assert matrix[0, 0] == 0 + assert matrix[5, 5] == 35 + source.covariance = (np.eye(6) * 2).ravel() + np.testing.assert_array_equal(list(source.covariance)[:6], [2, 0, 0, 0, 0, 0]) + assert np.trace(np.asarray(source.covariance).reshape(6, 6)) == 12 + + +@pytest.mark.parametrize("size", [0, 35, 37]) +def test_fixed_covariance_rejects_invalid_length(size: int) -> None: + with pytest.raises((ValueError, TypeError, RuntimeError)): + PoseWithCovariance(covariance=np.zeros(size)) + + +@pytest.mark.parametrize("xyz", [(0, 0, 0), (1, 2, 3), (-1, -2, -3), (100, -100, 0)]) +def test_parameterized_values(xyz: tuple[float, float, float]) -> None: + source = PoseWithCovariance(pose=Pose(position=Point(x=xyz[0], y=xyz[1], z=xyz[2]))) + position = source.pose.position + assert (position.x, position.y, position.z) == xyz diff --git a/dimos/msgs/geometry_msgs/test_PoseWithCovarianceStamped.py b/dimos/msgs/geometry_msgs/test_PoseWithCovarianceStamped.py index e72b96ad529..e2c1a33697d 100644 --- a/dimos/msgs/geometry_msgs/test_PoseWithCovarianceStamped.py +++ b/dimos/msgs/geometry_msgs/test_PoseWithCovarianceStamped.py @@ -12,211 +12,118 @@ # See the License for the specific language governing permissions and # limitations under the License. -import time - -import numpy as np - -from dimos.msgs.geometry_msgs.Pose import Pose -from dimos.msgs.geometry_msgs.PoseWithCovariance import PoseWithCovariance -from dimos.msgs.geometry_msgs.PoseWithCovarianceStamped import PoseWithCovarianceStamped - - -def test_pose_with_covariance_stamped_default_init() -> None: - """Test default initialization.""" - pose_cov_stamped = PoseWithCovarianceStamped() - - # Should have current timestamp - assert pose_cov_stamped.ts > 0 - assert pose_cov_stamped.frame_id == "" +"""Nested generated Pose covariance with explicit stamps and array conversion. - # Pose should be at origin with identity orientation - assert pose_cov_stamped.pose.position.x == 0.0 - assert pose_cov_stamped.pose.position.y == 0.0 - assert pose_cov_stamped.pose.position.z == 0.0 - assert pose_cov_stamped.pose.orientation.w == 1.0 +Legacy inheritance, custom repr, and implicit wall-clock defaults are retired; +field values, covariance matrices, and time conversion remain checked explicitly. +""" - # Covariance should be all zeros - assert np.all(pose_cov_stamped.covariance == 0.0) - - -def test_pose_with_covariance_stamped_with_timestamp() -> None: - """Test initialization with specific timestamp.""" - ts = 1234567890.123456 - frame_id = "base_link" - pose_cov_stamped = PoseWithCovarianceStamped(ts=ts, frame_id=frame_id) - - assert pose_cov_stamped.ts == ts - assert pose_cov_stamped.frame_id == frame_id - - -def test_pose_with_covariance_stamped_with_pose() -> None: - """Test initialization with pose.""" - ts = 1234567890.123456 - frame_id = "map" - pose = Pose(1.0, 2.0, 3.0, 0.1, 0.2, 0.3, 0.9) - covariance = np.arange(36, dtype=float) +import pickle +import time - pose_cov_stamped = PoseWithCovarianceStamped( - ts=ts, frame_id=frame_id, pose=pose, covariance=covariance +from dimos_generated.geometry_msgs.msg import ( + Point, + Pose, + PoseWithCovariance, + PoseWithCovarianceStamped, + Quaternion, +) +from dimos_generated.std_msgs.msg import Header +import numpy as np +import pytest +from rosbags.typesys import Stores, get_typestore + +from dimos.msgs.time import header_now, time_from_nanoseconds, time_from_seconds, to_nanoseconds + + +def test_generated_defaults() -> None: + source = PoseWithCovarianceStamped() + assert source.header.frame_id == "" + assert to_nanoseconds(source.header.stamp) == 0 + assert isinstance(source.pose, PoseWithCovariance) + assert isinstance(source.pose.pose, Pose) + np.testing.assert_array_equal(source.pose.covariance, np.zeros(36)) + value = source.pose.pose + np.testing.assert_array_equal( + [ + value.position.x, + value.position.y, + value.position.z, + value.orientation.x, + value.orientation.y, + value.orientation.z, + value.orientation.w, + ], + [0, 0, 0, 0, 0, 0, 1], ) - assert pose_cov_stamped.ts == ts - assert pose_cov_stamped.frame_id == frame_id - assert pose_cov_stamped.pose.position.x == 1.0 - assert pose_cov_stamped.pose.position.y == 2.0 - assert pose_cov_stamped.pose.position.z == 3.0 - assert np.array_equal(pose_cov_stamped.covariance, covariance) - -def test_pose_with_covariance_stamped_properties() -> None: - """Test convenience properties.""" - pose = Pose(1.0, 2.0, 3.0, 0.1, 0.2, 0.3, 0.9) - covariance = np.eye(6).flatten() - pose_cov_stamped = PoseWithCovarianceStamped( - ts=1234567890.0, frame_id="odom", pose=pose, covariance=covariance +@pytest.mark.parametrize("stamp", [0, 1234567890123456789]) +@pytest.mark.parametrize( + "covariance", + [np.zeros(36), np.eye(6).ravel(), np.arange(36, dtype=float), np.diag(np.arange(1, 7)).ravel()], +) +def test_fields_covariance_and_independent_cdr(stamp: int, covariance: np.ndarray) -> None: + source = PoseWithCovarianceStamped( + header=Header(stamp=time_from_nanoseconds(stamp), frame_id="camera_link"), + pose=PoseWithCovariance( + pose=Pose( + position=Point(x=1, y=2, z=3), orientation=Quaternion(x=0.1, y=0.2, z=0.3, w=0.9) + ), + covariance=covariance, + ), ) - - # Position properties - assert pose_cov_stamped.x == 1.0 - assert pose_cov_stamped.y == 2.0 - assert pose_cov_stamped.z == 3.0 - - # Orientation properties - assert pose_cov_stamped.orientation.x == 0.1 - assert pose_cov_stamped.orientation.y == 0.2 - assert pose_cov_stamped.orientation.z == 0.3 - assert pose_cov_stamped.orientation.w == 0.9 - - # Euler angles - assert pose_cov_stamped.roll == pose.roll - assert pose_cov_stamped.pitch == pose.pitch - assert pose_cov_stamped.yaw == pose.yaw - - # Covariance matrix - cov_matrix = pose_cov_stamped.covariance_matrix - assert cov_matrix.shape == (6, 6) - assert np.trace(cov_matrix) == 6.0 - - -def test_pose_with_covariance_stamped_str() -> None: - """Test string representation.""" - pose = Pose(1.234, 2.567, 3.891) - covariance = np.eye(6).flatten() * 2.0 - pose_cov_stamped = PoseWithCovarianceStamped( - ts=1234567890.0, frame_id="world", pose=pose, covariance=covariance + decoded = PoseWithCovarianceStamped.decode(source.encode()) + independent = get_typestore(Stores.ROS2_JAZZY).deserialize_cdr( + source.encode(), PoseWithCovarianceStamped.msg_name ) - - str_repr = str(pose_cov_stamped) - assert "PoseWithCovarianceStamped" in str_repr - assert "1.234" in str_repr - assert "2.567" in str_repr - assert "3.891" in str_repr - assert "cov_trace" in str_repr - assert "12.000" in str_repr # Trace of 2*identity is 12 - - -def test_pose_with_covariance_stamped_lcm_encode_decode() -> None: - """Test LCM encoding and decoding.""" - ts = 1234567890.123456 - frame_id = "camera_link" - pose = Pose(1.0, 2.0, 3.0, 0.1, 0.2, 0.3, 0.9) - covariance = np.arange(36, dtype=float) - - source = PoseWithCovarianceStamped(ts=ts, frame_id=frame_id, pose=pose, covariance=covariance) - - # Encode and decode - binary_msg = source.lcm_encode() - decoded = PoseWithCovarianceStamped.lcm_decode(binary_msg) - - # Check timestamp (may lose some precision) - assert abs(decoded.ts - ts) < 1e-6 - assert decoded.frame_id == frame_id - - # Check pose - assert decoded.pose.position.x == 1.0 - assert decoded.pose.position.y == 2.0 - assert decoded.pose.position.z == 3.0 - assert decoded.pose.orientation.x == 0.1 - assert decoded.pose.orientation.y == 0.2 - assert decoded.pose.orientation.z == 0.3 - assert decoded.pose.orientation.w == 0.9 - - # Check covariance - assert np.array_equal(decoded.covariance, covariance) - - -def test_pose_with_covariance_stamped_zero_timestamp() -> None: - """Test that an explicit zero timestamp is kept, and an omitted one stamps now.""" - assert PoseWithCovarianceStamped(ts=0.0).ts == 0.0 - - before = time.time() - assert before <= PoseWithCovarianceStamped().ts <= time.time() - - -def test_pose_with_covariance_stamped_inheritance() -> None: - """Test that it properly inherits from PoseWithCovariance and Timestamped.""" - pose = Pose(1.0, 2.0, 3.0) - covariance = np.eye(6).flatten() - pose_cov_stamped = PoseWithCovarianceStamped( - ts=1234567890.0, frame_id="test", pose=pose, covariance=covariance - ) - - # Should be instance of parent classes - assert isinstance(pose_cov_stamped, PoseWithCovariance) - - # Should have Timestamped attributes - assert hasattr(pose_cov_stamped, "ts") - assert hasattr(pose_cov_stamped, "frame_id") - - # Should have PoseWithCovariance attributes - assert hasattr(pose_cov_stamped, "pose") - assert hasattr(pose_cov_stamped, "covariance") - - -def test_pose_with_covariance_stamped_sec_nsec() -> None: - """Test the sec_nsec helper function.""" - from dimos.msgs.geometry_msgs.PoseWithCovarianceStamped import sec_nsec - - # Test integer seconds - s, ns = sec_nsec(1234567890.0) - assert s == 1234567890 - assert ns == 0 - - # Test fractional seconds - s, ns = sec_nsec(1234567890.123456789) - assert s == 1234567890 - assert abs(ns - 123456789) < 100 # Allow small rounding error - - # Test small fractional seconds - s, ns = sec_nsec(0.000000001) - assert s == 0 - assert ns == 1 - - # Test large timestamp - s, ns = sec_nsec(9999999999.999999999) - # Due to floating point precision, this might round to 10000000000 - assert s in [9999999999, 10000000000] - if s == 9999999999: - assert abs(ns - 999999999) < 10 - else: - assert ns == 0 - - -def test_pose_with_covariance_stamped_different_covariances() -> None: - """Test with different covariance patterns.""" - pose = Pose(1.0, 2.0, 3.0) - - # Zero covariance - zero_cov = np.zeros(36) - pose_cov1 = PoseWithCovarianceStamped(pose=pose, covariance=zero_cov) - assert np.all(pose_cov1.covariance == 0.0) - - # Identity covariance - identity_cov = np.eye(6).flatten() - pose_cov2 = PoseWithCovarianceStamped(pose=pose, covariance=identity_cov) - assert np.trace(pose_cov2.covariance_matrix) == 6.0 - - # Full covariance - full_cov = np.random.rand(36) - pose_cov3 = PoseWithCovarianceStamped(pose=pose, covariance=full_cov) - assert np.array_equal(pose_cov3.covariance, full_cov) + for result in (decoded, independent): + assert result.header.frame_id == "camera_link" + assert result.header.stamp.sec * 1000000000 + result.header.stamp.nanosec == stamp + value = result.pose.pose + np.testing.assert_array_equal( + [ + value.position.x, + value.position.y, + value.position.z, + value.orientation.x, + value.orientation.y, + value.orientation.z, + value.orientation.w, + ], + [1, 2, 3, 0.1, 0.2, 0.3, 0.9], + ) + np.testing.assert_array_equal(result.pose.covariance, covariance) + matrix = np.asarray(result.pose.covariance).reshape(6, 6) + assert matrix.shape == (6, 6) + assert np.trace(matrix) == np.trace(covariance.reshape(6, 6)) + copied = pickle.loads(pickle.dumps(source)) + assert copied.encode() == source.encode() + copied.pose.covariance[0] = 999 + assert source.pose.covariance[0] == covariance[0] + + +def test_explicit_current_header() -> None: + before = time.time_ns() + source = PoseWithCovarianceStamped(header=header_now("test")) + assert before <= to_nanoseconds(source.header.stamp) <= time.time_ns() + assert source.header.frame_id == "test" + + +@pytest.mark.parametrize( + "seconds,sec,nanosec", + [ + (1234567890.0, 1234567890, 0), + (1234567890.123456789, 1234567890, 123456789), + (0.000000001, 0, 1), + ], +) +def test_explicit_seconds_conversion(seconds: float, sec: int, nanosec: int) -> None: + stamp = time_from_seconds(seconds) + assert stamp.sec == sec + assert abs(stamp.nanosec - nanosec) < 100 + + +def test_seconds_outside_ros_int32_range_rejected() -> None: + with pytest.raises((TypeError, ValueError, OverflowError)): + time_from_seconds(9999999999.999999999) diff --git a/dimos/msgs/geometry_msgs/test_Quaternion.py b/dimos/msgs/geometry_msgs/test_Quaternion.py index 21c1e8caebf..06ba7e1ee2e 100644 --- a/dimos/msgs/geometry_msgs/test_Quaternion.py +++ b/dimos/msgs/geometry_msgs/test_Quaternion.py @@ -12,11 +12,21 @@ # See the License for the specific language governing permissions and # limitations under the License. -from dimos_lcm.geometry_msgs import Quaternion as LCMQuaternion +from dimos_generated.geometry_msgs.msg import Quaternion, Vector3 import numpy as np import pytest -from dimos.msgs.geometry_msgs.Quaternion import Quaternion +from dimos.msgs.geometry import ( + normalized_quaternion, + quaternion_array, + quaternion_conjugate, + quaternion_euler, + quaternion_from_array, + quaternion_inverse, + quaternion_product, + rotate_vector, + vector_from_array, +) def test_quaternion_default_init() -> None: @@ -26,33 +36,27 @@ def test_quaternion_default_init() -> None: assert q.y == 0.0 assert q.z == 0.0 assert q.w == 1.0 - assert q.to_tuple() == (0.0, 0.0, 0.0, 1.0) + assert tuple(quaternion_array(q)) == (0.0, 0.0, 0.0, 1.0) def test_quaternion_component_init() -> None: """Test initialization with four float components (x, y, z, w).""" - q = Quaternion(0.5, 0.5, 0.5, 0.5) + q = Quaternion(x=0.5, y=0.5, z=0.5, w=0.5) assert q.x == 0.5 assert q.y == 0.5 assert q.z == 0.5 assert q.w == 0.5 - - # Test with different values - q2 = Quaternion(1.0, 2.0, 3.0, 4.0) + q2 = Quaternion(x=1.0, y=2.0, z=3.0, w=4.0) assert q2.x == 1.0 assert q2.y == 2.0 assert q2.z == 3.0 assert q2.w == 4.0 - - # Test with negative values - q3 = Quaternion(-1.0, -2.0, -3.0, -4.0) + q3 = Quaternion(x=-1.0, y=-2.0, z=-3.0, w=-4.0) assert q3.x == -1.0 assert q3.y == -2.0 assert q3.z == -3.0 assert q3.w == -4.0 - - # Test with integers (should convert to float) - q4 = Quaternion(1, 2, 3, 4) + q4 = Quaternion(x=1, y=2, z=3, w=4) assert q4.x == 1.0 assert q4.y == 2.0 assert q4.z == 3.0 @@ -62,85 +66,67 @@ def test_quaternion_component_init() -> None: def test_quaternion_sequence_init() -> None: """Test initialization from sequence (list, tuple) of 4 numbers.""" - # From list - q1 = Quaternion([0.1, 0.2, 0.3, 0.4]) + q1 = quaternion_from_array([0.1, 0.2, 0.3, 0.4]) assert q1.x == 0.1 assert q1.y == 0.2 assert q1.z == 0.3 assert q1.w == 0.4 - - # From tuple - q2 = Quaternion((0.5, 0.6, 0.7, 0.8)) + q2 = quaternion_from_array((0.5, 0.6, 0.7, 0.8)) assert q2.x == 0.5 assert q2.y == 0.6 assert q2.z == 0.7 assert q2.w == 0.8 - - # Test with integers in sequence - q3 = Quaternion([1, 2, 3, 4]) + q3 = quaternion_from_array([1, 2, 3, 4]) assert q3.x == 1.0 assert q3.y == 2.0 assert q3.z == 3.0 assert q3.w == 4.0 - - # Test error with wrong length with pytest.raises(ValueError, match="Quaternion requires exactly 4 components"): - Quaternion([1, 2, 3]) # Only 3 components - + quaternion_from_array([1, 2, 3]) with pytest.raises(ValueError, match="Quaternion requires exactly 4 components"): - Quaternion([1, 2, 3, 4, 5]) # Too many components + quaternion_from_array([1, 2, 3, 4, 5]) def test_quaternion_numpy_init() -> None: """Test initialization from numpy array.""" - # From numpy array arr = np.array([0.1, 0.2, 0.3, 0.4]) - q1 = Quaternion(arr) + q1 = quaternion_from_array(arr) assert q1.x == 0.1 assert q1.y == 0.2 assert q1.z == 0.3 assert q1.w == 0.4 - - # Test with different dtypes arr_int = np.array([1, 2, 3, 4], dtype=int) - q2 = Quaternion(arr_int) + q2 = quaternion_from_array(arr_int) assert q2.x == 1.0 assert q2.y == 2.0 assert q2.z == 3.0 assert q2.w == 4.0 - - # Test error with wrong size with pytest.raises(ValueError, match="Quaternion requires exactly 4 components"): - Quaternion(np.array([1, 2, 3])) # Only 3 elements - + quaternion_from_array(np.array([1, 2, 3])) with pytest.raises(ValueError, match="Quaternion requires exactly 4 components"): - Quaternion(np.array([1, 2, 3, 4, 5])) # Too many elements + quaternion_from_array(np.array([1, 2, 3, 4, 5])) def test_quaternion_copy_init() -> None: """Test initialization from another Quaternion (copy constructor).""" - original = Quaternion(0.1, 0.2, 0.3, 0.4) - copy = Quaternion(original) - + original = Quaternion(x=0.1, y=0.2, z=0.3, w=0.4) + copy = Quaternion.decode(original.encode()) assert copy.x == 0.1 assert copy.y == 0.2 assert copy.z == 0.3 assert copy.w == 0.4 - - # Verify it's a copy, not the same object assert copy is not original assert copy == original -def test_quaternion_lcm_init() -> None: - """Test initialization from LCM Quaternion.""" - lcm_quat = LCMQuaternion() - lcm_quat.x = 0.1 - lcm_quat.y = 0.2 - lcm_quat.z = 0.3 - lcm_quat.w = 0.4 - - q = Quaternion(lcm_quat) +def test_quaternion_mutated_value_copy() -> None: + """Test initialization from generated Quaternion.""" + source_quaternion = Quaternion() + source_quaternion.x = 0.1 + source_quaternion.y = 0.2 + source_quaternion.z = 0.3 + source_quaternion.w = 0.4 + q = Quaternion.decode(source_quaternion.encode()) assert q.x == 0.1 assert q.y == 0.2 assert q.z == 0.3 @@ -149,62 +135,47 @@ def test_quaternion_lcm_init() -> None: def test_quaternion_properties() -> None: """Test quaternion component properties.""" - q = Quaternion(1.0, 2.0, 3.0, 4.0) - - # Test property access + q = Quaternion(x=1.0, y=2.0, z=3.0, w=4.0) assert q.x == 1.0 assert q.y == 2.0 assert q.z == 3.0 assert q.w == 4.0 - - # Test as_tuple property - assert q.to_tuple() == (1.0, 2.0, 3.0, 4.0) + assert tuple(quaternion_array(q)) == (1.0, 2.0, 3.0, 4.0) def test_quaternion_indexing() -> None: """Test quaternion indexing support.""" - q = Quaternion(1.0, 2.0, 3.0, 4.0) - - # Test indexing - assert q[0] == 1.0 - assert q[1] == 2.0 - assert q[2] == 3.0 - assert q[3] == 4.0 + q = Quaternion(x=1.0, y=2.0, z=3.0, w=4.0) + assert quaternion_array(q)[0] == 1.0 + assert quaternion_array(q)[1] == 2.0 + assert quaternion_array(q)[2] == 3.0 + assert quaternion_array(q)[3] == 4.0 def test_quaternion_euler() -> None: """Test quaternion to Euler angles conversion.""" - - # Test identity quaternion (should give zero angles) q_identity = Quaternion() - angles = q_identity.to_euler() - assert np.isclose(angles.x, 0.0, atol=1e-10) # roll - assert np.isclose(angles.y, 0.0, atol=1e-10) # pitch - assert np.isclose(angles.z, 0.0, atol=1e-10) # yaw - - # Test 90 degree rotation around Z-axis (yaw) - q_z90 = Quaternion(0, 0, np.sin(np.pi / 4), np.cos(np.pi / 4)) - angles_z90 = q_z90.to_euler() - assert np.isclose(angles_z90.roll, 0.0, atol=1e-10) # roll should be 0 - assert np.isclose(angles_z90.pitch, 0.0, atol=1e-10) # pitch should be 0 - assert np.isclose(angles_z90.yaw, np.pi / 2, atol=1e-10) # yaw should be π/2 (90 degrees) - - # Test 90 degree rotation around X-axis (roll) - q_x90 = Quaternion(np.sin(np.pi / 4), 0, 0, np.cos(np.pi / 4)) - angles_x90 = q_x90.to_euler() - assert np.isclose(angles_x90.x, np.pi / 2, atol=1e-10) # roll should be π/2 - assert np.isclose(angles_x90.y, 0.0, atol=1e-10) # pitch should be 0 - assert np.isclose(angles_x90.z, 0.0, atol=1e-10) # yaw should be 0 - - -def test_lcm_encode_decode() -> None: - """Test encoding and decoding of Quaternion to/from binary LCM format.""" - q_source = Quaternion(1.0, 2.0, 3.0, 4.0) - - binary_msg = q_source.lcm_encode() - - q_dest = Quaternion.lcm_decode(binary_msg) - + angles = vector_from_array(quaternion_euler(q_identity)) + assert np.isclose(angles.x, 0.0, atol=1e-10) + assert np.isclose(angles.y, 0.0, atol=1e-10) + assert np.isclose(angles.z, 0.0, atol=1e-10) + q_z90 = Quaternion(x=0, y=0, z=np.sin(np.pi / 4), w=np.cos(np.pi / 4)) + angles_z90 = vector_from_array(quaternion_euler(q_z90)) + assert np.isclose(angles_z90.x, 0.0, atol=1e-10) + assert np.isclose(angles_z90.y, 0.0, atol=1e-10) + assert np.isclose(angles_z90.z, np.pi / 2, atol=1e-10) + q_x90 = Quaternion(x=np.sin(np.pi / 4), y=0, z=0, w=np.cos(np.pi / 4)) + angles_x90 = vector_from_array(quaternion_euler(q_x90)) + assert np.isclose(angles_x90.x, np.pi / 2, atol=1e-10) + assert np.isclose(angles_x90.y, 0.0, atol=1e-10) + assert np.isclose(angles_x90.z, 0.0, atol=1e-10) + + +def test_cdr_encode_decode() -> None: + """Test encoding and decoding of Quaternion to/from binary CDR format.""" + q_source = Quaternion(x=1.0, y=2.0, z=3.0, w=4.0) + binary_msg = q_source.encode() + q_dest = Quaternion.decode(binary_msg) assert isinstance(q_dest, Quaternion) assert q_dest is not q_source assert q_dest == q_source @@ -212,32 +183,20 @@ def test_lcm_encode_decode() -> None: def test_quaternion_multiplication() -> None: """Test quaternion multiplication (Hamilton product).""" - # Test identity multiplication - q1 = Quaternion(0.5, 0.5, 0.5, 0.5) - identity = Quaternion(0, 0, 0, 1) - - result = q1 * identity + q1 = Quaternion(x=0.5, y=0.5, z=0.5, w=0.5) + identity = Quaternion(x=0, y=0, z=0, w=1) + result = quaternion_product(q1, identity) assert np.allclose([result.x, result.y, result.z, result.w], [q1.x, q1.y, q1.z, q1.w]) - - # Test multiplication order matters (non-commutative) - q2 = Quaternion(0.1, 0.2, 0.3, 0.4) - q3 = Quaternion(0.4, 0.3, 0.2, 0.1) - - result1 = q2 * q3 - result2 = q3 * q2 - - # Results should be different + q2 = Quaternion(x=0.1, y=0.2, z=0.3, w=0.4) + q3 = Quaternion(x=0.4, y=0.3, z=0.2, w=0.1) + result1 = quaternion_product(q2, q3) + result2 = quaternion_product(q3, q2) assert not np.allclose( [result1.x, result1.y, result1.z, result1.w], [result2.x, result2.y, result2.z, result2.w] ) - - # Test specific multiplication case - # 90 degree rotations around Z axis angle = np.pi / 2 - q_90z = Quaternion(0, 0, np.sin(angle / 2), np.cos(angle / 2)) - - # Two 90 degree rotations should give 180 degrees - result = q_90z * q_90z + q_90z = Quaternion(x=0, y=0, z=np.sin(angle / 2), w=np.cos(angle / 2)) + result = quaternion_product(q_90z, q_90z) expected_angle = np.pi assert np.isclose(result.x, 0, atol=1e-10) assert np.isclose(result.y, 0, atol=1e-10) @@ -247,50 +206,36 @@ def test_quaternion_multiplication() -> None: def test_quaternion_conjugate() -> None: """Test quaternion conjugate.""" - q = Quaternion(0.1, 0.2, 0.3, 0.4) - conj = q.conjugate() - - # Conjugate should negate x, y, z but keep w + q = Quaternion(x=0.1, y=0.2, z=0.3, w=0.4) + conj = quaternion_conjugate(q) assert conj.x == -q.x assert conj.y == -q.y assert conj.z == -q.z assert conj.w == q.w - - # Test that q * q^* gives a real quaternion (x=y=z=0) - result = q * conj + result = quaternion_product(q, conj) assert np.isclose(result.x, 0, atol=1e-10) assert np.isclose(result.y, 0, atol=1e-10) assert np.isclose(result.z, 0, atol=1e-10) - # w should be the squared norm expected_w = q.x**2 + q.y**2 + q.z**2 + q.w**2 assert np.isclose(result.w, expected_w, atol=1e-10) def test_quaternion_inverse() -> None: """Test quaternion inverse.""" - # Test with unit quaternion - q_unit = Quaternion(0, 0, 0, 1).normalize() # Already normalized but being explicit - inv = q_unit.inverse() - - # For unit quaternion, inverse equals conjugate - conj = q_unit.conjugate() + q_unit = normalized_quaternion(Quaternion(x=0, y=0, z=0, w=1)) + inv = quaternion_inverse(q_unit) + conj = quaternion_conjugate(q_unit) assert np.allclose([inv.x, inv.y, inv.z, inv.w], [conj.x, conj.y, conj.z, conj.w]) - - # Test that q * q^-1 = identity - q = Quaternion(0.5, 0.5, 0.5, 0.5) - inv = q.inverse() - result = q * inv - + q = Quaternion(x=0.5, y=0.5, z=0.5, w=0.5) + inv = quaternion_inverse(q) + result = quaternion_product(q, inv) assert np.isclose(result.x, 0, atol=1e-10) assert np.isclose(result.y, 0, atol=1e-10) assert np.isclose(result.z, 0, atol=1e-10) assert np.isclose(result.w, 1, atol=1e-10) - - # Test inverse of non-unit quaternion - q_non_unit = Quaternion(2, 0, 0, 0) # Non-unit quaternion - inv = q_non_unit.inverse() - result = q_non_unit * inv - + q_non_unit = Quaternion(x=2, y=0, z=0, w=0) + inv = quaternion_inverse(q_non_unit) + result = quaternion_product(q_non_unit, inv) assert np.isclose(result.x, 0, atol=1e-10) assert np.isclose(result.y, 0, atol=1e-10) assert np.isclose(result.z, 0, atol=1e-10) @@ -299,15 +244,10 @@ def test_quaternion_inverse() -> None: def test_quaternion_normalize() -> None: """Test quaternion normalization.""" - # Test non-unit quaternion - q = Quaternion(1, 2, 3, 4) - q_norm = q.normalize() - - # Check that magnitude is 1 + q = Quaternion(x=1, y=2, z=3, w=4) + q_norm = normalized_quaternion(q) magnitude = np.sqrt(q_norm.x**2 + q_norm.y**2 + q_norm.z**2 + q_norm.w**2) assert np.isclose(magnitude, 1.0, atol=1e-10) - - # Check that direction is preserved scale = np.sqrt(q.x**2 + q.y**2 + q.z**2 + q.w**2) assert np.isclose(q_norm.x, q.x / scale, atol=1e-10) assert np.isclose(q_norm.y, q.y / scale, atol=1e-10) @@ -317,44 +257,26 @@ def test_quaternion_normalize() -> None: def test_quaternion_rotate_vector() -> None: """Test rotating vectors with quaternions.""" - from dimos.msgs.geometry_msgs.Vector3 import Vector3 - - # Test rotation of unit vectors - # 90 degree rotation around Z axis angle = np.pi / 2 - q_rot = Quaternion(0, 0, np.sin(angle / 2), np.cos(angle / 2)) - - # Rotate X unit vector - v_x = Vector3(1, 0, 0) - v_rotated = q_rot.rotate_vector(v_x) - - # Should now point along Y axis + q_rot = Quaternion(x=0, y=0, z=np.sin(angle / 2), w=np.cos(angle / 2)) + v_x = Vector3(x=1, y=0, z=0) + v_rotated = rotate_vector(q_rot, v_x) assert np.isclose(v_rotated.x, 0, atol=1e-10) assert np.isclose(v_rotated.y, 1, atol=1e-10) assert np.isclose(v_rotated.z, 0, atol=1e-10) - - # Rotate Y unit vector - v_y = Vector3(0, 1, 0) - v_rotated = q_rot.rotate_vector(v_y) - - # Should now point along negative X axis + v_y = Vector3(x=0, y=1, z=0) + v_rotated = rotate_vector(q_rot, v_y) assert np.isclose(v_rotated.x, -1, atol=1e-10) assert np.isclose(v_rotated.y, 0, atol=1e-10) assert np.isclose(v_rotated.z, 0, atol=1e-10) - - # Test that Z vector is unchanged (rotation axis) - v_z = Vector3(0, 0, 1) - v_rotated = q_rot.rotate_vector(v_z) - + v_z = Vector3(x=0, y=0, z=1) + v_rotated = rotate_vector(q_rot, v_z) assert np.isclose(v_rotated.x, 0, atol=1e-10) assert np.isclose(v_rotated.y, 0, atol=1e-10) assert np.isclose(v_rotated.z, 1, atol=1e-10) - - # Test identity rotation - q_identity = Quaternion(0, 0, 0, 1) - v = Vector3(1, 2, 3) - v_rotated = q_identity.rotate_vector(v) - + q_identity = Quaternion(x=0, y=0, z=0, w=1) + v = Vector3(x=1, y=2, z=3) + v_rotated = rotate_vector(q_identity, v) assert np.isclose(v_rotated.x, v.x, atol=1e-10) assert np.isclose(v_rotated.y, v.y, atol=1e-10) assert np.isclose(v_rotated.z, v.z, atol=1e-10) @@ -362,26 +284,22 @@ def test_quaternion_rotate_vector() -> None: def test_quaternion_inverse_zero() -> None: """Test that inverting zero quaternion raises error.""" - q_zero = Quaternion(0, 0, 0, 0) - + q_zero = Quaternion(x=0, y=0, z=0, w=0) with pytest.raises(ZeroDivisionError, match="Cannot invert zero quaternion"): - q_zero.inverse() + quaternion_inverse(q_zero) def test_quaternion_normalize_zero() -> None: """Test that normalizing zero quaternion raises error.""" - q_zero = Quaternion(0, 0, 0, 0) - + q_zero = Quaternion(x=0, y=0, z=0, w=0) with pytest.raises(ZeroDivisionError, match="Cannot normalize zero quaternion"): - q_zero.normalize() + normalized_quaternion(q_zero) def test_quaternion_multiplication_type_error() -> None: """Test that multiplying quaternion with non-quaternion raises error.""" - q = Quaternion(1, 0, 0, 0) - + q = Quaternion(x=1, y=0, z=0, w=0) with pytest.raises(TypeError, match="Cannot multiply Quaternion with"): - q * 5.0 - + quaternion_product(q, 5.0) with pytest.raises(TypeError, match="Cannot multiply Quaternion with"): - q * [1, 2, 3, 4] + quaternion_product(q, [1, 2, 3, 4]) diff --git a/dimos/msgs/geometry_msgs/test_Transform.py b/dimos/msgs/geometry_msgs/test_Transform.py index 056238719a1..eb397dbe02c 100644 --- a/dimos/msgs/geometry_msgs/test_Transform.py +++ b/dimos/msgs/geometry_msgs/test_Transform.py @@ -13,20 +13,35 @@ # limitations under the License. import math -import time +from dimos_generated.builtin_interfaces.msg import Time +from dimos_generated.geometry_msgs.msg import ( + Point, + Pose, + PoseStamped, + Quaternion, + Transform, + TransformStamped, + Vector3, +) +from dimos_generated.std_msgs.msg import Header import numpy as np import pytest +from rosbags.typesys import Stores, get_typestore -from dimos.msgs.geometry_msgs.Pose import Pose -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Transform import Transform -from dimos.msgs.geometry_msgs.Vector3 import Vector3 +from dimos.msgs.geometry import ( + compose_transform_values, + compose_transforms, + point_from_array, + quaternion_from_euler, + relative_pose_transform, + transform_from_pose, + transform_pose_local, + vector_array, +) def test_transform_initialization() -> None: - # Test default initialization (identity transform) tf = Transform() assert tf.translation.x == 0.0 assert tf.translation.y == 0.0 @@ -35,390 +50,253 @@ def test_transform_initialization() -> None: assert tf.rotation.y == 0.0 assert tf.rotation.z == 0.0 assert tf.rotation.w == 1.0 - - # Test initialization with Vector3 and Quaternion - trans = Vector3(1.0, 2.0, 3.0) - rot = Quaternion(0.0, 0.0, 0.707107, 0.707107) # 90 degrees around Z + trans = Vector3(x=1.0, y=2.0, z=3.0) + rot = Quaternion(x=0.0, y=0.0, z=0.707107, w=0.707107) tf2 = Transform(translation=trans, rotation=rot) assert tf2.translation == trans assert tf2.rotation == rot - - # Test initialization with only translation - tf5 = Transform(translation=Vector3(7.0, 8.0, 9.0)) + tf5 = Transform(translation=Vector3(x=7.0, y=8.0, z=9.0)) assert tf5.translation.x == 7.0 assert tf5.translation.y == 8.0 assert tf5.translation.z == 9.0 - assert tf5.rotation.w == 1.0 # Identity rotation - - # Test initialization with only rotation - tf6 = Transform(rotation=Quaternion(0.0, 0.0, 0.0, 1.0)) - assert tf6.translation.is_zero() # Zero translation + assert tf5.rotation.w == 1.0 + tf6 = Transform(rotation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0)) + assert np.allclose(vector_array(tf6.translation), 0) assert tf6.rotation.w == 1.0 - - # Test keyword argument initialization - tf7 = Transform(translation=Vector3(1, 2, 3), rotation=Quaternion()) - assert tf7.translation == Vector3(1, 2, 3) + tf7 = Transform(translation=Vector3(x=1, y=2, z=3), rotation=Quaternion()) + assert tf7.translation == Vector3(x=1, y=2, z=3) assert tf7.rotation == Quaternion() - - # Test keyword with only translation - tf8 = Transform(translation=Vector3(4, 5, 6)) - assert tf8.translation == Vector3(4, 5, 6) + tf8 = Transform(translation=Vector3(x=4, y=5, z=6)) + assert tf8.translation == Vector3(x=4, y=5, z=6) assert tf8.rotation.w == 1.0 - - # Test keyword with only rotation - tf9 = Transform(rotation=Quaternion(0, 0, 1, 0)) - assert tf9.translation.is_zero() - assert tf9.rotation == Quaternion(0, 0, 1, 0) + tf9 = Transform(rotation=Quaternion(x=0, y=0, z=1, w=0)) + assert np.allclose(vector_array(tf9.translation), 0) + assert tf9.rotation == Quaternion(x=0, y=0, z=1, w=0) def test_transform_identity() -> None: - # Test identity class method - tf = Transform.identity() - assert tf.translation.is_zero() + tf = Transform() + assert np.allclose(vector_array(tf.translation), 0) assert tf.rotation.x == 0.0 assert tf.rotation.y == 0.0 assert tf.rotation.z == 0.0 assert tf.rotation.w == 1.0 - - # Identity should equal default constructor assert tf == Transform() def test_transform_equality() -> None: - tf1 = Transform(translation=Vector3(1, 2, 3), rotation=Quaternion(0, 0, 0, 1)) - tf2 = Transform(translation=Vector3(1, 2, 3), rotation=Quaternion(0, 0, 0, 1)) - tf3 = Transform(translation=Vector3(1, 2, 4), rotation=Quaternion(0, 0, 0, 1)) # Different z - tf4 = Transform( - translation=Vector3(1, 2, 3), rotation=Quaternion(0, 0, 1, 0) - ) # Different rotation - + tf1 = Transform(translation=Vector3(x=1, y=2, z=3), rotation=Quaternion(x=0, y=0, z=0, w=1)) + tf2 = Transform(translation=Vector3(x=1, y=2, z=3), rotation=Quaternion(x=0, y=0, z=0, w=1)) + tf3 = Transform(translation=Vector3(x=1, y=2, z=4), rotation=Quaternion(x=0, y=0, z=0, w=1)) + tf4 = Transform(translation=Vector3(x=1, y=2, z=3), rotation=Quaternion(x=0, y=0, z=1, w=0)) assert tf1 == tf2 assert tf1 != tf3 assert tf1 != tf4 assert tf1 != "not a transform" -def test_transform_string_representations() -> None: - tf = Transform( - translation=Vector3(1.5, -2.0, 3.14), rotation=Quaternion(0, 0, 0.707107, 0.707107) +def test_transform_independent_cdr_fields() -> None: + source = Transform( + translation=Vector3(x=1.5, y=-2.0, z=3.14), rotation=Quaternion(z=0.707107, w=0.707107) ) - - # Test repr - repr_str = repr(tf) - assert "Transform" in repr_str - assert "translation=" in repr_str - assert "rotation=" in repr_str - assert "1.5" in repr_str - - # Test str - str_str = str(tf) - assert "Translation:" in str_str - assert "Rotation:" in str_str + decoded = get_typestore(Stores.ROS2_JAZZY).deserialize_cdr(source.encode(), Transform.msg_name) + assert [decoded.translation.x, decoded.translation.y, decoded.translation.z] == [ + 1.5, + -2.0, + 3.14, + ] + assert [decoded.rotation.x, decoded.rotation.y, decoded.rotation.z, decoded.rotation.w] == [ + 0, + 0, + 0.707107, + 0.707107, + ] def test_pose_add_transform() -> None: - initial_pose = Pose(1.0, 0.0, 0.0) - - # 90 degree rotation around Z axis + initial_pose = Pose(position=Point(x=1.0, y=0.0, z=0.0)) angle = np.pi / 2 transform = Transform( - translation=Vector3(2.0, 1.0, 0.0), - rotation=Quaternion(0.0, 0.0, np.sin(angle / 2), np.cos(angle / 2)), + translation=Vector3(x=2.0, y=1.0, z=0.0), + rotation=Quaternion(x=0.0, y=0.0, z=np.sin(angle / 2), w=np.cos(angle / 2)), ) - - transformed_pose = initial_pose @ transform - - # - Translation (2, 1, 0) is added directly to position (1, 0, 0) - # - Result position: (3, 1, 0) + transformed_pose = transform_pose_local(initial_pose, transform) assert np.isclose(transformed_pose.position.x, 3.0, atol=1e-10) assert np.isclose(transformed_pose.position.y, 1.0, atol=1e-10) assert np.isclose(transformed_pose.position.z, 0.0, atol=1e-10) - - # Rotation should be 90 degrees around Z assert np.isclose(transformed_pose.orientation.x, 0.0, atol=1e-10) assert np.isclose(transformed_pose.orientation.y, 0.0, atol=1e-10) assert np.isclose(transformed_pose.orientation.z, np.sin(angle / 2), atol=1e-10) assert np.isclose(transformed_pose.orientation.w, np.cos(angle / 2), atol=1e-10) - - initial_pose_stamped = PoseStamped( - position=initial_pose.position, orientation=initial_pose.orientation - ) - transformed_pose_stamped = PoseStamped( - position=transformed_pose.position, orientation=transformed_pose.orientation - ) - - found_tf = initial_pose_stamped.find_transform(transformed_pose_stamped) - - assert found_tf.translation == transform.translation - assert found_tf.rotation == transform.rotation - assert found_tf.translation.x == transform.translation.x - assert found_tf.translation.y == transform.translation.y - assert found_tf.translation.z == transform.translation.z - - assert found_tf.rotation.x == transform.rotation.x - assert found_tf.rotation.y == transform.rotation.y - assert found_tf.rotation.z == transform.rotation.z - assert found_tf.rotation.w == transform.rotation.w - - print(found_tf.rotation, found_tf.translation) + found_tf = relative_pose_transform(initial_pose, transformed_pose) + assert found_tf == transform def test_pose_add_transform_with_rotation() -> None: - # Create a pose at (0, 0, 0) rotated 90 degrees around Z angle = np.pi / 2 - initial_pose = Pose(0.0, 0.0, 0.0, 0.0, 0.0, np.sin(angle / 2), np.cos(angle / 2)) - - # Add 45 degree rotation to transform1 - rotation_angle = np.pi / 4 # 45 degrees + initial_pose = Pose( + position=Point(x=0.0, y=0.0, z=0.0), + orientation=Quaternion(x=0.0, y=0.0, z=np.sin(angle / 2), w=np.cos(angle / 2)), + ) + rotation_angle = np.pi / 4 transform1 = Transform( - translation=Vector3(1.0, 0.0, 0.0), + translation=Vector3(x=1.0, y=0.0, z=0.0), rotation=Quaternion( - 0.0, 0.0, np.sin(rotation_angle / 2), np.cos(rotation_angle / 2) - ), # 45� around Z + x=0.0, y=0.0, z=np.sin(rotation_angle / 2), w=np.cos(rotation_angle / 2) + ), ) - transform2 = Transform( - translation=Vector3(0.0, 1.0, 1.0), - rotation=Quaternion(0.0, 0.0, 0.0, 1.0), # No rotation + translation=Vector3(x=0.0, y=1.0, z=1.0), rotation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0) + ) + transformed_pose1 = transform_pose_local(initial_pose, transform1) + transformed_pose2 = transform_pose_local( + transform_pose_local(initial_pose, transform1), transform2 ) - - transformed_pose1 = initial_pose @ transform1 - transformed_pose2 = initial_pose @ transform1 @ transform2 - - # Test transformed_pose1: initial_pose + transform1 - # Since the pose is rotated 90� (facing +Y), moving forward (local X) - # means moving in the +Y direction in world frame assert np.isclose(transformed_pose1.position.x, 0.0, atol=1e-10) assert np.isclose(transformed_pose1.position.y, 1.0, atol=1e-10) assert np.isclose(transformed_pose1.position.z, 0.0, atol=1e-10) - - # Orientation should be 90� + 45� = 135� around Z - total_angle1 = angle + rotation_angle # 135 degrees + total_angle1 = angle + rotation_angle assert np.isclose(transformed_pose1.orientation.x, 0.0, atol=1e-10) assert np.isclose(transformed_pose1.orientation.y, 0.0, atol=1e-10) assert np.isclose(transformed_pose1.orientation.z, np.sin(total_angle1 / 2), atol=1e-10) assert np.isclose(transformed_pose1.orientation.w, np.cos(total_angle1 / 2), atol=1e-10) - - # Test transformed_pose2: initial_pose + transform1 + transform2 - # Starting from (0, 0, 0) facing 90�: - # - # - Apply transform1: move 1 forward (along +Y) � (0, 1, 0), now facing 135� - # - # - Apply transform2: move 1 in local Y and 1 up - # At 135�, local Y points at 225� (135� + 90�) - # - # x += cos(225�) = -2/2, y += sin(225�) = -2/2 sqrt2_2 = np.sqrt(2) / 2 - expected_x = 0.0 - sqrt2_2 # 0 - 2/2 H -0.707 - expected_y = 1.0 - sqrt2_2 # 1 - 2/2 H 0.293 - expected_z = 1.0 # 0 + 1 - + expected_x = 0.0 - sqrt2_2 + expected_y = 1.0 - sqrt2_2 + expected_z = 1.0 assert np.isclose(transformed_pose2.position.x, expected_x, atol=1e-10) assert np.isclose(transformed_pose2.position.y, expected_y, atol=1e-10) assert np.isclose(transformed_pose2.position.z, expected_z, atol=1e-10) - - # Orientation should be 135� (only transform1 has rotation) - total_angle2 = total_angle1 # 135 degrees (transform2 has no rotation) + total_angle2 = total_angle1 assert np.isclose(transformed_pose2.orientation.x, 0.0, atol=1e-10) assert np.isclose(transformed_pose2.orientation.y, 0.0, atol=1e-10) assert np.isclose(transformed_pose2.orientation.z, np.sin(total_angle2 / 2), atol=1e-10) assert np.isclose(transformed_pose2.orientation.w, np.cos(total_angle2 / 2), atol=1e-10) -def test_lcm_encode_decode() -> None: +def test_encode_decode() -> None: angle = np.pi / 2 transform = Transform( - translation=Vector3(2.0, 1.0, 0.0), - rotation=Quaternion(0.0, 0.0, np.sin(angle / 2), np.cos(angle / 2)), + translation=Vector3(x=2.0, y=1.0, z=0.0), + rotation=Quaternion(x=0.0, y=0.0, z=np.sin(angle / 2), w=np.cos(angle / 2)), ) - - data = transform.lcm_encode() - - decoded_transform = Transform.lcm_decode(data) - + data = transform.encode() + decoded_transform = Transform.decode(data) assert decoded_transform == transform def test_transform_addition() -> None: - # Test 1: Simple translation addition (no rotation) - t1 = Transform( - translation=Vector3(1, 0, 0), - rotation=Quaternion(0, 0, 0, 1), # identity rotation - ) - t2 = Transform( - translation=Vector3(2, 0, 0), - rotation=Quaternion(0, 0, 0, 1), # identity rotation - ) - t3 = t1 + t2 - assert t3.translation == Vector3(3, 0, 0) - assert t3.rotation == Quaternion(0, 0, 0, 1) - - # Test 2: 90-degree rotation composition - # First transform: move 1 unit in X - t1 = Transform( - translation=Vector3(1, 0, 0), - rotation=Quaternion(0, 0, 0, 1), # identity - ) - # Second transform: move 1 unit in X with 90-degree rotation around Z + t1 = Transform(translation=Vector3(x=1, y=0, z=0), rotation=Quaternion(x=0, y=0, z=0, w=1)) + t2 = Transform(translation=Vector3(x=2, y=0, z=0), rotation=Quaternion(x=0, y=0, z=0, w=1)) + t3 = compose_transform_values(t1, t2) + assert t3.translation == Vector3(x=3, y=0, z=0) + assert t3.rotation == Quaternion(x=0, y=0, z=0, w=1) + t1 = Transform(translation=Vector3(x=1, y=0, z=0), rotation=Quaternion(x=0, y=0, z=0, w=1)) angle = np.pi / 2 t2 = Transform( - translation=Vector3(1, 0, 0), - rotation=Quaternion(0, 0, np.sin(angle / 2), np.cos(angle / 2)), + translation=Vector3(x=1, y=0, z=0), + rotation=Quaternion(x=0, y=0, z=np.sin(angle / 2), w=np.cos(angle / 2)), ) - t3 = t1 + t2 - assert t3.translation == Vector3(2, 0, 0) - # Rotation should be 90 degrees around Z + t3 = compose_transform_values(t1, t2) + assert t3.translation == Vector3(x=2, y=0, z=0) assert np.isclose(t3.rotation.x, 0.0, atol=1e-10) assert np.isclose(t3.rotation.y, 0.0, atol=1e-10) assert np.isclose(t3.rotation.z, np.sin(angle / 2), atol=1e-10) assert np.isclose(t3.rotation.w, np.cos(angle / 2), atol=1e-10) - - # Test 3: Rotation affects translation - # First transform: 90-degree rotation around Z t1 = Transform( - translation=Vector3(0, 0, 0), - rotation=Quaternion(0, 0, np.sin(angle / 2), np.cos(angle / 2)), # 90° around Z - ) - # Second transform: move 1 unit in X - t2 = Transform( - translation=Vector3(1, 0, 0), - rotation=Quaternion(0, 0, 0, 1), # identity + translation=Vector3(x=0, y=0, z=0), + rotation=Quaternion(x=0, y=0, z=np.sin(angle / 2), w=np.cos(angle / 2)), ) - t3 = t1 + t2 - # X direction rotated 90° becomes Y direction + t2 = Transform(translation=Vector3(x=1, y=0, z=0), rotation=Quaternion(x=0, y=0, z=0, w=1)) + t3 = compose_transform_values(t1, t2) assert np.isclose(t3.translation.x, 0.0, atol=1e-10) assert np.isclose(t3.translation.y, 1.0, atol=1e-10) assert np.isclose(t3.translation.z, 0.0, atol=1e-10) - # Rotation remains 90° around Z assert np.isclose(t3.rotation.z, np.sin(angle / 2), atol=1e-10) assert np.isclose(t3.rotation.w, np.cos(angle / 2), atol=1e-10) - # Test 4: Frame tracking - t1 = Transform( - translation=Vector3(1, 0, 0), - rotation=Quaternion(0, 0, 0, 1), - frame_id="world", + +def test_transform_frame_tracking() -> None: + first = TransformStamped( + header=Header(frame_id="world"), child_frame_id="robot", + transform=Transform(translation=Vector3(x=1)), ) - t2 = Transform( - translation=Vector3(2, 0, 0), - rotation=Quaternion(0, 0, 0, 1), - frame_id="robot", + second = TransformStamped( + header=Header(frame_id="robot"), child_frame_id="sensor", + transform=Transform(translation=Vector3(x=2)), ) - t3 = t1 + t2 - assert t3.frame_id == "world" - assert t3.child_frame_id == "sensor" - - # Test 5: Type error + result = compose_transforms(first, second) + assert result.header.frame_id == "world" + assert result.child_frame_id == "sensor" + assert result.transform.translation == Vector3(x=3) with pytest.raises(TypeError): - t1 + "not a transform" + compose_transform_values(first.transform, "not a transform") def test_transform_from_pose() -> None: - """Test converting Pose to Transform""" - # Create a Pose with position and orientation - pose = Pose( - position=Vector3(1.0, 2.0, 3.0), - orientation=Quaternion(0.0, 0.0, 0.707, 0.707), # 90 degrees around Z + pose = Pose(position=Point(x=1, y=2, z=3), orientation=Quaternion(z=0.707, w=0.707)) + result = transform_from_pose( + PoseStamped(header=Header(frame_id="world"), pose=pose), child_frame_id="base_link" ) - - # Convert to Transform - transform = Transform.from_pose("base_link", pose) - - # Check that translation and rotation match - assert transform.translation == pose.position - assert transform.rotation == pose.orientation - assert transform.frame_id == "world" # default frame_id - assert transform.child_frame_id == "base_link" # passed as first argument + np.testing.assert_array_equal( + vector_array(result.transform.translation), vector_array(pose.position) + ) + assert result.transform.rotation == pose.orientation + assert result.header.frame_id == "world" + assert result.child_frame_id == "base_link" -# validating results from example @ -# https://foxglove.dev/blog/understanding-ros-transforms def test_transform_from_ros() -> None: - """Test converting PoseStamped to Transform""" - test_time = time.time() - pose_stamped = PoseStamped( - ts=test_time, - frame_id="base_link", - position=Vector3(1, -1, 0), - orientation=Quaternion.from_euler(Vector3(0, 0, math.pi / 6)), + stamp = Time(sec=123, nanosec=456789123) + first = transform_from_pose( + PoseStamped( + header=Header(stamp=stamp, frame_id="base_link"), + pose=Pose( + position=Point(x=1, y=-1), orientation=quaternion_from_euler(0, 0, math.pi / 6) + ), + ), + child_frame_id="arm_base_link", ) - transform_base_link_to_arm = Transform.from_pose("arm_base_link", pose_stamped) - - transform_arm_to_end = Transform.from_pose( - "end", + second = transform_from_pose( PoseStamped( - ts=test_time, - frame_id="arm_base_link", - position=Vector3(1, 1, 0), - orientation=Quaternion.from_euler(Vector3(0, 0, math.pi / 6)), + header=Header(stamp=stamp, frame_id="arm_base_link"), + pose=Pose( + position=Point(x=1, y=1), orientation=quaternion_from_euler(0, 0, math.pi / 6) + ), ), + child_frame_id="end", ) - - print(transform_base_link_to_arm) - print(transform_arm_to_end) - - end_effector_global_pose = transform_base_link_to_arm + transform_arm_to_end - - assert end_effector_global_pose.translation.x == pytest.approx(1.366, abs=1e-3) - assert end_effector_global_pose.translation.y == pytest.approx(0.366, abs=1e-3) + result = compose_transforms(first, second) + assert result.transform.translation.x == pytest.approx(1.366, abs=1e-3) + assert result.transform.translation.y == pytest.approx(0.366, abs=1e-3) + assert result.header.stamp == stamp def test_transform_from_pose_stamped() -> None: - """Test converting PoseStamped to Transform""" - # Create a PoseStamped with position, orientation, timestamp and frame - test_time = time.time() - pose_stamped = PoseStamped( - ts=test_time, - frame_id="map", - position=Vector3(4.0, 5.0, 6.0), - orientation=Quaternion(0.0, 0.707, 0.0, 0.707), # 90 degrees around Y + pose = PoseStamped( + header=Header(stamp=Time(sec=123, nanosec=456789123), frame_id="map"), + pose=Pose(position=Point(x=4, y=5, z=6), orientation=Quaternion(y=0.707, w=0.707)), ) - - # Convert to Transform - transform = Transform.from_pose("robot_base", pose_stamped) - - # Check that all fields match - assert transform.translation == pose_stamped.position - assert transform.rotation == pose_stamped.orientation - assert transform.frame_id == pose_stamped.frame_id - assert transform.ts == pose_stamped.ts - assert transform.child_frame_id == "robot_base" # passed as first argument - - -def test_transform_from_pose_variants() -> None: - """Test from_pose with different Pose initialization methods""" - # Test with Pose created from x,y,z - pose1 = Pose(1.0, 2.0, 3.0) - transform1 = Transform.from_pose("base_link", pose1) - assert transform1.translation.x == 1.0 - assert transform1.translation.y == 2.0 - assert transform1.translation.z == 3.0 - assert transform1.rotation.w == 1.0 # Identity quaternion - - # Test with Pose created from tuple - pose2 = Pose(([7.0, 8.0, 9.0], [0.0, 0.0, 0.0, 1.0])) - transform2 = Transform.from_pose("base_link", pose2) - assert transform2.translation.x == 7.0 - assert transform2.translation.y == 8.0 - assert transform2.translation.z == 9.0 - - # Test with Pose created from dict - pose3 = Pose({"position": [10.0, 11.0, 12.0], "orientation": [0.0, 0.0, 0.0, 1.0]}) - transform3 = Transform.from_pose("base_link", pose3) - assert transform3.translation.x == 10.0 - assert transform3.translation.y == 11.0 - assert transform3.translation.z == 12.0 + result = transform_from_pose(pose, child_frame_id="robot_base") + np.testing.assert_array_equal( + vector_array(result.transform.translation), vector_array(pose.pose.position) + ) + assert result.transform.rotation == pose.pose.orientation + assert result.header == pose.header + assert result.child_frame_id == "robot_base" -def test_transform_from_pose_invalid_type() -> None: - """Test that from_pose raises TypeError for invalid types""" - with pytest.raises(TypeError): - Transform.from_pose("not a pose") +@pytest.mark.parametrize("values", [[1.0, 2.0, 3.0], (7.0, 8.0, 9.0), [10.0, 11.0, 12.0]]) +def test_transform_from_coordinate_arrays(values) -> None: + pose = PoseStamped(pose=Pose(position=point_from_array(values))) + result = transform_from_pose(pose, child_frame_id="base_link") + np.testing.assert_array_equal(vector_array(result.transform.translation), values) + assert result.transform.rotation == Quaternion() - with pytest.raises(TypeError): - Transform.from_pose(42) +@pytest.mark.parametrize("value", ["not a pose", 42, None]) +def test_transform_from_pose_invalid_type(value) -> None: with pytest.raises(TypeError): - Transform.from_pose(None) + transform_from_pose(value, child_frame_id="base_link") diff --git a/dimos/msgs/geometry_msgs/test_Twist.py b/dimos/msgs/geometry_msgs/test_Twist.py index a4dc93f3cc0..bff834b4c57 100644 --- a/dimos/msgs/geometry_msgs/test_Twist.py +++ b/dimos/msgs/geometry_msgs/test_Twist.py @@ -12,16 +12,16 @@ # See the License for the specific language governing permissions and # limitations under the License. -from dimos_lcm.geometry_msgs import Twist as LCMTwist +from copy import deepcopy + +from dimos_generated.geometry_msgs.msg import Quaternion, Twist, Vector3 import numpy as np +from rosbags.typesys import Stores, get_typestore -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Twist import Twist -from dimos.msgs.geometry_msgs.Vector3 import Vector3 +from dimos.msgs.geometry import quaternion_euler, vector_array, vector_from_array def test_twist_initialization() -> None: - # Test default initialization (zero twist) tw = Twist() assert tw.linear.x == 0.0 assert tw.linear.y == 0.0 @@ -29,173 +29,129 @@ def test_twist_initialization() -> None: assert tw.angular.x == 0.0 assert tw.angular.y == 0.0 assert tw.angular.z == 0.0 - - # Test initialization with Vector3 linear and angular - lin = Vector3(1.0, 2.0, 3.0) - ang = Vector3(0.1, 0.2, 0.3) - tw2 = Twist(lin, ang) + lin = Vector3(x=1.0, y=2.0, z=3.0) + ang = Vector3(x=0.1, y=0.2, z=0.3) + tw2 = Twist(linear=lin, angular=ang) assert tw2.linear == lin assert tw2.angular == ang - - # Test copy constructor - tw3 = Twist(tw2) + tw3 = deepcopy(tw2) assert tw3.linear == tw2.linear assert tw3.angular == tw2.angular assert tw3 == tw2 - # Ensure it's a deep copy tw3.linear.x = 10.0 assert tw2.linear.x == 1.0 - - # Test initialization from LCM Twist - lcm_tw = LCMTwist() - lcm_tw.linear = Vector3(4.0, 5.0, 6.0) - lcm_tw.angular = Vector3(0.4, 0.5, 0.6) - tw4 = Twist(lcm_tw) + source = Twist() + source.linear = Vector3(x=4.0, y=5.0, z=6.0) + source.angular = Vector3(x=0.4, y=0.5, z=0.6) + tw4 = Twist.decode(source.encode()) assert tw4.linear.x == 4.0 assert tw4.linear.y == 5.0 assert tw4.linear.z == 6.0 assert tw4.angular.x == 0.4 assert tw4.angular.y == 0.5 assert tw4.angular.z == 0.6 - - # Test initialization with linear and angular as quaternion - quat = Quaternion(0, 0, 0.707107, 0.707107) # 90 degrees around Z - tw5 = Twist(Vector3(1.0, 2.0, 3.0), quat) - assert tw5.linear == Vector3(1.0, 2.0, 3.0) - # Quaternion should be converted to euler angles - euler = quat.to_euler() + quat = Quaternion(x=0, y=0, z=0.707107, w=0.707107) + tw5 = Twist( + linear=Vector3(x=1.0, y=2.0, z=3.0), angular=vector_from_array(quaternion_euler(quat)) + ) + assert tw5.linear == Vector3(x=1.0, y=2.0, z=3.0) + euler = vector_from_array(quaternion_euler(quat)) assert np.allclose(tw5.angular.x, euler.x) assert np.allclose(tw5.angular.y, euler.y) assert np.allclose(tw5.angular.z, euler.z) - - # Test keyword argument initialization - tw7 = Twist(linear=Vector3(1, 2, 3), angular=Vector3(0.1, 0.2, 0.3)) - assert tw7.linear == Vector3(1, 2, 3) - assert tw7.angular == Vector3(0.1, 0.2, 0.3) - - # Test keyword with only linear - tw8 = Twist(linear=Vector3(4, 5, 6)) - assert tw8.linear == Vector3(4, 5, 6) - assert tw8.angular.is_zero() - - # Test keyword with only angular - tw9 = Twist(angular=Vector3(0.4, 0.5, 0.6)) - assert tw9.linear.is_zero() - assert tw9.angular == Vector3(0.4, 0.5, 0.6) - - # Test keyword with angular as quaternion - tw10 = Twist(angular=Quaternion(0, 0, 0.707107, 0.707107)) - assert tw10.linear.is_zero() - euler = Quaternion(0, 0, 0.707107, 0.707107).to_euler() + tw7 = Twist(linear=Vector3(x=1, y=2, z=3), angular=Vector3(x=0.1, y=0.2, z=0.3)) + assert tw7.linear == Vector3(x=1, y=2, z=3) + assert tw7.angular == Vector3(x=0.1, y=0.2, z=0.3) + tw8 = Twist(linear=Vector3(x=4, y=5, z=6)) + assert tw8.linear == Vector3(x=4, y=5, z=6) + assert np.allclose(vector_array(tw8.angular), 0) + tw9 = Twist(angular=Vector3(x=0.4, y=0.5, z=0.6)) + assert np.allclose(vector_array(tw9.linear), 0) + assert tw9.angular == Vector3(x=0.4, y=0.5, z=0.6) + tw10 = Twist( + angular=vector_from_array(quaternion_euler(Quaternion(x=0, y=0, z=0.707107, w=0.707107))) + ) + assert np.allclose(vector_array(tw10.linear), 0) + euler = vector_from_array(quaternion_euler(Quaternion(x=0, y=0, z=0.707107, w=0.707107))) assert np.allclose(tw10.angular.x, euler.x) assert np.allclose(tw10.angular.y, euler.y) assert np.allclose(tw10.angular.z, euler.z) - - # Test keyword with linear and angular as quaternion - tw11 = Twist(linear=Vector3(1, 0, 0), angular=Quaternion(0, 0, 0, 1)) - assert tw11.linear == Vector3(1, 0, 0) - assert tw11.angular.is_zero() # Identity quaternion -> zero euler angles + tw11 = Twist( + linear=Vector3(x=1, y=0, z=0), + angular=vector_from_array(quaternion_euler(Quaternion(x=0, y=0, z=0, w=1))), + ) + assert tw11.linear == Vector3(x=1, y=0, z=0) + assert np.allclose(vector_array(tw11.angular), 0) def test_twist_zero() -> None: - # Test zero class method - tw = Twist.zero() - assert tw.linear.is_zero() - assert tw.angular.is_zero() - assert tw.is_zero() - - # Zero should equal default constructor + tw = Twist() + assert np.allclose(vector_array(tw.linear), 0) + assert np.allclose(vector_array(tw.angular), 0) + assert np.allclose(np.concatenate((vector_array(tw.linear), vector_array(tw.angular))), 0) assert tw == Twist() def test_twist_equality() -> None: - tw1 = Twist(Vector3(1, 2, 3), Vector3(0.1, 0.2, 0.3)) - tw2 = Twist(Vector3(1, 2, 3), Vector3(0.1, 0.2, 0.3)) - tw3 = Twist(Vector3(1, 2, 4), Vector3(0.1, 0.2, 0.3)) # Different linear z - tw4 = Twist(Vector3(1, 2, 3), Vector3(0.1, 0.2, 0.4)) # Different angular z - + tw1 = Twist(linear=Vector3(x=1, y=2, z=3), angular=Vector3(x=0.1, y=0.2, z=0.3)) + tw2 = Twist(linear=Vector3(x=1, y=2, z=3), angular=Vector3(x=0.1, y=0.2, z=0.3)) + tw3 = Twist(linear=Vector3(x=1, y=2, z=4), angular=Vector3(x=0.1, y=0.2, z=0.3)) + tw4 = Twist(linear=Vector3(x=1, y=2, z=3), angular=Vector3(x=0.1, y=0.2, z=0.4)) assert tw1 == tw2 assert tw1 != tw3 assert tw1 != tw4 assert tw1 != "not a twist" -def test_twist_string_representations() -> None: - tw = Twist(Vector3(1.5, -2.0, 3.14), Vector3(0.1, -0.2, 0.3)) - - # Test repr - repr_str = repr(tw) - assert "Twist" in repr_str - assert "linear=" in repr_str - assert "angular=" in repr_str - assert "1.5" in repr_str - assert "0.1" in repr_str - - # Test str - str_str = str(tw) - assert "Twist:" in str_str - assert "Linear:" in str_str - assert "Angular:" in str_str +def test_twist_independent_cdr_fields() -> None: + tw = Twist(linear=Vector3(x=1.5, y=-2.0, z=3.14), angular=Vector3(x=0.1, y=-0.2, z=0.3)) + decoded = get_typestore(Stores.ROS2_JAZZY).deserialize_cdr( + tw.encode(), "geometry_msgs/msg/Twist" + ) + assert [decoded.linear.x, decoded.linear.y, decoded.linear.z] == [1.5, -2.0, 3.14] + assert [decoded.angular.x, decoded.angular.y, decoded.angular.z] == [0.1, -0.2, 0.3] def test_twist_is_zero() -> None: - # Test zero twist tw1 = Twist() - assert tw1.is_zero() + assert np.allclose(np.concatenate((vector_array(tw1.linear), vector_array(tw1.angular))), 0) + tw2 = Twist(linear=Vector3(x=0.1, y=0, z=0)) + assert not np.allclose(np.concatenate((vector_array(tw2.linear), vector_array(tw2.angular))), 0) + tw3 = Twist(angular=Vector3(x=0, y=0, z=0.1)) + assert not np.allclose(np.concatenate((vector_array(tw3.linear), vector_array(tw3.angular))), 0) + tw4 = Twist(linear=Vector3(x=1, y=2, z=3), angular=Vector3(x=0.1, y=0.2, z=0.3)) + assert not np.allclose(np.concatenate((vector_array(tw4.linear), vector_array(tw4.angular))), 0) - # Test non-zero linear - tw2 = Twist(linear=Vector3(0.1, 0, 0)) - assert not tw2.is_zero() - # Test non-zero angular - tw3 = Twist(angular=Vector3(0, 0, 0.1)) - assert not tw3.is_zero() - - # Test both non-zero - tw4 = Twist(Vector3(1, 2, 3), Vector3(0.1, 0.2, 0.3)) - assert not tw4.is_zero() - - -def test_twist_bool() -> None: - # Test zero twist is False +def test_twist_explicit_nonzero_predicate() -> None: tw1 = Twist() - assert not tw1 - - # Test non-zero twist is True - tw2 = Twist(linear=Vector3(1, 0, 0)) - assert tw2 - - tw3 = Twist(angular=Vector3(0, 0, 0.1)) - assert tw3 - - tw4 = Twist(Vector3(1, 2, 3), Vector3(0.1, 0.2, 0.3)) - assert tw4 - - -def test_twist_lcm_encoding() -> None: - # Test encoding and decoding - tw = Twist(Vector3(1.5, 2.5, 3.5), Vector3(0.1, 0.2, 0.3)) - - # Encode - encoded = tw.lcm_encode() + assert not (np.any(vector_array(tw1.linear)) or np.any(vector_array(tw1.angular))) + tw2 = Twist(linear=Vector3(x=1, y=0, z=0)) + assert np.any(vector_array(tw2.linear)) or np.any(vector_array(tw2.angular)) + tw3 = Twist(angular=Vector3(x=0, y=0, z=0.1)) + assert np.any(vector_array(tw3.linear)) or np.any(vector_array(tw3.angular)) + tw4 = Twist(linear=Vector3(x=1, y=2, z=3), angular=Vector3(x=0.1, y=0.2, z=0.3)) + assert np.any(vector_array(tw4.linear)) or np.any(vector_array(tw4.angular)) + + +def test_twist_cdr_encoding() -> None: + tw = Twist(linear=Vector3(x=1.5, y=2.5, z=3.5), angular=Vector3(x=0.1, y=0.2, z=0.3)) + encoded = tw.encode() assert isinstance(encoded, bytes) - - # Decode - decoded = Twist.lcm_decode(encoded) + decoded = Twist.decode(encoded) assert decoded.linear == tw.linear assert decoded.angular == tw.angular - assert isinstance(decoded.linear, Vector3) assert decoded == tw def test_twist_with_lists() -> None: - # Test initialization with lists instead of Vector3 - tw1 = Twist(linear=[1, 2, 3], angular=[0.1, 0.2, 0.3]) - assert tw1.linear == Vector3(1, 2, 3) - assert tw1.angular == Vector3(0.1, 0.2, 0.3) - - # Test with numpy arrays - tw2 = Twist(linear=np.array([4, 5, 6]), angular=np.array([0.4, 0.5, 0.6])) - assert tw2.linear == Vector3(4, 5, 6) - assert tw2.angular == Vector3(0.4, 0.5, 0.6) + tw1 = Twist(linear=vector_from_array([1, 2, 3]), angular=vector_from_array([0.1, 0.2, 0.3])) + assert tw1.linear == Vector3(x=1, y=2, z=3) + assert tw1.angular == Vector3(x=0.1, y=0.2, z=0.3) + tw2 = Twist( + linear=vector_from_array(np.array([4, 5, 6])), + angular=vector_from_array(np.array([0.4, 0.5, 0.6])), + ) + assert tw2.linear == Vector3(x=4, y=5, z=6) + assert tw2.angular == Vector3(x=0.4, y=0.5, z=0.6) diff --git a/dimos/msgs/geometry_msgs/test_TwistStamped.py b/dimos/msgs/geometry_msgs/test_TwistStamped.py index d37b4b27172..590f31291d8 100644 --- a/dimos/msgs/geometry_msgs/test_TwistStamped.py +++ b/dimos/msgs/geometry_msgs/test_TwistStamped.py @@ -13,42 +13,47 @@ # limitations under the License. import pickle -import time -from dimos.msgs.geometry_msgs.TwistStamped import TwistStamped +from dimos_generated.geometry_msgs.msg import Twist, TwistStamped, Vector3 +from dimos_generated.std_msgs.msg import Header +from rosbags.typesys import Stores, get_typestore +from dimos.msgs.time import time_from_nanoseconds -def test_lcm_encode_decode() -> None: - """Test encoding and decoding of TwistStamped to/from binary LCM format.""" - twist_source = TwistStamped( - ts=time.time(), - linear=(1.0, 2.0, 3.0), - angular=(0.1, 0.2, 0.3), - ) - binary_msg = twist_source.lcm_encode() - twist_dest = TwistStamped.lcm_decode(binary_msg) - - assert isinstance(twist_dest, TwistStamped) - assert twist_dest is not twist_source - print(twist_source.linear) - print(twist_source.angular) - - print(twist_dest.linear) - print(twist_dest.angular) - assert twist_dest == twist_source +def test_cdr_encode_decode() -> None: + source = TwistStamped( + header=Header(stamp=time_from_nanoseconds(1234567890123456789)), + twist=Twist(linear=Vector3(x=1, y=2, z=3), angular=Vector3(x=0.1, y=0.2, z=0.3)), + ) + binary = source.encode() + decoded = TwistStamped.decode(binary) + assert isinstance(decoded, TwistStamped) + assert decoded is not source + assert decoded.encode() == binary + independent = get_typestore(Stores.ROS2_JAZZY).deserialize_cdr(binary, source.msg_name) + assert (independent.header.stamp.sec, independent.header.stamp.nanosec) == ( + 1234567890, + 123456789, + ) + assert (independent.twist.linear.x, independent.twist.linear.y, independent.twist.linear.z) == ( + 1, + 2, + 3, + ) + assert ( + independent.twist.angular.x, + independent.twist.angular.y, + independent.twist.angular.z, + ) == (0.1, 0.2, 0.3) def test_pickle_encode_decode() -> None: - """Test encoding and decoding of TwistStamped to/from binary pickle format.""" - - twist_source = TwistStamped( - ts=time.time(), - linear=(1.0, 2.0, 3.0), - angular=(0.1, 0.2, 0.3), + source = TwistStamped( + header=Header(stamp=time_from_nanoseconds(1234567890123456789)), + twist=Twist(linear=Vector3(x=1, y=2, z=3), angular=Vector3(x=0.1, y=0.2, z=0.3)), ) - binary_msg = pickle.dumps(twist_source) - twist_dest = pickle.loads(binary_msg) - assert isinstance(twist_dest, TwistStamped) - assert twist_dest is not twist_source - assert twist_dest == twist_source + decoded = pickle.loads(pickle.dumps(source)) + assert isinstance(decoded, TwistStamped) + assert decoded is not source + assert decoded.encode() == source.encode() diff --git a/dimos/msgs/geometry_msgs/test_TwistWithCovariance.py b/dimos/msgs/geometry_msgs/test_TwistWithCovariance.py index 1d8ae820ad0..2814a6667c3 100644 --- a/dimos/msgs/geometry_msgs/test_TwistWithCovariance.py +++ b/dimos/msgs/geometry_msgs/test_TwistWithCovariance.py @@ -12,345 +12,120 @@ # See the License for the specific language governing permissions and # limitations under the License. -from dimos_lcm.geometry_msgs import TwistWithCovariance as LCMTwistWithCovariance -import numpy as np -import pytest - -from dimos.msgs.geometry_msgs.Twist import Twist -from dimos.msgs.geometry_msgs.TwistWithCovariance import TwistWithCovariance -from dimos.msgs.geometry_msgs.Vector3 import Vector3 - - -def test_twist_with_covariance_default_init() -> None: - """Test that default initialization creates a zero twist with zero covariance.""" - twist_cov = TwistWithCovariance() - - # Twist should be zero - assert twist_cov.twist.linear.x == 0.0 - assert twist_cov.twist.linear.y == 0.0 - assert twist_cov.twist.linear.z == 0.0 - assert twist_cov.twist.angular.x == 0.0 - assert twist_cov.twist.angular.y == 0.0 - assert twist_cov.twist.angular.z == 0.0 - - # Covariance should be all zeros - assert np.all(twist_cov.covariance == 0.0) - assert twist_cov.covariance.shape == (36,) - - -def test_twist_with_covariance_twist_init() -> None: - """Test initialization with a Twist object.""" - linear = Vector3(1.0, 2.0, 3.0) - angular = Vector3(0.1, 0.2, 0.3) - twist = Twist(linear, angular) - twist_cov = TwistWithCovariance(twist) - - # Twist should match - assert twist_cov.twist.linear.x == 1.0 - assert twist_cov.twist.linear.y == 2.0 - assert twist_cov.twist.linear.z == 3.0 - assert twist_cov.twist.angular.x == 0.1 - assert twist_cov.twist.angular.y == 0.2 - assert twist_cov.twist.angular.z == 0.3 - - # Covariance should be zeros by default - assert np.all(twist_cov.covariance == 0.0) - - -def test_twist_with_covariance_twist_and_covariance_init() -> None: - """Test initialization with twist and covariance.""" - twist = Twist(Vector3(1.0, 2.0, 3.0), Vector3(0.1, 0.2, 0.3)) - covariance = np.arange(36, dtype=float) - twist_cov = TwistWithCovariance(twist, covariance) - - # Twist should match - assert twist_cov.twist.linear.x == 1.0 - assert twist_cov.twist.linear.y == 2.0 - assert twist_cov.twist.linear.z == 3.0 - - # Covariance should match - assert np.array_equal(twist_cov.covariance, covariance) - - -def test_twist_with_covariance_tuple_init() -> None: - """Test initialization with tuple of (linear, angular) velocities.""" - linear = [1.0, 2.0, 3.0] - angular = [0.1, 0.2, 0.3] - covariance = np.arange(36, dtype=float) - twist_cov = TwistWithCovariance((linear, angular), covariance) - - # Twist should match - assert twist_cov.twist.linear.x == 1.0 - assert twist_cov.twist.linear.y == 2.0 - assert twist_cov.twist.linear.z == 3.0 - assert twist_cov.twist.angular.x == 0.1 - assert twist_cov.twist.angular.y == 0.2 - assert twist_cov.twist.angular.z == 0.3 - - # Covariance should match - assert np.array_equal(twist_cov.covariance, covariance) - - -def test_twist_with_covariance_list_covariance() -> None: - """Test initialization with covariance as a list.""" - twist = Twist(Vector3(1.0, 2.0, 3.0), Vector3(0.1, 0.2, 0.3)) - covariance_list = list(range(36)) - twist_cov = TwistWithCovariance(twist, covariance_list) - - # Covariance should be converted to numpy array - assert isinstance(twist_cov.covariance, np.ndarray) - assert np.array_equal(twist_cov.covariance, np.array(covariance_list)) - - -def test_twist_with_covariance_copy_init() -> None: - """Test copy constructor.""" - twist = Twist(Vector3(1.0, 2.0, 3.0), Vector3(0.1, 0.2, 0.3)) - covariance = np.arange(36, dtype=float) - original = TwistWithCovariance(twist, covariance) - copy = TwistWithCovariance(original) - - # Should be equal but not the same object - assert copy == original - assert copy is not original - assert copy.twist is not original.twist - assert copy.covariance is not original.covariance - - # Modify original to ensure they're independent - original.covariance[0] = 999.0 - assert copy.covariance[0] != 999.0 - - -def test_twist_with_covariance_lcm_init() -> None: - """Test initialization from LCM message.""" - lcm_msg = LCMTwistWithCovariance() - lcm_msg.twist.linear.x = 1.0 - lcm_msg.twist.linear.y = 2.0 - lcm_msg.twist.linear.z = 3.0 - lcm_msg.twist.angular.x = 0.1 - lcm_msg.twist.angular.y = 0.2 - lcm_msg.twist.angular.z = 0.3 - lcm_msg.covariance = list(range(36)) - - twist_cov = TwistWithCovariance(lcm_msg) - - # Twist should match - assert twist_cov.twist.linear.x == 1.0 - assert twist_cov.twist.linear.y == 2.0 - assert twist_cov.twist.linear.z == 3.0 - assert twist_cov.twist.angular.x == 0.1 - assert twist_cov.twist.angular.y == 0.2 - assert twist_cov.twist.angular.z == 0.3 - - # Covariance should match - assert np.array_equal(twist_cov.covariance, np.arange(36)) - - -def test_twist_with_covariance_dict_init() -> None: - """Test initialization from dictionary.""" - twist_dict = { - "twist": Twist(Vector3(1.0, 2.0, 3.0), Vector3(0.1, 0.2, 0.3)), - "covariance": list(range(36)), - } - twist_cov = TwistWithCovariance(twist_dict) - - assert twist_cov.twist.linear.x == 1.0 - assert twist_cov.twist.linear.y == 2.0 - assert twist_cov.twist.linear.z == 3.0 - assert np.array_equal(twist_cov.covariance, np.arange(36)) - - -def test_twist_with_covariance_dict_init_no_covariance() -> None: - """Test initialization from dictionary without covariance.""" - twist_dict = {"twist": Twist(Vector3(1.0, 2.0, 3.0), Vector3(0.1, 0.2, 0.3))} - twist_cov = TwistWithCovariance(twist_dict) - - assert twist_cov.twist.linear.x == 1.0 - assert np.all(twist_cov.covariance == 0.0) - - -def test_twist_with_covariance_tuple_of_tuple_init() -> None: - """Test initialization from tuple of (twist_tuple, covariance).""" - twist_tuple = ([1.0, 2.0, 3.0], [0.1, 0.2, 0.3]) - covariance = np.arange(36, dtype=float) - twist_cov = TwistWithCovariance((twist_tuple, covariance)) - - assert twist_cov.twist.linear.x == 1.0 - assert twist_cov.twist.linear.y == 2.0 - assert twist_cov.twist.linear.z == 3.0 - assert twist_cov.twist.angular.x == 0.1 - assert twist_cov.twist.angular.y == 0.2 - assert twist_cov.twist.angular.z == 0.3 - assert np.array_equal(twist_cov.covariance, covariance) +"""Generated covariance values, with explicit NumPy matrix operations. +Legacy polymorphic positional constructors and presentation methods are retired. +""" -def test_twist_with_covariance_properties() -> None: - """Test convenience properties.""" - twist = Twist(Vector3(1.0, 2.0, 3.0), Vector3(0.1, 0.2, 0.3)) - twist_cov = TwistWithCovariance(twist) - - # Linear and angular properties - assert twist_cov.linear.x == 1.0 - assert twist_cov.linear.y == 2.0 - assert twist_cov.linear.z == 3.0 - assert twist_cov.angular.x == 0.1 - assert twist_cov.angular.y == 0.2 - assert twist_cov.angular.z == 0.3 - - -def test_twist_with_covariance_matrix_property() -> None: - """Test covariance matrix property.""" - twist = Twist() - covariance_array = np.arange(36, dtype=float) - twist_cov = TwistWithCovariance(twist, covariance_array) - - # Get as matrix - cov_matrix = twist_cov.covariance_matrix - assert cov_matrix.shape == (6, 6) - assert cov_matrix[0, 0] == 0.0 - assert cov_matrix[5, 5] == 35.0 - - # Set from matrix - new_matrix = np.eye(6) * 2.0 - twist_cov.covariance_matrix = new_matrix - assert np.array_equal(twist_cov.covariance[:6], [2.0, 0.0, 0.0, 0.0, 0.0, 0.0]) - - -def test_twist_with_covariance_repr() -> None: - """Test string representation.""" - twist = Twist(Vector3(1.234, 2.567, 3.891), Vector3(0.1, 0.2, 0.3)) - twist_cov = TwistWithCovariance(twist) - - repr_str = repr(twist_cov) - assert "TwistWithCovariance" in repr_str - assert "twist=" in repr_str - assert "covariance=" in repr_str - assert "36 elements" in repr_str - - -def test_twist_with_covariance_str() -> None: - """Test string formatting.""" - twist = Twist(Vector3(1.234, 2.567, 3.891), Vector3(0.1, 0.2, 0.3)) - covariance = np.eye(6).flatten() - twist_cov = TwistWithCovariance(twist, covariance) - - str_repr = str(twist_cov) - assert "TwistWithCovariance" in str_repr - assert "1.234" in str_repr - assert "2.567" in str_repr - assert "3.891" in str_repr - assert "cov_trace" in str_repr - assert "6.000" in str_repr # Trace of identity matrix is 6 - - -def test_twist_with_covariance_equality() -> None: - """Test equality comparison.""" - twist1 = Twist(Vector3(1.0, 2.0, 3.0), Vector3(0.1, 0.2, 0.3)) - cov1 = np.arange(36, dtype=float) - twist_cov1 = TwistWithCovariance(twist1, cov1) - - twist2 = Twist(Vector3(1.0, 2.0, 3.0), Vector3(0.1, 0.2, 0.3)) - cov2 = np.arange(36, dtype=float) - twist_cov2 = TwistWithCovariance(twist2, cov2) - - # Equal - assert twist_cov1 == twist_cov2 - - # Different twist - twist3 = Twist(Vector3(1.1, 2.0, 3.0), Vector3(0.1, 0.2, 0.3)) - twist_cov3 = TwistWithCovariance(twist3, cov1) - assert twist_cov1 != twist_cov3 - - # Different covariance - cov3 = np.arange(36, dtype=float) + 1 - twist_cov4 = TwistWithCovariance(twist1, cov3) - assert twist_cov1 != twist_cov4 - - # Different type - assert twist_cov1 != "not a twist" - assert twist_cov1 is not None - - -def test_twist_with_covariance_is_zero() -> None: - """Test is_zero method.""" - # Zero twist - twist_cov1 = TwistWithCovariance() - assert twist_cov1.is_zero() - assert not twist_cov1 # Boolean conversion - - # Non-zero twist - twist = Twist(Vector3(1.0, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)) - twist_cov2 = TwistWithCovariance(twist) - assert not twist_cov2.is_zero() - assert twist_cov2 # Boolean conversion - - -def test_twist_with_covariance_lcm_encode_decode() -> None: - """Test LCM encoding and decoding.""" - twist = Twist(Vector3(1.0, 2.0, 3.0), Vector3(0.1, 0.2, 0.3)) - covariance = np.arange(36, dtype=float) - source = TwistWithCovariance(twist, covariance) - - # Encode and decode - binary_msg = source.lcm_encode() - decoded = TwistWithCovariance.lcm_decode(binary_msg) - - # Should be equal - assert decoded == source - assert isinstance(decoded, TwistWithCovariance) - assert isinstance(decoded.twist, Twist) - assert isinstance(decoded.covariance, np.ndarray) - - -def test_twist_with_covariance_zero_covariance() -> None: - """Test with zero covariance matrix.""" - twist = Twist(Vector3(1.0, 2.0, 3.0), Vector3(0.1, 0.2, 0.3)) - twist_cov = TwistWithCovariance(twist) - - assert np.all(twist_cov.covariance == 0.0) - assert np.trace(twist_cov.covariance_matrix) == 0.0 - - -def test_twist_with_covariance_diagonal_covariance() -> None: - """Test with diagonal covariance matrix.""" - twist = Twist() - covariance = np.zeros(36) - # Set diagonal elements - for i in range(6): - covariance[i * 6 + i] = i + 1 - - twist_cov = TwistWithCovariance(twist, covariance) - - cov_matrix = twist_cov.covariance_matrix - assert np.trace(cov_matrix) == sum(range(1, 7)) # 1+2+3+4+5+6 = 21 - - # Check diagonal elements - for i in range(6): - assert cov_matrix[i, i] == i + 1 - - # Check off-diagonal elements are zero - for i in range(6): - for j in range(6): - if i != j: - assert cov_matrix[i, j] == 0.0 +from dimos_generated.geometry_msgs.msg import Twist, TwistWithCovariance, Vector3 +import numpy as np +import pytest +from rosbags.typesys import Stores, get_typestore + + +def test_default_fields_and_covariance() -> None: + source = TwistWithCovariance() + value = source.twist + np.testing.assert_array_equal( + [ + value.linear.x, + value.linear.y, + value.linear.z, + value.angular.x, + value.angular.y, + value.angular.z, + ], + [0, 0, 0, 0, 0, 0], + ) + assert np.asarray(source.covariance).shape == (36,) + np.testing.assert_array_equal(source.covariance, np.zeros(36)) + + +@pytest.mark.parametrize("as_list", [False, True]) +@pytest.mark.parametrize( + "covariance", + [np.zeros(36), np.arange(36, dtype=float), np.eye(6).ravel(), np.diag(np.arange(1, 7)).ravel()], +) +def test_explicit_construction_and_independent_cdr(as_list: bool, covariance: np.ndarray) -> None: + source = TwistWithCovariance( + twist=Twist(linear=Vector3(x=1, y=2, z=3), angular=Vector3(x=0.1, y=0.2, z=0.3)), + covariance=covariance.tolist() if as_list else covariance, + ) + decoded = TwistWithCovariance.decode(source.encode()) + independent = get_typestore(Stores.ROS2_JAZZY).deserialize_cdr( + source.encode(), TwistWithCovariance.msg_name + ) + for result in (source, decoded, independent): + value = result.twist + np.testing.assert_array_equal( + [ + value.linear.x, + value.linear.y, + value.linear.z, + value.angular.x, + value.angular.y, + value.angular.z, + ], + [1, 2, 3, 0.1, 0.2, 0.3], + ) + np.testing.assert_array_equal(result.covariance, covariance) + matrix = np.asarray(result.covariance).reshape(6, 6) + for row in range(6): + for col in range(6): + assert matrix[row, col] == covariance[row * 6 + col] + assert np.trace(matrix) == np.trace(covariance.reshape(6, 6)) + + +def test_copy_equality_and_independent_storage() -> None: + original = TwistWithCovariance( + twist=Twist(linear=Vector3(x=1, y=2, z=3), angular=Vector3(x=0.1, y=0.2, z=0.3)), + covariance=np.arange(36, dtype=float), + ) + copied = TwistWithCovariance.decode(original.encode()) + assert copied == original + assert copied is not original + assert copied.twist is not original.twist + assert copied.covariance is not original.covariance + original.covariance[0] = 999 + assert copied.covariance[0] == 0 + assert copied != original + assert copied != "not a message" + assert copied is not None + + +def test_matrix_assignment() -> None: + source = TwistWithCovariance(covariance=np.arange(36, dtype=float)) + matrix = np.asarray(source.covariance).reshape(6, 6) + assert matrix[0, 0] == 0 + assert matrix[5, 5] == 35 + source.covariance = (np.eye(6) * 2).ravel() + np.testing.assert_array_equal(list(source.covariance)[:6], [2, 0, 0, 0, 0, 0]) + assert np.trace(np.asarray(source.covariance).reshape(6, 6)) == 12 + + +@pytest.mark.parametrize("size", [0, 35, 37]) +def test_fixed_covariance_rejects_invalid_length(size: int) -> None: + with pytest.raises((ValueError, TypeError, RuntimeError)): + TwistWithCovariance(covariance=np.zeros(size)) @pytest.mark.parametrize( - "linear,angular", + "xyz,angular", [ - ([0.0, 0.0, 0.0], [0.0, 0.0, 0.0]), - ([1.0, 2.0, 3.0], [0.1, 0.2, 0.3]), - ([-1.0, -2.0, -3.0], [-0.1, -0.2, -0.3]), - ([100.0, -100.0, 0.0], [3.14, -3.14, 0.0]), + ((0, 0, 0), (0, 0, 0)), + ((1, 2, 3), (0.1, 0.2, 0.3)), + ((-1, -2, -3), (-0.1, -0.2, -0.3)), + ((100, -100, 0), (3.14, -3.14, 0)), ], ) -def test_twist_with_covariance_parametrized_velocities(linear, angular) -> None: - """Parametrized test for various velocity values.""" - twist = Twist(linear, angular) - twist_cov = TwistWithCovariance(twist) - - assert twist_cov.linear.x == linear[0] - assert twist_cov.linear.y == linear[1] - assert twist_cov.linear.z == linear[2] - assert twist_cov.angular.x == angular[0] - assert twist_cov.angular.y == angular[1] - assert twist_cov.angular.z == angular[2] +def test_parameterized_values( + xyz: tuple[float, float, float], angular: tuple[float, float, float] +) -> None: + source = TwistWithCovariance( + twist=Twist( + linear=Vector3(x=xyz[0], y=xyz[1], z=xyz[2]), + angular=Vector3(x=angular[0], y=angular[1], z=angular[2]), + ) + ) + assert (source.twist.linear.x, source.twist.linear.y, source.twist.linear.z) == xyz + assert (source.twist.angular.x, source.twist.angular.y, source.twist.angular.z) == angular diff --git a/dimos/msgs/geometry_msgs/test_TwistWithCovarianceStamped.py b/dimos/msgs/geometry_msgs/test_TwistWithCovarianceStamped.py index e5195fb24d8..da7c1382ba0 100644 --- a/dimos/msgs/geometry_msgs/test_TwistWithCovarianceStamped.py +++ b/dimos/msgs/geometry_msgs/test_TwistWithCovarianceStamped.py @@ -12,240 +12,113 @@ # See the License for the specific language governing permissions and # limitations under the License. -import time - -import numpy as np - -from dimos.msgs.geometry_msgs.Twist import Twist -from dimos.msgs.geometry_msgs.TwistWithCovariance import TwistWithCovariance -from dimos.msgs.geometry_msgs.TwistWithCovarianceStamped import TwistWithCovarianceStamped -from dimos.msgs.geometry_msgs.Vector3 import Vector3 - - -def test_twist_with_covariance_stamped_default_init() -> None: - """Test default initialization.""" - twist_cov_stamped = TwistWithCovarianceStamped() - - # Should have current timestamp - assert twist_cov_stamped.ts > 0 - assert twist_cov_stamped.frame_id == "" - - # Twist should be zero - assert twist_cov_stamped.twist.linear.x == 0.0 - assert twist_cov_stamped.twist.linear.y == 0.0 - assert twist_cov_stamped.twist.linear.z == 0.0 - assert twist_cov_stamped.twist.angular.x == 0.0 - assert twist_cov_stamped.twist.angular.y == 0.0 - assert twist_cov_stamped.twist.angular.z == 0.0 - - # Covariance should be all zeros - assert np.all(twist_cov_stamped.covariance == 0.0) - - -def test_twist_with_covariance_stamped_with_timestamp() -> None: - """Test initialization with specific timestamp.""" - ts = 1234567890.123456 - frame_id = "base_link" - twist_cov_stamped = TwistWithCovarianceStamped(ts=ts, frame_id=frame_id) - - assert twist_cov_stamped.ts == ts - assert twist_cov_stamped.frame_id == frame_id - - -def test_twist_with_covariance_stamped_with_twist() -> None: - """Test initialization with twist.""" - ts = 1234567890.123456 - frame_id = "odom" - twist = Twist(Vector3(1.0, 2.0, 3.0), Vector3(0.1, 0.2, 0.3)) - covariance = np.arange(36, dtype=float) +"""Nested generated Twist covariance with explicit stamps and array conversion. - twist_cov_stamped = TwistWithCovarianceStamped( - ts=ts, frame_id=frame_id, twist=twist, covariance=covariance - ) - - assert twist_cov_stamped.ts == ts - assert twist_cov_stamped.frame_id == frame_id - assert twist_cov_stamped.twist.linear.x == 1.0 - assert twist_cov_stamped.twist.linear.y == 2.0 - assert twist_cov_stamped.twist.linear.z == 3.0 - assert np.array_equal(twist_cov_stamped.covariance, covariance) - - -def test_twist_with_covariance_stamped_with_tuple() -> None: - """Test initialization with tuple of velocities.""" - ts = 1234567890.123456 - frame_id = "robot_base" - linear = [1.0, 2.0, 3.0] - angular = [0.1, 0.2, 0.3] - covariance = np.arange(36, dtype=float) - - twist_cov_stamped = TwistWithCovarianceStamped( - ts=ts, frame_id=frame_id, twist=(linear, angular), covariance=covariance - ) - - assert twist_cov_stamped.ts == ts - assert twist_cov_stamped.frame_id == frame_id - assert twist_cov_stamped.twist.linear.x == 1.0 - assert twist_cov_stamped.twist.angular.x == 0.1 - assert np.array_equal(twist_cov_stamped.covariance, covariance) - - -def test_twist_with_covariance_stamped_properties() -> None: - """Test convenience properties.""" - twist = Twist(Vector3(1.0, 2.0, 3.0), Vector3(0.1, 0.2, 0.3)) - covariance = np.eye(6).flatten() - twist_cov_stamped = TwistWithCovarianceStamped( - ts=1234567890.0, frame_id="cmd_vel", twist=twist, covariance=covariance - ) - - # Linear and angular properties - assert twist_cov_stamped.linear.x == 1.0 - assert twist_cov_stamped.linear.y == 2.0 - assert twist_cov_stamped.linear.z == 3.0 - assert twist_cov_stamped.angular.x == 0.1 - assert twist_cov_stamped.angular.y == 0.2 - assert twist_cov_stamped.angular.z == 0.3 - - # Covariance matrix - cov_matrix = twist_cov_stamped.covariance_matrix - assert cov_matrix.shape == (6, 6) - assert np.trace(cov_matrix) == 6.0 +Legacy inheritance, custom repr, and implicit wall-clock defaults are retired; +field values, covariance matrices, and time conversion remain checked explicitly. +""" +import pickle +import time -def test_twist_with_covariance_stamped_str() -> None: - """Test string representation.""" - twist = Twist(Vector3(1.234, 2.567, 3.891), Vector3(0.111, 0.222, 0.333)) - covariance = np.eye(6).flatten() * 2.0 - twist_cov_stamped = TwistWithCovarianceStamped( - ts=1234567890.0, frame_id="world", twist=twist, covariance=covariance +from dimos_generated.geometry_msgs.msg import ( + Twist, + TwistWithCovariance, + TwistWithCovarianceStamped, + Vector3, +) +from dimos_generated.std_msgs.msg import Header +import numpy as np +import pytest +from rosbags.typesys import Stores, get_typestore + +from dimos.msgs.time import header_now, time_from_nanoseconds, time_from_seconds, to_nanoseconds + + +def test_generated_defaults() -> None: + source = TwistWithCovarianceStamped() + assert source.header.frame_id == "" + assert to_nanoseconds(source.header.stamp) == 0 + assert isinstance(source.twist, TwistWithCovariance) + assert isinstance(source.twist.twist, Twist) + np.testing.assert_array_equal(source.twist.covariance, np.zeros(36)) + value = source.twist.twist + np.testing.assert_array_equal( + [ + value.linear.x, + value.linear.y, + value.linear.z, + value.angular.x, + value.angular.y, + value.angular.z, + ], + np.zeros(6), ) - str_repr = str(twist_cov_stamped) - assert "TwistWithCovarianceStamped" in str_repr - assert "1.234" in str_repr - assert "2.567" in str_repr - assert "3.891" in str_repr - assert "cov_trace" in str_repr - assert "12.000" in str_repr # Trace of 2*identity is 12 - - -def test_twist_with_covariance_stamped_lcm_encode_decode() -> None: - """Test LCM encoding and decoding.""" - ts = 1234567890.123456 - frame_id = "camera_link" - twist = Twist(Vector3(1.0, 2.0, 3.0), Vector3(0.1, 0.2, 0.3)) - covariance = np.arange(36, dtype=float) +@pytest.mark.parametrize("stamp", [0, 1234567890123456789]) +@pytest.mark.parametrize( + "covariance", + [np.zeros(36), np.eye(6).ravel(), np.arange(36, dtype=float), np.diag(np.arange(1, 7)).ravel()], +) +def test_fields_covariance_and_independent_cdr(stamp: int, covariance: np.ndarray) -> None: source = TwistWithCovarianceStamped( - ts=ts, frame_id=frame_id, twist=twist, covariance=covariance + header=Header(stamp=time_from_nanoseconds(stamp), frame_id="camera_link"), + twist=TwistWithCovariance( + twist=Twist(linear=Vector3(x=1, y=2, z=3), angular=Vector3(x=0.1, y=0.2, z=0.3)), + covariance=covariance, + ), ) - - # Encode and decode - binary_msg = source.lcm_encode() - decoded = TwistWithCovarianceStamped.lcm_decode(binary_msg) - - # Check timestamp (may lose some precision) - assert abs(decoded.ts - ts) < 1e-6 - assert decoded.frame_id == frame_id - - # Check twist - assert decoded.twist.linear.x == 1.0 - assert decoded.twist.linear.y == 2.0 - assert decoded.twist.linear.z == 3.0 - assert decoded.twist.angular.x == 0.1 - assert decoded.twist.angular.y == 0.2 - assert decoded.twist.angular.z == 0.3 - - # Check covariance - assert np.array_equal(decoded.covariance, covariance) - - -def test_twist_with_covariance_stamped_zero_timestamp() -> None: - """Test that an explicit zero timestamp is kept, and an omitted one stamps now.""" - assert TwistWithCovarianceStamped(ts=0.0).ts == 0.0 - - before = time.time() - assert before <= TwistWithCovarianceStamped().ts <= time.time() - - -def test_twist_with_covariance_stamped_inheritance() -> None: - """Test that it properly inherits from TwistWithCovariance and Timestamped.""" - twist = Twist(Vector3(1.0, 2.0, 3.0), Vector3(0.1, 0.2, 0.3)) - covariance = np.eye(6).flatten() - twist_cov_stamped = TwistWithCovarianceStamped( - ts=1234567890.0, frame_id="test", twist=twist, covariance=covariance + decoded = TwistWithCovarianceStamped.decode(source.encode()) + independent = get_typestore(Stores.ROS2_JAZZY).deserialize_cdr( + source.encode(), TwistWithCovarianceStamped.msg_name ) - - # Should be instance of parent classes - assert isinstance(twist_cov_stamped, TwistWithCovariance) - - # Should have Timestamped attributes - assert hasattr(twist_cov_stamped, "ts") - assert hasattr(twist_cov_stamped, "frame_id") - - # Should have TwistWithCovariance attributes - assert hasattr(twist_cov_stamped, "twist") - assert hasattr(twist_cov_stamped, "covariance") - - -def test_twist_with_covariance_stamped_is_zero() -> None: - """Test is_zero method inheritance.""" - # Zero twist - twist_cov_stamped1 = TwistWithCovarianceStamped() - assert twist_cov_stamped1.is_zero() - assert not twist_cov_stamped1 # Boolean conversion - - # Non-zero twist - twist = Twist(Vector3(0.5, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)) - twist_cov_stamped2 = TwistWithCovarianceStamped(twist=twist) - assert not twist_cov_stamped2.is_zero() - assert twist_cov_stamped2 # Boolean conversion - - -def test_twist_with_covariance_stamped_sec_nsec() -> None: - """Test the sec_nsec helper function.""" - from dimos.msgs.geometry_msgs.TwistWithCovarianceStamped import sec_nsec - - # Test integer seconds - s, ns = sec_nsec(1234567890.0) - assert s == 1234567890 - assert ns == 0 - - # Test fractional seconds - s, ns = sec_nsec(1234567890.123456789) - assert s == 1234567890 - assert abs(ns - 123456789) < 100 # Allow small rounding error - - # Test small fractional seconds - s, ns = sec_nsec(0.000000001) - assert s == 0 - assert ns == 1 - - # Test large timestamp - s, ns = sec_nsec(9999999999.999999999) - # Due to floating point precision, this might round to 10000000000 - assert s in [9999999999, 10000000000] - if s == 9999999999: - assert abs(ns - 999999999) < 10 - else: - assert ns == 0 - - -def test_twist_with_covariance_stamped_different_covariances() -> None: - """Test with different covariance patterns.""" - twist = Twist(Vector3(1.0, 0.0, 0.0), Vector3(0.0, 0.0, 0.5)) - - # Zero covariance - zero_cov = np.zeros(36) - twist_cov1 = TwistWithCovarianceStamped(twist=twist, covariance=zero_cov) - assert np.all(twist_cov1.covariance == 0.0) - - # Identity covariance - identity_cov = np.eye(6).flatten() - twist_cov2 = TwistWithCovarianceStamped(twist=twist, covariance=identity_cov) - assert np.trace(twist_cov2.covariance_matrix) == 6.0 - - # Full covariance - full_cov = np.random.rand(36) - twist_cov3 = TwistWithCovarianceStamped(twist=twist, covariance=full_cov) - assert np.array_equal(twist_cov3.covariance, full_cov) + for result in (decoded, independent): + assert result.header.frame_id == "camera_link" + assert result.header.stamp.sec * 1000000000 + result.header.stamp.nanosec == stamp + value = result.twist.twist + np.testing.assert_array_equal( + [ + value.linear.x, + value.linear.y, + value.linear.z, + value.angular.x, + value.angular.y, + value.angular.z, + ], + [1, 2, 3, 0.1, 0.2, 0.3], + ) + np.testing.assert_array_equal(result.twist.covariance, covariance) + matrix = np.asarray(result.twist.covariance).reshape(6, 6) + assert matrix.shape == (6, 6) + assert np.trace(matrix) == np.trace(covariance.reshape(6, 6)) + copied = pickle.loads(pickle.dumps(source)) + assert copied.encode() == source.encode() + copied.twist.covariance[0] = 999 + assert source.twist.covariance[0] == covariance[0] + + +def test_explicit_current_header() -> None: + before = time.time_ns() + source = TwistWithCovarianceStamped(header=header_now("test")) + assert before <= to_nanoseconds(source.header.stamp) <= time.time_ns() + assert source.header.frame_id == "test" + + +@pytest.mark.parametrize( + "seconds,sec,nanosec", + [ + (1234567890.0, 1234567890, 0), + (1234567890.123456789, 1234567890, 123456789), + (0.000000001, 0, 1), + ], +) +def test_explicit_seconds_conversion(seconds: float, sec: int, nanosec: int) -> None: + stamp = time_from_seconds(seconds) + assert stamp.sec == sec + assert abs(stamp.nanosec - nanosec) < 100 + + +def test_seconds_outside_ros_int32_range_rejected() -> None: + with pytest.raises((TypeError, ValueError, OverflowError)): + time_from_seconds(9999999999.999999999) diff --git a/dimos/msgs/geometry_msgs/test_Vector3.py b/dimos/msgs/geometry_msgs/test_Vector3.py index 099e35eb19e..05941ddea08 100644 --- a/dimos/msgs/geometry_msgs/test_Vector3.py +++ b/dimos/msgs/geometry_msgs/test_Vector3.py @@ -12,10 +12,17 @@ # See the License for the specific language governing permissions and # limitations under the License. +from dimos_generated.geometry_msgs.msg import Vector3 import numpy as np import pytest -from dimos.msgs.geometry_msgs.Vector3 import Vector3 +from dimos.msgs.geometry import ( + normalized_vector, + quaternion_euler, + quaternion_from_euler, + vector_array, + vector_from_array, +) def test_vector_default_init() -> None: @@ -24,55 +31,47 @@ def test_vector_default_init() -> None: assert v.x == 0.0 assert v.y == 0.0 assert v.z == 0.0 - assert len(v.data) == 3 - assert v.to_list() == [0.0, 0.0, 0.0] - assert v.is_zero() # Zero vector should be considered zero + assert len(vector_array(v)) == 3 + assert vector_array(v).tolist() == [0.0, 0.0, 0.0] + assert np.allclose(vector_array(v), 0) def test_vector_specific_init() -> None: """Test initialization with specific values and different input types.""" - - v1 = Vector3(1.0, 2.0) # 2D vector (now becomes 3D with z=0) + v1 = Vector3(x=1.0, y=2.0) assert v1.x == 1.0 assert v1.y == 2.0 assert v1.z == 0.0 - - v2 = Vector3(3.0, 4.0, 5.0) # 3D vector + v2 = Vector3(x=3.0, y=4.0, z=5.0) assert v2.x == 3.0 assert v2.y == 4.0 assert v2.z == 5.0 - - v3 = Vector3([6.0, 7.0, 8.0]) + v3 = vector_from_array([6.0, 7.0, 8.0]) assert v3.x == 6.0 assert v3.y == 7.0 assert v3.z == 8.0 - - v4 = Vector3((9.0, 10.0, 11.0)) + v4 = vector_from_array((9.0, 10.0, 11.0)) assert v4.x == 9.0 assert v4.y == 10.0 assert v4.z == 11.0 - - v5 = Vector3(np.array([12.0, 13.0, 14.0])) + v5 = vector_from_array(np.array([12.0, 13.0, 14.0])) assert v5.x == 12.0 assert v5.y == 13.0 assert v5.z == 14.0 - - original = Vector3([15.0, 16.0, 17.0]) - v6 = Vector3(original) + original = vector_from_array([15.0, 16.0, 17.0]) + v6 = Vector3.decode(original.encode()) assert v6.x == 15.0 assert v6.y == 16.0 assert v6.z == 17.0 - assert v6 is not original assert v6 == original def test_vector_addition() -> None: """Test vector addition.""" - v1 = Vector3(1.0, 2.0, 3.0) - v2 = Vector3(4.0, 5.0, 6.0) - - v_add = v1 + v2 + v1 = Vector3(x=1.0, y=2.0, z=3.0) + v2 = Vector3(x=4.0, y=5.0, z=6.0) + v_add = vector_from_array(vector_array(v1) + vector_array(v2)) assert v_add.x == 5.0 assert v_add.y == 7.0 assert v_add.z == 9.0 @@ -80,10 +79,9 @@ def test_vector_addition() -> None: def test_vector_subtraction() -> None: """Test vector subtraction.""" - v1 = Vector3(1.0, 2.0, 3.0) - v2 = Vector3(4.0, 5.0, 6.0) - - v_sub = v2 - v1 + v1 = Vector3(x=1.0, y=2.0, z=3.0) + v2 = Vector3(x=4.0, y=5.0, z=6.0) + v_sub = vector_from_array(vector_array(v2) - vector_array(v1)) assert v_sub.x == 3.0 assert v_sub.y == 3.0 assert v_sub.z == 3.0 @@ -91,15 +89,12 @@ def test_vector_subtraction() -> None: def test_vector_scalar_multiplication() -> None: """Test vector multiplication by a scalar.""" - v1 = Vector3(1.0, 2.0, 3.0) - - v_mul = v1 * 2.0 + v1 = Vector3(x=1.0, y=2.0, z=3.0) + v_mul = vector_from_array(vector_array(v1) * 2.0) assert v_mul.x == 2.0 assert v_mul.y == 4.0 assert v_mul.z == 6.0 - - # Test right multiplication - v_rmul = 2.0 * v1 + v_rmul = vector_from_array(2.0 * vector_array(v1)) assert v_rmul.x == 2.0 assert v_rmul.y == 4.0 assert v_rmul.z == 6.0 @@ -107,9 +102,8 @@ def test_vector_scalar_multiplication() -> None: def test_vector_scalar_division() -> None: """Test vector division by a scalar.""" - v2 = Vector3(4.0, 5.0, 6.0) - - v_div = v2 / 2.0 + v2 = Vector3(x=4.0, y=5.0, z=6.0) + v_div = vector_from_array(vector_array(v2) / 2.0) assert v_div.x == 2.0 assert v_div.y == 2.5 assert v_div.z == 3.0 @@ -117,67 +111,54 @@ def test_vector_scalar_division() -> None: def test_vector_dot_product() -> None: """Test vector dot product.""" - v1 = Vector3(1.0, 2.0, 3.0) - v2 = Vector3(4.0, 5.0, 6.0) - - dot = v1.dot(v2) + v1 = Vector3(x=1.0, y=2.0, z=3.0) + v2 = Vector3(x=4.0, y=5.0, z=6.0) + dot = np.dot(vector_array(v1), vector_array(v2)) assert dot == 32.0 def test_vector_length() -> None: """Test vector length calculation.""" - # 2D vector with length 5 (now 3D with z=0) - v1 = Vector3(3.0, 4.0) - assert v1.length() == 5.0 - - # 3D vector - v2 = Vector3(2.0, 3.0, 6.0) - assert v2.length() == pytest.approx(7.0, 0.001) - - # Test length_squared - assert v1.length_squared() == 25.0 - assert v2.length_squared() == 49.0 + v1 = Vector3(x=3.0, y=4.0) + assert np.linalg.norm(vector_array(v1)) == 5.0 + v2 = Vector3(x=2.0, y=3.0, z=6.0) + assert np.linalg.norm(vector_array(v2)) == pytest.approx(7.0, 0.001) + assert np.dot(vector_array(v1), vector_array(v1)) == 25.0 + assert np.dot(vector_array(v2), vector_array(v2)) == 49.0 def test_vector_normalize() -> None: """Test vector normalization.""" - v = Vector3(2.0, 3.0, 6.0) - assert not v.is_zero() - - v_norm = v.normalize() - length = v.length() + v = Vector3(x=2.0, y=3.0, z=6.0) + assert not np.allclose(vector_array(v), 0) + v_norm = normalized_vector(v) + length = np.linalg.norm(vector_array(v)) expected_x = 2.0 / length expected_y = 3.0 / length expected_z = 6.0 / length - assert np.isclose(v_norm.x, expected_x) assert np.isclose(v_norm.y, expected_y) assert np.isclose(v_norm.z, expected_z) - assert np.isclose(v_norm.length(), 1.0) - assert not v_norm.is_zero() - - # Test normalizing a zero vector - v_zero = Vector3(0.0, 0.0, 0.0) - assert v_zero.is_zero() - v_zero_norm = v_zero.normalize() + assert np.isclose(np.linalg.norm(vector_array(v_norm)), 1.0) + assert not np.allclose(vector_array(v_norm), 0) + v_zero = Vector3(x=0.0, y=0.0, z=0.0) + assert np.allclose(vector_array(v_zero), 0) + v_zero_norm = normalized_vector(v_zero) assert v_zero_norm.x == 0.0 assert v_zero_norm.y == 0.0 assert v_zero_norm.z == 0.0 - assert v_zero_norm.is_zero() + assert np.allclose(vector_array(v_zero_norm), 0) def test_vector_to_2d() -> None: """Test conversion to 2D vector.""" - v = Vector3(2.0, 3.0, 6.0) - - v_2d = v.to_2d() + v = Vector3(x=2.0, y=3.0, z=6.0) + v_2d = Vector3(x=v.x, y=v.y) assert v_2d.x == 2.0 assert v_2d.y == 3.0 - assert v_2d.z == 0.0 # z should be 0 for 2D conversion - - # Already 2D vector (z=0) - v2 = Vector3(4.0, 5.0) - v2_2d = v2.to_2d() + assert v_2d.z == 0.0 + v2 = Vector3(x=4.0, y=5.0) + v2_2d = Vector3(x=v2.x, y=v2.y) assert v2_2d.x == 4.0 assert v2_2d.y == 5.0 assert v2_2d.z == 0.0 @@ -185,46 +166,32 @@ def test_vector_to_2d() -> None: def test_vector_distance() -> None: """Test distance calculations between vectors.""" - v1 = Vector3(1.0, 2.0, 3.0) - v2 = Vector3(4.0, 6.0, 8.0) - - # Distance - dist = v1.distance(v2) - expected_dist = np.sqrt(9.0 + 16.0 + 25.0) # sqrt((4-1)² + (6-2)² + (8-3)²) + v1 = Vector3(x=1.0, y=2.0, z=3.0) + v2 = Vector3(x=4.0, y=6.0, z=8.0) + dist = np.linalg.norm(vector_array(v1) - vector_array(v2)) + expected_dist = np.sqrt(9.0 + 16.0 + 25.0) assert dist == pytest.approx(expected_dist) - - # Distance squared - dist_sq = v1.distance_squared(v2) - assert dist_sq == 50.0 # 9 + 16 + 25 + dist_sq = np.sum((vector_array(v1) - vector_array(v2)) ** 2) + assert dist_sq == 50.0 def test_vector_cross_product() -> None: """Test vector cross product.""" - v1 = Vector3(1.0, 0.0, 0.0) # Unit x vector - v2 = Vector3(0.0, 1.0, 0.0) # Unit y vector - - # v1 × v2 should be unit z vector - cross = v1.cross(v2) + v1 = Vector3(x=1.0, y=0.0, z=0.0) + v2 = Vector3(x=0.0, y=1.0, z=0.0) + cross = vector_from_array(np.cross(vector_array(v1), vector_array(v2))) assert cross.x == 0.0 assert cross.y == 0.0 assert cross.z == 1.0 - - # Test with more complex vectors - a = Vector3(2.0, 3.0, 4.0) - b = Vector3(5.0, 6.0, 7.0) - c = a.cross(b) - - # Cross product manually calculated: - # (3*7-4*6, 4*5-2*7, 2*6-3*5) + a = Vector3(x=2.0, y=3.0, z=4.0) + b = Vector3(x=5.0, y=6.0, z=7.0) + c = vector_from_array(np.cross(vector_array(a), vector_array(b))) assert c.x == -3.0 assert c.y == 6.0 assert c.z == -3.0 - - # Test with vectors that have z=0 (still works as they're 3D) - v_2d1 = Vector3(1.0, 2.0) # (1, 2, 0) - v_2d2 = Vector3(3.0, 4.0) # (3, 4, 0) - cross_2d = v_2d1.cross(v_2d2) - # (2*0-0*4, 0*3-1*0, 1*4-2*3) = (0, 0, -2) + v_2d1 = Vector3(x=1.0, y=2.0) + v_2d2 = Vector3(x=3.0, y=4.0) + cross_2d = vector_from_array(np.cross(vector_array(v_2d1), vector_array(v_2d2))) assert cross_2d.x == 0.0 assert cross_2d.y == 0.0 assert cross_2d.z == -2.0 @@ -232,18 +199,16 @@ def test_vector_cross_product() -> None: def test_vector_zeros() -> None: """Test Vector3.zeros class method.""" - # 3D zero vector - v_zeros = Vector3.zeros() + v_zeros = Vector3() assert v_zeros.x == 0.0 assert v_zeros.y == 0.0 assert v_zeros.z == 0.0 - assert v_zeros.is_zero() + assert np.allclose(vector_array(v_zeros), 0) def test_vector_ones() -> None: """Test Vector3.ones class method.""" - # 3D ones vector - v_ones = Vector3.ones() + v_ones = Vector3(x=1, y=1, z=1) assert v_ones.x == 1.0 assert v_ones.y == 1.0 assert v_ones.z == 1.0 @@ -251,198 +216,134 @@ def test_vector_ones() -> None: def test_vector_conversion_methods() -> None: """Test vector conversion methods (to_list, to_tuple, to_numpy).""" - v = Vector3(1.0, 2.0, 3.0) - - # to_list - assert v.to_list() == [1.0, 2.0, 3.0] - - # to_tuple - assert v.to_tuple() == (1.0, 2.0, 3.0) - - # to_numpy - np_array = v.to_numpy() + v = Vector3(x=1.0, y=2.0, z=3.0) + assert vector_array(v).tolist() == [1.0, 2.0, 3.0] + assert tuple(vector_array(v)) == (1.0, 2.0, 3.0) + np_array = vector_array(v) assert isinstance(np_array, np.ndarray) assert np.array_equal(np_array, np.array([1.0, 2.0, 3.0])) def test_vector_equality() -> None: """Test vector equality.""" - v1 = Vector3(1, 2, 3) - v2 = Vector3(1, 2, 3) - v3 = Vector3(4, 5, 6) - + v1 = Vector3(x=1, y=2, z=3) + v2 = Vector3(x=1, y=2, z=3) + v3 = Vector3(x=4, y=5, z=6) assert v1 == v2 assert v1 != v3 - assert v1 != Vector3(1, 2) # Now (1, 2, 0) vs (1, 2, 3) - assert v1 != Vector3(1.1, 2, 3) # Different values + assert v1 != Vector3(x=1, y=2) + assert v1 != Vector3(x=1.1, y=2, z=3) assert v1 != [1, 2, 3] def test_vector_is_zero() -> None: """Test is_zero method for vectors.""" - # Default zero vector v0 = Vector3() - assert v0.is_zero() - - # Explicit zero vector - v1 = Vector3(0.0, 0.0, 0.0) - assert v1.is_zero() - - # Zero vector with different initialization (now always 3D) - v2 = Vector3(0.0, 0.0) # Becomes (0, 0, 0) - assert v2.is_zero() - - # Non-zero vectors - v3 = Vector3(1.0, 0.0, 0.0) - assert not v3.is_zero() - - v4 = Vector3(0.0, 2.0, 0.0) - assert not v4.is_zero() - - v5 = Vector3(0.0, 0.0, 3.0) - assert not v5.is_zero() - - # Almost zero (within tolerance) - v6 = Vector3(1e-10, 1e-10, 1e-10) - assert v6.is_zero() - - # Almost zero (outside tolerance) - v7 = Vector3(1e-6, 1e-6, 1e-6) - assert not v7.is_zero() + assert np.allclose(vector_array(v0), 0) + v1 = Vector3(x=0.0, y=0.0, z=0.0) + assert np.allclose(vector_array(v1), 0) + v2 = Vector3(x=0.0, y=0.0) + assert np.allclose(vector_array(v2), 0) + v3 = Vector3(x=1.0, y=0.0, z=0.0) + assert not np.allclose(vector_array(v3), 0) + v4 = Vector3(x=0.0, y=2.0, z=0.0) + assert not np.allclose(vector_array(v4), 0) + v5 = Vector3(x=0.0, y=0.0, z=3.0) + assert not np.allclose(vector_array(v5), 0) + v6 = Vector3(x=1e-10, y=1e-10, z=1e-10) + assert np.allclose(vector_array(v6), 0) + v7 = Vector3(x=1e-06, y=1e-06, z=1e-06) + assert not np.allclose(vector_array(v7), 0) def test_vector_bool_conversion(): """Test boolean conversion of vectors.""" - # Zero vectors should be False v0 = Vector3() - assert not bool(v0) - - v1 = Vector3(0.0, 0.0, 0.0) - assert not bool(v1) - - # Almost zero vectors should be False - v2 = Vector3(1e-10, 1e-10, 1e-10) - assert not bool(v2) - - # Non-zero vectors should be True - v3 = Vector3(1.0, 0.0, 0.0) - assert bool(v3) - - v4 = Vector3(0.0, 2.0, 0.0) - assert bool(v4) - - v5 = Vector3(0.0, 0.0, 3.0) - assert bool(v5) - - # Direct use in if statements - if v0: + assert not not np.allclose(vector_array(v0), 0) + v1 = Vector3(x=0.0, y=0.0, z=0.0) + assert not not np.allclose(vector_array(v1), 0) + v2 = Vector3(x=1e-10, y=1e-10, z=1e-10) + assert not not np.allclose(vector_array(v2), 0) + v3 = Vector3(x=1.0, y=0.0, z=0.0) + assert not np.allclose(vector_array(v3), 0) + v4 = Vector3(x=0.0, y=2.0, z=0.0) + assert not np.allclose(vector_array(v4), 0) + v5 = Vector3(x=0.0, y=0.0, z=3.0) + assert not np.allclose(vector_array(v5), 0) + if not np.allclose(vector_array(v0), 0): raise AssertionError("Zero vector should be False in boolean context") else: - pass # Expected path - - if v3: - pass # Expected path + pass + if not np.allclose(vector_array(v3), 0): + pass else: raise AssertionError("Non-zero vector should be True in boolean context") def test_vector_add() -> None: """Test vector addition operator.""" - v1 = Vector3(1.0, 2.0, 3.0) - v2 = Vector3(4.0, 5.0, 6.0) - - # Using __add__ method - v_add = v1.__add__(v2) + v1 = Vector3(x=1.0, y=2.0, z=3.0) + v2 = Vector3(x=4.0, y=5.0, z=6.0) + v_add = vector_from_array(vector_array(v1) + vector_array(v2)) assert v_add.x == 5.0 assert v_add.y == 7.0 assert v_add.z == 9.0 - - # Using + operator - v_add_op = v1 + v2 + v_add_op = vector_from_array(vector_array(v1) + vector_array(v2)) assert v_add_op.x == 5.0 assert v_add_op.y == 7.0 assert v_add_op.z == 9.0 - - # Adding zero vector should return original vector - v_zero = Vector3.zeros() - assert (v1 + v_zero) == v1 + v_zero = Vector3() + assert vector_from_array(vector_array(v1) + vector_array(v_zero)) == v1 def test_vector_add_dim_mismatch() -> None: """Test vector addition with different input dimensions (now all vectors are 3D).""" - v1 = Vector3(1.0, 2.0) # Becomes (1, 2, 0) - v2 = Vector3(4.0, 5.0, 6.0) # (4, 5, 6) - - # Using + operator - should work fine now since both are 3D - v_add_op = v1 + v2 - assert v_add_op.x == 5.0 # 1 + 4 - assert v_add_op.y == 7.0 # 2 + 5 - assert v_add_op.z == 6.0 # 0 + 6 + v1 = Vector3(x=1.0, y=2.0) + v2 = Vector3(x=4.0, y=5.0, z=6.0) + v_add_op = vector_from_array(vector_array(v1) + vector_array(v2)) + assert v_add_op.x == 5.0 + assert v_add_op.y == 7.0 + assert v_add_op.z == 6.0 def test_yaw_pitch_roll_accessors() -> None: """Test yaw, pitch, and roll accessor properties.""" - # Test with a 3D vector - v = Vector3(1.0, 2.0, 3.0) - - # According to standard convention: - # roll = rotation around x-axis = x component - # pitch = rotation around y-axis = y component - # yaw = rotation around z-axis = z component - assert v.roll == 1.0 # Should return x component - assert v.pitch == 2.0 # Should return y component - assert v.yaw == 3.0 # Should return z component - - # Test with a 2D vector (z should be 0.0) - v_2d = Vector3(4.0, 5.0) - assert v_2d.roll == 4.0 # Should return x component - assert v_2d.pitch == 5.0 # Should return y component - assert v_2d.yaw == 0.0 # Should return z component (defaults to 0 for 2D) - - # Test with empty vector (all should be 0.0) + v = Vector3(x=1.0, y=2.0, z=3.0) + assert v.x == 1.0 + assert v.y == 2.0 + assert v.z == 3.0 + v_2d = Vector3(x=4.0, y=5.0) + assert v_2d.x == 4.0 + assert v_2d.y == 5.0 + assert v_2d.z == 0.0 v_empty = Vector3() - assert v_empty.roll == 0.0 - assert v_empty.pitch == 0.0 - assert v_empty.yaw == 0.0 - - # Test with negative values - v_neg = Vector3(-1.5, -2.5, -3.5) - assert v_neg.roll == -1.5 - assert v_neg.pitch == -2.5 - assert v_neg.yaw == -3.5 + assert v_empty.x == 0.0 + assert v_empty.y == 0.0 + assert v_empty.z == 0.0 + v_neg = Vector3(x=-1.5, y=-2.5, z=-3.5) + assert v_neg.x == -1.5 + assert v_neg.y == -2.5 + assert v_neg.z == -3.5 def test_vector_to_quaternion() -> None: """Test vector to quaternion conversion.""" - # Test with zero Euler angles (should produce identity quaternion) - v_zero = Vector3(0.0, 0.0, 0.0) - q_identity = v_zero.to_quaternion() - - # Identity quaternion should have w=1, x=y=z=0 + v_zero = Vector3(x=0.0, y=0.0, z=0.0) + q_identity = quaternion_from_euler(v_zero.x, v_zero.y, v_zero.z) assert np.isclose(q_identity.x, 0.0, atol=1e-10) assert np.isclose(q_identity.y, 0.0, atol=1e-10) assert np.isclose(q_identity.z, 0.0, atol=1e-10) assert np.isclose(q_identity.w, 1.0, atol=1e-10) - - # Test with small angles (to avoid gimbal lock issues) - v_small = Vector3(0.1, 0.2, 0.3) # Small roll, pitch, yaw - q_small = v_small.to_quaternion() - - # Quaternion should be normalized (magnitude = 1) + v_small = Vector3(x=0.1, y=0.2, z=0.3) + q_small = quaternion_from_euler(v_small.x, v_small.y, v_small.z) magnitude = np.sqrt(q_small.x**2 + q_small.y**2 + q_small.z**2 + q_small.w**2) assert np.isclose(magnitude, 1.0, atol=1e-10) - - # Test conversion back to Euler (should be close to original) - v_back = q_small.to_euler() - assert np.isclose(v_back.x, 0.1, atol=1e-6) - assert np.isclose(v_back.y, 0.2, atol=1e-6) - assert np.isclose(v_back.z, 0.3, atol=1e-6) - - # Test with π/2 rotation around x-axis - v_x_90 = Vector3(np.pi / 2, 0.0, 0.0) - q_x_90 = v_x_90.to_quaternion() - - # Should be approximately (sin(π/4), 0, 0, cos(π/4)) = (√2/2, 0, 0, √2/2) + v_back = vector_from_array(quaternion_euler(q_small)) + assert np.isclose(v_back.x, 0.1, atol=1e-06) + assert np.isclose(v_back.y, 0.2, atol=1e-06) + assert np.isclose(v_back.z, 0.3, atol=1e-06) + v_x_90 = Vector3(x=np.pi / 2, y=0.0, z=0.0) + q_x_90 = quaternion_from_euler(v_x_90.x, v_x_90.y, v_x_90.z) expected = np.sqrt(2) / 2 assert np.isclose(q_x_90.x, expected, atol=1e-10) assert np.isclose(q_x_90.y, 0.0, atol=1e-10) @@ -450,13 +351,10 @@ def test_vector_to_quaternion() -> None: assert np.isclose(q_x_90.w, expected, atol=1e-10) -def test_lcm_encode_decode() -> None: - v_source = Vector3(1.0, 2.0, 3.0) - - binary_msg = v_source.lcm_encode() - - v_dest = Vector3.lcm_decode(binary_msg) - +def test_cdr_encode_decode() -> None: + v_source = Vector3(x=1.0, y=2.0, z=3.0) + binary_msg = v_source.encode() + v_dest = Vector3.decode(binary_msg) assert isinstance(v_dest, Vector3) assert v_dest is not v_source assert v_dest == v_source diff --git a/dimos/msgs/geometry_msgs/test_WrenchStamped.py b/dimos/msgs/geometry_msgs/test_WrenchStamped.py index e493b1cd396..2c3b932f12f 100644 --- a/dimos/msgs/geometry_msgs/test_WrenchStamped.py +++ b/dimos/msgs/geometry_msgs/test_WrenchStamped.py @@ -12,129 +12,109 @@ # See the License for the specific language governing permissions and # limitations under the License. +"""Generated force/torque values and explicit external numeric operations.""" + import pickle import time +from dimos_generated.geometry_msgs.msg import Vector3, Wrench, WrenchStamped +from dimos_generated.std_msgs.msg import Header import numpy as np import pytest +from rosbags.typesys import Stores, get_typestore -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.msgs.geometry_msgs.Wrench import Wrench -from dimos.msgs.geometry_msgs.WrenchStamped import WrenchStamped - - -def test_wrench_empty_is_zero(): - w = Wrench() - assert w.force.to_tuple() == (0.0, 0.0, 0.0) - assert w.torque.to_tuple() == (0.0, 0.0, 0.0) - assert w.is_zero() - assert not w - - -def test_wrench_from_two_sequences(): - w = Wrench([1, 2, 3], [4, 5, 6]) - assert w.force.to_tuple() == (1.0, 2.0, 3.0) - assert w.torque.to_tuple() == (4.0, 5.0, 6.0) - +from dimos.msgs.geometry import wrench_array, wrench_from_array +from dimos.msgs.time import header_now, time_from_seconds, to_nanoseconds -def test_wrench_from_two_vector3(): - w = Wrench(Vector3(1, 2, 3), Vector3(4, 5, 6)) - assert w.force.to_tuple() == (1.0, 2.0, 3.0) - assert w.torque.to_tuple() == (4.0, 5.0, 6.0) +def test_wrench_empty_is_zero() -> None: + np.testing.assert_array_equal(wrench_array(Wrench()), np.zeros(6)) -def test_wrench_keywords(): - assert Wrench(force=[1, 2, 3]).torque.to_tuple() == (0.0, 0.0, 0.0) - assert Wrench(torque=[4, 5, 6]).force.to_tuple() == (0.0, 0.0, 0.0) +def test_wrench_from_array_and_vectors() -> None: + source = wrench_from_array([1, 2, 3, 4, 5, 6]) + explicit = Wrench(force=Vector3(x=1, y=2, z=3), torque=Vector3(x=4, y=5, z=6)) + assert source == explicit + np.testing.assert_array_equal(wrench_array(source), [1, 2, 3, 4, 5, 6]) -def test_wrench_copy_constructor(): - src = Wrench([1, 2, 3], [4, 5, 6]) - copy = Wrench(src) - assert copy == src - assert copy.force is not src.force - -def test_wrench_unknown_keyword_raises(): +def test_wrench_keywords() -> None: + assert Wrench(force=Vector3(x=1, y=2, z=3)).torque == Vector3() + assert Wrench(torque=Vector3(x=4, y=5, z=6)).force == Vector3() with pytest.raises(TypeError): Wrench(bogus=1) -def test_wrench_array_roundtrip(): - ft = [1.0, 2.0, 3.0, 0.1, 0.2, 0.3] - assert np.allclose(Wrench.from_array(ft).to_array(), ft) - - -def test_wrench_from_array_wrong_length_raises(): - with pytest.raises(ValueError, match="6 elements"): - Wrench.from_array([1.0, 2.0, 3.0]) - - -def test_wrench_add_and_sub(): - a = Wrench([1, 2, 3], [4, 5, 6]) - b = Wrench([1, 1, 1], [1, 1, 1]) - assert (a + b).force.to_tuple() == (2.0, 3.0, 4.0) - assert (a - b).torque.to_tuple() == (3.0, 4.0, 5.0) +def test_wrench_copy_storage() -> None: + source = wrench_from_array([1, 2, 3, 4, 5, 6]) + copied = Wrench.decode(source.encode()) + assert copied == source + copied.force.x = 10 + assert source.force.x == 1 + array = wrench_array(source) + array[0] = 20 + assert source.force.x == 1 -def test_stamped_is_a_wrench(): - assert isinstance(WrenchStamped(), Wrench) +def test_wrench_array_roundtrip() -> None: + values = [1, 2, 3, 0.1, 0.2, 0.3] + np.testing.assert_allclose(wrench_array(wrench_from_array(values)), values) -def test_stamped_inherits_wrench_positional_form(): - ws = WrenchStamped([1, 2, 3], [4, 5, 6]) - assert ws.force.to_tuple() == (1.0, 2.0, 3.0) - assert ws.torque.to_tuple() == (4.0, 5.0, 6.0) - - -def test_stamped_keeps_explicit_ts_and_frame(): - ws = WrenchStamped(ts=5.0, frame_id="ft_sensor", force=[1, 2, 3], torque=[4, 5, 6]) - assert (ws.ts, ws.frame_id) == (5.0, "ft_sensor") - assert ws.force.to_tuple() == (1.0, 2.0, 3.0) - - -def test_stamped_positional_ts_and_frame(): - ws = WrenchStamped(5.0, "ft_sensor") - assert (ws.ts, ws.frame_id) == (5.0, "ft_sensor") +@pytest.mark.parametrize("values", [[1, 2, 3], [], [[1, 2, 3], [4, 5, 6]]]) +def test_wrench_from_array_wrong_shape_raises(values) -> None: + with pytest.raises(ValueError, match="6 elements"): + wrench_from_array(values) -def test_stamped_defaults_stamp_to_now(): - before = time.time() - assert before <= WrenchStamped().ts <= time.time() +def test_wrench_add_and_sub() -> None: + a = wrench_from_array([1, 2, 3, 4, 5, 6]) + b = wrench_from_array([1, 1, 1, 1, 1, 1]) + added = wrench_from_array(wrench_array(a) + wrench_array(b)) + subtracted = wrench_from_array(wrench_array(a) - wrench_array(b)) + assert (added.force.x, added.force.y, added.force.z) == (2, 3, 4) + assert (subtracted.torque.x, subtracted.torque.y, subtracted.torque.z) == (3, 4, 5) -def test_stamped_zero_ts_is_a_real_timestamp(): - assert WrenchStamped(ts=0.0).ts == 0.0 +def test_stamped_nested_wrench_and_explicit_header() -> None: + source = WrenchStamped( + header=Header(stamp=time_from_seconds(5), frame_id="tool0"), + wrench=wrench_from_array([1, 2, 3, 0.1, 0.2, 0.3]), + ) + assert isinstance(source.wrench, Wrench) + assert to_nanoseconds(source.header.stamp) == 5000000000 + assert source.header.frame_id == "tool0" + np.testing.assert_array_equal(wrench_array(source.wrench), [1, 2, 3, 0.1, 0.2, 0.3]) -def test_stamped_from_array(): - ws = WrenchStamped.from_array([1.0, 2.0, 3.0, 0.1, 0.2, 0.3], frame_id="tool0", ts=5.0) - assert (ws.ts, ws.frame_id) == (5.0, "tool0") - assert ws.force.to_tuple() == (1.0, 2.0, 3.0) - assert ws.torque.to_tuple() == (0.1, 0.2, 0.3) +def test_explicit_current_time_and_zero_default() -> None: + before = time.time_ns() + source = WrenchStamped(header=header_now()) + assert before <= to_nanoseconds(source.header.stamp) <= time.time_ns() + assert to_nanoseconds(WrenchStamped().header.stamp) == 0 -def test_lcm_encode_decode(): +@pytest.mark.parametrize("stamp", [0, 5.25]) +def test_cdr_and_independent_decoding(stamp: float) -> None: source = WrenchStamped( - ts=5.25, frame_id="ft_sensor", force=(1.0, 2.0, 3.0), torque=(0.1, 0.2, 0.3) + header=Header(stamp=time_from_seconds(stamp), frame_id="ft_sensor"), + wrench=wrench_from_array([1, 2, 3, 0.1, 0.2, 0.3]), ) - dest = WrenchStamped.lcm_decode(source.lcm_encode()) - - assert isinstance(dest, WrenchStamped) - assert dest is not source - assert dest == source - assert dest.frame_id == "ft_sensor" - assert dest.ts == pytest.approx(5.25) - - -def test_lcm_decode_of_an_unstamped_message_keeps_the_zero_stamp(): - decoded = WrenchStamped.lcm_decode(WrenchStamped(ts=0.0, frame_id="f").lcm_encode()) - assert decoded.ts == 0.0 + decoded = WrenchStamped.decode(source.encode()) + assert decoded is not source + assert decoded == source + independent = get_typestore(Stores.ROS2_JAZZY).deserialize_cdr( + source.encode(), WrenchStamped.msg_name + ) + assert independent.header.frame_id == "ft_sensor" + assert independent.header.stamp.sec * 1000000000 + independent.header.stamp.nanosec == int( + stamp * 1000000000 + ) + np.testing.assert_array_equal(wrench_array(independent.wrench), [1, 2, 3, 0.1, 0.2, 0.3]) -def test_pickle_encode_decode(): - source = WrenchStamped(ts=time.time(), force=(1.0, 2.0, 3.0), torque=(0.1, 0.2, 0.3)) - dest = pickle.loads(pickle.dumps(source)) - assert isinstance(dest, WrenchStamped) - assert dest is not source - assert dest == source +def test_pickle_encode_decode() -> None: + source = WrenchStamped(header=header_now(), wrench=wrench_from_array([1, 2, 3, 0.1, 0.2, 0.3])) + decoded = pickle.loads(pickle.dumps(source)) + assert decoded is not source + assert decoded == source diff --git a/dimos/msgs/geometry_msgs/tool_publish.py b/dimos/msgs/geometry_msgs/tool_publish.py index a6276c19e56..a428513e86d 100644 --- a/dimos/msgs/geometry_msgs/tool_publish.py +++ b/dimos/msgs/geometry_msgs/tool_publish.py @@ -14,16 +14,15 @@ import time +from dimos_generated.geometry_msgs.msg import Vector3 import lcm -from dimos.msgs.geometry_msgs.Vector3 import Vector3 - def test_runpublish() -> None: for i in range(10): - msg = Vector3(-5 + i, -5 + i, i) + msg = Vector3(x=-5 + i, y=-5 + i, z=i) lc = lcm.LCM() - lc.publish("thing1_vector3#geometry_msgs.Vector3", msg.encode()) + lc.publish("thing1_vector3#geometry_msgs/msg/Vector3", msg.encode()) time.sleep(0.1) print(f"Published: {msg}") @@ -31,11 +30,10 @@ def test_runpublish() -> None: def test_receive() -> None: lc = lcm.LCM() - def receive(bla, msg) -> None: - # print("receive", bla, msg) + def receive(channel: str, msg: bytes) -> None: print(Vector3.decode(msg)) - lc.subscribe("thing1_vector3#geometry_msgs.Vector3", receive) + lc.subscribe("thing1_vector3#geometry_msgs/msg/Vector3", receive) def _loop() -> None: while True: diff --git a/dimos/msgs/helpers.py b/dimos/msgs/helpers.py index 59f9bbf0589..278092a47fa 100644 --- a/dimos/msgs/helpers.py +++ b/dimos/msgs/helpers.py @@ -15,50 +15,15 @@ from __future__ import annotations from functools import lru_cache -import importlib -from typing import TYPE_CHECKING, Any +from typing import TYPE_CHECKING, cast + +from dimos.message_codegen.registry import message_types if TYPE_CHECKING: from dimos.msgs.protocol import DimosMsg -def lcm_msg_type(msg_name: str) -> type[Any]: - """The generated dimos_lcm class of a '.' name. ImportError - when the wheel has no such message.""" - package, name = msg_name.split(".") - lcm_type: type[Any] = getattr(importlib.import_module(f"dimos_lcm.{package}.{name}"), name) - return lcm_type - - @lru_cache(maxsize=256) def resolve_msg_type(type_name: str) -> type[DimosMsg] | None: - """Resolve a message type name to its class. - - Args: - type_name: Type name in format "module.ClassName" (e.g., "geometry_msgs.Vector3") - - Returns: - The message class or None if not found. - """ - try: - module_name, class_name = type_name.rsplit(".", 1) - except ValueError: - return None - - # Try different import paths - # First try the direct submodule path (e.g., dimos.msgs.geometry_msgs.Quaternion) - # then fall back to parent package (for dimos_lcm or other packages) - import_paths = [ - f"dimos.msgs.{module_name}.{class_name}", - f"dimos.msgs.{module_name}", - f"dimos_lcm.{module_name}", - ] - - for path in import_paths: - try: - module = importlib.import_module(path) - return getattr(module, class_name) # type: ignore[no-any-return] - except (ImportError, AttributeError): - continue - - return None + """Resolve a qualified package/msg/Type through installed schema providers.""" + return cast("type[DimosMsg] | None", message_types().get(type_name)) diff --git a/dimos/msgs/image.py b/dimos/msgs/image.py new file mode 100644 index 00000000000..8b3167ef29e --- /dev/null +++ b/dimos/msgs/image.py @@ -0,0 +1,317 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Array views and compression for generated ROS image messages.""" + +from __future__ import annotations + +from pathlib import Path +from typing import Any + +from dimos_generated.sensor_msgs.msg import CompressedImage, Image +from dimos_generated.std_msgs.msg import Header +import numpy as np +from numpy.typing import NDArray +from PIL import Image as PILImage + +_FORMATS = { + "rgb8": ("u1", 3), + "bgr8": ("u1", 3), + "rgba8": ("u1", 4), + "bgra8": ("u1", 4), + "mono8": ("u1", 1), + "mono16": ("u2", 1), + "16UC1": ("u2", 1), + "32FC1": ("f4", 1), + "64FC1": ("f8", 1), + "16SC1": ("i2", 1), + "32FC3": ("f4", 3), +} + + +def image_from_array(pixels: NDArray[Any], *, encoding: str, header: Header | None = None) -> Image: + """Copy an array into a generated image with an explicit pixel encoding.""" + if encoding not in _FORMATS: + raise ValueError(f"unsupported image encoding {encoding!r}") + kind, channels = _FORMATS[encoding] + expected = np.dtype(kind) + if pixels.dtype.kind != expected.kind or pixels.dtype.itemsize != expected.itemsize: + raise ValueError(f"array dtype does not match {encoding}") + if (channels == 1 and pixels.ndim != 2) or ( + channels != 1 and (pixels.ndim != 3 or pixels.shape[2] != channels) + ): + raise ValueError(f"array shape does not match {encoding}") + contiguous = np.ascontiguousarray(pixels) + return Image( + header=header if header is not None else Header(), + width=pixels.shape[1], + height=pixels.shape[0], + encoding=encoding, + is_bigendian=int(not pixels.dtype.isnative if np.little_endian else pixels.dtype.isnative), + step=pixels.shape[1] * channels * pixels.dtype.itemsize, + data=contiguous.view(np.uint8).ravel(), + ) + + +def image_view(msg: Image) -> NDArray[Any]: + """Borrow a read-only array, retaining the message and respecting row padding/endian.""" + if msg.encoding not in _FORMATS: + raise ValueError(f"unsupported image encoding {msg.encoding!r}") + kind, channels = _FORMATS[msg.encoding] + dtype = np.dtype((">" if msg.is_bigendian else "<") + kind) + row_bytes = msg.width * channels * dtype.itemsize + if msg.step < row_bytes or len(msg.data) != msg.height * msg.step: + raise ValueError("image data length/step does not match its dimensions") + shape = (msg.height, msg.width) if channels == 1 else (msg.height, msg.width, channels) + strides = ( + (msg.step, dtype.itemsize) + if channels == 1 + else (msg.step, channels * dtype.itemsize, dtype.itemsize) + ) + return np.ndarray(shape, dtype=dtype, buffer=msg.data.view(), strides=strides) + + +def image_to_jpeg(msg: Image, quality: int = 75) -> bytes: + """Encode an 8-bit gray or color image using OpenCV's JPEG encoder.""" + import cv2 + + if isinstance(quality, bool) or not 0 <= quality <= 100: + raise ValueError("JPEG quality must be in 0..100") + pixels = image_view(msg) + conversions = { + "rgb8": cv2.COLOR_RGB2BGR, + "rgba8": cv2.COLOR_RGBA2BGR, + "bgra8": cv2.COLOR_BGRA2BGR, + } + if msg.encoding in conversions: + pixels = cv2.cvtColor(pixels, conversions[msg.encoding]) + elif msg.encoding not in ("bgr8", "mono8"): + raise ValueError(f"JPEG requires 8-bit color or gray, got {msg.encoding!r}") + ok, encoded = cv2.imencode(".jpg", pixels, [cv2.IMWRITE_JPEG_QUALITY, quality]) + if not ok: + raise ValueError("JPEG encoding failed") + return bytes(encoded) + + +def compressed_image_from_image( + message: Image, + quality: int = 75, + *, + format: str = "jpeg", + max_width: int | None = None, + effort: int | None = None, +) -> CompressedImage: + """Compress generated pixels, retaining the exact header. + + PNG preserves uint16 depth. JXL preserves uint16/float32 depth losslessly; + it requires imagecodecs and is an explicit codec extension for consumers. + """ + if not isinstance(message, Image): + raise TypeError("compression expects a generated Image") + if max_width is not None: + message, _ = image_resize_to_fit(message, max_width, max_width) + if format == "jpeg": + output_encoding = "mono8" if message.encoding == "mono8" else "bgr8" + data = image_to_jpeg(message, quality=quality) + elif format == "png": + import cv2 + + pixels = image_view(message) + if pixels.dtype.kind != "u" or pixels.dtype.itemsize not in (1, 2): + raise ValueError("PNG cannot encode floating-point depth") + if message.encoding in ("rgb8", "rgba8"): + code = cv2.COLOR_RGB2BGR if message.encoding == "rgb8" else cv2.COLOR_RGBA2BGRA + pixels = cv2.cvtColor(pixels, code) + pixels = np.ascontiguousarray(pixels, dtype=pixels.dtype.newbyteorder("=")) + ok, encoded = cv2.imencode(".png", pixels) + if not ok: + raise ValueError("PNG encoding failed") + data = bytes(encoded) + output_encoding = {"rgb8": "bgr8", "rgba8": "bgra8"}.get(message.encoding, message.encoding) + elif format == "jxl": + import imagecodecs + + pixels = image_view(message) + if pixels.dtype.kind not in ("u", "f") or ( + pixels.dtype.kind, + pixels.dtype.itemsize, + ) not in (("u", 1), ("u", 2), ("f", 4)): + raise ValueError(f"JXL cannot encode dtype {pixels.dtype}") + if message.encoding == "bgr8": + pixels = pixels[..., ::-1] + elif message.encoding == "bgra8": + pixels = pixels[..., [2, 1, 0, 3]] + pixels = np.ascontiguousarray(pixels, dtype=pixels.dtype.newbyteorder("=")) + if pixels.dtype == np.uint8: + data = bytes(imagecodecs.jpegxl_encode(pixels, level=quality, effort=effort or 3)) + else: + data = bytes(imagecodecs.jpegxl_encode(pixels, lossless=True, effort=effort or 1)) + output_encoding = {"bgr8": "rgb8", "bgra8": "rgba8"}.get(message.encoding, message.encoding) + else: + raise ValueError(f"unsupported compression format {format!r}") + return CompressedImage( + header=message.header, + format=f"{message.encoding}; {format} compressed {output_encoding}", + data=data, + ) + + +def image_from_compressed(message: CompressedImage) -> Image: + """Decode JPEG/PNG or the JXL extension into explicitly encoded generated pixels.""" + import cv2 + + format_name = message.format.lower() + if "jxl" in format_name: + import imagecodecs + + jxl_pixels = imagecodecs.jpegxl_decode(bytes(message.data)) + if jxl_pixels.ndim == 2: + encoding = { + np.dtype("float32"): "32FC1", + np.dtype("uint16"): "mono16", + np.dtype("uint8"): "mono8", + }.get(jxl_pixels.dtype) + elif jxl_pixels.ndim == 3 and jxl_pixels.shape[2] in (3, 4): + encoding = "rgb8" if jxl_pixels.shape[2] == 3 else "rgba8" + else: + encoding = None + if encoding is None: + raise ValueError("unsupported decoded JXL layout") + if encoding == "mono16" and format_name.startswith("16uc1;"): + encoding = "16UC1" + return image_from_array(jxl_pixels, encoding=encoding, header=message.header) + if not any(name in format_name for name in ("jpeg", "jpg", "png")): + raise ValueError(f"unsupported compressed image format {message.format!r}") + pixels = cv2.imdecode(np.frombuffer(bytes(message.data), dtype=np.uint8), cv2.IMREAD_UNCHANGED) + if pixels is None: + raise ValueError("invalid compressed image data") + if pixels.ndim == 2: + encoding = "mono16" if pixels.dtype.itemsize == 2 else "mono8" + elif pixels.ndim == 3 and pixels.shape[2] == 3: + encoding = "bgr8" + elif pixels.ndim == 3 and pixels.shape[2] == 4: + encoding = "bgra8" + else: + raise ValueError("unsupported decoded image layout") + if encoding == "mono16" and format_name.startswith("16uc1;"): + encoding = "16UC1" + return image_from_array(pixels, encoding=encoding, header=message.header) + + +def image_brightness(message: Image) -> float: + """Sample mean pixel intensity in [0, 1] for 8/16-bit image encodings. + + Respect row padding and endian. Floating-point depth has no normalized + intensity scale and is rejected rather than treated as a color image. + """ + pixels = image_view(message) + if pixels.dtype.kind != "u": + raise ValueError("brightness requires unsigned integer pixels") + stride = max(1, max(message.height, message.width) // 256) + return float(pixels[::stride, ::stride].mean() / np.iinfo(pixels.dtype).max) + + +def image_sharpness(message: Image) -> float: + """Laplacian variance of an 8-bit visual image, downsampled to 160 pixels wide.""" + import cv2 + + pixels = image_view(message) + codes = { + "rgb8": cv2.COLOR_RGB2GRAY, + "bgr8": cv2.COLOR_BGR2GRAY, + "rgba8": cv2.COLOR_RGBA2GRAY, + "bgra8": cv2.COLOR_BGRA2GRAY, + } + if message.encoding == "mono8": + gray = pixels + elif message.encoding in codes: + gray = cv2.cvtColor(pixels, codes[message.encoding]) + else: + raise ValueError("Sharpness requires an 8-bit visual image encoding") + if gray.size == 0: + raise ValueError("Sharpness requires a nonempty image") + if message.width > 160: + gray = cv2.resize( + gray, + (160, max(1, round(message.height * 160 / message.width))), + interpolation=cv2.INTER_AREA, + ) + return float(cv2.Laplacian(gray, cv2.CV_64F).var()) + + +def image_to_bgr(message: Image) -> NDArray[np.uint8]: + """Return an independent BGR8 array for drawing or OpenCV color operations.""" + import cv2 + + pixels = image_view(message) + if message.encoding == "bgr8": + return pixels.copy() + codes = { + "rgb8": cv2.COLOR_RGB2BGR, + "rgba8": cv2.COLOR_RGBA2BGR, + "bgra8": cv2.COLOR_BGRA2BGR, + "mono8": cv2.COLOR_GRAY2BGR, + } + if message.encoding not in codes: + raise ValueError(f"Cannot convert {message.encoding!r} to BGR8") + return np.asarray(cv2.cvtColor(pixels, codes[message.encoding]), dtype=np.uint8) + + +def image_to_rgb(message: Image) -> NDArray[np.uint8]: + """Copy a display image as RGB8; 16-bit grayscale uses its high byte. + + Alpha is discarded. Floating-point depth needs an explicit visualization + scale and is rejected rather than assigning pixel values to meters. + """ + import cv2 + + pixels = image_view(message) + if message.encoding == "rgb8": + return pixels.copy() + if message.encoding in ("mono16", "16UC1"): + return np.asarray( + cv2.cvtColor((pixels / 256).astype(np.uint8), cv2.COLOR_GRAY2RGB), dtype=np.uint8 + ) + codes = { + "bgr8": cv2.COLOR_BGR2RGB, + "rgba8": cv2.COLOR_RGBA2RGB, + "bgra8": cv2.COLOR_BGRA2RGB, + "mono8": cv2.COLOR_GRAY2RGB, + } + if message.encoding not in codes: + raise ValueError(f"Cannot convert {message.encoding!r} to RGB8") + return np.asarray(cv2.cvtColor(pixels, codes[message.encoding]), dtype=np.uint8) + + +def image_resize_to_fit(message: Image, max_width: int, max_height: int) -> tuple[Image, float]: + """Downscale a generated image while preserving encoding, aspect and exact header.""" + import cv2 + + if min(max_width, max_height, message.width, message.height) <= 0: + raise ValueError("Image and target dimensions must be positive") + if message.width <= max_width and message.height <= max_height: + return message, 1.0 + scale = min(max_width / message.width, max_height / message.height) + pixels = cv2.resize( + image_view(message), + (max(1, int(message.width * scale)), max(1, int(message.height * scale))), + interpolation=cv2.INTER_LINEAR, + ) + return image_from_array(pixels, encoding=message.encoding, header=message.header), scale + + +def image_from_file(path: str | Path, *, header: Header | None = None) -> Image: + """Decode an image file as generated RGB8 with an explicit optional source header.""" + with PILImage.open(path) as image: + return image_from_array(np.asarray(image.convert("RGB")), encoding="rgb8", header=header) diff --git a/dimos/msgs/manipulation_msgs/GraspCandidate.py b/dimos/msgs/manipulation_msgs/GraspCandidate.py deleted file mode 100644 index f83db6016f4..00000000000 --- a/dimos/msgs/manipulation_msgs/GraspCandidate.py +++ /dev/null @@ -1,40 +0,0 @@ -# Copyright 2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from __future__ import annotations - -import math -import pickle - -from dimos.msgs.geometry_msgs.Pose import Pose - - -class GraspCandidate: - """A robot TCP pose and its generator-local ranking score.""" - - msg_name = "manipulation_msgs.GraspCandidate" - - def __init__(self, pose: Pose | None = None, score: float = 0.0) -> None: - self.pose = pose if pose is not None else Pose(0.0, 0.0, 0.0) - self.score = float(score) - if not math.isfinite(self.score): - raise ValueError("GraspCandidate.score must be finite") - - def encode(self) -> bytes: - return pickle.dumps({"pose": self.pose, "score": self.score}) - - @classmethod - def decode(cls, data: bytes) -> GraspCandidate: - value = pickle.loads(data) - return cls(value["pose"], value["score"]) diff --git a/dimos/msgs/manipulation_msgs/GraspCandidateArray.py b/dimos/msgs/manipulation_msgs/GraspCandidateArray.py deleted file mode 100644 index 5144d0b5b91..00000000000 --- a/dimos/msgs/manipulation_msgs/GraspCandidateArray.py +++ /dev/null @@ -1,48 +0,0 @@ -# Copyright 2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from __future__ import annotations - -from collections.abc import Iterator -import pickle - -from dimos.msgs.manipulation_msgs.GraspCandidate import GraspCandidate -from dimos.msgs.std_msgs.Header import Header - - -class GraspCandidateArray: - """Ordered grasp proposals sharing one input-cloud header.""" - - msg_name = "manipulation_msgs.GraspCandidateArray" - - def __init__( - self, header: Header | None = None, candidates: list[GraspCandidate] | None = None - ) -> None: - self.header = header if header is not None else Header(0.0) - self.candidates = candidates if candidates is not None else [] - - def __len__(self) -> int: - return len(self.candidates) - - def __iter__(self) -> Iterator[GraspCandidate]: - return iter(self.candidates) - - def encode(self) -> bytes: - """Encode using the repository's pickle transport convention.""" - return pickle.dumps({"header": self.header, "candidates": self.candidates}) - - @classmethod - def decode(cls, data: bytes) -> GraspCandidateArray: - value = pickle.loads(data) - return cls(value["header"], value["candidates"]) diff --git a/dimos/msgs/manipulation_msgs/test_grasp_candidate.py b/dimos/msgs/manipulation_msgs/test_grasp_candidate.py index 9538beda52e..eb676b1441e 100644 --- a/dimos/msgs/manipulation_msgs/test_grasp_candidate.py +++ b/dimos/msgs/manipulation_msgs/test_grasp_candidate.py @@ -12,16 +12,15 @@ # See the License for the specific language governing permissions and # limitations under the License. -import pytest +from dimos_generated.dimos_msgs.msg import GraspCandidate, GraspCandidateArray +from dimos_generated.geometry_msgs.msg import Point, Pose +from dimos_generated.std_msgs.msg import Header -from dimos.msgs.geometry_msgs.Pose import Pose -from dimos.msgs.manipulation_msgs.GraspCandidate import GraspCandidate -from dimos.msgs.manipulation_msgs.GraspCandidateArray import GraspCandidateArray -from dimos.msgs.std_msgs.Header import Header +from dimos.msgs.time import time_from_seconds def test_grasp_candidate_round_trip_preserves_pose_and_score() -> None: - candidate = GraspCandidate(Pose(0.4, -0.2, 0.3), 0.75) + candidate = GraspCandidate(pose=Pose(position=Point(x=0.4, y=-0.2, z=0.3)), score=0.75) decoded = GraspCandidate.decode(candidate.encode()) @@ -31,21 +30,18 @@ def test_grasp_candidate_round_trip_preserves_pose_and_score() -> None: assert decoded.score == 0.75 -def test_grasp_candidate_rejects_non_finite_score() -> None: - with pytest.raises(ValueError, match="score must be finite"): - GraspCandidate(score=float("nan")) - - def test_grasp_candidate_array_round_trip_preserves_header_and_order() -> None: candidates = [ - GraspCandidate(Pose(0.1, 0.0, 0.2), 0.9), - GraspCandidate(Pose(0.2, 0.0, 0.2), 0.7), + GraspCandidate(pose=Pose(position=Point(x=0.1, z=0.2)), score=0.9), + GraspCandidate(pose=Pose(position=Point(x=0.2, z=0.2)), score=0.7), ] - proposals = GraspCandidateArray(Header(123.0, "world"), candidates) + proposals = GraspCandidateArray( + header=Header(stamp=time_from_seconds(123.0), frame_id="world"), candidates=candidates + ) decoded = GraspCandidateArray.decode(proposals.encode()) - assert decoded.header.timestamp == 123.0 + assert decoded.header.stamp == time_from_seconds(123.0) assert decoded.header.frame_id == "world" - assert [candidate.score for candidate in decoded] == [0.9, 0.7] - assert [candidate.pose.position.x for candidate in decoded] == [0.1, 0.2] + assert [candidate.score for candidate in decoded.candidates] == [0.9, 0.7] + assert [candidate.pose.position.x for candidate in decoded.candidates] == [0.1, 0.2] diff --git a/dimos/msgs/nav_msgs/LineSegments3D.py b/dimos/msgs/nav_msgs/LineSegments3D.py deleted file mode 100644 index ddb355efc5d..00000000000 --- a/dimos/msgs/nav_msgs/LineSegments3D.py +++ /dev/null @@ -1,130 +0,0 @@ -# Copyright 2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -"""LineSegments3D: 3D line segments with a per-segment weight. - -On the wire uses ``nav_msgs/Path``. Consecutive pose pairs form segments and -``orientation.w`` carries the weight. -""" - -from __future__ import annotations - -import struct -import time -from typing import TYPE_CHECKING, BinaryIO - -import numpy as np - -from dimos.types.timestamped import Timestamped - -if TYPE_CHECKING: - from numpy.typing import ArrayLike, NDArray - from rerun._baseclasses import Archetype - -# Path prefix after the 8 byte fingerprint: poses_length, header seq, stamp sec, stamp nsec, frame_id length. -_PREFIX = struct.Struct(">iiiiI") -# PoseStamped bytes before its frame_id text: header seq, stamp sec, stamp nsec, frame_id length. -_POSE_HEAD = 16 -_POSE_DOUBLES = 7 -_POSE_TAIL = _POSE_DOUBLES * 8 - - -class LineSegments3D(Timestamped): - """Line segments as an (N, 2, 3) array plus one weight per segment.""" - - msg_name = "nav_msgs.LineSegments3D" - ts: float - frame_id: str - segments: NDArray[np.float64] - weights: NDArray[np.float64] - - def __init__( - self, - ts: float | None = None, - frame_id: str = "map", - segments: ArrayLike | None = None, - weights: ArrayLike | None = None, - ) -> None: - self.frame_id = frame_id - self.ts = time.time() if ts is None else ts - self.segments = np.asarray( - segments if segments is not None else np.empty((0, 2, 3)), dtype=np.float64 - ).reshape(-1, 2, 3) - self.weights = ( - np.ones(len(self.segments)) - if weights is None - else np.asarray(weights, dtype=np.float64).reshape(-1) - ) - - def lcm_encode(self) -> bytes: - raise NotImplementedError("Encoded on C++ side") - - @classmethod - def lcm_decode(cls, data: bytes | BinaryIO) -> LineSegments3D: - """Read the Path payload through strided array views. - - Every pose header must carry the same frame_id length, which is what the planner emits. - """ - raw = data if isinstance(data, bytes) else data.read() - count, _, sec, nsec, frame_len = _PREFIX.unpack_from(raw, 8) - offset = 8 + _PREFIX.size - frame_id = raw[offset : offset + frame_len][:-1].decode("utf-8", "replace") - offset += frame_len - ts = sec + nsec / 1e9 - if count == 0: - return cls(ts=ts, frame_id=frame_id) - if count % 2: - raise ValueError(f"LineSegments3D needs pose pairs, got {count} poses") - (pose_frame_len,) = struct.unpack_from(">I", raw, offset + _POSE_HEAD - 4) - stride = _POSE_HEAD + pose_frame_len + _POSE_TAIL - lens = np.ndarray( - (count,), dtype=">u4", buffer=raw, offset=offset + _POSE_HEAD - 4, strides=(stride,) - ) - if len(raw) != offset + count * stride or not np.all(lens == pose_frame_len): - raise ValueError("LineSegments3D poses must share one frame_id length") - poses = np.ndarray( - (count, _POSE_DOUBLES), - dtype=">f8", - buffer=raw, - offset=offset + _POSE_HEAD + pose_frame_len, - strides=(stride, 8), - ) - return cls( - ts=ts, - frame_id=frame_id, - segments=poses[:, :3].astype(np.float64).reshape(-1, 2, 3), - weights=poses[0::2, 6].astype(np.float64), - ) - - def to_rerun(self, z_offset: float = 0.0, radii: float = 0.04) -> Archetype: - """Render as ``rr.LineStrips3D``, green to red by log-scale weight.""" - import rerun as rr - - if len(self.segments) == 0: - return rr.LineStrips3D([]) - strips = self.segments.astype(np.float32) - strips[:, :, 2] += z_offset - log_w = np.log10(np.maximum(self.weights, 1e-6)) - lo, hi = float(log_w.min()), float(log_w.max()) - norm = (log_w - lo) / (hi - lo) if hi > lo else np.zeros_like(log_w) - r = (255 * norm).astype(np.uint8) - g = (255 * (1.0 - norm)).astype(np.uint8) - colors = np.column_stack([r, g, np.full_like(r, 60), np.full_like(r, 220)]) - return rr.LineStrips3D(strips, colors=colors, radii=radii) - - def __len__(self) -> int: - return len(self.segments) - - def __str__(self) -> str: - return f"LineSegments3D(frame_id='{self.frame_id}', segments={len(self.segments)})" diff --git a/dimos/msgs/nav_msgs/OccupancyGrid.py b/dimos/msgs/nav_msgs/OccupancyGrid.py deleted file mode 100644 index 7370cd6bf70..00000000000 --- a/dimos/msgs/nav_msgs/OccupancyGrid.py +++ /dev/null @@ -1,598 +0,0 @@ -# Copyright 2025-2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from __future__ import annotations - -from enum import IntEnum -from functools import lru_cache -import time -from typing import TYPE_CHECKING, BinaryIO - -from dimos_lcm.nav_msgs import ( - MapMetaData, - OccupancyGrid as LCMOccupancyGrid, -) -from dimos_lcm.std_msgs import Time as LCMTime -import numpy as np - -from dimos.msgs.geometry_msgs.Pose import Pose -from dimos.msgs.geometry_msgs.Vector3 import Vector3, VectorLike -from dimos.types.timestamped import Timestamped - - -@lru_cache(maxsize=16) -def _get_matplotlib_cmap(name: str): # type: ignore[no-untyped-def] - """Get a matplotlib colormap by name (cached for performance).""" - # Lazy import to save time. - import matplotlib.pyplot as plt - - return plt.get_cmap(name) - - -@lru_cache(maxsize=16) -def _build_occupancy_lut( - colormap: str | None, - opacity: float, - background: str | None, -) -> np.ndarray: - """Build a 102-entry RGBA LUT for grid values -1..100. One-time cost per config.""" - bg_rgb = np.array([0, 0, 0], dtype=np.float32) - if background is not None: - bg = background.lstrip("#") - bg_rgb = np.array([int(bg[i : i + 2], 16) for i in (0, 2, 4)], dtype=np.float32) - - # LUT: index 0 = grid value -1 (unknown), index 1 = grid value 0 (free), index 2..101 = grid values 1..100 - lut = np.zeros((102, 4), dtype=np.uint8) - - # Unknown (-1) → index 0: black - lut[0] = [0, 0, 0, 255] - - if colormap is not None: - cmap = _get_matplotlib_cmap(colormap) - # Free (0) → index 1 - fg = np.array(cmap(0.0)[:3]) * 255 - blended = fg * opacity + bg_rgb * (1 - opacity) - lut[1, :3] = blended.astype(np.uint8) - lut[1, 3] = 255 - - # Occupied (1..100) → index 2..101 - for cost in range(1, 101): - cost_norm = 0.5 + (cost / 100) * 0.5 - fg = np.array(cmap(cost_norm)[:3]) * 255 - blended = fg * opacity + bg_rgb * (1 - opacity) - lut[cost + 1, :3] = blended.astype(np.uint8) - lut[cost + 1, 3] = 255 - else: - # Foxglove-style: free = #484981, occupied = gradient to black - fg_free = np.array([72, 73, 129], dtype=np.float32) - blended_free = fg_free * opacity + bg_rgb * (1 - opacity) - lut[1, :3] = blended_free.astype(np.uint8) - lut[1, 3] = 255 - - for cost in range(1, 101): - factor = 1 - cost / 100 - fg = np.array([72 * factor, 73 * factor, 129 * factor], dtype=np.float32) - blended = fg * opacity + bg_rgb * (1 - opacity) - lut[cost + 1, :3] = blended.astype(np.uint8) - lut[cost + 1, 3] = 255 - - return lut - - -if TYPE_CHECKING: - from pathlib import Path - - from numpy.typing import NDArray - from rerun._baseclasses import Archetype - - -def block_max_reduce(cells: NDArray[np.int8], factor: int) -> NDArray[np.int8]: - """Coarsen an occupancy grid by taking the max of factor x factor blocks. - - Block maximum (not mean) so coarsening never erases an obstacle; a block - is unknown (-1) only when every cell in it is unknown. Trailing remainder - rows/cols are trimmed, keeping row 0/col 0 - the origin corner - so the - grid origin stays valid. Grids thinner than `factor` pass through. - """ - h, w = cells.shape[:2] - new_h, new_w = h // factor, w // factor - if new_h == 0 or new_w == 0: - return cells - trimmed = cells[: new_h * factor, : new_w * factor] - blocks = trimmed.reshape(new_h, factor, new_w, factor) - # Sink unknown below every known value for the max, then map it back. - as_int = blocks.astype(np.int16) - known = np.where(as_int < 0, -1000, as_int) - reduced = known.max(axis=(1, 3)) - reduced[reduced == -1000] = -1 - result: NDArray[np.int8] = reduced.astype(np.int8) - return result - - -class CostValues(IntEnum): - """Standard cost values for occupancy grid cells. - - These values follow the ROS nav_msgs/OccupancyGrid convention: - - 0: Free space - - 1-99: Occupied space with varying cost levels - - 100: Lethal obstacle (definitely occupied) - - -1: Unknown space - """ - - UNKNOWN = -1 # Unknown space - FREE = 0 # Free space - OCCUPIED = 100 # Occupied/lethal space - - -class OccupancyGrid(Timestamped): - """ - Convenience wrapper for nav_msgs/OccupancyGrid with numpy array support. - """ - - msg_name = "nav_msgs.OccupancyGrid" - - # Attributes - ts: float - frame_id: str - info: MapMetaData - grid: NDArray[np.int8] - - def __init__( - self, - grid: NDArray[np.int8] | None = None, - width: int | None = None, - height: int | None = None, - resolution: float = 0.05, - origin: Pose | None = None, - frame_id: str = "world", - ts: float | None = None, - ) -> None: - """Initialize OccupancyGrid. - - Args: - grid: 2D numpy array of int8 values (height x width) - width: Width in cells (used if grid is None) - height: Height in cells (used if grid is None) - resolution: Grid resolution in meters/cell - origin: Origin pose of the grid - frame_id: Reference frame - ts: Timestamp (defaults to current time if 0) - """ - - self.frame_id = frame_id - self.ts = time.time() if ts is None else ts - - if grid is not None: - # Initialize from numpy array - if grid.ndim != 2: - raise ValueError("Grid must be a 2D array") - height, width = grid.shape - self.info = MapMetaData( - map_load_time=self._to_lcm_time(), # type: ignore[no-untyped-call] - resolution=resolution, - width=width, - height=height, - origin=origin or Pose(), - ) - self.grid = grid.astype(np.int8) - elif width is not None and height is not None: - # Initialize with dimensions - self.info = MapMetaData( - map_load_time=self._to_lcm_time(), # type: ignore[no-untyped-call] - resolution=resolution, - width=width, - height=height, - origin=origin or Pose(), - ) - self.grid = np.full((height, width), -1, dtype=np.int8) - else: - # Initialize empty - self.info = MapMetaData(map_load_time=self._to_lcm_time()) # type: ignore[no-untyped-call] - self.grid = np.array([], dtype=np.int8) - - def _to_lcm_time(self): # type: ignore[no-untyped-def] - """Convert timestamp to LCM Time.""" - - s = int(self.ts) - return LCMTime(sec=s, nsec=int((self.ts - s) * 1_000_000_000)) - - @property - def width(self) -> int: - """Width of the grid in cells.""" - return self.info.width # type: ignore[no-any-return] - - @property - def height(self) -> int: - """Height of the grid in cells.""" - return self.info.height # type: ignore[no-any-return] - - @property - def resolution(self) -> float: - """Grid resolution in meters/cell.""" - return self.info.resolution # type: ignore[no-any-return] - - @property - def origin(self) -> Pose: - """Origin pose of the grid.""" - return self.info.origin # type: ignore[no-any-return] - - @property - def total_cells(self) -> int: - """Total number of cells in the grid.""" - return self.width * self.height - - @property - def occupied_cells(self) -> int: - """Number of occupied cells (value >= 1).""" - return int(np.sum(self.grid >= 1)) - - @property - def free_cells(self) -> int: - """Number of free cells (value == 0).""" - return int(np.sum(self.grid == 0)) - - @property - def unknown_cells(self) -> int: - """Number of unknown cells (value == -1).""" - return int(np.sum(self.grid == -1)) - - @property - def occupied_percent(self) -> float: - """Percentage of cells that are occupied.""" - return (self.occupied_cells / self.total_cells * 100) if self.total_cells > 0 else 0.0 - - @property - def free_percent(self) -> float: - """Percentage of cells that are free.""" - return (self.free_cells / self.total_cells * 100) if self.total_cells > 0 else 0.0 - - @property - def unknown_percent(self) -> float: - """Percentage of cells that are unknown.""" - return (self.unknown_cells / self.total_cells * 100) if self.total_cells > 0 else 0.0 - - @classmethod - def from_path(cls, path: Path) -> OccupancyGrid: - match path.suffix.lower(): - case ".npy": - return cls(grid=np.load(path)) - case ".png": - # Lazy import: Pillow serves only this file loader; message - # transport must not depend on it. - from PIL import Image - - img = Image.open(path).convert("L") - return cls(grid=np.array(img).astype(np.int8)) - case _: - raise NotImplementedError(f"Unsupported file format: {path.suffix}") - - def world_to_grid(self, point: VectorLike) -> Vector3: - """Convert world coordinates to grid coordinates. - - Args: - point: A vector-like object containing X,Y coordinates - - Returns: - Vector3 with grid coordinates - """ - positionVector = Vector3(point) - # Get origin position - ox = self.origin.position.x - oy = self.origin.position.y - - # Convert to grid coordinates (simplified, assuming no rotation) - grid_x = (positionVector.x - ox) / self.resolution - grid_y = (positionVector.y - oy) / self.resolution - - return Vector3(grid_x, grid_y, 0.0) - - def grid_to_world(self, grid_point: VectorLike) -> Vector3: - """Convert grid coordinates to world coordinates. - - Args: - grid_point: Vector-like object containing grid coordinates - - Returns: - World position as Vector3 - """ - gridVector = Vector3(grid_point) - # Get origin position - ox = self.origin.position.x - oy = self.origin.position.y - - # Convert to world (simplified, no rotation) - x = ox + gridVector.x * self.resolution - y = oy + gridVector.y * self.resolution - - return Vector3(x, y, 0.0) - - def __str__(self) -> str: - """Create a concise string representation.""" - origin_pos = self.origin.position - - parts = [ - f"▦ OccupancyGrid[{self.frame_id}]", - f"{self.width}x{self.height}", - f"({self.width * self.resolution:.1f}x{self.height * self.resolution:.1f}m @", - f"{1 / self.resolution:.0f}cm res)", - f"Origin: ({origin_pos.x:.2f}, {origin_pos.y:.2f})", - f"▣ {self.occupied_percent:.1f}%", - f"□ {self.free_percent:.1f}%", - f"◌ {self.unknown_percent:.1f}%", - ] - - return " ".join(parts) - - def __repr__(self) -> str: - """Create a detailed representation.""" - return ( - f"OccupancyGrid(width={self.width}, height={self.height}, " - f"resolution={self.resolution}, frame_id='{self.frame_id}', " - f"occupied={self.occupied_cells}, free={self.free_cells}, " - f"unknown={self.unknown_cells})" - ) - - def lcm_encode(self) -> bytes: - """Encode OccupancyGrid to LCM bytes.""" - # Create LCM message - lcm_msg = LCMOccupancyGrid() - - # Build header on demand - s = int(self.ts) - lcm_msg.header.stamp.sec = s - lcm_msg.header.stamp.nsec = int((self.ts - s) * 1_000_000_000) - lcm_msg.header.frame_id = self.frame_id - - # Copy map metadata - lcm_msg.info = self.info - - # Convert numpy array to flat data list - if self.grid.size > 0: - flat_data = self.grid.flatten() - lcm_msg.data_length = len(flat_data) - lcm_msg.data = flat_data.tolist() - else: - lcm_msg.data_length = 0 - lcm_msg.data = [] - - return lcm_msg.lcm_encode() # type: ignore[no-any-return] - - @classmethod - def lcm_decode(cls, data: bytes | BinaryIO) -> OccupancyGrid: - """Decode LCM bytes to OccupancyGrid.""" - lcm_msg = LCMOccupancyGrid.lcm_decode(data) - - # Extract timestamp and frame_id from header - ts = lcm_msg.header.stamp.sec + (lcm_msg.header.stamp.nsec / 1_000_000_000) - frame_id = lcm_msg.header.frame_id - - # Extract grid data; empty stays 2-D so the constructor accepts it. - if lcm_msg.data and lcm_msg.info.width > 0 and lcm_msg.info.height > 0: - grid = np.array(lcm_msg.data, dtype=np.int8).reshape( - (lcm_msg.info.height, lcm_msg.info.width) - ) - else: - grid = np.zeros((0, 0), dtype=np.int8) - - # The wire origin decodes as a plain generated pose without the dimos - # Pose surface (.yaw etc.); rebuild it field by field so the origin - # property honors its declared type. - o = lcm_msg.info.origin - lcm_msg.info.origin = Pose( - position=[o.position.x, o.position.y, o.position.z], - orientation=[o.orientation.x, o.orientation.y, o.orientation.z, o.orientation.w], - ) - - # Create new instance - instance = cls( - grid=grid, - resolution=lcm_msg.info.resolution, - origin=lcm_msg.info.origin, - frame_id=frame_id, - ts=ts, - ) - instance.info = lcm_msg.info - return instance - - def filter_above(self, threshold: int) -> OccupancyGrid: - """Create a new OccupancyGrid with only values above threshold. - - Args: - threshold: Keep cells with values > threshold - - Returns: - New OccupancyGrid where: - - Cells > threshold: kept as-is - - Cells <= threshold: set to -1 (unknown) - - Unknown cells (-1): preserved - """ - new_grid = self.grid.copy() - - # Create mask for cells to filter (not unknown and <= threshold) - filter_mask = (new_grid != -1) & (new_grid <= threshold) - - # Set filtered cells to unknown - new_grid[filter_mask] = -1 - - # Create new OccupancyGrid - filtered = OccupancyGrid( - new_grid, - resolution=self.resolution, - origin=self.origin, - frame_id=self.frame_id, - ts=self.ts, - ) - - return filtered - - def filter_below(self, threshold: int) -> OccupancyGrid: - """Create a new OccupancyGrid with only values below threshold. - - Args: - threshold: Keep cells with values < threshold - - Returns: - New OccupancyGrid where: - - Cells < threshold: kept as-is - - Cells >= threshold: set to -1 (unknown) - - Unknown cells (-1): preserved - """ - new_grid = self.grid.copy() - - # Create mask for cells to filter (not unknown and >= threshold) - filter_mask = (new_grid != -1) & (new_grid >= threshold) - - # Set filtered cells to unknown - new_grid[filter_mask] = -1 - - # Create new OccupancyGrid - filtered = OccupancyGrid( - new_grid, - resolution=self.resolution, - origin=self.origin, - frame_id=self.frame_id, - ts=self.ts, - ) - - return filtered - - def max(self) -> OccupancyGrid: - """Create a new OccupancyGrid with all non-unknown cells set to maximum value. - - Returns: - New OccupancyGrid where: - - All non-unknown cells: set to CostValues.OCCUPIED (100) - - Unknown cells: preserved as CostValues.UNKNOWN (-1) - """ - new_grid = self.grid.copy() - - # Set all non-unknown cells to max - new_grid[new_grid != CostValues.UNKNOWN] = CostValues.OCCUPIED - - # Create new OccupancyGrid - maxed = OccupancyGrid( - new_grid, - resolution=self.resolution, - origin=self.origin, - frame_id=self.frame_id, - ts=self.ts, - ) - - return maxed - - def copy(self) -> OccupancyGrid: - """Create a deep copy of the OccupancyGrid. - - Returns: - A new OccupancyGrid instance with copied data. - """ - return OccupancyGrid( - grid=self.grid.copy(), - resolution=self.resolution, - origin=self.origin, - frame_id=self.frame_id, - ts=self.ts, - ) - - def cell_value(self, world_position: Vector3) -> int: - grid_position = self.world_to_grid(world_position) - x = int(grid_position.x) - y = int(grid_position.y) - - if not (0 <= x < self.width and 0 <= y < self.height): - return CostValues.UNKNOWN - - return int(self.grid[y, x]) - - def to_rerun( - self, - colormap: str | None = None, - z_offset: float = 0, - opacity: float = 1.0, - cost_range: tuple[int, int] | None = None, - background: str | None = None, - color_lookup_table: np.ndarray | None = None, - ) -> Archetype: - """Convert to 3D textured mesh overlay on floor plane. - - Uses a single quad with the occupancy grid as a texture. - Much more efficient than per-cell quads (4 vertices vs n_cells*4). - """ - import rerun as rr - - if self.grid.size == 0: - return rr.Mesh3D(vertex_positions=[]) - - # Downsample grid before LUT to avoid allocating huge RGBA array - max_tex = 256 - grid = self.grid[::-1] # flip for world coords (view, no copy) - if grid.shape[0] > max_tex or grid.shape[1] > max_tex: - step_h = max(1, grid.shape[0] // max_tex) - step_w = max(1, grid.shape[1] // max_tex) - grid = grid[::step_h, ::step_w] - - if color_lookup_table is None: - color_lookup_table = _build_occupancy_lut(colormap, opacity, background) - rgba = np.ascontiguousarray(color_lookup_table[np.clip(grid + 1, 0, 101)]) - - # Apply cost_range filter on downsampled grid - if cost_range is not None: - out_of_range = ((grid < cost_range[0]) | (grid > cost_range[1])) & (grid != -1) - if np.any(out_of_range): - bg_rgb = [0, 0, 0] - if background is not None: - bg = background.lstrip("#") - bg_rgb = [int(bg[i : i + 2], 16) for i in (0, 2, 4)] - rgba[out_of_range, :3] = bg_rgb - rgba[out_of_range, 3] = 255 - - # Single quad covering entire grid - ox = self.origin.position.x - oy = self.origin.position.y - w = self.width * self.resolution - h = self.height * self.resolution - - vertices = np.array( - [ - [ox, oy, z_offset], # 0: bottom-left (world) - [ox + w, oy, z_offset], # 1: bottom-right - [ox + w, oy + h, z_offset], # 2: top-right - [ox, oy + h, z_offset], # 3: top-left - ], - dtype=np.float32, - ) - - indices = np.array([[0, 1, 2], [0, 2, 3]], dtype=np.uint32) - - # UV coords: Rerun uses top-left origin for textures - # Grid row 0 is at world y=oy (bottom), row H-1 at y=oy+h (top) - # Texture row 0 = grid row 0, so: - # world bottom (v0,v1) -> texture v=1 (bottom of texture) - # world top (v2,v3) -> texture v=0 (top of texture) - texcoords = np.array( - [ - [0.0, 1.0], # v0: bottom-left world -> bottom-left tex - [1.0, 1.0], # v1: bottom-right world -> bottom-right tex - [1.0, 0.0], # v2: top-right world -> top-right tex - [0.0, 0.0], # v3: top-left world -> top-left tex - ], - dtype=np.float32, - ) - - return rr.Mesh3D( - vertex_positions=vertices, - triangle_indices=indices, - vertex_texcoords=texcoords, - albedo_texture=rgba, - ) diff --git a/dimos/msgs/nav_msgs/Odometry.py b/dimos/msgs/nav_msgs/Odometry.py deleted file mode 100644 index 5e986672ea9..00000000000 --- a/dimos/msgs/nav_msgs/Odometry.py +++ /dev/null @@ -1,249 +0,0 @@ -# Copyright 2025-2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from __future__ import annotations - -import time -from typing import TYPE_CHECKING - -if TYPE_CHECKING: - from rerun._baseclasses import Archetype - -from dimos_lcm.nav_msgs import Odometry as LCMOdometry -import numpy as np - -from dimos.msgs.geometry_msgs.Pose import Pose -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.geometry_msgs.PoseWithCovariance import PoseWithCovariance -from dimos.msgs.geometry_msgs.Twist import Twist -from dimos.msgs.geometry_msgs.TwistWithCovariance import TwistWithCovariance -from dimos.types.timestamped import Timestamped - -if TYPE_CHECKING: - from dimos.msgs.geometry_msgs.Quaternion import Quaternion - from dimos.msgs.geometry_msgs.Vector3 import Vector3 - - -class Odometry(Timestamped): - """Odometry message with pose, twist, and frame information.""" - - msg_name = "nav_msgs.Odometry" - - def __init__( - self, - ts: float | None = None, - frame_id: str = "", - child_frame_id: str = "", - pose: PoseWithCovariance | Pose | None = None, - twist: TwistWithCovariance | Twist | None = None, - ) -> None: - self.ts = time.time() if ts is None else ts - self.frame_id = frame_id - self.child_frame_id = child_frame_id - - if pose is None: - self.pose = PoseWithCovariance() - elif isinstance(pose, Pose): - self.pose = PoseWithCovariance(pose) - else: - self.pose = pose - - if twist is None: - self.twist = TwistWithCovariance() - elif isinstance(twist, Twist): - self.twist = TwistWithCovariance(twist) - else: - self.twist = twist - - # -- Convenience properties -- - - @property - def position(self) -> Vector3: - return self.pose.position - - @property - def orientation(self) -> Quaternion: - return self.pose.orientation - - @property - def linear_velocity(self) -> Vector3: - return self.twist.linear - - @property - def angular_velocity(self) -> Vector3: - return self.twist.angular - - @property - def x(self) -> float: - return self.pose.x - - @property - def y(self) -> float: - return self.pose.y - - @property - def z(self) -> float: - return self.pose.z - - @property - def vx(self) -> float: - return self.twist.linear.x - - @property - def vy(self) -> float: - return self.twist.linear.y - - @property - def vz(self) -> float: - return self.twist.linear.z - - @property - def wx(self) -> float: - return self.twist.angular.x - - @property - def wy(self) -> float: - return self.twist.angular.y - - @property - def wz(self) -> float: - return self.twist.angular.z - - @property - def roll(self) -> float: - return self.pose.roll - - @property - def pitch(self) -> float: - return self.pose.pitch - - @property - def yaw(self) -> float: - return self.pose.yaw - - def to_pose_stamped(self) -> PoseStamped: - return PoseStamped( - ts=self.ts, - frame_id=self.frame_id, - position=[self.position.x, self.position.y, self.position.z], - orientation=[ - self.orientation.x, - self.orientation.y, - self.orientation.z, - self.orientation.w, - ], - ) - - # -- Serialization -- - - def lcm_encode(self) -> bytes: - lcm_msg = LCMOdometry() - - lcm_msg.header.stamp.sec, lcm_msg.header.stamp.nsec = self.ros_timestamp() - lcm_msg.header.frame_id = self.frame_id - lcm_msg.child_frame_id = self.child_frame_id - - lcm_msg.pose.pose = self.pose.pose - lcm_msg.pose.covariance = list(np.asarray(self.pose.covariance)) - - lcm_msg.twist.twist = self.twist.twist - lcm_msg.twist.covariance = list(np.asarray(self.twist.covariance)) - - return lcm_msg.lcm_encode() # type: ignore[no-any-return] - - @classmethod - def lcm_decode(cls, data: bytes) -> Odometry: - lcm_msg = LCMOdometry.lcm_decode(data) - - ts = lcm_msg.header.stamp.sec + (lcm_msg.header.stamp.nsec / 1_000_000_000) - - pose = Pose( - position=[ - lcm_msg.pose.pose.position.x, - lcm_msg.pose.pose.position.y, - lcm_msg.pose.pose.position.z, - ], - orientation=[ - lcm_msg.pose.pose.orientation.x, - lcm_msg.pose.pose.orientation.y, - lcm_msg.pose.pose.orientation.z, - lcm_msg.pose.pose.orientation.w, - ], - ) - twist = Twist( - linear=[ - lcm_msg.twist.twist.linear.x, - lcm_msg.twist.twist.linear.y, - lcm_msg.twist.twist.linear.z, - ], - angular=[ - lcm_msg.twist.twist.angular.x, - lcm_msg.twist.twist.angular.y, - lcm_msg.twist.twist.angular.z, - ], - ) - - return cls( - ts=ts, - frame_id=lcm_msg.header.frame_id, - child_frame_id=lcm_msg.child_frame_id, - pose=PoseWithCovariance(pose, lcm_msg.pose.covariance), - twist=TwistWithCovariance(twist, lcm_msg.twist.covariance), - ) - - # -- Comparison / display -- - - def __eq__(self, other: object) -> bool: - if not isinstance(other, Odometry): - return False - return ( - abs(self.ts - other.ts) < 1e-6 - and self.frame_id == other.frame_id - and self.child_frame_id == other.child_frame_id - and self.pose == other.pose - and self.twist == other.twist - ) - - def __repr__(self) -> str: - return ( - f"Odometry(ts={self.ts:.6f}, frame_id='{self.frame_id}', " - f"child_frame_id='{self.child_frame_id}', pose={self.pose!r}, twist={self.twist!r})" - ) - - def __str__(self) -> str: - return ( - f"Odometry:\n" - f" Timestamp: {self.ts:.6f}\n" - f" Frame: {self.frame_id} -> {self.child_frame_id}\n" - f" Position: [{self.x:.3f}, {self.y:.3f}, {self.z:.3f}]\n" - f" Orientation: [roll={self.roll:.3f}, pitch={self.pitch:.3f}, yaw={self.yaw:.3f}]\n" - f" Linear Velocity: [{self.vx:.3f}, {self.vy:.3f}, {self.vz:.3f}]\n" - f" Angular Velocity: [{self.wx:.3f}, {self.wy:.3f}, {self.wz:.3f}]" - ) - - def to_rerun(self) -> Archetype: - """Convert to rerun Transform3D for visualizing the pose.""" - import rerun as rr - - return rr.Transform3D( - translation=[self.x, self.y, self.z], - rotation=rr.Quaternion( - xyzw=[ - self.orientation.x, - self.orientation.y, - self.orientation.z, - self.orientation.w, - ] - ), - ) diff --git a/dimos/msgs/nav_msgs/Path.py b/dimos/msgs/nav_msgs/Path.py deleted file mode 100644 index cd901f2a06f..00000000000 --- a/dimos/msgs/nav_msgs/Path.py +++ /dev/null @@ -1,214 +0,0 @@ -# Copyright 2025-2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from __future__ import annotations - -import time -from typing import TYPE_CHECKING, BinaryIO - -from dimos_lcm.geometry_msgs import ( - Point as LCMPoint, - Pose as LCMPose, - PoseStamped as LCMPoseStamped, - Quaternion as LCMQuaternion, -) -from dimos_lcm.nav_msgs import Path as LCMPath -from dimos_lcm.std_msgs import Header as LCMHeader, Time as LCMTime - -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.types.timestamped import Timestamped - -if TYPE_CHECKING: - from collections.abc import Iterator - - from rerun._baseclasses import Archetype - - -def sec_nsec(ts): # type: ignore[no-untyped-def] - s = int(ts) - return [s, int((ts - s) * 1_000_000_000)] - - -class Path(Timestamped): - msg_name = "nav_msgs.Path" - ts: float - frame_id: str - poses: list[PoseStamped] - - def __init__( # type: ignore[no-untyped-def] - self, - ts: float | None = None, - frame_id: str = "world", - poses: list[PoseStamped] | None = None, - **kwargs, - ) -> None: - self.frame_id = frame_id - self.ts = time.time() if ts is None else ts - self.poses = poses if poses is not None else [] - - def __len__(self) -> int: - """Return the number of poses in the path.""" - return len(self.poses) - - def __bool__(self) -> bool: - """Return True if path has poses.""" - return len(self.poses) > 0 - - def head(self) -> PoseStamped | None: - """Return the first pose in the path, or None if empty.""" - return self.poses[0] if self.poses else None - - def last(self) -> PoseStamped | None: - """Return the last pose in the path, or None if empty.""" - return self.poses[-1] if self.poses else None - - def tail(self) -> Path: - """Return a new Path with all poses except the first.""" - return Path(ts=self.ts, frame_id=self.frame_id, poses=self.poses[1:] if self.poses else []) - - def push(self, pose: PoseStamped) -> Path: - """Return a new Path with the pose appended (immutable).""" - return Path(ts=self.ts, frame_id=self.frame_id, poses=[*self.poses, pose]) - - def push_mut(self, pose: PoseStamped) -> None: - """Append a pose to this path (mutable).""" - self.poses.append(pose) - - def lcm_encode(self) -> bytes: - """Encode Path to LCM bytes.""" - lcm_msg = LCMPath() - - # Set poses - lcm_msg.poses_length = len(self.poses) - lcm_poses = [] # Build list separately to avoid LCM library reuse issues - for pose in self.poses: - lcm_pose = LCMPoseStamped() - # Create new pose objects to avoid LCM library reuse bug - lcm_pose.pose = LCMPose() - lcm_pose.pose.position = LCMPoint() - lcm_pose.pose.orientation = LCMQuaternion() - - # Set the pose geometry data - lcm_pose.pose.position.x = pose.x - lcm_pose.pose.position.y = pose.y - lcm_pose.pose.position.z = pose.z - lcm_pose.pose.orientation.x = pose.orientation.x - lcm_pose.pose.orientation.y = pose.orientation.y - lcm_pose.pose.orientation.z = pose.orientation.z - lcm_pose.pose.orientation.w = pose.orientation.w - - # Create new header to avoid reuse - lcm_pose.header = LCMHeader() - lcm_pose.header.stamp = LCMTime() - - # Set the header with pose timestamp but path's frame_id - [lcm_pose.header.stamp.sec, lcm_pose.header.stamp.nsec] = sec_nsec(pose.ts) # type: ignore[no-untyped-call] - lcm_pose.header.frame_id = self.frame_id # All poses use path's frame_id - lcm_poses.append(lcm_pose) - lcm_msg.poses = lcm_poses - - # Set header with path's own timestamp - [lcm_msg.header.stamp.sec, lcm_msg.header.stamp.nsec] = sec_nsec(self.ts) # type: ignore[no-untyped-call] - lcm_msg.header.frame_id = self.frame_id - - return lcm_msg.lcm_encode() # type: ignore[no-any-return] - - @classmethod - def lcm_decode(cls, data: bytes | BinaryIO) -> Path: - """Decode LCM bytes to Path.""" - lcm_msg = LCMPath.lcm_decode(data) - - # Decode header - header_ts = lcm_msg.header.stamp.sec + (lcm_msg.header.stamp.nsec / 1_000_000_000) - frame_id = lcm_msg.header.frame_id - - # Decode poses - all use the path's frame_id - poses = [] - for lcm_pose in lcm_msg.poses: - pose = PoseStamped( - ts=lcm_pose.header.stamp.sec + (lcm_pose.header.stamp.nsec / 1_000_000_000), - frame_id=frame_id, # Use path's frame_id for all poses - position=[ - lcm_pose.pose.position.x, - lcm_pose.pose.position.y, - lcm_pose.pose.position.z, - ], - orientation=[ - lcm_pose.pose.orientation.x, - lcm_pose.pose.orientation.y, - lcm_pose.pose.orientation.z, - lcm_pose.pose.orientation.w, - ], - ) - poses.append(pose) - - # Use header timestamp for the path - return cls(ts=header_ts, frame_id=frame_id, poses=poses) - - def __str__(self) -> str: - """String representation of Path.""" - return f"Path(frame_id='{self.frame_id}', poses={len(self.poses)})" - - def __getitem__(self, index: int | slice) -> PoseStamped | list[PoseStamped]: - """Allow indexing and slicing of poses.""" - return self.poses[index] - - def __iter__(self) -> Iterator: # type: ignore[type-arg] - """Allow iteration over poses.""" - return iter(self.poses) - - def slice(self, start: int, end: int | None = None) -> Path: - """Return a new Path with a slice of poses.""" - return Path(ts=self.ts, frame_id=self.frame_id, poses=self.poses[start:end]) - - def extend(self, other: Path) -> Path: - """Return a new Path with poses from both paths (immutable).""" - return Path(ts=self.ts, frame_id=self.frame_id, poses=self.poses + other.poses) - - def extend_mut(self, other: Path) -> None: - """Extend this path with poses from another path (mutable).""" - self.poses.extend(other.poses) - - def reverse(self) -> Path: - """Return a new Path with poses in reverse order.""" - return Path(ts=self.ts, frame_id=self.frame_id, poses=list(reversed(self.poses))) - - def clear(self) -> None: - """Clear all poses from this path (mutable).""" - self.poses.clear() - - def to_rerun( - self, - color: tuple[int, int, int] = (0, 255, 128), - z_offset: float = 0.5, - radii: float = 0.05, - ) -> Archetype: - """Convert to rerun LineStrips3D format. - - Args: - color: RGB color tuple for the path line - z_offset: Height above floor to render path (default 0.2m to avoid costmap occlusion) - radii: Thickness of the path line (default 0.05m = 5cm) - - Returns: - rr.LineStrips3D archetype for logging to rerun - """ - import rerun as rr - - if not self.poses: - return rr.LineStrips3D([]) - - # Lift path above floor so it's visible over costmap - points = [[p.x, p.y, p.z + z_offset] for p in self.poses] - return rr.LineStrips3D([points], colors=[color], radii=radii) diff --git a/dimos/msgs/nav_msgs/test_LineSegments3D.py b/dimos/msgs/nav_msgs/test_LineSegments3D.py index 40ee6d73b5d..ae1db2309d4 100644 --- a/dimos/msgs/nav_msgs/test_LineSegments3D.py +++ b/dimos/msgs/nav_msgs/test_LineSegments3D.py @@ -12,54 +12,58 @@ # See the License for the specific language governing permissions and # limitations under the License. -from dimos_lcm.geometry_msgs.Point import Point -from dimos_lcm.geometry_msgs.Pose import Pose -from dimos_lcm.geometry_msgs.PoseStamped import PoseStamped -from dimos_lcm.geometry_msgs.Quaternion import Quaternion -from dimos_lcm.nav_msgs.Path import Path -from dimos_lcm.std_msgs.Header import Header -from dimos_lcm.std_msgs.Time import Time +"""Explicit line messages replace the old Path/orientation weight encoding.""" + +from dimos_generated.dimos_msgs.msg import LineSegment3D, LineSegments3D +from dimos_generated.geometry_msgs.msg import Point +from dimos_generated.std_msgs.msg import Header import numpy as np import pytest +from rosbags.typesys import Stores, get_types_from_msg, get_typestore -from dimos.msgs.nav_msgs.LineSegments3D import LineSegments3D - - -def encode_edges(n_segments: int, frame_ids: list[str] | None = None) -> bytes: - """A Path of consecutive pose pairs with orientation.w carrying the weight.""" - poses = [ - PoseStamped( - header=Header(frame_id=frame_ids[i % len(frame_ids)] if frame_ids else "odom"), - pose=Pose( - position=Point(float(i), float(i) * 0.5, -1.0), - orientation=Quaternion(w=float(i // 2) * 0.1), - ), - ) - for i in range(2 * n_segments) - ] - header = Header(stamp=Time(12, 500_000_000), frame_id="odom") - return Path(poses_length=len(poses), header=header, poses=poses).lcm_encode() - - -def expected_segments(n_segments: int) -> np.ndarray: - i = np.arange(2 * n_segments, dtype=np.float64) - return np.stack([i, i * 0.5, np.full_like(i, -1.0)], axis=1).reshape(-1, 2, 3) - +from dimos.msgs.time import time_from_nanoseconds, to_nanoseconds -def test_decode_matches_the_wire_layout() -> None: - msg = LineSegments3D.lcm_decode(encode_edges(50)) - assert msg.frame_id == "odom" - assert msg.ts == 12.5 - np.testing.assert_array_equal(msg.segments, expected_segments(50)) - np.testing.assert_allclose(msg.weights, np.arange(50) * 0.1) - empty = LineSegments3D.lcm_decode(encode_edges(0)) - assert len(empty) == 0 - assert empty.segments.shape == (0, 2, 3) - assert empty.weights.shape == (0,) +@pytest.mark.parametrize("count", [0, 1, 50]) +def test_cdr_segment_layout_and_independent_schema(count: int) -> None: + source = LineSegments3D( + header=Header(stamp=time_from_nanoseconds(12500000000), frame_id="odom"), + segments=[ + LineSegment3D( + start=Point(x=2 * i, y=i, z=-1), + end=Point(x=2 * i + 1, y=(2 * i + 1) * 0.5, z=-1), + weight=i * 0.1, + ) + for i in range(count) + ], + ) + decoded = LineSegments3D.decode(source.encode()) + assert decoded.header.frame_id == "odom" + assert to_nanoseconds(decoded.header.stamp) == 12500000000 + assert len(decoded.segments) == count + coordinates = np.array( + [[[s.start.x, s.start.y, s.start.z], [s.end.x, s.end.y, s.end.z]] for s in decoded.segments] + ).reshape(-1, 2, 3) + indices = np.arange(2 * count, dtype=np.float64) + expected = np.stack([indices, indices * 0.5, np.full_like(indices, -1)], axis=1).reshape( + -1, 2, 3 + ) + np.testing.assert_array_equal(coordinates, expected) + np.testing.assert_allclose([s.weight for s in decoded.segments], np.arange(count) * 0.1) + store = get_typestore(Stores.ROS2_JAZZY) + store.register(get_types_from_msg(LineSegments3D.schema, LineSegments3D.msg_name)) + independent = store.deserialize_cdr(source.encode(), LineSegments3D.msg_name) + assert len(independent.segments) == count + assert independent.header.frame_id == "odom" + for i, segment in enumerate(independent.segments): + assert segment.start.x == 2 * i + assert segment.end.y == (2 * i + 1) * 0.5 + assert segment.weight == pytest.approx(i * 0.1) -def test_mixed_frame_id_lengths_are_rejected() -> None: - raw = encode_edges(20, frame_ids=["odom", "map", "base_link"]) - with pytest.raises(ValueError, match="frame_id length"): - LineSegments3D.lcm_decode(raw) +@pytest.mark.parametrize("frame", ["odom", "map", "base_link"]) +def test_explicit_segment_schema_accepts_variable_frame_lengths(frame: str) -> None: + source = LineSegments3D(header=Header(frame_id=frame), segments=[LineSegment3D(weight=0.7)]) + decoded = LineSegments3D.decode(source.encode()) + assert decoded.header.frame_id == frame + assert decoded.segments[0].weight == pytest.approx(0.7) diff --git a/dimos/msgs/nav_msgs/test_OccupancyGrid.py b/dimos/msgs/nav_msgs/test_OccupancyGrid.py index 98297f6f395..050c6420900 100644 --- a/dimos/msgs/nav_msgs/test_OccupancyGrid.py +++ b/dimos/msgs/nav_msgs/test_OccupancyGrid.py @@ -13,379 +13,276 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""Test the OccupancyGrid convenience class.""" - -import pickle +"""Generated occupancy grids with external array and coordinate operations.""" +from dimos_generated.geometry_msgs.msg import Point, Pose +from dimos_generated.nav_msgs.msg import OccupancyGrid +from dimos_generated.std_msgs.msg import Header import numpy as np import pytest -from dimos.mapping.occupancy.gradient import gradient from dimos.mapping.occupancy.inflation import simple_inflate from dimos.mapping.pointclouds.occupancy import general_occupancy -from dimos.msgs.geometry_msgs.Pose import Pose -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.msgs.nav_msgs.OccupancyGrid import OccupancyGrid, block_max_reduce -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 -from dimos.utils.data import get_data +from dimos.msgs.geometry import quaternion_from_euler, yaw +from dimos.msgs.occupancy import ( + block_max_reduce, + grid_to_world, + occupancy_extent, + occupancy_from_array, + occupancy_view, + world_to_grid, +) +from dimos.msgs.pointcloud import pointcloud_from_xyz def test_empty_grid() -> None: """Test creating an empty grid.""" grid = OccupancyGrid() - assert grid.width == 0 - assert grid.height == 0 - assert grid.grid.shape == (0,) - assert grid.total_cells == 0 - assert grid.frame_id == "world" + assert grid.info.width == 0 + assert grid.info.height == 0 + assert occupancy_view(grid).shape == (0, 0) + assert len(grid.data) == 0 + assert grid.header.frame_id == "" def test_grid_with_dimensions() -> None: """Test creating a grid with specified dimensions.""" - grid = OccupancyGrid(width=10, height=10, resolution=0.1, frame_id="map") - assert grid.width == 10 - assert grid.height == 10 - assert grid.resolution == 0.1 - assert grid.frame_id == "map" - assert grid.grid.shape == (10, 10) - assert np.all(grid.grid == -1) # All unknown - assert grid.unknown_cells == 100 - assert grid.unknown_percent == 100.0 + grid = occupancy_from_array( + np.full((10, 10), -1, dtype=np.int8), resolution=0.1, header=Header(frame_id="map") + ) + assert grid.info.width == 10 + assert grid.info.height == 10 + assert grid.info.resolution == pytest.approx(0.1) + assert grid.header.frame_id == "map" + assert occupancy_view(grid).shape == (10, 10) + assert np.all(occupancy_view(grid) == -1) + assert np.count_nonzero(occupancy_view(grid) == -1) == 100 + assert (np.count_nonzero(occupancy_view(grid) == -1) / len(grid.data) * 100) == 100.0 def test_grid_from_numpy_array() -> None: """Test creating a grid from a numpy array.""" data = np.zeros((20, 30), dtype=np.int8) - data[5:10, 10:20] = 100 # Add some obstacles - data[15:18, 5:8] = -1 # Add unknown area - - origin = Pose(1.0, 2.0, 0.0) - grid = OccupancyGrid(grid=data, resolution=0.05, origin=origin, frame_id="odom") - - assert grid.width == 30 - assert grid.height == 20 - assert grid.resolution == 0.05 - assert grid.frame_id == "odom" - assert grid.origin.position.x == 1.0 - assert grid.origin.position.y == 2.0 - assert grid.grid.shape == (20, 30) - - # Check cell counts - assert grid.occupied_cells == 50 # 5x10 obstacle area - assert grid.free_cells == 541 # Total - occupied - unknown - assert grid.unknown_cells == 9 # 3x3 unknown area - - # Check percentages (approximately) - assert abs(grid.occupied_percent - 8.33) < 0.1 - assert abs(grid.free_percent - 90.17) < 0.1 - assert abs(grid.unknown_percent - 1.5) < 0.1 + data[5:10, 10:20] = 100 + data[15:18, 5:8] = -1 + origin = Pose(position=Point(x=1, y=2)) + grid = occupancy_from_array( + data, resolution=0.05, origin=origin, header=Header(frame_id="odom") + ) + assert grid.info.width == 30 + assert grid.info.height == 20 + assert grid.info.resolution == pytest.approx(0.05) + assert grid.header.frame_id == "odom" + assert grid.info.origin.position.x == 1.0 + assert grid.info.origin.position.y == 2.0 + assert occupancy_view(grid).shape == (20, 30) + assert np.count_nonzero(occupancy_view(grid) == 100) == 50 + assert np.count_nonzero(occupancy_view(grid) == 0) == 541 + assert np.count_nonzero(occupancy_view(grid) == -1) == 9 + assert abs((np.count_nonzero(occupancy_view(grid) == 100) / len(grid.data) * 100) - 8.33) < 0.1 + assert abs((np.count_nonzero(occupancy_view(grid) == 0) / len(grid.data) * 100) - 90.17) < 0.1 + assert abs((np.count_nonzero(occupancy_view(grid) == -1) / len(grid.data) * 100) - 1.5) < 0.1 def test_world_grid_coordinate_conversion() -> None: """Test converting between world and grid coordinates.""" data = np.zeros((20, 30), dtype=np.int8) - origin = Pose(1.0, 2.0, 0.0) - grid = OccupancyGrid(grid=data, resolution=0.05, origin=origin, frame_id="odom") - - # Test world to grid - grid_pos = grid.world_to_grid((2.5, 3.0)) - assert int(grid_pos.x) == 30 - assert int(grid_pos.y) == 20 - - # Test grid to world - world_pos = grid.grid_to_world((10, 5)) - assert world_pos.x == 1.5 - assert world_pos.y == 2.25 + origin = Pose(position=Point(x=1, y=2)) + grid = occupancy_from_array( + data, resolution=0.05, origin=origin, header=Header(frame_id="odom") + ) + grid_pos = world_to_grid(grid, Point(x=2.5, y=3.0)) + assert grid_pos[0] == pytest.approx(30) + assert grid_pos[1] == pytest.approx(20) + world_pos = grid_to_world(grid, (10, 5)) + assert world_pos.x == pytest.approx(1.5) + assert world_pos.y == pytest.approx(2.25) -def test_lcm_encode_decode() -> None: +def test_cdr_encode_decode() -> None: """Test LCM encoding and decoding.""" data = np.zeros((20, 30), dtype=np.int8) - data[5:10, 10:20] = 100 # Add some obstacles - data[15:18, 5:8] = -1 # Add unknown area - origin = Pose(1.0, 2.0, 0.0) - grid = OccupancyGrid(grid=data, resolution=0.05, origin=origin, frame_id="odom") - - # Set a specific value for testing - # Convert world coordinates to grid indices - grid_pos = grid.world_to_grid((1.5, 2.25)) - grid.grid[int(grid_pos.y), int(grid_pos.x)] = 50 - - # Encode - lcm_data = grid.lcm_encode() + data[5:10, 10:20] = 100 + data[15:18, 5:8] = -1 + origin = Pose(position=Point(x=1, y=2)) + grid = occupancy_from_array( + data, resolution=0.05, origin=origin, header=Header(frame_id="odom") + ) + grid_pos = world_to_grid(grid, Point(x=1.5, y=2.25)) + grid.data[round(grid_pos[1]) * grid.info.width + round(grid_pos[0])] = 50 + lcm_data = grid.encode() assert isinstance(lcm_data, bytes) assert len(lcm_data) > 0 - - # Decode - decoded = OccupancyGrid.lcm_decode(lcm_data) - - # Check that data matches exactly (grid arrays should be identical) - assert np.array_equal(grid.grid, decoded.grid) - assert grid.width == decoded.width - assert grid.height == decoded.height - assert abs(grid.resolution - decoded.resolution) < 1e-6 # Use approximate equality for floats - assert abs(grid.origin.position.x - decoded.origin.position.x) < 1e-6 - assert abs(grid.origin.position.y - decoded.origin.position.y) < 1e-6 - assert grid.frame_id == decoded.frame_id - - # Check that the actual grid data was preserved (don't rely on float conversions) - assert decoded.grid[5, 10] == 50 # Value we set should be preserved in grid - - -def test_lcm_decode_origin_is_dimos_pose() -> None: - """The decoded origin must be the dimos Pose (with .yaw), not the raw - generated pose from the wire (the relay costmap encoder reads origin.yaw).""" - quat = Quaternion.from_euler(Vector3(0.0, 0.0, 0.5)) - origin = Pose(1.0, 2.0, 0.0, quat.x, quat.y, quat.z, quat.w) - grid = OccupancyGrid(grid=np.zeros((2, 3), dtype=np.int8), resolution=0.05, origin=origin) - - decoded = OccupancyGrid.lcm_decode(grid.lcm_encode()) - - assert isinstance(decoded.origin, Pose) - assert decoded.origin.yaw == pytest.approx(0.5) + decoded = OccupancyGrid.decode(lcm_data) + assert np.array_equal(occupancy_view(grid), occupancy_view(decoded)) + assert grid.info.width == decoded.info.width + assert grid.info.height == decoded.info.height + assert abs(grid.info.resolution - decoded.info.resolution) < 1e-06 + assert abs(grid.info.origin.position.x - decoded.info.origin.position.x) < 1e-06 + assert abs(grid.info.origin.position.y - decoded.info.origin.position.y) < 1e-06 + assert grid.header.frame_id == decoded.header.frame_id + assert occupancy_view(decoded)[5, 10] == 50 + + +def test_cdr_decode_origin_is_dimos_pose() -> None: + """Generated origin orientation works with the external yaw helper.""" + quat = quaternion_from_euler(0, 0, 0.5) + origin = Pose(position=Point(x=1, y=2), orientation=quat) + grid = occupancy_from_array( + np.zeros((2, 3), dtype=np.int8), resolution=0.05, origin=origin, header=Header(frame_id="") + ) + decoded = OccupancyGrid.decode(grid.encode()) + assert isinstance(decoded.info.origin, Pose) + assert yaw(decoded.info.origin.orientation) == pytest.approx(0.5) -def test_lcm_decode_empty_grid() -> None: +def test_cdr_decode_empty_grid() -> None: """An empty grid (mapper warming up) must survive the wire; the decoder used to rebuild it as a 1-D array the constructor rejects.""" - decoded = OccupancyGrid.lcm_decode(OccupancyGrid().lcm_encode()) - assert decoded.grid.size == 0 - assert isinstance(decoded.origin, Pose) + decoded = OccupancyGrid.decode(OccupancyGrid().encode()) + assert occupancy_view(decoded).size == 0 + assert isinstance(decoded.info.origin, Pose) -def test_string_representation() -> None: - """Test string representations.""" - grid = OccupancyGrid(width=10, height=10, resolution=0.1, frame_id="map") - - # Test __str__ - str_repr = str(grid) - assert "OccupancyGrid[map]" in str_repr - assert "10x10" in str_repr - assert "1.0x1.0m" in str_repr - assert "10cm res" in str_repr - - # Test __repr__ - repr_str = repr(grid) - assert "OccupancyGrid(" in repr_str - assert "width=10" in repr_str - assert "height=10" in repr_str - assert "resolution=0.1" in repr_str +def test_physical_extent() -> None: + grid = occupancy_from_array( + np.full((10, 10), -1, dtype=np.int8), header=Header(frame_id="map"), resolution=0.1 + ) + assert occupancy_extent(grid) == pytest.approx((1, 1)) def test_grid_property_sync() -> None: """Test that the grid property works correctly.""" - grid = OccupancyGrid(width=5, height=5, resolution=0.1, frame_id="map") - - # Modify via numpy array - grid.grid[2, 3] = 100 - assert grid.grid[2, 3] == 100 - - # Check that we can access grid values - grid.grid[0, 0] = 50 - assert grid.grid[0, 0] == 50 + grid = occupancy_from_array( + np.full((5, 5), -1, dtype=np.int8), resolution=0.1, header=Header(frame_id="map") + ) + grid.data[2 * grid.info.width + 3] = 100 + assert occupancy_view(grid)[2, 3] == 100 + grid.data[0] = 50 + assert occupancy_view(grid)[0, 0] == 50 def test_invalid_grid_dimensions() -> None: """Test handling of invalid grid dimensions.""" - # Test with non-2D array - with pytest.raises(ValueError, match="Grid must be a 2D array"): - OccupancyGrid(grid=np.zeros(10), resolution=0.1) + with pytest.raises(ValueError, match="2D integer array"): + occupancy_from_array(np.zeros(10), resolution=0.1, header=Header(frame_id="")) def test_from_pointcloud() -> None: """Test creating OccupancyGrid from PointCloud2.""" - file_path = get_data("lcm_msgs") / "sensor_msgs/PointCloud2.pickle" - with open(file_path, "rb") as f: - lcm_msg = pickle.loads(f.read()) - - pointcloud = PointCloud2.lcm_decode(lcm_msg) - - # Convert pointcloud to occupancy grid + x, y = np.meshgrid(np.arange(10) * 0.05, np.arange(10) * 0.05) + points = np.column_stack((x.ravel(), y.ravel(), np.full(x.size, 0.5))) + pointcloud = pointcloud_from_xyz(points, header=Header(frame_id="world")) occupancygrid = general_occupancy(pointcloud, resolution=0.05, min_height=0.1, max_height=2.0) - # Apply inflation separately if needed occupancygrid = simple_inflate(occupancygrid, 0.1) - - # Check that grid was created with reasonable properties - assert occupancygrid.width > 0 - assert occupancygrid.height > 0 - assert occupancygrid.resolution == 0.05 - assert occupancygrid.frame_id == pointcloud.frame_id - assert occupancygrid.occupied_cells > 0 # Should have some occupied cells - - -def test_gradient() -> None: - """Test converting occupancy grid to gradient field.""" - # Create a small test grid with an obstacle in the middle - data = np.zeros((10, 10), dtype=np.int8) - data[4:6, 4:6] = 100 # 2x2 obstacle in center - - grid = OccupancyGrid(grid=data, resolution=0.1) # 0.1m per cell - - # Convert to gradient - gradient_grid = gradient(grid, obstacle_threshold=50, max_distance=1.0) - - # Check that we get an OccupancyGrid back - assert isinstance(gradient_grid, OccupancyGrid) - assert gradient_grid.grid.shape == (10, 10) - assert gradient_grid.resolution == grid.resolution - assert gradient_grid.frame_id == grid.frame_id - - # Obstacle cells should have value 100 - assert gradient_grid.grid[4, 4] == 100 - assert gradient_grid.grid[5, 5] == 100 - - # Adjacent cells should have high values (near obstacles) - assert gradient_grid.grid[3, 4] > 85 # Very close to obstacle - assert gradient_grid.grid[4, 3] > 85 # Very close to obstacle - - # Cells at moderate distance should have moderate values - assert 30 < gradient_grid.grid[0, 0] < 60 # Corner is ~0.57m away - - # Check that gradient decreases with distance - assert gradient_grid.grid[3, 4] > gradient_grid.grid[2, 4] # Closer is higher - assert gradient_grid.grid[2, 4] > gradient_grid.grid[0, 4] # Further is lower - - # Test with unknown cells - data_with_unknown = data.copy() - data_with_unknown[0:2, 0:2] = -1 # Add unknown area (close to obstacle) - data_with_unknown[8:10, 8:10] = -1 # Add unknown area (far from obstacle) - - grid_with_unknown = OccupancyGrid(data_with_unknown, resolution=0.1) - gradient_with_unknown = gradient(grid_with_unknown, max_distance=1.0) # 1m max distance - - # Unknown cells should remain unknown (new behavior - unknowns are preserved) - assert gradient_with_unknown.grid[0, 0] == -1 # Should remain unknown - assert gradient_with_unknown.grid[1, 1] == -1 # Should remain unknown - assert gradient_with_unknown.grid[8, 8] == -1 # Should remain unknown - assert gradient_with_unknown.grid[9, 9] == -1 # Should remain unknown - - # Unknown cells count should be preserved - assert gradient_with_unknown.unknown_cells == 8 # All unknowns preserved + assert occupancygrid.info.width > 0 + assert occupancygrid.info.height > 0 + assert occupancygrid.info.resolution == pytest.approx(0.05) + assert occupancygrid.header.frame_id == pointcloud.header.frame_id + assert np.count_nonzero(occupancy_view(occupancygrid) == 100) > 0 def test_filter_above() -> None: """Test filtering cells above threshold.""" - # Create test grid with various values data = np.array( [[-1, 0, 20, 50], [10, 30, 60, 80], [40, 70, 90, 100], [-1, 15, 25, -1]], dtype=np.int8 ) - - grid = OccupancyGrid(grid=data, resolution=0.1) - - # Filter to keep only values > 50 - filtered = grid.filter_above(50) - - # Check that values > 50 are preserved - assert filtered.grid[1, 2] == 60 - assert filtered.grid[1, 3] == 80 - assert filtered.grid[2, 1] == 70 - assert filtered.grid[2, 2] == 90 - assert filtered.grid[2, 3] == 100 - - # Check that values <= 50 are set to -1 (unknown) - assert filtered.grid[0, 1] == -1 # was 0 - assert filtered.grid[0, 2] == -1 # was 20 - assert filtered.grid[0, 3] == -1 # was 50 - assert filtered.grid[1, 0] == -1 # was 10 - assert filtered.grid[1, 1] == -1 # was 30 - assert filtered.grid[2, 0] == -1 # was 40 - - # Check that unknown cells are preserved - assert filtered.grid[0, 0] == -1 - assert filtered.grid[3, 0] == -1 - assert filtered.grid[3, 3] == -1 - - # Check dimensions and metadata preserved - assert filtered.width == grid.width - assert filtered.height == grid.height - assert filtered.resolution == grid.resolution - assert filtered.frame_id == grid.frame_id + grid = occupancy_from_array(data, resolution=0.1, header=Header(frame_id="")) + filtered = occupancy_from_array( + np.where(occupancy_view(grid) > 50, occupancy_view(grid), -1).astype(np.int8), + header=grid.header, + resolution=grid.info.resolution, + origin=grid.info.origin, + ) + assert occupancy_view(filtered)[1, 2] == 60 + assert occupancy_view(filtered)[1, 3] == 80 + assert occupancy_view(filtered)[2, 1] == 70 + assert occupancy_view(filtered)[2, 2] == 90 + assert occupancy_view(filtered)[2, 3] == 100 + assert occupancy_view(filtered)[0, 1] == -1 + assert occupancy_view(filtered)[0, 2] == -1 + assert occupancy_view(filtered)[0, 3] == -1 + assert occupancy_view(filtered)[1, 0] == -1 + assert occupancy_view(filtered)[1, 1] == -1 + assert occupancy_view(filtered)[2, 0] == -1 + assert occupancy_view(filtered)[0, 0] == -1 + assert occupancy_view(filtered)[3, 0] == -1 + assert occupancy_view(filtered)[3, 3] == -1 + assert filtered.info.width == grid.info.width + assert filtered.info.height == grid.info.height + assert filtered.info.resolution == grid.info.resolution + assert filtered.header.frame_id == grid.header.frame_id def test_filter_below() -> None: """Test filtering cells below threshold.""" - # Create test grid with various values data = np.array( [[-1, 0, 20, 50], [10, 30, 60, 80], [40, 70, 90, 100], [-1, 15, 25, -1]], dtype=np.int8 ) - - grid = OccupancyGrid(grid=data, resolution=0.1) - - # Filter to keep only values < 50 - filtered = grid.filter_below(50) - - # Check that values < 50 are preserved - assert filtered.grid[0, 1] == 0 - assert filtered.grid[0, 2] == 20 - assert filtered.grid[1, 0] == 10 - assert filtered.grid[1, 1] == 30 - assert filtered.grid[2, 0] == 40 - assert filtered.grid[3, 1] == 15 - assert filtered.grid[3, 2] == 25 - - # Check that values >= 50 are set to -1 (unknown) - assert filtered.grid[0, 3] == -1 # was 50 - assert filtered.grid[1, 2] == -1 # was 60 - assert filtered.grid[1, 3] == -1 # was 80 - assert filtered.grid[2, 1] == -1 # was 70 - assert filtered.grid[2, 2] == -1 # was 90 - assert filtered.grid[2, 3] == -1 # was 100 - - # Check that unknown cells are preserved - assert filtered.grid[0, 0] == -1 - assert filtered.grid[3, 0] == -1 - assert filtered.grid[3, 3] == -1 - - # Check dimensions and metadata preserved - assert filtered.width == grid.width - assert filtered.height == grid.height - assert filtered.resolution == grid.resolution - assert filtered.frame_id == grid.frame_id + grid = occupancy_from_array(data, resolution=0.1, header=Header(frame_id="")) + filtered = occupancy_from_array( + np.where(occupancy_view(grid) < 50, occupancy_view(grid), -1).astype(np.int8), + header=grid.header, + resolution=grid.info.resolution, + origin=grid.info.origin, + ) + assert occupancy_view(filtered)[0, 1] == 0 + assert occupancy_view(filtered)[0, 2] == 20 + assert occupancy_view(filtered)[1, 0] == 10 + assert occupancy_view(filtered)[1, 1] == 30 + assert occupancy_view(filtered)[2, 0] == 40 + assert occupancy_view(filtered)[3, 1] == 15 + assert occupancy_view(filtered)[3, 2] == 25 + assert occupancy_view(filtered)[0, 3] == -1 + assert occupancy_view(filtered)[1, 2] == -1 + assert occupancy_view(filtered)[1, 3] == -1 + assert occupancy_view(filtered)[2, 1] == -1 + assert occupancy_view(filtered)[2, 2] == -1 + assert occupancy_view(filtered)[2, 3] == -1 + assert occupancy_view(filtered)[0, 0] == -1 + assert occupancy_view(filtered)[3, 0] == -1 + assert occupancy_view(filtered)[3, 3] == -1 + assert filtered.info.width == grid.info.width + assert filtered.info.height == grid.info.height + assert filtered.info.resolution == grid.info.resolution + assert filtered.header.frame_id == grid.header.frame_id def test_max() -> None: """Test setting all non-unknown cells to maximum.""" - # Create test grid with various values data = np.array( [[-1, 0, 20, 50], [10, 30, 60, 80], [40, 70, 90, 100], [-1, 15, 25, -1]], dtype=np.int8 ) - - grid = OccupancyGrid(grid=data, resolution=0.1) - - # Apply max - maxed = grid.max() - - # Check that all non-unknown cells are set to 100 - assert maxed.grid[0, 1] == 100 # was 0 - assert maxed.grid[0, 2] == 100 # was 20 - assert maxed.grid[0, 3] == 100 # was 50 - assert maxed.grid[1, 0] == 100 # was 10 - assert maxed.grid[1, 1] == 100 # was 30 - assert maxed.grid[1, 2] == 100 # was 60 - assert maxed.grid[1, 3] == 100 # was 80 - assert maxed.grid[2, 0] == 100 # was 40 - assert maxed.grid[2, 1] == 100 # was 70 - assert maxed.grid[2, 2] == 100 # was 90 - assert maxed.grid[2, 3] == 100 # was 100 (already max) - assert maxed.grid[3, 1] == 100 # was 15 - assert maxed.grid[3, 2] == 100 # was 25 - - # Check that unknown cells are preserved - assert maxed.grid[0, 0] == -1 - assert maxed.grid[3, 0] == -1 - assert maxed.grid[3, 3] == -1 - - # Check dimensions and metadata preserved - assert maxed.width == grid.width - assert maxed.height == grid.height - assert maxed.resolution == grid.resolution - assert maxed.frame_id == grid.frame_id - - # Verify statistics - assert maxed.unknown_cells == 3 # Same as original - assert maxed.occupied_cells == 13 # All non-unknown cells - assert maxed.free_cells == 0 # No free cells + grid = occupancy_from_array(data, resolution=0.1, header=Header(frame_id="")) + maxed = occupancy_from_array( + np.where(occupancy_view(grid) >= 0, 100, -1).astype(np.int8), + header=grid.header, + resolution=grid.info.resolution, + origin=grid.info.origin, + ) + assert occupancy_view(maxed)[0, 1] == 100 + assert occupancy_view(maxed)[0, 2] == 100 + assert occupancy_view(maxed)[0, 3] == 100 + assert occupancy_view(maxed)[1, 0] == 100 + assert occupancy_view(maxed)[1, 1] == 100 + assert occupancy_view(maxed)[1, 2] == 100 + assert occupancy_view(maxed)[1, 3] == 100 + assert occupancy_view(maxed)[2, 0] == 100 + assert occupancy_view(maxed)[2, 1] == 100 + assert occupancy_view(maxed)[2, 2] == 100 + assert occupancy_view(maxed)[2, 3] == 100 + assert occupancy_view(maxed)[3, 1] == 100 + assert occupancy_view(maxed)[3, 2] == 100 + assert occupancy_view(maxed)[0, 0] == -1 + assert occupancy_view(maxed)[3, 0] == -1 + assert occupancy_view(maxed)[3, 3] == -1 + assert maxed.info.width == grid.info.width + assert maxed.info.height == grid.info.height + assert maxed.info.resolution == grid.info.resolution + assert maxed.header.frame_id == grid.header.frame_id + assert np.count_nonzero(occupancy_view(maxed) == -1) == 3 + assert np.count_nonzero(occupancy_view(maxed) == 100) == 13 + assert np.count_nonzero(occupancy_view(maxed) == 0) == 0 def test_block_max_reduce_preserves_lone_obstacle() -> None: @@ -408,10 +305,9 @@ def test_block_max_reduce_unknown_only_when_whole_block_unknown() -> None: def test_block_max_reduce_trims_remainder() -> None: cells = np.arange(35, dtype=np.int8).reshape(7, 5) reduced = block_max_reduce(cells, 2) - # 7x5 at factor 2 keeps rows 0-5 and cols 0-3 (origin corner side). assert reduced.shape == (3, 2) - assert reduced[0, 0] == 6 # max of rows 0-1, cols 0-1 - assert reduced[2, 1] == 28 # max of rows 4-5, cols 2-3 + assert reduced[0, 0] == 6 + assert reduced[2, 1] == 28 def test_block_max_reduce_thin_grid_passes_through() -> None: diff --git a/dimos/msgs/nav_msgs/test_Odometry.py b/dimos/msgs/nav_msgs/test_Odometry.py index 9a0e77c5587..73af9d94e05 100644 --- a/dimos/msgs/nav_msgs/test_Odometry.py +++ b/dimos/msgs/nav_msgs/test_Odometry.py @@ -14,50 +14,60 @@ import time +from dimos_generated.geometry_msgs.msg import ( + Point, + Pose, + PoseWithCovariance, + Quaternion, + Twist, + TwistWithCovariance, + Vector3, +) +from dimos_generated.nav_msgs.msg import Odometry +from dimos_generated.std_msgs.msg import Header import numpy as np +from rosbags.typesys import Stores, get_typestore -from dimos.msgs.geometry_msgs.Pose import Pose -from dimos.msgs.geometry_msgs.PoseWithCovariance import PoseWithCovariance -from dimos.msgs.geometry_msgs.Twist import Twist -from dimos.msgs.geometry_msgs.TwistWithCovariance import TwistWithCovariance -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.msgs.nav_msgs.Odometry import Odometry +from dimos.msgs.geometry import quaternion_euler +from dimos.msgs.time import header_now, time_from_seconds, to_seconds def test_odometry_default_init() -> None: odom = Odometry() - - assert odom.ts > 0 - assert odom.frame_id == "" + assert to_seconds(odom.header.stamp) == 0 + assert odom.header.frame_id == "" assert odom.child_frame_id == "" - - assert odom.pose.position.x == 0.0 - assert odom.pose.position.y == 0.0 - assert odom.pose.position.z == 0.0 - assert odom.pose.orientation.w == 1.0 - - assert odom.twist.linear.x == 0.0 - assert odom.twist.angular.x == 0.0 - - assert np.all(odom.pose.covariance == 0.0) - assert np.all(odom.twist.covariance == 0.0) + assert odom.pose.pose.position.x == 0.0 + assert odom.pose.pose.position.y == 0.0 + assert odom.pose.pose.position.z == 0.0 + assert odom.pose.pose.orientation.w == 1.0 + assert odom.twist.twist.linear.x == 0.0 + assert odom.twist.twist.angular.x == 0.0 + assert np.all(np.asarray(odom.pose.covariance) == 0.0) + assert np.all(np.asarray(odom.twist.covariance) == 0.0) def test_odometry_with_frames() -> None: ts = 1234567890.123456 - odom = Odometry(ts=ts, frame_id="odom", child_frame_id="base_link") - - assert odom.ts == ts - assert odom.frame_id == "odom" + odom = Odometry( + child_frame_id="base_link", header=Header(stamp=time_from_seconds(ts), frame_id="odom") + ) + assert to_seconds(odom.header.stamp) == ts + assert odom.header.frame_id == "odom" assert odom.child_frame_id == "base_link" def test_odometry_with_pose_and_twist() -> None: - pose = Pose(1.0, 2.0, 3.0, 0.1, 0.2, 0.3, 0.9) - twist = Twist(Vector3(0.5, 0.0, 0.0), Vector3(0.0, 0.0, 0.1)) - - odom = Odometry(ts=1000.0, frame_id="odom", child_frame_id="base_link", pose=pose, twist=twist) - + pose = Pose( + position=Point(x=1.0, y=2.0, z=3.0), orientation=Quaternion(x=0.1, y=0.2, z=0.3, w=0.9) + ) + twist = Twist(linear=Vector3(x=0.5, y=0.0, z=0.0), angular=Vector3(x=0.0, y=0.0, z=0.1)) + odom = Odometry( + child_frame_id="base_link", + pose=PoseWithCovariance(pose=pose), + twist=TwistWithCovariance(twist=twist), + header=Header(stamp=time_from_seconds(1000.0), frame_id="odom"), + ) assert odom.pose.pose.position.x == 1.0 assert odom.pose.pose.position.y == 2.0 assert odom.pose.pose.position.z == 3.0 @@ -66,145 +76,149 @@ def test_odometry_with_pose_and_twist() -> None: def test_odometry_with_covariances() -> None: - pose = Pose(1.0, 2.0, 3.0) + pose = Pose(position=Point(x=1.0, y=2.0, z=3.0)) pose_cov = np.arange(36, dtype=float) - pose_with_cov = PoseWithCovariance(pose, pose_cov) - - twist = Twist(Vector3(0.5, 0.0, 0.0), Vector3(0.0, 0.0, 0.1)) + pose_with_cov = PoseWithCovariance(pose=pose, covariance=pose_cov) + twist = Twist(linear=Vector3(x=0.5, y=0.0, z=0.0), angular=Vector3(x=0.0, y=0.0, z=0.1)) twist_cov = np.arange(36, 72, dtype=float) - twist_with_cov = TwistWithCovariance(twist, twist_cov) - + twist_with_cov = TwistWithCovariance(twist=twist, covariance=twist_cov) odom = Odometry( - ts=1000.0, - frame_id="odom", child_frame_id="base_link", pose=pose_with_cov, twist=twist_with_cov, + header=Header(stamp=time_from_seconds(1000.0), frame_id="odom"), ) - - assert odom.pose.position.x == 1.0 + assert odom.pose.pose.position.x == 1.0 assert np.array_equal(odom.pose.covariance, pose_cov) - assert odom.twist.linear.x == 0.5 + assert odom.twist.twist.linear.x == 0.5 assert np.array_equal(odom.twist.covariance, twist_cov) def test_odometry_properties() -> None: - pose = Pose(1.0, 2.0, 3.0, 0.1, 0.2, 0.3, 0.9) - twist = Twist(Vector3(0.5, 0.6, 0.7), Vector3(0.1, 0.2, 0.3)) - - odom = Odometry(ts=1000.0, frame_id="odom", child_frame_id="base_link", pose=pose, twist=twist) - - assert odom.x == 1.0 - assert odom.y == 2.0 - assert odom.z == 3.0 - assert odom.position.x == 1.0 - - assert odom.orientation.x == 0.1 - - assert odom.vx == 0.5 - assert odom.vy == 0.6 - assert odom.vz == 0.7 - assert odom.linear_velocity.x == 0.5 - - assert odom.wx == 0.1 - assert odom.wy == 0.2 - assert odom.wz == 0.3 - assert odom.angular_velocity.x == 0.1 - - assert odom.roll == pose.roll - assert odom.pitch == pose.pitch - assert odom.yaw == pose.yaw - - -def test_odometry_str_repr() -> None: + pose = Pose( + position=Point(x=1.0, y=2.0, z=3.0), orientation=Quaternion(x=0.1, y=0.2, z=0.3, w=0.9) + ) + twist = Twist(linear=Vector3(x=0.5, y=0.6, z=0.7), angular=Vector3(x=0.1, y=0.2, z=0.3)) odom = Odometry( - ts=1234567890.123456, - frame_id="odom", child_frame_id="base_link", - pose=Pose(1.234, 2.567, 3.891), - twist=Twist(Vector3(0.5, 0.0, 0.0), Vector3(0.0, 0.0, 0.1)), + pose=PoseWithCovariance(pose=pose), + twist=TwistWithCovariance(twist=twist), + header=Header(stamp=time_from_seconds(1000.0), frame_id="odom"), ) + assert odom.pose.pose.position.x == 1.0 + assert odom.pose.pose.position.y == 2.0 + assert odom.pose.pose.position.z == 3.0 + assert odom.pose.pose.position.x == 1.0 + assert odom.pose.pose.orientation.x == 0.1 + assert odom.twist.twist.linear.x == 0.5 + assert odom.twist.twist.linear.y == 0.6 + assert odom.twist.twist.linear.z == 0.7 + assert odom.twist.twist.linear.x == 0.5 + assert odom.twist.twist.angular.x == 0.1 + assert odom.twist.twist.angular.y == 0.2 + assert odom.twist.twist.angular.z == 0.3 + assert odom.twist.twist.angular.x == 0.1 + assert quaternion_euler(odom.pose.pose.orientation)[0] == quaternion_euler(pose.orientation)[0] + assert quaternion_euler(odom.pose.pose.orientation)[1] == quaternion_euler(pose.orientation)[1] + assert quaternion_euler(odom.pose.pose.orientation)[2] == quaternion_euler(pose.orientation)[2] - assert "Odometry" in repr(odom) - assert "1234567890.123456" in repr(odom) - s = str(odom) - assert "odom -> base_link" in s - assert "1.234" in s +def test_independent_ros_decoding() -> None: + source = Odometry( + header=Header(stamp=time_from_seconds(1000), frame_id="odom"), + child_frame_id="base_link", + pose=PoseWithCovariance(pose=Pose(position=Point(x=1.234, y=2.567, z=3.891))), + twist=TwistWithCovariance(twist=Twist(linear=Vector3(x=0.5), angular=Vector3(z=0.1))), + ) + decoded = get_typestore(Stores.ROS2_JAZZY).deserialize_cdr(source.encode(), Odometry.msg_name) + assert decoded.header.frame_id == "odom" + assert decoded.child_frame_id == "base_link" + assert ( + decoded.pose.pose.position.x, + decoded.pose.pose.position.y, + decoded.pose.pose.position.z, + ) == (1.234, 2.567, 3.891) + assert decoded.twist.twist.linear.x == 0.5 + assert decoded.twist.twist.angular.z == 0.1 def test_odometry_equality() -> None: kwargs = dict( - ts=1000.0, - frame_id="odom", child_frame_id="base_link", - pose=Pose(1.0, 2.0, 3.0), - twist=Twist(Vector3(0.5, 0.0, 0.0), Vector3(0.0, 0.0, 0.1)), + pose=PoseWithCovariance(pose=Pose(position=Point(x=1.0, y=2.0, z=3.0))), + twist=TwistWithCovariance( + twist=Twist(linear=Vector3(x=0.5, y=0.0, z=0.0), angular=Vector3(x=0.0, y=0.0, z=0.1)) + ), + header=Header(stamp=time_from_seconds(1000.0), frame_id="odom"), ) - assert Odometry(**kwargs) == Odometry(**kwargs) - assert Odometry(**kwargs) != Odometry(**{**kwargs, "pose": Pose(1.1, 2.0, 3.0)}) + assert Odometry(**kwargs) != Odometry( + **{**kwargs, "pose": PoseWithCovariance(pose=Pose(position=Point(x=1.1, y=2.0, z=3.0)))} + ) assert Odometry(**kwargs) != "not an odometry" -def test_odometry_lcm_roundtrip() -> None: - pose = Pose(1.0, 2.0, 3.0, 0.1, 0.2, 0.3, 0.9) +def test_odometry_cdr_roundtrip() -> None: + pose = Pose( + position=Point(x=1.0, y=2.0, z=3.0), orientation=Quaternion(x=0.1, y=0.2, z=0.3, w=0.9) + ) pose_cov = np.arange(36, dtype=float) - twist = Twist(Vector3(0.5, 0.6, 0.7), Vector3(0.1, 0.2, 0.3)) + twist = Twist(linear=Vector3(x=0.5, y=0.6, z=0.7), angular=Vector3(x=0.1, y=0.2, z=0.3)) twist_cov = np.arange(36, 72, dtype=float) - source = Odometry( - ts=1234567890.123456, - frame_id="odom", child_frame_id="base_link", - pose=PoseWithCovariance(pose, pose_cov), - twist=TwistWithCovariance(twist, twist_cov), + pose=PoseWithCovariance(pose=pose, covariance=pose_cov), + twist=TwistWithCovariance(twist=twist, covariance=twist_cov), + header=Header(stamp=time_from_seconds(1234567890.123456), frame_id="odom"), ) - - decoded = Odometry.lcm_decode(source.lcm_encode()) - - assert abs(decoded.ts - source.ts) < 1e-6 - assert decoded.frame_id == source.frame_id + decoded = Odometry.decode(source.encode()) + assert abs(to_seconds(decoded.header.stamp) - to_seconds(source.header.stamp)) < 1e-06 + assert decoded.header.frame_id == source.header.frame_id assert decoded.child_frame_id == source.child_frame_id assert decoded.pose == source.pose assert decoded.twist == source.twist def test_odometry_zero_timestamp() -> None: - """An explicit zero timestamp is kept; an omitted one stamps now.""" - assert Odometry(ts=0.0).ts == 0.0 - + assert to_seconds(Odometry().header.stamp) == 0 before = time.time() - assert before <= Odometry().ts <= time.time() + assert before <= to_seconds(Odometry(header=header_now()).header.stamp) <= time.time() def test_odometry_with_just_pose() -> None: - odom = Odometry(pose=Pose(1.0, 2.0, 3.0)) - - assert odom.pose.position.x == 1.0 - assert np.all(odom.pose.covariance == 0.0) - assert np.all(odom.twist.covariance == 0.0) + odom = Odometry(pose=PoseWithCovariance(pose=Pose(position=Point(x=1.0, y=2.0, z=3.0)))) + assert odom.pose.pose.position.x == 1.0 + assert np.all(np.asarray(odom.pose.covariance) == 0.0) + assert np.all(np.asarray(odom.twist.covariance) == 0.0) def test_odometry_with_just_twist() -> None: - odom = Odometry(twist=Twist(Vector3(0.5, 0.0, 0.0), Vector3(0.0, 0.0, 0.1))) - - assert odom.twist.linear.x == 0.5 - assert odom.twist.angular.z == 0.1 - assert np.all(odom.twist.covariance == 0.0) + odom = Odometry( + twist=TwistWithCovariance( + twist=Twist(linear=Vector3(x=0.5, y=0.0, z=0.0), angular=Vector3(x=0.0, y=0.0, z=0.1)) + ) + ) + assert odom.twist.twist.linear.x == 0.5 + assert odom.twist.twist.angular.z == 0.1 + assert np.all(np.asarray(odom.twist.covariance) == 0.0) def test_odometry_typical_robot_scenario() -> None: odom = Odometry( - ts=1000.0, - frame_id="odom", child_frame_id="base_footprint", - pose=Pose(10.0, 5.0, 0.0, 0.0, 0.0, np.sin(0.1), np.cos(0.1)), - twist=Twist(Vector3(0.5, 0.0, 0.0), Vector3(0.0, 0.0, 0.05)), + pose=PoseWithCovariance( + pose=Pose( + position=Point(x=10.0, y=5.0, z=0.0), + orientation=Quaternion(x=0.0, y=0.0, z=np.sin(0.1), w=np.cos(0.1)), + ) + ), + twist=TwistWithCovariance( + twist=Twist(linear=Vector3(x=0.5, y=0.0, z=0.0), angular=Vector3(x=0.0, y=0.0, z=0.05)) + ), + header=Header(stamp=time_from_seconds(1000.0), frame_id="odom"), ) - - assert odom.x == 10.0 - assert odom.y == 5.0 - assert abs(odom.yaw - 0.2) < 0.01 - assert odom.vx == 0.5 - assert odom.wz == 0.05 + assert odom.pose.pose.position.x == 10.0 + assert odom.pose.pose.position.y == 5.0 + assert abs(quaternion_euler(odom.pose.pose.orientation)[2] - 0.2) < 0.01 + assert odom.twist.twist.linear.x == 0.5 + assert odom.twist.twist.angular.z == 0.05 diff --git a/dimos/msgs/nav_msgs/test_Path.py b/dimos/msgs/nav_msgs/test_Path.py index 9bd0cc92b6b..bd68698c5fc 100644 --- a/dimos/msgs/nav_msgs/test_Path.py +++ b/dimos/msgs/nav_msgs/test_Path.py @@ -13,35 +13,37 @@ # limitations under the License. -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.nav_msgs.Path import Path +from dimos_generated.geometry_msgs.msg import Point, Pose, PoseStamped, Quaternion +from dimos_generated.nav_msgs.msg import Path +from dimos_generated.std_msgs.msg import Header +from rosbags.typesys import Stores, get_typestore + +from dimos.msgs.time import time_from_nanoseconds def create_test_pose(x: float, y: float, z: float, frame_id: str = "map") -> PoseStamped: """Helper to create a test PoseStamped.""" return PoseStamped( - frame_id=frame_id, - position=[x, y, z], - orientation=Quaternion(0, 0, 0, 1), # Identity quaternion + header=Header(frame_id=frame_id), + pose=Pose(position=Point(x=x, y=y, z=z), orientation=Quaternion(w=1)), ) def test_init_empty() -> None: """Test creating an empty path.""" - path = Path(frame_id="map") - assert path.frame_id == "map" - assert len(path) == 0 - assert not path # Should be falsy when empty + path = Path(header=Header(frame_id="map")) + assert path.header.frame_id == "map" + assert len(path.poses) == 0 + assert not list(path.poses) # Should be falsy when empty assert path.poses == [] def test_init_with_poses() -> None: """Test creating a path with initial poses.""" poses = [create_test_pose(i, i, 0) for i in range(3)] - path = Path(frame_id="map", poses=poses) - assert len(path) == 3 - assert bool(path) # Should be truthy when has poses + path = Path(header=Header(frame_id="map"), poses=poses) + assert len(path.poses) == 3 + assert bool(list(path.poses)) # Should be truthy when has poses assert path.poses == poses @@ -49,74 +51,74 @@ def test_head() -> None: """Test getting the first pose.""" poses = [create_test_pose(i, i, 0) for i in range(3)] path = Path(poses=poses) - assert path.head() == poses[0] + assert (path.poses[0] if path.poses else None) == poses[0] # Test empty path empty_path = Path() - assert empty_path.head() is None + assert (empty_path.poses[0] if empty_path.poses else None) is None def test_last() -> None: """Test getting the last pose.""" poses = [create_test_pose(i, i, 0) for i in range(3)] path = Path(poses=poses) - assert path.last() == poses[-1] + assert (path.poses[-1] if path.poses else None) == poses[-1] # Test empty path empty_path = Path() - assert empty_path.last() is None + assert (empty_path.poses[-1] if empty_path.poses else None) is None def test_tail() -> None: """Test getting all poses except the first.""" poses = [create_test_pose(i, i, 0) for i in range(3)] path = Path(poses=poses) - tail = path.tail() - assert len(tail) == 2 + tail = Path(header=path.header, poses=list(path.poses)[1:]) + assert len(tail.poses) == 2 assert tail.poses == poses[1:] - assert tail.frame_id == path.frame_id + assert tail.header.frame_id == path.header.frame_id # Test single element path single_path = Path(poses=[poses[0]]) - assert len(single_path.tail()) == 0 + assert len(list(single_path.poses)[1:]) == 0 # Test empty path empty_path = Path() - assert len(empty_path.tail()) == 0 + assert len(list(empty_path.poses)[1:]) == 0 def test_push_immutable() -> None: """Test immutable push operation.""" - path = Path(frame_id="map") + path = Path(header=Header(frame_id="map")) pose1 = create_test_pose(1, 1, 0) pose2 = create_test_pose(2, 2, 0) # Push should return new path - path2 = path.push(pose1) - assert len(path) == 0 # Original unchanged - assert len(path2) == 1 + path2 = Path(header=path.header, poses=[*path.poses, pose1]) + assert len(path.poses) == 0 # Original unchanged + assert len(path2.poses) == 1 assert path2.poses[0] == pose1 # Chain pushes - path3 = path2.push(pose2) - assert len(path2) == 1 # Previous unchanged - assert len(path3) == 2 + path3 = Path(header=path2.header, poses=[*path2.poses, pose2]) + assert len(path2.poses) == 1 # Previous unchanged + assert len(path3.poses) == 2 assert path3.poses == [pose1, pose2] def test_push_mutable() -> None: """Test mutable push operation.""" - path = Path(frame_id="map") + path = Path(header=Header(frame_id="map")) pose1 = create_test_pose(1, 1, 0) pose2 = create_test_pose(2, 2, 0) # Push should modify in place - path.push_mut(pose1) - assert len(path) == 1 + path.poses.append(pose1) + assert len(path.poses) == 1 assert path.poses[0] == pose1 - path.push_mut(pose2) - assert len(path) == 2 + path.poses.append(pose2) + assert len(path.poses) == 2 assert path.poses == [pose1, pose2] @@ -126,13 +128,13 @@ def test_indexing() -> None: path = Path(poses=poses) # Single index - assert path[0] == poses[0] - assert path[-1] == poses[-1] + assert next(iter(path.poses)) == poses[0] + assert list(path.poses)[-1] == poses[-1] # Slicing - assert path[1:3] == poses[1:3] - assert path[:2] == poses[:2] - assert path[3:] == poses[3:] + assert list(path.poses)[1:3] == poses[1:3] + assert list(path.poses)[:2] == poses[:2] + assert list(path.poses)[3:] == poses[3:] def test_iteration() -> None: @@ -141,7 +143,7 @@ def test_iteration() -> None: path = Path(poses=poses) collected = [] - for pose in path: + for pose in path.poses: collected.append(pose) assert collected == poses @@ -149,15 +151,15 @@ def test_iteration() -> None: def test_slice_method() -> None: """Test slice method.""" poses = [create_test_pose(i, i, 0) for i in range(5)] - path = Path(frame_id="map", poses=poses) + path = Path(header=Header(frame_id="map"), poses=poses) - sliced = path.slice(1, 4) - assert len(sliced) == 3 + sliced = Path(header=path.header, poses=list(path.poses)[1:4]) + assert len(sliced.poses) == 3 assert sliced.poses == poses[1:4] - assert sliced.frame_id == "map" + assert sliced.header.frame_id == "map" # Test open-ended slice - sliced2 = path.slice(2) + sliced2 = Path(header=path.header, poses=list(path.poses)[2:]) assert sliced2.poses == poses[2:] @@ -166,14 +168,14 @@ def test_extend_immutable() -> None: poses1 = [create_test_pose(i, i, 0) for i in range(2)] poses2 = [create_test_pose(i + 2, i + 2, 0) for i in range(2)] - path1 = Path(frame_id="map", poses=poses1) - path2 = Path(frame_id="odom", poses=poses2) + path1 = Path(header=Header(frame_id="map"), poses=poses1) + path2 = Path(header=Header(frame_id="odom"), poses=poses2) - extended = path1.extend(path2) - assert len(path1) == 2 # Original unchanged - assert len(extended) == 4 + extended = Path(header=path1.header, poses=[*path1.poses, *path2.poses]) + assert len(path1.poses) == 2 # Original unchanged + assert len(extended.poses) == 4 assert extended.poses == poses1 + poses2 - assert extended.frame_id == "map" # Keeps first path's frame + assert extended.header.frame_id == "map" # Keeps first path's frame def test_extend_mutable() -> None: @@ -181,16 +183,18 @@ def test_extend_mutable() -> None: poses1 = [create_test_pose(i, i, 0) for i in range(2)] poses2 = [create_test_pose(i + 2, i + 2, 0) for i in range(2)] - path1 = Path(frame_id="map", poses=poses1.copy()) # Use copy to avoid modifying original - path2 = Path(frame_id="odom", poses=poses2) + path1 = Path( + header=Header(frame_id="map"), poses=poses1.copy() + ) # Use copy to avoid modifying original + path2 = Path(header=Header(frame_id="odom"), poses=poses2) - path1.extend_mut(path2) - assert len(path1) == 4 + path1.poses.extend(path2.poses) + assert len(path1.poses) == 4 # Check poses are the same as concatenation for _i, (p1, p2) in enumerate(zip(path1.poses, poses1 + poses2, strict=False)): - assert p1.x == p2.x - assert p1.y == p2.y - assert p1.z == p2.z + assert p1.pose.position.x == p2.pose.position.x + assert p1.pose.position.y == p2.pose.position.y + assert p1.pose.position.z == p2.pose.position.z def test_reverse() -> None: @@ -198,8 +202,8 @@ def test_reverse() -> None: poses = [create_test_pose(i, i, 0) for i in range(3)] path = Path(poses=poses) - reversed_path = path.reverse() - assert len(path) == 3 # Original unchanged + reversed_path = Path(header=path.header, poses=list(reversed(list(path.poses)))) + assert len(path.poses) == 3 # Original unchanged assert reversed_path.poses == list(reversed(poses)) @@ -208,80 +212,56 @@ def test_clear() -> None: poses = [create_test_pose(i, i, 0) for i in range(3)] path = Path(poses=poses) - path.clear() - assert len(path) == 0 + path.poses.clear() + assert len(path.poses) == 0 assert path.poses == [] -def test_lcm_encode_decode() -> None: - """Test encoding and decoding of Path to/from binary LCM format.""" - # Create path with poses - # Use timestamps that can be represented exactly in float64 - path_ts = 1234567890.5 +def test_cdr_roundtrip_preserves_per_pose_frames_and_stamps() -> None: poses = [ PoseStamped( - ts=1234567890.0 + i * 0.1, # Use simpler timestamps - frame_id=f"frame_{i}", - position=[i * 1.5, i * 2.5, i * 3.5], - orientation=(0.1 * i, 0.2 * i, 0.3 * i, 0.9), + header=Header( + stamp=time_from_nanoseconds(1234567890000000000 + i * 100000000), + frame_id=f"frame_{i}", + ), + pose=Pose( + position=Point(x=i * 1.5, y=i * 2.5, z=i * 3.5), + orientation=Quaternion(x=0.1 * i, y=0.2 * i, z=0.3 * i, w=0.9), + ), ) for i in range(3) ] - - path_source = Path(ts=path_ts, frame_id="world", poses=poses) - - # Encode to binary - binary_msg = path_source.lcm_encode() - - # Decode from binary - path_dest = Path.lcm_decode(binary_msg) - - assert isinstance(path_dest, Path) - assert path_dest is not path_source - - # Check header - assert path_dest.frame_id == path_source.frame_id - # Path timestamp should be preserved - assert abs(path_dest.ts - path_source.ts) < 1e-6 # Microsecond precision - - # Check poses - assert len(path_dest.poses) == len(path_source.poses) - - for orig, decoded in zip(path_source.poses, path_dest.poses, strict=False): - # Check pose timestamps - assert abs(decoded.ts - orig.ts) < 1e-6 - # All poses should have the path's frame_id - assert decoded.frame_id == path_dest.frame_id - - # Check position - assert decoded.x == orig.x - assert decoded.y == orig.y - assert decoded.z == orig.z - - # Check orientation - assert decoded.orientation.x == orig.orientation.x - assert decoded.orientation.y == orig.orientation.y - assert decoded.orientation.z == orig.orientation.z - assert decoded.orientation.w == orig.orientation.w - - -def test_lcm_encode_decode_empty() -> None: - """Test encoding and decoding of empty Path.""" - path_source = Path(frame_id="base_link") - - binary_msg = path_source.lcm_encode() - path_dest = Path.lcm_decode(binary_msg) - - assert isinstance(path_dest, Path) - assert path_dest.frame_id == path_source.frame_id - assert len(path_dest.poses) == 0 - - -def test_str_representation() -> None: - """Test string representation.""" - path = Path(frame_id="map") - assert str(path) == "Path(frame_id='map', poses=0)" - - path.push_mut(create_test_pose(1, 1, 0)) - path.push_mut(create_test_pose(2, 2, 0)) - assert str(path) == "Path(frame_id='map', poses=2)" + source = Path( + header=Header(stamp=time_from_nanoseconds(1234567890500000000), frame_id="world"), + poses=poses, + ) + decoded = Path.decode(source.encode()) + assert decoded is not source + assert decoded.header == source.header + assert list(decoded.poses) == poses + independent = get_typestore(Stores.ROS2_JAZZY).deserialize_cdr(source.encode(), Path.msg_name) + assert independent.header.frame_id == "world" + assert independent.header.stamp.sec == 1234567890 + assert independent.header.stamp.nanosec == 500000000 + for i, value in enumerate(independent.poses): + assert value.header.frame_id == f"frame_{i}" + assert value.header.stamp.sec == 1234567890 + assert value.header.stamp.nanosec == i * 100000000 + assert (value.pose.position.x, value.pose.position.y, value.pose.position.z) == ( + i * 1.5, + i * 2.5, + i * 3.5, + ) + assert ( + value.pose.orientation.x, + value.pose.orientation.y, + value.pose.orientation.z, + value.pose.orientation.w, + ) == (0.1 * i, 0.2 * i, 0.3 * i, 0.9) + + +def test_cdr_empty_path() -> None: + source = Path(header=Header(frame_id="base_link")) + decoded = Path.decode(source.encode()) + assert decoded.header.frame_id == "base_link" + assert len(decoded.poses) == 0 diff --git a/dimos/msgs/occupancy.py b/dimos/msgs/occupancy.py new file mode 100644 index 00000000000..773c34fe137 --- /dev/null +++ b/dimos/msgs/occupancy.py @@ -0,0 +1,132 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Array operations on generated occupancy grids.""" + +import math +from pathlib import Path +from typing import Any + +from dimos_generated.geometry_msgs.msg import Point, Pose, Quaternion +from dimos_generated.nav_msgs.msg import MapMetaData, OccupancyGrid +from dimos_generated.std_msgs.msg import Header +import numpy as np +from numpy.typing import NDArray + +from dimos.msgs.geometry import pose_matrix + + +def occupancy_view(msg: OccupancyGrid) -> NDArray[np.int8]: + """Borrow a read-only row-major occupancy grid, retaining its owner.""" + if len(msg.data) != msg.info.height * msg.info.width: + raise ValueError("occupancy data length does not match dimensions") + return msg.data.view().reshape(msg.info.height, msg.info.width) + + +def block_max_reduce(cells: NDArray[np.int8], factor: int) -> NDArray[np.int8]: + """Coarsen an occupancy grid by taking the max of factor x factor blocks. + + Block maximum (not mean) so coarsening never erases an obstacle; a block + is unknown (-1) only when every cell in it is unknown. Trailing remainder + rows/cols are trimmed, keeping row 0/col 0 - the origin corner - so the + grid origin stays valid. Grids thinner than `factor` pass through. + """ + h, w = cells.shape[:2] + new_h, new_w = h // factor, w // factor + if new_h == 0 or new_w == 0: + return cells + trimmed = cells[: new_h * factor, : new_w * factor] + blocks = trimmed.reshape(new_h, factor, new_w, factor) + # Sink unknown below every known value for the max, then map it back. + as_int = blocks.astype(np.int16) + known = np.where(as_int < 0, -1000, as_int) + reduced = known.max(axis=(1, 3)) + reduced[reduced == -1000] = -1 + result: NDArray[np.int8] = reduced.astype(np.int8) + return result + + +def occupancy_extent(message: OccupancyGrid) -> tuple[float, float]: + """Return physical width and height, rejecting an invalid grid resolution.""" + info = message.info + if not math.isfinite(info.resolution) or info.resolution <= 0: + raise ValueError("OccupancyGrid resolution must be finite and positive") + return info.width * info.resolution, info.height * info.resolution + + +def world_to_grid(message: OccupancyGrid, point: Point) -> tuple[float, float]: + """Convert a world point to continuous grid coordinates, including origin rotation.""" + occupancy_extent(message) + matrix = pose_matrix(message.info.origin) + local = matrix[:3, :3].T @ (np.array([point.x, point.y, point.z]) - matrix[:3, 3]) + if not np.isfinite(local).all(): + raise ValueError("grid coordinates must be finite") + return float(local[0] / message.info.resolution), float(local[1] / message.info.resolution) + + +def grid_to_world(message: OccupancyGrid, coordinates: tuple[float, float]) -> Point: + """Convert continuous grid coordinates to the grid plane in world coordinates.""" + occupancy_extent(message) + x, y = coordinates + if not math.isfinite(x) or not math.isfinite(y): + raise ValueError("grid coordinates must be finite") + result = pose_matrix(message.info.origin) @ np.array( + [x * message.info.resolution, y * message.info.resolution, 0.0, 1.0] + ) + return Point(x=float(result[0]), y=float(result[1]), z=float(result[2])) + + +def occupancy_from_file(path: Path, *, header: Header, resolution: float = 0.05) -> OccupancyGrid: + """Load a static .npy or grayscale PNG grid with explicit frame/time metadata. + + Cells must be ROS occupancy values (-1 unknown, 0..100 known); PNG 255 is + unknown. NumPy object/pickle loading is disabled. Row order is unchanged. + """ + if not math.isfinite(resolution) or resolution <= 0: + raise ValueError("OccupancyGrid resolution must be finite and positive") + cells: NDArray[Any] + if path.suffix == ".npy": + cells = np.load(path, allow_pickle=False) + elif path.suffix == ".png": + from PIL import Image + + with Image.open(path) as image: + cells = np.asarray(image.convert("L")).astype(np.int16) + cells[cells == 255] = -1 + else: + raise ValueError(f"Unsupported occupancy file format: {path.suffix}") + return occupancy_from_array(cells, header=header, resolution=resolution) + + +def occupancy_from_array( + cells: NDArray[Any], *, header: Header, resolution: float, origin: Pose | None = None +) -> OccupancyGrid: + """Copy a 2D ROS occupancy array with explicit map metadata.""" + if not math.isfinite(resolution) or resolution <= 0: + raise ValueError("OccupancyGrid resolution must be finite and positive") + if cells.ndim != 2 or cells.dtype.kind not in "iu": + raise ValueError("Occupancy cells must be a 2D integer array") + if np.any((cells < -1) | (cells > 100)): + raise ValueError("Occupancy cells must be -1 or 0..100") + return OccupancyGrid( + header=header, + info=MapMetaData( + map_load_time=header.stamp, + resolution=resolution, + height=cells.shape[0], + width=cells.shape[1], + origin=origin if origin is not None else Pose(orientation=Quaternion(w=1)), + ), + data=cells.astype(np.int8).reshape(-1), + ) diff --git a/dimos/msgs/pointcloud.py b/dimos/msgs/pointcloud.py new file mode 100644 index 00000000000..aeb81a58a56 --- /dev/null +++ b/dimos/msgs/pointcloud.py @@ -0,0 +1,348 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +# Copyright 2026 Dimensional Inc. +# SPDX-License-Identifier: Apache-2.0 + +"""NumPy conversions for generated PointCloud2 messages.""" + +import math +from typing import Any + +from dimos_generated.geometry_msgs.msg import TransformStamped +from dimos_generated.sensor_msgs.msg import CameraInfo, Image, PointCloud2, PointField +from dimos_generated.std_msgs.msg import Header +import numpy as np +from numpy.typing import NDArray + +from dimos.msgs.camera_info import intrinsic_matrix +from dimos.msgs.geometry import transform_matrix +from dimos.msgs.image import image_to_rgb, image_view +from dimos.msgs.time import to_nanoseconds + +_FIELD_KINDS = {1: "i1", 2: "u1", 3: "i2", 4: "u2", 5: "i4", 6: "u4", 7: "f4", 8: "f8"} + + +def pointcloud_view(message: PointCloud2) -> NDArray[Any]: + """Borrow read-only structured (height, width) points, including row/field padding. + + Fields retain their declared counts and endianness. The returned array keeps + the message's storage alive. Use ``view.copy()`` for independent mutable data. + """ + if message.width and not message.point_step: + raise ValueError("nonempty point cloud requires a positive point_step") + if message.row_step < message.width * message.point_step: + raise ValueError("point cloud row_step is smaller than its row of points") + if len(message.data) != message.height * message.row_step: + raise ValueError("point cloud data length does not match height * row_step") + names: list[str] = [] + formats: list[Any] = [] + offsets: list[int] = [] + for field in message.fields: + if not field.name or field.name in names: + raise ValueError("point cloud field names must be nonempty and unique") + if field.datatype not in _FIELD_KINDS or not field.count: + raise ValueError(f"invalid point cloud datatype/count for {field.name!r}") + dtype = np.dtype((">" if message.is_bigendian else "<") + _FIELD_KINDS[field.datatype]) + if field.offset + dtype.itemsize * field.count > message.point_step: + raise ValueError(f"point cloud field {field.name!r} exceeds point_step") + names.append(field.name) + formats.append(dtype if field.count == 1 else (dtype, (field.count,))) + offsets.append(field.offset) + dtype = np.dtype( + {"names": names, "formats": formats, "offsets": offsets, "itemsize": message.point_step} + ) + return np.ndarray( + (message.height, message.width), + dtype=dtype, + buffer=message.data.view(), + strides=(message.row_step, message.point_step), + ) + + +def pointcloud_xyz(message: PointCloud2) -> NDArray[np.float64]: + """Copy scalar x/y/z fields into an N-by-3 float64 array, preserving point order.""" + points = pointcloud_view(message) + for name in ("x", "y", "z"): + if name not in (points.dtype.names or ()) or points[name].ndim != 2: + raise ValueError(f"point cloud requires a scalar {name!r} field") + return np.stack([points[name].ravel() for name in ("x", "y", "z")], axis=-1).astype(np.float64) + + +def pointcloud_from_xyz(points: NDArray[Any], *, header: Header) -> PointCloud2: + """Copy Nx3 or HxWx3 coordinates into a tightly packed little-endian XYZ cloud. + + XYZ fields are float32. NaNs/infinities remain missing-point markers and set + is_dense=False; finite coordinates outside float32 range are rejected. + """ + values = np.asarray(points) + if values.ndim not in (2, 3) or values.shape[-1] != 3: + raise ValueError("XYZ coordinates must have shape (N, 3) or (H, W, 3)") + if values.dtype.kind not in "fiu": + raise ValueError("XYZ coordinates must be numeric") + finite = np.isfinite(values) + if np.any(np.abs(values[finite].astype(np.float64)) > np.finfo(np.float32).max): + raise ValueError("XYZ coordinates exceed float32 range") + packed = np.ascontiguousarray(values, dtype=" PointCloud2: + """Copy selected point records in row order, retaining every field and point byte. + + The result is unorganized with no row padding. Point padding, endianness, + field metadata, source header, and the conservative density flag survive. + """ + pointcloud_view(message) # Validate the full declared layout before selecting bytes. + if keep.dtype != np.bool_ or keep.shape != (message.height * message.width,): + raise ValueError("point selection must be a flat boolean mask matching the point count") + records: NDArray[np.uint8] = np.ndarray( + (message.height, message.width, message.point_step), + dtype=np.uint8, + buffer=message.data.view(), + strides=(message.row_step, message.point_step, 1), + ) + selected = records.reshape(message.height * message.width, message.point_step)[keep] + width = len(selected) + return PointCloud2( + header=message.header, + height=1, + width=width, + fields=message.fields, + is_bigendian=message.is_bigendian, + point_step=message.point_step, + row_step=width * message.point_step, + data=selected.tobytes(), + is_dense=message.is_dense, + ) + + +def pointcloud_rgb(message: PointCloud2) -> NDArray[np.uint8] | None: + """Copy RGB bytes from the standard packed rgb/rgba field, if present. + + FLOAT32 fields carry color bits, not numeric floating-point color values. + Both representations preserve the declared byte order and row padding. + """ + points = pointcloud_view(message) + names = points.dtype.names or () + name = next((name for name in ("rgb", "rgba") if name in names), None) + if name is None: + return None + field = points[name] + if field.ndim != 2 or field.dtype.kind not in "fu" or field.dtype.itemsize != 4: + raise ValueError("packed point cloud color must be a scalar FLOAT32 or UINT32 field") + packed = field.view(np.dtype(">u4" if message.is_bigendian else "> shift) & 255 for shift in (16, 8, 0)], axis=-1).astype(np.uint8) + + +def concatenate_clouds(first: PointCloud2, second: PointCloud2) -> PointCloud2: + """Join compatible point records, preserving fields, endian, and point padding. + + Row padding is removed. The first frame and newest exact source stamp survive. + Empty inputs are identities; differing nonempty frames/layouts are rejected. + """ + pointcloud_view(first) + pointcloud_view(second) + if not first.width * first.height: + return PointCloud2.decode(second.encode()) + if not second.width * second.height: + return PointCloud2.decode(first.encode()) + if first.header.frame_id != second.header.frame_id: + raise ValueError("cannot concatenate point clouds in different frames") + if ( + first.fields != second.fields + or first.point_step != second.point_step + or first.is_bigendian != second.is_bigendian + ): + raise ValueError("cannot concatenate point clouds with different record layouts") + a = select_points(first, np.ones(first.width * first.height, dtype=np.bool_)) + b = select_points(second, np.ones(second.width * second.height, dtype=np.bool_)) + stamp = max((first.header.stamp, second.header.stamp), key=to_nanoseconds) + return PointCloud2( + header=Header(frame_id=first.header.frame_id, stamp=stamp), + height=1, + width=a.width + b.width, + fields=first.fields, + is_bigendian=first.is_bigendian, + point_step=first.point_step, + row_step=a.row_step + b.row_step, + data=bytes(a.data) + bytes(b.data), + is_dense=first.is_dense and second.is_dense, + ) + + +def transform_cloud(message: PointCloud2, transform: TransformStamped) -> PointCloud2: + """Apply parent←child to XYZ while retaining the complete organized record layout. + + Copy storage before changing coordinates. Keep all other fields, point/row + padding, endian and the cloud's exact source stamp. Frames must match; + integer XYZ is rejected because transformed coordinates would be truncated. + """ + if message.header.frame_id != transform.child_frame_id: + raise ValueError("point cloud frame does not match transform child frame") + view = pointcloud_view(message) + for name in ("x", "y", "z"): + if name not in (view.dtype.names or ()) or view.dtype[name].kind != "f": + raise ValueError("transform requires scalar floating-point XYZ fields") + matrix = transform_matrix(transform.transform) + xyz = pointcloud_xyz(message) + transformed = xyz @ matrix[:3, :3].T + matrix[:3, 3] + for index, name in enumerate(("x", "y", "z")): + values = transformed[:, index] + if np.any(np.abs(values[np.isfinite(values)]) > np.finfo(view.dtype[name]).max): + raise ValueError(f"transformed {name} coordinates exceed field range") + payload = bytearray(bytes(message.data)) + writable = np.ndarray( + (message.height, message.width), + dtype=view.dtype, + buffer=payload, + strides=(message.row_step, message.point_step), + ) + for index, name in enumerate(("x", "y", "z")): + writable[name] = transformed[:, index].reshape(message.height, message.width) + output = PointCloud2.decode(message.encode()) + output.data = bytes(payload) + output.header = Header(stamp=message.header.stamp, frame_id=transform.header.frame_id) + return output + + +def pointcloud_from_xyz_rgb( + points: NDArray[Any], colors: NDArray[np.uint8], *, header: Header +) -> PointCloud2: + """Copy XYZ and RGB8 into a declared XYZ FLOAT32 / packed RGB UINT32 layout.""" + cloud = pointcloud_from_xyz(points, header=header) + rgb = np.asarray(colors) + if rgb.dtype != np.uint8 or rgb.shape != np.asarray(points).shape: + raise ValueError("RGB must be uint8 with the same (..., 3) shape as XYZ") + records = np.empty((cloud.height, cloud.width), dtype=[("xyz", " PointCloud2: + """Project a rectified RGB/depth pair into the depth optical frame. + + Integer depth is multiplied by depth_scale to obtain meters; 32FC1 already + represents meters. Invalid/nonpositive/too-distant samples are removed. + Scale K when calibration resolution differs, retaining the depth header. + """ + if ( + not math.isfinite(depth_scale) + or depth_scale <= 0 + or not math.isfinite(depth_trunc) + or depth_trunc <= 0 + ): + raise ValueError("depth scale and truncation must be finite and positive") + if (color.width, color.height) != (depth.width, depth.height): + raise ValueError("color and depth dimensions do not match") + if depth.encoding not in ("16UC1", "mono16", "32FC1"): + raise ValueError("RGBD requires uint16 or float32 depth") + if calibration.width <= 0 or calibration.height <= 0: + raise ValueError("calibration dimensions must be positive") + matrix = intrinsic_matrix(calibration).copy() + matrix[0, :] *= depth.width / calibration.width + matrix[1, :] *= depth.height / calibration.height + distances = image_view(depth).astype(np.float64) + if depth.encoding != "32FC1": + distances *= depth_scale + valid = np.isfinite(distances) & (distances > 0) & (distances < depth_trunc) + rows, columns = np.indices(distances.shape) + z = distances[valid] + xyz = np.column_stack( + ( + (columns[valid] - matrix[0, 2]) * z / matrix[0, 0], + (rows[valid] - matrix[1, 2]) * z / matrix[1, 1], + z, + ) + ) + return pointcloud_from_xyz_rgb(xyz, image_to_rgb(color)[valid], header=depth.header) + + +def voxel_downsample_cloud(message: PointCloud2, voxel_size: float = 0.025) -> PointCloud2: + """Downsample XYZ/RGB clouds with the existing Open3D tensor backend. + + Non-positive sizes and clouds below twenty points retain the existing fast + path. Unsupported extra fields are rejected rather than silently dropped. + Source headers are preserved; generated messages own no numerical methods. + """ + if not math.isfinite(voxel_size): + raise ValueError("voxel_size must be finite") + if voxel_size <= 0 or message.width * message.height < 20: + return message + names = {field.name for field in message.fields} + if names not in ({"x", "y", "z"}, {"x", "y", "z", "rgb"}): + raise ValueError("voxel downsampling supports only XYZ and optional RGB fields") + points = pointcloud_xyz(message).astype(np.float32) + if not np.isfinite(points).all(): + raise ValueError("voxel downsampling requires finite XYZ positions") + colors = pointcloud_rgb(message) + import open3d as o3d # type: ignore[import-untyped] + + cloud = o3d.t.geometry.PointCloud(o3d.core.Tensor(points)) + if colors is not None: + cloud.point["colors"] = o3d.core.Tensor(colors.astype(np.float32) / 255.0) + reduced = cloud.voxel_down_sample(voxel_size) + xyz = reduced.point["positions"].numpy() + if colors is None: + return pointcloud_from_xyz(xyz, header=message.header) + rgb = np.clip(reduced.point["colors"].numpy() * 255.0, 0, 255).astype(np.uint8) + return pointcloud_from_xyz_rgb(xyz, rgb, header=message.header) + + +def pointcloud_to_open3d(message: PointCloud2) -> Any: + """Copy generated XYZ/RGB values into a native geometry algorithm input.""" + import open3d as o3d # type: ignore[import-untyped] + + result = o3d.geometry.PointCloud() + result.points = o3d.utility.Vector3dVector(pointcloud_xyz(message)) + colors = pointcloud_rgb(message) + if colors is not None: + result.colors = o3d.utility.Vector3dVector(colors.astype(np.float64) / 255.0) + return result + + +def cloud_bounds_intersect(first: PointCloud2, second: PointCloud2) -> bool: + """Test closed axis-aligned bounds; empty or entirely nonfinite clouds do not intersect.""" + a, b = pointcloud_xyz(first), pointcloud_xyz(second) + a, b = a[np.isfinite(a).all(axis=1)], b[np.isfinite(b).all(axis=1)] + if len(a) == 0 or len(b) == 0: + return False + return bool(np.all(a.min(axis=0) <= b.max(axis=0)) and np.all(a.max(axis=0) >= b.min(axis=0))) diff --git a/dimos/msgs/protocol.py b/dimos/msgs/protocol.py index 38d7ca57e29..11e218e9009 100644 --- a/dimos/msgs/protocol.py +++ b/dimos/msgs/protocol.py @@ -12,20 +12,23 @@ # See the License for the specific language governing permissions and # limitations under the License. -from typing import Protocol, runtime_checkable +from typing import ClassVar, Protocol, runtime_checkable + +from typing_extensions import Self @runtime_checkable class DimosMsg(Protocol): - """Protocol for dimos message types (LCM-based messages from dimos.msgs).""" + """Transport-neutral contract implemented by generated CDR messages.""" - msg_name: str + msg_name: ClassVar[str] + schema: ClassVar[str] @classmethod - def lcm_decode(cls, data: bytes) -> "DimosMsg": + def decode(cls, data: bytes) -> Self: """Decode bytes into a message instance.""" ... - def lcm_encode(self) -> bytes: + def encode(self, little_endian: bool = True) -> bytes: """Encode this message instance into bytes.""" ... diff --git a/dimos/msgs/sensor_msgs/CameraInfo.py b/dimos/msgs/sensor_msgs/CameraInfo.py deleted file mode 100644 index ac2d38032f0..00000000000 --- a/dimos/msgs/sensor_msgs/CameraInfo.py +++ /dev/null @@ -1,539 +0,0 @@ -# Copyright 2025-2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from __future__ import annotations - -import math -import time -from typing import TYPE_CHECKING, Any - -if TYPE_CHECKING: - from dimos.visualization.rerun.bridge import RerunData - -# Import LCM types -from dimos_lcm.sensor_msgs import CameraInfo as LCMCameraInfo -from dimos_lcm.std_msgs.Header import Header -import numpy as np - -from dimos.types.timestamped import Timestamped - - -class CameraInfo(Timestamped): - """Camera calibration information message.""" - - msg_name = "sensor_msgs.CameraInfo" - - def __init__( - self, - height: int = 0, - width: int = 0, - distortion_model: str = "", - D: list[float] | None = None, - K: list[float] | None = None, - R: list[float] | None = None, - P: list[float] | None = None, - binning_x: int = 0, - binning_y: int = 0, - frame_id: str = "", - ts: float | None = None, - ) -> None: - """Initialize CameraInfo. - - Args: - height: Image height - width: Image width - distortion_model: Name of distortion model (e.g., "plumb_bob") - D: Distortion coefficients - K: 3x3 intrinsic camera matrix - R: 3x3 rectification matrix - P: 3x4 projection matrix - binning_x: Horizontal binning - binning_y: Vertical binning - frame_id: Frame ID - ts: Timestamp - """ - self.ts = ts if ts is not None else time.time() - self.frame_id = frame_id - self.height = height - self.width = width - self.distortion_model = distortion_model - - # Initialize distortion coefficients - self.D = D if D is not None else [] - - # Initialize 3x3 intrinsic camera matrix (row-major) - self.K = K if K is not None else [0.0] * 9 - - # Initialize 3x3 rectification matrix (row-major) - self.R = R if R is not None else [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0] - - # Initialize 3x4 projection matrix (row-major) - self.P = P if P is not None else [0.0] * 12 - - self.binning_x = binning_x - self.binning_y = binning_y - - # Region of interest (not used in basic implementation) - self.roi_x_offset = 0 - self.roi_y_offset = 0 - self.roi_height = 0 - self.roi_width = 0 - self.roi_do_rectify = False - - @classmethod - def from_intrinsics( - cls, - fx: float, - fy: float, - cx: float, - cy: float, - width: int, - height: int, - frame_id: str = "", - ) -> CameraInfo: - """Create CameraInfo from pinhole intrinsics (no distortion). - - Args: - fx: Focal length x (pixels) - fy: Focal length y (pixels) - cx: Principal point x (pixels) - cy: Principal point y (pixels) - width: Image width - height: Image height - frame_id: Frame ID - """ - return cls( - height=height, - width=width, - distortion_model="plumb_bob", - D=[0.0, 0.0, 0.0, 0.0, 0.0], - K=[fx, 0.0, cx, 0.0, fy, cy, 0.0, 0.0, 1.0], - P=[fx, 0.0, cx, 0.0, 0.0, fy, cy, 0.0, 0.0, 0.0, 1.0, 0.0], - frame_id=frame_id, - ) - - @classmethod - def from_fov( - cls, - fov_deg: float, - width: int, - height: int, - axis: str = "vertical", - frame_id: str = "", - ) -> CameraInfo: - """Create CameraInfo from field of view and image dimensions. - - Args: - fov_deg: Field of view in degrees along ``axis`` - width: Image width (pixels) - height: Image height (pixels) - axis: Which image axis ``fov_deg`` refers to ("vertical" or "horizontal") - frame_id: Frame ID - """ - fov_rad = math.radians(fov_deg) - if axis == "vertical": - f = (height / 2) / math.tan(fov_rad / 2) - elif axis == "horizontal": - f = (width / 2) / math.tan(fov_rad / 2) - else: - raise ValueError(f"axis must be 'vertical' or 'horizontal', got {axis!r}") - return cls.from_intrinsics( - fx=f, - fy=f, - cx=width / 2.0, - cy=height / 2.0, - width=width, - height=height, - frame_id=frame_id, - ) - - def with_ts(self, ts: float) -> CameraInfo: - """Return a copy of this CameraInfo with the given timestamp. - - Args: - ts: New timestamp - - Returns: - New CameraInfo instance with updated timestamp - """ - return CameraInfo( - height=self.height, - width=self.width, - distortion_model=self.distortion_model, - D=self.D.copy(), - K=self.K.copy(), - R=self.R.copy(), - P=self.P.copy(), - binning_x=self.binning_x, - binning_y=self.binning_y, - frame_id=self.frame_id, - ts=ts, - ) - - @classmethod - def from_yaml(cls, yaml_file: str, frame_id: str = "camera_optical") -> CameraInfo: - """Create CameraInfo from YAML file. - - Args: - yaml_file: Path to YAML file containing camera calibration data - frame_id: Optical frame the intrinsics apply to; must match the - ``Image.frame_id`` published for that camera - - Returns: - CameraInfo instance with loaded calibration data - """ - import yaml - - with open(yaml_file) as f: - data = yaml.safe_load(f) - - # Extract basic parameters - width = data.get("image_width", 0) - height = data.get("image_height", 0) - distortion_model = data.get("distortion_model", "") - - # Extract matrices - camera_matrix = data.get("camera_matrix", {}) - K = camera_matrix.get("data", [0.0] * 9) - - distortion_coeffs = data.get("distortion_coefficients", {}) - D = distortion_coeffs.get("data", []) - - rect_matrix = data.get("rectification_matrix", {}) - R = rect_matrix.get("data", [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]) - - proj_matrix = data.get("projection_matrix", {}) - P = proj_matrix.get("data", [0.0] * 12) - - # Create CameraInfo instance - return cls( - height=height, - width=width, - distortion_model=distortion_model, - D=D, - K=K, - R=R, - P=P, - frame_id=frame_id, - ) - - def get_K_matrix(self) -> np.ndarray: - """Get intrinsic matrix as numpy array.""" - return np.array(self.K, dtype=np.float64).reshape(3, 3) - - def get_P_matrix(self) -> np.ndarray: - """Get projection matrix as numpy array.""" - return np.array(self.P, dtype=np.float64).reshape(3, 4) - - def get_R_matrix(self) -> np.ndarray: - """Get rectification matrix as numpy array.""" - return np.array(self.R, dtype=np.float64).reshape(3, 3) - - def get_D_coeffs(self) -> np.ndarray: - """Get distortion coefficients as numpy array.""" - return np.array(self.D, dtype=np.float64) - - def set_K_matrix(self, K: np.ndarray): # type: ignore[no-untyped-def] - """Set intrinsic matrix from numpy array.""" - if K.shape != (3, 3): - raise ValueError(f"K matrix must be 3x3, got {K.shape}") - self.K = K.flatten().tolist() - - def set_P_matrix(self, P: np.ndarray): # type: ignore[no-untyped-def] - """Set projection matrix from numpy array.""" - if P.shape != (3, 4): - raise ValueError(f"P matrix must be 3x4, got {P.shape}") - self.P = P.flatten().tolist() - - def set_R_matrix(self, R: np.ndarray): # type: ignore[no-untyped-def] - """Set rectification matrix from numpy array.""" - if R.shape != (3, 3): - raise ValueError(f"R matrix must be 3x3, got {R.shape}") - self.R = R.flatten().tolist() - - def set_D_coeffs(self, D: np.ndarray) -> None: - """Set distortion coefficients from numpy array.""" - self.D = D.flatten().tolist() - - def lcm_encode(self) -> bytes: - """Convert to LCM CameraInfo message.""" - msg = LCMCameraInfo() - - # Header - msg.header = Header() - msg.header.seq = 0 - msg.header.frame_id = self.frame_id - msg.header.stamp.sec = int(self.ts) - msg.header.stamp.nsec = int((self.ts - int(self.ts)) * 1e9) - - # Image dimensions - msg.height = self.height - msg.width = self.width - - # Distortion model - msg.distortion_model = self.distortion_model - - # Distortion coefficients - msg.D_length = len(self.D) - msg.D = self.D - - # Camera matrices (all stored as row-major) - msg.K = self.K - msg.R = self.R - msg.P = self.P - - # Binning - msg.binning_x = self.binning_x - msg.binning_y = self.binning_y - - # ROI - msg.roi.x_offset = self.roi_x_offset - msg.roi.y_offset = self.roi_y_offset - msg.roi.height = self.roi_height - msg.roi.width = self.roi_width - msg.roi.do_rectify = self.roi_do_rectify - - return msg.lcm_encode() # type: ignore[no-any-return] - - @classmethod - def lcm_decode(cls, data: bytes) -> CameraInfo: - """Decode from LCM CameraInfo bytes.""" - msg = LCMCameraInfo.lcm_decode(data) - - # Extract timestamp - ts = msg.header.stamp.sec + msg.header.stamp.nsec / 1e9 if hasattr(msg, "header") else None - - camera_info = cls( - height=msg.height, - width=msg.width, - distortion_model=msg.distortion_model, - D=list(msg.D) if msg.D_length > 0 else [], - K=list(msg.K), - R=list(msg.R), - P=list(msg.P), - binning_x=msg.binning_x, - binning_y=msg.binning_y, - frame_id=msg.header.frame_id if hasattr(msg, "header") else "", - ts=ts, - ) - - # Set ROI if present - if hasattr(msg, "roi"): - camera_info.roi_x_offset = msg.roi.x_offset - camera_info.roi_y_offset = msg.roi.y_offset - camera_info.roi_height = msg.roi.height - camera_info.roi_width = msg.roi.width - camera_info.roi_do_rectify = msg.roi.do_rectify - - return camera_info - - def __repr__(self) -> str: - """String representation.""" - return ( - f"CameraInfo(height={self.height}, width={self.width}, " - f"distortion_model='{self.distortion_model}', " - f"frame_id='{self.frame_id}', ts={self.ts})" - ) - - def __str__(self) -> str: - """Human-readable string.""" - return ( - f"CameraInfo:\n" - f" Resolution: {self.width}x{self.height}\n" - f" Distortion model: {self.distortion_model}\n" - f" Frame ID: {self.frame_id}\n" - f" Binning: {self.binning_x}x{self.binning_y}" - ) - - def __eq__(self, other) -> bool: # type: ignore[no-untyped-def] - """Check if two CameraInfo messages are equal.""" - if not isinstance(other, CameraInfo): - return False - - return ( - self.height == other.height - and self.width == other.width - and self.distortion_model == other.distortion_model - and self.D == other.D - and self.K == other.K - and self.R == other.R - and self.P == other.P - and self.binning_x == other.binning_x - and self.binning_y == other.binning_y - and self.frame_id == other.frame_id - ) - - def to_rerun( - self, - image_plane_distance: float = 1.0, - # These are defaults for a typical RGB camera with a known transform - # - # TODO this should be done by the actual emitting modules, - # they know the camera image topic, spatial relationships etc - # - # Poor CameraInfo class has no idea on this - # We just provide the parameters here for convenience in case your - # module doesn't implement this correctly - image_topic: str | None = None, - optical_frame: str | None = None, - ) -> RerunData: - """Convert to Rerun Pinhole archetype for camera frustum visualization. - - Args: - image_plane_distance: Distance to draw the image plane in the frustum - - Returns: - rr.Pinhole archetype for logging to Rerun - """ - import rerun as rr - - # Extract intrinsics from K matrix - # K = [fx, 0, cx, 0, fy, cy, 0, 0, 1] - fx, fy = self.K[0], self.K[4] - cx, cy = self.K[2], self.K[5] - - pinhole_kwargs: dict[str, Any] = dict( - focal_length=[fx, fy], - principal_point=[cx, cy], - width=self.width, - height=self.height, - image_plane_distance=image_plane_distance, - ) - - # If no image topic is specified, We don't know which Image this CameraInfo refers to - # return just the pinhole - if not image_topic: - return rr.Pinhole(**pinhole_kwargs) - - return [(image_topic, self.to_rerun_pinhole(image_plane_distance, optical_frame))] - - def to_rerun_pinhole( - self, image_plane_distance: float = 1.0, optical_frame: str | None = None - ) -> Any: - """The Pinhole to log on this camera's image entity, parented to its optical frame.""" - import rerun as rr - - fx, fy = self.K[0], self.K[4] - cx, cy = self.K[2], self.K[5] - # Re-parent the image entity here rather than with a separate - # Transform3D, which would give it a second parent and lose the frustum. - frame = optical_frame or self.frame_id - return rr.Pinhole( - focal_length=[fx, fy], - principal_point=[cx, cy], - width=self.width, - height=self.height, - image_plane_distance=image_plane_distance, - parent_frame=f"tf#/{frame}" if frame else None, - ) - - -class CalibrationProvider: - """Provides lazy-loaded access to camera calibration YAML files in a directory.""" - - def __init__(self, calibration_dir) -> None: # type: ignore[no-untyped-def] - """Initialize with a directory containing calibration YAML files. - - Args: - calibration_dir: Path to directory containing .yaml calibration files - """ - from pathlib import Path - - self._calibration_dir = Path(calibration_dir) - self._cache = {} # type: ignore[var-annotated] - - def _to_snake_case(self, name: str) -> str: - """Convert PascalCase to snake_case.""" - import re - - # Insert underscore before capital letters (except first char) - s1 = re.sub("(.)([A-Z][a-z]+)", r"\1_\2", name) - # Insert underscore before capital letter followed by lowercase - return re.sub("([a-z0-9])([A-Z])", r"\1_\2", s1).lower() - - def _find_yaml_file(self, name: str): # type: ignore[no-untyped-def] - """Find YAML file matching the given name (tries both snake_case and exact match). - - Args: - name: Attribute name to look for - - Returns: - Path to YAML file if found, None otherwise - """ - # Try exact match first - yaml_file = self._calibration_dir / f"{name}.yaml" - if yaml_file.exists(): - return yaml_file - - # Try snake_case conversion for PascalCase names - snake_name = self._to_snake_case(name) - if snake_name != name: - yaml_file = self._calibration_dir / f"{snake_name}.yaml" - if yaml_file.exists(): - return yaml_file - - return None - - def __getattr__(self, name: str) -> CameraInfo: - """Load calibration YAML file on first access. - - Supports both snake_case and PascalCase attribute names. - For example, both 'single_webcam' and 'SingleWebcam' will load 'single_webcam.yaml'. - - Args: - name: Attribute name (can be PascalCase or snake_case) - - Returns: - CameraInfo object loaded from the YAML file - - Raises: - AttributeError: If no matching YAML file exists - """ - # Check cache first - if name in self._cache: - return self._cache[name] # type: ignore[no-any-return] - - # Also check if the snake_case version is cached (for PascalCase access) - snake_name = self._to_snake_case(name) - if snake_name != name and snake_name in self._cache: - return self._cache[snake_name] # type: ignore[no-any-return] - - # Find matching YAML file - yaml_file = self._find_yaml_file(name) - if not yaml_file: - raise AttributeError(f"No calibration file found for: {name}") - - # Load and cache the CameraInfo - camera_info = CameraInfo.from_yaml(str(yaml_file)) - - # Cache both the requested name and the snake_case version - self._cache[name] = camera_info - if snake_name != name: - self._cache[snake_name] = camera_info - - return camera_info - - def __dir__(self): # type: ignore[no-untyped-def] - """List available calibrations in both snake_case and PascalCase.""" - calibrations = [] - if self._calibration_dir.exists() and self._calibration_dir.is_dir(): - yaml_files = self._calibration_dir.glob("*.yaml") - for f in yaml_files: - stem = f.stem - calibrations.append(stem) - # Add PascalCase version - pascal = "".join(word.capitalize() for word in stem.split("_")) - if pascal != stem: - calibrations.append(pascal) - return calibrations diff --git a/dimos/msgs/sensor_msgs/CompressedImage.py b/dimos/msgs/sensor_msgs/CompressedImage.py deleted file mode 100644 index f1853986fc5..00000000000 --- a/dimos/msgs/sensor_msgs/CompressedImage.py +++ /dev/null @@ -1,175 +0,0 @@ -# Copyright 2025-2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from __future__ import annotations - -from dataclasses import dataclass, field -import time -from typing import TYPE_CHECKING, Any, Literal - -from dimos_lcm.sensor_msgs.CompressedImage import CompressedImage as LCMCompressedImage -from dimos_lcm.std_msgs.Header import Header - -from dimos.types.timestamped import Timestamped, to_human_readable - -# cv2/turbojpeg/Image are imported lazily so this module stays cheap for -# byte-level consumers that never encode or decode pixels. -if TYPE_CHECKING: - from dimos.msgs.sensor_msgs.Image import Image - -CompressionFormat = Literal["jpeg", "png", "jxl"] - - -@dataclass -class CompressedImage(Timestamped): - """Compressed image bytes (JPEG/PNG/JPEG-XL) — ROS sensor_msgs/CompressedImage.""" - - msg_name = "sensor_msgs.CompressedImage" - - data: bytes = b"" - format: str = "jpeg" - frame_id: str = "" - ts: float = field(default_factory=time.time) - - def __str__(self) -> str: - return ( - f"CompressedImage(format={self.format}, bytes={len(self.data)}, " - f"ts={to_human_readable(self.ts)})" - ) - - @classmethod - def from_image( - cls, - image: Image, - format: CompressionFormat = "jpeg", - quality: int = 75, - max_width: int | None = None, - effort: int | None = None, - ) -> CompressedImage: - """Encode a raw Image. - - JPEG rejects 16-bit/depth formats; PNG rejects float depth. JXL takes - everything: lossless for 16-bit/float data (depth must survive the wire - exactly), `quality` for uint8. `effort` (jxl only, 1-9) trades encode - cpu for size; defaults are pinned for camera-rate streaming. - """ - from dimos.msgs.sensor_msgs.Image import Image, ImageFormat - - if not isinstance(image, Image): - raise TypeError(f"from_image expects Image, got {type(image).__name__}") - if max_width is not None: - image, _ = image.resize_to_fit(max_width, max_width) - if format == "jpeg": - if image.format in (ImageFormat.GRAY16, ImageFormat.DEPTH, ImageFormat.DEPTH16): - raise ValueError(f"JPEG cannot encode {image.format.value}; use format='png'") - data = image.to_jpeg_bytes(quality=quality) - elif format == "png": - import cv2 - - if image.format in (ImageFormat.DEPTH, ImageFormat.DEPTH16): - raise ValueError(f"PNG cannot encode {image.format.value}") - arr = image.data if image.channels == 1 else image.to_bgr().data - ok, buf = cv2.imencode(".png", arr) - if not ok: - raise ValueError("PNG encoding failed") - data = buf.tobytes() - elif format == "jxl": - import imagecodecs - import numpy as np - - # zero-copy passthrough unless channel order actually needs fixing - if image.channels == 1 or image.format is ImageFormat.RGB: - arr = image.data - else: - arr = image.to_rgb().data - if arr.dtype not in (np.uint8, np.uint16, np.float32): - raise ValueError(f"JXL cannot encode dtype {arr.dtype}") - arr = np.ascontiguousarray(arr) - # libjxl's default effort=7 costs 83-300ms per 720p frame; these - # pins keep camera-rate (13ms lossy / 3ms lossless) for ~5% size - if arr.dtype == np.uint8: - data = bytes(imagecodecs.jpegxl_encode(arr, level=quality, effort=effort or 3)) - else: - data = bytes(imagecodecs.jpegxl_encode(arr, lossless=True, effort=effort or 1)) - else: - raise ValueError(f"unsupported format {format!r}") - return cls(data=data, format=format, frame_id=image.frame_id, ts=image.ts) - - def decode(self) -> Image: - """Decompress to a raw Image; ts/frame_id preserved.""" - from dimos.msgs.sensor_msgs.Image import Image, ImageFormat - - if self.format.startswith("jpeg"): - from turbojpeg import TJPF_RGB, TurboJPEG - - arr = TurboJPEG().decode(self.data, pixel_format=TJPF_RGB) - fmt = ImageFormat.RGB - elif self.format.startswith("png"): - import cv2 - import numpy as np - - arr = cv2.imdecode(np.frombuffer(self.data, dtype=np.uint8), cv2.IMREAD_UNCHANGED) - if arr is None: - raise ValueError("PNG decoding failed") - if arr.ndim == 2: - fmt = ImageFormat.GRAY16 if arr.dtype == np.uint16 else ImageFormat.GRAY - elif arr.shape[2] == 4: - fmt = ImageFormat.BGRA - else: - fmt = ImageFormat.BGR - elif self.format.startswith("jxl"): - import imagecodecs - import numpy as np - - arr = imagecodecs.jpegxl_decode(self.data) - if arr.ndim == 2: - fmt = {np.float32: ImageFormat.DEPTH, np.uint16: ImageFormat.GRAY16}.get( - arr.dtype.type, ImageFormat.GRAY - ) - else: - fmt = ImageFormat.RGBA if arr.shape[2] == 4 else ImageFormat.RGB - else: - raise ValueError(f"unsupported format {self.format!r}") - return Image(data=arr, format=fmt, frame_id=self.frame_id, ts=self.ts) - - def lcm_encode(self, frame_id: str | None = None) -> bytes: - msg = LCMCompressedImage() - msg.header = Header() - msg.header.seq = 0 - msg.header.frame_id = frame_id or self.frame_id - msg.header.stamp.sec = int(self.ts) - msg.header.stamp.nsec = int((self.ts - int(self.ts)) * 1e9) - msg.format = self.format - msg.data = self.data - msg.data_length = len(self.data) - return msg.lcm_encode() # type: ignore[no-any-return] - - @classmethod - def lcm_decode(cls, data: bytes, **kwargs: Any) -> CompressedImage: - msg = LCMCompressedImage.lcm_decode(data) - return cls( - data=bytes(msg.data), - format=msg.format, - frame_id=msg.header.frame_id, - ts=msg.header.stamp.sec + msg.header.stamp.nsec / 1e9, - ) - - def to_rerun(self) -> Any: - import rerun as rr - - if self.format.startswith(("jpeg", "png")): - media_type = "image/jpeg" if self.format.startswith("jpeg") else "image/png" - return rr.EncodedImage(contents=self.data, media_type=media_type) - # formats rerun's EncodedImage can't decode: send pixels instead - return self.decode().to_rerun() diff --git a/dimos/msgs/sensor_msgs/Image.py b/dimos/msgs/sensor_msgs/Image.py deleted file mode 100644 index 05b05225757..00000000000 --- a/dimos/msgs/sensor_msgs/Image.py +++ /dev/null @@ -1,700 +0,0 @@ -# Copyright 2025-2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from __future__ import annotations - -import base64 -from dataclasses import dataclass, field -from enum import Enum -import time -from typing import TYPE_CHECKING, Any, Literal, TypedDict -import warnings - -from dimos_lcm.sensor_msgs.Image import Image as LCMImage -from dimos_lcm.std_msgs.Header import Header -import numpy as np -import reactivex as rx -from reactivex import operators as ops -from turbojpeg import TJPF_RGB - -from dimos.types.timestamped import Timestamped, TimestampedBufferCollection, to_human_readable -from dimos.utils.reactive import quality_barrier -from dimos.utils.turbojpeg import get_turbojpeg - -if TYPE_CHECKING: - from collections.abc import Callable - import os - - from reactivex.abc import SchedulerBase - from reactivex.observable import Observable - - -class ImageFormat(Enum): - BGR = "BGR" - RGB = "RGB" - RGBA = "RGBA" - BGRA = "BGRA" - GRAY = "GRAY" - GRAY16 = "GRAY16" - DEPTH = "DEPTH" - DEPTH16 = "DEPTH16" - - -class AgentImageMessage(TypedDict): - """Type definition for agent-compatible image representation.""" - - type: Literal["image"] - source_type: Literal["base64"] - mime_type: Literal["image/jpeg", "image/png"] - data: str # Base64 encoded image data - - -@dataclass -class Image(Timestamped): - """Simple NumPy-based image container.""" - - msg_name = "sensor_msgs.Image" - - data: np.ndarray[Any, np.dtype[Any]] = field( - default_factory=lambda: np.zeros((1, 1, 3), dtype=np.uint8) - ) - format: ImageFormat = field(default=ImageFormat.BGR) - frame_id: str = field(default="") - ts: float = field(default_factory=time.time) - - def __post_init__(self) -> None: - if not isinstance(self.data, np.ndarray): - self.data = np.asarray(self.data) - if self.data.ndim < 2: - raise ValueError("Image requires a 2D/3D NumPy array") - - def __str__(self) -> str: - return ( - f"Image(shape={self.shape}, format={self.format.value}, dtype={self.dtype}, " - f"ts={to_human_readable(self.ts)})" - ) - - def __repr__(self) -> str: - return f"Image(shape={self.shape}, format={self.format.value}, dtype={self.dtype}, frame_id='{self.frame_id}', ts={self.ts})" - - def __eq__(self, other: object) -> bool: - if not isinstance(other, Image): - return False - return ( - np.array_equal(self.data, other.data) - and self.format == other.format - and self.frame_id == other.frame_id - and abs(self.ts - other.ts) < 1e-6 - ) - - def __len__(self) -> int: - return int(self.height * self.width) - - def __getstate__(self) -> dict[str, Any]: - return {"data": self.data, "format": self.format, "frame_id": self.frame_id, "ts": self.ts} - - def __setstate__(self, state: dict[str, Any]) -> None: - self.data = state.get("data", np.zeros((1, 1, 3), dtype=np.uint8)) - self.format = state.get("format", ImageFormat.BGR) - self.frame_id = state.get("frame_id", "") - self.ts = state.get("ts", time.time()) - - @property - def height(self) -> int: - return int(self.data.shape[0]) - - @property - def width(self) -> int: - return int(self.data.shape[1]) - - @property - def channels(self) -> int: - if self.data.ndim == 2: - return 1 - if self.data.ndim == 3: - return int(self.data.shape[2]) - raise ValueError("Invalid image dimensions") - - @property - def shape(self) -> tuple[int, ...]: - return tuple(self.data.shape) - - @property - def dtype(self) -> np.dtype[Any]: - return self.data.dtype - - def copy(self) -> Image: - return Image(data=self.data.copy(), format=self.format, frame_id=self.frame_id, ts=self.ts) - - @classmethod - def from_numpy( - cls, - np_image: np.ndarray, - format: ImageFormat = ImageFormat.BGR, - frame_id: str = "", - ts: float | None = None, - ) -> Image: - return cls( - data=np.asarray(np_image), - format=format, - frame_id=frame_id, - ts=ts if ts is not None else time.time(), - ) - - @classmethod - def from_file( - cls, - filepath: str | os.PathLike[str], - format: ImageFormat = ImageFormat.RGB, - ) -> Image: - import cv2 - - arr = cv2.imread(str(filepath), cv2.IMREAD_UNCHANGED) - if arr is None: - raise ValueError(f"Could not load image from {filepath}") - if arr.ndim == 2: - detected = ImageFormat.GRAY16 if arr.dtype == np.uint16 else ImageFormat.GRAY - elif arr.shape[2] == 3: - detected = ImageFormat.BGR # OpenCV default - elif arr.shape[2] == 4: - detected = ImageFormat.BGRA # OpenCV default - else: - detected = format - return cls(data=arr, format=detected) - - @classmethod - def from_opencv( - cls, - cv_image: np.ndarray, - format: ImageFormat = ImageFormat.BGR, - frame_id: str = "", - ts: float | None = None, - ) -> Image: - """Construct from an OpenCV image (NumPy array).""" - return cls( - data=cv_image, - format=format, - frame_id=frame_id, - ts=ts if ts is not None else time.time(), - ) - - def to_opencv(self) -> np.ndarray: - """Convert to OpenCV BGR format.""" - import cv2 - - arr = self.data - if self.format == ImageFormat.BGR: - return arr - if self.format == ImageFormat.RGB: - return cv2.cvtColor(arr, cv2.COLOR_RGB2BGR) - if self.format == ImageFormat.RGBA: - return cv2.cvtColor(arr, cv2.COLOR_RGBA2BGR) - if self.format == ImageFormat.BGRA: - return cv2.cvtColor(arr, cv2.COLOR_BGRA2BGR) - if self.format in ( - ImageFormat.GRAY, - ImageFormat.GRAY16, - ImageFormat.DEPTH, - ImageFormat.DEPTH16, - ): - return arr - raise ValueError(f"Unsupported format: {self.format}") - - def as_numpy(self) -> np.ndarray: - """Get image data as numpy array.""" - return self.data - - def to_rgb(self) -> Image: - import cv2 - - if self.format == ImageFormat.RGB: - return self.copy() - arr = self.data - if self.format == ImageFormat.BGR: - return Image( - data=cv2.cvtColor(arr, cv2.COLOR_BGR2RGB), - format=ImageFormat.RGB, - frame_id=self.frame_id, - ts=self.ts, - ) - if self.format == ImageFormat.RGBA: - warnings.warn("to_rgb() drops alpha channel from RGBA image", stacklevel=2) - return Image( - data=cv2.cvtColor(arr, cv2.COLOR_RGBA2RGB), - format=ImageFormat.RGB, - frame_id=self.frame_id, - ts=self.ts, - ) - if self.format == ImageFormat.BGRA: - warnings.warn("to_rgb() drops alpha channel from BGRA image", stacklevel=2) - return Image( - data=cv2.cvtColor(arr, cv2.COLOR_BGRA2RGB), - format=ImageFormat.RGB, - frame_id=self.frame_id, - ts=self.ts, - ) - if self.format in (ImageFormat.GRAY, ImageFormat.GRAY16, ImageFormat.DEPTH16): - gray8 = (arr / 256).astype(np.uint8) if self.format != ImageFormat.GRAY else arr - rgb = cv2.cvtColor(gray8, cv2.COLOR_GRAY2RGB) - return Image(data=rgb, format=ImageFormat.RGB, frame_id=self.frame_id, ts=self.ts) - return self.copy() - - def to_bgr(self) -> Image: - import cv2 - - if self.format == ImageFormat.BGR: - return self.copy() - arr = self.data - if self.format == ImageFormat.RGB: - return Image( - data=cv2.cvtColor(arr, cv2.COLOR_RGB2BGR), - format=ImageFormat.BGR, - frame_id=self.frame_id, - ts=self.ts, - ) - if self.format == ImageFormat.RGBA: - warnings.warn("to_bgr() drops alpha channel from RGBA image", stacklevel=2) - return Image( - data=cv2.cvtColor(arr, cv2.COLOR_RGBA2BGR), - format=ImageFormat.BGR, - frame_id=self.frame_id, - ts=self.ts, - ) - if self.format == ImageFormat.BGRA: - warnings.warn("to_bgr() drops alpha channel from BGRA image", stacklevel=2) - return Image( - data=cv2.cvtColor(arr, cv2.COLOR_BGRA2BGR), - format=ImageFormat.BGR, - frame_id=self.frame_id, - ts=self.ts, - ) - if self.format in (ImageFormat.GRAY, ImageFormat.GRAY16, ImageFormat.DEPTH16): - gray8 = (arr / 256).astype(np.uint8) if self.format != ImageFormat.GRAY else arr - return Image( - data=cv2.cvtColor(gray8, cv2.COLOR_GRAY2BGR), - format=ImageFormat.BGR, - frame_id=self.frame_id, - ts=self.ts, - ) - return self.copy() - - def to_grayscale(self) -> Image: - import cv2 - - if self.format in (ImageFormat.GRAY, ImageFormat.GRAY16, ImageFormat.DEPTH): - return self.copy() - if self.format == ImageFormat.BGR: - return Image( - data=cv2.cvtColor(self.data, cv2.COLOR_BGR2GRAY), - format=ImageFormat.GRAY, - frame_id=self.frame_id, - ts=self.ts, - ) - if self.format == ImageFormat.RGB: - return Image( - data=cv2.cvtColor(self.data, cv2.COLOR_RGB2GRAY), - format=ImageFormat.GRAY, - frame_id=self.frame_id, - ts=self.ts, - ) - if self.format in (ImageFormat.RGBA, ImageFormat.BGRA): - code = cv2.COLOR_RGBA2GRAY if self.format == ImageFormat.RGBA else cv2.COLOR_BGRA2GRAY - return Image( - data=cv2.cvtColor(self.data, code), - format=ImageFormat.GRAY, - frame_id=self.frame_id, - ts=self.ts, - ) - raise ValueError(f"Unsupported format: {self.format}") - - def to_rerun(self) -> Any: - """Convert to a Rerun archetype: JPEG-encoded for color images, raw for depth.""" - import rerun as rr - - match self.format: - case ImageFormat.DEPTH | ImageFormat.DEPTH16: - return rr.DepthImage(self.data) - case ImageFormat.GRAY16: - return rr.Image(self.data, color_model="L") - case _: - return rr.EncodedImage(contents=self.to_jpeg_bytes(), media_type="image/jpeg") - - def resize(self, width: int, height: int, interpolation: int | None = None) -> Image: - import cv2 - - if interpolation is None: - interpolation = cv2.INTER_LINEAR - return Image( - data=cv2.resize(self.data, (width, height), interpolation=interpolation), - format=self.format, - frame_id=self.frame_id, - ts=self.ts, - ) - - def resize_to_fit( - self, max_width: int, max_height: int, interpolation: int | None = None - ) -> tuple[Image, float]: - """Resize image to fit within max dimensions while preserving aspect ratio. - - Only scales down if image exceeds max dimensions. Returns self if already fits. - - Returns: - Tuple of (resized_image, scale_factor). Scale factor is 1.0 if no resize needed. - """ - if self.width <= max_width and self.height <= max_height: - return self, 1.0 - - scale = min(max_width / self.width, max_height / self.height) - new_width = int(self.width * scale) - new_height = int(self.height * scale) - return self.resize(new_width, new_height, interpolation), scale - - def crop(self, x: int, y: int, width: int, height: int) -> Image: - """Crop the image to the specified region. - - Args: - x: Starting x coordinate (left edge) - y: Starting y coordinate (top edge) - width: Width of the cropped region - height: Height of the cropped region - - Returns: - A new Image containing the cropped region - """ - img_height, img_width = self.data.shape[:2] - - # Clamp the crop region to image bounds - x = max(0, min(x, img_width)) - y = max(0, min(y, img_height)) - x_end = min(x + width, img_width) - y_end = min(y + height, img_height) - - # Perform the crop using array slicing - if self.data.ndim == 2: - cropped_data = self.data[y:y_end, x:x_end] - else: - cropped_data = self.data[y:y_end, x:x_end, :] - - return Image(data=cropped_data, format=self.format, frame_id=self.frame_id, ts=self.ts) - - @property - def brightness(self) -> float: - """Return mean brightness in [0, 1]. - - Strides to ~256px on the long edge first — ~O(N/step²) cheaper than - reading every pixel, and the mean converges quickly (CLT). - """ - max_val = 65535.0 if self.format in (ImageFormat.GRAY16, ImageFormat.DEPTH16) else 255.0 - step = max(1, max(self.data.shape[:2]) // 256) - return float(self.data[::step, ::step].mean() / max_val) - - @property - def sharpness(self) -> float: - """Return sharpness score. - - Downsamples to ~160px wide before computing Laplacian variance - for fast evaluation (~10-20x cheaper than full-res Sobel). - """ - import cv2 - - gray = self.to_grayscale().data - # Downsample to ~160px wide for cheap evaluation - h, w = gray.shape[:2] - if w > 160: - scale = 160.0 / w - gray = cv2.resize(gray, (160, int(h * scale)), interpolation=cv2.INTER_AREA) - lap_var = cv2.Laplacian(gray, cv2.CV_32F).var() - if lap_var <= 0: - return 0.0 - return float(np.clip((np.log10(lap_var + 1) - 1.0) / 3.0, 0.0, 1.0)) - - def save(self, filepath: str) -> bool: - import cv2 - - arr = self.to_opencv() - return cv2.imwrite(filepath, arr) - - def to_base64( - self, - quality: int = 80, - *, - max_width: int | None = None, - max_height: int | None = None, - ) -> str: - """Encode the image as a base64 JPEG string. - - Args: - quality: JPEG quality (0-100). - max_width: Optional maximum width to constrain the encoded image. - max_height: Optional maximum height to constrain the encoded image. - - Returns: - Base64-encoded JPEG representation of the image. - """ - import cv2 - - bgr_image = self.to_bgr().to_opencv() - height, width = bgr_image.shape[:2] - - scale = 1.0 - if max_width is not None and width > max_width: - scale = min(scale, max_width / width) - if max_height is not None and height > max_height: - scale = min(scale, max_height / height) - - if scale < 1.0: - new_width = max(1, round(width * scale)) - new_height = max(1, round(height * scale)) - bgr_image = cv2.resize(bgr_image, (new_width, new_height), interpolation=cv2.INTER_AREA) - - encode_param = [int(cv2.IMWRITE_JPEG_QUALITY), int(np.clip(quality, 0, 100))] - success, buffer = cv2.imencode(".jpg", bgr_image, encode_param) - if not success: - raise ValueError("Failed to encode image as JPEG") - - return base64.b64encode(buffer.tobytes()).decode("utf-8") - - def agent_encode(self) -> AgentImageMessage: - return [ # type: ignore[return-value] - { - "type": "image_url", - "image_url": {"url": f"data:image/jpeg;base64,{self.to_base64()}"}, - } - ] - - # LCM encode/decode - def lcm_encode(self, frame_id: str | None = None) -> bytes: - """Convert to LCM Image message.""" - msg = LCMImage() - - # Header - msg.header = Header() - msg.header.seq = 0 - msg.header.frame_id = frame_id or self.frame_id - - # Set timestamp - if self.ts is not None: - msg.header.stamp.sec = int(self.ts) - msg.header.stamp.nsec = int((self.ts - int(self.ts)) * 1e9) - else: - now = time.time() - msg.header.stamp.sec = int(now) - msg.header.stamp.nsec = int((now - int(now)) * 1e9) - - # Image properties - msg.height = self.height - msg.width = self.width - msg.encoding = _get_lcm_encoding(self.format, self.dtype) - msg.is_bigendian = False - - # Calculate step (bytes per row) - channels = 1 if self.data.ndim == 2 else self.data.shape[2] - msg.step = self.width * self.dtype.itemsize * channels - - view = memoryview(np.ascontiguousarray(self.data)).cast("B") # type: ignore[arg-type] - msg.data_length = len(view) - msg.data = view - - return msg.lcm_encode() # type: ignore[no-any-return] - - @classmethod - def lcm_decode(cls, data: bytes, **kwargs: Any) -> Image: - msg = LCMImage.lcm_decode(data) - - # JPEG-compressed images use a different decode path. - if msg.encoding == "jpeg": - return cls.lcm_jpeg_decode(data, **kwargs) - - fmt, dtype, channels = _parse_lcm_encoding(msg.encoding) - arr: np.ndarray[Any, Any] = np.frombuffer(msg.data, dtype=dtype) - if channels == 1: - arr = arr.reshape((msg.height, msg.width)) - else: - arr = arr.reshape((msg.height, msg.width, channels)) - return cls( - data=arr, - format=fmt, - frame_id=msg.header.frame_id if hasattr(msg, "header") else "", - ts=( - msg.header.stamp.sec + msg.header.stamp.nsec / 1e9 - if hasattr(msg, "header") - and hasattr(msg.header, "stamp") - and msg.header.stamp.sec > 0 - else time.time() - ), - ) - - def to_jpeg_bytes(self, quality: int = 75) -> bytes: - """Encode this image as JPEG bytes using TurboJPEG. - - Args: - quality: JPEG compression quality (0-100, default 75). - - Returns: - Raw JPEG bytes. - """ - # Canonicalize to RGB so JPEG bytes are deterministic regardless of input format. - rgb_array = self.to_rgb().data - return get_turbojpeg().encode(rgb_array, quality=quality, pixel_format=TJPF_RGB) # type: ignore[no-any-return] - - def lcm_jpeg_encode(self, quality: int = 75, frame_id: str | None = None) -> bytes: - """Convert to LCM Image message with JPEG-compressed data. - - Args: - quality: JPEG compression quality (0-100, default 75) - frame_id: Optional frame ID override - - Returns: - LCM-encoded bytes with JPEG-compressed image data - """ - msg = LCMImage() - - # Header - msg.header = Header() - msg.header.seq = 0 - msg.header.frame_id = frame_id or self.frame_id - - # Set timestamp - if self.ts is not None: - msg.header.stamp.sec = int(self.ts) - msg.header.stamp.nsec = int((self.ts - int(self.ts)) * 1e9) - else: - now = time.time() - msg.header.stamp.sec = int(now) - msg.header.stamp.nsec = int((now - int(now)) * 1e9) - - jpeg_data = self.to_jpeg_bytes(quality=quality) - - # Store JPEG data and metadata - msg.height = self.height - msg.width = self.width - msg.encoding = "jpeg" - msg.is_bigendian = False - msg.step = 0 # Not applicable for compressed format - - msg.data_length = len(jpeg_data) - msg.data = jpeg_data - - return msg.lcm_encode() # type: ignore[no-any-return] - - @classmethod - def lcm_jpeg_decode(cls, data: bytes, **kwargs: Any) -> Image: - """Decode an LCM Image message with JPEG-compressed data. - - Args: - data: LCM-encoded bytes containing JPEG-compressed image - - Returns: - Image instance - """ - msg = LCMImage.lcm_decode(data) - - if msg.encoding != "jpeg": - raise ValueError(f"Expected JPEG encoding, got {msg.encoding}") - - rgb_array = get_turbojpeg().decode(msg.data, pixel_format=TJPF_RGB) - - return cls( - data=rgb_array, - format=ImageFormat.RGB, - frame_id=msg.header.frame_id if hasattr(msg, "header") else "", - ts=( - msg.header.stamp.sec + msg.header.stamp.nsec / 1e9 - if hasattr(msg, "header") - and hasattr(msg.header, "stamp") - and msg.header.stamp.sec > 0 - else time.time() - ), - ) - - -def sharpness_window(target_frequency: float, source: Observable[Image]) -> Observable[Image]: - """Emit the sharpest Image seen within each sliding time window.""" - from reactivex.scheduler import ThreadPoolScheduler - - if target_frequency <= 0: - raise ValueError("target_frequency must be positive") - - window = TimestampedBufferCollection(1.0 / target_frequency) # type: ignore[var-annotated] - source.subscribe(window.add) - - thread_scheduler = ThreadPoolScheduler(max_workers=1) - - def find_best(*_args: Any) -> Image | None: - if len(window) == 0: - return None - return max(window, key=lambda img: img.sharpness) # type: ignore[no-any-return] - - return rx.interval(1.0 / target_frequency).pipe( # type: ignore[misc] - ops.observe_on(thread_scheduler), - ops.map(find_best), - ops.filter(lambda img: img is not None), - ) - - -def sharpness_barrier( - target_frequency: float, scheduler: SchedulerBase | None = None -) -> Callable[[Observable[Image]], Observable[Image]]: - """Select the sharpest Image within each time window.""" - if target_frequency <= 0: - raise ValueError("target_frequency must be positive") - return quality_barrier(lambda image: image.sharpness, target_frequency, scheduler) - - -def _get_lcm_encoding(fmt: ImageFormat, dtype: np.dtype) -> str: - if fmt == ImageFormat.GRAY: - if dtype == np.uint8: - return "mono8" - if dtype == np.uint16: - return "mono16" - if fmt == ImageFormat.GRAY16: - return "mono16" - if fmt == ImageFormat.RGB: - return "rgb8" - if fmt == ImageFormat.RGBA: - return "rgba8" - if fmt == ImageFormat.BGR: - return "bgr8" - if fmt == ImageFormat.BGRA: - return "bgra8" - if fmt == ImageFormat.DEPTH: - if dtype == np.float32: - return "32FC1" - if dtype == np.float64: - return "64FC1" - if fmt == ImageFormat.DEPTH16: - if dtype == np.uint16: - return "16UC1" - if dtype == np.int16: - return "16SC1" - raise ValueError(f"Unsupported LCM encoding for fmt={fmt}, dtype={dtype}") - - -def _parse_lcm_encoding(enc: str) -> tuple[ImageFormat, type, int]: - m = { - "mono8": (ImageFormat.GRAY, np.uint8, 1), - "mono16": (ImageFormat.GRAY16, np.uint16, 1), - "rgb8": (ImageFormat.RGB, np.uint8, 3), - "rgba8": (ImageFormat.RGBA, np.uint8, 4), - "bgr8": (ImageFormat.BGR, np.uint8, 3), - "bgra8": (ImageFormat.BGRA, np.uint8, 4), - "32FC1": (ImageFormat.DEPTH, np.float32, 1), - "32FC3": (ImageFormat.RGB, np.float32, 3), - "64FC1": (ImageFormat.DEPTH, np.float64, 1), - "16UC1": (ImageFormat.DEPTH16, np.uint16, 1), - "16SC1": (ImageFormat.DEPTH16, np.int16, 1), - } - if enc not in m: - raise ValueError(f"Unsupported encoding: {enc}") - return m[enc] diff --git a/dimos/msgs/sensor_msgs/Imu.py b/dimos/msgs/sensor_msgs/Imu.py deleted file mode 100644 index e1cd1f6b621..00000000000 --- a/dimos/msgs/sensor_msgs/Imu.py +++ /dev/null @@ -1,125 +0,0 @@ -# Copyright 2025-2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from __future__ import annotations - -import time - -from dimos_lcm.sensor_msgs.Imu import Imu as LCMImu - -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.types.timestamped import Timestamped - - -class Imu(Timestamped): - """IMU sensor message mirroring ROS sensor_msgs/Imu. - - Contains orientation, angular velocity, and linear acceleration - with optional covariance matrices (3x3 row-major as flat 9-element lists). - """ - - msg_name = "sensor_msgs.Imu" - - def __init__( - self, - angular_velocity: Vector3 | None = None, - linear_acceleration: Vector3 | None = None, - orientation: Quaternion | None = None, - orientation_covariance: list[float] | None = None, - angular_velocity_covariance: list[float] | None = None, - linear_acceleration_covariance: list[float] | None = None, - frame_id: str = "imu_link", - ts: float | None = None, - ) -> None: - self.ts = ts if ts is not None else time.time() - self.frame_id = frame_id - self.angular_velocity = angular_velocity or Vector3(0.0, 0.0, 0.0) - self.linear_acceleration = linear_acceleration or Vector3(0.0, 0.0, 0.0) - self.orientation = orientation or Quaternion(0.0, 0.0, 0.0, 1.0) - self.orientation_covariance = orientation_covariance or [0.0] * 9 - self.angular_velocity_covariance = angular_velocity_covariance or [0.0] * 9 - self.linear_acceleration_covariance = linear_acceleration_covariance or [0.0] * 9 - - def lcm_encode(self) -> bytes: - msg = LCMImu() - [msg.header.stamp.sec, msg.header.stamp.nsec] = self.ros_timestamp() - msg.header.frame_id = self.frame_id - - msg.orientation.x = self.orientation.x - msg.orientation.y = self.orientation.y - msg.orientation.z = self.orientation.z - msg.orientation.w = self.orientation.w - msg.orientation_covariance = self.orientation_covariance - - msg.angular_velocity.x = self.angular_velocity.x - msg.angular_velocity.y = self.angular_velocity.y - msg.angular_velocity.z = self.angular_velocity.z - msg.angular_velocity_covariance = self.angular_velocity_covariance - - msg.linear_acceleration.x = self.linear_acceleration.x - msg.linear_acceleration.y = self.linear_acceleration.y - msg.linear_acceleration.z = self.linear_acceleration.z - msg.linear_acceleration_covariance = self.linear_acceleration_covariance - - return msg.lcm_encode() # type: ignore[no-any-return] - - @classmethod - def lcm_decode(cls, data: bytes) -> Imu: - msg = LCMImu.lcm_decode(data) - ts = msg.header.stamp.sec + (msg.header.stamp.nsec / 1_000_000_000) - return cls( - angular_velocity=Vector3( - msg.angular_velocity.x, - msg.angular_velocity.y, - msg.angular_velocity.z, - ), - linear_acceleration=Vector3( - msg.linear_acceleration.x, - msg.linear_acceleration.y, - msg.linear_acceleration.z, - ), - orientation=Quaternion( - msg.orientation.x, - msg.orientation.y, - msg.orientation.z, - msg.orientation.w, - ), - orientation_covariance=list(msg.orientation_covariance), - angular_velocity_covariance=list(msg.angular_velocity_covariance), - linear_acceleration_covariance=list(msg.linear_acceleration_covariance), - frame_id=msg.header.frame_id, - ts=ts, - ) - - def __str__(self) -> str: - return ( - f"Imu(frame_id='{self.frame_id}', " - f"gyro=({self.angular_velocity.x:.3f}, {self.angular_velocity.y:.3f}, {self.angular_velocity.z:.3f}), " - f"accel=({self.linear_acceleration.x:.3f}, {self.linear_acceleration.y:.3f}, {self.linear_acceleration.z:.3f}))" - ) - - def __repr__(self) -> str: - return ( - f"Imu(ts={self.ts}, frame_id='{self.frame_id}', " - f"angular_velocity={self.angular_velocity}, " - f"linear_acceleration={self.linear_acceleration}, " - f"orientation={self.orientation})" - ) - - def to_rerun(self, axis_length: float = 0.2): # type: ignore[no-untyped-def] - """Orientation axes for rerun.""" - import rerun as rr - - return rr.TransformAxes3D(axis_length=axis_length) diff --git a/dimos/msgs/sensor_msgs/ImuInfo.py b/dimos/msgs/sensor_msgs/ImuInfo.py deleted file mode 100644 index 216d7e42a58..00000000000 --- a/dimos/msgs/sensor_msgs/ImuInfo.py +++ /dev/null @@ -1,158 +0,0 @@ -# Copyright 2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -"""IMU noise-model information, the inertial sibling of ``CameraInfo``. - -Where the IMU sits comes from tf (``header.frame_id`` names the frame), just as -camera extrinsics do; this message carries what tf cannot: the continuous-time -noise model consumers such as VIO preintegration need. The values are -Allan-variance constants, measured once per model (or per unit); IMUs do not -report them at runtime, so a driver publishes them from its config the same way -``camera_info`` is published alongside images. - -LCM schema (fingerprint-compatible bindings must be generated from this):: - - package sensor_msgs; - - struct ImuInfo { - std_msgs.Header header; - double gyro_noise_density; - double gyro_random_walk; - double accel_noise_density; - double accel_random_walk; - double frequency; - } -""" - -from __future__ import annotations - -from io import BytesIO -import struct - -from dimos_lcm.std_msgs.Header import Header - -from dimos.types.timestamped import Timestamped - -# lcm-gen's base hash for the struct above; the wire fingerprint mixes in -# Header's recursively, matching the generated C++ in dimSLAM. -_BASE_HASH = 0x437A196CBD6B4E49 - - -def _packed_fingerprint() -> bytes: - tmphash = (_BASE_HASH + Header._get_hash_recursive([ImuInfo])) & 0xFFFFFFFFFFFFFFFF - tmphash = (((tmphash << 1) & 0xFFFFFFFFFFFFFFFF) + (tmphash >> 63)) & 0xFFFFFFFFFFFFFFFF - return struct.pack(">Q", tmphash) - - -class ImuInfo(Timestamped): - """IMU noise model: Allan-variance constants plus the delivered sample rate.""" - - msg_name = "sensor_msgs.ImuInfo" - - def __init__( - self, - gyro_noise_density: float = 0.0, - gyro_random_walk: float = 0.0, - accel_noise_density: float = 0.0, - accel_random_walk: float = 0.0, - frequency: float = 0.0, - frame_id: str = "", - ts: float | None = None, - ) -> None: - """Initialize ImuInfo. - - Args: - gyro_noise_density: Gyroscope white noise, rad/s/sqrt(Hz) - gyro_random_walk: Gyroscope bias random walk, rad/s^2/sqrt(Hz) - accel_noise_density: Accelerometer white noise, m/s^2/sqrt(Hz) - accel_random_walk: Accelerometer bias random walk, m/s^3/sqrt(Hz) - frequency: Rate the samples are actually delivered at, Hz - frame_id: The IMU frame; tf places it against the rest of the rig - ts: Timestamp (defaults to now) - """ - import time - - super().__init__(ts if ts is not None else time.time()) - self.gyro_noise_density = gyro_noise_density - self.gyro_random_walk = gyro_random_walk - self.accel_noise_density = accel_noise_density - self.accel_random_walk = accel_random_walk - self.frequency = frequency - self.frame_id = frame_id - - def with_ts(self, ts: float) -> ImuInfo: - """Return a copy of this ImuInfo with the given timestamp.""" - return ImuInfo( - gyro_noise_density=self.gyro_noise_density, - gyro_random_walk=self.gyro_random_walk, - accel_noise_density=self.accel_noise_density, - accel_random_walk=self.accel_random_walk, - frequency=self.frequency, - frame_id=self.frame_id, - ts=ts, - ) - - def with_frame_id(self, frame_id: str) -> ImuInfo: - """Return a copy of this ImuInfo stamped with the given frame.""" - copy = self.with_ts(self.ts) - copy.frame_id = frame_id - return copy - - def lcm_encode(self) -> bytes: - header = Header() - header.seq = 0 - header.frame_id = self.frame_id - header.stamp.sec = int(self.ts) - header.stamp.nsec = int((self.ts - int(self.ts)) * 1e9) - - buf = BytesIO() - buf.write(_packed_fingerprint()) - header._encode_one(buf) - buf.write( - struct.pack( - ">ddddd", - self.gyro_noise_density, - self.gyro_random_walk, - self.accel_noise_density, - self.accel_random_walk, - self.frequency, - ) - ) - return buf.getvalue() - - @classmethod - def lcm_decode(cls, data: bytes) -> ImuInfo: - buf = BytesIO(data) - if buf.read(8) != _packed_fingerprint(): - raise ValueError("Decode error") - header = Header._decode_one(buf) - values = struct.unpack(">ddddd", buf.read(40)) - return cls( - gyro_noise_density=values[0], - gyro_random_walk=values[1], - accel_noise_density=values[2], - accel_random_walk=values[3], - frequency=values[4], - frame_id=header.frame_id, - ts=header.stamp.sec + header.stamp.nsec / 1e9, - ) - - def __repr__(self) -> str: - return ( - f"ImuInfo(gyro_noise_density={self.gyro_noise_density}, " - f"gyro_random_walk={self.gyro_random_walk}, " - f"accel_noise_density={self.accel_noise_density}, " - f"accel_random_walk={self.accel_random_walk}, " - f"frequency={self.frequency}, frame_id='{self.frame_id}')" - ) diff --git a/dimos/msgs/sensor_msgs/JointCommand.py b/dimos/msgs/sensor_msgs/JointCommand.py deleted file mode 100644 index 78c541c50ec..00000000000 --- a/dimos/msgs/sensor_msgs/JointCommand.py +++ /dev/null @@ -1,143 +0,0 @@ -# Copyright 2025-2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -"""LCM type definitions -This file automatically generated by lcm. -DO NOT MODIFY BY HAND!!!! -""" - -from io import BytesIO -import struct -import time - - -class JointCommand: - """ - Joint command message for robotic manipulators. - - Supports variable number of joints (DOF) with float64 values. - Can be used for position commands or velocity commands. - Includes timestamp for synchronization. - """ - - msg_name = "sensor_msgs.JointCommand" - - __slots__ = ["num_joints", "positions", "timestamp"] - - __typenames__ = ["double", "int32_t", "double"] - - __dimensions__ = [None, None, ["num_joints"]] - - def __init__( - self, positions: list[float] | None = None, timestamp: float | None = None - ) -> None: - """ - Initialize JointCommand. - - Args: - positions: List of joint values (positions or velocities) - timestamp: Unix timestamp (seconds since epoch). If None, uses current time. - """ - if positions is None: - positions = [] - - if timestamp is None: - timestamp = time.time() - - # LCM Type: double (timestamp) - self.timestamp = timestamp - # LCM Type: int32_t - self.num_joints = len(positions) - # LCM Type: double[num_joints] - self.positions = list(positions) - - def lcm_encode(self): # type: ignore[no-untyped-def] - """Encode for LCM transport (dimos uses lcm_encode method name).""" - return self.encode() # type: ignore[no-untyped-call] - - def encode(self): # type: ignore[no-untyped-def] - buf = BytesIO() - buf.write(JointCommand._get_packed_fingerprint()) # type: ignore[no-untyped-call] - self._encode_one(buf) - return buf.getvalue() - - def _encode_one(self, buf) -> None: # type: ignore[no-untyped-def] - # Encode timestamp - buf.write(struct.pack(">d", self.timestamp)) - - # Encode num_joints - buf.write(struct.pack(">i", self.num_joints)) - - # Encode positions array - for i in range(self.num_joints): - buf.write(struct.pack(">d", self.positions[i])) - - @classmethod - def lcm_decode(cls, data: bytes): # type: ignore[no-untyped-def] - """Decode from LCM transport (dimos uses lcm_decode method name).""" - return cls.decode(data) - - @classmethod - def decode(cls, data: bytes): # type: ignore[no-untyped-def] - if hasattr(data, "read"): - buf = data - else: - buf = BytesIO(data) # type: ignore[assignment] - if buf.read(8) != cls._get_packed_fingerprint(): # type: ignore[no-untyped-call] - raise ValueError("Decode error") - return cls._decode_one(buf) # type: ignore[no-untyped-call] - - @classmethod - def _decode_one(cls, buf): # type: ignore[no-untyped-def] - self = JointCommand.__new__(JointCommand) - - # Decode timestamp - self.timestamp = struct.unpack(">d", buf.read(8))[0] - - # Decode num_joints - self.num_joints = struct.unpack(">i", buf.read(4))[0] - - # Decode positions array - self.positions = [] - for _i in range(self.num_joints): - self.positions.append(struct.unpack(">d", buf.read(8))[0]) - - return self - - @classmethod - def _get_hash_recursive(cls, parents): # type: ignore[no-untyped-def] - if cls in parents: - return 0 - # Hash for variable-length double array message - tmphash = (0x8A3D2E1C5F4B6A9D) & 0xFFFFFFFFFFFFFFFF - tmphash = (((tmphash << 1) & 0xFFFFFFFFFFFFFFFF) + (tmphash >> 63)) & 0xFFFFFFFFFFFFFFFF - return tmphash - - _packed_fingerprint = None - - @classmethod - def _get_packed_fingerprint(cls): # type: ignore[no-untyped-def] - if cls._packed_fingerprint is None: - cls._packed_fingerprint = struct.pack(">Q", cls._get_hash_recursive([])) # type: ignore[no-untyped-call] - return cls._packed_fingerprint - - def get_hash(self): # type: ignore[no-untyped-def] - """Get the LCM hash of the struct""" - return struct.unpack(">Q", JointCommand._get_packed_fingerprint())[0] # type: ignore[no-untyped-call] - - def __str__(self) -> str: - return f"JointCommand(timestamp={self.timestamp:.6f}, num_joints={self.num_joints}, positions={self.positions})" - - def __repr__(self) -> str: - return f"JointCommand(positions={self.positions}, timestamp={self.timestamp})" diff --git a/dimos/msgs/sensor_msgs/JointState.py b/dimos/msgs/sensor_msgs/JointState.py deleted file mode 100644 index 3cbb74e0338..00000000000 --- a/dimos/msgs/sensor_msgs/JointState.py +++ /dev/null @@ -1,144 +0,0 @@ -# Copyright 2025-2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from __future__ import annotations - -import time -from typing import Any, TypeAlias - -from dimos_lcm.sensor_msgs import JointState as LCMJointState - -from dimos.types.timestamped import Timestamped - -# Types that can be converted to/from JointState -JointStateConvertable: TypeAlias = dict[str, list[str] | list[float]] | LCMJointState - -_FIELDS = ("ts", "frame_id", "name", "position", "velocity", "effort") - - -def sec_nsec(ts): # type: ignore[no-untyped-def] - s = int(ts) - return [s, int((ts - s) * 1_000_000_000)] - - -def _fields_of(source: Any) -> tuple[Any, ...]: - """Pull the six JointState fields out of a dict, a JointState or an LCM message.""" - if isinstance(source, dict): - return ( - source.get("ts"), - source.get("frame_id", ""), - source.get("name"), - source.get("position"), - source.get("velocity"), - source.get("effort"), - ) - if isinstance(source, JointState): - return (source.ts, source.frame_id, *(list(getattr(source, f)) for f in _FIELDS[2:])) - return ( - source.header.stamp.sec + (source.header.stamp.nsec / 1_000_000_000), - source.header.frame_id, - *(list(getattr(source, f) or []) for f in _FIELDS[2:]), - ) - - -class JointState(Timestamped): - msg_name = "sensor_msgs.JointState" - ts: float - frame_id: str - name: list[str] - position: list[float] - velocity: list[float] - effort: list[float] - - def __init__( - self, - ts: float | JointStateConvertable | JointState | None = None, - frame_id: str = "", - name: list[str] | None = None, - position: list[float] | None = None, - velocity: list[float] | None = None, - effort: list[float] | None = None, - ) -> None: - """Initialize a JointState message. - - The first argument doubles as the source for the copy/dict/LCM forms: - ``JointState(other)``, ``JointState({...})``, ``JointState(lcm_msg)``. - - Args: - ts: Timestamp in seconds - frame_id: Frame ID for the message - name: List of joint names - position: List of joint positions (rad or m); gripper joints use - the unit their adapter declares - velocity: List of joint velocities (rad/s or m/s) - effort: List of joint efforts (Nm or N) - """ - stamp: Any = ts - if isinstance(stamp, dict | JointState | LCMJointState): - stamp, frame_id, name, position, velocity, effort = _fields_of(stamp) - - self.ts = time.time() if stamp is None else stamp - self.frame_id = frame_id - self.name = name if name is not None else [] - self.position = position if position is not None else [] - self.velocity = velocity if velocity is not None else [] - self.effort = effort if effort is not None else [] - - def lcm_encode(self) -> bytes: - lcm_msg = LCMJointState() - [lcm_msg.header.stamp.sec, lcm_msg.header.stamp.nsec] = sec_nsec(self.ts) # type: ignore[no-untyped-call] - lcm_msg.header.frame_id = self.frame_id - lcm_msg.name_length = len(self.name) - lcm_msg.name = self.name - lcm_msg.position_length = len(self.position) - lcm_msg.position = self.position - lcm_msg.velocity_length = len(self.velocity) - lcm_msg.velocity = self.velocity - lcm_msg.effort_length = len(self.effort) - lcm_msg.effort = self.effort - return lcm_msg.lcm_encode() # type: ignore[no-any-return] - - @classmethod - def lcm_decode(cls, data: bytes) -> JointState: - lcm_msg = LCMJointState.lcm_decode(data) - return cls( - ts=lcm_msg.header.stamp.sec + (lcm_msg.header.stamp.nsec / 1_000_000_000), - frame_id=lcm_msg.header.frame_id, - name=list(lcm_msg.name) if lcm_msg.name else [], - position=list(lcm_msg.position) if lcm_msg.position else [], - velocity=list(lcm_msg.velocity) if lcm_msg.velocity else [], - effort=list(lcm_msg.effort) if lcm_msg.effort else [], - ) - - def __str__(self) -> str: - return f"JointState({len(self.name)} joints, frame_id='{self.frame_id}')" - - def __repr__(self) -> str: - return ( - f"JointState(ts={self.ts}, frame_id='{self.frame_id}', " - f"name={self.name}, position={self.position}, " - f"velocity={self.velocity}, effort={self.effort})" - ) - - def __eq__(self, other) -> bool: # type: ignore[no-untyped-def] - """Check if two JointState messages are equal.""" - if not isinstance(other, JointState): - return False - return ( - self.name == other.name - and self.position == other.position - and self.velocity == other.velocity - and self.effort == other.effort - and self.frame_id == other.frame_id - ) diff --git a/dimos/msgs/sensor_msgs/Joy.py b/dimos/msgs/sensor_msgs/Joy.py deleted file mode 100644 index 9443770b67e..00000000000 --- a/dimos/msgs/sensor_msgs/Joy.py +++ /dev/null @@ -1,130 +0,0 @@ -# Copyright 2025-2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from __future__ import annotations - -import time -from typing import Any, TypeAlias - -from dimos_lcm.sensor_msgs import Joy as LCMJoy - -from dimos.types.timestamped import Timestamped - -# Types that can be converted to/from Joy -JoyConvertable: TypeAlias = ( - tuple[list[float], list[int]] | dict[str, list[float] | list[int]] | LCMJoy -) - - -def sec_nsec(ts): # type: ignore[no-untyped-def] - s = int(ts) - return [s, int((ts - s) * 1_000_000_000)] - - -def _fields_of(source: Any) -> tuple[Any, ...]: - """Pull the four Joy fields out of a dict, an (axes, buttons) pair, a Joy or an LCM message.""" - if isinstance(source, dict): - return ( - source.get("ts"), - source.get("frame_id", ""), - source.get("axes"), - source.get("buttons"), - ) - if isinstance(source, tuple | list): - return (None, "", list(source[0]), list(source[1])) - if isinstance(source, Joy): - return (source.ts, source.frame_id, list(source.axes), list(source.buttons)) - return ( - source.header.stamp.sec + (source.header.stamp.nsec / 1_000_000_000), - source.header.frame_id, - list(source.axes), - list(source.buttons), - ) - - -class Joy(Timestamped): - msg_name = "sensor_msgs.Joy" - ts: float - frame_id: str - axes: list[float] - buttons: list[int] - - def __init__( - self, - ts: float | JoyConvertable | Joy | None = None, - frame_id: str = "", - axes: list[float] | None = None, - buttons: list[int] | None = None, - ) -> None: - """Initialize a Joy message. - - The first argument doubles as the source for the copy/dict/pair/LCM forms: - ``Joy(other)``, ``Joy({...})``, ``Joy((axes, buttons))``, ``Joy(lcm_msg)``. - - Args: - ts: Timestamp in seconds - frame_id: Frame ID for the message - axes: List of axis values (typically -1.0 to 1.0) - buttons: List of button states (0 or 1) - """ - stamp: Any = ts - if isinstance(stamp, dict | tuple | list | Joy | LCMJoy): - stamp, frame_id, axes, buttons = _fields_of(stamp) - - self.ts = time.time() if stamp is None else stamp - self.frame_id = frame_id - self.axes = axes if axes is not None else [] - self.buttons = buttons if buttons is not None else [] - - def lcm_encode(self) -> bytes: - lcm_msg = LCMJoy() - [lcm_msg.header.stamp.sec, lcm_msg.header.stamp.nsec] = sec_nsec(self.ts) # type: ignore[no-untyped-call] - lcm_msg.header.frame_id = self.frame_id - lcm_msg.axes_length = len(self.axes) - lcm_msg.axes = self.axes - lcm_msg.buttons_length = len(self.buttons) - lcm_msg.buttons = self.buttons - return lcm_msg.lcm_encode() # type: ignore[no-any-return] - - @classmethod - def lcm_decode(cls, data: bytes) -> Joy: - lcm_msg = LCMJoy.lcm_decode(data) - return cls( - ts=lcm_msg.header.stamp.sec + (lcm_msg.header.stamp.nsec / 1_000_000_000), - frame_id=lcm_msg.header.frame_id, - axes=list(lcm_msg.axes) if lcm_msg.axes else [], - buttons=list(lcm_msg.buttons) if lcm_msg.buttons else [], - ) - - def __str__(self) -> str: - return ( - f"Joy(axes={len(self.axes)} values, buttons={len(self.buttons)} values, " - f"frame_id='{self.frame_id}')" - ) - - def __repr__(self) -> str: - return ( - f"Joy(ts={self.ts}, frame_id='{self.frame_id}', " - f"axes={self.axes}, buttons={self.buttons})" - ) - - def __eq__(self, other) -> bool: # type: ignore[no-untyped-def] - """Check if two Joy messages are equal.""" - if not isinstance(other, Joy): - return False - return ( - self.axes == other.axes - and self.buttons == other.buttons - and self.frame_id == other.frame_id - ) diff --git a/dimos/msgs/sensor_msgs/MotorCommandArray.py b/dimos/msgs/sensor_msgs/MotorCommandArray.py deleted file mode 100644 index 577b03c7655..00000000000 --- a/dimos/msgs/sensor_msgs/MotorCommandArray.py +++ /dev/null @@ -1,133 +0,0 @@ -# Copyright 2025-2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -"""LCM type for low-level motor commands: q, dq, kp, kd, tau per motor.""" - -from io import BytesIO -import struct -import time - - -class MotorCommandArray: - """Per-motor q/dq/kp/kd/tau. Joint order is implicit (caller-defined).""" - - msg_name = "sensor_msgs.MotorCommandArray" - - __slots__ = ["dq", "kd", "kp", "num_joints", "q", "tau", "timestamp"] - - def __init__( - self, - q: list[float] | None = None, - dq: list[float] | None = None, - kp: list[float] | None = None, - kd: list[float] | None = None, - tau: list[float] | None = None, - timestamp: float | None = None, - ) -> None: - if q is None: - q = [] - if timestamp is None: - timestamp = time.time() - - n = len(q) - if dq is None: - dq = [0.0] * n - if kp is None: - kp = [0.0] * n - if kd is None: - kd = [0.0] * n - if tau is None: - tau = [0.0] * n - - if not (len(dq) == len(kp) == len(kd) == len(tau) == n): - raise ValueError( - f"All arrays must have length {n}; got " - f"dq={len(dq)} kp={len(kp)} kd={len(kd)} tau={len(tau)}" - ) - - self.timestamp = timestamp - self.num_joints = n - self.q = list(q) - self.dq = list(dq) - self.kp = list(kp) - self.kd = list(kd) - self.tau = list(tau) - - def lcm_encode(self) -> bytes: - return self.encode() - - def encode(self) -> bytes: - buf = BytesIO() - buf.write(MotorCommandArray._get_packed_fingerprint()) - self._encode_one(buf) - return buf.getvalue() - - def _encode_one(self, buf: BytesIO) -> None: - buf.write(struct.pack(">d", self.timestamp)) - buf.write(struct.pack(">i", self.num_joints)) - for arr in (self.q, self.dq, self.kp, self.kd, self.tau): - for v in arr: - buf.write(struct.pack(">d", v)) - - @classmethod - def lcm_decode(cls, data: bytes) -> "MotorCommandArray": - return cls.decode(data) - - @classmethod - def decode(cls, data: bytes | BytesIO) -> "MotorCommandArray": - buf: BytesIO = data if isinstance(data, BytesIO) else BytesIO(data) - if buf.read(8) != cls._get_packed_fingerprint(): - raise ValueError("Decode error") - return cls._decode_one(buf) - - @classmethod - def _decode_one(cls, buf: BytesIO) -> "MotorCommandArray": - self = MotorCommandArray.__new__(MotorCommandArray) - self.timestamp = struct.unpack(">d", buf.read(8))[0] - self.num_joints = struct.unpack(">i", buf.read(4))[0] - n = self.num_joints - arrays = [] - for _ in range(5): - arrays.append([struct.unpack(">d", buf.read(8))[0] for _ in range(n)]) - self.q, self.dq, self.kp, self.kd, self.tau = arrays - return self - - @classmethod - def _get_hash_recursive(cls, parents: list[type]) -> int: - if cls in parents: - return 0 - # Distinct fingerprint from JointCommand (0x8A3D2E1C5F4B6A9D) - tmphash = (0x9B4E3F2D6A5C7B8E) & 0xFFFFFFFFFFFFFFFF - tmphash = (((tmphash << 1) & 0xFFFFFFFFFFFFFFFF) + (tmphash >> 63)) & 0xFFFFFFFFFFFFFFFF - return tmphash - - _packed_fingerprint: bytes | None = None - - @classmethod - def _get_packed_fingerprint(cls) -> bytes: - if cls._packed_fingerprint is None: - cls._packed_fingerprint = struct.pack(">Q", cls._get_hash_recursive([])) - return cls._packed_fingerprint - - def get_hash(self) -> int: - return int(struct.unpack(">Q", MotorCommandArray._get_packed_fingerprint())[0]) - - def __str__(self) -> str: - return f"MotorCommandArray(timestamp={self.timestamp:.6f}, num_joints={self.num_joints})" - - def __repr__(self) -> str: - return ( - f"MotorCommandArray(q={self.q}, dq={self.dq}, kp={self.kp}, " - f"kd={self.kd}, tau={self.tau}, timestamp={self.timestamp})" - ) diff --git a/dimos/msgs/sensor_msgs/PointCloud2.py b/dimos/msgs/sensor_msgs/PointCloud2.py deleted file mode 100644 index 9ec2bf6ea82..00000000000 --- a/dimos/msgs/sensor_msgs/PointCloud2.py +++ /dev/null @@ -1,1003 +0,0 @@ -# Copyright 2025-2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from __future__ import annotations - -import functools -import struct -from typing import TYPE_CHECKING, Any - -# Import LCM types -from dimos_lcm.sensor_msgs.PointCloud2 import ( - PointCloud2 as LCMPointCloud2, -) -from dimos_lcm.sensor_msgs.PointField import PointField -from dimos_lcm.std_msgs.Header import Header -import numpy as np - -from dimos.msgs.geometry_msgs.Transform import Transform -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.types.timestamped import Timestamped - -if TYPE_CHECKING: - import open3d as o3d # type: ignore[import-untyped] - from rerun._baseclasses import Archetype - - from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo - from dimos.msgs.sensor_msgs.Image import Image - - -@functools.lru_cache(maxsize=16) -def _get_matplotlib_cmap(name: str): # type: ignore[no-untyped-def] - """Get a matplotlib colormap by name (cached for performance).""" - import matplotlib.pyplot as plt - - return plt.get_cmap(name) - - -@functools.lru_cache(maxsize=16) -def _get_colormap_lut(name: str) -> np.ndarray: - """Build a 256-entry uint8 LUT from a matplotlib colormap (one-time cost).""" - cmap = _get_matplotlib_cmap(name) - t = np.linspace(0, 1, 256) - return (cmap(t)[:, :3] * 255).astype(np.uint8) # type: ignore[no-any-return] - - -def register_colormap_annotation(name: str = "turbo") -> None: - """Register a colormap as AnnotationContext so Rerun resolves colors viewer-side.""" - import rerun as rr - - lut = _get_colormap_lut(name) - rr.log( - "/", - rr.AnnotationContext( - [ - rr.datatypes.ClassDescription( - info=rr.datatypes.AnnotationInfo(id=i, color=lut[i].tolist()) - ) - for i in range(256) - ] - ), - static=True, - ) - - -# TODO: encode/decode need to be updated to work with full spectrum of pointcloud2 fields -class PointCloud2(Timestamped): - msg_name = "sensor_msgs.PointCloud2" - - def __init__( - self, - pointcloud: o3d.geometry.PointCloud | o3d.t.geometry.PointCloud | None = None, - frame_id: str = "world", - ts: float | None = None, - ) -> None: - import open3d as o3d # type: ignore[import-untyped] - - self.ts = ts # type: ignore[assignment] - self.frame_id = frame_id - - # Store internally as tensor pointcloud for speed - if pointcloud is None: - self._pcd_tensor: o3d.t.geometry.PointCloud = o3d.t.geometry.PointCloud() - elif isinstance(pointcloud, o3d.t.geometry.PointCloud): - self._pcd_tensor = pointcloud - elif len(pointcloud.points) == 0: - # from_legacy() warns on empty legacy clouds; build an empty tensor instead - self._pcd_tensor = o3d.t.geometry.PointCloud() - else: - self._pcd_tensor = o3d.t.geometry.PointCloud.from_legacy(pointcloud) - self._pcd_legacy_cache: o3d.geometry.PointCloud | None = None - - def _ensure_tensor_initialized(self) -> None: - """Ensure _pcd_tensor and _pcd_legacy_cache exist (handles unpickled old objects).""" - import open3d as o3d # type: ignore[import-untyped] - - # Always ensure _pcd_legacy_cache exists - if not hasattr(self, "_pcd_legacy_cache"): - self._pcd_legacy_cache = None - - # Check for old pickled format: 'pointcloud' directly in __dict__ - # This takes priority even if _pcd_tensor exists (it might be empty) - old_pcd = self.__dict__.get("pointcloud") - if old_pcd is not None and isinstance(old_pcd, o3d.geometry.PointCloud): - self._pcd_tensor = o3d.t.geometry.PointCloud.from_legacy(old_pcd) - self._pcd_legacy_cache = old_pcd # reuse it - del self.__dict__["pointcloud"] - return - - if not hasattr(self, "_pcd_tensor"): - self._pcd_tensor = o3d.t.geometry.PointCloud() - - def __getstate__(self) -> dict[str, object]: - """Serialize to numpy for pickling (tensors don't pickle well).""" - self._ensure_tensor_initialized() - state = self.__dict__.copy() - # Convert tensor to numpy for serialization - if "positions" in self._pcd_tensor.point: - state["_pcd_numpy"] = self._pcd_tensor.point["positions"].numpy() - else: - state["_pcd_numpy"] = np.zeros((0, 3), dtype=np.float32) - # Remove non-picklable objects - del state["_pcd_tensor"] - state["_pcd_legacy_cache"] = None - # Remove all cached_property entries - for key in list(state): - if isinstance(getattr(type(self), key, None), functools.cached_property): - del state[key] - return state - - def __setstate__(self, state: dict[str, object]) -> None: - """Restore from pickled state.""" - import open3d as o3d # type: ignore[import-untyped] - import open3d.core as o3c # type: ignore[import-untyped] - - points_obj = state.pop("_pcd_numpy", None) - points: np.ndarray[tuple[int, int], np.dtype[np.float32]] = ( - points_obj if isinstance(points_obj, np.ndarray) else np.zeros((0, 3), dtype=np.float32) - ) - self.__dict__.update(state) - # Recreate tensor from numpy - self._pcd_tensor = o3d.t.geometry.PointCloud() - if len(points) > 0: - self._pcd_tensor.point["positions"] = o3c.Tensor(points, dtype=o3c.float32) - - @property - def pointcloud(self) -> o3d.geometry.PointCloud: - """Legacy pointcloud property for backwards compatibility. Cached.""" - self._ensure_tensor_initialized() - if self._pcd_legacy_cache is None: - self._pcd_legacy_cache = self._pcd_tensor.to_legacy() - return self._pcd_legacy_cache - - @pointcloud.setter - def pointcloud(self, value: o3d.geometry.PointCloud | o3d.t.geometry.PointCloud) -> None: - import open3d as o3d # type: ignore[import-untyped] - - if isinstance(value, o3d.t.geometry.PointCloud): - self._pcd_tensor = value - elif len(value.points) == 0: - self._pcd_tensor = o3d.t.geometry.PointCloud() - else: - self._pcd_tensor = o3d.t.geometry.PointCloud.from_legacy(value) - self._pcd_legacy_cache = None - - @property - def pointcloud_tensor(self) -> o3d.t.geometry.PointCloud: - """Direct access to tensor pointcloud (faster, no conversion).""" - self._ensure_tensor_initialized() - return self._pcd_tensor - - @classmethod - def from_numpy( - cls, - points: np.ndarray, - frame_id: str = "world", - timestamp: float | None = None, - intensities: np.ndarray | None = None, - offset_times: np.ndarray | None = None, - tags: np.ndarray | None = None, - lines: np.ndarray | None = None, - ) -> PointCloud2: - """Create PointCloud2 from numpy array of shape (N, 3). - - Args: - points: Nx3 numpy array of 3D points - frame_id: Frame ID for the point cloud - timestamp: Timestamp for the point cloud (defaults to current time) - intensities: Optional Nx1 or (N,) float array of per-point intensity values - offset_times: Optional (N,) uint32 array of per-point capture-time - offsets in nanoseconds relative to the header stamp - tags: Optional (N,) uint8 array of per-point sensor tag bytes - lines: Optional (N,) uint8 array of per-point laser line numbers - - Returns: - PointCloud2 instance - """ - import open3d as o3d # type: ignore[import-untyped] - import open3d.core as o3c # type: ignore[import-untyped] - - for name, values in ( - ("intensities", intensities), - ("offset_times", offset_times), - ("tags", tags), - ("lines", lines), - ): - if values is not None and len(values) != len(points): - raise ValueError(f"{name} has {len(values)} entries for {len(points)} points") - pcd_t = o3d.t.geometry.PointCloud() - pcd_t.point["positions"] = o3c.Tensor(points.astype(np.float32), dtype=o3c.float32) - if intensities is not None: - arr = intensities.astype(np.float32) - if arr.ndim == 1: - arr = arr.reshape(-1, 1) - pcd_t.point["intensities"] = o3c.Tensor(arr, dtype=o3c.float32) - if offset_times is not None: - pcd_t.point["offset_times"] = o3c.Tensor( - offset_times.astype(np.uint32).reshape(-1, 1), dtype=o3c.uint32 - ) - if tags is not None: - pcd_t.point["tags"] = o3c.Tensor(tags.astype(np.uint8).reshape(-1, 1), dtype=o3c.uint8) - if lines is not None: - pcd_t.point["lines"] = o3c.Tensor( - lines.astype(np.uint8).reshape(-1, 1), dtype=o3c.uint8 - ) - return cls(pointcloud=pcd_t, ts=timestamp, frame_id=frame_id) - - @classmethod - def from_rgbd( - cls, - color_image: Image, - depth_image: Image, - camera_info: CameraInfo, - depth_scale: float = 1.0, - depth_trunc: float = 5.0, - ) -> PointCloud2: - """Create PointCloud2 from RGB and depth Image messages. - - Uses frame_id and timestamp from the depth image. - - Args: - color_image: RGB/BGR color Image message - depth_image: Depth Image message (float32 meters or uint16 mm) - camera_info: CameraInfo message with intrinsics - depth_scale: Scale factor to convert depth to meters (default 1.0 for float32) - depth_trunc: Maximum depth in meters to include - - Returns: - PointCloud2 instance with colored points - """ - import open3d as o3d # type: ignore[import-untyped] - - # Get color as RGB numpy array - color_data = color_image.to_rgb().data - if hasattr(color_data, "get"): # CuPy array - color_data = color_data.get() - color_data = np.ascontiguousarray(color_data) - - # Get depth numpy array - depth_data = depth_image.data - if hasattr(depth_data, "get"): # CuPy array - depth_data = depth_data.get() - - # Convert depth to float32 meters if needed - if depth_data.dtype == np.uint16: - depth_data = depth_data.astype(np.float32) * depth_scale - elif depth_data.dtype != np.float32: - depth_data = depth_data.astype(np.float32) - depth_data = np.ascontiguousarray(depth_data) - - # Verify dimensions match - color_h, color_w = color_data.shape[:2] - depth_h, depth_w = depth_data.shape[:2] - if (color_h, color_w) != (depth_h, depth_w): - raise ValueError( - f"Color {color_w}x{color_h} and depth {depth_w}x{depth_h} dimensions don't match" - ) - - # Get intrinsics from camera_info - intrinsic = camera_info.get_K_matrix() - fx, fy = intrinsic[0, 0], intrinsic[1, 1] - cx, cy = intrinsic[0, 2], intrinsic[1, 2] - - # Verify intrinsics match image dimensions - if camera_info.width != color_w or camera_info.height != color_h: - # Scale intrinsics if resolution differs - scale_x = color_w / camera_info.width - scale_y = color_h / camera_info.height - fx *= scale_x - fy *= scale_y - cx *= scale_x - cy *= scale_y - - # Create Open3D images - color_o3d = o3d.geometry.Image(color_data.astype(np.uint8)) - - # Filter invalid depth values - depth_filtered = depth_data.copy() - valid_mask = np.isfinite(depth_filtered) & (depth_filtered > 0) - depth_filtered[~valid_mask] = 0.0 - depth_o3d = o3d.geometry.Image(depth_filtered.astype(np.float32)) - - o3d_intrinsic = o3d.camera.PinholeCameraIntrinsic( - width=color_w, - height=color_h, - fx=fx, - fy=fy, - cx=cx, - cy=cy, - ) - - # Create RGBD image and point cloud - rgbd = o3d.geometry.RGBDImage.create_from_color_and_depth( - color_o3d, - depth_o3d, - depth_scale=1.0, # Already scaled - depth_trunc=depth_trunc, - convert_rgb_to_intensity=False, - ) - - pcd = o3d.geometry.PointCloud.create_from_rgbd_image(rgbd, o3d_intrinsic) - - return cls( - pointcloud=pcd, - frame_id=depth_image.frame_id, - ts=depth_image.ts, - ) - - def __str__(self) -> str: - return f"PointCloud2(frame_id='{self.frame_id}', num_points={len(self)})" - - @functools.cached_property - def center(self) -> Vector3: - """Calculate the center of the pointcloud in world frame.""" - center = np.asarray(self.pointcloud.points).mean(axis=0) - return Vector3(*center) - - def points(self): # type: ignore[no-untyped-def] - """Get points (returns tensor positions, use as_numpy() for numpy array).""" - import open3d.core as o3c # type: ignore[import-untyped] - - self._ensure_tensor_initialized() - if "positions" not in self._pcd_tensor.point: - return o3c.Tensor(np.zeros((0, 3), dtype=np.float32)) - return self._pcd_tensor.point["positions"] - - def __add__(self, other: PointCloud2) -> PointCloud2: - """Combine two PointCloud2 instances into one. - - The resulting point cloud contains points from both inputs. - The frame_id and timestamp are taken from the first point cloud. - - Args: - other: Another PointCloud2 instance to combine with - - Returns: - New PointCloud2 instance containing combined points - """ - if not isinstance(other, PointCloud2): - raise ValueError("Can only add PointCloud2 to another PointCloud2") - - return PointCloud2( - pointcloud=self.pointcloud + other.pointcloud, - frame_id=self.frame_id, - ts=max(self.ts, other.ts), - ) - - def transform(self, tf: Transform) -> PointCloud2: - """Transform the pointcloud using a Transform object. - - Applies the rotation and translation from the transform to all points, - converting them into the transform's frame_id. - - Args: - tf: Transform object containing rotation and translation - - Returns: - New PointCloud2 instance with transformed points in the new frame - """ - import open3d as o3d # type: ignore[import-untyped] - - points, _ = self.as_numpy() - - if len(points) == 0: - return PointCloud2( - pointcloud=o3d.geometry.PointCloud(), - frame_id=tf.frame_id, - ts=self.ts, - ) - - # Build 4x4 transformation matrix from Transform - transform_matrix = tf.to_matrix() - - # Convert points to homogeneous coordinates (N, 4) - ones = np.ones((len(points), 1)) - points_homogeneous = np.hstack([points, ones]) - - # Apply transformation: (4, 4) @ (4, N) -> (4, N) -> transpose to (N, 4) - transformed_points = (transform_matrix @ points_homogeneous.T).T - - # Extract xyz coordinates (drop homogeneous coordinate) - transformed_xyz = transformed_points[:, :3].astype(np.float64) - - # Create new Open3D point cloud - new_pcd = o3d.geometry.PointCloud() - new_pcd.points = o3d.utility.Vector3dVector(transformed_xyz) - - # Colors are frame-independent, carry them through. - if self.pointcloud.has_colors(): - new_pcd.colors = self.pointcloud.colors - - return PointCloud2( - pointcloud=new_pcd, - frame_id=tf.frame_id, - ts=self.ts, - ) - - def voxel_downsample(self, voxel_size: float = 0.025) -> PointCloud2: - """Downsample the pointcloud with a voxel grid.""" - if voxel_size <= 0: - return self - if len(self.pointcloud.points) < 20: - return self - downsampled = self._pcd_tensor.voxel_down_sample(voxel_size) - return PointCloud2(pointcloud=downsampled, frame_id=self.frame_id, ts=self.ts) - - def as_numpy( - self, - ) -> tuple[np.ndarray[Any, Any], np.ndarray[Any, Any] | None]: - """Get points and colors as numpy arrays. - - Returns: - Tuple of (points, colors) where: - - points: Nx3 numpy array of 3D points - - colors: Nx3 array in [0, 1] range, or None if no colors - """ - points = np.asarray(self.pointcloud.points) - colors = np.asarray(self.pointcloud.colors) if self.pointcloud.has_colors() else None - return points, colors - - def points_f32(self) -> np.ndarray: - """Get positions as float32 numpy array, bypassing legacy float64 conversion.""" - self._ensure_tensor_initialized() - if "positions" in self._pcd_tensor.point: - arr = self._pcd_tensor.point["positions"].numpy() - return arr.astype(np.float32) if arr.dtype != np.float32 else arr # type: ignore[no-any-return] - return np.zeros((0, 3), dtype=np.float32) - - def intensities_f32(self) -> np.ndarray | None: - """Get per-point intensity values as a flat float32 array, or None if absent.""" - self._ensure_tensor_initialized() - if "intensities" in self._pcd_tensor.point: - arr = self._pcd_tensor.point["intensities"].numpy().flatten() - return arr.astype(np.float32) if arr.dtype != np.float32 else arr # type: ignore[no-any-return] - return None - - def _per_point_field(self, name: str, np_dtype: type) -> np.ndarray | None: - """Read a per-point attribute from the tensor, or None if absent.""" - self._ensure_tensor_initialized() - if name not in self._pcd_tensor.point: - return None - arr = self._pcd_tensor.point[name].numpy().flatten() - return arr.astype(np_dtype) if arr.dtype != np_dtype else arr # type: ignore[no-any-return] - - def offset_times_u32(self) -> np.ndarray | None: - """Per-point time offsets (ns relative to header stamp) as flat uint32, or None.""" - return self._per_point_field("offset_times", np.uint32) - - def tags_u8(self) -> np.ndarray | None: - """Per-point sensor tag bytes as flat uint8, or None if absent.""" - return self._per_point_field("tags", np.uint8) - - def lines_u8(self) -> np.ndarray | None: - """Per-point laser line numbers as flat uint8, or None if absent.""" - return self._per_point_field("lines", np.uint8) - - @functools.cached_property - def axis_aligned_bounding_box(self) -> o3d.geometry.AxisAlignedBoundingBox: - """Get axis-aligned bounding box of the point cloud.""" - return self.pointcloud.get_axis_aligned_bounding_box() - - @functools.cached_property - def oriented_bounding_box(self) -> o3d.geometry.OrientedBoundingBox: - """Get oriented bounding box of the point cloud.""" - return self.pointcloud.get_oriented_bounding_box() - - @functools.cached_property - def bounding_box_dimensions(self) -> tuple[float, float, float]: - """Get dimensions (width, height, depth) of axis-aligned bounding box.""" - bbox = self.axis_aligned_bounding_box - extent = bbox.get_extent() - return tuple(extent) - - def bounding_box_intersects(self, other: PointCloud2) -> bool: - # Get axis-aligned bounding boxes - bbox1 = self.axis_aligned_bounding_box - bbox2 = other.axis_aligned_bounding_box - - # Get min and max bounds - min1 = bbox1.get_min_bound() - max1 = bbox1.get_max_bound() - min2 = bbox2.get_min_bound() - max2 = bbox2.get_max_bound() - - # Check overlap in all three dimensions - # Boxes intersect if they overlap in ALL dimensions - return ( # type: ignore[no-any-return] - min1[0] <= max2[0] - and max1[0] >= min2[0] - and min1[1] <= max2[1] - and max1[1] >= min2[1] - and min1[2] <= max2[2] - and max1[2] >= min2[2] - ) - - def lcm_encode(self, frame_id: str | None = None) -> bytes: - """Convert to LCM PointCloud2 message with optional RGB colors.""" - msg = LCMPointCloud2() - - # Header - msg.header = Header() - msg.header.seq = 0 - msg.header.frame_id = frame_id or self.frame_id - - msg.header.stamp.sec = int(self.ts) - msg.header.stamp.nsec = int((self.ts - int(self.ts)) * 1e9) - - points, _ = self.as_numpy() - - # Check if pointcloud has colors - self._ensure_tensor_initialized() - has_colors = "colors" in self._pcd_tensor.point - - if len(points) == 0: - msg.height = 0 - msg.width = 0 - msg.point_step = 16 - msg.row_step = 0 - msg.data_length = 0 - msg.data = b"" - msg.is_dense = True - msg.is_bigendian = False - msg.fields_length = 4 - msg.fields = self._create_xyzrgb_fields() if has_colors else self._create_xyz_fields() - return msg.lcm_encode() # type: ignore[no-any-return] - - msg.height = 1 - msg.width = len(points) - - if has_colors: - # Get colors (0-1 range) and convert to uint8 - colors = self._pcd_tensor.point["colors"].numpy() - if colors.max() <= 1.0: - colors = (colors * 255).astype(np.uint8) - else: - colors = colors.astype(np.uint8) - - # Pack RGB into float32 (ROS convention: bytes are [padding, r, g, b]) - rgb_packed = np.zeros(len(points), dtype=np.float32) - rgb_uint32 = ( - (colors[:, 0].astype(np.uint32) << 16) - | (colors[:, 1].astype(np.uint32) << 8) - | colors[:, 2].astype(np.uint32) - ) - rgb_packed = rgb_uint32.view(np.float32) - - msg.fields = self._create_xyzrgb_fields() - msg.fields_length = 4 - msg.point_step = 16 # x, y, z, rgb (4 floats) - - point_data = np.column_stack([points, rgb_packed]).astype(np.float32) - else: - msg.fields = self._create_xyz_fields() - msg.fields_length = 4 - msg.point_step = 16 # x, y, z, intensity - - if "intensities" in self._pcd_tensor.point: - intensities = ( - self._pcd_tensor.point["intensities"].numpy().flatten().astype(np.float32) - ) - else: - intensities = np.zeros(len(points), dtype=np.float32) - - point_data = np.column_stack([points, intensities]).astype(np.float32) - - # Optional per-point attributes (offset_time/tag/line) extend the - # base 16-byte layout with packed extra fields, emitted only when present. - extras: list[tuple[str, np.dtype, int, np.ndarray]] = [] - offset_times = self.offset_times_u32() - if offset_times is not None: - extras.append(("offset_time", np.dtype(" None: - """Preload the heavy imports lcm_decode needs. - - Called at subscribe time (see LCMEncoderMixin.subscribe) so the first - decode doesn't stall the LCM handler thread on the open3d import. - """ - import open3d.core # type: ignore[import-untyped] # noqa: F401 - - @classmethod - def lcm_decode(cls, data: bytes) -> PointCloud2: - import open3d as o3d # type: ignore[import-untyped] - import open3d.core as o3c # type: ignore[import-untyped] - - msg = LCMPointCloud2.lcm_decode(data) - - if msg.width == 0 or msg.height == 0: - pc = o3d.geometry.PointCloud() - return cls( - pointcloud=pc, - frame_id=msg.header.frame_id if hasattr(msg, "header") else "", - ts=msg.header.stamp.sec + msg.header.stamp.nsec / 1e9 - if hasattr(msg, "header") and msg.header.stamp.sec > 0 - else None, - ) - - # Parse field offsets. The message is self-describing; a known field is - # honored only when its advertised datatype matches what we read it as, - # otherwise it is treated as absent rather than misread. - _expected_datatype = { - "x": PointField.FLOAT32, - "y": PointField.FLOAT32, - "z": PointField.FLOAT32, - "rgb": PointField.FLOAT32, - "intensity": PointField.FLOAT32, - "offset_time": PointField.UINT32, - "tag": PointField.UINT8, - "line": PointField.UINT8, - } - x_offset = y_offset = z_offset = rgb_offset = intensity_offset = None - offset_time_offset = tag_offset = line_offset = None - for msgfield in msg.fields: - if _expected_datatype.get(msgfield.name) != msgfield.datatype: - continue - if msgfield.name == "x": - x_offset = msgfield.offset - elif msgfield.name == "y": - y_offset = msgfield.offset - elif msgfield.name == "z": - z_offset = msgfield.offset - elif msgfield.name == "rgb": - rgb_offset = msgfield.offset - elif msgfield.name == "intensity": - intensity_offset = msgfield.offset - elif msgfield.name == "offset_time": - offset_time_offset = msgfield.offset - elif msgfield.name == "tag": - tag_offset = msgfield.offset - elif msgfield.name == "line": - line_offset = msgfield.offset - - if any(offset is None for offset in [x_offset, y_offset, z_offset]): - raise ValueError("PointCloud2 message missing X, Y, or Z msgfields") - - num_points = msg.width * msg.height - raw_data = msg.data - point_step = msg.point_step - - # Fast path for standard layout - if x_offset == 0 and y_offset == 4 and z_offset == 8 and point_step >= 12: - if point_step == 12: - points = np.frombuffer(raw_data, dtype=np.float32).reshape(-1, 3) - else: - dt = np.dtype( - [("x", " np.ndarray: - item_size = np.dtype(np_dtype).itemsize - dt_s = np.dtype( - [ - ("_pre", f"V{field_offset}"), - ("value", np_dtype), - ("_post", f"V{point_step - field_offset - item_size}"), - ] - ) - structured_s = np.frombuffer(raw_data, dtype=dt_s, count=num_points) - return np.ascontiguousarray(structured_s["value"]) - - if offset_time_offset is not None: - pcd_t.point["offset_times"] = o3c.Tensor( - _extract_scalar_field(offset_time_offset, "> 16) & 0xFF).astype(np.float32) / 255.0 - g = ((rgb_packed >> 8) & 0xFF).astype(np.float32) / 255.0 - b = (rgb_packed & 0xFF).astype(np.float32) / 255.0 - colors = np.column_stack([r, g, b]) - pcd_t.point["colors"] = o3c.Tensor(colors, dtype=o3c.float32) - - return cls( - pointcloud=pcd_t, - frame_id=msg.header.frame_id if hasattr(msg, "header") else "", - ts=msg.header.stamp.sec + msg.header.stamp.nsec / 1e9 - if hasattr(msg, "header") and msg.header.stamp.sec > 0 - else None, - ) - - def _create_xyz_fields(self) -> list: # type: ignore[type-arg] - """Create X, Y, Z, intensity field definitions.""" - fields = [] - for i, name in enumerate(["x", "y", "z", "intensity"]): - field = PointField() - field.name = name - field.offset = i * 4 - field.datatype = 7 # FLOAT32 - field.count = 1 - fields.append(field) - return fields - - def _create_xyzrgb_fields(self) -> list: # type: ignore[type-arg] - """Create X, Y, Z, RGB field definitions for colored pointclouds.""" - fields = [] - for i, name in enumerate(["x", "y", "z"]): - field = PointField() - field.name = name - field.offset = i * 4 - field.datatype = 7 # FLOAT32 - field.count = 1 - fields.append(field) - - # RGB field (packed as float32, ROS convention) - rgb_field = PointField() - rgb_field.name = "rgb" - rgb_field.offset = 12 - rgb_field.datatype = 7 # FLOAT32 (contains packed RGB) - rgb_field.count = 1 - fields.append(rgb_field) - - return fields - - def __len__(self) -> int: - """Return number of points.""" - self._ensure_tensor_initialized() - if "positions" not in self._pcd_tensor.point: - return 0 - return int(self._pcd_tensor.point["positions"].shape[0]) - - def to_rerun( - self, - voxel_size: float = 0.05, - colors: list[int] | None = None, - mode: str = "spheres", - fill_mode: str = "solid", - bottom_cutoff: float | None = None, - ui_radius: float = 2.0, - rgb: bool = True, - **kwargs: object, - ) -> Archetype: - """Convert to Rerun archetype for visualization. - - Args: - voxel_size: size for visualization - colors: Optional RGB color [r, g, b] for all points (0-255). - If None, uses height-based turbo colormap via class_ids - (requires register_colormap_annotation() called once). - mode: "points" for flat screen-space dots, "boxes" for cubes, or - "spheres" (default) for world-sized spheres. Only "points" holds a - constant on-screen size as you zoom; the others scale with voxel_size. - fill_mode: Fill mode for boxes - "solid", "majorwireframe", or "densewireframe" - ui_radius: Dot radius in screen-space UI points; "points" mode only. - rgb: Paint with the cloud's own per-point colors when it has any (an - RGBD cloud); off, or on a colorless cloud, the height colormap. - **kwargs: Additional args (ignored for compatibility) - - Returns: - rr.Points3D or rr.Boxes3D archetype for logging to Rerun - """ - import rerun as rr - - points = self.points_f32() - if len(points) == 0: - return rr.Points3D([]) if mode != "boxes" else rr.Boxes3D(centers=[]) - - if bottom_cutoff is not None: - points = points[points[:, 2] >= bottom_cutoff] - if len(points) == 0: - return rr.Points3D([]) if mode != "boxes" else rr.Boxes3D(centers=[]) - - # Use class_ids for height-based colormap (viewer resolves colors via AnnotationContext) - # Fall back to explicit colors when provided - class_ids = None - point_colors: Any = None - if colors is not None: - point_colors = colors - elif rgb and self.pointcloud.has_colors(): - own = np.asarray(self.pointcloud.colors) - if bottom_cutoff is not None: - own = own[np.asarray(self.pointcloud.points)[:, 2] >= bottom_cutoff] - point_colors = (own * 255).astype(np.uint8) - else: - z = points[:, 2] - class_ids = ((z - z.min()) / (z.max() - z.min() + 1e-8) * 255).astype(np.uint8) - - if mode == "points": - # Negative radii are screen-space UI points in rerun. - return rr.Points3D( - positions=points, colors=point_colors, class_ids=class_ids, radii=-ui_radius - ) - elif mode == "boxes": - half = voxel_size / 2 - return rr.Boxes3D( - centers=points, - half_sizes=[half, half, half], - colors=point_colors, - class_ids=class_ids, - fill_mode=fill_mode, # type: ignore[arg-type] - ) - else: - return rr.Points3D( - positions=points, - radii=voxel_size / 2, - colors=point_colors, - class_ids=class_ids, - ) - - def filter_by_height( - self, - min_height: float | None = None, - max_height: float | None = None, - ) -> PointCloud2: - """Filter points based on their height (z-coordinate). - - This method creates a new PointCloud2 containing only points within the specified - height range. All metadata (frame_id, timestamp) is preserved. - - Args: - min_height: Optional minimum height threshold. Points with z < min_height are filtered out. - If None, no lower limit is applied. - max_height: Optional maximum height threshold. Points with z > max_height are filtered out. - If None, no upper limit is applied. - - Returns: - New PointCloud2 instance containing only the filtered points. - - Raises: - ValueError: If both min_height and max_height are None (no filtering would occur). - - Example: - # Remove ground points below 0.1m height - filtered_pc = pointcloud.filter_by_height(min_height=0.1) - - # Keep only points between ground level and 2m height - filtered_pc = pointcloud.filter_by_height(min_height=0.0, max_height=2.0) - - # Remove points above 1.5m (e.g., ceiling) - filtered_pc = pointcloud.filter_by_height(max_height=1.5) - """ - import open3d as o3d # type: ignore[import-untyped] - - # Validate that at least one threshold is provided - if min_height is None and max_height is None: - raise ValueError("At least one of min_height or max_height must be specified") - - # Get points as numpy array - points, _ = self.as_numpy() - - if len(points) == 0: - # Empty pointcloud - return a copy - return PointCloud2( - pointcloud=o3d.geometry.PointCloud(), - frame_id=self.frame_id, - ts=self.ts, - ) - - # Extract z-coordinates (height values) - column index 2 - heights = points[:, 2] - - # Create boolean mask for filtering based on height thresholds - # Start with all True values - mask = np.ones(len(points), dtype=bool) - - # Apply minimum height filter if specified - if min_height is not None: - mask &= heights >= min_height - - # Apply maximum height filter if specified - if max_height is not None: - mask &= heights <= max_height - - # Apply mask to filter points - filtered_points = points[mask] - - # Create new PointCloud2 with filtered points - return PointCloud2.from_numpy( - points=filtered_points, - frame_id=self.frame_id, - timestamp=self.ts, - ) - - def __repr__(self) -> str: - """String representation.""" - return f"PointCloud(points={len(self)}, frame_id='{self.frame_id}', ts={self.ts})" diff --git a/dimos/msgs/sensor_msgs/RobotState.py b/dimos/msgs/sensor_msgs/RobotState.py deleted file mode 100644 index 20e41e7d248..00000000000 --- a/dimos/msgs/sensor_msgs/RobotState.py +++ /dev/null @@ -1,188 +0,0 @@ -# Copyright 2025-2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -"""LCM type definitions -This file automatically generated by lcm. -DO NOT MODIFY BY HAND!!!! -""" - -from io import BytesIO -import struct - - -class RobotState: - msg_name = "sensor_msgs.RobotState" - - __slots__ = [ - "cmdnum", - "error_code", - "joints", - "mode", - "mt_able", - "mt_brake", - "state", - "tcp_offset", - "tcp_pose", - "warn_code", - ] - - __typenames__ = [ - "int32_t", - "int32_t", - "int32_t", - "int32_t", - "int32_t", - "int32_t", - "int32_t", - "float", - "float", - "float", - ] - - __dimensions__ = [None, None, None, None, None, None, None, None, None, None] - - def __init__( # type: ignore[no-untyped-def] - self, - state: int = 0, - mode: int = 0, - error_code: int = 0, - warn_code: int = 0, - cmdnum: int = 0, - mt_brake: int = 0, - mt_able: int = 0, - tcp_pose=None, - tcp_offset=None, - joints=None, - ) -> None: - # LCM Type: int32_t - self.state = state - # LCM Type: int32_t - self.mode = mode - # LCM Type: int32_t - self.error_code = error_code - # LCM Type: int32_t - self.warn_code = warn_code - # LCM Type: int32_t - self.cmdnum = cmdnum - # LCM Type: int32_t - self.mt_brake = mt_brake - # LCM Type: int32_t - self.mt_able = mt_able - # LCM Type: float[] - TCP pose [x, y, z, roll, pitch, yaw] - self.tcp_pose = tcp_pose if tcp_pose is not None else [] - # LCM Type: float[] - TCP offset [x, y, z, roll, pitch, yaw] - self.tcp_offset = tcp_offset if tcp_offset is not None else [] - # LCM Type: float[] - Joint positions (variable length based on robot DOF) - self.joints = joints if joints is not None else [] - - def lcm_encode(self): # type: ignore[no-untyped-def] - """Encode for LCM transport (dimos uses lcm_encode method name).""" - return self.encode() # type: ignore[no-untyped-call] - - def encode(self): # type: ignore[no-untyped-def] - buf = BytesIO() - buf.write(RobotState._get_packed_fingerprint()) # type: ignore[no-untyped-call] - self._encode_one(buf) - return buf.getvalue() - - def _encode_one(self, buf) -> None: # type: ignore[no-untyped-def] - buf.write( - struct.pack( - ">iiiiiii", - self.state, - self.mode, - self.error_code, - self.warn_code, - self.cmdnum, - self.mt_brake, - self.mt_able, - ) - ) - # Encode tcp_pose array - buf.write(struct.pack(">i", len(self.tcp_pose))) - for val in self.tcp_pose: - buf.write(struct.pack(">f", val)) - # Encode tcp_offset array - buf.write(struct.pack(">i", len(self.tcp_offset))) - for val in self.tcp_offset: - buf.write(struct.pack(">f", val)) - # Encode joints array - buf.write(struct.pack(">i", len(self.joints))) - for val in self.joints: - buf.write(struct.pack(">f", val)) - - @classmethod - def lcm_decode(cls, data: bytes): # type: ignore[no-untyped-def] - """Decode from LCM transport (dimos uses lcm_decode method name).""" - return cls.decode(data) - - @classmethod - def decode(cls, data: bytes): # type: ignore[no-untyped-def] - if hasattr(data, "read"): - buf = data - else: - buf = BytesIO(data) # type: ignore[assignment] - if buf.read(8) != cls._get_packed_fingerprint(): # type: ignore[no-untyped-call] - raise ValueError("Decode error") - return cls._decode_one(buf) # type: ignore[no-untyped-call] - - @classmethod - def _decode_one(cls, buf): # type: ignore[no-untyped-def] - self = RobotState() - ( - self.state, - self.mode, - self.error_code, - self.warn_code, - self.cmdnum, - self.mt_brake, - self.mt_able, - ) = struct.unpack(">iiiiiii", buf.read(28)) - # Decode tcp_pose array - tcp_pose_len = struct.unpack(">i", buf.read(4))[0] - self.tcp_pose = [] - for _ in range(tcp_pose_len): - self.tcp_pose.append(struct.unpack(">f", buf.read(4))[0]) - # Decode tcp_offset array - tcp_offset_len = struct.unpack(">i", buf.read(4))[0] - self.tcp_offset = [] - for _ in range(tcp_offset_len): - self.tcp_offset.append(struct.unpack(">f", buf.read(4))[0]) - # Decode joints array - joints_len = struct.unpack(">i", buf.read(4))[0] - self.joints = [] - for _ in range(joints_len): - self.joints.append(struct.unpack(">f", buf.read(4))[0]) - return self - - @classmethod - def _get_hash_recursive(cls, parents): # type: ignore[no-untyped-def] - if cls in parents: - return 0 - # Updated hash to reflect new fields: tcp_pose, tcp_offset, joints - tmphash = (0x8C3B9A1FE7D24E6A) & 0xFFFFFFFFFFFFFFFF - tmphash = (((tmphash << 1) & 0xFFFFFFFFFFFFFFFF) + (tmphash >> 63)) & 0xFFFFFFFFFFFFFFFF - return tmphash - - _packed_fingerprint = None - - @classmethod - def _get_packed_fingerprint(cls): # type: ignore[no-untyped-def] - if cls._packed_fingerprint is None: - cls._packed_fingerprint = struct.pack(">Q", cls._get_hash_recursive([])) # type: ignore[no-untyped-call] - return cls._packed_fingerprint - - def get_hash(self): # type: ignore[no-untyped-def] - """Get the LCM hash of the struct""" - return struct.unpack(">Q", RobotState._get_packed_fingerprint())[0] # type: ignore[no-untyped-call] diff --git a/dimos/msgs/sensor_msgs/test_CameraInfo.py b/dimos/msgs/sensor_msgs/test_CameraInfo.py index e8a1c3a789d..6967b9c0fd3 100644 --- a/dimos/msgs/sensor_msgs/test_CameraInfo.py +++ b/dimos/msgs/sensor_msgs/test_CameraInfo.py @@ -13,23 +13,27 @@ # See the License for the specific language governing permissions and # limitations under the License. +from dimos_generated.sensor_msgs.msg import CameraInfo +from dimos_generated.std_msgs.msg import Header import numpy as np +from rosbags.typesys import Stores, get_typestore from dimos.constants import DIMOS_PROJECT_ROOT -from dimos.msgs.sensor_msgs.CameraInfo import CalibrationProvider, CameraInfo +from dimos.msgs.camera_info import CalibrationProvider, camera_info_from_yaml, intrinsic_matrix +from dimos.msgs.time import time_from_nanoseconds, to_nanoseconds -def test_lcm_encode_decode() -> None: - """Test LCM encode/decode preserves CameraInfo data.""" - print("Testing CameraInfo LCM encode/decode...") +def test_encode_decode() -> None: + """Test CDR encode/decode preserves CameraInfo data.""" + print("Testing CameraInfo CDR encode/decode...") # Create test camera info with sample calibration data original = CameraInfo( height=480, width=640, distortion_model="plumb_bob", - D=[-0.1, 0.05, 0.001, -0.002, 0.0], # 5 distortion coefficients - K=[ + d=[-0.1, 0.05, 0.001, -0.002, 0.0], # 5 distortion coefficients + k=[ 500.0, 0.0, 320.0, # fx, 0, cx @@ -40,8 +44,8 @@ def test_lcm_encode_decode() -> None: 0.0, 1.0, ], # 0, 0, 1 - R=[1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0], - P=[ + r=[1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0], + p=[ 500.0, 0.0, 320.0, @@ -57,20 +61,25 @@ def test_lcm_encode_decode() -> None: ], # 0, 0, 1, 0 binning_x=2, binning_y=2, - frame_id="camera_optical_frame", - ts=1234567890.123456, + header=Header( + frame_id="camera_optical_frame", stamp=time_from_nanoseconds(1234567890123456789) + ), ) # Set ROI - original.roi_x_offset = 100 - original.roi_y_offset = 50 - original.roi_height = 200 - original.roi_width = 300 - original.roi_do_rectify = True + original.roi.x_offset = 100 + original.roi.y_offset = 50 + original.roi.height = 200 + original.roi.width = 300 + original.roi.do_rectify = True # Encode and decode - binary_msg = original.lcm_encode() - decoded = CameraInfo.lcm_decode(binary_msg) + binary_msg = original.encode() + decoded = CameraInfo.decode(binary_msg) + independent = get_typestore(Stores.ROS2_JAZZY).deserialize_cdr(binary_msg, CameraInfo.msg_name) + assert independent.header.stamp.nanosec == 123456789 + assert independent.roi.x_offset == 100 + np.testing.assert_array_equal(independent.k, original.k) # Check basic properties assert original.height == decoded.height, ( @@ -85,27 +94,27 @@ def test_lcm_encode_decode() -> None: print(f"✓ Distortion model preserved: '{decoded.distortion_model}'") # Check distortion coefficients - assert len(original.D) == len(decoded.D), ( - f"D length mismatch: {len(original.D)} vs {len(decoded.D)}" + assert len(original.d) == len(decoded.d), ( + f"D length mismatch: {len(original.d)} vs {len(decoded.d)}" ) np.testing.assert_allclose( - original.D, decoded.D, rtol=1e-9, atol=1e-9, err_msg="Distortion coefficients don't match" + original.d, decoded.d, rtol=1e-9, atol=1e-9, err_msg="Distortion coefficients don't match" ) - print(f"✓ Distortion coefficients preserved: {len(decoded.D)} coefficients") + print(f"✓ Distortion coefficients preserved: {len(decoded.d)} coefficients") # Check camera matrices np.testing.assert_allclose( - original.K, decoded.K, rtol=1e-9, atol=1e-9, err_msg="K matrix doesn't match" + original.k, decoded.k, rtol=1e-9, atol=1e-9, err_msg="K matrix doesn't match" ) print("✓ Intrinsic matrix K preserved") np.testing.assert_allclose( - original.R, decoded.R, rtol=1e-9, atol=1e-9, err_msg="R matrix doesn't match" + original.r, decoded.r, rtol=1e-9, atol=1e-9, err_msg="R matrix doesn't match" ) print("✓ Rectification matrix R preserved") np.testing.assert_allclose( - original.P, decoded.P, rtol=1e-9, atol=1e-9, err_msg="P matrix doesn't match" + original.p, decoded.p, rtol=1e-9, atol=1e-9, err_msg="P matrix doesn't match" ) print("✓ Projection matrix P preserved") @@ -119,25 +128,25 @@ def test_lcm_encode_decode() -> None: print(f"✓ Binning preserved: {decoded.binning_x}x{decoded.binning_y}") # Check ROI - assert original.roi_x_offset == decoded.roi_x_offset, "ROI x_offset mismatch" - assert original.roi_y_offset == decoded.roi_y_offset, "ROI y_offset mismatch" - assert original.roi_height == decoded.roi_height, "ROI height mismatch" - assert original.roi_width == decoded.roi_width, "ROI width mismatch" - assert original.roi_do_rectify == decoded.roi_do_rectify, "ROI do_rectify mismatch" + assert original.roi.x_offset == decoded.roi.x_offset, "ROI x_offset mismatch" + assert original.roi.y_offset == decoded.roi.y_offset, "ROI y_offset mismatch" + assert original.roi.height == decoded.roi.height, "ROI height mismatch" + assert original.roi.width == decoded.roi.width, "ROI width mismatch" + assert original.roi.do_rectify == decoded.roi.do_rectify, "ROI do_rectify mismatch" print("✓ ROI preserved") # Check metadata - assert original.frame_id == decoded.frame_id, ( - f"Frame ID mismatch: '{original.frame_id}' vs '{decoded.frame_id}'" + assert original.header.frame_id == decoded.header.frame_id, ( + f"Frame ID mismatch: '{original.header.frame_id}' vs '{decoded.header.frame_id}'" ) - print(f"✓ Frame ID preserved: '{decoded.frame_id}'") + print(f"✓ Frame ID preserved: '{decoded.header.frame_id}'") - assert abs(original.ts - decoded.ts) < 1e-6, ( - f"Timestamp mismatch: {original.ts} vs {decoded.ts}" + assert to_nanoseconds(original.header.stamp) == to_nanoseconds(decoded.header.stamp), ( + f"Timestamp mismatch: {to_nanoseconds(original.header.stamp)} vs {to_nanoseconds(decoded.header.stamp)}" ) - print(f"✓ Timestamp preserved: {decoded.ts}") + print(f"✓ Timestamp preserved: {to_nanoseconds(decoded.header.stamp)}") - print("✓ LCM encode/decode test passed - all properties preserved!") + print("✓ CDR encode/decode test passed - all properties preserved!") def test_numpy_matrix_operations() -> None: @@ -148,29 +157,29 @@ def test_numpy_matrix_operations() -> None: # Test K matrix K = np.array([[525.0, 0.0, 319.5], [0.0, 525.0, 239.5], [0.0, 0.0, 1.0]]) - camera_info.set_K_matrix(K) - K_retrieved = camera_info.get_K_matrix() + camera_info.k = K.ravel() + K_retrieved = intrinsic_matrix(camera_info) np.testing.assert_allclose(K, K_retrieved, rtol=1e-9, atol=1e-9) print("✓ K matrix setter/getter works") # Test P matrix P = np.array([[525.0, 0.0, 319.5, 0.0], [0.0, 525.0, 239.5, 0.0], [0.0, 0.0, 1.0, 0.0]]) - camera_info.set_P_matrix(P) - P_retrieved = camera_info.get_P_matrix() + camera_info.p = P.ravel() + P_retrieved = np.asarray(camera_info.p).reshape(3, 4) np.testing.assert_allclose(P, P_retrieved, rtol=1e-9, atol=1e-9) print("✓ P matrix setter/getter works") # Test R matrix R = np.eye(3) - camera_info.set_R_matrix(R) - R_retrieved = camera_info.get_R_matrix() + camera_info.r = R.ravel() + R_retrieved = np.asarray(camera_info.r).reshape(3, 3) np.testing.assert_allclose(R, R_retrieved, rtol=1e-9, atol=1e-9) print("✓ R matrix setter/getter works") # Test D coefficients D = np.array([-0.2, 0.1, 0.001, -0.002, 0.05]) - camera_info.set_D_coeffs(D) - D_retrieved = camera_info.get_D_coeffs() + camera_info.d = D + D_retrieved = np.asarray(camera_info.d) np.testing.assert_allclose(D, D_retrieved, rtol=1e-9, atol=1e-9) print("✓ D coefficients setter/getter works") @@ -185,24 +194,24 @@ def test_equality() -> None: height=480, width=640, distortion_model="plumb_bob", - D=[-0.1, 0.05, 0.0, 0.0, 0.0], - frame_id="camera1", + d=[-0.1, 0.05, 0.0, 0.0, 0.0], + header=Header(frame_id="camera1"), ) info2 = CameraInfo( height=480, width=640, distortion_model="plumb_bob", - D=[-0.1, 0.05, 0.0, 0.0, 0.0], - frame_id="camera1", + d=[-0.1, 0.05, 0.0, 0.0, 0.0], + header=Header(frame_id="camera1"), ) info3 = CameraInfo( height=720, width=1280, # Different resolution distortion_model="plumb_bob", - D=[-0.1, 0.05, 0.0, 0.0, 0.0], - frame_id="camera1", + d=[-0.1, 0.05, 0.0, 0.0, 0.0], + header=Header(frame_id="camera1"), ) assert info1 == info2, "Identical CameraInfo objects should be equal" @@ -227,16 +236,16 @@ def test_camera_info_from_yaml() -> None: ) # Load CameraInfo from YAML - camera_info = CameraInfo.from_yaml(str(yaml_path)) + camera_info = camera_info_from_yaml(yaml_path, header=Header(frame_id="camera_optical")) # Verify loaded values assert camera_info.width == 640 assert camera_info.height == 376 assert camera_info.distortion_model == "plumb_bob" - assert camera_info.frame_id == "camera_optical" + assert camera_info.header.frame_id == "camera_optical" # Check camera matrix K - K = camera_info.get_K_matrix() + K = intrinsic_matrix(camera_info) assert K.shape == (3, 3) assert np.isclose(K[0, 0], 379.45267) # fx assert np.isclose(K[1, 1], 380.67871) # fy @@ -244,12 +253,12 @@ def test_camera_info_from_yaml() -> None: assert np.isclose(K[1, 2], 228.00954) # cy # Check distortion coefficients - D = camera_info.get_D_coeffs() + D = np.asarray(camera_info.d) assert len(D) == 5 assert np.isclose(D[0], -0.309435) # Check projection matrix P - P = camera_info.get_P_matrix() + P = np.asarray(camera_info.p).reshape(3, 4) assert P.shape == (3, 4) assert np.isclose(P[0, 0], 291.12888) diff --git a/dimos/msgs/sensor_msgs/test_CompressedImage.py b/dimos/msgs/sensor_msgs/test_CompressedImage.py index a7d54e3e735..ecae6d26b06 100644 --- a/dimos/msgs/sensor_msgs/test_CompressedImage.py +++ b/dimos/msgs/sensor_msgs/test_CompressedImage.py @@ -12,13 +12,21 @@ # See the License for the specific language governing permissions and # limitations under the License. +from dimos_generated.sensor_msgs.msg import CompressedImage +from dimos_generated.std_msgs.msg import Header import numpy as np import pytest -from dimos.msgs.sensor_msgs.CompressedImage import CompressedImage -from dimos.msgs.sensor_msgs.Image import Image, ImageFormat - -pytestmark = pytest.mark.skipif_no_turbojpeg +from dimos.msgs.image import ( + compressed_image_from_image, + image_from_array, + image_from_compressed, + image_to_bgr, + image_to_rgb, + image_view, +) +from dimos.msgs.time import time_from_seconds, to_nanoseconds +from dimos.visualization.rerun.message_helpers import image_archetype @pytest.fixture() @@ -27,40 +35,43 @@ def rgb_image(): gradient = np.linspace(0, 255, 640, dtype=np.uint8) data = np.broadcast_to(gradient, (480, 640)).copy() data = np.stack([data, data // 2, rng.randint(0, 50, (480, 640), dtype=np.uint8)], axis=-1) - return Image(data=data, format=ImageFormat.RGB, frame_id="cam", ts=1234.5678) + return image_from_array( + data, encoding="rgb8", header=Header(frame_id="cam", stamp=time_from_seconds(1234.5678)) + ) def test_jpeg_roundtrip_preserves_meta_and_pixels(rgb_image) -> None: - ci = CompressedImage.from_image(rgb_image, quality=90) - assert ci.format == "jpeg" - assert ci.frame_id == "cam" - assert ci.ts == rgb_image.ts - assert 0 < len(ci.data) < rgb_image.data.nbytes // 4 - - img = ci.decode() - assert img.format == ImageFormat.RGB - assert img.frame_id == "cam" - assert img.ts == rgb_image.ts - assert img.shape == rgb_image.shape - diff = np.abs(img.data.astype(int) - rgb_image.data.astype(int)).mean() + ci = compressed_image_from_image(rgb_image, quality=90) + assert "; jpeg compressed " in ci.format + assert ci.header.frame_id == "cam" + assert to_nanoseconds(ci.header.stamp) == to_nanoseconds(rgb_image.header.stamp) + assert 0 < len(ci.data) < image_view(rgb_image).nbytes // 4 + img = image_from_compressed(ci) + assert img.encoding in ("rgb8", "bgr8") + assert img.header.frame_id == "cam" + assert to_nanoseconds(img.header.stamp) == to_nanoseconds(rgb_image.header.stamp) + assert image_view(img).shape == image_view(rgb_image).shape + diff = np.abs(image_to_rgb(img).astype(int) - image_view(rgb_image).astype(int)).mean() assert diff < 5, f"JPEG q90 mean pixel error too high: {diff}" def test_png_roundtrip_is_lossless_bgr(rgb_image) -> None: - bgr = rgb_image.to_bgr() - ci = CompressedImage.from_image(bgr, format="png") - img = ci.decode() - assert img.format == ImageFormat.BGR - assert np.array_equal(img.data, bgr.data) - assert img.ts == bgr.ts + bgr = image_from_array(image_to_bgr(rgb_image), encoding="bgr8", header=rgb_image.header) + ci = compressed_image_from_image(bgr, format="png") + img = image_from_compressed(ci) + assert img.encoding == "bgr8" + assert np.array_equal(image_view(img), image_view(bgr)) + assert to_nanoseconds(img.header.stamp) == to_nanoseconds(bgr.header.stamp) def test_png_roundtrip_is_lossless_gray16() -> None: data = np.arange(100 * 80, dtype=np.uint16).reshape(100, 80) - src = Image(data=data, format=ImageFormat.GRAY16, frame_id="d", ts=1.0) - img = CompressedImage.from_image(src, format="png").decode() - assert img.format == ImageFormat.GRAY16 - assert np.array_equal(img.data, data) + src = image_from_array( + data, encoding="mono16", header=Header(frame_id="d", stamp=time_from_seconds(1.0)) + ) + img = image_from_compressed(compressed_image_from_image(src, format="png")) + assert img.encoding == "mono16" + assert np.array_equal(image_view(img), data) def test_png_decode_alpha_is_bgra() -> None: @@ -70,40 +81,44 @@ def test_png_decode_alpha_is_bgra() -> None: arr[..., 3] = 128 ok, buf = cv2.imencode(".png", arr) assert ok - img = CompressedImage(data=buf.tobytes(), format="png").decode() - assert img.format == ImageFormat.BGRA - assert np.array_equal(img.data, arr) + img = image_from_compressed(CompressedImage(data=buf.tobytes(), format="png")) + assert img.encoding == "bgra8" + assert np.array_equal(image_view(img), arr) def test_lcm_roundtrip(rgb_image) -> None: - ci = CompressedImage.from_image(rgb_image) - wire = ci.lcm_encode() - out = CompressedImage.lcm_decode(wire) + ci = compressed_image_from_image(rgb_image) + wire = ci.encode() + out = CompressedImage.decode(wire) assert out.data == ci.data - assert out.format == "jpeg" - assert out.frame_id == "cam" - assert abs(out.ts - ci.ts) < 1e-6 + assert "; jpeg compressed " in out.format + assert out.header.frame_id == "cam" + assert abs(to_nanoseconds(out.header.stamp) - to_nanoseconds(ci.header.stamp)) < 1e-06 def test_jpeg_rejects_depth_formats() -> None: - depth = Image(data=np.zeros((10, 10), dtype=np.uint16), format=ImageFormat.GRAY16) - with pytest.raises(ValueError, match="use format='png'"): - CompressedImage.from_image(depth) + depth = image_from_array( + np.zeros((10, 10), dtype=np.uint16), encoding="mono16", header=Header() + ) + with pytest.raises(ValueError, match="JPEG requires 8-bit"): + compressed_image_from_image(depth) def test_png_rejects_float_depth() -> None: - depth = Image(data=np.zeros((10, 10), dtype=np.float32), format=ImageFormat.DEPTH) + depth = image_from_array( + np.zeros((10, 10), dtype=np.float32), encoding="32FC1", header=Header() + ) with pytest.raises(ValueError, match="PNG cannot encode"): - CompressedImage.from_image(depth, format="png") + compressed_image_from_image(depth, format="png") def test_from_image_rejects_non_image() -> None: with pytest.raises(TypeError): - CompressedImage.from_image(b"not an image") # type: ignore[arg-type] + compressed_image_from_image(b"not an image") def test_max_width_resizes(rgb_image) -> None: - img = CompressedImage.from_image(rgb_image, max_width=320).decode() + img = image_from_compressed(compressed_image_from_image(rgb_image, max_width=320)) assert img.width <= 320 assert img.height <= 320 @@ -111,46 +126,73 @@ def test_max_width_resizes(rgb_image) -> None: def test_to_rerun_is_encoded_image(rgb_image) -> None: import rerun as rr - assert isinstance(CompressedImage.from_image(rgb_image).to_rerun(), rr.EncodedImage) + assert isinstance(image_archetype(compressed_image_from_image(rgb_image)), rr.EncodedImage) def test_jxl_roundtrip_is_lossless_gray16() -> None: data = np.arange(100 * 80, dtype=np.uint16).reshape(100, 80) - src = Image(data=data, format=ImageFormat.GRAY16, frame_id="d", ts=1.0) - ci = CompressedImage.from_image(src, format="jxl") - assert ci.format == "jxl" - img = ci.decode() - assert img.format == ImageFormat.GRAY16 - assert np.array_equal(img.data, data) + src = image_from_array( + data, encoding="mono16", header=Header(frame_id="d", stamp=time_from_seconds(1.0)) + ) + ci = compressed_image_from_image(src, format="jxl") + assert "; jxl compressed " in ci.format + img = image_from_compressed(ci) + assert img.encoding == "mono16" + assert np.array_equal(image_view(img), data) def test_jxl_roundtrip_is_lossless_float_depth() -> None: data = np.linspace(0.1, 10.0, 100 * 80, dtype=np.float32).reshape(100, 80) - src = Image(data=data, format=ImageFormat.DEPTH, frame_id="d", ts=2.0) - ci = CompressedImage.from_image(src, format="jxl") - img = ci.decode() - assert img.format == ImageFormat.DEPTH - assert np.array_equal(img.data, data) - assert img.ts == src.ts - assert img.frame_id == "d" + src = image_from_array( + data, encoding="32FC1", header=Header(frame_id="d", stamp=time_from_seconds(2.0)) + ) + ci = compressed_image_from_image(src, format="jxl") + img = image_from_compressed(ci) + assert img.encoding == "32FC1" + assert np.array_equal(image_view(img), data) + assert to_nanoseconds(img.header.stamp) == to_nanoseconds(src.header.stamp) + assert img.header.frame_id == "d" def test_jxl_lossy_rgb_honors_quality(rgb_image) -> None: - ci = CompressedImage.from_image(rgb_image, format="jxl", quality=90) - assert 0 < len(ci.data) < rgb_image.data.nbytes // 4 - img = ci.decode() - assert img.format == ImageFormat.RGB - diff = np.abs(img.data.astype(int) - rgb_image.data.astype(int)).mean() + ci = compressed_image_from_image(rgb_image, format="jxl", quality=90) + assert 0 < len(ci.data) < image_view(rgb_image).nbytes // 4 + img = image_from_compressed(ci) + assert img.encoding in ("rgb8", "bgr8") + diff = np.abs(image_to_rgb(img).astype(int) - image_view(rgb_image).astype(int)).mean() assert diff < 5, f"JXL q90 mean pixel error too high: {diff}" def test_jxl_effort_trades_cpu_for_size(rgb_image) -> None: - fast = CompressedImage.from_image(rgb_image, format="jxl", effort=1) - thorough = CompressedImage.from_image(rgb_image, format="jxl", effort=7) + fast = compressed_image_from_image(rgb_image, format="jxl", effort=1) + thorough = compressed_image_from_image(rgb_image, format="jxl", effort=7) assert len(thorough.data) <= len(fast.data) def test_jxl_rejects_float64() -> None: - depth = Image(data=np.zeros((10, 10), dtype=np.float64), format=ImageFormat.DEPTH) + depth = image_from_array(np.zeros((10, 10), dtype=np.float64), encoding="64FC1") with pytest.raises(ValueError, match="JXL cannot encode"): - CompressedImage.from_image(depth, format="jxl") + compressed_image_from_image(depth, format="jxl") + + +@pytest.mark.parametrize("codec", ["png", "jxl"]) +def test_compressed_uint16_depth_keeps_units_and_rerun_depth(codec: str) -> None: + import rerun as rr + + pixels = np.arange(80, dtype=np.uint16).reshape(10, 8) + source = image_from_array(pixels, encoding="16UC1", header=Header(frame_id="depth")) + compressed = compressed_image_from_image(source, format=codec) + decoded = image_from_compressed(compressed) + assert decoded.encoding == "16UC1" + assert decoded.header == source.header + np.testing.assert_array_equal(image_view(decoded), pixels) + if codec == "jxl": + assert isinstance(image_archetype(compressed), rr.DepthImage) + + +@pytest.mark.parametrize("codec", ["png", "jxl"]) +def test_compressed_big_endian_depth(codec: str) -> None: + pixels = np.arange(80, dtype=">u2").reshape(10, 8) * 257 + source = image_from_array(pixels.astype(">u2"), encoding="16UC1") + decoded = image_from_compressed(compressed_image_from_image(source, format=codec)) + np.testing.assert_array_equal(image_view(decoded), pixels) diff --git a/dimos/msgs/sensor_msgs/test_Joy.py b/dimos/msgs/sensor_msgs/test_Joy.py index 499c2bb860f..d49c81bee90 100644 --- a/dimos/msgs/sensor_msgs/test_Joy.py +++ b/dimos/msgs/sensor_msgs/test_Joy.py @@ -14,175 +14,133 @@ # limitations under the License. -from dimos.msgs.sensor_msgs.Joy import Joy +from dimos_generated.sensor_msgs.msg import Joy +from dimos_generated.std_msgs.msg import Header +import pytest +from rosbags.typesys import Stores, get_typestore +from dimos.msgs.time import time_from_nanoseconds, time_from_seconds, to_nanoseconds -def test_lcm_encode_decode() -> None: - """Test LCM encode/decode preserves Joy data.""" - print("Testing Joy LCM encode/decode...") - # Create test joy message with sample gamepad data +def test_cdr_encode_decode() -> None: + """Test CDR encode/decode preserves Joy data.""" + print("Testing Joy CDR encode/decode...") original = Joy( - ts=1234567890.123456789, - frame_id="gamepad", - axes=[0.5, -0.25, 1.0, -1.0, 0.0, 0.75], # 6 axes (e.g., left/right sticks + triggers) - buttons=[1, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0], # 12 buttons + axes=[0.5, -0.25, 1.0, -1.0, 0.0, 0.75], + buttons=[1, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0], + header=Header(stamp=time_from_seconds(1234567890.1234567), frame_id="gamepad"), ) - - # Encode to LCM bytes - encoded = original.lcm_encode() + encoded = original.encode() assert isinstance(encoded, bytes) assert len(encoded) > 0 - - # Decode back - decoded = Joy.lcm_decode(encoded) - - # Verify all fields match - assert abs(decoded.ts - original.ts) < 1e-9 - assert decoded.frame_id == original.frame_id + decoded = Joy.decode(encoded) + assert abs(to_nanoseconds(decoded.header.stamp) - to_nanoseconds(original.header.stamp)) < 1e-09 + assert decoded.header.frame_id == original.header.frame_id assert decoded.axes == original.axes assert decoded.buttons == original.buttons - - print("✓ Joy LCM encode/decode test passed") + print("✓ Joy CDR encode/decode test passed") def test_initialization_methods() -> None: """Test various initialization methods for Joy.""" print("Testing Joy initialization methods...") - - # Test default initialization joy1 = Joy() assert joy1.axes == [] assert joy1.buttons == [] - assert joy1.frame_id == "" - assert joy1.ts > 0 # Should have current time - - # Test full initialization - joy2 = Joy(ts=1234567890.0, frame_id="xbox_controller", axes=[0.1, 0.2, 0.3], buttons=[1, 0, 1]) - assert joy2.ts == 1234567890.0 - assert joy2.frame_id == "xbox_controller" - assert joy2.axes == [0.1, 0.2, 0.3] + assert joy1.header.frame_id == "" + assert to_nanoseconds(joy1.header.stamp) == 0 + joy2 = Joy( + axes=[0.1, 0.2, 0.3], + buttons=[1, 0, 1], + header=Header(stamp=time_from_seconds(1234567890.0), frame_id="xbox_controller"), + ) + assert to_nanoseconds(joy2.header.stamp) == 1234567890000000000 + assert joy2.header.frame_id == "xbox_controller" + assert list(joy2.axes) == pytest.approx([0.1, 0.2, 0.3], abs=1e-6) assert joy2.buttons == [1, 0, 1] - - # Test tuple initialization - joy3 = Joy(([0.5, -0.5], [1, 1, 0])) + joy3 = Joy(axes=[0.5, -0.5], buttons=[1, 1, 0]) assert joy3.axes == [0.5, -0.5] assert joy3.buttons == [1, 1, 0] - - # Test dict initialization - joy4 = Joy({"axes": [0.7, 0.8], "buttons": [0, 1], "frame_id": "ps4_controller"}) - assert joy4.axes == [0.7, 0.8] + joy4 = Joy(axes=[0.7, 0.8], buttons=[0, 1], header=Header(frame_id="ps4_controller")) + assert list(joy4.axes) == pytest.approx([0.7, 0.8], abs=1e-6) assert joy4.buttons == [0, 1] - assert joy4.frame_id == "ps4_controller" - - # Test copy constructor - joy5 = Joy(joy2) - assert joy5.ts == joy2.ts - assert joy5.frame_id == joy2.frame_id + assert joy4.header.frame_id == "ps4_controller" + joy5 = Joy.decode(joy2.encode()) + assert to_nanoseconds(joy5.header.stamp) == to_nanoseconds(joy2.header.stamp) + assert joy5.header.frame_id == joy2.header.frame_id assert joy5.axes == joy2.axes assert joy5.buttons == joy2.buttons - assert joy5 is not joy2 # Different objects - + assert joy5 is not joy2 print("✓ Joy initialization methods test passed") def test_equality() -> None: """Test Joy equality comparison.""" print("Testing Joy equality...") - - joy1 = Joy(ts=1000.0, frame_id="controller1", axes=[0.5, -0.5], buttons=[1, 0, 1]) - - joy2 = Joy(ts=1000.0, frame_id="controller1", axes=[0.5, -0.5], buttons=[1, 0, 1]) - + joy1 = Joy( + axes=[0.5, -0.5], + buttons=[1, 0, 1], + header=Header(stamp=time_from_seconds(1000.0), frame_id="controller1"), + ) + joy2 = Joy( + axes=[0.5, -0.5], + buttons=[1, 0, 1], + header=Header(stamp=time_from_seconds(1000.0), frame_id="controller1"), + ) joy3 = Joy( - ts=1000.0, - frame_id="controller2", # Different frame_id axes=[0.5, -0.5], buttons=[1, 0, 1], + header=Header(stamp=time_from_seconds(1000.0), frame_id="controller2"), ) - joy4 = Joy( - ts=1000.0, - frame_id="controller1", - axes=[0.6, -0.5], # Different axes + axes=[0.6, -0.5], buttons=[1, 0, 1], + header=Header(stamp=time_from_seconds(1000.0), frame_id="controller1"), ) - - # Same content should be equal assert joy1 == joy2 - - # Different frame_id should not be equal assert joy1 != joy3 - - # Different axes should not be equal assert joy1 != joy4 - - # Different type should not be equal assert joy1 != "not a joy" assert joy1 != 42 - print("✓ Joy equality test passed") -def test_string_representation() -> None: - """Test Joy string representations.""" - print("Testing Joy string representations...") - +def test_independent_schema_decoding() -> None: joy = Joy( - ts=1234567890.123, - frame_id="test_controller", + header=Header(stamp=time_from_nanoseconds(1234567890123456789), frame_id="test_controller"), axes=[0.1, -0.2, 0.3, 0.4], buttons=[1, 0, 1, 0, 0, 1], ) - - # Test __str__ - str_repr = str(joy) - assert "Joy" in str_repr - assert "axes=4 values" in str_repr - assert "buttons=6 values" in str_repr - assert "test_controller" in str_repr - - # Test __repr__ - repr_str = repr(joy) - assert "Joy" in repr_str - assert "1234567890.123" in repr_str - assert "test_controller" in repr_str - assert "[0.1, -0.2, 0.3, 0.4]" in repr_str - assert "[1, 0, 1, 0, 0, 1]" in repr_str - - print("✓ Joy string representation test passed") + decoded = get_typestore(Stores.ROS2_JAZZY).deserialize_cdr(joy.encode(), Joy.msg_name) + assert decoded.header.frame_id == "test_controller" + assert decoded.header.stamp.sec == 1234567890 + assert decoded.header.stamp.nanosec == 123456789 + assert list(decoded.axes) == pytest.approx([0.1, -0.2, 0.3, 0.4], abs=1e-6) + assert list(decoded.buttons) == [1, 0, 1, 0, 0, 1] def test_edge_cases() -> None: """Test Joy with edge cases.""" print("Testing Joy edge cases...") - - # Empty axes and buttons joy1 = Joy(axes=[], buttons=[]) assert joy1.axes == [] assert joy1.buttons == [] - encoded = joy1.lcm_encode() - decoded = Joy.lcm_decode(encoded) + encoded = joy1.encode() + decoded = Joy.decode(encoded) assert decoded.axes == [] assert decoded.buttons == [] - - # Large number of axes and buttons many_axes = [float(i) / 100.0 for i in range(20)] many_buttons = [i % 2 for i in range(32)] joy2 = Joy(axes=many_axes, buttons=many_buttons) assert len(joy2.axes) == 20 assert len(joy2.buttons) == 32 - encoded = joy2.lcm_encode() - decoded = Joy.lcm_decode(encoded) - # Check axes with floating point tolerance + encoded = joy2.encode() + decoded = Joy.decode(encoded) assert len(decoded.axes) == len(many_axes) for i, (a, b) in enumerate(zip(decoded.axes, many_axes, strict=False)): - assert abs(a - b) < 1e-6, f"Axis {i}: {a} != {b}" + assert abs(a - b) < 1e-06, f"Axis {i}: {a} != {b}" assert decoded.buttons == many_buttons - - # Extreme axis values extreme_axes = [-1.0, 1.0, 0.0, -0.999999, 0.999999] joy3 = Joy(axes=extreme_axes) - assert joy3.axes == extreme_axes - + assert list(joy3.axes) == pytest.approx(extreme_axes, abs=1e-6) print("✓ Joy edge cases test passed") diff --git a/dimos/msgs/sensor_msgs/test_PointCloud2.py b/dimos/msgs/sensor_msgs/test_PointCloud2.py index f02c07957a1..400edefa7ab 100644 --- a/dimos/msgs/sensor_msgs/test_PointCloud2.py +++ b/dimos/msgs/sensor_msgs/test_PointCloud2.py @@ -14,26 +14,92 @@ # limitations under the License. +from dimos_generated.sensor_msgs.msg import PointCloud2, PointField +from dimos_generated.std_msgs.msg import Header import numpy as np import pytest -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 +from dimos.msgs.pointcloud import ( + cloud_bounds_intersect, + pointcloud_from_xyz, + pointcloud_from_xyz_rgb, + pointcloud_to_open3d, + pointcloud_view, + pointcloud_xyz, +) +from dimos.msgs.time import time_from_seconds, to_nanoseconds from dimos.robot.unitree.type.lidar import pointcloud2_from_webrtc_lidar -from dimos.utils.testing.replay import SensorReplay +from dimos.visualization.rerun.message_helpers import cloud_archetype + + +def _cloud( + points, + *, + frame_id="", + timestamp=0.0, + intensities=None, + offset_times=None, + tags=None, + lines=None, +): + extras = [ + (name, values, dtype, datatype) + for name, values, dtype, datatype in [ + ("intensity", intensities, " None: - """Test LCM encode/decode preserves pointcloud data.""" - replay = SensorReplay("office_lidar", autocast=pointcloud2_from_webrtc_lidar) - lidar_msg: PointCloud2 = replay.load_one("lidar_data_021") - binary_msg = lidar_msg.lcm_encode() - decoded = PointCloud2.lcm_decode(binary_msg) +def test_cdr_encode_decode() -> None: + points = np.arange(300, dtype=np.float32).reshape(100, 3) / 10 + lidar_msg = pointcloud2_from_webrtc_lidar({"data": {"stamp": 12.5, "data": {"points": points}}}) + + binary_msg = lidar_msg.encode() + decoded = PointCloud2.decode(binary_msg) # 1. Check number of points - original_points, _ = lidar_msg.as_numpy() - decoded_points, _ = decoded.as_numpy() + original_points = pointcloud_xyz(lidar_msg) + decoded_points = pointcloud_xyz(decoded) assert len(original_points) == len(decoded_points), ( f"Point count mismatch: {len(original_points)} vs {len(decoded_points)}" @@ -50,68 +116,72 @@ def test_lcm_encode_decode() -> None: ) # 3. Check frame_id is preserved - assert lidar_msg.frame_id == decoded.frame_id, ( - f"Frame ID mismatch: '{lidar_msg.frame_id}' vs '{decoded.frame_id}'" + assert lidar_msg.header.frame_id == decoded.header.frame_id, ( + f"Frame ID mismatch: '{lidar_msg.header.frame_id}' vs '{decoded.header.frame_id}'" ) # 4. Check timestamp is preserved (within reasonable tolerance for float precision) - if lidar_msg.ts is not None and decoded.ts is not None: - assert abs(lidar_msg.ts - decoded.ts) < 1e-6, ( - f"Timestamp mismatch: {lidar_msg.ts} vs {decoded.ts}" + if ( + to_nanoseconds(lidar_msg.header.stamp) is not None + and to_nanoseconds(decoded.header.stamp) is not None + ): + assert ( + abs(to_nanoseconds(lidar_msg.header.stamp) - to_nanoseconds(decoded.header.stamp)) + < 1e-6 + ), ( + f"Timestamp mismatch: {to_nanoseconds(lidar_msg.header.stamp)} vs {to_nanoseconds(decoded.header.stamp)}" ) # 5. Check pointcloud properties - assert len(lidar_msg.pointcloud.points) == len(decoded.pointcloud.points), ( - "Open3D pointcloud size mismatch" - ) + assert len(pointcloud_to_open3d(lidar_msg).points) == len( + pointcloud_to_open3d(decoded).points + ), "Open3D pointcloud size mismatch" -def test_lcm_intensity_round_trip() -> None: +def test_cdr_intensity_round_trip() -> None: """Test that intensity values survive an lcm_encode → lcm_decode round trip.""" points = np.array([[1.0, 2.0, 3.0], [4.0, 5.0, 6.0], [7.0, 8.0, 9.0]], dtype=np.float32) intensities = np.array([0.25, 1.1, 0.0], dtype=np.float32) - original = PointCloud2.from_numpy( - points, frame_id="map", timestamp=42.0, intensities=intensities - ) + original = _cloud(points, frame_id="map", timestamp=42.0, intensities=intensities) # Verify getter before encoding - got = original.intensities_f32() + got = _field(original, "intensity") assert got is not None, "intensities_f32() returned None on source cloud" np.testing.assert_allclose(got, intensities, atol=1e-6) # Round-trip through LCM - binary = original.lcm_encode() - decoded = PointCloud2.lcm_decode(binary) + binary = original.encode() + decoded = PointCloud2.decode(binary) # Positions preserved - decoded_pts, _ = decoded.as_numpy() + decoded_pts = pointcloud_xyz(decoded) np.testing.assert_allclose(decoded_pts.astype(np.float32), points, atol=1e-6) # Intensities preserved - decoded_intensities = decoded.intensities_f32() + decoded_intensities = _field(decoded, "intensity") assert decoded_intensities is not None, "intensities lost after lcm_decode" np.testing.assert_allclose(decoded_intensities, intensities, atol=1e-6) -def test_lcm_no_intensity_round_trip() -> None: +def test_cdr_no_intensity_round_trip() -> None: """Clouds without intensity should round-trip without creating spurious intensities.""" points = np.array([[1.0, 2.0, 3.0], [4.0, 5.0, 6.0]], dtype=np.float32) - original = PointCloud2.from_numpy(points, frame_id="map", timestamp=1.0) + original = _cloud(points, frame_id="map", timestamp=1.0) - assert original.intensities_f32() is None + assert _field(original, "intensity") is None - binary = original.lcm_encode() - decoded = PointCloud2.lcm_decode(binary) + binary = original.encode() + decoded = PointCloud2.decode(binary) # No intensities should appear (all-zero wire data is ignored) - assert decoded.intensities_f32() is None, "Spurious intensities created from zero wire data" + assert _field(decoded, "intensity") is None, "Spurious intensities created from zero wire data" - decoded_pts, _ = decoded.as_numpy() + decoded_pts = pointcloud_xyz(decoded) np.testing.assert_allclose(decoded_pts.astype(np.float32), points, atol=1e-6) -def test_lcm_per_point_timing_round_trip() -> None: +def test_cdr_per_point_timing_round_trip() -> None: """offset_time/tag/line survive an lcm_encode → lcm_decode round trip.""" points = np.array([[1.0, 2.0, 3.0], [4.0, 5.0, 6.0], [7.0, 8.0, 9.0]], dtype=np.float32) intensities = np.array([10.0, 20.0, 30.0], dtype=np.float32) @@ -120,7 +190,7 @@ def test_lcm_per_point_timing_round_trip() -> None: tags = np.array([0, 16, 32], dtype=np.uint8) lines = np.array([0, 1, 3], dtype=np.uint8) - original = PointCloud2.from_numpy( + original = _cloud( points, frame_id="mid360_link", timestamp=100.5, @@ -130,28 +200,28 @@ def test_lcm_per_point_timing_round_trip() -> None: lines=lines, ) - got_offsets = original.offset_times_u32() + got_offsets = _field(original, "offset_time") assert got_offsets is not None np.testing.assert_array_equal(got_offsets, offset_times) - decoded = PointCloud2.lcm_decode(original.lcm_encode()) + decoded = PointCloud2.decode(original.encode()) - decoded_pts, _ = decoded.as_numpy() + decoded_pts = pointcloud_xyz(decoded) np.testing.assert_allclose(decoded_pts.astype(np.float32), points, atol=1e-6) - decoded_intensities = decoded.intensities_f32() + decoded_intensities = _field(decoded, "intensity") assert decoded_intensities is not None np.testing.assert_allclose(decoded_intensities, intensities, atol=1e-6) - decoded_offsets = decoded.offset_times_u32() + decoded_offsets = _field(decoded, "offset_time") assert decoded_offsets is not None, "offset_time lost after lcm_decode" assert decoded_offsets.dtype == np.uint32 np.testing.assert_array_equal(decoded_offsets, offset_times) - decoded_tags = decoded.tags_u8() + decoded_tags = _field(decoded, "tag") assert decoded_tags is not None, "tag lost after lcm_decode" np.testing.assert_array_equal(decoded_tags, tags) - decoded_lines = decoded.lines_u8() + decoded_lines = _field(decoded, "line") assert decoded_lines is not None, "line lost after lcm_decode" np.testing.assert_array_equal(decoded_lines, lines) @@ -159,80 +229,72 @@ def test_lcm_per_point_timing_round_trip() -> None: def test_bounding_box_intersects() -> None: """Test bounding_box_intersects method with various scenarios.""" # Test 1: Overlapping boxes - pc1 = PointCloud2.from_numpy(np.array([[0, 0, 0], [2, 2, 2]])) - pc2 = PointCloud2.from_numpy(np.array([[1, 1, 1], [3, 3, 3]])) - assert pc1.bounding_box_intersects(pc2) - assert pc2.bounding_box_intersects(pc1) # Should be symmetric + pc1 = _cloud(np.array([[0, 0, 0], [2, 2, 2]])) + pc2 = _cloud(np.array([[1, 1, 1], [3, 3, 3]])) + assert cloud_bounds_intersect(pc1, pc2) + assert cloud_bounds_intersect(pc2, pc1) # Should be symmetric # Test 2: Non-overlapping boxes - pc3 = PointCloud2.from_numpy(np.array([[0, 0, 0], [1, 1, 1]])) - pc4 = PointCloud2.from_numpy(np.array([[2, 2, 2], [3, 3, 3]])) - assert not pc3.bounding_box_intersects(pc4) - assert not pc4.bounding_box_intersects(pc3) + pc3 = _cloud(np.array([[0, 0, 0], [1, 1, 1]])) + pc4 = _cloud(np.array([[2, 2, 2], [3, 3, 3]])) + assert not cloud_bounds_intersect(pc3, pc4) + assert not cloud_bounds_intersect(pc4, pc3) # Test 3: Touching boxes (edge case - should be True) - pc5 = PointCloud2.from_numpy(np.array([[0, 0, 0], [1, 1, 1]])) - pc6 = PointCloud2.from_numpy(np.array([[1, 1, 1], [2, 2, 2]])) - assert pc5.bounding_box_intersects(pc6) - assert pc6.bounding_box_intersects(pc5) + pc5 = _cloud(np.array([[0, 0, 0], [1, 1, 1]])) + pc6 = _cloud(np.array([[1, 1, 1], [2, 2, 2]])) + assert cloud_bounds_intersect(pc5, pc6) + assert cloud_bounds_intersect(pc6, pc5) # Test 4: One box completely inside another - pc7 = PointCloud2.from_numpy(np.array([[0, 0, 0], [3, 3, 3]])) - pc8 = PointCloud2.from_numpy(np.array([[1, 1, 1], [2, 2, 2]])) - assert pc7.bounding_box_intersects(pc8) - assert pc8.bounding_box_intersects(pc7) + pc7 = _cloud(np.array([[0, 0, 0], [3, 3, 3]])) + pc8 = _cloud(np.array([[1, 1, 1], [2, 2, 2]])) + assert cloud_bounds_intersect(pc7, pc8) + assert cloud_bounds_intersect(pc8, pc7) # Test 5: Boxes overlapping only in 2 dimensions (not all 3) - pc9 = PointCloud2.from_numpy(np.array([[0, 0, 0], [2, 2, 1]])) - pc10 = PointCloud2.from_numpy(np.array([[1, 1, 2], [3, 3, 3]])) - assert not pc9.bounding_box_intersects(pc10) - assert not pc10.bounding_box_intersects(pc9) + pc9 = _cloud(np.array([[0, 0, 0], [2, 2, 1]])) + pc10 = _cloud(np.array([[1, 1, 2], [3, 3, 3]])) + assert not cloud_bounds_intersect(pc9, pc10) + assert not cloud_bounds_intersect(pc10, pc9) # Test 6: Real-world detection scenario with floating point coordinates detection1_points = np.array( [[-3.5, -0.3, 0.1], [-3.3, -0.2, 0.1], [-3.5, -0.3, 0.3], [-3.3, -0.2, 0.3]] ) - pc_det1 = PointCloud2.from_numpy(detection1_points) + pc_det1 = _cloud(detection1_points) detection2_points = np.array( [[-3.4, -0.25, 0.15], [-3.2, -0.15, 0.15], [-3.4, -0.25, 0.35], [-3.2, -0.15, 0.35]] ) - pc_det2 = PointCloud2.from_numpy(detection2_points) + pc_det2 = _cloud(detection2_points) - assert pc_det1.bounding_box_intersects(pc_det2) + assert cloud_bounds_intersect(pc_det1, pc_det2) # Test 7: Single point clouds - pc_single1 = PointCloud2.from_numpy(np.array([[1.0, 1.0, 1.0]])) - pc_single2 = PointCloud2.from_numpy(np.array([[1.0, 1.0, 1.0]])) - pc_single3 = PointCloud2.from_numpy(np.array([[2.0, 2.0, 2.0]])) + pc_single1 = _cloud(np.array([[1.0, 1.0, 1.0]])) + pc_single2 = _cloud(np.array([[1.0, 1.0, 1.0]])) + pc_single3 = _cloud(np.array([[2.0, 2.0, 2.0]])) # Same point should intersect - assert pc_single1.bounding_box_intersects(pc_single2) + assert cloud_bounds_intersect(pc_single1, pc_single2) # Different points should not intersect - assert not pc_single1.bounding_box_intersects(pc_single3) + assert not cloud_bounds_intersect(pc_single1, pc_single3) # Test 8: Empty point clouds - pc_empty1 = PointCloud2.from_numpy(np.array([]).reshape(0, 3)) - pc_empty2 = PointCloud2.from_numpy(np.array([]).reshape(0, 3)) - PointCloud2.from_numpy(np.array([[1.0, 1.0, 1.0]])) + pc_empty1 = _cloud(np.array([]).reshape(0, 3)) + pc_empty2 = _cloud(np.array([]).reshape(0, 3)) + _cloud(np.array([[1.0, 1.0, 1.0]])) - # Empty clouds should handle gracefully (Open3D returns inf bounds) - # This might raise an exception or return False - we should handle gracefully - try: - result = pc_empty1.bounding_box_intersects(pc_empty2) - # If no exception, verify behavior is consistent - assert isinstance(result, bool) - except Exception: - # If it raises an exception, that's also acceptable for empty clouds - pass + assert not cloud_bounds_intersect(pc_empty1, pc_empty2) def test_to_rerun_points_mode_is_screen_space() -> None: """ "points" must be flat screen-space dots, not the world-space spheres branch.""" - cloud = PointCloud2.from_numpy(np.array([[0.0, 0.0, 0.0], [1.0, 1.0, 1.0]])) + cloud = _cloud(np.array([[0.0, 0.0, 0.0], [1.0, 1.0, 1.0]])) - points = cloud.to_rerun(mode="points", voxel_size=0.05, ui_radius=1.5) - spheres = cloud.to_rerun(mode="spheres", voxel_size=0.05) + points = cloud_archetype(cloud, mode="points", voxel_size=0.05, ui_radius=1.5) + spheres = cloud_archetype(cloud, mode="spheres", voxel_size=0.05) # Negative radii are UI points in rerun; positive ones are world-space. assert points.radii.as_arrow_array().to_pylist() == pytest.approx([-1.5]) @@ -241,17 +303,17 @@ def test_to_rerun_points_mode_is_screen_space() -> None: def test_to_rerun_keeps_the_clouds_own_rgb() -> None: """An RGBD cloud renders in its own colors; rgb=False falls back to the height ramp.""" - import open3d as o3d - - pcd = o3d.geometry.PointCloud() - pcd.points = o3d.utility.Vector3dVector(np.array([[0.0, 0.0, 0.0], [1.0, 1.0, 1.0]])) - pcd.colors = o3d.utility.Vector3dVector(np.array([[1.0, 0.0, 0.0], [0.0, 0.0, 1.0]])) - cloud = PointCloud2(pointcloud=pcd) + cloud = pointcloud_from_xyz_rgb( + np.array([[0.0, 0.0, 0.0], [1.0, 1.0, 1.0]]), + np.array([[255, 0, 0], [0, 0, 255]], dtype=np.uint8), + header=Header(), + ) - colored = cloud.to_rerun(mode="points") + colored = cloud_archetype(cloud, mode="points") assert colored.colors is not None assert colored.class_ids is None - ramp = cloud.to_rerun(mode="points", rgb=False) - assert ramp.colors is None - assert ramp.class_ids is not None + ramp = cloud_archetype(cloud, mode="points", rgb=False) + assert ramp.colors is not None + assert ramp.class_ids is None + assert ramp.colors.as_arrow_array().to_pylist() != colored.colors.as_arrow_array().to_pylist() diff --git a/dimos/msgs/sensor_msgs/test_image.py b/dimos/msgs/sensor_msgs/test_image.py index 21590c5e658..be1cca8a22b 100644 --- a/dimos/msgs/sensor_msgs/test_image.py +++ b/dimos/msgs/sensor_msgs/test_image.py @@ -12,85 +12,90 @@ # See the License for the specific language governing permissions and # limitations under the License. -from unittest.mock import MagicMock +"""File, color, CDR and reactive selection checks on generated images.""" +from pathlib import Path + +from dimos_generated.sensor_msgs.msg import Image +from dimos_generated.std_msgs.msg import Header import numpy as np +from PIL import Image as PILImage import pytest from reactivex.testing import ReactiveTest, TestScheduler -from dimos.msgs.sensor_msgs.Image import Image, ImageFormat, sharpness_barrier -from dimos.utils.data import get_data +from dimos.msgs.image import ( + image_from_array, + image_from_file, + image_sharpness, + image_to_bgr, + image_to_rgb, + image_view, +) +from dimos.msgs.time import time_from_nanoseconds, to_nanoseconds +from dimos.utils.reactive import quality_barrier @pytest.fixture -def img(): - image_file_path = get_data("cafe.jpg") - return Image.from_file(str(image_file_path)) +def img(tmp_path: Path) -> Image: + pixels = np.zeros((771, 1024, 3), dtype=np.uint8) + pixels[..., 0] = 201 + pixels[..., 1] = 37 + path = tmp_path / "camera.png" + PILImage.fromarray(pixels).save(path) + return image_from_file(path, header=Header(stamp=time_from_nanoseconds(1234567890123456789))) def test_file_load(img: Image) -> None: - assert isinstance(img.data, np.ndarray) - assert img.width == 1024 - assert img.height == 771 - assert img.channels == 3 - assert img.shape == (771, 1024, 3) - assert img.data.dtype == np.uint8 - assert img.format == ImageFormat.BGR - assert img.frame_id == "" - assert isinstance(img.ts, float) - assert img.ts > 0 - assert img.data.flags["C_CONTIGUOUS"] - + pixels = image_view(img) + assert isinstance(pixels, np.ndarray) + assert (img.width, img.height) == (1024, 771) + assert pixels.shape == (771, 1024, 3) + assert pixels.dtype == np.uint8 + assert img.encoding == "rgb8" + assert img.header.frame_id == "" + assert to_nanoseconds(img.header.stamp) == 1234567890123456789 + assert pixels.flags["C_CONTIGUOUS"] + np.testing.assert_array_equal(pixels[0, 0], [201, 37, 0]) -def test_lcm_encode_decode(img: Image) -> None: - binary_msg = img.lcm_encode() - decoded_img = Image.lcm_decode(binary_msg) - assert isinstance(decoded_img, Image) - assert decoded_img is not img - assert decoded_img == img +def test_cdr_encode_decode(img: Image) -> None: + decoded = Image.decode(img.encode()) + assert decoded is not img + assert decoded == img def test_rgb_bgr_conversion(img: Image) -> None: - rgb = img.to_rgb() - assert not rgb == img - assert rgb.to_bgr() == img + bgr = image_from_array(image_to_bgr(img), encoding="bgr8", header=img.header) + assert bgr != img + restored = image_from_array(image_to_rgb(bgr), encoding="rgb8", header=img.header) + assert restored == img def test_opencv_conversion(img: Image) -> None: - ocv = img.to_opencv() - decoded_img = Image.from_opencv(ocv) - - # artificially patch timestamp - decoded_img.ts = img.ts - assert decoded_img == img + pixels = image_to_bgr(img) + generated = image_from_array(pixels, encoding="bgr8", header=img.header) + np.testing.assert_array_equal(image_to_rgb(generated), image_view(img)) + assert generated.header == img.header def test_sharpness_barrier() -> None: - # Mock images with known sharpness values, avoiding real data from disk - sharpness_values = [0.3711, 0.3241, 0.3067, 0.2583, 0.3665] - mock_images = [] - for sharp in sharpness_values: - img = MagicMock() - img.sharpness = sharp - mock_images.append(img) - - # sharpness_barrier(20) -> 0.05s windows in virtual time. Subscription is at - # t=200, so windows close at 200.05, 200.10, ... Items 1-4 land in the first - # window, item 5 in the second. + # Real generated images: the first and fifth win the same two virtual windows. + pattern = (np.indices((8, 8)).sum(axis=0) % 2).astype(np.uint8) + images = [ + image_from_array(pattern * level, encoding="mono8") for level in (255, 200, 150, 50, 230) + ] + assert image_sharpness(images[0]) > max(image_sharpness(value) for value in images[1:4]) scheduler = TestScheduler() source = scheduler.create_hot_observable( - ReactiveTest.on_next(200.01, mock_images[0]), - ReactiveTest.on_next(200.02, mock_images[1]), - ReactiveTest.on_next(200.03, mock_images[2]), - ReactiveTest.on_next(200.04, mock_images[3]), - ReactiveTest.on_next(200.06, mock_images[4]), + ReactiveTest.on_next(200.01, images[0]), + ReactiveTest.on_next(200.02, images[1]), + ReactiveTest.on_next(200.03, images[2]), + ReactiveTest.on_next(200.04, images[3]), + ReactiveTest.on_next(200.06, images[4]), ReactiveTest.on_completed(200.08), ) - - results = scheduler.start(lambda: source.pipe(sharpness_barrier(20, scheduler=scheduler))) - - emitted = [m.value.value for m in results.messages if m.value.kind == "N"] - assert len(emitted) == 2, f"Expected one emission per window, got {len(emitted)}" - assert emitted[0].sharpness == 0.3711 # Sharpest of the 4 in the first window - assert emitted[1].sharpness == 0.3665 # Only item in the second window + results = scheduler.start(lambda: source.pipe(quality_barrier(image_sharpness, 20, scheduler))) + emitted = [message.value.value for message in results.messages if message.value.kind == "N"] + assert len(emitted) == 2 + assert emitted[0] is images[0] + assert emitted[1] is images[4] diff --git a/dimos/msgs/sensor_msgs/tool_image.py b/dimos/msgs/sensor_msgs/tool_image.py index af7f04073e4..1f81e8591bc 100644 --- a/dimos/msgs/sensor_msgs/tool_image.py +++ b/dimos/msgs/sensor_msgs/tool_image.py @@ -12,20 +12,21 @@ # See the License for the specific language governing permissions and # limitations under the License. -from dimos.msgs.sensor_msgs.Image import Image +from typing import Any + +from dimos.msgs.image import image_from_array, image_sharpness from dimos.utils.data import get_data from dimos.utils.testing.legacy_pickle import LegacyPickleStore def test_sharpness_stream() -> None: get_data("unitree_office_walk") # Preload data for testing - video_store = LegacyPickleStore( - "unitree_office_walk/video", autocast=lambda x: Image.from_numpy(x).to_rgb() - ) + video_store: LegacyPickleStore[Any] = LegacyPickleStore("unitree_office_walk/video") cnt = 0 - for image in video_store.iterate(): + for pixels in video_store.iterate(): + image = image_from_array(pixels, encoding="bgr8") cnt = cnt + 1 - print(image.sharpness) + print(image_sharpness(image)) if cnt > 30: return diff --git a/dimos/msgs/std_msgs/Bool.py b/dimos/msgs/std_msgs/Bool.py deleted file mode 100644 index 4421447edfc..00000000000 --- a/dimos/msgs/std_msgs/Bool.py +++ /dev/null @@ -1,28 +0,0 @@ -#!/usr/bin/env python3 -# Copyright 2025-2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -"""Bool message type.""" - -from dimos_lcm.std_msgs import Bool as LCMBool - - -class Bool(LCMBool): # type: ignore[misc] - """Bool message.""" - - msg_name = "std_msgs.Bool" - - def __init__(self, data: bool = False) -> None: - """Initialize Bool with data value.""" - self.data = data diff --git a/dimos/msgs/std_msgs/Float32.py b/dimos/msgs/std_msgs/Float32.py deleted file mode 100644 index f5a508d21c0..00000000000 --- a/dimos/msgs/std_msgs/Float32.py +++ /dev/null @@ -1,30 +0,0 @@ -#!/usr/bin/env python3 -# Copyright 2025-2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -"""Float32 message type.""" - -from typing import ClassVar - -from dimos_lcm.std_msgs import Float32 as LCMFloat32 - - -class Float32(LCMFloat32): # type: ignore[misc] - """Float32 message.""" - - msg_name: ClassVar[str] = "std_msgs.Float32" - - def __init__(self, data: float = 0.0) -> None: - """Initialize Float32 with data value.""" - self.data = data diff --git a/dimos/msgs/std_msgs/Header.py b/dimos/msgs/std_msgs/Header.py deleted file mode 100644 index 5c542004972..00000000000 --- a/dimos/msgs/std_msgs/Header.py +++ /dev/null @@ -1,106 +0,0 @@ -# Copyright 2025-2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from __future__ import annotations - -from datetime import datetime -import time - -from dimos_lcm.std_msgs import Header as LCMHeader, Time as LCMTime -from plum import dispatch - -# Import the actual LCM header type that's returned from decoding -try: - from lcm_msgs.std_msgs.Header import ( # type: ignore[import-not-found] - Header as DecodedLCMHeader, - ) -except ImportError: - DecodedLCMHeader = None - - -class Header(LCMHeader): # type: ignore[misc] - msg_name = "std_msgs.Header" - ts: float - - @dispatch - def __init__(self) -> None: - """Initialize a Header with current time and empty frame_id.""" - self.ts = time.time() - sec = int(self.ts) - nsec = int((self.ts - sec) * 1_000_000_000) - super().__init__(seq=0, stamp=LCMTime(sec=sec, nsec=nsec), frame_id="") - - @dispatch # type: ignore[no-redef] - def __init__(self, frame_id: str) -> None: - """Initialize a Header with current time and specified frame_id.""" - self.ts = time.time() - sec = int(self.ts) - nsec = int((self.ts - sec) * 1_000_000_000) - super().__init__(seq=1, stamp=LCMTime(sec=sec, nsec=nsec), frame_id=frame_id) - - @dispatch # type: ignore[no-redef] - def __init__(self, timestamp: float, frame_id: str = "", seq: int = 1) -> None: - """Initialize a Header with Unix timestamp, frame_id, and optional seq.""" - sec = int(timestamp) - nsec = int((timestamp - sec) * 1_000_000_000) - super().__init__(seq=seq, stamp=LCMTime(sec=sec, nsec=nsec), frame_id=frame_id) - - @dispatch # type: ignore[no-redef] - def __init__(self, timestamp: datetime, frame_id: str = "") -> None: - """Initialize a Header with datetime object and frame_id.""" - self.ts = timestamp.timestamp() - sec = int(self.ts) - nsec = int((self.ts - sec) * 1_000_000_000) - super().__init__(seq=1, stamp=LCMTime(sec=sec, nsec=nsec), frame_id=frame_id) - - @dispatch # type: ignore[no-redef] - def __init__(self, seq: int, stamp: LCMTime, frame_id: str) -> None: - """Initialize with explicit seq, stamp, and frame_id (LCM compatibility).""" - super().__init__(seq=seq, stamp=stamp, frame_id=frame_id) - - @dispatch # type: ignore[no-redef] - def __init__(self, header: LCMHeader) -> None: - """Initialize from another Header (copy constructor).""" - super().__init__(seq=header.seq, stamp=header.stamp, frame_id=header.frame_id) - - @dispatch # type: ignore[no-redef] - def __init__(self, header: object) -> None: - """Initialize from a decoded LCM header object.""" - # Handle the case where we get an lcm_msgs.std_msgs.Header.Header object - if hasattr(header, "seq") and hasattr(header, "stamp") and hasattr(header, "frame_id"): - super().__init__(seq=header.seq, stamp=header.stamp, frame_id=header.frame_id) - else: - raise ValueError(f"Cannot create Header from {type(header)}") - - @classmethod - def now(cls, frame_id: str = "", seq: int = 1) -> Header: - """Create a Header with current timestamp.""" - ts = time.time() - return cls(ts, frame_id, seq) - - @property - def timestamp(self) -> float: - """Get timestamp as Unix time (float).""" - return self.stamp.sec + (self.stamp.nsec / 1_000_000_000) # type: ignore[no-any-return] - - @property - def datetime(self) -> datetime: - """Get timestamp as datetime object.""" - return datetime.fromtimestamp(self.timestamp) - - def __str__(self) -> str: - return f"Header(seq={self.seq}, time={self.timestamp:.6f}, frame_id='{self.frame_id}')" - - def __repr__(self) -> str: - return f"Header(seq={self.seq}, stamp=Time(sec={self.stamp.sec}, nsec={self.stamp.nsec}), frame_id='{self.frame_id}')" diff --git a/dimos/msgs/std_msgs/Int32.py b/dimos/msgs/std_msgs/Int32.py deleted file mode 100644 index ba4906f485d..00000000000 --- a/dimos/msgs/std_msgs/Int32.py +++ /dev/null @@ -1,32 +0,0 @@ -#!/usr/bin/env python3 -# Copyright 2025-2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -# Copyright 2025-2026 Dimensional Inc. - -"""Int32 message type.""" - -from typing import ClassVar - -from dimos_lcm.std_msgs import Int32 as LCMInt32 - - -class Int32(LCMInt32): # type: ignore[misc] - """ROS-compatible Int32 message.""" - - msg_name: ClassVar[str] = "std_msgs.Int32" - - def __init__(self, data: int = 0) -> None: - """Initialize Int32 with data value.""" - self.data = data diff --git a/dimos/msgs/std_msgs/Int8.py b/dimos/msgs/std_msgs/Int8.py deleted file mode 100644 index ca871403534..00000000000 --- a/dimos/msgs/std_msgs/Int8.py +++ /dev/null @@ -1,32 +0,0 @@ -#!/usr/bin/env python3 -# Copyright 2025-2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -# Copyright 2025-2026 Dimensional Inc. - -"""Int32 message type.""" - -from typing import ClassVar - -from dimos_lcm.std_msgs import Int8 as LCMInt8 - - -class Int8(LCMInt8): # type: ignore[misc] - """Int8 message.""" - - msg_name: ClassVar[str] = "std_msgs.Int8" - - def __init__(self, data: int = 0) -> None: - """Initialize Int8 with data value.""" - self.data = data diff --git a/dimos/msgs/std_msgs/String.py b/dimos/msgs/std_msgs/String.py deleted file mode 100644 index ef48233dfd3..00000000000 --- a/dimos/msgs/std_msgs/String.py +++ /dev/null @@ -1,30 +0,0 @@ -#!/usr/bin/env python3 -# Copyright 2025-2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -"""String message type.""" - -from typing import ClassVar - -from dimos_lcm.std_msgs import String as LCMString - - -class String(LCMString): # type: ignore[misc] - """ROS-compatible String message.""" - - msg_name: ClassVar[str] = "std_msgs.String" - - def __init__(self, data: str = "") -> None: - """Initialize String with data value.""" - self.data = data diff --git a/dimos/msgs/std_msgs/UInt32.py b/dimos/msgs/std_msgs/UInt32.py deleted file mode 100644 index e617c782fed..00000000000 --- a/dimos/msgs/std_msgs/UInt32.py +++ /dev/null @@ -1,30 +0,0 @@ -#!/usr/bin/env python3 -# Copyright 2025-2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -"""UInt32 message type.""" - -from typing import ClassVar - -from dimos_lcm.std_msgs import UInt32 as LCMUInt32 - - -class UInt32(LCMUInt32): # type: ignore[misc] - """ROS-compatible UInt32 message.""" - - msg_name: ClassVar[str] = "std_msgs.UInt32" - - def __init__(self, data: int = 0) -> None: - """Initialize UInt32 with data value.""" - self.data = data diff --git a/dimos/msgs/std_msgs/test_header.py b/dimos/msgs/std_msgs/test_header.py index 29f4ee2c0e9..a1849aae5fd 100644 --- a/dimos/msgs/std_msgs/test_header.py +++ b/dimos/msgs/std_msgs/test_header.py @@ -12,87 +12,51 @@ # See the License for the specific language governing permissions and # limitations under the License. -from datetime import datetime -import time - -from dimos.msgs.std_msgs.Header import Header - - -def test_header_initialization_methods() -> None: - """Test various ways to initialize a Header.""" - - # Method 1: With timestamp and frame_id - header1 = Header(123.456, "world") - assert header1.seq == 1 - assert header1.stamp.sec == 123 - assert header1.stamp.nsec == 456000000 - assert header1.frame_id == "world" - - # Method 2: With just frame_id (uses current time) - header2 = Header("base_link") - assert header2.seq == 1 - assert header2.frame_id == "base_link" - # Timestamp should be close to current time - assert abs(header2.timestamp - time.time()) < 0.1 - - # Method 3: Empty header (current time, empty frame_id) - header3 = Header() - assert header3.seq == 0 - assert header3.frame_id == "" +"""ROS2 headers use explicit time conversion; no legacy sequence counter.""" - # Method 4: With datetime object - dt = datetime(2025, 1, 18, 12, 30, 45, 500000) # 500ms - header4 = Header(dt, "sensor") - assert header4.seq == 1 - assert header4.frame_id == "sensor" - expected_timestamp = dt.timestamp() - assert abs(header4.timestamp - expected_timestamp) < 1e-6 - - # Method 5: With custom seq number - header5 = Header(999.123, "custom", seq=42) - assert header5.seq == 42 - assert header5.stamp.sec == 999 - assert header5.stamp.nsec == 123000000 - assert header5.frame_id == "custom" - - # Method 6: Using now() class method - header6 = Header.now("camera") - assert header6.seq == 1 - assert header6.frame_id == "camera" - assert abs(header6.timestamp - time.time()) < 0.1 - - # Method 7: now() with custom seq - header7 = Header.now("lidar", seq=99) - assert header7.seq == 99 - assert header7.frame_id == "lidar" +from datetime import datetime, timezone +import time +from dimos_generated.std_msgs.msg import Header +from rosbags.typesys import Stores, get_typestore -def test_header_properties() -> None: - """Test Header property accessors.""" - header = Header(1234567890.123456789, "test") +from dimos.msgs.time import header_now, time_from_nanoseconds, time_from_seconds, to_seconds - # Test timestamp property - assert abs(header.timestamp - 1234567890.123456789) < 1e-6 - # Test datetime property - dt = header.datetime - assert isinstance(dt, datetime) +def test_header_initialization_methods() -> None: + header = Header(stamp=time_from_seconds(123.456), frame_id="world") + assert header.stamp.sec == 123 + assert header.stamp.nanosec == 456000000 + assert header.frame_id == "world" + before = time.time_ns() + current = header_now("base_link") + after = time.time_ns() + assert before <= current.stamp.sec * 1000000000 + current.stamp.nanosec <= after + assert current.frame_id == "base_link" + empty = Header() + assert empty.stamp.sec == empty.stamp.nanosec == 0 + assert empty.frame_id == "" + dt = datetime(2025, 1, 18, 12, 30, 45, 500000, tzinfo=timezone.utc) + dated = Header(stamp=time_from_seconds(dt.timestamp()), frame_id="sensor") + assert dated.frame_id == "sensor" + assert abs(to_seconds(dated.stamp) - dt.timestamp()) < 1e-6 + custom = Header(stamp=time_from_seconds(999.123), frame_id="custom") + assert custom.stamp.sec == 999 + assert custom.stamp.nanosec == 123000000 + assert custom.frame_id == "custom" + + +def test_header_datetime_conversion() -> None: + header = Header(stamp=time_from_nanoseconds(1234567890123456789), frame_id="test") + dt = datetime.fromtimestamp(to_seconds(header.stamp), tz=timezone.utc) assert abs(dt.timestamp() - 1234567890.123456789) < 1e-6 + assert header.stamp.nanosec == 123456789 -def test_header_string_representation() -> None: - """Test Header string representations.""" - header = Header(100.5, "map", seq=10) - - # Test __str__ - str_repr = str(header) - assert "seq=10" in str_repr - assert "time=100.5" in str_repr - assert "frame_id='map'" in str_repr - - # Test __repr__ - repr_str = repr(header) - assert "Header(" in repr_str - assert "seq=10" in repr_str - assert "Time(sec=100, nsec=500000000)" in repr_str - assert "frame_id='map'" in repr_str +def test_header_independent_cdr_fields() -> None: + header = Header(stamp=time_from_seconds(100.5), frame_id="map") + decoded = get_typestore(Stores.ROS2_JAZZY).deserialize_cdr(header.encode(), Header.msg_name) + assert decoded.stamp.sec == 100 + assert decoded.stamp.nanosec == 500000000 + assert decoded.frame_id == "map" + assert "seq" not in Header.schema diff --git a/dimos/msgs/test_camera_info.py b/dimos/msgs/test_camera_info.py new file mode 100644 index 00000000000..641f71e3220 --- /dev/null +++ b/dimos/msgs/test_camera_info.py @@ -0,0 +1,127 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Calibration helpers preserve generated ROS fields and reject malformed input.""" + +import math +from pathlib import Path + +from dimos_generated.builtin_interfaces.msg import Time +from dimos_generated.sensor_msgs.msg import CameraInfo +from dimos_generated.std_msgs.msg import Header +import numpy as np +import pytest +import yaml + +from dimos.msgs.camera_info import ( + camera_info_from_fov, + camera_info_from_intrinsics, + camera_info_from_yaml, + camera_info_with_stamp, + intrinsic_matrix, +) + +_CALIBRATION = Path(__file__).parents[1] / "robot/unitree/go2/front_camera_720.yaml" + + +def test_real_go2_calibration_survives_cdr_with_exact_header(): + header = Header(frame_id="camera_optical", stamp=Time(sec=1700000000, nanosec=123456789)) + source = yaml.safe_load(_CALIBRATION.read_text()) + message = camera_info_from_yaml(_CALIBRATION, header=header) + decoded = CameraInfo.decode(message.encode()) + assert decoded.header == header + assert (decoded.width, decoded.height) == (1280, 720) + assert decoded.distortion_model == "equidistant" + for field, key in [ + ("k", "camera_matrix"), + ("d", "distortion_coefficients"), + ("r", "rectification_matrix"), + ("p", "projection_matrix"), + ]: + np.testing.assert_array_equal(getattr(decoded, field), source[key]["data"]) + matrix = intrinsic_matrix(decoded) + assert matrix.shape == (3, 3) + matrix[0, 0] = 0 + assert decoded.k[0] == source["camera_matrix"]["data"][0] + decoded.header.frame_id = "changed" + assert header.frame_id == "camera_optical" + + +@pytest.mark.parametrize("axis,focal", [("vertical", 240), ("horizontal", 320)]) +def test_fov_converts_selected_axis_and_builds_projection(axis, focal): + message = camera_info_from_fov(90, 640, 480, header=Header(frame_id="optical"), axis=axis) + np.testing.assert_allclose(message.k, [focal, 0, 320, 0, focal, 240, 0, 0, 1]) + np.testing.assert_allclose(message.p, [focal, 0, 320, 0, 0, focal, 240, 0, 0, 0, 1, 0]) + np.testing.assert_array_equal(message.r, np.eye(3).reshape(-1)) + assert message.header.frame_id == "optical" + + +@pytest.mark.parametrize("fov", [0, -1, 180, 181, math.nan, math.inf]) +def test_fov_rejects_nonphysical_angles(fov): + with pytest.raises(ValueError, match="Field of view"): + camera_info_from_fov(fov, 640, 480, header=Header()) + + +@pytest.mark.parametrize("width,height", [(0, 480), (640, -1), (True, 480), (1.5, 480)]) +def test_intrinsics_reject_invalid_dimensions(width, height): + with pytest.raises(ValueError, match="positive integers"): + camera_info_from_intrinsics(300, 300, 320, 240, width, height, header=Header()) + + +@pytest.mark.parametrize("fx", [0, -1, math.nan, math.inf]) +def test_intrinsics_reject_invalid_focal_length(fx): + with pytest.raises(ValueError, match="positive focal"): + camera_info_from_intrinsics(fx, 300, 320, 240, 640, 480, header=Header()) + + +@pytest.mark.parametrize("mutation", ["rows", "length", "nonfinite"]) +def test_yaml_rejects_invalid_camera_matrix(tmp_path, mutation): + data = yaml.safe_load(_CALIBRATION.read_text()) + matrix = data["camera_matrix"] + if mutation == "rows": + matrix["rows"] = 1 + elif mutation == "length": + matrix["data"].pop() + else: + matrix["data"][0] = math.nan + path = tmp_path / "invalid.yaml" + path.write_text(yaml.safe_dump(data)) + with pytest.raises(ValueError, match="camera_matrix"): + camera_info_from_yaml(path, header=Header()) + + +def test_stamp_copy_preserves_calibration_and_does_not_mutate_prior_publish(): + original = camera_info_from_intrinsics( + 500, + 510, + 320, + 240, + 640, + 480, + header=Header(frame_id="optical", stamp=Time(sec=1, nanosec=987654321)), + ) + original.binning_x = 2 + original.roi.x_offset = 12 + stamp = Time(sec=2, nanosec=123456789) + copied = camera_info_with_stamp(original, stamp) + assert copied.header.stamp == stamp + assert copied.header.frame_id == original.header.frame_id + assert list(copied.k) == list(original.k) + assert copied.binning_x == 2 and copied.roi.x_offset == 12 + copied.k[0] = 100 + copied.roi.x_offset = 24 + stamp.nanosec = 0 + assert original.k[0] == 500 and original.roi.x_offset == 12 + assert original.header.stamp == Time(sec=1, nanosec=987654321) + assert copied.header.stamp.nanosec == 123456789 diff --git a/dimos/msgs/test_generated_projection.py b/dimos/msgs/test_generated_projection.py new file mode 100644 index 00000000000..3c200468093 --- /dev/null +++ b/dimos/msgs/test_generated_projection.py @@ -0,0 +1,98 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Stored image projection uses generated transforms and padded depth pixels.""" + +from dimos_generated.geometry_msgs.msg import ( + Point, + Quaternion, + Transform, + TransformStamped, + Vector3, +) +from dimos_generated.sensor_msgs.msg import CameraInfo, Image +from dimos_generated.std_msgs.msg import Header +import numpy as np + +from dimos.memory.type.observation import Observation +from dimos.msgs.geometry import transform_matrix +from dimos.perception.detection.project import _world_to_optical, sees + + +def test_stored_pose_projection_inverts_world_camera_chain() -> None: + obs = Observation( + id=0, ts=0.0, data_type=Image, _data=Image(), pose_tuple=(2.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0) + ) + extrinsic = TransformStamped( + header=Header(frame_id="base_link"), + child_frame_id="camera_optical", + transform=Transform(translation=Vector3(x=1), rotation=Quaternion(w=1)), + ) + result = _world_to_optical(obs, "world", None, extrinsic, "camera_optical", 5.0) + assert result is not None + assert result.header.frame_id == "camera_optical" + assert result.child_frame_id == "world" + np.testing.assert_allclose(transform_matrix(result.transform) @ [3, 0, 2, 1], [0, 0, 2, 1]) + + +def test_depth_occlusion_reads_big_endian_rows_and_millimeters() -> None: + obs = Observation( + id=0, ts=0.0, data_type=Image, _data=Image(), pose_tuple=(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0) + ) + extrinsic = TransformStamped( + header=Header(frame_id="base_link"), + child_frame_id="camera_optical", + transform=Transform(rotation=Quaternion(w=1)), + ) + calibration = CameraInfo(width=2, height=2, k=[1, 0, 0, 0, 1, 0, 0, 0, 1]) + # Pixel (0, 1) = 1500mm; padding cannot be interpreted as pixels. + depth = Image( + width=2, + height=2, + step=6, + encoding="16UC1", + is_bigendian=1, + data=[0, 0, 0, 0, 99, 99, 5, 220, 0, 0, 99, 99], + ) + visible = sees( + Point(x=0, y=1, z=1), calibration, base_to_optical=extrinsic, depth=lambda _: depth + ) + hidden = sees( + Point(x=0, y=2, z=2), calibration, base_to_optical=extrinsic, depth=lambda _: depth + ) + assert visible(obs) + assert not hidden(obs) + + +def test_timestamp_adapter_matches_generated_cloud_without_mutating_header() -> None: + from dimos_generated.sensor_msgs.msg import PointCloud2 + from reactivex.subject import Subject + + from dimos.msgs.time import time_from_nanoseconds, to_seconds + from dimos.types.timestamped import TimestampedData, align_timestamped + + source = PointCloud2(header=Header(stamp=time_from_nanoseconds(1700000000123456789))) + primary = Subject() + secondary = Subject() + received = [] + disposable = align_timestamped(primary, secondary, match_tolerance=0.0).subscribe( + received.append + ) + timed = TimestampedData(source, to_seconds(source.header.stamp)) + secondary.on_next(timed) + primary.on_next(TimestampedData("detection", timed.ts)) + assert len(received) == 1 + assert received[0][1].value is source + assert source.header.stamp.nanosec == 123456789 + disposable.dispose() diff --git a/dimos/msgs/test_generated_views.py b/dimos/msgs/test_generated_views.py new file mode 100644 index 00000000000..54f600a24ee --- /dev/null +++ b/dimos/msgs/test_generated_views.py @@ -0,0 +1,232 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Borrowed buffers preserve padding, byte order, lifetime, and ROS value semantics.""" + +import gc +import json +import math + +import cv2 +from dimos_generated.geometry_msgs.msg import Point, Pose, PoseStamped, Quaternion +from dimos_generated.nav_msgs.msg import MapMetaData, OccupancyGrid, Path +from dimos_generated.sensor_msgs.msg import Image +from dimos_generated.std_msgs.msg import Header +import numpy as np +from PIL import Image as PILImage +import pytest + +from dimos.msgs.geometry import yaw +from dimos.msgs.image import ( + image_brightness, + image_from_array, + image_from_file, + image_sharpness, + image_to_jpeg, + image_to_rgb, + image_view, +) +from dimos.msgs.occupancy import block_max_reduce, occupancy_view +from dimos.msgs.time import time_from_nanoseconds +from dimos.web.relay_bridge.builtin_codecs import decode_point, encode_path, encode_pose + + +def test_padded_image_view_retains_owner_and_is_readonly() -> None: + msg = Image(width=2, height=2, step=4, encoding="mono8", data=[1, 2, 99, 99, 3, 4, 99, 99]) + pixels = image_view(msg) + np.testing.assert_array_equal(pixels, [[1, 2], [3, 4]]) + assert pixels.strides == (4, 1) + with pytest.raises(ValueError): + pixels[0, 0] = 9 + del msg + gc.collect() + np.testing.assert_array_equal(pixels, [[1, 2], [3, 4]]) + copied = pixels.copy() + copied[0, 0] = 9 + assert pixels[0, 0] == 1 + + +def test_big_endian_depth_view() -> None: + msg = Image( + width=2, height=1, step=4, encoding="16UC1", is_bigendian=1, data=[0x01, 0x02, 0x03, 0x04] + ) + np.testing.assert_array_equal(image_view(msg), [[258, 772]]) + + +def test_image_array_copy_preserves_endian_and_handles_strides() -> None: + pixels = np.arange(24, dtype=">u2").reshape(4, 6)[::2, ::2] + msg = image_from_array(pixels, encoding="16UC1") + assert (msg.width, msg.height, msg.step, msg.is_bigendian) == (3, 2, 6, 1) + expected = pixels.copy() + pixels[:] = 0 + np.testing.assert_array_equal(image_view(msg), expected) + + +@pytest.mark.parametrize( + "pixels,encoding", + [(np.zeros((2, 2), dtype=np.float32), "mono8"), (np.zeros((2, 2), dtype=np.uint8), "rgb8")], +) +def test_image_array_requires_matching_encoding(pixels: np.ndarray, encoding: str) -> None: + with pytest.raises(ValueError, match="does not match"): + image_from_array(pixels, encoding=encoding) + + +@pytest.mark.parametrize("step,data", [(1, [1]), (2, [1]), (2, [1, 2, 3])]) +def test_invalid_image_layout(step: int, data: list[int]) -> None: + with pytest.raises(ValueError, match="dimensions"): + image_view(Image(width=2, height=1, step=step, data=data, encoding="mono8")) + + +def test_jpeg_preserves_rgb_color() -> None: + msg = Image(width=16, height=16, step=48, encoding="rgb8", data=[255, 0, 0] * 256) + jpeg = image_to_jpeg(msg, quality=95) + bgr = cv2.imdecode(np.frombuffer(jpeg, np.uint8), cv2.IMREAD_COLOR) + assert bgr.shape == (16, 16, 3) + assert bgr[8, 8, 2] > 250 and bgr[8, 8, 0] < 5 + + +def test_occupancy_view_and_obstacle_preserving_reduction() -> None: + msg = OccupancyGrid(info=MapMetaData(width=4, height=2), data=[-1, -1, 0, 100, -1, -1, 10, 20]) + cells = occupancy_view(msg) + assert not cells.flags.writeable + np.testing.assert_array_equal(block_max_reduce(cells, 2), [[-1, 100]]) + msg.info.width = 3 + with pytest.raises(ValueError, match="dimensions"): + occupancy_view(msg) + + +def test_web_pose_path_and_goal_use_nested_ros_fields() -> None: + pose = PoseStamped( + header=Header(stamp=time_from_nanoseconds(1500000000)), + pose=Pose( + position=Point(x=2, y=3), orientation=Quaternion(z=math.sqrt(0.5), w=math.sqrt(0.5)) + ), + ) + assert yaw(pose.pose.orientation) == pytest.approx(math.pi / 2) + value = json.loads(encode_pose(pose)) + assert value == {"x": 2, "y": 3, "z": 0, "yaw": pytest.approx(math.pi / 2), "ts": 1.5} + assert json.loads(encode_path(Path(poses=[pose]))) == [[2, 3]] + goal = decode_point({"x": 4, "y": 5}) + assert goal.point == Point(x=4, y=5) and goal.header.frame_id == "world" + + +@pytest.mark.parametrize("encoding", ["rgb8", "bgr8", "rgba8", "bgra8", "mono8"]) +def test_sharpness_prefers_edges_to_blurred_pixels(encoding): + mono = ((np.indices((64, 64)).sum(axis=0) % 2) * 255).astype(np.uint8) + channels = 1 if encoding == "mono8" else (4 if "a" in encoding else 3) + pixels = mono if channels == 1 else np.repeat(mono[:, :, None], channels, axis=2) + blurred = cv2.GaussianBlur(pixels, (5, 5), 0) + sharp = image_from_array(pixels, encoding=encoding) + soft = image_from_array(blurred, encoding=encoding) + assert image_sharpness(sharp) > image_sharpness(soft) + + +def test_sharpness_rejects_depth_and_empty_images(): + depth = image_from_array(np.zeros((2, 2), dtype=np.float32), encoding="32FC1") + with pytest.raises(ValueError, match="8-bit visual"): + image_sharpness(depth) + with pytest.raises(ValueError, match="nonempty"): + image_sharpness(Image(encoding="mono8")) + + +@pytest.mark.parametrize( + "encoding,pixel", + [ + ("rgb8", [255, 0, 17]), + ("bgr8", [17, 0, 255]), + ("rgba8", [255, 0, 17, 23]), + ("bgra8", [17, 0, 255, 23]), + ], +) +def test_rgb_model_input_is_color_correct_and_independent(encoding: str, pixel: list[int]) -> None: + source = image_from_array(np.array([[pixel]], dtype=np.uint8), encoding=encoding) + restored = Image.decode(source.encode()) + rgb = image_to_rgb(restored) + np.testing.assert_array_equal(rgb, [[[255, 0, 17]]]) + rgb[0, 0] = 0 + np.testing.assert_array_equal(image_view(restored), [[pixel]]) + + +def test_rgb_model_input_respects_big_endian_padded_grayscale() -> None: + msg = Image( + width=2, + height=2, + step=6, + encoding="mono16", + is_bigendian=1, + data=[0x12, 0x34, 0xFF, 0xFF, 99, 99, 0x01, 0x00, 0, 0, 99, 99], + ) + np.testing.assert_array_equal( + image_to_rgb(msg), [[[18, 18, 18], [255, 255, 255]], [[1, 1, 1], [0, 0, 0]]] + ) + + +def test_rgb_model_input_requires_explicit_float_depth_scale() -> None: + msg = image_from_array(np.array([[2.5]], dtype=np.float32), encoding="32FC1") + with pytest.raises(ValueError, match="RGB8"): + image_to_rgb(msg) + + +def test_mosaic_uses_generated_pixels_and_preserves_rgb_colors() -> None: + from dimos.memory.vis.utils import mosaic + + red = image_from_array(np.array([[[255, 0, 0]]], dtype=np.uint8), encoding="rgb8") + blue = image_from_array(np.array([[[255, 0, 0]]], dtype=np.uint8), encoding="bgr8") + observation = mosaic([red, blue], cols=2, cell_height=1) + result = Image.decode(observation.data.encode()) + assert result.encoding == "bgr8" + np.testing.assert_array_equal(image_to_rgb(result), [[[255, 0, 0], [0, 0, 255]]]) + assert observation.tags == {"mosaic": True} + assert observation.pose is None + + +def test_brightness_ignores_padding_and_normalizes_big_endian_u16() -> None: + message = Image( + width=2, + height=1, + step=6, + encoding="mono16", + is_bigendian=1, + data=[0, 0, 255, 255, 255, 255], + ) + assert image_brightness(message) == pytest.approx(0.5) + depth = image_from_array(np.array([[1.0]], dtype=np.float32), encoding="32FC1") + with pytest.raises(ValueError, match="unsigned integer"): + image_brightness(depth) + + +@pytest.mark.parametrize("mode", ["L", "RGBA"]) +def test_image_file_decodes_rgb_and_preserves_source_header(tmp_path, mode): + path = tmp_path / "frame.png" + pixels = np.array([[7, 19]], dtype=np.uint8) + source = PILImage.fromarray(pixels).convert(mode) + source.save(path) + header = Header(stamp=time_from_nanoseconds(1234567890), frame_id="camera") + message = image_from_file(path, header=header) + assert message.encoding == "rgb8" + assert message.header.encode() == header.encode() + np.testing.assert_array_equal(image_view(message), [[[7, 7, 7], [19, 19, 19]]]) + + +@pytest.mark.parametrize( + "encoding,dtype,shape", + [("64FC1", ">f8", (3, 4)), ("16SC1", ">i2", (3, 4)), ("32FC3", ">f4", (3, 4, 3))], +) +def test_remaining_raw_encodings_keep_signed_float_and_endian_values(encoding, dtype, shape): + pixels = (np.arange(np.prod(shape)).reshape(shape) - 5).astype(dtype) + source = image_from_array(pixels, encoding=encoding) + decoded = Image.decode(source.encode()) + assert decoded.encoding == encoding + assert decoded.is_bigendian == 1 + np.testing.assert_array_equal(image_view(decoded), pixels) diff --git a/dimos/msgs/test_geometry.py b/dimos/msgs/test_geometry.py new file mode 100644 index 00000000000..38780853d5f --- /dev/null +++ b/dimos/msgs/test_geometry.py @@ -0,0 +1,244 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +# Copyright 2026 Dimensional Inc. +# SPDX-License-Identifier: Apache-2.0 + +import math + +from dimos_generated.builtin_interfaces.msg import Time +from dimos_generated.geometry_msgs.msg import ( + Point, + Pose, + PoseStamped, + Quaternion, + Transform, + TransformStamped, + Vector3, +) +from dimos_generated.nav_msgs.msg import Odometry +from dimos_generated.std_msgs.msg import Header +import numpy as np +import pytest + +from dimos.msgs.geometry import ( + compose_transforms, + inverse_transform, + pose_from_matrix, + pose_from_transform, + pose_matrix, + quaternion_euler, + quaternion_from_euler, + transform_from_odometry, + transform_from_pose, + transform_matrix, + translate_pose_local, + yaw, +) + + +@pytest.mark.parametrize("seconds,nanoseconds", [(0, 0), (-1, 500000000), (1700000000, 123456789)]) +def test_odometry_transform_preserves_frames_stamp_and_pose(seconds, nanoseconds): + message = Odometry(child_frame_id="robot/lidar") + message.header.frame_id = "robot/odom" + message.header.stamp = Time(sec=seconds, nanosec=nanoseconds) + message.pose.pose.position = Point(x=1, y=2, z=3) + message.pose.pose.orientation = Quaternion(z=0.6, w=0.8) + + transformed = transform_from_odometry(message) + + assert transformed.header.frame_id == "robot/odom" + assert transformed.child_frame_id == "robot/lidar" + assert transformed.header.stamp.sec == seconds + assert transformed.header.stamp.nanosec == nanoseconds + assert ( + transformed.transform.translation.x, + transformed.transform.translation.y, + transformed.transform.translation.z, + ) == (1, 2, 3) + assert transformed.transform.rotation == message.pose.pose.orientation + transformed.header.frame_id = "changed" + transformed.transform.rotation.w = 0 + assert message.header.frame_id == "robot/odom" + assert message.pose.pose.orientation.w == 0.8 + + +def _edge(parent, child, xyz, rotation, stamp=Time(sec=1700000000, nanosec=123456789)): + return TransformStamped( + header=Header(frame_id=parent, stamp=stamp), + child_frame_id=child, + transform=Transform(translation=Vector3(x=xyz[0], y=xyz[1], z=xyz[2]), rotation=rotation), + ) + + +def test_transform_composition_rotates_translation_and_preserves_exact_source_stamp(): + first = _edge("world", "arm", (1, -1, 0), quaternion_from_euler(0, 0, math.pi / 2)) + second = _edge("arm", "tool", (2, 3, 4), Quaternion(w=1), Time(sec=7)) + + result = compose_transforms(first, second) + + assert result.header == first.header + assert result.child_frame_id == "tool" + translation = result.transform.translation + assert (translation.x, translation.y, translation.z) == pytest.approx((-2, 1, 4)) + assert yaw(result.transform.rotation) == pytest.approx(math.pi / 2) + result.header.stamp.nanosec = 0 + result.transform.translation.x = 99 + assert first.header.stamp.nanosec == 123456789 + assert first.transform.translation.x == 1 + assert second.transform.translation.x == 2 + + +def test_inverse_composes_to_identity_and_swaps_frames_without_losing_nanoseconds(): + original = _edge("map", "camera", (2, 3, -4), quaternion_from_euler(0.3, -0.2, 1.1)) + inverse = inverse_transform(original) + result = compose_transforms(original, inverse) + + assert inverse.header.frame_id == "camera" + assert inverse.child_frame_id == "map" + assert inverse.header.stamp == original.header.stamp + translation = result.transform.translation + assert (translation.x, translation.y, translation.z) == pytest.approx((0, 0, 0), abs=1e-12) + rotation = result.transform.rotation + assert (rotation.x, rotation.y, rotation.z, abs(rotation.w)) == pytest.approx((0, 0, 0, 1)) + + +def test_transform_composition_rejects_disconnected_frames(): + first = _edge("a", "b", (0, 0, 0), Quaternion(w=1)) + second = _edge("c", "d", (0, 0, 0), Quaternion(w=1)) + with pytest.raises(ValueError, match="Cannot compose frames"): + compose_transforms(first, second) + + +@pytest.mark.parametrize( + "rotation", [Quaternion(w=0), Quaternion(x=math.nan, w=1), Quaternion(w=math.inf)] +) +def test_transform_math_rejects_invalid_rotations(rotation): + edge = _edge("a", "b", (0, 0, 0), rotation) + with pytest.raises(ValueError): + inverse_transform(edge) + with pytest.raises(ValueError): + compose_transforms(edge, _edge("b", "c", (0, 0, 0), Quaternion(w=1))) + + +@pytest.mark.parametrize("value", [math.nan, math.inf, -math.inf]) +def test_transform_math_rejects_nonfinite_translations_and_angles(value): + edge = _edge("a", "b", (value, 0, 0), Quaternion(w=1)) + with pytest.raises(ValueError, match="Translation components"): + inverse_transform(edge) + with pytest.raises(ValueError, match="Euler angles"): + quaternion_from_euler(value, 0, 0) + + +def test_pose_conversion_copies_nested_fields_and_preserves_header(): + edge = _edge("map", "robot", (1, 2, 3), Quaternion(z=0.6, w=0.8)) + pose = pose_from_transform(edge) + assert pose.header == edge.header + assert pose.pose.position == Point(x=1, y=2, z=3) + assert pose.pose.orientation == edge.transform.rotation + pose.header.frame_id = "changed" + pose.pose.orientation.w = 0 + assert edge.header.frame_id == "map" + assert edge.transform.rotation.w == 0.8 + + +def test_pose_transform_preserves_header_and_does_not_alias_input(): + source = PoseStamped( + header=Header(frame_id="odom", stamp=Time(sec=-1, nanosec=987654321)), + pose=Pose(position=Point(x=1, y=2, z=3), orientation=Quaternion(z=0.6, w=0.8)), + ) + result = transform_from_pose(source, child_frame_id="robot/base") + decoded = TransformStamped.decode(result.encode()) + assert decoded.header == source.header + assert decoded.child_frame_id == "robot/base" + assert decoded.transform.translation == Vector3(x=1, y=2, z=3) + assert decoded.transform.rotation == source.pose.orientation + result.header.frame_id = "changed" + result.transform.rotation.w = 0 + assert source.header.frame_id == "odom" + assert source.pose.orientation.w == 0.8 + + +def test_transform_matrix_maps_rotation_and_translation_without_aliasing(): + value = Transform( + translation=Vector3(x=2.0, y=-1.0, z=3.0), + rotation=quaternion_from_euler(0.0, 0.0, math.pi / 2), + ) + matrix = transform_matrix(value) + np.testing.assert_allclose(matrix @ [1.0, 0.0, 0.0, 1.0], [2.0, 0.0, 3.0, 1.0], atol=1e-12) + value.translation.x = 10.0 + assert matrix[0, 3] == 2.0 + + +def test_pose_matrix_roundtrip_preserves_rigid_motion_without_aliasing(): + pose = Pose( + position=Point(x=2, y=-1, z=3), + orientation=quaternion_from_euler(0, 0, math.pi / 2), + ) + matrix = pose_matrix(pose) + np.testing.assert_allclose(matrix @ [1, 0, 0, 1], [2, 0, 3, 1], atol=1e-12) + decoded = Pose.decode(pose_from_matrix(matrix).encode()) + np.testing.assert_allclose(pose_matrix(decoded), matrix, atol=1e-12) + matrix[0, 3] = 99 + decoded.position.y = 42 + assert pose.position == Point(x=2, y=-1, z=3) + assert decoded.position.x == 2 + + +@pytest.mark.parametrize( + "matrix", + [ + np.eye(3), + np.full((4, 4), math.nan), + np.diag([1, 1, 1, 2]), + np.diag([2, 1, 1, 1]), + np.diag([-1, 1, 1, 1]), + ], +) +def test_pose_from_matrix_rejects_nonrigid_or_invalid_matrices(matrix): + with pytest.raises(ValueError): + pose_from_matrix(matrix) + + +@pytest.mark.parametrize( + "pose", + [ + Pose(position=Point(x=math.inf), orientation=Quaternion(w=1)), + Pose(orientation=Quaternion(w=0)), + Pose(orientation=Quaternion(w=math.nan)), + ], +) +def test_pose_matrix_rejects_invalid_geometry(pose): + with pytest.raises(ValueError): + pose_matrix(pose) + + +def test_local_offset_uses_pose_axes_and_copies_orientation(): + pose = Pose( + position=Point(x=1, y=2, z=3), + orientation=quaternion_from_euler(0, 0, math.pi / 2), + ) + translated = translate_pose_local(pose, Vector3(x=2, z=-1)) + position = translated.position + assert (position.x, position.y, position.z) == pytest.approx((1, 4, 2)) + assert quaternion_euler(translated.orientation) == pytest.approx((0, 0, math.pi / 2)) + translated.orientation.w = 0 + assert pose.orientation.w == pytest.approx(math.sqrt(0.5)) + assert pose.position == Point(x=1, y=2, z=3) + + +@pytest.mark.parametrize("value", [math.nan, math.inf, -math.inf]) +def test_local_offset_rejects_nonfinite_displacements(value): + with pytest.raises(ValueError, match="Offset components"): + translate_pose_local(Pose(orientation=Quaternion(w=1)), Vector3(x=value)) diff --git a/dimos/msgs/test_helpers.py b/dimos/msgs/test_helpers.py index 72958fc1525..b3a7c516b93 100644 --- a/dimos/msgs/test_helpers.py +++ b/dimos/msgs/test_helpers.py @@ -12,15 +12,16 @@ # See the License for the specific language governing permissions and # limitations under the License. -from dimos_lcm.geometry_msgs import Pose -import pytest +from dimos_generated.geometry_msgs.msg import Pose -from dimos.msgs.helpers import lcm_msg_type +from dimos.msgs.helpers import resolve_msg_type -def test_lcm_msg_type() -> None: - assert lcm_msg_type("geometry_msgs.Pose") is Pose - with pytest.raises(ImportError): - lcm_msg_type("geometry_msgs.Nope") - with pytest.raises(ValueError): - lcm_msg_type("Bare") +def test_installed_generated_type_resolves_by_schema_name() -> None: + assert resolve_msg_type("geometry_msgs/msg/Pose") is Pose + + +def test_unknown_names_do_not_import_a_legacy_message_package() -> None: + assert resolve_msg_type("geometry_msgs/msg/Nope") is None + assert resolve_msg_type("geometry_msgs.Pose") is None + assert resolve_msg_type("Bare") is None diff --git a/dimos/msgs/test_msg_construction.py b/dimos/msgs/test_msg_construction.py index c4f314753c5..3b49b4661fd 100644 --- a/dimos/msgs/test_msg_construction.py +++ b/dimos/msgs/test_msg_construction.py @@ -12,616 +12,199 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""Construction semantics for the hot message types. +"""Generated field construction replaces polymorphic rich-message constructors. -Every accepted construction form for the types whose ``__init__`` used to be a -``plum`` multiple-dispatch stack, plus the error cases. These pin the contract -that the dispatch removal had to preserve. +Array/dictionary inputs are unpacked explicitly. Stamped types contain a header +and a payload; timestamps default to zero and never sample the wall clock. """ -import time +from copy import deepcopy -from dimos_lcm.geometry_msgs import ( - Point as LCMPoint, - Pose as LCMPose, - PoseWithCovariance as LCMPoseWithCovariance, - Quaternion as LCMQuaternion, - Twist as LCMTwist, - TwistWithCovariance as LCMTwistWithCovariance, - Vector3 as LCMVector3, +from dimos_generated.builtin_interfaces.msg import Time +from dimos_generated.geometry_msgs.msg import ( + Point, + PointStamped, + Pose, + PoseStamped, + PoseWithCovariance, + PoseWithCovarianceStamped, + Quaternion, + TransformStamped, + Twist, + TwistStamped, + TwistWithCovariance, + TwistWithCovarianceStamped, + Vector3, ) -from dimos_lcm.sensor_msgs import JointState as LCMJointState, Joy as LCMJoy +from dimos_generated.nav_msgs.msg import Odometry, Path +from dimos_generated.sensor_msgs.msg import JointState, Joy +from dimos_generated.std_msgs.msg import Header import numpy as np import pytest - -from dimos.msgs.geometry_msgs.PointStamped import PointStamped -from dimos.msgs.geometry_msgs.Pose import Pose -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.geometry_msgs.PoseWithCovariance import PoseWithCovariance -from dimos.msgs.geometry_msgs.PoseWithCovarianceStamped import PoseWithCovarianceStamped -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Transform import Transform -from dimos.msgs.geometry_msgs.Twist import Twist -from dimos.msgs.geometry_msgs.TwistStamped import TwistStamped -from dimos.msgs.geometry_msgs.TwistWithCovariance import TwistWithCovariance -from dimos.msgs.geometry_msgs.TwistWithCovarianceStamped import TwistWithCovarianceStamped -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.msgs.nav_msgs.Odometry import Odometry -from dimos.msgs.nav_msgs.Path import Path -from dimos.msgs.sensor_msgs.JointState import JointState -from dimos.msgs.sensor_msgs.Joy import Joy - -IDENTITY = (0.0, 0.0, 0.0, 1.0) - - -# Quaternion - - -def test_quaternion_empty_is_identity() -> None: - assert Quaternion().to_tuple() == IDENTITY - - -def test_quaternion_four_positional_floats() -> None: - q = Quaternion(1.0, 2.0, 3.0, 4.0) - assert q.to_tuple() == (1.0, 2.0, 3.0, 4.0) - - -def test_quaternion_four_positional_ints_are_coerced_to_float() -> None: - q = Quaternion(1, 2, 3, 4) - assert q.to_tuple() == (1.0, 2.0, 3.0, 4.0) - assert all(type(c) is float for c in q.to_tuple()) - - -def test_quaternion_from_list() -> None: - assert Quaternion([1.0, 2.0, 3.0, 4.0]).to_tuple() == (1.0, 2.0, 3.0, 4.0) - - -def test_quaternion_from_tuple() -> None: - assert Quaternion((1.0, 2.0, 3.0, 4.0)).to_tuple() == (1.0, 2.0, 3.0, 4.0) - - -def test_quaternion_from_range() -> None: - assert Quaternion(range(4)).to_tuple() == (0.0, 1.0, 2.0, 3.0) - - -def test_quaternion_from_ndarray() -> None: - assert Quaternion(np.array([1.0, 2.0, 3.0, 4.0])).to_tuple() == (1.0, 2.0, 3.0, 4.0) - - -def test_quaternion_copy_constructor() -> None: - src = Quaternion(0.1, 0.2, 0.3, 0.4) - assert Quaternion(src).to_tuple() == src.to_tuple() - - -def test_quaternion_from_lcm_keeps_lcm_defaults() -> None: - """The LCM base defaults w to 0.0, unlike the dimos identity default.""" - assert Quaternion(LCMQuaternion()).to_tuple() == (0.0, 0.0, 0.0, 0.0) - - -def test_quaternion_from_lcm_with_values() -> None: - assert Quaternion(LCMQuaternion(1.0, 2.0, 3.0, 4.0)).to_tuple() == (1.0, 2.0, 3.0, 4.0) - - -def test_quaternion_sequence_wrong_length_raises() -> None: - for bad in ([1, 2, 3], [1, 2, 3, 4, 5], (1, 2)): - with pytest.raises(ValueError, match=r"exactly 4 components"): - Quaternion(bad) - - -def test_quaternion_ndarray_wrong_size_raises() -> None: - with pytest.raises(ValueError, match=r"exactly 4 components"): - Quaternion(np.array([1.0, 2.0, 3.0])) - - -def test_quaternion_lcm_roundtrip_uses_the_sequence_form() -> None: - """``_lcm_decode_one`` builds from a 4-tuple, so decoding exercises that path.""" - decoded = Quaternion.lcm_decode(Quaternion(1.0, 2.0, 3.0, 4.0).lcm_encode()) - assert decoded.to_tuple() == (1.0, 2.0, 3.0, 4.0) - - -# Pose - - -def test_pose_empty_is_origin_with_identity() -> None: - p = Pose() - assert p.position.to_tuple() == (0.0, 0.0, 0.0) - assert p.orientation.to_tuple() == IDENTITY - - -def test_pose_three_positional_floats() -> None: - p = Pose(1, 2, 3) - assert p.position.to_tuple() == (1.0, 2.0, 3.0) - assert p.orientation.to_tuple() == IDENTITY - - -def test_pose_seven_positional_floats() -> None: - p = Pose(1, 2, 3, 0, 0, 0, 1) - assert p.position.to_tuple() == (1.0, 2.0, 3.0) - assert p.orientation.to_tuple() == IDENTITY - - -def test_pose_from_position_sequence_only() -> None: - p = Pose([1, 2, 3]) - assert p.position.to_tuple() == (1.0, 2.0, 3.0) - assert p.orientation.to_tuple() == IDENTITY - - -def test_pose_from_position_and_orientation_sequences() -> None: - p = Pose([1, 2, 3], [0, 0, 0, 1]) - assert p.position.to_tuple() == (1.0, 2.0, 3.0) - assert p.orientation.to_tuple() == IDENTITY - - -def test_pose_from_vector3_and_quaternion() -> None: - p = Pose(Vector3(1, 2, 3), Quaternion(0, 0, 0, 1)) - assert p.position.to_tuple() == (1.0, 2.0, 3.0) - assert p.orientation.to_tuple() == IDENTITY - - -def test_pose_from_ndarrays() -> None: - p = Pose(np.array([1.0, 2.0, 3.0]), np.array([0.0, 0.0, 0.0, 1.0])) - assert p.position.to_tuple() == (1.0, 2.0, 3.0) - assert p.orientation.to_tuple() == IDENTITY - - -def test_pose_keyword_position_only() -> None: - p = Pose(position=[1, 2, 3]) - assert p.position.to_tuple() == (1.0, 2.0, 3.0) - assert p.orientation.to_tuple() == IDENTITY - - -def test_pose_keyword_orientation_only() -> None: - p = Pose(orientation=[0, 0, 0, 1]) - assert p.position.to_tuple() == (0.0, 0.0, 0.0) - assert p.orientation.to_tuple() == IDENTITY - - -def test_pose_keyword_both() -> None: - p = Pose(position=[1, 2, 3], orientation=[0, 0, 0, 1]) - assert p.position.to_tuple() == (1.0, 2.0, 3.0) - - -def test_pose_positional_position_with_keyword_orientation() -> None: - p = Pose([1, 2, 3], orientation=[0, 0, 0, 1]) - assert p.position.to_tuple() == (1.0, 2.0, 3.0) - assert p.orientation.to_tuple() == IDENTITY - - -def test_pose_explicit_none_falls_back_to_defaults() -> None: - p = Pose(None, None) - assert p.position.to_tuple() == (0.0, 0.0, 0.0) - assert p.orientation.to_tuple() == IDENTITY - - -def test_pose_from_pair_tuple() -> None: - p = Pose(([1, 2, 3], [0, 0, 0, 1])) - assert p.position.to_tuple() == (1.0, 2.0, 3.0) - assert p.orientation.to_tuple() == IDENTITY - - -def test_pose_from_three_tuple_is_a_position() -> None: - """A 3-tuple of numbers is a position, not a (position, orientation) pair.""" - p = Pose((1, 2, 3)) - assert p.position.to_tuple() == (1.0, 2.0, 3.0) - - -def test_pose_from_dict() -> None: - p = Pose({"position": [1, 2, 3], "orientation": [0, 0, 0, 1]}) - assert p.position.to_tuple() == (1.0, 2.0, 3.0) - assert p.orientation.to_tuple() == IDENTITY +from rosbags.typesys import Stores, get_typestore + +from dimos.msgs.geometry import ( + point_from_array, + quaternion_array, + quaternion_euler, + quaternion_from_array, + vector_array, + vector_from_array, +) +from dimos.msgs.time import to_nanoseconds -def test_pose_from_dict_missing_key_raises_keyerror() -> None: - with pytest.raises(KeyError): - Pose({"position": [1, 2, 3]}) - with pytest.raises(KeyError): - Pose({}) +@pytest.mark.parametrize( + "values", [[1.0, 2.0, 3.0, 4.0], (1, 2, 3, 4), range(4), np.array([1.0, 2.0, 3.0, 4.0])] +) +def test_quaternion_explicit_array_conversion(values) -> None: + q = quaternion_from_array(values) + np.testing.assert_array_equal(quaternion_array(q), values) + assert all(type(value) is float for value in (q.x, q.y, q.z, q.w)) + assert Quaternion.decode(q.encode()) == q -def test_pose_copy_constructor() -> None: - src = Pose(1, 2, 3, 0, 0, 0, 1) - copy = Pose(src) - assert copy.position.to_tuple() == (1.0, 2.0, 3.0) - assert copy.position is not src.position +def test_quaternion_defaults_and_partial_fields() -> None: + assert Quaternion() == Quaternion(x=0, y=0, z=0, w=1) + assert Quaternion(z=1) == Quaternion(x=0, y=0, z=1, w=1) + # Explicit zero w is retained, even though it is not a spatial rotation. + assert Quaternion.decode(Quaternion(w=0).encode()).w == 0 -def test_pose_from_posestamped_downcasts_to_pose() -> None: - src = PoseStamped(ts=5.0, frame_id="odom", position=[1, 2, 3]) - p = Pose(src) - assert type(p) is Pose - assert p.position.to_tuple() == (1.0, 2.0, 3.0) +@pytest.mark.parametrize("values", [[1, 2, 3], [1, 2, 3, 4, 5], (1, 2), np.array([1.0, 2.0, 3.0])]) +def test_quaternion_array_wrong_size(values) -> None: + with pytest.raises(ValueError, match="exactly 4 components"): + quaternion_from_array(values) -def test_pose_from_lcm() -> None: - # Build the sub-messages explicitly: the generated LCM classes share one - # mutable default instance across every construction, so LCMPose().position - # carries whatever the last writer left there. - p = Pose( - LCMPose(position=LCMPoint(1.0, 2.0, 3.0), orientation=LCMQuaternion(0.0, 0.0, 0.0, 1.0)) +@pytest.mark.parametrize("position", [[1, 2, 3], (1, 2, 3), np.array([1.0, 2.0, 3.0])]) +@pytest.mark.parametrize( + "orientation", [[0, 0, 0, 1], (0, 0, 0, 1), np.array([0.0, 0.0, 0.0, 1.0])] +) +def test_pose_explicit_numeric_fields(position, orientation) -> None: + pose = Pose(position=point_from_array(position), orientation=quaternion_from_array(orientation)) + np.testing.assert_array_equal(vector_array(pose.position), [1, 2, 3]) + np.testing.assert_array_equal(quaternion_array(pose.orientation), [0, 0, 0, 1]) + assert type(pose.position) is Point + assert type(pose.orientation) is Quaternion + + +def test_pose_explicit_dictionary_and_pair_unpacking() -> None: + data = {"position": [1, 2, 3], "orientation": [0, 0, 0, 1]} + pair = (data["position"], data["orientation"]) + expected = Pose(position=Point(x=1, y=2, z=3)) + assert ( + Pose( + position=point_from_array(data["position"]), + orientation=quaternion_from_array(data["orientation"]), + ) + == expected ) - assert p.position.to_tuple() == (1.0, 2.0, 3.0) - assert p.orientation.to_tuple() == IDENTITY - - -def test_pose_position_and_orientation_are_wrapper_types() -> None: - p = Pose(1, 2, 3) - assert isinstance(p.position, Vector3) - assert isinstance(p.orientation, Quaternion) - - -# Twist - - -def test_twist_empty_is_zero() -> None: - t = Twist() - assert t.linear.to_tuple() == (0.0, 0.0, 0.0) - assert t.angular.to_tuple() == (0.0, 0.0, 0.0) - - -def test_twist_from_two_sequences() -> None: - t = Twist([1, 2, 3], [4, 5, 6]) - assert t.linear.to_tuple() == (1.0, 2.0, 3.0) - assert t.angular.to_tuple() == (4.0, 5.0, 6.0) - - -def test_twist_from_two_vector3() -> None: - t = Twist(Vector3(1, 2, 3), Vector3(4, 5, 6)) - assert t.linear.to_tuple() == (1.0, 2.0, 3.0) - assert t.angular.to_tuple() == (4.0, 5.0, 6.0) - - -def test_twist_from_ndarrays() -> None: - t = Twist(np.array([1.0, 2.0, 3.0]), np.array([4.0, 5.0, 6.0])) - assert t.linear.to_tuple() == (1.0, 2.0, 3.0) - assert t.angular.to_tuple() == (4.0, 5.0, 6.0) - - -def test_twist_angular_quaternion_is_converted_to_euler() -> None: - t = Twist([1, 2, 3], Quaternion(0, 0, 0, 1)) - assert t.linear.to_tuple() == (1.0, 2.0, 3.0) - assert t.angular.to_tuple() == (0.0, 0.0, 0.0) - - -def test_twist_copy_constructor() -> None: - src = Twist([1, 2, 3], [4, 5, 6]) - copy = Twist(src) - assert copy.linear.to_tuple() == (1.0, 2.0, 3.0) - assert copy.linear is not src.linear - - -def test_twist_from_lcm() -> None: - t = Twist(LCMTwist(linear=LCMVector3(1.0, 2.0, 3.0), angular=LCMVector3(4.0, 5.0, 6.0))) - assert t.linear.to_tuple() == (1.0, 2.0, 3.0) - assert t.angular.to_tuple() == (4.0, 5.0, 6.0) - - -def test_twist_from_twiststamped_downcasts() -> None: - t = Twist(TwistStamped(linear=[1, 2, 3], angular=[4, 5, 6])) - assert type(t) is Twist - assert t.linear.to_tuple() == (1.0, 2.0, 3.0) - - -def test_twist_keyword_both() -> None: - t = Twist(linear=[1, 2, 3], angular=[4, 5, 6]) - assert t.linear.to_tuple() == (1.0, 2.0, 3.0) - assert t.angular.to_tuple() == (4.0, 5.0, 6.0) - - -def test_twist_keyword_linear_only() -> None: - t = Twist(linear=[1, 2, 3]) - assert t.linear.to_tuple() == (1.0, 2.0, 3.0) - assert t.angular.to_tuple() == (0.0, 0.0, 0.0) - - -def test_twist_keyword_angular_only() -> None: - t = Twist(angular=[4, 5, 6]) - assert t.linear.to_tuple() == (0.0, 0.0, 0.0) - assert t.angular.to_tuple() == (4.0, 5.0, 6.0) - - -def test_twist_keyword_angular_quaternion_is_converted_to_euler() -> None: - t = Twist(linear=[1, 2, 3], angular=Quaternion(0, 0, 0, 1)) - assert t.angular.to_tuple() == (0.0, 0.0, 0.0) - - -# PoseWithCovariance - - -def test_pwc_empty() -> None: - p = PoseWithCovariance() - assert p.pose.position.to_tuple() == (0.0, 0.0, 0.0) - assert np.array_equal(p.covariance, np.zeros(36)) - - -def test_pwc_from_pose_only() -> None: - p = PoseWithCovariance(Pose(1, 2, 3)) - assert p.pose.position.to_tuple() == (1.0, 2.0, 3.0) - assert np.array_equal(p.covariance, np.zeros(36)) - - -def test_pwc_from_pose_and_covariance() -> None: - p = PoseWithCovariance(Pose(1, 2, 3), np.arange(36.0)) - assert np.array_equal(p.covariance, np.arange(36.0)) - - -def test_pwc_explicit_none_covariance_is_zeros() -> None: - assert np.array_equal(PoseWithCovariance(Pose(1, 2, 3), None).covariance, np.zeros(36)) - - -def test_pwc_covariance_wrong_size_raises() -> None: - with pytest.raises(ValueError, match=r"reshape"): - PoseWithCovariance(Pose(1, 2, 3), [1, 2, 3]) - - -def test_pwc_copy_constructor_copies_covariance() -> None: - src = PoseWithCovariance(Pose(1, 2, 3), np.arange(36.0)) - copy = PoseWithCovariance(src) - assert np.array_equal(copy.covariance, src.covariance) - assert copy.covariance is not src.covariance - - -def test_pwc_from_lcm() -> None: - lcm_pose = LCMPose( - position=LCMPoint(1.0, 2.0, 3.0), orientation=LCMQuaternion(0.0, 0.0, 0.0, 1.0) + assert ( + Pose(position=point_from_array(pair[0]), orientation=quaternion_from_array(pair[1])) + == expected ) - p = PoseWithCovariance(LCMPoseWithCovariance(pose=lcm_pose, covariance=list(range(36)))) - assert p.pose.position.to_tuple() == (1.0, 2.0, 3.0) - assert np.array_equal(p.covariance, np.arange(36.0)) - - -def test_pwc_from_dict_with_pose_object() -> None: - p = PoseWithCovariance({"pose": Pose(1, 2, 3)}) - assert p.pose.position.to_tuple() == (1.0, 2.0, 3.0) - assert np.array_equal(p.covariance, np.zeros(36)) - - -def test_pwc_from_dict_with_covariance() -> None: - p = PoseWithCovariance({"pose": Pose(1, 2, 3), "covariance": list(range(36))}) - assert np.array_equal(p.covariance, np.arange(36.0)) - - -def test_pwc_covariance_matrix_view() -> None: - p = PoseWithCovariance(Pose(), np.arange(36.0)) - assert p.covariance_matrix.shape == (6, 6) - - -# TwistWithCovariance - - -def test_twc_empty() -> None: - t = TwistWithCovariance() - assert t.twist.linear.to_tuple() == (0.0, 0.0, 0.0) - assert np.array_equal(t.covariance, np.zeros(36)) - - -def test_twc_from_twist_only() -> None: - t = TwistWithCovariance(Twist([1, 2, 3], [4, 5, 6])) - assert t.twist.linear.to_tuple() == (1.0, 2.0, 3.0) - - -def test_twc_from_twist_and_covariance() -> None: - t = TwistWithCovariance(Twist([1, 2, 3], [4, 5, 6]), np.arange(36.0)) - assert np.array_equal(t.covariance, np.arange(36.0)) - - -def test_twc_from_linear_angular_pair() -> None: - t = TwistWithCovariance(([1, 2, 3], [4, 5, 6])) - assert t.twist.linear.to_tuple() == (1.0, 2.0, 3.0) - assert t.twist.angular.to_tuple() == (4.0, 5.0, 6.0) - + assert Pose(position=Point(x=5)).position == Point(x=5, y=0, z=0) -def test_twc_copy_constructor() -> None: - src = TwistWithCovariance(Twist([1, 2, 3], [4, 5, 6]), np.arange(36.0)) - copy = TwistWithCovariance(src) - assert np.array_equal(copy.covariance, src.covariance) - assert copy.covariance is not src.covariance +@pytest.mark.parametrize("values", [[1, 2, 3], (1, 2, 3), np.array([1.0, 2.0, 3.0])]) +def test_twist_explicit_arrays(values) -> None: + twist = Twist(linear=vector_from_array(values), angular=vector_from_array([4, 5, 6])) + np.testing.assert_array_equal(vector_array(twist.linear), [1, 2, 3]) + np.testing.assert_array_equal(vector_array(twist.angular), [4, 5, 6]) + assert Twist(linear=twist.linear).angular == Vector3() + assert Twist(angular=twist.angular).linear == Vector3() -def test_twc_from_lcm() -> None: - lcm_twist = LCMTwist(linear=LCMVector3(1.0, 2.0, 3.0), angular=LCMVector3(4.0, 5.0, 6.0)) - t = TwistWithCovariance(LCMTwistWithCovariance(twist=lcm_twist, covariance=list(range(36)))) - assert t.twist.linear.to_tuple() == (1.0, 2.0, 3.0) - assert np.array_equal(t.covariance, np.arange(36.0)) - -def test_twc_from_dict() -> None: - t = TwistWithCovariance({"twist": Twist([1, 2, 3], [4, 5, 6])}) - assert t.twist.linear.to_tuple() == (1.0, 2.0, 3.0) - assert np.array_equal(t.covariance, np.zeros(36)) - - -def test_twc_from_dict_with_covariance() -> None: - t = TwistWithCovariance({"twist": Twist([1, 2, 3], [4, 5, 6]), "covariance": list(range(36))}) - assert np.array_equal(t.covariance, np.arange(36.0)) - - -# JointState - - -def test_jointstate_empty_has_empty_lists() -> None: - j = JointState() - assert (j.name, j.position, j.velocity, j.effort) == ([], [], [], []) - assert j.frame_id == "" - - -def test_jointstate_keywords() -> None: - j = JointState(ts=5.0, frame_id="f", name=["a"], position=[1.0]) - assert (j.ts, j.frame_id, j.name, j.position) == (5.0, "f", ["a"], [1.0]) - assert (j.velocity, j.effort) == ([], []) - - -def test_jointstate_all_positional() -> None: - j = JointState(5.0, "f", ["a"], [1.0], [2.0], [3.0]) - assert (j.name, j.position, j.velocity, j.effort) == (["a"], [1.0], [2.0], [3.0]) - - -def test_jointstate_from_dict() -> None: - j = JointState({"name": ["a"], "position": [1.0]}) - assert (j.name, j.position) == (["a"], [1.0]) - - -def test_jointstate_from_empty_dict() -> None: - j = JointState({}) - assert (j.name, j.position, j.velocity, j.effort) == ([], [], [], []) - - -def test_jointstate_copy_constructor_copies_lists() -> None: - src = JointState(ts=5.0, frame_id="f", name=["a"], position=[1.0]) - copy = JointState(src) - assert copy.ts == 5.0 - assert copy.name == ["a"] - assert copy.name is not src.name - - -def test_jointstate_from_lcm() -> None: - j = JointState(LCMJointState()) - assert j.name == [] - - -# Joy - - -def test_joy_empty() -> None: - j = Joy() - assert (j.axes, j.buttons, j.frame_id) == ([], [], "") - - -def test_joy_keywords() -> None: - j = Joy(ts=5.0, frame_id="f", axes=[1.0], buttons=[1]) - assert (j.ts, j.frame_id, j.axes, j.buttons) == (5.0, "f", [1.0], [1]) - - -def test_joy_all_positional() -> None: - j = Joy(5.0, "f", [1.0], [1]) - assert (j.axes, j.buttons) == ([1.0], [1]) - - -def test_joy_from_axes_buttons_pair() -> None: - j = Joy(([1.0, 2.0], [1, 0])) - assert (j.axes, j.buttons) == ([1.0, 2.0], [1, 0]) - - -def test_joy_from_dict() -> None: - j = Joy({"axes": [1.0], "buttons": [1]}) - assert (j.axes, j.buttons) == ([1.0], [1]) - - -def test_joy_from_empty_dict() -> None: - j = Joy({}) - assert (j.axes, j.buttons) == ([], []) - - -def test_joy_copy_constructor_copies_lists() -> None: - src = Joy(ts=5.0, axes=[1.0], buttons=[1]) - copy = Joy(src) - assert copy.ts == 5.0 - assert copy.axes == [1.0] - assert copy.axes is not src.axes - - -def test_joy_from_lcm() -> None: - j = Joy(LCMJoy()) - assert j.axes == [] - - -# Stamped wrappers: ts / frame_id plumbing - - -def test_posestamped_defaults_stamp_to_now() -> None: - before = time.time() - ps = PoseStamped() - assert before <= ps.ts <= time.time() - assert ps.frame_id == "" - assert ps.position.to_tuple() == (0.0, 0.0, 0.0) - - -def test_posestamped_keeps_explicit_ts_and_frame() -> None: - ps = PoseStamped(ts=5.0, frame_id="odom", position=[1, 2, 3], orientation=[0, 0, 0, 1]) - assert (ps.ts, ps.frame_id) == (5.0, "odom") - assert ps.position.to_tuple() == (1.0, 2.0, 3.0) - - -def test_posestamped_positional_ts_and_frame() -> None: - ps = PoseStamped(5.0, "odom") - assert (ps.ts, ps.frame_id) == (5.0, "odom") - - -def test_posestamped_is_a_pose() -> None: - assert isinstance(PoseStamped(), Pose) - - -def test_posestamped_inherits_pose_positional_forms() -> None: - """PoseStamped(x, y, z) and the 7-float form come from Pose.""" - assert PoseStamped(3.2, 1.5, 0.0).position.to_tuple() == (3.2, 1.5, 0.0) - seven = PoseStamped(1, 2, 0, 0, 0, 0.1, 1) - assert seven.position.to_tuple() == (1.0, 2.0, 0.0) - assert seven.orientation.to_tuple() == (0.0, 0.0, 0.1, 1.0) - - -def test_posestamped_inherits_pose_sequence_forms() -> None: - ps = PoseStamped([1, 2, 3], [0, 0, 0, 1]) - assert ps.position.to_tuple() == (1.0, 2.0, 3.0) - assert ps.orientation.to_tuple() == IDENTITY - - -def test_twiststamped_inherits_twist_positional_form() -> None: - ts = TwistStamped([1, 2, 3], [4, 5, 6]) - assert ts.linear.to_tuple() == (1.0, 2.0, 3.0) - assert ts.angular.to_tuple() == (4.0, 5.0, 6.0) - - -def test_twiststamped_keeps_explicit_ts_and_frame() -> None: - ts = TwistStamped(ts=5.0, frame_id="base", linear=[1, 2, 3], angular=[4, 5, 6]) - assert (ts.ts, ts.frame_id) == (5.0, "base") - assert ts.linear.to_tuple() == (1.0, 2.0, 3.0) - - -def test_twiststamped_defaults_stamp_to_now() -> None: - before = time.time() - t = TwistStamped() - assert before <= t.ts <= time.time() - - -def test_pwcs_keeps_explicit_ts_and_frame() -> None: - p = PoseWithCovarianceStamped(ts=5.0, frame_id="map", pose=Pose(1, 2, 3)) - assert (p.ts, p.frame_id) == (5.0, "map") - assert p.pose.position.to_tuple() == (1.0, 2.0, 3.0) - - -def test_pwcs_with_covariance() -> None: - p = PoseWithCovarianceStamped(ts=5.0, pose=Pose(1, 2, 3), covariance=list(range(36))) - assert np.array_equal(p.covariance, np.arange(36.0)) - - -def test_pwcs_empty_defaults() -> None: - p = PoseWithCovarianceStamped() - assert p.pose.position.to_tuple() == (0.0, 0.0, 0.0) - assert np.array_equal(p.covariance, np.zeros(36)) - - -def test_twcs_keeps_explicit_ts_and_frame() -> None: - t = TwistWithCovarianceStamped(ts=5.0, frame_id="base", twist=Twist([1, 2, 3], [4, 5, 6])) - assert (t.ts, t.frame_id) == (5.0, "base") - assert t.twist.linear.to_tuple() == (1.0, 2.0, 3.0) - - -def test_twcs_with_covariance() -> None: - t = TwistWithCovarianceStamped( - ts=5.0, twist=Twist([1, 2, 3], [4, 5, 6]), covariance=list(range(36)) +def test_twist_euler_conversion_is_explicit() -> None: + twist = Twist( + linear=Vector3(x=1, y=2, z=3), angular=vector_from_array(quaternion_euler(Quaternion())) ) - assert np.array_equal(t.covariance, np.arange(36.0)) - - -def test_twcs_empty_defaults() -> None: - t = TwistWithCovarianceStamped() - assert t.twist.linear.to_tuple() == (0.0, 0.0, 0.0) - + assert twist.linear == Vector3(x=1, y=2, z=3) + assert twist.angular == Vector3() + + +@pytest.mark.parametrize( + "message", + [ + Quaternion(x=0.1, y=0.2, z=0.3, w=0.4), + Pose(position=Point(x=1, y=2, z=3)), + Twist(linear=Vector3(x=1, y=2, z=3), angular=Vector3(x=4, y=5, z=6)), + PoseWithCovariance(pose=Pose(position=Point(x=1, y=2, z=3)), covariance=list(range(36))), + TwistWithCovariance(twist=Twist(linear=Vector3(x=1, y=2, z=3)), covariance=list(range(36))), + JointState(header=Header(stamp=Time(sec=5), frame_id="f"), name=["a"], position=[1.0]), + Joy(header=Header(stamp=Time(sec=5), frame_id="f"), axes=[1.0], buttons=[1]), + ], +) +def test_copy_and_cdr_roundtrip_preserve_fields(message) -> None: + copied = deepcopy(message) + assert copied == message + assert copied is not message + assert type(message).decode(message.encode()) == message + independent = get_typestore(Stores.ROS2_JAZZY).deserialize_cdr( + message.encode(), message.msg_name + ) + assert independent.__msgtype__ == message.msg_name + + +def test_copy_mutation_is_independent() -> None: + pose = Pose(position=Point(x=1, y=2, z=3)) + pose_copy = deepcopy(pose) + pose_copy.position.x = 99 + assert pose.position.x == 1 + twist = Twist(linear=Vector3(x=1, y=2, z=3)) + twist_copy = deepcopy(twist) + twist_copy.linear.x = 99 + assert twist.linear.x == 1 + covariance = PoseWithCovariance(covariance=list(range(36))) + covariance_copy = deepcopy(covariance) + covariance_copy.covariance[0] = 99 + assert covariance.covariance[0] == 0 + joints = JointState(name=["a"], position=[1.0]) + joints_copy = deepcopy(joints) + joints_copy.name[0] = "b" + assert joints.name == ["a"] + joy = Joy(axes=[1.0], buttons=[1]) + joy_copy = deepcopy(joy) + joy_copy.axes[0] = 2 + assert joy.axes == [1.0] + + +@pytest.mark.parametrize("message_type", [PoseWithCovariance, TwistWithCovariance]) +def test_covariance_defaults_layout_and_length(message_type) -> None: + assert list(message_type().covariance) == [0.0] * 36 + covariance = np.arange(36.0).reshape(6, 6) + message = message_type(covariance=covariance.flatten().tolist()) + np.testing.assert_array_equal(np.asarray(message.covariance).reshape(6, 6), covariance) + for invalid in ([1, 2, 3], [0.0] * 35, [0.0] * 37): + with pytest.raises(RuntimeError, match="Unable to cast"): + message_type(covariance=invalid) + + +def test_sensor_explicit_fields_and_defaults() -> None: + joints = JointState( + header=Header(stamp=Time(sec=5), frame_id="f"), + name=["a"], + position=[1.0], + velocity=[2.0], + effort=[3.0], + ) + assert (joints.name, joints.position, joints.velocity, joints.effort) == ( + ["a"], + [1.0], + [2.0], + [3.0], + ) + assert joints.header == Header(stamp=Time(sec=5), frame_id="f") + assert JointState().name == [] + assert JointState().velocity == [] + assert JointState().effort == [] + pair = ([1.0, 2.0], [1, 0]) + joy = Joy(axes=pair[0], buttons=pair[1]) + assert (joy.axes, joy.buttons) == pair + assert (Joy().axes, Joy().buttons) == ([], []) + assert Joy().header.frame_id == "" -# The ``ts`` sentinel -# -# ``ts`` defaults to None and means "stamp me now". 0.0 is an ordinary -# timestamp and is stored as given — it used to be swallowed by a -# ``ts if ts != 0`` sentinel that made it unrepresentable. STAMPED_TYPES = [ PoseStamped, @@ -632,152 +215,77 @@ def test_twcs_empty_defaults() -> None: Joy, Path, PointStamped, - Transform, + TransformStamped, Odometry, ] -@pytest.mark.parametrize("msg_type", STAMPED_TYPES, ids=lambda t: t.__name__) -def test_zero_ts_is_a_real_timestamp(msg_type: type) -> None: - assert msg_type(ts=0.0).ts == 0.0 - - -@pytest.mark.parametrize("msg_type", STAMPED_TYPES, ids=lambda t: t.__name__) -def test_omitted_ts_stamps_now(msg_type: type) -> None: - before = time.time() - assert before <= msg_type().ts <= time.time() - - -@pytest.mark.parametrize("msg_type", STAMPED_TYPES, ids=lambda t: t.__name__) -def test_explicit_ts_is_honoured(msg_type: type) -> None: - assert msg_type(ts=5.0).ts == 5.0 - - -def test_lcm_decode_of_an_unstamped_message_keeps_the_zero_stamp() -> None: - """A producer that never set the header now decodes to ts=0.0, not receive-time.""" - decoded = PoseStamped.lcm_decode(PoseStamped(ts=0.0, frame_id="odom").lcm_encode()) - assert decoded.ts == 0.0 - - -# Forms that used to build a silently-broken object -# -# Under plum these calls matched no overload and fell through to the LCM base -# __init__, which stores its arguments verbatim — leaving fields holding raw -# scalars/lists instead of Vector3/Quaternion/Pose, to blow up later in -# __repr__ or on encode. They now either do the obvious thing or raise. - - -def test_pose_from_scalar_is_an_x_only_position() -> None: - """Vector3(x) means (x, 0, 0), so Pose(5) is a position, not a raw 5.""" - assert Pose(5).position.to_tuple() == (5.0, 0.0, 0.0) - - -def test_pose_from_string_raises() -> None: - with pytest.raises(TypeError): - Pose("x") - - -def test_pose_from_two_numeric_positionals_raises() -> None: - """(1, 2) is a position and an orientation, and 2 is not a quaternion.""" - with pytest.raises(TypeError): - Pose(1, 2) - - -def test_pose_from_three_tuple_of_sequences_raises() -> None: - with pytest.raises((TypeError, ValueError)): - Pose(([1, 2, 3], [0, 0, 0, 1], [9])) - - -def test_quaternion_partial_positionals_raise() -> None: - """A quaternion needs all four components; 3 no longer zero-fills w.""" - for bad in ((1, 2, 3), (1, 2), (5,)): - with pytest.raises((TypeError, ValueError)): - Quaternion(*bad) - - -def test_quaternion_accepts_field_keywords() -> None: - assert Quaternion(x=1, y=2, z=3, w=4).to_tuple() == (1.0, 2.0, 3.0, 4.0) - - -def test_quaternion_partial_field_keywords_default_to_identity() -> None: - assert Quaternion(z=1).to_tuple() == (0.0, 0.0, 1.0, 1.0) - - -def test_quaternion_unknown_keyword_raises() -> None: - with pytest.raises(TypeError): - Quaternion(bogus=1) - - -def test_twist_from_single_sequence_is_linear_only() -> None: - t = Twist([1, 2, 3]) - assert t.linear.to_tuple() == (1.0, 2.0, 3.0) - assert t.angular.to_tuple() == (0.0, 0.0, 0.0) - - -def test_twist_from_scalars_raises() -> None: - with pytest.raises(TypeError): - Twist("x", "y") - - -def test_twist_mixed_positional_and_keyword_converts_both() -> None: - t = Twist([1, 2, 3], angular=[4, 5, 6]) - assert isinstance(t.linear, Vector3) - assert t.linear.to_tuple() == (1.0, 2.0, 3.0) - assert t.angular.to_tuple() == (4.0, 5.0, 6.0) - - -def test_twist_unknown_keyword_raises() -> None: - with pytest.raises(TypeError): - Twist(bogus=1) +@pytest.mark.parametrize("message_type", STAMPED_TYPES) +@pytest.mark.parametrize( + "stamp", + [Time(), Time(sec=5), Time(sec=-1, nanosec=999999999), Time(sec=1700000000, nanosec=123456789)], +) +def test_exact_stamps_survive_cdr(message_type, stamp) -> None: + message = message_type(header=Header(stamp=stamp, frame_id="odom")) + decoded = message_type.decode(message.encode()) + assert decoded.header == message.header + assert to_nanoseconds(decoded.header.stamp) == stamp.sec * 1000000000 + stamp.nanosec + + +@pytest.mark.parametrize("message_type", STAMPED_TYPES) +def test_omitted_stamp_is_zero_without_wall_clock(message_type) -> None: + message = message_type() + assert message.header.stamp == Time() + assert message_type.decode(message.encode()).header.stamp == Time() + + +def test_stamped_payloads_are_nested_values() -> None: + pose = Pose(position=Point(x=1, y=2, z=3), orientation=Quaternion(z=0.1, w=1)) + stamped = PoseStamped(header=Header(stamp=Time(sec=5), frame_id="odom"), pose=pose) + assert not isinstance(stamped, Pose) + assert stamped.pose == pose + assert deepcopy(stamped.pose) == pose + twist = Twist(linear=Vector3(x=1, y=2, z=3), angular=Vector3(x=4, y=5, z=6)) + assert TwistStamped(twist=twist).twist == twist + pwc = PoseWithCovarianceStamped(pose=PoseWithCovariance(pose=pose, covariance=list(range(36)))) + assert pwc.pose.pose == pose + assert list(pwc.pose.covariance) == list(range(36)) + twc = TwistWithCovarianceStamped( + twist=TwistWithCovariance(twist=twist, covariance=list(range(36))) + ) + assert twc.twist.twist == twist + assert list(twc.twist.covariance) == list(range(36)) -def test_pose_unknown_keyword_raises() -> None: +@pytest.mark.parametrize( + "message_type", + [Pose, Quaternion, Twist, PoseWithCovariance, TwistWithCovariance, JointState, Joy], +) +def test_unknown_fields_rejected(message_type) -> None: with pytest.raises(TypeError): - Pose(bogus=1) - - -def test_pwc_pair_tuple_is_a_pose_and_a_covariance() -> None: - """The documented (pose, covariance) tuple form now actually fires.""" - p = PoseWithCovariance((Pose(1, 2, 3), list(range(36)))) - assert p.pose.position.to_tuple() == (1.0, 2.0, 3.0) - assert np.array_equal(p.covariance, np.arange(36.0)) - - -def test_pwc_accepts_field_keywords() -> None: - p = PoseWithCovariance(pose=Pose(1, 2, 3), covariance=list(range(36))) - assert p.pose.position.to_tuple() == (1.0, 2.0, 3.0) - assert np.array_equal(p.covariance, np.arange(36.0)) - - -def test_twc_accepts_field_keywords() -> None: - t = TwistWithCovariance(twist=Twist([1, 2, 3], [4, 5, 6])) - assert t.twist.linear.to_tuple() == (1.0, 2.0, 3.0) - - -def test_twc_pair_tuple_is_a_twist_and_a_covariance() -> None: - t = TwistWithCovariance((Twist([1, 2, 3], [4, 5, 6]), list(range(36)))) - assert t.twist.linear.to_tuple() == (1.0, 2.0, 3.0) - assert np.array_equal(t.covariance, np.arange(36.0)) - - -def test_pwc_dict_pose_may_be_any_pose_convertable() -> None: - p = PoseWithCovariance({"pose": [1, 2, 3], "covariance": list(range(36))}) - assert p.pose.position.to_tuple() == (1.0, 2.0, 3.0) - assert np.array_equal(p.covariance, np.arange(36.0)) - - -def test_jointstate_dict_carries_ts_and_frame_id() -> None: - j = JointState({"ts": 5.0, "frame_id": "f"}) - assert (j.ts, j.frame_id) == (5.0, "f") - assert j.name == [] - - -def test_joy_dict_carries_ts_and_frame_id() -> None: - j = Joy({"ts": 5.0, "frame_id": "f"}) - assert (j.ts, j.frame_id) == (5.0, "f") - assert j.axes == [] + message_type(bogus=1) + + +@pytest.mark.parametrize( + "message_type,kwargs", + [ + (Pose, {"position": [1, 2, 3]}), + (Pose, {"position": "x"}), + (Pose, {"orientation": [0, 0, 0, 1]}), + (Pose, {"position": None}), + (Twist, {"linear": [1, 2, 3]}), + (Twist, {"angular": Quaternion()}), + (PoseStamped, {"ts": 5}), + (PoseStamped, {"frame_id": "odom"}), + (PoseStamped, {"position": Point(x=1)}), + ], +) +def test_retired_polymorphic_fields_rejected(message_type, kwargs) -> None: + with pytest.raises((TypeError, RuntimeError)): + message_type(**kwargs) -def test_posestamped_string_positional_is_a_timestamp_not_a_position() -> None: - """A str first positional is a (bogus) ts; it no longer corrupts the position.""" - assert PoseStamped("f").position.to_tuple() == (0.0, 0.0, 0.0) +@pytest.mark.parametrize("values", [[], [1, 2], [1, 2, 3, 4], [[1, 2, 3], [4, 5, 6]]]) +def test_position_array_shape_rejected(values) -> None: + with pytest.raises(ValueError, match="3 vector components"): + point_from_array(values) diff --git a/dimos/msgs/test_occupancy.py b/dimos/msgs/test_occupancy.py new file mode 100644 index 00000000000..8773aa1a0f9 --- /dev/null +++ b/dimos/msgs/test_occupancy.py @@ -0,0 +1,146 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import math + +from dimos_generated.builtin_interfaces.msg import Time +from dimos_generated.nav_msgs.msg import MapMetaData, OccupancyGrid +from dimos_generated.std_msgs.msg import Header +import numpy as np +from PIL import Image +import pytest + +from dimos.mapping.occupancy.gradient import gradient +from dimos.msgs.occupancy import occupancy_extent, occupancy_from_file, occupancy_view + + +def test_occupancy_cells_keep_ros_row_order_and_do_not_alias_message(): + message = OccupancyGrid( + info=MapMetaData(width=3, height=2, resolution=0.1), data=[-1, 0, 100, 25, 50, 75] + ) + decoded = OccupancyGrid.decode(message.encode()) + values = occupancy_view(decoded) + np.testing.assert_array_equal(values, [[-1, 0, 100], [25, 50, 75]]) + with pytest.raises(ValueError, match="read-only"): + values[0, 0] = 0 + copied = values.copy() + copied[0, 0] = 0 + assert decoded.data[0] == -1 + + +def test_occupancy_rejects_dimensions_that_do_not_match_payload(): + message = OccupancyGrid(info=MapMetaData(width=2, height=2, resolution=1.0), data=[0]) + with pytest.raises(ValueError, match="data length"): + occupancy_view(message) + + +@pytest.mark.parametrize("resolution", [0.0, -0.1, math.nan, math.inf]) +def test_occupancy_rejects_invalid_resolution_for_nonempty_grid(resolution): + message = OccupancyGrid(info=MapMetaData(width=1, height=1, resolution=resolution), data=[0]) + with pytest.raises(ValueError, match="resolution"): + occupancy_extent(message) + + +def test_empty_default_occupancy_grid_has_no_cells(): + assert occupancy_view(OccupancyGrid()).shape == (0, 0) + + +@pytest.mark.parametrize("suffix", [".npy", ".png"]) +def test_static_occupancy_loader_preserves_cells_header_and_identity(tmp_path, suffix): + cells = np.array([[-1, 0, 100], [25, 50, 75]], dtype=np.int8) + path = tmp_path / ("grid" + suffix) + if suffix == ".npy": + np.save(path, cells) + else: + Image.fromarray(cells.astype(np.uint8)).save(path) + header = Header(frame_id="map", stamp=Time(sec=123, nanosec=456)) + output = OccupancyGrid.decode(occupancy_from_file(path, header=header, resolution=0.2).encode()) + np.testing.assert_array_equal(occupancy_view(output), cells) + assert output.header == header + assert output.info.map_load_time == header.stamp + assert output.info.origin.orientation.w == 1 + assert output.info.resolution == float(np.float32(0.2)) + + +@pytest.mark.parametrize( + "cells", + [np.zeros((2, 2), dtype=float), np.zeros(3, dtype=int), np.array([[101]]), np.array([[-2]])], +) +def test_static_occupancy_loader_rejects_invalid_cells(tmp_path, cells): + path = tmp_path / "grid.npy" + np.save(path, cells) + with pytest.raises(ValueError): + occupancy_from_file(path, header=Header()) + + +def test_gradient() -> None: + """Test converting occupancy grid to gradient field.""" + # Create a small test grid with an obstacle in the middle + data = np.zeros((10, 10), dtype=np.int8) + data[4:6, 4:6] = 100 # 2x2 obstacle in center + + grid = OccupancyGrid( + header=Header(frame_id="world"), + info=MapMetaData(width=10, height=10, resolution=0.1), + data=data.ravel(), + ) # 0.1m per cell + + # Convert to gradient + gradient_grid = gradient(grid, obstacle_threshold=50, max_distance=1.0) + + # Check that we get an OccupancyGrid back + assert isinstance(gradient_grid, OccupancyGrid) + assert occupancy_view(gradient_grid).shape == (10, 10) + assert gradient_grid.info.resolution == grid.info.resolution + assert gradient_grid.header == grid.header + + # Obstacle cells should have value 100 + assert occupancy_view(gradient_grid)[4, 4] == 100 + assert occupancy_view(gradient_grid)[5, 5] == 100 + + # Adjacent cells should have high values (near obstacles) + assert occupancy_view(gradient_grid)[3, 4] > 85 # Very close to obstacle + assert occupancy_view(gradient_grid)[4, 3] > 85 # Very close to obstacle + + # Cells at moderate distance should have moderate values + assert 30 < occupancy_view(gradient_grid)[0, 0] < 60 # Corner is ~0.57m away + + # Check that gradient decreases with distance + assert ( + occupancy_view(gradient_grid)[3, 4] > occupancy_view(gradient_grid)[2, 4] + ) # Closer is higher + assert ( + occupancy_view(gradient_grid)[2, 4] > occupancy_view(gradient_grid)[0, 4] + ) # Further is lower + + # Test with unknown cells + data_with_unknown = data.copy() + data_with_unknown[0:2, 0:2] = -1 # Add unknown area (close to obstacle) + data_with_unknown[8:10, 8:10] = -1 # Add unknown area (far from obstacle) + + grid_with_unknown = OccupancyGrid( + header=grid.header, info=grid.info, data=data_with_unknown.ravel() + ) + gradient_with_unknown = gradient(grid_with_unknown, max_distance=1.0) # 1m max distance + + # Unknown cells should remain unknown (new behavior - unknowns are preserved) + assert occupancy_view(gradient_with_unknown)[0, 0] == -1 # Should remain unknown + assert occupancy_view(gradient_with_unknown)[1, 1] == -1 # Should remain unknown + assert occupancy_view(gradient_with_unknown)[8, 8] == -1 # Should remain unknown + assert occupancy_view(gradient_with_unknown)[9, 9] == -1 # Should remain unknown + + # Unknown cells count should be preserved + assert ( + np.count_nonzero(occupancy_view(gradient_with_unknown) == -1) == 8 + ) # All unknowns preserved diff --git a/dimos/msgs/test_pointcloud.py b/dimos/msgs/test_pointcloud.py new file mode 100644 index 00000000000..e38565a6a2a --- /dev/null +++ b/dimos/msgs/test_pointcloud.py @@ -0,0 +1,384 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +# Copyright 2026 Dimensional Inc. +# SPDX-License-Identifier: Apache-2.0 + +import gc +import struct + +from dimos_generated.builtin_interfaces.msg import Time +from dimos_generated.geometry_msgs.msg import Quaternion, Transform, TransformStamped, Vector3 +from dimos_generated.sensor_msgs.msg import PointCloud2, PointField +from dimos_generated.std_msgs.msg import Header +import numpy as np +import pytest + +from dimos.msgs.camera_info import camera_info_from_intrinsics +from dimos.msgs.image import image_from_array +from dimos.msgs.pointcloud import ( + concatenate_clouds, + pointcloud_from_rgbd, + pointcloud_from_xyz, + pointcloud_from_xyz_rgb, + pointcloud_rgb, + pointcloud_view, + pointcloud_xyz, + select_points, + transform_cloud, + voxel_downsample_cloud, +) + + +@pytest.fixture(params=[False, True], ids=["little-endian", "big-endian"]) +def padded_cloud(request): + # Two organized rows, two points per row; fields are deliberately reordered. + endian = ">" if request.param else "<" + payload = bytearray(128) + for row in range(2): + for column in range(2): + value = row * 2 + column + 1 + struct.pack_into( + endian + "df2Hf", + payload, + row * 64 + column * 24, + value * 3.0, + value * 1.0, + value, + value + 10, + value * 2.0, + ) + return PointCloud2( + height=2, + width=2, + point_step=24, + row_step=64, + is_bigendian=request.param, + data=bytes(payload), + fields=[ + PointField(name="z", offset=0, datatype=8, count=1), + PointField(name="x", offset=8, datatype=7, count=1), + PointField(name="tags", offset=12, datatype=4, count=2), + PointField(name="y", offset=16, datatype=7, count=1), + ], + ) + + +def test_field_counts_padding_and_endianness(padded_cloud): + view = pointcloud_view(padded_cloud) + assert view["tags"].tolist() == [[[1, 11], [2, 12]], [[3, 13], [4, 14]]] + assert pointcloud_xyz(padded_cloud).tolist() == [[1, 2, 3], [2, 4, 6], [3, 6, 9], [4, 8, 12]] + assert not view.flags.writeable + with pytest.raises(ValueError, match="WRITEABLE"): + view.setflags(write=True) + copied = view.copy() + copied["x"][0, 0] = 99 + assert view["x"][0, 0] == 1 + + +def test_view_retains_storage_after_message_is_deleted(): + message = PointCloud2( + height=1, + width=1, + point_step=4, + row_step=4, + fields=[PointField(name="x", datatype=7, count=1)], + data=struct.pack("u2"), encoding="16UC1", header=header) + # Add nonzero row padding to big-endian depth. + payload = bytes(depth.data) + depth.data = payload[:6] + b"xx" + payload[6:] + b"yy" + depth.step = 8 + calibration = camera_info_from_intrinsics(4, 8, 2, 2, 6, 4, header=header) + cloud = PointCloud2.decode( + pointcloud_from_rgbd(color, depth, calibration, depth_scale=0.001, depth_trunc=4).encode() + ) + np.testing.assert_allclose( + pointcloud_xyz(cloud), [[-0.5, -0.25, 1], [1, -0.5, 2], [-1.5, 0, 3]] + ) + np.testing.assert_array_equal(pointcloud_rgb(cloud), rgb.reshape(-1, 3)[[0, 2, 3]]) + assert cloud.header == header + + +@pytest.mark.parametrize( + "scale,trunc", [(0, 1), (-1, 1), (float("nan"), 1), (1, 0), (1, float("inf"))] +) +def test_rgbd_rejects_invalid_units(scale, trunc): + header = Header() + color = image_from_array(np.zeros((1, 1, 3), dtype=np.uint8), encoding="rgb8", header=header) + depth = image_from_array(np.ones((1, 1), dtype=np.uint16), encoding="16UC1", header=header) + calibration = camera_info_from_intrinsics(1, 1, 0, 0, 1, 1, header=header) + with pytest.raises(ValueError, match="finite and positive"): + pointcloud_from_rgbd(color, depth, calibration, depth_scale=scale, depth_trunc=trunc) + + +def test_transform_rejects_coordinate_overflow(): + cloud = pointcloud_from_xyz(np.array([[3e38, 0, 0]]), header=Header(frame_id="child")) + transform = TransformStamped( + header=Header(frame_id="parent"), + child_frame_id="child", + transform=Transform(translation=Vector3(x=3e38), rotation=Quaternion(w=1)), + ) + with pytest.raises(ValueError, match="exceed field range"): + transform_cloud(cloud, transform) + assert np.isfinite(pointcloud_xyz(cloud)).all() + + +def test_voxel_downsample_retains_existing_small_cloud_fast_path(): + message = pointcloud_from_xyz(np.zeros((19, 3)), header=Header(frame_id="lidar")) + assert voxel_downsample_cloud(message, 0.5) is message + assert voxel_downsample_cloud(message, 0.0) is message + with pytest.raises(ValueError, match="finite"): + voxel_downsample_cloud(message, float("nan")) + + +def test_voxel_downsample_native_centroids_colors_and_exact_header(): + pytest.importorskip("open3d", reason="native tensor downsampling requires Open3D") + points = np.repeat([[0.1, 0.1, 0.1], [0.3, 0.3, 0.3]], 20, axis=0) + colors = np.repeat([[0, 0, 0], [255, 255, 255]], 20, axis=0).astype(np.uint8) + header = Header(frame_id="optical", stamp=Time(sec=5, nanosec=123456789)) + source = pointcloud_from_xyz_rgb(points, colors, header=header) + result = voxel_downsample_cloud(source, 1.0) + assert result.header == header + assert result.width == 1 + np.testing.assert_allclose(pointcloud_xyz(result), [[0.2, 0.2, 0.2]], atol=1e-6) + np.testing.assert_array_equal(pointcloud_rgb(result), [[127, 127, 127]]) + assert source.width == 40 diff --git a/dimos/msgs/test_pointcloud_colors.py b/dimos/msgs/test_pointcloud_colors.py new file mode 100644 index 00000000000..fe2ea97cdd3 --- /dev/null +++ b/dimos/msgs/test_pointcloud_colors.py @@ -0,0 +1,65 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +# Copyright 2026 Dimensional Inc. +# SPDX-License-Identifier: Apache-2.0 + +from dimos_generated.sensor_msgs.msg import PointCloud2, PointField +import numpy as np +import pytest + +from dimos.msgs.pointcloud import pointcloud_rgb + + +@pytest.mark.parametrize("bigendian", [False, True]) +@pytest.mark.parametrize("name", ["rgb", "rgba"]) +@pytest.mark.parametrize("datatype", [PointField.FLOAT32, PointField.UINT32]) +def test_packed_colors_preserve_bits_and_row_padding(bigendian, name, datatype): + order = ">" if bigendian else "<" + data = bytearray(48) + values = np.ndarray((2, 2), dtype=order + "u4", buffer=data, offset=4, strides=(24, 8)) + values[:] = [[0xFF123456, 0x00ABCDEF], [0x00FF0000, 0xFF00FF00]] + cloud = PointCloud2( + height=2, + width=2, + point_step=8, + row_step=24, + is_bigendian=bigendian, + data=bytes(data), + fields=[PointField(name=name, offset=4, count=1, datatype=datatype)], + ) + decoded = PointCloud2.decode(cloud.encode()) + np.testing.assert_array_equal( + pointcloud_rgb(decoded), [[0x12, 0x34, 0x56], [0xAB, 0xCD, 0xEF], [255, 0, 0], [0, 255, 0]] + ) + + +def test_missing_colors(): + assert pointcloud_rgb(PointCloud2()) is None + + +@pytest.mark.parametrize( + "datatype,count", [(PointField.FLOAT64, 1), (PointField.INT32, 1), (PointField.UINT32, 2)] +) +def test_invalid_color_layout(datatype, count): + cloud = PointCloud2( + height=1, + width=1, + point_step=8, + row_step=8, + data=bytes(8), + fields=[PointField(name="rgb", offset=0, count=count, datatype=datatype)], + ) + with pytest.raises(ValueError, match="packed point cloud color"): + pointcloud_rgb(cloud) diff --git a/dimos/msgs/test_time.py b/dimos/msgs/test_time.py new file mode 100644 index 00000000000..209e3b5653b --- /dev/null +++ b/dimos/msgs/test_time.py @@ -0,0 +1,81 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import pytest + +from dimos.msgs.time import ( + duration_from_nanoseconds, + duration_from_seconds, + header_now, + time_from_nanoseconds, + time_from_seconds, + to_nanoseconds, + to_seconds, +) + + +@pytest.mark.parametrize("factory", [time_from_nanoseconds, duration_from_nanoseconds]) +@pytest.mark.parametrize("value", [0, -1, -1_250_000_000, 1_700_000_000_123_456_789]) +def test_integer_time_round_trip_preserves_every_nanosecond(factory, value): + stamp = factory(value) + + assert 0 <= stamp.nanosec < 1_000_000_000 + assert to_nanoseconds(stamp) == value + assert to_nanoseconds(type(stamp).decode(stamp.encode())) == value + + +@pytest.mark.parametrize("factory", [time_from_seconds, duration_from_seconds]) +@pytest.mark.parametrize("value,expected", [(-1.25, (-2, 750_000_000)), (0.9999999996, (1, 0))]) +def test_float_conversion_normalizes_negative_values_and_fractional_carry(factory, value, expected): + stamp = factory(value) + + assert (stamp.sec, stamp.nanosec) == expected + assert to_seconds(stamp) == pytest.approx(value, abs=1e-9) + + +@pytest.mark.parametrize("value", [float("nan"), float("inf"), -float("inf")]) +def test_nonfinite_seconds_are_rejected(value): + with pytest.raises(ValueError, match="finite"): + time_from_seconds(value) + + +def test_invalid_ros_fraction_is_rejected(): + stamp = time_from_nanoseconds(0) + stamp.nanosec = 1_000_000_000 + + with pytest.raises(ValueError, match="nanosec"): + to_nanoseconds(stamp) + + +def test_now_reads_integer_clock_and_preserves_frame(monkeypatch): + monkeypatch.setattr("dimos.msgs.time.time.time_ns", lambda: 1_700_000_000_123_456_789) + + header = header_now("camera") + + assert to_nanoseconds(header.stamp) == 1_700_000_000_123_456_789 + assert header.frame_id == "camera" + + +@pytest.mark.parametrize("factory", [time_from_nanoseconds, duration_from_nanoseconds]) +@pytest.mark.parametrize("seconds", [-(2**31), 2**31 - 1]) +def test_ros_seconds_range_boundaries(factory, seconds): + value = seconds * 1_000_000_000 + 999_999_999 + assert to_nanoseconds(factory(value)) == value + + +@pytest.mark.parametrize("factory", [time_from_nanoseconds, duration_from_nanoseconds]) +@pytest.mark.parametrize("value", [-(2**31) * 1_000_000_000 - 1, 2**31 * 1_000_000_000]) +def test_ros_seconds_overflow_is_explicit(factory, value): + with pytest.raises(ValueError, match="signed 32-bit"): + factory(value) diff --git a/dimos/msgs/test_trajectory.py b/dimos/msgs/test_trajectory.py new file mode 100644 index 00000000000..922fa3a2e4b --- /dev/null +++ b/dimos/msgs/test_trajectory.py @@ -0,0 +1,75 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from dimos_generated.builtin_interfaces.msg import Duration, Time +from dimos_generated.dimos_msgs.msg import TrajectoryStatus +from dimos_generated.std_msgs.msg import Header +from dimos_generated.trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint +import pytest + +from dimos.msgs.time import duration_from_seconds, to_nanoseconds +from dimos.msgs.trajectory import TrajectoryState, sample_trajectory, trajectory_duration + + +@pytest.mark.parametrize( + "seconds,expected", [(-1.0, [1.0, 2.0]), (0.5, [2.0, 1.0]), (3.0, [3.0, 0.0])] +) +@pytest.mark.parametrize("with_velocities", [False, True]) +def test_cdr_trajectory_sampling_and_copy_isolation(seconds, expected, with_velocities): + trajectory = JointTrajectory( + header=Header(frame_id="robot", stamp=Time(sec=1700000000, nanosec=123456789)), + joint_names=["a", "b"], + points=[ + JointTrajectoryPoint( + positions=[1.0, 2.0], velocities=[0.0, 1.0] if with_velocities else [] + ), + JointTrajectoryPoint( + positions=[3.0, 0.0], + velocities=[2.0, 3.0] if with_velocities else [], + time_from_start=duration_from_seconds(1.0), + ), + ], + ) + decoded = JointTrajectory.decode(trajectory.encode()) + positions, velocities = sample_trajectory(decoded, seconds) + assert positions == expected + alpha = max(0.0, min(seconds, 1.0)) + assert velocities == ([2 * alpha, 1 + 2 * alpha] if with_velocities else [0.0, 0.0]) + positions[0] = 999.0 + assert list(decoded.points[0].positions) == [1.0, 2.0] + assert trajectory_duration(decoded) == 1.0 + assert to_nanoseconds(decoded.header.stamp) == 1700000000123456789 + + +def test_empty_trajectory_sampling_and_invalid_sample_time(): + assert trajectory_duration(JointTrajectory()) == 0.0 + assert sample_trajectory(JointTrajectory(), 0.0) == ([], []) + with pytest.raises(ValueError, match="finite"): + sample_trajectory(JointTrajectory(), float("nan")) + + +def test_generated_status_roundtrips_duration_fields_and_state_constants(): + status = TrajectoryStatus( + header=Header(stamp=Time(sec=0, nanosec=7)), + state=TrajectoryState.EXECUTING, + progress=0.25, + time_elapsed=Duration(sec=1, nanosec=123456789), + time_remaining=Duration(sec=3, nanosec=987654321), + ) + decoded = TrajectoryStatus.decode(status.encode()) + assert decoded == status + assert decoded.state == TrajectoryStatus.EXECUTING + assert TrajectoryState(decoded.state).name == "EXECUTING" + assert to_nanoseconds(decoded.time_elapsed) == 1123456789 + assert to_nanoseconds(decoded.time_remaining) == 3987654321 diff --git a/dimos/msgs/tf2_msgs/TFMessage.py b/dimos/msgs/tf2_msgs/TFMessage.py deleted file mode 100644 index 5e44b7762d9..00000000000 --- a/dimos/msgs/tf2_msgs/TFMessage.py +++ /dev/null @@ -1,194 +0,0 @@ -# Copyright 2025-2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from __future__ import annotations - -from typing import TYPE_CHECKING, BinaryIO - -from dimos_lcm.tf2_msgs import TFMessage as LCMTFMessage - -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Transform import Transform -from dimos.msgs.geometry_msgs.Vector3 import Vector3 - -if TYPE_CHECKING: - from collections.abc import Iterable, Iterator - - from dimos.visualization.rerun.bridge import RerunMulti - -DEFAULT_LINKS_ROOT = "tf_links" -DEFAULT_TF_ROOT = "world/tf" -DEFAULT_AXIS_LENGTH = 0.5 -DEPTH_SCALE = 0.8 -AXIS_WIDTH_UI_POINTS = 2.0 -AXIS_COLORS = [[255, 0, 0], [0, 255, 0], [0, 0, 255]] - - -def _triad(length: float): # type: ignore[no-untyped-def] - """XYZ arrows, red green blue.""" - import rerun as rr - - return rr.Arrows3D( - origins=[[0.0, 0.0, 0.0]] * 3, - vectors=[[length, 0.0, 0.0], [0.0, length, 0.0], [0.0, 0.0, length]], - colors=AXIS_COLORS, - radii=rr.components.Radius.ui_points(AXIS_WIDTH_UI_POINTS), - ) - - -class TfFrameTree: - """Store each frame under its parents. This lets us view the tree in the left panel.""" - - def __init__( - self, axis_length: float = DEFAULT_AXIS_LENGTH, root: str = DEFAULT_TF_ROOT - ) -> None: - self.axis_length = axis_length - self.root = root - self._parents: dict[str, str] = {} - self._placed: dict[str, tuple[str, int]] = {} # frame -> (path, depth) - - def placements(self) -> dict[str, str]: - return {frame: path for frame, (path, _depth) in self._placed.items()} - - def update(self, transforms: Iterable[Transform]) -> None: - learned = False - for transform in transforms: - if self._parents.get(transform.child_frame_id) != transform.frame_id: - self._parents[transform.child_frame_id] = transform.frame_id - learned = True - if learned: - self._redraw() - - def _place(self, frame: str, walked: frozenset[str]) -> tuple[str, int]: - import rerun as rr - - part = rr.escape_entity_path_part(frame) - parent = self._parents.get(frame) - if parent is None or parent in walked: - return f"{self.root}/{part}", 0 - parent_path, parent_depth = self._place(parent, walked | {frame}) - return f"{parent_path}/{part}", parent_depth + 1 - - def _redraw(self) -> None: - import rerun as rr - - frames = {*self._parents, *self._parents.values()} - placed = {frame: self._place(frame, frozenset()) for frame in frames} - - for frame, (old_path, _depth) in self._placed.items(): - new = placed.get(frame) - if new is None or new[0] != old_path: - rr.log(old_path, rr.Arrows3D(origins=[], vectors=[]), static=True) - - for frame, (path, depth) in placed.items(): - if self._placed.get(frame) != (path, depth): - rr.log( - path, - rr.CoordinateFrame(f"tf#/{frame}"), - _triad(self.axis_length * DEPTH_SCALE**depth), - static=True, - ) - - self._placed = placed - - -class TFMessage: - """TFMessage that accepts Transform objects and encodes to LCM format.""" - - transforms: list[Transform] - msg_name = "tf2_msgs.TFMessage" - - def __init__(self, *transforms: Transform) -> None: - self.transforms = list(transforms) - - def add_transform(self, transform: Transform, child_frame_id: str = "base_link") -> None: - """Add a transform to the message.""" - self.transforms.append(transform) - self.transforms_length = len(self.transforms) - - def lcm_encode(self) -> bytes: - """Encode as LCM TFMessage.""" - res = [t.lcm_transform() for t in self.transforms] - - lcm_msg = LCMTFMessage( - transforms_length=len(self.transforms), - transforms=res, - ) - - return lcm_msg.lcm_encode() # type: ignore[no-any-return] - - @classmethod - def lcm_decode(cls, data: bytes | BinaryIO) -> TFMessage: - """Decode from LCM TFMessage bytes.""" - lcm_msg = LCMTFMessage.lcm_decode(data) - - transforms = [] - for lcm_transform_stamped in lcm_msg.transforms: - ts = lcm_transform_stamped.header.stamp.sec + ( - lcm_transform_stamped.header.stamp.nsec / 1_000_000_000 - ) - - lcm_trans = lcm_transform_stamped.transform.translation - lcm_rot = lcm_transform_stamped.transform.rotation - - transform = Transform( - translation=Vector3(lcm_trans.x, lcm_trans.y, lcm_trans.z), - rotation=Quaternion(lcm_rot.x, lcm_rot.y, lcm_rot.z, lcm_rot.w), - frame_id=lcm_transform_stamped.header.frame_id, - child_frame_id=lcm_transform_stamped.child_frame_id, - ts=ts, - ) - transforms.append(transform) - - return cls(*transforms) - - def __len__(self) -> int: - """Return number of transforms.""" - return len(self.transforms) - - def __getitem__(self, index: int) -> Transform: - """Get transform by index.""" - return self.transforms[index] - - def __iter__(self) -> Iterator: # type: ignore[type-arg] - """Iterate over transforms.""" - return iter(self.transforms) - - def __repr__(self) -> str: - return f"TFMessage({len(self.transforms)} transforms)" - - def __str__(self) -> str: - lines = [f"TFMessage with {len(self.transforms)} transforms:"] - for i, transform in enumerate(self.transforms): - lines.append( - f" [{i}] {transform.frame_id} → {transform.child_frame_id} @ {transform.ts:.3f}" - ) - return "\n".join(lines) - - def to_rerun(self, tree: TfFrameTree | None = None) -> RerunMulti: - """Convert to (entity_path, archetype) pairs to log to rerun. - - Pass a TfFrameTree to also nest a triad per frame under its ancestors, - matching the tf tree's shape in the entity panel. - """ - import rerun as rr - - results: RerunMulti = [] - for transform in self.transforms: - path = f"{DEFAULT_LINKS_ROOT}/{rr.escape_entity_path_part(transform.child_frame_id)}" - results.append((path, transform.to_rerun())) - - if tree is not None: - tree.update(self.transforms) - return results diff --git a/dimos/msgs/tf2_msgs/test_TFMessage.py b/dimos/msgs/tf2_msgs/test_TFMessage.py index c379481f1dd..327ade99ba7 100644 --- a/dimos/msgs/tf2_msgs/test_TFMessage.py +++ b/dimos/msgs/tf2_msgs/test_TFMessage.py @@ -12,131 +12,102 @@ # See the License for the specific language governing permissions and # limitations under the License. -from dimos_lcm.tf2_msgs import TFMessage as LCMTFMessage +from dimos_generated.geometry_msgs.msg import Quaternion, Transform, TransformStamped, Vector3 +from dimos_generated.std_msgs.msg import Header +from dimos_generated.tf2_msgs.msg import TFMessage +from rosbags.typesys import Stores, get_typestore -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Transform import Transform -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.msgs.tf2_msgs.TFMessage import TFMessage +from dimos.msgs.time import time_from_seconds def test_tfmessage_initialization() -> None: - """Test TFMessage initialization with Transform objects.""" - # Create some transforms - tf1 = Transform( - translation=Vector3(1, 2, 3), rotation=Quaternion(0, 0, 0, 1), frame_id="world", ts=100.0 + first = TransformStamped( + header=Header(stamp=time_from_seconds(100), frame_id="world"), + transform=Transform(translation=Vector3(x=1, y=2, z=3), rotation=Quaternion(w=1)), ) - tf2 = Transform( - translation=Vector3(4, 5, 6), - rotation=Quaternion(0, 0, 0.707, 0.707), - frame_id="map", - ts=101.0, + second = TransformStamped( + header=Header(stamp=time_from_seconds(101), frame_id="map"), + transform=Transform( + translation=Vector3(x=4, y=5, z=6), rotation=Quaternion(z=0.707, w=0.707) + ), ) - - # Create TFMessage with transforms - msg = TFMessage(tf1, tf2) - - assert len(msg) == 2 - assert msg[0] == tf1 - assert msg[1] == tf2 - - # Test iteration - transforms = list(msg) - assert transforms == [tf1, tf2] + message = TFMessage(transforms=[first, second]) + assert len(message.transforms) == 2 + assert [item.encode() for item in message.transforms] == [first.encode(), second.encode()] def test_tfmessage_empty() -> None: - """Test empty TFMessage.""" - msg = TFMessage() - assert len(msg) == 0 - assert list(msg) == [] + message = TFMessage() + assert len(message.transforms) == 0 + assert list(message.transforms) == [] def test_tfmessage_add_transform() -> None: - """Test adding transforms to TFMessage.""" - msg = TFMessage() - - tf = Transform(translation=Vector3(1, 2, 3), frame_id="base", ts=200.0) - - msg.add_transform(tf) - assert len(msg) == 1 - assert msg[0] == tf + message = TFMessage() + transform = TransformStamped( + header=Header(stamp=time_from_seconds(200), frame_id="base"), + transform=Transform(translation=Vector3(x=1, y=2, z=3)), + ) + message.transforms.append(transform) + assert len(message.transforms) == 1 + assert message.transforms[0].encode() == transform.encode() def test_tfmessage_tree() -> None: - """Test adding transforms to TFMessage.""" - msg = TFMessage() - - msg.add_transform( - Transform( - translation=Vector3(1, 2, 0.5), frame_id="world", child_frame_id="robot", ts=200.0 + message = TFMessage() + edges = [ + ("world", "robot", (1, 2, 0.5)), + ("robot", "camera", (0.1, 0, 1)), + ("robot", "lidar", (0.2, 0, 0.2)), + ("lidar", "lidar_scanner", (0.05, 0, 0)), + ] + for parent, child, (x, y, z) in edges: + message.transforms.append( + TransformStamped( + header=Header(stamp=time_from_seconds(200), frame_id=parent), + child_frame_id=child, + transform=Transform(translation=Vector3(x=x, y=y, z=z)), + ) ) - ) - msg.add_transform( - Transform( - translation=Vector3(0.1, 0, 1.0), frame_id="robot", child_frame_id="camera", ts=200.0 + assert len(message.transforms) == 4 + assert [ + ( + t.header.frame_id, + t.child_frame_id, + (t.transform.translation.x, t.transform.translation.y, t.transform.translation.z), ) - ) - msg.add_transform( - Transform( - translation=Vector3(0.2, 0, 0.2), frame_id="robot", child_frame_id="lidar", ts=200.0 - ) - ) - msg.add_transform( - Transform( - translation=Vector3(0.05, 0, 0.0), - frame_id="lidar", - child_frame_id="lidar_scanner", - ts=200.0, - ) - ) - - assert len(msg) == 4 - print(msg) + for t in TFMessage.decode(message.encode()).transforms + ] == edges -def test_tfmessage_lcm_encode_decode() -> None: - """Test encoding TFMessage to LCM bytes.""" - # Create transforms - tf1 = Transform( - translation=Vector3(1.0, 2.0, 3.0), - rotation=Quaternion(0.0, 0.0, 0.0, 1.0), +def test_tfmessage_cdr_encode_decode() -> None: + first = TransformStamped( + header=Header(stamp=time_from_seconds(123.456), frame_id="world"), child_frame_id="robot", - frame_id="world", - ts=123.456, + transform=Transform(translation=Vector3(x=1, y=2, z=3), rotation=Quaternion(w=1)), ) - tf2 = Transform( - translation=Vector3(4.0, 5.0, 6.0), - rotation=Quaternion(0.0, 0.0, 0.707, 0.707), - frame_id="robot", + second = TransformStamped( + header=Header(stamp=time_from_seconds(124.567), frame_id="robot"), child_frame_id="target", - ts=124.567, + transform=Transform( + translation=Vector3(x=4, y=5, z=6), rotation=Quaternion(z=0.707, w=0.707) + ), + ) + message = TFMessage(transforms=[first, second]) + decoded = get_typestore(Stores.ROS2_JAZZY).deserialize_cdr(message.encode(), TFMessage.msg_name) + assert len(decoded.transforms) == 2 + first_result, second_result = decoded.transforms + assert first_result.header.frame_id == "world" + assert first_result.child_frame_id == "robot" + assert (first_result.header.stamp.sec, first_result.header.stamp.nanosec) == (123, 456000000) + assert ( + first_result.transform.translation.x, + first_result.transform.translation.y, + first_result.transform.translation.z, + ) == (1, 2, 3) + assert second_result.header.frame_id == "robot" + assert second_result.child_frame_id == "target" + assert (second_result.transform.rotation.z, second_result.transform.rotation.w) == ( + 0.707, + 0.707, ) - - # Create TFMessage - msg = TFMessage(tf1, tf2) - - # Encode with custom child_frame_ids - encoded = msg.lcm_encode() - - # Decode using LCM to verify - lcm_msg = LCMTFMessage.lcm_decode(encoded) - - assert lcm_msg.transforms_length == 2 - - # Check first transform - ts1 = lcm_msg.transforms[0] - assert ts1.header.frame_id == "world" - assert ts1.child_frame_id == "robot" - assert ts1.header.stamp.sec == 123 - assert ts1.header.stamp.nsec == 456000000 - assert ts1.transform.translation.x == 1.0 - assert ts1.transform.translation.y == 2.0 - assert ts1.transform.translation.z == 3.0 - - # Check second transform - ts2 = lcm_msg.transforms[1] - assert ts2.header.frame_id == "robot" - assert ts2.child_frame_id == "target" - assert ts2.transform.rotation.z == 0.707 - assert ts2.transform.rotation.w == 0.707 diff --git a/dimos/msgs/tf2_msgs/test_TFMessage_lcmpub.py b/dimos/msgs/tf2_msgs/test_TFMessage_lcmpub.py index 6df5e499fc5..c20cc55b291 100644 --- a/dimos/msgs/tf2_msgs/test_TFMessage_lcmpub.py +++ b/dimos/msgs/tf2_msgs/test_TFMessage_lcmpub.py @@ -15,12 +15,12 @@ from collections.abc import Iterator import time +from dimos_generated.geometry_msgs.msg import Quaternion, Transform, TransformStamped, Vector3 +from dimos_generated.std_msgs.msg import Header +from dimos_generated.tf2_msgs.msg import TFMessage import pytest -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Transform import Transform -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.msgs.tf2_msgs.TFMessage import TFMessage +from dimos.msgs.time import time_from_seconds from dimos.protocol.pubsub.impl.lcmpubsub import LCM, Topic from dimos.utils.testing.collector import CallbackCollector @@ -38,7 +38,7 @@ def lcm(lcm_url: str) -> Iterator[LCM]: # Publishes a series of transforms representing a robot kinematic chain # to actual LCM messages, rerun running in parallel should render this def test_publish_transforms(lcm: LCM) -> None: - topic = Topic(topic="/tf", lcm_type=TFMessage) + topic = Topic(topic="/tf", msg_type=TFMessage) collector = CallbackCollector(2) lcm.subscribe(topic, collector) @@ -46,42 +46,45 @@ def test_publish_transforms(lcm: LCM) -> None: current_time = time.time() # 1. World to base_link transform (robot at position) - world_to_base = Transform( - translation=Vector3(4.0, 3.0, 0.0), - rotation=Quaternion(0.0, 0.0, 0.382683, 0.923880), # 45 degrees around Z - frame_id="world", + world_to_base = TransformStamped( + transform=Transform( + translation=Vector3(x=4.0, y=3.0, z=0.0), + rotation=Quaternion(z=0.382683, w=0.923880), # 45 degrees around Z + ), + header=Header(frame_id="world", stamp=time_from_seconds(current_time)), child_frame_id="base_link", - ts=current_time, ) # 2. Base to arm transform (arm lifted up) - base_to_arm = Transform( - translation=Vector3(0.2, 0.0, 1.5), - rotation=Quaternion(0.0, 0.258819, 0.0, 0.965926), # 30 degrees around Y - frame_id="base_link", + base_to_arm = TransformStamped( + transform=Transform( + translation=Vector3(x=0.2, y=0.0, z=1.5), + rotation=Quaternion(y=0.258819, w=0.965926), # 30 degrees around Y + ), + header=Header(frame_id="base_link", stamp=time_from_seconds(current_time)), child_frame_id="arm_link", - ts=current_time, ) # 3. Arm to gripper transform (gripper extended) - arm_to_gripper = Transform( - translation=Vector3(0.5, 0.0, 0.0), - rotation=Quaternion(0.0, 0.0, 0.0, 1.0), # No rotation - frame_id="arm_link", + arm_to_gripper = TransformStamped( + transform=Transform( + translation=Vector3(x=0.5, y=0.0, z=0.0), + rotation=Quaternion(w=1.0), # No rotation + ), + header=Header(frame_id="arm_link", stamp=time_from_seconds(current_time)), child_frame_id="gripper_link", - ts=current_time, ) - lcm.publish(topic, TFMessage(world_to_base, base_to_arm)) - lcm.publish(topic, TFMessage(world_to_base, arm_to_gripper)) + lcm.publish(topic, TFMessage(transforms=[world_to_base, base_to_arm])) + lcm.publish(topic, TFMessage(transforms=[world_to_base, arm_to_gripper])) collector.wait() assert len(collector.results) == 2 first, _ = collector.results[0] assert isinstance(first, TFMessage) - assert [t.child_frame_id for t in first] == ["base_link", "arm_link"] + assert [t.child_frame_id for t in first.transforms] == ["base_link", "arm_link"] second, _ = collector.results[1] assert isinstance(second, TFMessage) - assert [t.child_frame_id for t in second] == ["base_link", "gripper_link"] + assert [t.child_frame_id for t in second.transforms] == ["base_link", "gripper_link"] diff --git a/dimos/msgs/time.py b/dimos/msgs/time.py new file mode 100644 index 00000000000..020fcd2f2ab --- /dev/null +++ b/dimos/msgs/time.py @@ -0,0 +1,70 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Explicit conversions for generated ROS time, duration, and header fields.""" + +import math +import time + +from dimos_generated.builtin_interfaces.msg import Duration, Time +from dimos_generated.std_msgs.msg import Header + +NANOSECONDS_PER_SECOND = 1_000_000_000 + + +def to_nanoseconds(stamp: Time | Duration) -> int: + """Convert a normalized ROS time or duration without losing integer precision.""" + if not 0 <= stamp.nanosec < NANOSECONDS_PER_SECOND: + raise ValueError("nanosec must be in [0, 1000000000)") + return stamp.sec * NANOSECONDS_PER_SECOND + stamp.nanosec + + +def to_seconds(stamp: Time | Duration) -> float: + """Convert to floating seconds for APIs that require them.""" + return to_nanoseconds(stamp) / NANOSECONDS_PER_SECOND + + +def time_from_nanoseconds(value: int) -> Time: + sec, nanosec = divmod(value, NANOSECONDS_PER_SECOND) + if not -(2**31) <= sec < 2**31: + raise ValueError("seconds must fit the ROS signed 32-bit field") + return Time(sec=sec, nanosec=nanosec) + + +def duration_from_nanoseconds(value: int) -> Duration: + sec, nanosec = divmod(value, NANOSECONDS_PER_SECOND) + if not -(2**31) <= sec < 2**31: + raise ValueError("seconds must fit the ROS signed 32-bit field") + return Duration(sec=sec, nanosec=nanosec) + + +def _seconds_to_nanoseconds(value: float) -> int: + if not math.isfinite(value): + raise ValueError("seconds must be finite") + sec = math.floor(value) + return sec * NANOSECONDS_PER_SECOND + round((value - sec) * NANOSECONDS_PER_SECOND) + + +def time_from_seconds(value: float) -> Time: + """Round to the nearest nanosecond, normalizing negative and carried fractions.""" + return time_from_nanoseconds(_seconds_to_nanoseconds(value)) + + +def duration_from_seconds(value: float) -> Duration: + return duration_from_nanoseconds(_seconds_to_nanoseconds(value)) + + +def header_now(frame_id: str = "") -> Header: + """Construct a wall-clock header; generated defaults themselves remain zero.""" + return Header(stamp=time_from_nanoseconds(time.time_ns()), frame_id=frame_id) diff --git a/dimos/msgs/trajectory.py b/dimos/msgs/trajectory.py new file mode 100644 index 00000000000..02bc27861aa --- /dev/null +++ b/dimos/msgs/trajectory.py @@ -0,0 +1,79 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Execution state and interpolation for generated ROS2 trajectories.""" + +from enum import IntEnum +import math + +from dimos_generated.dimos_msgs.msg import TrajectoryStatus +from dimos_generated.trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint + +from dimos.msgs.time import to_seconds + + +class TrajectoryState(IntEnum): + """Internal state-machine names matching generated wire constants.""" + + IDLE = TrajectoryStatus.IDLE + EXECUTING = TrajectoryStatus.EXECUTING + COMPLETED = TrajectoryStatus.COMPLETED + ABORTED = TrajectoryStatus.ABORTED + FAULT = TrajectoryStatus.FAULT + + +def trajectory_duration(trajectory: JointTrajectory) -> float: + """Return the last waypoint time in seconds; validated paths have increasing times.""" + return to_seconds(trajectory.points[-1].time_from_start) if trajectory.points else 0.0 + + +def _sample_point(point: JointTrajectoryPoint) -> tuple[list[float], list[float]]: + return list(point.positions), list(point.velocities) or [0.0] * len(point.positions) + + +def sample_trajectory( + trajectory: JointTrajectory, seconds: float +) -> tuple[list[float], list[float]]: + """Linearly sample a validated path, clamping outside its waypoint time range. + + ROS permits absent velocities; this position controller's feed-forward then + defaults to zero. Inputs remain unchanged and returned arrays are independent. + """ + if not math.isfinite(seconds): + raise ValueError("sample time must be finite") + if not trajectory.points: + return [], [] + first, last = trajectory.points[0], trajectory.points[-1] + if seconds <= to_seconds(first.time_from_start): + return _sample_point(first) + if seconds >= to_seconds(last.time_from_start): + return _sample_point(last) + for index in range(len(trajectory.points) - 1): + before, after = trajectory.points[index], trajectory.points[index + 1] + start, end = to_seconds(before.time_from_start), to_seconds(after.time_from_start) + if start <= seconds <= end: + alpha = (seconds - start) / (end - start) + positions0, velocities0 = _sample_point(before) + positions1, velocities1 = _sample_point(after) + return ( + [ + left + alpha * (right - left) + for left, right in zip(positions0, positions1, strict=True) + ], + [ + left + alpha * (right - left) + for left, right in zip(velocities0, velocities1, strict=True) + ], + ) + raise ValueError("trajectory times must be increasing") diff --git a/dimos/msgs/trajectory_msgs/JointTrajectory.py b/dimos/msgs/trajectory_msgs/JointTrajectory.py deleted file mode 100644 index ae2ad55fd1b..00000000000 --- a/dimos/msgs/trajectory_msgs/JointTrajectory.py +++ /dev/null @@ -1,211 +0,0 @@ -# Copyright 2025-2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -""" -JointTrajectory message type. - -A sequence of joint trajectory points representing a full trajectory. -Similar to ROS trajectory_msgs/JointTrajectory. -""" - -from io import BytesIO -import struct -import time - -from dimos.msgs.trajectory_msgs.TrajectoryPoint import TrajectoryPoint - - -class JointTrajectory: - """ - A joint-space trajectory consisting of timestamped waypoints. - - Attributes: - timestamp: When trajectory was created (seconds since epoch) - joint_names: Names of joints (optional) - points: Sequence of TrajectoryPoints - duration: Total trajectory duration (seconds) - """ - - msg_name = "trajectory_msgs.JointTrajectory" - - __slots__ = ["duration", "joint_names", "num_joints", "num_points", "points", "timestamp"] - - def __init__( - self, - points: list[TrajectoryPoint] | None = None, - joint_names: list[str] | None = None, - timestamp: float | None = None, - ) -> None: - """ - Initialize JointTrajectory. - - Args: - points: List of TrajectoryPoints - joint_names: Names of joints (optional) - timestamp: Creation timestamp (defaults to now) - """ - self.timestamp = timestamp if timestamp is not None else time.time() - self.points = list(points) if points else [] - self.num_points = len(self.points) - self.joint_names = list(joint_names) if joint_names else [] - self.num_joints = ( - len(self.joint_names) - if self.joint_names - else (self.points[0].num_joints if self.points else 0) - ) - - # Compute duration from last point - if self.points: - self.duration = max(p.time_from_start for p in self.points) - else: - self.duration = 0.0 - - def sample(self, t: float) -> tuple[list[float], list[float]]: - """ - Sample the trajectory at time t using linear interpolation. - - Args: - t: Time from trajectory start (seconds) - - Returns: - Tuple of (positions, velocities) at time t - """ - if not self.points: - return [], [] - - # Clamp t to valid range - t = max(0.0, min(t, self.duration)) - - # Find bracketing points - if t <= self.points[0].time_from_start: - return list(self.points[0].positions), list(self.points[0].velocities) - - if t >= self.points[-1].time_from_start: - return list(self.points[-1].positions), list(self.points[-1].velocities) - - # Find interval - for i in range(len(self.points) - 1): - t0 = self.points[i].time_from_start - t1 = self.points[i + 1].time_from_start - - if t0 <= t <= t1: - # Linear interpolation - alpha = (t - t0) / (t1 - t0) if t1 > t0 else 0.0 - p0 = self.points[i] - p1 = self.points[i + 1] - - positions = [ - p0.positions[j] + alpha * (p1.positions[j] - p0.positions[j]) - for j in range(len(p0.positions)) - ] - velocities = [ - p0.velocities[j] + alpha * (p1.velocities[j] - p0.velocities[j]) - for j in range(len(p0.velocities)) - ] - return positions, velocities - - # Fallback - return list(self.points[-1].positions), list(self.points[-1].velocities) - - def lcm_encode(self) -> bytes: - """Encode for LCM transport.""" - return self.encode() - - def encode(self) -> bytes: - buf = BytesIO() - buf.write(JointTrajectory._get_packed_fingerprint()) - self._encode_one(buf) - return buf.getvalue() - - def _encode_one(self, buf: BytesIO) -> None: - # timestamp (double) - buf.write(struct.pack(">d", self.timestamp)) - # duration (double) - buf.write(struct.pack(">d", self.duration)) - # num_joint_names (int32) - actual count of joint names - buf.write(struct.pack(">i", len(self.joint_names))) - # joint_names (string[num_joint_names]) - for name in self.joint_names: - name_bytes = name.encode("utf-8") - buf.write(struct.pack(">i", len(name_bytes))) - buf.write(name_bytes) - # num_points (int32) - buf.write(struct.pack(">i", self.num_points)) - # points (TrajectoryPoint[num_points]) - for point in self.points: - point._encode_one(buf) - - @classmethod - def lcm_decode(cls, data: bytes) -> "JointTrajectory": - """Decode from LCM transport.""" - return cls.decode(data) - - @classmethod - def decode(cls, data: bytes) -> "JointTrajectory": - buf = BytesIO(data) if not hasattr(data, "read") else data - if buf.read(8) != cls._get_packed_fingerprint(): - raise ValueError("Decode error: fingerprint mismatch") - return cls._decode_one(buf) # type: ignore[arg-type] - - @classmethod - def _decode_one(cls, buf: BytesIO) -> "JointTrajectory": - self = cls.__new__(cls) - self.timestamp = struct.unpack(">d", buf.read(8))[0] - self.duration = struct.unpack(">d", buf.read(8))[0] - - # Read joint names - num_joint_names = struct.unpack(">i", buf.read(4))[0] - self.joint_names = [] - for _ in range(num_joint_names): - name_len = struct.unpack(">i", buf.read(4))[0] - self.joint_names.append(buf.read(name_len).decode("utf-8")) - - # Read points - self.num_points = struct.unpack(">i", buf.read(4))[0] - self.points = [TrajectoryPoint._decode_one(buf) for _ in range(self.num_points)] - - # Set num_joints from joint_names or points - self.num_joints = ( - len(self.joint_names) - if self.joint_names - else (self.points[0].num_joints if self.points else 0) - ) - - return self - - _packed_fingerprint = None - - @classmethod - def _get_hash_recursive(cls, parents): # type: ignore[no-untyped-def] - if cls in parents: - return 0 - return 0x2B3C4D5E6F708192 & 0xFFFFFFFFFFFFFFFF - - @classmethod - def _get_packed_fingerprint(cls) -> bytes: - if cls._packed_fingerprint is None: - cls._packed_fingerprint = struct.pack(">Q", cls._get_hash_recursive([])) # type: ignore[no-untyped-call] - return cls._packed_fingerprint - - def __str__(self) -> str: - return f"JointTrajectory({self.num_points} points, duration={self.duration:.3f}s)" - - def __repr__(self) -> str: - return ( - f"JointTrajectory(points={self.points}, joint_names={self.joint_names}, " - f"timestamp={self.timestamp})" - ) - - def __len__(self) -> int: - return self.num_points diff --git a/dimos/msgs/trajectory_msgs/TrajectoryPoint.py b/dimos/msgs/trajectory_msgs/TrajectoryPoint.py deleted file mode 100644 index b2b9ab8406c..00000000000 --- a/dimos/msgs/trajectory_msgs/TrajectoryPoint.py +++ /dev/null @@ -1,136 +0,0 @@ -# Copyright 2025-2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -""" -TrajectoryPoint message type. - -A single point in a joint trajectory with positions, velocities, and time. -Similar to ROS trajectory_msgs/JointTrajectoryPoint. -""" - -from io import BytesIO -import struct - - -class TrajectoryPoint: - """ - A single point in a joint trajectory. - - Attributes: - time_from_start: Time from trajectory start (seconds) - positions: Joint positions (radians) - velocities: Joint velocities (rad/s) - """ - - msg_name = "trajectory_msgs.TrajectoryPoint" - - __slots__ = ["num_joints", "positions", "time_from_start", "velocities"] - - def __init__( - self, - time_from_start: float = 0.0, - positions: list[float] | None = None, - velocities: list[float] | None = None, - ) -> None: - """ - Initialize TrajectoryPoint. - - Args: - time_from_start: Time from trajectory start (seconds) - positions: Joint positions (radians) - velocities: Joint velocities (rad/s), defaults to zeros if None - """ - self.time_from_start = time_from_start - self.positions = list(positions) if positions else [] - self.num_joints = len(self.positions) - - if velocities is not None: - self.velocities = list(velocities) - else: - self.velocities = [0.0] * self.num_joints - - def lcm_encode(self) -> bytes: - """Encode for LCM transport.""" - return self.encode() - - def encode(self) -> bytes: - buf = BytesIO() - buf.write(TrajectoryPoint._get_packed_fingerprint()) - self._encode_one(buf) - return buf.getvalue() - - def _encode_one(self, buf: BytesIO) -> None: - # time_from_start (double) - buf.write(struct.pack(">d", self.time_from_start)) - # num_joints (int32) - buf.write(struct.pack(">i", self.num_joints)) - # positions (double[num_joints]) - for p in self.positions: - buf.write(struct.pack(">d", p)) - # velocities (double[num_joints]) - for v in self.velocities: - buf.write(struct.pack(">d", v)) - - @classmethod - def lcm_decode(cls, data: bytes) -> "TrajectoryPoint": - """Decode from LCM transport.""" - return cls.decode(data) - - @classmethod - def decode(cls, data: bytes) -> "TrajectoryPoint": - buf = BytesIO(data) if not hasattr(data, "read") else data - if buf.read(8) != cls._get_packed_fingerprint(): - raise ValueError("Decode error: fingerprint mismatch") - return cls._decode_one(buf) # type: ignore[arg-type] - - @classmethod - def _decode_one(cls, buf: BytesIO) -> "TrajectoryPoint": - self = cls.__new__(cls) - self.time_from_start = struct.unpack(">d", buf.read(8))[0] - self.num_joints = struct.unpack(">i", buf.read(4))[0] - self.positions = [struct.unpack(">d", buf.read(8))[0] for _ in range(self.num_joints)] - self.velocities = [struct.unpack(">d", buf.read(8))[0] for _ in range(self.num_joints)] - return self - - _packed_fingerprint = None - - @classmethod - def _get_hash_recursive(cls, parents): # type: ignore[no-untyped-def] - if cls in parents: - return 0 - return 0x1A2B3C4D5E6F7081 & 0xFFFFFFFFFFFFFFFF - - @classmethod - def _get_packed_fingerprint(cls) -> bytes: - if cls._packed_fingerprint is None: - cls._packed_fingerprint = struct.pack(">Q", cls._get_hash_recursive([])) # type: ignore[no-untyped-call] - return cls._packed_fingerprint - - def __str__(self) -> str: - return f"TrajectoryPoint(t={self.time_from_start:.3f}s, {self.num_joints} joints)" - - def __repr__(self) -> str: - return ( - f"TrajectoryPoint(time_from_start={self.time_from_start}, " - f"positions={self.positions}, velocities={self.velocities})" - ) - - def __eq__(self, other) -> bool: # type: ignore[no-untyped-def] - if not isinstance(other, TrajectoryPoint): - return False - return ( - self.time_from_start == other.time_from_start - and self.positions == other.positions - and self.velocities == other.velocities - ) diff --git a/dimos/msgs/trajectory_msgs/TrajectoryStatus.py b/dimos/msgs/trajectory_msgs/TrajectoryStatus.py deleted file mode 100644 index 0a3c117e685..00000000000 --- a/dimos/msgs/trajectory_msgs/TrajectoryStatus.py +++ /dev/null @@ -1,170 +0,0 @@ -# Copyright 2025-2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -""" -TrajectoryStatus message type. - -Status feedback for trajectory execution. -""" - -from enum import IntEnum -from io import BytesIO -import struct -import time - - -class TrajectoryState(IntEnum): - """States for trajectory execution.""" - - IDLE = 0 # No trajectory, ready to accept - EXECUTING = 1 # Currently executing trajectory - COMPLETED = 2 # Trajectory finished successfully - ABORTED = 3 # Trajectory was cancelled - FAULT = 4 # Error occurred, requires reset() - - -class TrajectoryStatus: - """ - Status of trajectory execution. - - Attributes: - timestamp: When status was generated - state: Current TrajectoryState - progress: Progress 0.0 to 1.0 - time_elapsed: Seconds since trajectory start - time_remaining: Estimated seconds remaining - error: Error message if FAULT state (empty string otherwise) - """ - - msg_name = "trajectory_msgs.TrajectoryStatus" - - __slots__ = ["error", "progress", "state", "time_elapsed", "time_remaining", "timestamp"] - - def __init__( - self, - state: TrajectoryState = TrajectoryState.IDLE, - progress: float = 0.0, - time_elapsed: float = 0.0, - time_remaining: float = 0.0, - error: str = "", - timestamp: float | None = None, - ) -> None: - """ - Initialize TrajectoryStatus. - - Args: - state: Current execution state - progress: Progress through trajectory (0.0 to 1.0) - time_elapsed: Time since trajectory start (seconds) - time_remaining: Estimated time remaining (seconds) - error: Error message if in FAULT state - timestamp: When status was generated (defaults to now) - """ - self.timestamp = timestamp if timestamp is not None else time.time() - self.state = state - self.progress = progress - self.time_elapsed = time_elapsed - self.time_remaining = time_remaining - self.error = error - - @property - def state_name(self) -> str: - """Get human-readable state name.""" - return self.state.name - - def is_done(self) -> bool: - """Check if trajectory execution is finished (completed, aborted, or fault).""" - return self.state in ( - TrajectoryState.COMPLETED, - TrajectoryState.ABORTED, - TrajectoryState.FAULT, - ) - - def is_active(self) -> bool: - """Check if trajectory is currently executing.""" - return self.state == TrajectoryState.EXECUTING - - def lcm_encode(self) -> bytes: - """Encode for LCM transport.""" - return self.encode() - - def encode(self) -> bytes: - buf = BytesIO() - buf.write(TrajectoryStatus._get_packed_fingerprint()) - self._encode_one(buf) - return buf.getvalue() - - def _encode_one(self, buf: BytesIO) -> None: - # timestamp (double) - buf.write(struct.pack(">d", self.timestamp)) - # state (int32) - buf.write(struct.pack(">i", int(self.state))) - # progress (double) - buf.write(struct.pack(">d", self.progress)) - # time_elapsed (double) - buf.write(struct.pack(">d", self.time_elapsed)) - # time_remaining (double) - buf.write(struct.pack(">d", self.time_remaining)) - # error (string) - error_bytes = self.error.encode("utf-8") - buf.write(struct.pack(">i", len(error_bytes))) - buf.write(error_bytes) - - @classmethod - def lcm_decode(cls, data: bytes) -> "TrajectoryStatus": - """Decode from LCM transport.""" - return cls.decode(data) - - @classmethod - def decode(cls, data: bytes) -> "TrajectoryStatus": - buf = BytesIO(data) if not hasattr(data, "read") else data - if buf.read(8) != cls._get_packed_fingerprint(): - raise ValueError("Decode error: fingerprint mismatch") - return cls._decode_one(buf) # type: ignore[arg-type] - - @classmethod - def _decode_one(cls, buf: BytesIO) -> "TrajectoryStatus": - self = cls.__new__(cls) - self.timestamp = struct.unpack(">d", buf.read(8))[0] - self.state = TrajectoryState(struct.unpack(">i", buf.read(4))[0]) - self.progress = struct.unpack(">d", buf.read(8))[0] - self.time_elapsed = struct.unpack(">d", buf.read(8))[0] - self.time_remaining = struct.unpack(">d", buf.read(8))[0] - error_len = struct.unpack(">i", buf.read(4))[0] - self.error = buf.read(error_len).decode("utf-8") - return self - - _packed_fingerprint = None - - @classmethod - def _get_hash_recursive(cls, parents): # type: ignore[no-untyped-def] - if cls in parents: - return 0 - return 0x3C4D5E6F708192A3 & 0xFFFFFFFFFFFFFFFF - - @classmethod - def _get_packed_fingerprint(cls) -> bytes: - if cls._packed_fingerprint is None: - cls._packed_fingerprint = struct.pack(">Q", cls._get_hash_recursive([])) # type: ignore[no-untyped-call] - return cls._packed_fingerprint - - def __str__(self) -> str: - return f"TrajectoryStatus({self.state_name}, progress={self.progress:.1%})" - - def __repr__(self) -> str: - return ( - f"TrajectoryStatus(state={self.state_name}, progress={self.progress}, " - f"time_elapsed={self.time_elapsed}, time_remaining={self.time_remaining}, " - f"error='{self.error}')" - ) diff --git a/dimos/msgs/vision_msgs/BoundingBox3DArray.py b/dimos/msgs/vision_msgs/BoundingBox3DArray.py deleted file mode 100644 index d8d7775f919..00000000000 --- a/dimos/msgs/vision_msgs/BoundingBox3DArray.py +++ /dev/null @@ -1,21 +0,0 @@ -# Copyright 2025-2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from dimos_lcm.vision_msgs.BoundingBox3DArray import ( - BoundingBox3DArray as LCMBoundingBox3DArray, -) - - -class BoundingBox3DArray(LCMBoundingBox3DArray): # type: ignore[misc] - msg_name = "vision_msgs.BoundingBox3DArray" diff --git a/dimos/msgs/vision_msgs/Detection2D.py b/dimos/msgs/vision_msgs/Detection2D.py deleted file mode 100644 index aa957f80614..00000000000 --- a/dimos/msgs/vision_msgs/Detection2D.py +++ /dev/null @@ -1,27 +0,0 @@ -# Copyright 2025-2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. -from dimos_lcm.vision_msgs.Detection2D import Detection2D as LCMDetection2D - -from dimos.types.timestamped import to_timestamp - - -class Detection2D(LCMDetection2D): # type: ignore[misc] - msg_name = "vision_msgs.Detection2D" - - # for _get_field_type() to work when decoding in _decode_one() - __annotations__ = LCMDetection2D.__annotations__ - - @property - def ts(self) -> float: - return to_timestamp(self.header.stamp) diff --git a/dimos/msgs/vision_msgs/Detection2DArray.py b/dimos/msgs/vision_msgs/Detection2DArray.py deleted file mode 100644 index f33cc4cc2ae..00000000000 --- a/dimos/msgs/vision_msgs/Detection2DArray.py +++ /dev/null @@ -1,29 +0,0 @@ -# Copyright 2025-2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. -from dimos_lcm.vision_msgs.Detection2DArray import ( - Detection2DArray as LCMDetection2DArray, -) - -from dimos.types.timestamped import to_timestamp - - -class Detection2DArray(LCMDetection2DArray): # type: ignore[misc] - msg_name = "vision_msgs.Detection2DArray" - - # for _get_field_type() to work when decoding in _decode_one() - __annotations__ = LCMDetection2DArray.__annotations__ - - @property - def ts(self) -> float: - return to_timestamp(self.header.stamp) diff --git a/dimos/msgs/vision_msgs/Detection3D.py b/dimos/msgs/vision_msgs/Detection3D.py deleted file mode 100644 index 76a838041a4..00000000000 --- a/dimos/msgs/vision_msgs/Detection3D.py +++ /dev/null @@ -1,31 +0,0 @@ -# Copyright 2025-2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. -from dimos_lcm.vision_msgs.Detection3D import Detection3D as LCMDetection3D - -from dimos.types.timestamped import to_timestamp - - -class Detection3D(LCMDetection3D): # type: ignore[misc] - msg_name = "vision_msgs.Detection3D" - - # for _get_field_type() to work when decoding in _decode_one() - __annotations__ = LCMDetection3D.__annotations__ - - @property - def ts(self) -> float: - return to_timestamp(self.header.stamp) - - @property - def frame_id(self) -> str: - return str(self.header.frame_id) diff --git a/dimos/msgs/vision_msgs/Detection3DArray.py b/dimos/msgs/vision_msgs/Detection3DArray.py deleted file mode 100644 index ffc5da35ee3..00000000000 --- a/dimos/msgs/vision_msgs/Detection3DArray.py +++ /dev/null @@ -1,80 +0,0 @@ -# Copyright 2025-2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. -from typing import Any - -from dimos_lcm.vision_msgs.Detection3DArray import Detection3DArray as LCMDetection3DArray - -from dimos.types.timestamped import to_timestamp - - -class Detection3DArray(LCMDetection3DArray): # type: ignore[misc] - msg_name = "vision_msgs.Detection3DArray" - - # for _get_field_type() to work when decoding in _decode_one() - __annotations__ = LCMDetection3DArray.__annotations__ - - @property - def ts(self) -> float: - return to_timestamp(self.header.stamp) - - @property - def frame_id(self) -> str: - return str(self.header.frame_id) - - def to_rerun(self) -> Any: - """Convert detections to a Rerun Boxes3D archetype.""" - import rerun as rr - - centers: list[tuple[float, float, float]] = [] - half_sizes: list[tuple[float, float, float]] = [] - quaternions: list[tuple[float, float, float, float]] = [] - labels: list[str] = [] - - for detection in self.detections[: self.detections_length]: - bbox = detection.bbox - center = bbox.center.position - orientation = bbox.center.orientation - size = bbox.size - - centers.append((center.x, center.y, center.z)) - half_sizes.append((size.x / 2.0, size.y / 2.0, size.z / 2.0)) - quaternions.append( - ( - orientation.x, - orientation.y, - orientation.z, - orientation.w, - ) - ) - labels.append(_label_for_detection(detection)) - - return rr.Boxes3D( - centers=centers, - half_sizes=half_sizes, - quaternions=quaternions, - labels=labels, - ) - - -def _label_for_detection(detection: Any) -> str: - marker_id = str(getattr(detection, "id", "")).strip() - for result in detection.results[: detection.results_length]: - class_id = str(result.hypothesis.class_id).strip() - if marker_id and class_id: - return f"{class_id} id={marker_id}" - if class_id: - return class_id - if marker_id: - return f"id={marker_id}" - return "" diff --git a/dimos/msgs/vision_msgs/test_detection3d_rerun.py b/dimos/msgs/vision_msgs/test_detection3d_rerun.py index b0b4247052d..df75cfe4936 100644 --- a/dimos/msgs/vision_msgs/test_detection3d_rerun.py +++ b/dimos/msgs/vision_msgs/test_detection3d_rerun.py @@ -12,16 +12,20 @@ # See the License for the specific language governing permissions and # limitations under the License. -from dimos_lcm.vision_msgs import BoundingBox3D, ObjectHypothesis, ObjectHypothesisWithPose +from dimos_generated.geometry_msgs.msg import Point, Pose, Quaternion, Vector3 +from dimos_generated.std_msgs.msg import Header +from dimos_generated.vision_msgs.msg import ( + BoundingBox3D, + Detection3D, + Detection3DArray, + ObjectHypothesis, + ObjectHypothesisWithPose, +) import pytest import rerun as rr -from dimos.msgs.geometry_msgs.Pose import Pose -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.msgs.std_msgs.Header import Header -from dimos.msgs.vision_msgs.Detection3D import Detection3D -from dimos.msgs.vision_msgs.Detection3DArray import Detection3DArray +from dimos.msgs.time import time_from_seconds +from dimos.visualization.rerun.message_helpers import detection_boxes def _detection3d( @@ -32,7 +36,7 @@ def _detection3d( class_id: str = "DICT_APRILTAG_36h11:7", ) -> Detection3D: det = Detection3D() - det.header = Header(ts, frame_id) + det.header = Header(stamp=time_from_seconds(ts), frame_id=frame_id) det.id = marker_id det.results = [ ObjectHypothesisWithPose( @@ -42,13 +46,12 @@ def _detection3d( ) ) ] - det.results_length = len(det.results) det.bbox = BoundingBox3D( center=Pose( - position=Vector3(1.0, 2.0, 3.0), - orientation=Quaternion(0.0, 0.0, 0.70710678, 0.70710678), + position=Point(x=1.0, y=2.0, z=3.0), + orientation=Quaternion(z=0.70710678, w=0.70710678), ), - size=Vector3(0.2, 0.4, 0.0), + size=Vector3(x=0.2, y=0.4), ) return det @@ -56,19 +59,18 @@ def _detection3d( def test_detection3d_frame_id_comes_from_header() -> None: det = _detection3d(frame_id="map") - assert det.frame_id == "map" + assert det.header.frame_id == "map" def test_detection3darray_to_rerun_preserves_wire_pose_size_and_identity() -> None: msg = Detection3DArray( - header=Header(12.5, "world"), + header=Header(stamp=time_from_seconds(12.5), frame_id="world"), detections=[_detection3d()], - detections_length=1, ) - boxes = msg.to_rerun() + boxes = detection_boxes(msg) - assert msg.frame_id == "world" + assert msg.header.frame_id == "world" assert isinstance(boxes, rr.Boxes3D) assert boxes.centers.as_arrow_array().to_pylist() == [[1.0, 2.0, 3.0]] assert boxes.half_sizes.as_arrow_array().to_pylist()[0] == pytest.approx([0.1, 0.2, 0.0]) @@ -80,12 +82,11 @@ def test_detection3darray_to_rerun_preserves_wire_pose_size_and_identity() -> No def test_detection3darray_to_rerun_empty_array_is_safe() -> None: msg = Detection3DArray( - header=Header(12.5, "world"), + header=Header(stamp=time_from_seconds(12.5), frame_id="world"), detections=[], - detections_length=0, ) - boxes = msg.to_rerun() + boxes = detection_boxes(msg) assert isinstance(boxes, rr.Boxes3D) assert boxes.centers.as_arrow_array().to_pylist() == [] diff --git a/dimos/msgs/visualization_msgs/EntityMarkers.py b/dimos/msgs/visualization_msgs/EntityMarkers.py deleted file mode 100644 index 86ba74d72e9..00000000000 --- a/dimos/msgs/visualization_msgs/EntityMarkers.py +++ /dev/null @@ -1,145 +0,0 @@ -# Copyright 2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -"""Labeled 3D entity markers for Rerun visualization. - -Published by modules that track entities in world coordinates. -The Rerun bridge picks these up via ``to_rerun()`` and renders them -as labeled colored points overlaid on the 3D scene. -""" - -from __future__ import annotations - -from dataclasses import dataclass -from io import BytesIO -import json -import struct -import time -from typing import TYPE_CHECKING - -if TYPE_CHECKING: - from rerun._baseclasses import Archetype - -from dimos.types.timestamped import Timestamped - -# Entity type → RGBA color -TYPE_COLORS: dict[str, tuple[int, int, int, int]] = { - "person": (255, 100, 100, 255), - "object": (100, 255, 100, 255), - "location": (100, 100, 255, 255), -} -DEFAULT_COLOR = (200, 200, 200, 255) - - -@dataclass -class Marker: - """A single entity marker in world coordinates.""" - - entity_id: str - label: str - entity_type: str # person | object | location - x: float - y: float - z: float - - -class EntityMarkers(Timestamped): - """A batch of labeled 3D entity markers. - - Wire format: JSON-encoded list of markers over LCM string channel. - Rerun: ``rr.Points3D`` with per-point labels and colors. - """ - - msg_name = "visualization_msgs.EntityMarkers" - - def __init__( - self, - markers: list[Marker] | None = None, - ts: float | None = None, - ) -> None: - self.markers: list[Marker] = markers or [] - self.ts: float = ts or time.time() - - # -- LCM serialization (JSON payload) -- - - def _encode_one(self, buf: BytesIO) -> None: - payload = json.dumps( - [ - { - "id": m.entity_id, - "label": m.label, - "type": m.entity_type, - "x": m.x, - "y": m.y, - "z": m.z, - } - for m in self.markers - ] - ).encode() - buf.write(struct.pack(">I", len(payload))) - buf.write(payload) - - def encode(self) -> bytes: - buf = BytesIO() - self._encode_one(buf) - return buf.getvalue() - - @classmethod - def _decode_one(cls, buf: BytesIO) -> EntityMarkers: - (length,) = struct.unpack(">I", buf.read(4)) - payload = json.loads(buf.read(length).decode()) - markers = [ - Marker( - entity_id=m["id"], - label=m["label"], - entity_type=m["type"], - x=m["x"], - y=m["y"], - z=m["z"], - ) - for m in payload - ] - return cls(markers=markers) - - @classmethod - def decode(cls, data: bytes) -> EntityMarkers: - return cls._decode_one(BytesIO(data)) - - # -- Rerun conversion -- - - def to_rerun(self) -> Archetype: - import rerun as rr - - if not self.markers: - return rr.Points3D([]) - - positions = [[m.x, m.y, m.z] for m in self.markers] - labels = [f"{m.entity_id}: {m.label[:40]}" for m in self.markers] - colors = [TYPE_COLORS.get(m.entity_type, DEFAULT_COLOR) for m in self.markers] - - return rr.Points3D( - positions=positions, - labels=labels, - colors=colors, - radii=[0.15] * len(self.markers), - ) - - # -- LCM compat (so autoconnect assigns LCMTransport, not pLCM) -- - - def lcm_encode(self) -> bytes: - return self.encode() - - @classmethod - def lcm_decode(cls, data: bytes, **kwargs: object) -> EntityMarkers: - return cls.decode(data) diff --git a/dimos/navigation/base.py b/dimos/navigation/base.py index dc9cbbe9c27..affc0ad1a53 100644 --- a/dimos/navigation/base.py +++ b/dimos/navigation/base.py @@ -16,7 +16,7 @@ from abc import ABC, abstractmethod from enum import Enum -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped +from dimos_generated.geometry_msgs.msg import PoseStamped class NavigationState(Enum): diff --git a/dimos/navigation/basic_path_follower/module.py b/dimos/navigation/basic_path_follower/module.py index f27068fbb22..6645841d7f4 100644 --- a/dimos/navigation/basic_path_follower/module.py +++ b/dimos/navigation/basic_path_follower/module.py @@ -19,7 +19,10 @@ import time from typing import Any -from dimos_lcm.std_msgs import Bool +from dimos_generated.geometry_msgs.msg import PoseStamped, Twist, Vector3 +from dimos_generated.nav_msgs.msg import Path +from dimos_generated.std_msgs.msg import Bool +from dimos_generated.tf2_msgs.msg import TFMessage import numpy as np from numpy.typing import NDArray from reactivex.disposable import Disposable @@ -28,11 +31,7 @@ from dimos.core.core import rpc from dimos.core.module import Module, ModuleConfig from dimos.core.stream import In, Out -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.geometry_msgs.Twist import Twist -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.msgs.nav_msgs.Path import Path -from dimos.msgs.tf2_msgs.TFMessage import TFMessage +from dimos.msgs.geometry import quaternion_euler from dimos.utils.logging_config import setup_logger from dimos.utils.trigonometry import angle_diff @@ -121,7 +120,9 @@ def _on_path(self, path: Path) -> None: self._waypoints = None self.nav_cmd_vel.publish(Twist()) return - waypoints = np.array([[p.position.x, p.position.y] for p in path.poses], dtype=np.float32) + waypoints = np.array( + [[p.pose.position.x, p.pose.position.y] for p in path.poses], dtype=np.float32 + ) with self._lock: self._waypoints = waypoints @@ -144,20 +145,20 @@ def _follow(self) -> None: self._stop_event.wait(max(0.0, period - elapsed)) def _step(self, pose: PoseStamped, waypoints: NDArray[np.float32]) -> None: - position = np.array([pose.position.x, pose.position.y], dtype=np.float32) + position = np.array([pose.pose.position.x, pose.pose.position.y], dtype=np.float32) if float(np.linalg.norm(waypoints[-1] - position)) < self.config.goal_tolerance: self.nav_cmd_vel.publish(Twist()) with self._lock: if self._waypoints is waypoints: self._waypoints = None - self.goal_reached.publish(Bool(True)) + self.goal_reached.publish(Bool(data=True)) logger.info("Goal reached") return target = self._lookahead_point(waypoints, position) yaw_error = angle_diff( math.atan2(target[1] - position[1], target[0] - position[0]), - pose.orientation.euler[2], + quaternion_euler(pose.pose.orientation)[2], ) angular = max( @@ -165,7 +166,7 @@ def _step(self, pose: PoseStamped, waypoints: NDArray[np.float32]) -> None: min(self.config.max_angular, self.config.heading_gain * yaw_error), ) linear = self.config.speed * max(0.0, math.cos(yaw_error)) - self.nav_cmd_vel.publish(Twist(Vector3(linear, 0, 0), Vector3(0, 0, angular))) + self.nav_cmd_vel.publish(Twist(linear=Vector3(x=linear), angular=Vector3(z=angular))) def _lookahead_point( self, waypoints: NDArray[np.float32], position: NDArray[np.float32] diff --git a/dimos/navigation/basic_path_follower/test_module.py b/dimos/navigation/basic_path_follower/test_module.py index 43948eabb86..eb10d76fbf6 100644 --- a/dimos/navigation/basic_path_follower/test_module.py +++ b/dimos/navigation/basic_path_follower/test_module.py @@ -12,9 +12,20 @@ # See the License for the specific language governing permissions and # limitations under the License. -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Transform import Transform -from dimos.msgs.geometry_msgs.Vector3 import Vector3 +from dimos_generated.builtin_interfaces.msg import Time +from dimos_generated.geometry_msgs.msg import ( + Point, + Pose, + PoseStamped, + Quaternion, + Transform, + TransformStamped, + Twist, + Vector3, +) +from dimos_generated.nav_msgs.msg import Path +from dimos_generated.std_msgs.msg import Bool, Header + from dimos.navigation.basic_path_follower.module import BasicPathFollower, lookahead_distance from dimos.protocol.tf.tf import MultiTBuffer @@ -36,7 +47,7 @@ def get( time_tolerance: float | None = None, *, forward_tolerance: float = 0.0, - ) -> Transform | None: + ) -> TransformStamped | None: self.gets += 1 return super().get( parent_frame, @@ -50,23 +61,19 @@ def dispose(self) -> None: pass -def _mount() -> Transform: - return Transform( - translation=Vector3(0.0, 0.0, MOUNT_Z), - rotation=Quaternion(0.0, 0.0, 0.0, 1.0), - frame_id="base_link", +def _mount() -> TransformStamped: + return TransformStamped( + header=Header(frame_id="base_link", stamp=Time(sec=1)), child_frame_id="mid360_link", - ts=1.0, + transform=Transform(translation=Vector3(z=MOUNT_Z), rotation=Quaternion(w=1)), ) -def _odom_edge() -> Transform: - return Transform( - translation=Vector3(1.0, 2.0, 3.0), - rotation=Quaternion(0.0, 0.0, 0.0, 1.0), - frame_id="odom", +def _odom_edge() -> TransformStamped: + return TransformStamped( + header=Header(frame_id="odom", stamp=Time(sec=1)), child_frame_id="mid360_link", - ts=1.0, + transform=Transform(translation=Vector3(x=1, y=2, z=3), rotation=Quaternion(w=1)), ) @@ -79,7 +86,7 @@ def test_lookup_pose_steers_from_the_tf_base_pose() -> None: try: pose = module._lookup_pose() assert pose is not None - assert abs(pose.position.z - (3.0 - MOUNT_Z)) < 1e-9 + assert abs(pose.pose.position.z - (3.0 - MOUNT_Z)) < 1e-9 finally: module.stop() @@ -121,3 +128,31 @@ def test_lookahead_scales_in_linear_region() -> None: def test_lookahead_clamped_at_ceiling() -> None: assert lookahead_distance(2.0, 1.5, 0.4, 1.5) == 1.5 + + +def test_generated_path_produces_velocity_and_arrival(): + module = BasicPathFollower() + commands = [] + arrivals = [] + module.nav_cmd_vel.subscribe(lambda value: commands.append(Twist.decode(value.encode()))) + module.goal_reached.subscribe(lambda value: arrivals.append(Bool.decode(value.encode()))) + try: + path = Path( + poses=[ + PoseStamped(pose=Pose(position=Point(x=x), orientation=Quaternion(w=1))) + for x in [0, 1, 2] + ] + ) + module._on_path(Path.decode(path.encode())) + waypoints = module._waypoints + assert waypoints is not None + module._step(path.poses[0], waypoints) + assert commands[-1].linear.x > 0 + assert not arrivals + module._step(path.poses[-1], waypoints) + assert commands[-1] == Twist() + assert arrivals[-1].data + module._on_path(Path()) + assert module._waypoints is None + finally: + module.stop() diff --git a/dimos/navigation/bbox_navigation.py b/dimos/navigation/bbox_navigation.py index c5511cf7d00..de56e99ba18 100644 --- a/dimos/navigation/bbox_navigation.py +++ b/dimos/navigation/bbox_navigation.py @@ -14,16 +14,14 @@ import logging -from dimos_lcm.sensor_msgs import CameraInfo +from dimos_generated.geometry_msgs.msg import Point, Pose, PoseStamped, Quaternion +from dimos_generated.sensor_msgs.msg import CameraInfo +from dimos_generated.vision_msgs.msg import Detection2DArray from reactivex.disposable import Disposable from dimos.core.core import rpc from dimos.core.module import Module, ModuleConfig from dimos.core.stream import In, Out -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.msgs.vision_msgs.Detection2DArray import Detection2DArray from dimos.utils.logging_config import setup_logger logger = setup_logger(level=logging.DEBUG) @@ -46,7 +44,7 @@ class BBoxNavigationModule(Module): @rpc def start(self) -> None: unsub = self.camera_info.subscribe( - lambda msg: setattr(self, "camera_intrinsics", [msg.K[0], msg.K[4], msg.K[2], msg.K[5]]) + lambda msg: setattr(self, "camera_intrinsics", [msg.k[0], msg.k[4], msg.k[2], msg.k[5]]) ) self.register_disposable(Disposable(unsub)) @@ -58,7 +56,7 @@ def stop(self) -> None: super().stop() def _on_detection(self, det: Detection2DArray) -> None: - if det.detections_length == 0 or not self.camera_intrinsics: + if not det.detections or not self.camera_intrinsics: return fx, fy, cx, cy = self.camera_intrinsics center_x, center_y = ( @@ -71,9 +69,8 @@ def _on_detection(self, det: Detection2DArray) -> None: self.config.goal_distance, ) goal = PoseStamped( - position=Vector3(z, -x, -y), - orientation=Quaternion(0, 0, 0, 1), - frame_id=det.header.frame_id, + header=det.header, + pose=Pose(position=Point(x=z, y=-x, z=-y), orientation=Quaternion(w=1)), ) logger.debug( f"BBox center: ({center_x:.1f}, {center_y:.1f}) → " diff --git a/dimos/navigation/dannav/geometry/path_distancer.py b/dimos/navigation/dannav/geometry/path_distancer.py index 8b10287fa0b..44fce100d7a 100644 --- a/dimos/navigation/dannav/geometry/path_distancer.py +++ b/dimos/navigation/dannav/geometry/path_distancer.py @@ -18,11 +18,10 @@ import math from typing import cast +from dimos_generated.nav_msgs.msg import Path import numpy as np from numpy.typing import NDArray -from dimos.msgs.nav_msgs.Path import Path - @dataclass(frozen=True) class PolylineProjection: @@ -136,7 +135,7 @@ class PathDistancer: _cumulative_dists: NDArray[np.float64] def __init__(self, path: Path) -> None: - self._path = np.array([[p.position.x, p.position.y] for p in path.poses]) + self._path = np.array([[p.pose.position.x, p.pose.position.y] for p in path.poses]) self._cumulative_dists = _make_cumulative_distance_array(self._path) @property diff --git a/dimos/navigation/dannav/holonomic_tc/command_limits.py b/dimos/navigation/dannav/holonomic_tc/command_limits.py index 337c74cad3a..c26b55b0462 100644 --- a/dimos/navigation/dannav/holonomic_tc/command_limits.py +++ b/dimos/navigation/dannav/holonomic_tc/command_limits.py @@ -30,8 +30,7 @@ from dataclasses import dataclass import math -from dimos.msgs.geometry_msgs.Twist import Twist -from dimos.msgs.geometry_msgs.Vector3 import Vector3 +from dimos_generated.geometry_msgs.msg import Twist, Vector3 @dataclass(frozen=True) @@ -105,13 +104,13 @@ def clamp_holonomic_cmd_vel( return Twist( linear=Vector3( - s_x, - s_y, - float(raw_cmd.linear.z), + x=s_x, + y=s_y, + z=float(raw_cmd.linear.z), ), angular=Vector3( - float(raw_cmd.angular.x), - float(raw_cmd.angular.y), - wz, + x=float(raw_cmd.angular.x), + y=float(raw_cmd.angular.y), + z=wz, ), ) diff --git a/dimos/navigation/dannav/holonomic_tc/holonomic_path_controller.py b/dimos/navigation/dannav/holonomic_tc/holonomic_path_controller.py index 8796fe78f8d..2c77f0ad35a 100644 --- a/dimos/navigation/dannav/holonomic_tc/holonomic_path_controller.py +++ b/dimos/navigation/dannav/holonomic_tc/holonomic_path_controller.py @@ -12,14 +12,13 @@ # See the License for the specific language governing permissions and # limitations under the License. +from copy import deepcopy + +from dimos_generated.geometry_msgs.msg import Point, Pose, PoseStamped, Twist, Vector3 import numpy as np from dimos.core.global_config import GlobalConfig -from dimos.msgs.geometry_msgs.Pose import Pose -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Twist import Twist -from dimos.msgs.geometry_msgs.Vector3 import Vector3 +from dimos.msgs.geometry import quaternion_euler, quaternion_from_euler from dimos.navigation.dannav.holonomic_tc.command_limits import ( HolonomicCommandLimits, clamp_holonomic_cmd_vel, @@ -37,13 +36,13 @@ def _pose_from_xy_yaw(x: float, y: float, yaw: float) -> Pose: return Pose( - position=Vector3(x, y, 0.0), - orientation=Quaternion.from_euler(Vector3(0.0, 0.0, float(yaw))), + position=Point(x=x, y=y), + orientation=quaternion_from_euler(0.0, 0.0, float(yaw)), ) def _pose_from_pose_stamped(odom: PoseStamped) -> Pose: - return Pose(odom.position, odom.orientation) + return deepcopy(odom.pose) class HolonomicPathController: @@ -122,16 +121,16 @@ def rotate( if wz != 0.0 and abs(wz) < 0.2: wz = 0.2 * (1.0 if wz > 0 else -1.0) t = Twist( - linear=Vector3(0.0, 0.0, 0.0), - angular=Vector3(0.0, 0.0, wz), + linear=Vector3(x=0.0, y=0.0, z=0.0), + angular=Vector3(x=0.0, y=0.0, z=wz), ) return self._limit_output(self._apply_sim_angular(t)) - robot_yaw = float(current_odom.orientation.euler[2]) + robot_yaw = float(quaternion_euler(current_odom.pose.orientation)[2]) target_yaw = float(normalize_angle(robot_yaw + yaw_error)) p = _pose_from_xy_yaw( - float(current_odom.position.x), - float(current_odom.position.y), + float(current_odom.pose.position.x), + float(current_odom.pose.position.y), target_yaw, ) ref = TrajectoryReferenceSample(0.0, p, Twist()) @@ -149,8 +148,8 @@ def _apply_sim_angular(self, t: Twist) -> Twist: if self._global_config.simulation and 1e-9 < abs(wz) < 0.8: wz = 0.8 * (1.0 if wz > 0 else -1.0) return Twist( - linear=Vector3(float(t.linear.x), float(t.linear.y), float(t.linear.z)), - angular=Vector3(0.0, 0.0, wz), + linear=Vector3(x=float(t.linear.x), y=float(t.linear.y), z=float(t.linear.z)), + angular=Vector3(x=0.0, y=0.0, z=wz), ) def _limit_output(self, raw: Twist) -> Twist: @@ -160,5 +159,5 @@ def _limit_output(self, raw: Twist) -> Twist: self._limits, 1.0 / self._control_frequency, ) - self._previous_cmd = Twist(out) + self._previous_cmd = deepcopy(out) return out diff --git a/dimos/navigation/dannav/holonomic_tc/holonomic_tracking_controller.py b/dimos/navigation/dannav/holonomic_tc/holonomic_tracking_controller.py index cc205b73bbe..8c7216fa6e9 100644 --- a/dimos/navigation/dannav/holonomic_tc/holonomic_tracking_controller.py +++ b/dimos/navigation/dannav/holonomic_tc/holonomic_tracking_controller.py @@ -25,11 +25,12 @@ from __future__ import annotations +from copy import deepcopy import math -from dimos.msgs.geometry_msgs.Pose import Pose -from dimos.msgs.geometry_msgs.Twist import Twist -from dimos.msgs.geometry_msgs.Vector3 import Vector3 +from dimos_generated.geometry_msgs.msg import Pose, Twist, Vector3 + +from dimos.msgs.geometry import quaternion_euler from dimos.navigation.dannav.holonomic_tc.command_limits import HolonomicCommandLimits from dimos.navigation.dannav.holonomic_tc.types import ( TrajectoryMeasuredSample, @@ -39,24 +40,24 @@ def _planar_yaw_rad(pose_plan: Pose) -> float: - return float(pose_plan.orientation.euler.z) + return float(quaternion_euler(pose_plan.orientation)[2]) def _scale_planar_twist(cmd: Twist, max_planar_speed_m_s: float) -> Twist: sp = math.hypot(float(cmd.linear.x), float(cmd.linear.y)) if sp <= max_planar_speed_m_s or sp < 1e-15: - return Twist(cmd) + return deepcopy(cmd) f = max_planar_speed_m_s / sp return Twist( linear=Vector3( - float(cmd.linear.x) * f, - float(cmd.linear.y) * f, - float(cmd.linear.z), + x=float(cmd.linear.x) * f, + y=float(cmd.linear.y) * f, + z=float(cmd.linear.z), ), angular=Vector3( - float(cmd.angular.x), - float(cmd.angular.y), - float(cmd.angular.z), + x=float(cmd.angular.x), + y=float(cmd.angular.y), + z=float(cmd.angular.z), ), ) @@ -66,14 +67,14 @@ def _clamp_yaw_rate(cmd: Twist, max_abs_wz: float) -> Twist: wz = max(-max_abs_wz, min(max_abs_wz, wz)) return Twist( linear=Vector3( - float(cmd.linear.x), - float(cmd.linear.y), - float(cmd.linear.z), + x=float(cmd.linear.x), + y=float(cmd.linear.y), + z=float(cmd.linear.z), ), angular=Vector3( - float(cmd.angular.x), - float(cmd.angular.y), - wz, + x=float(cmd.angular.x), + y=float(cmd.angular.y), + z=wz, ), ) @@ -153,11 +154,11 @@ def control( wz = wz_ff - self._ky * e_psi - self._kw * wz_err raw = Twist( - linear=Vector3(vx, vy, float(ref.linear.z)), + linear=Vector3(x=vx, y=vy, z=float(ref.linear.z)), angular=Vector3( - float(ref.angular.x), - float(ref.angular.y), - wz, + x=float(ref.angular.x), + y=float(ref.angular.y), + z=wz, ), ) lim = self._limits diff --git a/dimos/navigation/dannav/holonomic_tc/module.py b/dimos/navigation/dannav/holonomic_tc/module.py index 73fc8e7752d..c01891c4739 100644 --- a/dimos/navigation/dannav/holonomic_tc/module.py +++ b/dimos/navigation/dannav/holonomic_tc/module.py @@ -24,6 +24,7 @@ from __future__ import annotations +from copy import deepcopy from dataclasses import dataclass import math from threading import Event, RLock, Thread, current_thread @@ -31,7 +32,9 @@ import traceback from typing import Any, Literal, TypeAlias -from dimos_lcm.std_msgs import Bool +from dimos_generated.geometry_msgs.msg import PoseStamped, Twist, Vector3 +from dimos_generated.nav_msgs.msg import Path +from dimos_generated.std_msgs.msg import Bool import numpy as np from reactivex import Subject from reactivex.disposable import Disposable @@ -41,10 +44,8 @@ from dimos.core.global_config import GlobalConfig from dimos.core.module import Module, ModuleConfig from dimos.core.stream import In, Out -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.geometry_msgs.Twist import Twist -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.msgs.nav_msgs.Path import Path +from dimos.msgs.geometry import quaternion_euler +from dimos.msgs.time import to_nanoseconds, to_seconds from dimos.navigation.base import NavigationState from dimos.navigation.dannav.geometry.path_distancer import PathDistancer from dimos.navigation.dannav.geometry.path_speed_profile import ( @@ -329,8 +330,8 @@ def _initial_state(self, path: Path, current_odom: PoseStamped | None) -> Planne if not self._align_heading_before_move or current_odom is None or len(path.poses) == 0: return "path_following" - first_yaw = path.poses[0].orientation.euler[2] - robot_yaw = current_odom.orientation.euler[2] + first_yaw = quaternion_euler(path.poses[0].pose.orientation)[2] + robot_yaw = quaternion_euler(current_odom.pose.orientation)[2] initial_yaw_error = angle_diff(first_yaw, robot_yaw) if abs(initial_yaw_error) < self._orientation_tolerance: return "path_following" @@ -387,8 +388,8 @@ def _compute_initial_rotation(self) -> Twist: assert current_odom is not None first_pose = path.poses[0] - first_yaw = first_pose.orientation.euler[2] - robot_yaw = current_odom.orientation.euler[2] + first_yaw = quaternion_euler(first_pose.pose.orientation)[2] + robot_yaw = quaternion_euler(current_odom.pose.orientation)[2] yaw_error = angle_diff(first_yaw, robot_yaw) if abs(yaw_error) < self._orientation_tolerance: @@ -408,7 +409,7 @@ def _compute_path_following(self) -> Twist: assert path_distancer is not None assert current_odom is not None - current_pos = np.array([current_odom.position.x, current_odom.position.y]) + current_pos = np.array([current_odom.pose.position.x, current_odom.pose.position.y]) if path_distancer.distance_to_goal(current_pos) < self._goal_tolerance: if self._align_goal_yaw: @@ -440,7 +441,7 @@ def _lookahead_reference_sample( self, path_distancer: PathDistancer, current_odom: PoseStamped, - current_pos: np.ndarray, + current_pos: np.ndarray[Any, np.dtype[np.float64]], path_speed: float, ) -> TrajectoryReferenceSample: projection = path_distancer.project(current_pos) @@ -449,7 +450,7 @@ def _lookahead_reference_sample( path_distancer.path_length_m, s_start + path_distancer.lookahead_distance_m, ) - now_s = float(current_odom.ts) + now_s = to_seconds(current_odom.header.stamp) if not math.isfinite(now_s): now_s = 0.0 travel_s = max(0.0, s_end - s_start) @@ -474,8 +475,8 @@ def _reference_sample_at_progress( # the reference while turning. No costmap yaw-lock in this stack. path_yaw = path_distancer.yaw_at_progress(progress_m) feedforward = Twist( - linear=Vector3(path_speed, 0.0, 0.0), - angular=Vector3(0.0, 0.0, 0.0), + linear=Vector3(x=path_speed, y=0.0, z=0.0), + angular=Vector3(x=0.0, y=0.0, z=0.0), ) return TrajectoryReferenceSample( time_s=time_s, @@ -486,7 +487,7 @@ def _reference_sample_at_progress( def _path_speed_at_position( self, path_distancer: PathDistancer, - current_pos: np.ndarray, + current_pos: np.ndarray[Any, np.dtype[np.float64]], ) -> float: envelope = self._active_envelope progress_m = float(path_distancer.project(current_pos).s_along_path_m) @@ -535,8 +536,8 @@ def _compute_final_rotation(self) -> Twist: assert path is not None assert current_odom is not None - goal_yaw = path.poses[-1].orientation.euler[2] - robot_yaw = current_odom.orientation.euler[2] + goal_yaw = quaternion_euler(path.poses[-1].pose.orientation)[2] + robot_yaw = quaternion_euler(current_odom.pose.orientation)[2] yaw_error = angle_diff(goal_yaw, robot_yaw) if abs(yaw_error) < self._orientation_tolerance: @@ -560,29 +561,31 @@ def _reset_state(self) -> None: def _estimate_measured_body_twist(self, current_odom: PoseStamped) -> Twist: previous = self._previous_odom_for_velocity - self._previous_odom_for_velocity = current_odom + self._previous_odom_for_velocity = deepcopy(current_odom) if previous is None: return Twist() - dt = float(current_odom.ts) - float(previous.ts) + dt = ( + to_nanoseconds(current_odom.header.stamp) - to_nanoseconds(previous.header.stamp) + ) / 1e9 if not math.isfinite(dt) or dt <= 0.0: return Twist() - vx_w = (float(current_odom.position.x) - float(previous.position.x)) / dt - vy_w = (float(current_odom.position.y) - float(previous.position.y)) / dt - yaw = float(current_odom.orientation.euler[2]) + vx_w = (float(current_odom.pose.position.x) - float(previous.pose.position.x)) / dt + vy_w = (float(current_odom.pose.position.y) - float(previous.pose.position.y)) / dt + yaw = float(quaternion_euler(current_odom.pose.orientation)[2]) c = math.cos(yaw) s = math.sin(yaw) vx_b = c * vx_w + s * vy_w vy_b = -s * vx_w + c * vy_w wz = ( angle_diff( - float(current_odom.orientation.euler[2]), - float(previous.orientation.euler[2]), + float(quaternion_euler(current_odom.pose.orientation)[2]), + float(quaternion_euler(previous.pose.orientation)[2]), ) / dt ) return Twist( - linear=Vector3(vx_b, vy_b, 0.0), - angular=Vector3(0.0, 0.0, wz), + linear=Vector3(x=vx_b, y=vy_b, z=0.0), + angular=Vector3(x=0.0, y=0.0, z=wz), ) @@ -657,7 +660,7 @@ def _on_stop(self, msg: Bool) -> None: def _on_core_stopped(self, msg: StopMessage) -> None: if msg == "arrived": - self.goal_reached.publish(Bool(True)) + self.goal_reached.publish(Bool(data=True)) logger.info("Goal reached") # On "error" the core has already published a zero Twist and cleared the # route; there is no planner here to ask for a replan. diff --git a/dimos/navigation/dannav/holonomic_tc/test_command_limits.py b/dimos/navigation/dannav/holonomic_tc/test_command_limits.py index 00ca10453ab..31de279cf01 100644 --- a/dimos/navigation/dannav/holonomic_tc/test_command_limits.py +++ b/dimos/navigation/dannav/holonomic_tc/test_command_limits.py @@ -23,10 +23,9 @@ import math +from dimos_generated.geometry_msgs.msg import Twist, Vector3 import pytest -from dimos.msgs.geometry_msgs.Twist import Twist -from dimos.msgs.geometry_msgs.Vector3 import Vector3 from dimos.navigation.dannav.holonomic_tc.command_limits import ( HolonomicCommandLimits, clamp_holonomic_cmd_vel, @@ -40,7 +39,7 @@ def test_clamp_planar_speed_accel_and_slew() -> None: speed_out = clamp_holonomic_cmd_vel( Twist(), - Twist(linear=Vector3(3.0, 4.0, 0.0)), + Twist(linear=Vector3(x=3.0, y=4.0, z=0.0)), HolonomicCommandLimits(v_max_m_s, 10.0, 10.0, 10.0), 1.0, ) @@ -50,25 +49,25 @@ def test_clamp_planar_speed_accel_and_slew() -> None: accel_limits = HolonomicCommandLimits(10.0, 10.0, a_max_m_s2, 10.0) from_rest = clamp_holonomic_cmd_vel( Twist(), - Twist(linear=Vector3(5.0, 0.0, 0.0)), + Twist(linear=Vector3(x=5.0, y=0.0, z=0.0)), accel_limits, dt_s, ) assert from_rest.linear.x == pytest.approx(a_max_m_s2 * dt_s) - prev = Twist(linear=Vector3(0.8, 0.0, 0.0)) + prev = Twist(linear=Vector3(x=0.8, y=0.0, z=0.0)) slew = clamp_holonomic_cmd_vel( prev, - Twist(linear=Vector3(5.0, 0.0, 0.0)), + Twist(linear=Vector3(x=5.0, y=0.0, z=0.0)), HolonomicCommandLimits(10.0, 10.0, 1.0, 10.0), dt_s, ) assert slew.linear.x == pytest.approx(0.8 + 1.0 * dt_s) - prev_moving = Twist(linear=Vector3(0.6, 0.8, 0.0)) + prev_moving = Twist(linear=Vector3(x=0.6, y=0.8, z=0.0)) reversal = clamp_holonomic_cmd_vel( prev_moving, - Twist(linear=Vector3(-0.6, -0.8, 0.0)), + Twist(linear=Vector3(x=-0.6, y=-0.8, z=0.0)), accel_limits, dt_s, ) @@ -86,7 +85,7 @@ def test_clamp_yaw_rate_and_accel() -> None: w_max_rad_s = 0.5 rate_capped = clamp_holonomic_cmd_vel( Twist(), - Twist(angular=Vector3(0.0, 0.0, 1.0)), + Twist(angular=Vector3(x=0.0, y=0.0, z=1.0)), HolonomicCommandLimits(1.0, w_max_rad_s, 1.0, 5.0), 1.0, ) @@ -96,7 +95,7 @@ def test_clamp_yaw_rate_and_accel() -> None: w_accel_rad_s2 = 2.0 accel_capped = clamp_holonomic_cmd_vel( Twist(), - Twist(angular=Vector3(0.0, 0.0, 1.0)), + Twist(angular=Vector3(x=0.0, y=0.0, z=1.0)), HolonomicCommandLimits(1.0, 10.0, 1.0, w_accel_rad_s2), dt_s, ) diff --git a/dimos/navigation/dannav/holonomic_tc/test_dan_holonomic_tc.py b/dimos/navigation/dannav/holonomic_tc/test_dan_holonomic_tc.py index 50a0096c8e6..08274412305 100644 --- a/dimos/navigation/dannav/holonomic_tc/test_dan_holonomic_tc.py +++ b/dimos/navigation/dannav/holonomic_tc/test_dan_holonomic_tc.py @@ -23,14 +23,13 @@ import time from typing import Any, Literal -from dimos_lcm.std_msgs import Bool +from dimos_generated.geometry_msgs.msg import Point, Pose, PoseStamped, Quaternion, Twist +from dimos_generated.nav_msgs.msg import Path +from dimos_generated.std_msgs.msg import Bool, Header import pytest from dimos.core.stream import Stream, Transport -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Twist import Twist -from dimos.msgs.nav_msgs.Path import Path +from dimos.msgs.time import time_from_seconds from dimos.navigation.base import NavigationState from dimos.navigation.dannav.holonomic_tc.module import DanHolonomicTC @@ -45,7 +44,7 @@ def __init__(self) -> None: def broadcast(self, _selfstream: Any, value: Any) -> None: for callback in list(self._subscribers): - callback(value) + callback(type(value).decode(value.encode())) def subscribe( self, callback: Callable[[Any], Any], _selfstream: Stream[Any] | None = None @@ -71,15 +70,13 @@ class _Captured: def _yaw_quaternion(yaw_rad: float) -> Quaternion: - return Quaternion(0.0, 0.0, math.sin(yaw_rad / 2.0), math.cos(yaw_rad / 2.0)) + return Quaternion(z=math.sin(yaw_rad / 2.0), w=math.cos(yaw_rad / 2.0)) def _odom(x: float, y: float, yaw_rad: float, *, ts: float = 1.0) -> PoseStamped: return PoseStamped( - ts=ts, - frame_id="map", - position=[x, y, 0.0], - orientation=_yaw_quaternion(yaw_rad), + header=Header(stamp=time_from_seconds(ts), frame_id="map"), + pose=Pose(position=Point(x=x, y=y), orientation=_yaw_quaternion(yaw_rad)), ) @@ -92,15 +89,8 @@ def _path_from_points(points: list[tuple[float, float]]) -> Path: else: prev_point = points[index - 1] yaw = math.atan2(point[1] - prev_point[1], point[0] - prev_point[0]) - poses.append( - PoseStamped( - ts=1.0, - frame_id="map", - position=[point[0], point[1], 0.0], - orientation=_yaw_quaternion(yaw), - ) - ) - return Path(frame_id="map", poses=poses) + poses.append(_odom(point[0], point[1], yaw)) + return Path(header=Header(frame_id="map"), poses=poses) def _is_zero_twist(cmd: Twist) -> bool: @@ -135,10 +125,10 @@ def feed_path(self, path: Path) -> None: self.module.path.transport.broadcast(None, path) def feed_empty_path(self) -> None: - self.module.path.transport.broadcast(None, Path(frame_id="map", poses=[])) + self.module.path.transport.broadcast(None, Path(header=Header(frame_id="map"), poses=[])) def feed_stop(self, value: bool = True) -> None: - self.module.stop_movement.transport.broadcast(None, Bool(value)) + self.module.stop_movement.transport.broadcast(None, Bool(data=value)) def close(self) -> None: for unsub in self._unsubs: diff --git a/dimos/navigation/dannav/holonomic_tc/test_holonomic_path_follower.py b/dimos/navigation/dannav/holonomic_tc/test_holonomic_path_follower.py index 52b5b8093b5..0e90d056520 100644 --- a/dimos/navigation/dannav/holonomic_tc/test_holonomic_path_follower.py +++ b/dimos/navigation/dannav/holonomic_tc/test_holonomic_path_follower.py @@ -24,10 +24,11 @@ import math import time -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Twist import Twist -from dimos.msgs.nav_msgs.Path import Path +from dimos_generated.geometry_msgs.msg import Point, Pose, PoseStamped, Quaternion, Twist +from dimos_generated.nav_msgs.msg import Path +from dimos_generated.std_msgs.msg import Header + +from dimos.msgs.time import time_from_seconds from dimos.navigation.dannav.geometry.path_speed_profile import ( PathSpeedProfileLimits, ) @@ -44,15 +45,13 @@ def _planar_speed_m_s(cmd: Twist) -> float: def _yaw_quaternion(yaw_rad: float) -> Quaternion: - return Quaternion(0.0, 0.0, math.sin(yaw_rad / 2.0), math.cos(yaw_rad / 2.0)) + return Quaternion(x=0.0, y=0.0, z=math.sin(yaw_rad / 2.0), w=math.cos(yaw_rad / 2.0)) def _pose_stamped(x: float, y: float, yaw_rad: float, *, ts: float = 1.0) -> PoseStamped: return PoseStamped( - ts=ts, - frame_id="map", - position=[x, y, 0.0], - orientation=_yaw_quaternion(yaw_rad), + header=Header(stamp=time_from_seconds(ts), frame_id="map"), + pose=Pose(position=Point(x=x, y=y), orientation=_yaw_quaternion(yaw_rad)), ) @@ -66,7 +65,7 @@ def _path_from_points(points: list[tuple[float, float]]) -> Path: prev_point = points[index - 1] yaw = math.atan2(point[1] - prev_point[1], point[0] - prev_point[0]) poses.append(_pose_stamped(point[0], point[1], yaw)) - return Path(frame_id="map", poses=poses) + return Path(header=Header(frame_id="map"), poses=poses) def _make_follower(**overrides: object) -> _HolonomicPathFollower: @@ -129,8 +128,8 @@ def _run_follower( def _on_cmd_vel(cmd: Twist) -> None: nonlocal latest_cmd - latest_cmd = Twist(cmd) - command_history.append(Twist(cmd)) + latest_cmd = Twist.decode(cmd.encode()) + command_history.append(Twist.decode(cmd.encode())) cmd_sub = core.cmd_vel.subscribe(_on_cmd_vel) stop_sub = core.stopped_navigating.subscribe(stop_messages.append) @@ -224,3 +223,18 @@ def test_closed_loop_rotate_first_then_arrives() -> None: for cmd in result.command_history[:20] ) assert abs(result.final_yaw_rad) < 0.35 + + +def test_velocity_uses_nanosecond_interval_and_copies_previous_pose() -> None: + core = _make_follower() + first = _pose_stamped(0.0, 0.0, 0.0, ts=1700000000.0) + second = _pose_stamped(0.0, 0.0, 0.0, ts=1700000000.0) + second.header.stamp.nanosec = 1 + second.pose.position.x = 1e-9 + try: + assert core._estimate_measured_body_twist(first).linear.x == 0.0 + first.pose.position.x = 99.0 + decoded = Twist.decode(core._estimate_measured_body_twist(second).encode()) + assert abs(decoded.linear.x - 1.0) < 1e-12 + finally: + core.close() diff --git a/dimos/navigation/dannav/holonomic_tc/test_holonomic_tracking_controller.py b/dimos/navigation/dannav/holonomic_tc/test_holonomic_tracking_controller.py index 605b91b0509..3050e00b465 100644 --- a/dimos/navigation/dannav/holonomic_tc/test_holonomic_tracking_controller.py +++ b/dimos/navigation/dannav/holonomic_tc/test_holonomic_tracking_controller.py @@ -23,11 +23,9 @@ import math +from dimos_generated.geometry_msgs.msg import Point, Pose, Quaternion, Twist, Vector3 import pytest -from dimos.msgs.geometry_msgs.Pose import Pose -from dimos.msgs.geometry_msgs.Twist import Twist -from dimos.msgs.geometry_msgs.Vector3 import Vector3 from dimos.navigation.dannav.holonomic_tc.command_limits import ( HolonomicCommandLimits, ) @@ -43,13 +41,7 @@ def _pose_xy_yaw(x: float, y: float, yaw: float) -> Pose: return Pose( - x, - y, - 0.0, - 0.0, - 0.0, - math.sin(yaw / 2.0), - math.cos(yaw / 2.0), + position=Point(x=x, y=y), orientation=Quaternion(z=math.sin(yaw / 2), w=math.cos(yaw / 2)) ) @@ -67,7 +59,7 @@ def _planar_speed(cmd: Twist) -> float: def test_tracking_feedforward_frame_and_pose_correction() -> None: origin = _pose_xy_yaw(0.0, 0.0, 0.0) - ref_twist = Twist(linear=Vector3(0.4, -0.1, 0.0), angular=Vector3(0.0, 0.0, 0.05)) + ref_twist = Twist(linear=Vector3(x=0.4, y=-0.1, z=0.0), angular=Vector3(x=0.0, y=0.0, z=0.05)) aligned_pose = _pose_xy_yaw(1.0, -2.0, math.pi / 6) feedforward = HolonomicTrackingController(k_position_per_s=0.0, k_yaw_per_s=0.0) @@ -78,7 +70,7 @@ def test_tracking_feedforward_frame_and_pose_correction() -> None: ref_rotated = _pose_xy_yaw(0.0, 1.0, math.pi / 2.0) frame_out = feedforward.control( - _ref(ref_rotated, Twist(linear=Vector3(0.5, 0.0, 0.0))), + _ref(ref_rotated, Twist(linear=Vector3(x=0.5, y=0.0, z=0.0))), _meas(origin, Twist()), ) assert frame_out.linear.x == pytest.approx(0.0, abs=1e-12) @@ -103,9 +95,13 @@ def test_tracking_feedforward_frame_and_pose_correction() -> None: def test_tracking_damping_is_one_sided_and_respects_limits() -> None: origin = _pose_xy_yaw(0.0, 0.0, 0.0) - ref_twist = Twist(linear=Vector3(0.5, 0.2, 0.0), angular=Vector3(0.0, 0.0, 0.4)) - overspeed_meas = Twist(linear=Vector3(1.1, 0.6, 0.0), angular=Vector3(0.0, 0.0, 0.9)) - underspeed_meas = Twist(linear=Vector3(0.2, -0.1, 0.0), angular=Vector3(0.0, 0.0, 0.1)) + ref_twist = Twist(linear=Vector3(x=0.5, y=0.2, z=0.0), angular=Vector3(x=0.0, y=0.0, z=0.4)) + overspeed_meas = Twist( + linear=Vector3(x=1.1, y=0.6, z=0.0), angular=Vector3(x=0.0, y=0.0, z=0.9) + ) + underspeed_meas = Twist( + linear=Vector3(x=0.2, y=-0.1, z=0.0), angular=Vector3(x=0.0, y=0.0, z=0.1) + ) undamped = HolonomicTrackingController(k_position_per_s=0.0, k_yaw_per_s=0.0) undamped_out = undamped.control(_ref(origin, ref_twist), _meas(origin, overspeed_meas)) @@ -148,3 +144,20 @@ def test_tracking_damping_is_one_sided_and_respects_limits() -> None: ) assert _planar_speed(planar_out) == pytest.approx(max_planar_m_s) assert abs(yaw_out.angular.z) == pytest.approx(max_yaw_rad_s) + + +def test_samples_own_values_and_control_survives_cdr() -> None: + pose = _pose_xy_yaw(1.0, 0.0, 0.0) + twist = Twist(linear=Vector3(x=0.25)) + reference = _ref(Pose.decode(pose.encode()), Twist.decode(twist.encode())) + measurement = _meas(_pose_xy_yaw(0.0, 0.0, math.pi / 2), Twist()) + sample = _ref(pose, twist) + pose.position.x = 99.0 + twist.linear.x = 99.0 + assert sample.pose_plan.position.x == 1.0 + assert sample.twist_body.linear.x == 0.25 + controller = HolonomicTrackingController(k_position_per_s=1.0, k_yaw_per_s=0.0) + output = controller.control(reference, measurement) + decoded = Twist.decode(output.encode()) + assert decoded.linear.x == pytest.approx(0.0, abs=1e-12) + assert decoded.linear.y == pytest.approx(-1.25) diff --git a/dimos/navigation/dannav/holonomic_tc/test_run_envelope.py b/dimos/navigation/dannav/holonomic_tc/test_run_envelope.py index 05482f82b85..33ab9027469 100644 --- a/dimos/navigation/dannav/holonomic_tc/test_run_envelope.py +++ b/dimos/navigation/dannav/holonomic_tc/test_run_envelope.py @@ -23,10 +23,11 @@ import math import time -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Twist import Twist -from dimos.msgs.nav_msgs.Path import Path +from dimos_generated.geometry_msgs.msg import Point, Pose, PoseStamped, Quaternion, Twist +from dimos_generated.nav_msgs.msg import Path +from dimos_generated.std_msgs.msg import Header + +from dimos.msgs.time import time_from_seconds from dimos.navigation.dannav.holonomic_tc.module import ( DanHolonomicTCConfig, _HolonomicPathFollower, @@ -34,15 +35,13 @@ def _yaw_quaternion(yaw_rad: float) -> Quaternion: - return Quaternion(0.0, 0.0, math.sin(yaw_rad / 2.0), math.cos(yaw_rad / 2.0)) + return Quaternion(x=0.0, y=0.0, z=math.sin(yaw_rad / 2.0), w=math.cos(yaw_rad / 2.0)) def _pose_stamped(x: float, y: float, yaw_rad: float, *, ts: float = 1.0) -> PoseStamped: return PoseStamped( - ts=ts, - frame_id="map", - position=[x, y, 0.0], - orientation=_yaw_quaternion(yaw_rad), + header=Header(stamp=time_from_seconds(ts), frame_id="map"), + pose=Pose(position=Point(x=x, y=y), orientation=_yaw_quaternion(yaw_rad)), ) @@ -56,7 +55,7 @@ def _path_from_points(points: list[tuple[float, float]]) -> Path: prev_point = points[index - 1] yaw = math.atan2(point[1] - prev_point[1], point[0] - prev_point[0]) poses.append(_pose_stamped(point[0], point[1], yaw)) - return Path(frame_id="map", poses=poses) + return Path(header=Header(frame_id="map"), poses=poses) def _make_follower(**overrides: object) -> _HolonomicPathFollower: @@ -81,7 +80,7 @@ def _closed_loop_max_planar_speed( def _on_cmd_vel(cmd: Twist) -> None: nonlocal latest_cmd - latest_cmd = Twist(cmd) + latest_cmd = Twist.decode(cmd.encode()) commanded_speeds.append(_planar_speed_m_s(cmd)) cmd_sub = core.cmd_vel.subscribe(_on_cmd_vel) diff --git a/dimos/navigation/dannav/holonomic_tc/types.py b/dimos/navigation/dannav/holonomic_tc/types.py index 20f03897642..f57b849d118 100644 --- a/dimos/navigation/dannav/holonomic_tc/types.py +++ b/dimos/navigation/dannav/holonomic_tc/types.py @@ -34,10 +34,10 @@ from __future__ import annotations +from copy import deepcopy from dataclasses import dataclass -from dimos.msgs.geometry_msgs.Pose import Pose -from dimos.msgs.geometry_msgs.Twist import Twist +from dimos_generated.geometry_msgs.msg import Pose, Twist @dataclass(frozen=True) @@ -49,8 +49,8 @@ class TrajectorySample: twist_body: Twist def __post_init__(self) -> None: - object.__setattr__(self, "pose_plan", Pose(self.pose_plan)) - object.__setattr__(self, "twist_body", Twist(self.twist_body)) + object.__setattr__(self, "pose_plan", deepcopy(self.pose_plan)) + object.__setattr__(self, "twist_body", deepcopy(self.twist_body)) @dataclass(frozen=True) diff --git a/dimos/navigation/dannav/local_planner/module.py b/dimos/navigation/dannav/local_planner/module.py index 9f21003afaa..d0156630c72 100644 --- a/dimos/navigation/dannav/local_planner/module.py +++ b/dimos/navigation/dannav/local_planner/module.py @@ -36,6 +36,8 @@ import math from typing import Any +from dimos_generated.geometry_msgs.msg import PointStamped, PoseStamped +from dimos_generated.nav_msgs.msg import Path import numpy as np from numpy.typing import NDArray from reactivex.disposable import Disposable @@ -44,10 +46,6 @@ from dimos.core.module import Module, ModuleConfig from dimos.core.stream import In, Out from dimos.mapping.occupancy.path_resampling import smooth_resample_path -from dimos.msgs.geometry_msgs.PointStamped import PointStamped -from dimos.msgs.geometry_msgs.Pose import Pose -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.nav_msgs.Path import Path from dimos.navigation.dannav.geometry.path_distancer import PathDistancer from dimos.utils.logging_config import setup_logger @@ -98,7 +96,7 @@ def __init__(self, config: DanLocalPlannerConfig) -> None: self._anchor_progress_m: float = 0.0 def on_odom(self, odom: PoseStamped) -> None: - self._robot_xy = np.array([odom.position.x, odom.position.y], dtype=float) + self._robot_xy = np.array([odom.pose.position.x, odom.pose.position.y], dtype=float) def on_goal(self, goal: PointStamped) -> None: """Arm or disarm the fresh-click commit. @@ -108,11 +106,11 @@ def on_goal(self, goal: PointStamped) -> None: drops the committed path). ``DanLocalPlanner`` does not touch ``stop_movement``. """ - if math.isnan(goal.x) or math.isnan(goal.y): + if math.isnan(goal.point.x) or math.isnan(goal.point.y): self._armed_goal = None self._drop_committed() return - self._armed_goal = np.array([goal.x, goal.y], dtype=float) + self._armed_goal = np.array([goal.point.x, goal.point.y], dtype=float) def on_planner_path(self, path: Path) -> Path | None: """Return the path to forward, or ``None`` to suppress it. @@ -171,7 +169,7 @@ def _smooth(self, path: Path) -> Path: if self._cfg.resample_spacing_m <= 0.0 or len(path.poses) == 0: return path last = path.poses[-1] - goal_pose = Pose(last.position, last.orientation) + goal_pose = last.pose return smooth_resample_path( path, goal_pose, self._cfg.resample_spacing_m, self._cfg.smoothing_window ) @@ -201,7 +199,10 @@ def _progress_on_committed(self) -> float: def _ends_near(path: Path, goal_xy: NDArray[np.float64], tolerance_m: float) -> bool: """True when ``path``'s last pose is within ``tolerance_m`` of ``goal_xy``.""" end = path.poses[-1] - return float(math.hypot(end.x - goal_xy[0], end.y - goal_xy[1])) <= tolerance_m + return ( + float(math.hypot(end.pose.position.x - goal_xy[0], end.pose.position.y - goal_xy[1])) + <= tolerance_m + ) class DanLocalPlanner(Module): diff --git a/dimos/navigation/dannav/local_planner/test_dan_local_planner.py b/dimos/navigation/dannav/local_planner/test_dan_local_planner.py index c0a5d3dad4a..88da5610fd9 100644 --- a/dimos/navigation/dannav/local_planner/test_dan_local_planner.py +++ b/dimos/navigation/dannav/local_planner/test_dan_local_planner.py @@ -19,9 +19,11 @@ import math from typing import Any -from dimos.msgs.geometry_msgs.PointStamped import PointStamped -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.nav_msgs.Path import Path +from dimos_generated.geometry_msgs.msg import Point, PointStamped, Pose, PoseStamped, Quaternion +from dimos_generated.nav_msgs.msg import Path +from dimos_generated.std_msgs.msg import Header + +from dimos.msgs.time import time_from_seconds from dimos.navigation.dannav.local_planner.module import ( DanLocalPlannerConfig, _ReplanGate, @@ -29,26 +31,21 @@ def _path_from_points(points: list[tuple[float, float]]) -> Path: - poses = [ - PoseStamped( - ts=1.0, frame_id="world", position=[x, y, 0.0], orientation=[0.0, 0.0, 0.0, 1.0] - ) - for x, y in points - ] - return Path(frame_id="world", poses=poses) + value = Path(header=Header(frame_id="world"), poses=[_odom(x, y) for x, y in points]) + return Path.decode(value.encode()) def _odom(x: float, y: float, *, ts: float = 1.0) -> PoseStamped: - return PoseStamped( - ts=ts, - frame_id="world", - position=[x, y, 0.0], - orientation=[0.0, 0.0, 0.0, 1.0], + value = PoseStamped( + header=Header(stamp=time_from_seconds(ts), frame_id="world"), + pose=Pose(position=Point(x=x, y=y), orientation=Quaternion(w=1.0)), ) + return PoseStamped.decode(value.encode()) def _point(x: float, y: float) -> PointStamped: - return PointStamped(x=x, y=y, z=0.0, frame_id="world") + value = PointStamped(point=Point(x=x, y=y), header=Header(frame_id="world")) + return PointStamped.decode(value.encode()) def _gate(**config: Any) -> _ReplanGate: @@ -110,7 +107,7 @@ def test_empty_path_published_and_resets_gate() -> None: # An empty path (nothing safe ahead) forwards immediately as a stop and # drops the committed path. - empty = Path(frame_id="world", poses=[]) + empty = Path(header=Header(frame_id="world"), poses=[]) assert gate.on_planner_path(empty) is empty assert gate._committed is None @@ -145,3 +142,21 @@ def test_lock_replan_zero_commits_every_non_empty_path() -> None: _path_from_points([(0.0, 0.0), (1.0, 1.0)]), ): assert gate.on_planner_path(path) is not None + + +def test_smoothing_preserves_cdr_header_and_endpoints() -> None: + gate = _gate(resample_spacing_m=0.1, smoothing_window=3) + path = _path_from_points([(0.0, 0.0), (0.5, 0.5), (1.0, 0.0)]) + path.header.stamp.sec = 1700000000 + path.header.stamp.nanosec = 123456789 + output = gate.on_planner_path(path) + assert output is not None + decoded = Path.decode(output.encode()) + assert decoded.header.frame_id == "world" + assert decoded.header.stamp.sec == 1700000000 + assert decoded.header.stamp.nanosec == 123456789 + assert len(decoded.poses) > len(path.poses) + assert decoded.poses[0].pose.position.x == 0.0 + assert decoded.poses[-1].pose.position.x == 1.0 + assert decoded.poses[-1].pose.position.y == 0.0 + assert path.poses[1].pose.position.y == 0.5 diff --git a/dimos/navigation/frontier_exploration/test_generated_frontiers.py b/dimos/navigation/frontier_exploration/test_generated_frontiers.py new file mode 100644 index 00000000000..a1cbc366f96 --- /dev/null +++ b/dimos/navigation/frontier_exploration/test_generated_frontiers.py @@ -0,0 +1,102 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import math + +from dimos_generated.builtin_interfaces.msg import Time +from dimos_generated.geometry_msgs.msg import Point, Pose, PoseStamped, Quaternion +from dimos_generated.nav_msgs.msg import MapMetaData, OccupancyGrid +from dimos_generated.std_msgs.msg import Header +import numpy as np +import pytest + +from dimos.msgs.geometry import quaternion_from_euler +from dimos.msgs.occupancy import grid_to_world +from dimos.navigation.frontier_exploration.wavefront_frontier_goal_selector import ( + WavefrontFrontierExplorer, +) + + +@pytest.mark.parametrize("yaw", [0, math.pi / 2]) +def test_generated_frontier_centroid_respects_map_origin(request, yaw): + explorer = WavefrontFrontierExplorer(min_frontier_perimeter=0.1) + request.addfinalizer(explorer.stop) + cells = np.full((20, 20), -1, dtype=np.int8) + cells[5:15, 5:15] = 0 + grid = OccupancyGrid( + header=Header(frame_id="map"), + info=MapMetaData( + width=20, + height=20, + resolution=0.5, + origin=Pose( + position=Point(x=2, y=3, z=1), orientation=quaternion_from_euler(0, 0, yaw) + ), + ), + data=cells.ravel(), + ) + received = OccupancyGrid.decode(grid.encode()) + centroids = explorer.detect_frontiers(grid_to_world(received, (10, 10)), received) + assert len(centroids) == 1 + expected = grid_to_world(received, (9.5, 9.5)) + assert (centroids[0].x, centroids[0].y, centroids[0].z) == pytest.approx( + (expected.x, expected.y, expected.z) + ) + + +def test_stopping_exploration_retains_current_pose_header(request): + explorer = WavefrontFrontierExplorer() + request.addfinalizer(explorer.stop) + source = PoseStamped( + header=Header(frame_id="odom", stamp=Time(sec=1700000000, nanosec=123456789)), + pose=Pose(position=Point(x=2, y=3), orientation=Quaternion(w=1)), + ) + explorer.latest_odometry = source + explorer.exploration_active = True + goals = [] + explorer.goal_request.subscribe(lambda value: goals.append(PoseStamped.decode(value.encode()))) + assert explorer.stop_exploration() + assert goals == [source] + source.pose.position.x = 100 + assert goals[0].pose.position.x == 2 + + +def test_exploration_loop_publishes_generated_goal_with_map_header(request): + explorer = WavefrontFrontierExplorer(goal_timeout=0.001) + request.addfinalizer(explorer.stop) + cells = np.full((20, 20), -1, dtype=np.int8) + cells[:, :10] = 0 + header = Header(frame_id="odom", stamp=Time(sec=1700000000, nanosec=123456789)) + explorer.latest_costmap = OccupancyGrid( + header=header, + info=MapMetaData( + width=20, height=20, resolution=0.5, origin=Pose(orientation=Quaternion(w=1)) + ), + data=cells.ravel(), + ) + explorer.latest_odometry = PoseStamped( + header=header, pose=Pose(position=Point(x=2, y=5), orientation=Quaternion(w=1)) + ) + goals = [] + + def receive(value): + goals.append(PoseStamped.decode(value.encode())) + explorer.exploration_active = False + + explorer.goal_request.subscribe(receive) + explorer.exploration_active = True + explorer._run_exploration_loop() + assert len(goals) == 1 + assert goals[0].header == header + assert goals[0].pose.position.x == 5 diff --git a/dimos/navigation/frontier_exploration/wavefront_frontier_goal_selector.py b/dimos/navigation/frontier_exploration/wavefront_frontier_goal_selector.py index 93af3b398ab..f42128b3c32 100644 --- a/dimos/navigation/frontier_exploration/wavefront_frontier_goal_selector.py +++ b/dimos/navigation/frontier_exploration/wavefront_frontier_goal_selector.py @@ -25,7 +25,9 @@ import threading from typing import Any -from dimos_lcm.std_msgs import Bool +from dimos_generated.geometry_msgs.msg import Point, Pose, PoseStamped, Quaternion, Vector3 +from dimos_generated.nav_msgs.msg import OccupancyGrid +from dimos_generated.std_msgs.msg import Bool import numpy as np from reactivex.disposable import Disposable @@ -36,11 +38,9 @@ from dimos.core.module import Module, ModuleConfig from dimos.core.stream import In, Out from dimos.mapping.occupancy.inflation import simple_inflate -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.msgs.nav_msgs.OccupancyGrid import CostValues, OccupancyGrid +from dimos.msgs.geometry import point_distance +from dimos.msgs.occupancy import grid_to_world, occupancy_view, world_to_grid from dimos.utils.logging_config import setup_logger -from dimos.utils.transform_utils import get_distance logger = setup_logger() @@ -110,7 +110,7 @@ class WavefrontFrontierExplorer(Module): config: WavefrontConfig - # LCM inputs + # Typed inputs global_costmap: In[OccupancyGrid] odom: In[PoseStamped] goal_reached: In[Bool] @@ -135,7 +135,7 @@ def __init__(self, **kwargs: Any) -> None: super().__init__(**kwargs) self._cache = FrontierCache() self.explored_goals = [] # type: ignore[var-annotated] # list of explored goals - self.exploration_direction = Vector3(0.0, 0.0, 0.0) # current exploration direction + self.exploration_direction = Vector3(x=0.0, y=0.0, z=0.0) # current exploration direction self.last_costmap = None # store last costmap for information comparison self.no_gain_counter = 0 # track consecutive no-gain attempts @@ -223,8 +223,8 @@ def _count_costmap_information(self, costmap: OccupancyGrid) -> int: Returns: Number of cells that are free space or obstacles (not unknown) """ - free_count = np.sum(costmap.grid == CostValues.FREE) - obstacle_count = np.sum(costmap.grid >= self.config.occupancy_threshold) + free_count = np.sum(occupancy_view(costmap) == 0) + obstacle_count = np.sum(occupancy_view(costmap) >= self.config.occupancy_threshold) return int(free_count + obstacle_count) def _get_neighbors(self, point: GridPoint, costmap: OccupancyGrid) -> list[GridPoint]: @@ -240,7 +240,7 @@ def _get_neighbors(self, point: GridPoint, costmap: OccupancyGrid) -> list[GridP nx, ny = point.x + dx, point.y + dy # Check bounds - if 0 <= nx < costmap.width and 0 <= ny < costmap.height: + if 0 <= nx < costmap.info.width and 0 <= ny < costmap.info.height: neighbors.append(self._cache.get_point(nx, ny)) return neighbors @@ -252,21 +252,21 @@ def _is_frontier_point(self, point: GridPoint, costmap: OccupancyGrid) -> bool: and not adjacent to any occupied cells. """ # Point must be unknown - cost = costmap.grid[point.y, point.x] - if cost != CostValues.UNKNOWN: + cost = occupancy_view(costmap)[point.y, point.x] + if cost != -1: return False has_free = False for neighbor in self._get_neighbors(point, costmap): - neighbor_cost = costmap.grid[neighbor.y, neighbor.x] + neighbor_cost = occupancy_view(costmap)[neighbor.y, neighbor.x] # If adjacent to occupied space, not a frontier if neighbor_cost > self.config.occupancy_threshold: return False # Check if adjacent to free space - if neighbor_cost == CostValues.FREE: + if neighbor_cost == 0: has_free = True return has_free @@ -288,7 +288,7 @@ def _find_free_space( visited.add((point.x, point.y)) # Check if this point is free space - if costmap.grid[point.y, point.x] == CostValues.FREE: + if occupancy_view(costmap)[point.y, point.x] == 0: return (point.x, point.y) # Add neighbors to search @@ -299,18 +299,18 @@ def _find_free_space( # If no free space found, return original position return (start_x, start_y) - def _compute_centroid(self, frontier_points: list[Vector3]) -> Vector3: + def _compute_centroid(self, frontier_points: list[Point]) -> Point: """Compute the centroid of a list of frontier points.""" if not frontier_points: - return Vector3(0.0, 0.0, 0.0) + return Point(x=0.0, y=0.0, z=0.0) # Vectorized approach using numpy - points_array = np.array([[point.x, point.y] for point in frontier_points]) + points_array = np.array([[point.x, point.y, point.z] for point in frontier_points]) centroid = np.mean(points_array, axis=0) - return Vector3(centroid[0], centroid[1], 0.0) + return Point(x=centroid[0], y=centroid[1], z=centroid[2]) - def detect_frontiers(self, robot_pose: Vector3, costmap: OccupancyGrid) -> list[Vector3]: + def detect_frontiers(self, robot_pose: Point, costmap: OccupancyGrid) -> list[Point]: """ Main frontier detection algorithm using wavefront exploration. @@ -324,8 +324,8 @@ def detect_frontiers(self, robot_pose: Vector3, costmap: OccupancyGrid) -> list[ self._cache.clear() # Convert robot pose to grid coordinates - grid_pos = costmap.world_to_grid(robot_pose) - grid_x, grid_y = int(grid_pos.x), int(grid_pos.y) + grid_pos = world_to_grid(costmap, robot_pose) + grid_x, grid_y = int(np.floor(round(grid_pos[0], 9))), int(np.floor(round(grid_pos[1], 9))) # Find nearest free space to start exploration free_x, free_y = self._find_free_space(grid_x, grid_y, costmap) @@ -386,13 +386,13 @@ def detect_frontiers(self, robot_pose: Vector3, costmap: OccupancyGrid) -> list[ # Check if we found a large enough frontier # Convert minimum perimeter to minimum number of cells based on resolution - min_cells = int(self.config.min_frontier_perimeter / costmap.resolution) + min_cells = max( + 1, int(self.config.min_frontier_perimeter / costmap.info.resolution) + ) if len(new_frontier) >= min_cells: world_points = [] for point in new_frontier: - world_pos = costmap.grid_to_world( - Vector3(float(point.x), float(point.y), 0.0) - ) + world_pos = grid_to_world(costmap, (float(point.x), float(point.y))) world_points.append(world_pos) # Compute centroid in world coordinates (already correctly scaled) @@ -407,10 +407,10 @@ def detect_frontiers(self, robot_pose: Vector3, costmap: OccupancyGrid) -> list[ & (PointClassification.MapOpen | PointClassification.MapClosed) ): # Check if neighbor is free space or unknown (explorable) - neighbor_cost = costmap.grid[neighbor.y, neighbor.x] + neighbor_cost = occupancy_view(costmap)[neighbor.y, neighbor.x] # Add free space and unknown space to exploration queue - if neighbor_cost == CostValues.FREE or neighbor_cost == CostValues.UNKNOWN: + if neighbor_cost == 0 or neighbor_cost == -1: neighbor.classification |= PointClassification.MapOpen map_queue.append(neighbor) @@ -428,25 +428,27 @@ def detect_frontiers(self, robot_pose: Vector3, costmap: OccupancyGrid) -> list[ return ranked_frontiers def _update_exploration_direction( - self, robot_pose: Vector3, goal_pose: Vector3 | None = None + self, robot_pose: Point, goal_pose: Point | None = None ) -> None: """Update the current exploration direction based on robot movement or selected goal.""" if goal_pose is not None: # Calculate direction from robot to goal - direction = Vector3(goal_pose.x - robot_pose.x, goal_pose.y - robot_pose.y, 0.0) + direction = Vector3(x=goal_pose.x - robot_pose.x, y=goal_pose.y - robot_pose.y, z=0.0) magnitude = np.sqrt(direction.x**2 + direction.y**2) if magnitude > 0.1: # Avoid division by zero for very close goals self.exploration_direction = Vector3( - direction.x / magnitude, direction.y / magnitude, 0.0 + x=direction.x / magnitude, y=direction.y / magnitude, z=0.0 ) - def _compute_direction_momentum_score(self, frontier: Vector3, robot_pose: Vector3) -> float: + def _compute_direction_momentum_score(self, frontier: Point, robot_pose: Point) -> float: """Compute direction momentum score for a frontier.""" if self.exploration_direction.x == 0 and self.exploration_direction.y == 0: return 0.0 # No momentum if no previous direction # Calculate direction from robot to frontier - frontier_direction = Vector3(frontier.x - robot_pose.x, frontier.y - robot_pose.y, 0.0) + frontier_direction = Vector3( + x=frontier.x - robot_pose.x, y=frontier.y - robot_pose.y, z=0.0 + ) magnitude = np.sqrt(frontier_direction.x**2 + frontier_direction.y**2) if magnitude < 0.1: @@ -454,7 +456,7 @@ def _compute_direction_momentum_score(self, frontier: Vector3, robot_pose: Vecto # Normalize frontier direction frontier_direction = Vector3( - frontier_direction.x / magnitude, frontier_direction.y / magnitude, 0.0 + x=frontier_direction.x / magnitude, y=frontier_direction.y / magnitude, z=0.0 ) # Calculate dot product for directional alignment @@ -466,7 +468,7 @@ def _compute_direction_momentum_score(self, frontier: Vector3, robot_pose: Vecto # Return momentum score (higher for same direction, lower for opposite) return max(0.0, dot_product) # Only positive momentum, no penalty for different directions - def _compute_distance_to_explored_goals(self, frontier: Vector3) -> float: + def _compute_distance_to_explored_goals(self, frontier: Point) -> float: """Compute distance from frontier to the nearest explored goal.""" if not self.explored_goals: return 5.0 # Default consistent value when no explored goals @@ -478,7 +480,7 @@ def _compute_distance_to_explored_goals(self, frontier: Vector3) -> float: return min_distance - def _compute_distance_to_obstacles(self, frontier: Vector3, costmap: OccupancyGrid) -> float: + def _compute_distance_to_obstacles(self, frontier: Point, costmap: OccupancyGrid) -> float: """ Compute the minimum distance from a frontier point to the nearest obstacle. @@ -490,16 +492,21 @@ def _compute_distance_to_obstacles(self, frontier: Vector3, costmap: OccupancyGr Minimum distance to nearest obstacle in meters """ # Convert frontier to grid coordinates - grid_pos = costmap.world_to_grid(frontier) - grid_x, grid_y = int(grid_pos.x), int(grid_pos.y) + grid_pos = world_to_grid(costmap, frontier) + grid_x, grid_y = int(np.floor(round(grid_pos[0], 9))), int(np.floor(round(grid_pos[1], 9))) # Check if frontier is within costmap bounds - if grid_x < 0 or grid_x >= costmap.width or grid_y < 0 or grid_y >= costmap.height: + if ( + grid_x < 0 + or grid_x >= costmap.info.width + or grid_y < 0 + or grid_y >= costmap.info.height + ): return 0.0 # Consider out-of-bounds as obstacle min_distance = float("inf") search_radius = ( - int(self.config.safe_distance / costmap.resolution) + 5 + int(self.config.safe_distance / costmap.info.resolution) + 5 ) # Search a bit beyond minimum # Search in a square around the frontier point @@ -511,16 +518,16 @@ def _compute_distance_to_obstacles(self, frontier: Vector3, costmap: OccupancyGr # Skip if out of bounds if ( check_x < 0 - or check_x >= costmap.width + or check_x >= costmap.info.width or check_y < 0 - or check_y >= costmap.height + or check_y >= costmap.info.height ): continue # Check if this cell is an obstacle - if costmap.grid[check_y, check_x] >= self.config.occupancy_threshold: + if occupancy_view(costmap)[check_y, check_x] >= self.config.occupancy_threshold: # Calculate distance in meters - distance = np.sqrt(dx**2 + dy**2) * costmap.resolution + distance = np.sqrt(dx**2 + dy**2) * costmap.info.resolution min_distance = min(min_distance, distance) # If no obstacles found within search radius, return the safe distance @@ -528,12 +535,12 @@ def _compute_distance_to_obstacles(self, frontier: Vector3, costmap: OccupancyGr return min_distance if min_distance != float("inf") else self.config.safe_distance def _compute_comprehensive_frontier_score( - self, frontier: Vector3, frontier_size: int, robot_pose: Vector3, costmap: OccupancyGrid + self, frontier: Point, frontier_size: int, robot_pose: Point, costmap: OccupancyGrid ) -> float: """Compute comprehensive score considering multiple criteria.""" # 1. Distance from robot (preference for moderate distances) - robot_distance = get_distance(frontier, robot_pose) + robot_distance = point_distance(frontier, robot_pose) # Distance score: prefer moderate distances (not too close, not too far) # Normalized to 0-1 range @@ -541,7 +548,9 @@ def _compute_comprehensive_frontier_score( # 2. Information gain (frontier size) # Normalize by a reasonable max frontier size - max_expected_frontier_size = self.config.min_frontier_perimeter / costmap.resolution * 10 + max_expected_frontier_size = ( + self.config.min_frontier_perimeter / costmap.info.resolution * 10 + ) info_gain_score = min(frontier_size / max_expected_frontier_size, 1.0) # 3. Distance to explored goals (bonus for being far from explored areas) @@ -577,11 +586,11 @@ def _compute_comprehensive_frontier_score( def _rank_frontiers( self, - frontier_centroids: list[Vector3], + frontier_centroids: list[Point], frontier_sizes: list[int], - robot_pose: Vector3, + robot_pose: Point, costmap: OccupancyGrid, - ) -> list[Vector3]: + ) -> list[Point]: """ Find the single best frontier using comprehensive scoring and filtering. @@ -619,7 +628,7 @@ def _rank_frontiers( # Extract just the frontiers (remove scores) and return as list return [frontier for frontier, _ in valid_frontiers] - def get_exploration_goal(self, robot_pose: Vector3, costmap: OccupancyGrid) -> Vector3 | None: + def get_exploration_goal(self, robot_pose: Point, costmap: OccupancyGrid) -> Point | None: """ Get the single best exploration goal using comprehensive frontier scoring. @@ -683,7 +692,7 @@ def get_exploration_goal(self, robot_pose: Vector3, costmap: OccupancyGrid) -> V self.last_costmap = costmap # type: ignore[assignment] return None - def mark_explored_goal(self, goal: Vector3) -> None: + def mark_explored_goal(self, goal: Point) -> None: """Mark a goal as explored.""" self.explored_goals.append(goal) @@ -695,7 +704,7 @@ def reset_exploration_session(self) -> None: needs to forget its previous exploration history. """ self.explored_goals.clear() # Clear all previously explored goals - self.exploration_direction = Vector3(0.0, 0.0, 0.0) # Reset exploration direction + self.exploration_direction = Vector3(x=0.0, y=0.0, z=0.0) # Reset exploration direction self.last_costmap = None # Clear last costmap comparison self.no_gain_counter = 0 # Reset no-gain attempt counter self._cache.clear() # Clear frontier point cache @@ -749,12 +758,7 @@ def stop_exploration(self) -> bool: # Publish current location as goal to stop the robot. if self.latest_odometry is not None: - goal = PoseStamped( - position=self.latest_odometry.position, - orientation=self.latest_odometry.orientation, - frame_id="world", - ts=self.latest_odometry.ts, - ) + goal = PoseStamped(header=self.latest_odometry.header, pose=self.latest_odometry.pose) self.goal_request.publish(goal) logger.info("Stopped autonomous frontier exploration") @@ -791,8 +795,10 @@ def _run_exploration_loop(self) -> None: continue # Get robot pose from odometry - robot_pose = Vector3( - self.latest_odometry.position.x, self.latest_odometry.position.y, 0.0 + robot_pose = Point( + x=self.latest_odometry.pose.position.x, + y=self.latest_odometry.pose.position.y, + z=0.0, ) # Get exploration goal @@ -801,13 +807,10 @@ def _run_exploration_loop(self) -> None: if goal: # Publish goal to navigator - goal_msg = PoseStamped() - goal_msg.position.x = goal.x - goal_msg.position.y = goal.y - goal_msg.position.z = 0.0 - goal_msg.orientation.w = 1.0 # No rotation - goal_msg.frame_id = "world" - goal_msg.ts = self.latest_costmap.ts + goal_msg = PoseStamped( + header=self.latest_costmap.header, + pose=Pose(position=goal, orientation=Quaternion(w=1)), + ) self.goal_request.publish(goal_msg) logger.info(f"Published frontier goal: ({goal.x:.2f}, {goal.y:.2f})") diff --git a/dimos/navigation/movement_manager/movement_manager.py b/dimos/navigation/movement_manager/movement_manager.py index 4f83428f9a7..da3cce4fed6 100644 --- a/dimos/navigation/movement_manager/movement_manager.py +++ b/dimos/navigation/movement_manager/movement_manager.py @@ -25,15 +25,14 @@ import time from typing import Any -from dimos_lcm.std_msgs import Bool +from dimos_generated.geometry_msgs.msg import Point, PointStamped, Twist, Vector3 +from dimos_generated.std_msgs.msg import Bool from reactivex.disposable import Disposable from dimos.core.core import rpc from dimos.core.module import Module, ModuleConfig from dimos.core.stream import In, Out -from dimos.msgs.geometry_msgs.PointStamped import PointStamped -from dimos.msgs.geometry_msgs.Twist import Twist -from dimos.msgs.geometry_msgs.Vector3 import Vector3 +from dimos.msgs.time import header_now from dimos.utils.logging_config import setup_logger logger = setup_logger() @@ -45,7 +44,9 @@ class MovementManagerConfig(ModuleConfig): tele_cooldown_sec: float = 1.0 - tele_cmd_vel_scaling: Twist = Twist(Vector3(1, 1, 1), Vector3(1, 1, 1)) + tele_cmd_vel_scaling: Twist = Twist( + linear=Vector3(x=1, y=1, z=1), angular=Vector3(x=1, y=1, z=1) + ) class MovementManager(Module): @@ -82,18 +83,22 @@ def stop(self) -> None: super().stop() def _on_click(self, msg: PointStamped) -> None: - if not all(math.isfinite(v) for v in (msg.x, msg.y, msg.z)): - logger.warning("Ignored invalid click", x=msg.x, y=msg.y, z=msg.z) + if not all(math.isfinite(v) for v in (msg.point.x, msg.point.y, msg.point.z)): + logger.warning("Ignored invalid click", x=msg.point.x, y=msg.point.y, z=msg.point.z) return if ( - abs(msg.x) > MAX_CLICK_HORIZONTAL_M - or abs(msg.y) > MAX_CLICK_HORIZONTAL_M - or abs(msg.z) > MAX_CLICK_VERTICAL_M + abs(msg.point.x) > MAX_CLICK_HORIZONTAL_M + or abs(msg.point.y) > MAX_CLICK_HORIZONTAL_M + or abs(msg.point.z) > MAX_CLICK_VERTICAL_M ): - logger.warning("Ignored out-of-range click", x=msg.x, y=msg.y, z=msg.z) + logger.warning( + "Ignored out-of-range click", x=msg.point.x, y=msg.point.y, z=msg.point.z + ) return - logger.debug("Goal", x=round(msg.x, 1), y=round(msg.y, 1), z=round(msg.z, 1)) + logger.debug( + "Goal", x=round(msg.point.x, 1), y=round(msg.point.y, 1), z=round(msg.point.z, 1) + ) self.way_point.publish(msg) self.goal.publish(msg) @@ -103,7 +108,7 @@ def _cancel_goal(self) -> None: # It can be REALLY bad if a robot is supposed to stop moving but wont # we should probably think a more robust/strict requirement on planners cancel = PointStamped( - ts=time.time(), frame_id="map", x=float("nan"), y=float("nan"), z=float("nan") + header=header_now("map"), point=Point(x=float("nan"), y=float("nan"), z=float("nan")) ) self.way_point.publish(cancel) self.goal.publish(cancel) @@ -129,14 +134,14 @@ def _on_teleop(self, msg: Twist) -> None: scale = self.config.tele_cmd_vel_scaling scaled = Twist( linear=Vector3( - msg.linear.x * scale.linear.x, - msg.linear.y * scale.linear.y, - msg.linear.z * scale.linear.z, + x=msg.linear.x * scale.linear.x, + y=msg.linear.y * scale.linear.y, + z=msg.linear.z * scale.linear.z, ), angular=Vector3( - msg.angular.x * scale.angular.x, - msg.angular.y * scale.angular.y, - msg.angular.z * scale.angular.z, + x=msg.angular.x * scale.angular.x, + y=msg.angular.y * scale.angular.y, + z=msg.angular.z * scale.angular.z, ), ) self.cmd_vel.publish(scaled) diff --git a/dimos/navigation/movement_manager/test_movement_manager.py b/dimos/navigation/movement_manager/test_movement_manager.py index e266ad3e983..0c860e0d2ad 100644 --- a/dimos/navigation/movement_manager/test_movement_manager.py +++ b/dimos/navigation/movement_manager/test_movement_manager.py @@ -19,12 +19,11 @@ import math import time +from dimos_generated.geometry_msgs.msg import Point, PointStamped, Twist, Vector3 import pytest from dimos.constants import DEFAULT_THREAD_JOIN_TIMEOUT -from dimos.msgs.geometry_msgs.PointStamped import PointStamped -from dimos.msgs.geometry_msgs.Twist import Twist -from dimos.msgs.geometry_msgs.Vector3 import Vector3 +from dimos.msgs.time import header_now from dimos.navigation.movement_manager.movement_manager import ( MovementManager, ) @@ -44,10 +43,16 @@ def _attach(module): """Subscribe to every Out port; return (captured, unsubscribers).""" captured = Captured() unsubs = [ - module.cmd_vel.subscribe(captured.cmd_vel.append), - module.stop_movement.subscribe(captured.stop_movement.append), - module.goal.subscribe(captured.goal.append), - module.way_point.subscribe(captured.way_point.append), + module.cmd_vel.subscribe( + lambda msg: captured.cmd_vel.append(type(msg).decode(msg.encode())) + ), + module.stop_movement.subscribe( + lambda msg: captured.stop_movement.append(type(msg).decode(msg.encode())) + ), + module.goal.subscribe(lambda msg: captured.goal.append(type(msg).decode(msg.encode()))), + module.way_point.subscribe( + lambda msg: captured.way_point.append(type(msg).decode(msg.encode())) + ), ] return captured, unsubs @@ -65,11 +70,11 @@ def manager_and_captured() -> Generator[tuple[MovementManager, Captured], None, def _twist(lx=0.0): - return Twist(linear=Vector3(lx, 0, 0), angular=Vector3(0, 0, 0)) + return Twist(linear=Vector3(x=lx, y=0, z=0), angular=Vector3(x=0, y=0, z=0)) def _click(x=1.0, y=2.0, z=0.0): - return PointStamped(ts=time.time(), frame_id="map", x=x, y=y, z=z) + return PointStamped(header=header_now("map"), point=Point(x=x, y=y, z=z)) def test_teleop_suppresses_nav_and_cancels_goal(manager_and_captured): @@ -88,7 +93,7 @@ def test_teleop_suppresses_nav_and_cancels_goal(manager_and_captured): # Goal cancelled with NaN assert len(captured.goal) == 1 - assert math.isnan(captured.goal[0].x) + assert math.isnan(captured.goal[0].point.x) def test_nav_resumes_after_cooldown(manager_and_captured): @@ -127,11 +132,11 @@ def test_invalid_clicks_rejected(manager_and_captured): def test_tele_cmd_vel_scaling(manager_and_captured): """tele_cmd_vel_scaling multiplies each teleop twist component independently.""" manager, captured = manager_and_captured - scaling = Twist(Vector3(0.5, 2.0, 0.0), Vector3(1.0, 1.0, 0.25)) + scaling = Twist(linear=Vector3(x=0.5, y=2.0, z=0.0), angular=Vector3(x=1.0, y=1.0, z=0.25)) manager.config.tele_cmd_vel_scaling = scaling manager.config.tele_cooldown_sec = 10.0 - manager._on_teleop(Twist(Vector3(1, 1, 1), Vector3(1, 1, 1))) + manager._on_teleop(Twist(linear=Vector3(x=1, y=1, z=1), angular=Vector3(x=1, y=1, z=1))) assert len(captured.cmd_vel) == 1 published = captured.cmd_vel[0] diff --git a/dimos/navigation/nav_3d/evaluator/cases.py b/dimos/navigation/nav_3d/evaluator/cases.py index 130063e2d5d..a1a0755f8dc 100644 --- a/dimos/navigation/nav_3d/evaluator/cases.py +++ b/dimos/navigation/nav_3d/evaluator/cases.py @@ -20,6 +20,8 @@ from pathlib import Path from typing import TYPE_CHECKING +from dimos_generated.sensor_msgs.msg import PointCloud2 + from dimos.constants import DIMOS_PROJECT_ROOT from dimos.utils.data import resolve_named_path @@ -72,8 +74,8 @@ def trajectory(self) -> Trajectory: def frame_count(self) -> int: """Upper bound on the frames world_frames yields, for a progress bar.""" + from dimos.memory.store.sqlite import SqliteStore - from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 store = SqliteStore(path=str(self.db_path())) with store: diff --git a/dimos/navigation/nav_3d/evaluator/recording.py b/dimos/navigation/nav_3d/evaluator/recording.py index cb06d94d6d6..59485e150fc 100644 --- a/dimos/navigation/nav_3d/evaluator/recording.py +++ b/dimos/navigation/nav_3d/evaluator/recording.py @@ -19,14 +19,13 @@ from dataclasses import dataclass from typing import TYPE_CHECKING +from dimos_generated.nav_msgs.msg import Odometry +from dimos_generated.sensor_msgs.msg import PointCloud2 import numpy as np from dimos.memory.store.sqlite import SqliteStore -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Transform import Transform -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.msgs.nav_msgs.Odometry import Odometry -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 +from dimos.msgs.geometry import pose_matrix +from dimos.msgs.pointcloud import pointcloud_xyz from dimos.navigation.nav_3d.evaluator.metrics import arc_lengths if TYPE_CHECKING: @@ -79,25 +78,24 @@ def iter_world_frames( odom = store.stream(odom_stream, Odometry).order_by("ts") for pair_obs in lidar.align(odom, tolerance=align_tol): lidar_obs, odom_obs = pair_obs.data - if lidar_obs.data.frame_id == "world": + if lidar_obs.data.header.frame_id == "world": raise ValueError( f"{db_path}: stream {lidar_stream!r} has pre-registered world-frame " "clouds; this legacy format is not supported for evaluation" ) o = odom_obs.data - mat = Transform( - translation=Vector3(o.position.x, o.position.y, o.position.z), - rotation=Quaternion( - o.orientation.x, o.orientation.y, o.orientation.z, o.orientation.w - ), - ).to_matrix() + mat = pose_matrix(o.pose.pose) rot = mat[:3, :3].astype(np.float32) trans = mat[:3, 3].astype(np.float32) - pts = lidar_obs.data.points_f32() @ rot.T + trans + pts = pointcloud_xyz(lidar_obs.data).astype(np.float32) @ rot.T + trans yield Frame( ts=lidar_obs.ts, points=pts, - origin=(float(o.position.x), float(o.position.y), float(o.position.z)), + origin=( + float(o.pose.pose.position.x), + float(o.pose.pose.position.y), + float(o.pose.pose.position.z), + ), ) @@ -109,7 +107,13 @@ def load_trajectory(db_path: Path, odom_stream: str) -> Trajectory: for obs in store.stream(odom_stream, Odometry).order_by("ts"): o = obs.data ts.append(obs.ts) - positions.append((float(o.position.x), float(o.position.y), float(o.position.z))) + positions.append( + ( + float(o.pose.pose.position.x), + float(o.pose.pose.position.y), + float(o.pose.pose.position.z), + ) + ) if not positions: raise ValueError(f"{db_path}: no odometry in stream {odom_stream!r}") return Trajectory( diff --git a/dimos/navigation/nav_3d/evaluator/test_nav_eval.py b/dimos/navigation/nav_3d/evaluator/test_nav_eval.py index d29ae0bd0f7..2a9e980ff14 100644 --- a/dimos/navigation/nav_3d/evaluator/test_nav_eval.py +++ b/dimos/navigation/nav_3d/evaluator/test_nav_eval.py @@ -18,15 +18,23 @@ from pathlib import Path from typing import TYPE_CHECKING, cast +from dimos_generated.geometry_msgs.msg import ( + Point as MessagePoint, + Pose, + PoseWithCovariance, + Quaternion, +) +from dimos_generated.nav_msgs.msg import Odometry +from dimos_generated.sensor_msgs.msg import PointCloud2 +from dimos_generated.std_msgs.msg import Header import numpy as np from numpy.typing import NDArray import pytest import typer from dimos.memory.store.sqlite import SqliteStore -from dimos.msgs.geometry_msgs.Pose import Pose -from dimos.msgs.nav_msgs.Odometry import Odometry -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 +from dimos.msgs.pointcloud import pointcloud_from_xyz +from dimos.msgs.time import time_from_seconds from dimos.navigation.nav_3d.evaluator import metrics, runner from dimos.navigation.nav_3d.evaluator.cases import Case, Suite, load_suite, save_suite from dimos.navigation.nav_3d.evaluator.cli import _apply_overrides @@ -444,10 +452,25 @@ def _write_recording( with SqliteStore(path=str(path)) as store: lidar = store.stream("lidar", PointCloud2) for ts, pts, frame_id in clouds: - lidar.append(PointCloud2.from_numpy(pts, frame_id=frame_id, timestamp=ts), ts=ts) + lidar.append( + pointcloud_from_xyz( + pts, header=Header(frame_id=frame_id, stamp=time_from_seconds(ts)) + ), + ts=ts, + ) odom = store.stream("odom", Odometry) for ts, (x, y, z) in poses: - odom.append(Odometry(ts=ts, pose=Pose(x, y, z)), ts=ts) + odom.append( + Odometry( + header=Header(stamp=time_from_seconds(ts), frame_id="world"), + pose=PoseWithCovariance( + pose=Pose( + position=MessagePoint(x=x, y=y, z=z), orientation=Quaternion(w=1.0) + ) + ), + ), + ts=ts, + ) def test_recording_reads_frames_trajectory_and_rotation(tmp_path: Path) -> None: @@ -481,10 +504,21 @@ def test_recording_reads_frames_trajectory_and_rotation(tmp_path: Path) -> None: quarter = np.sqrt(0.5) # 90 degrees about +z: sensor +x points along world +y with SqliteStore(path=str(yaw_db)) as store: store.stream("lidar", PointCloud2).append( - PointCloud2.from_numpy(ahead, frame_id="lidar", timestamp=1.0), ts=1.0 + pointcloud_from_xyz( + ahead, header=Header(frame_id="lidar", stamp=time_from_seconds(1.0)) + ), + ts=1.0, ) store.stream("odom", Odometry).append( - Odometry(ts=1.0, pose=Pose(5.0, 0.0, 0.0, 0.0, 0.0, quarter, quarter)), ts=1.0 + Odometry( + header=Header(stamp=time_from_seconds(1.0), frame_id="world"), + pose=PoseWithCovariance( + pose=Pose( + position=MessagePoint(x=5.0), orientation=Quaternion(z=quarter, w=quarter) + ) + ), + ), + ts=1.0, ) yawed = list(iter_world_frames(yaw_db, "lidar", "odom")) assert np.allclose(yawed[0].points, [[5.0, 1.0, 0.0]], atol=1e-5) diff --git a/dimos/navigation/nav_3d/mls_planner/mls_planner_native.py b/dimos/navigation/nav_3d/mls_planner/mls_planner_native.py index 595ece115c4..9aac9450e20 100644 --- a/dimos/navigation/nav_3d/mls_planner/mls_planner_native.py +++ b/dimos/navigation/nav_3d/mls_planner/mls_planner_native.py @@ -16,15 +16,15 @@ from __future__ import annotations +from dimos_generated.dimos_msgs.msg import LineSegments3D +from dimos_generated.geometry_msgs.msg import PointStamped, PoseStamped +from dimos_generated.nav_msgs.msg import Path +from dimos_generated.sensor_msgs.msg import PointCloud2 +from dimos_generated.tf2_msgs.msg import TFMessage + from dimos.constants import DIMOS_PROJECT_ROOT from dimos.core.native_module import NativeModule, NativeModuleConfig from dimos.core.stream import In, Out -from dimos.msgs.geometry_msgs.PointStamped import PointStamped -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.nav_msgs.LineSegments3D import LineSegments3D -from dimos.msgs.nav_msgs.Path import Path -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 -from dimos.msgs.tf2_msgs.TFMessage import TFMessage class MLSPlannerNativeConfig(NativeModuleConfig): diff --git a/dimos/navigation/nav_3d/mls_planner/rust/Cargo.toml b/dimos/navigation/nav_3d/mls_planner/rust/Cargo.toml index 2c5d39de041..b4cd6f8d22d 100644 --- a/dimos/navigation/nav_3d/mls_planner/rust/Cargo.toml +++ b/dimos/navigation/nav_3d/mls_planner/rust/Cargo.toml @@ -22,21 +22,21 @@ python = "dimos.navigation.nav_3d.mls_planner.mls_planner_native:MLSPlannerNativ threads = 2 [package.metadata.dimos.module.mls_planner.inputs] -global_map = "sensor_msgs.PointCloud2" -local_map = "sensor_msgs.PointCloud2" -region_bounds = "geometry_msgs.PoseStamped" -goal = "geometry_msgs.PointStamped" -tf = "tf2_msgs.TFMessage" +global_map = "sensor_msgs/msg/PointCloud2" +local_map = "sensor_msgs/msg/PointCloud2" +region_bounds = "geometry_msgs/msg/PoseStamped" +goal = "geometry_msgs/msg/PointStamped" +tf = "tf2_msgs/msg/TFMessage" [package.metadata.dimos.module.mls_planner.outputs] -surface_map = "sensor_msgs.PointCloud2" -nodes = "sensor_msgs.PointCloud2" -node_edges = "nav_msgs.LineSegments3D" -path = "nav_msgs.Path" +surface_map = "sensor_msgs/msg/PointCloud2" +nodes = "sensor_msgs/msg/PointCloud2" +node_edges = "dimos_msgs/msg/LineSegments3D" +path = "nav_msgs/msg/Path" [dependencies] dimos-module = { path = "../../../../../native/rust/dimos-module" } -lcm-msgs = { git = "https://github.com/dimensionalOS/dimos-lcm.git", branch = "rust-codegen" } +dimos-generated-messages = { path = "../../../../message_codegen" } tokio = { version = "1", features = ["rt-multi-thread", "macros", "signal"] } serde = { version = "1", features = ["derive"] } ahash = "0.8" diff --git a/dimos/navigation/nav_3d/mls_planner/rust/src/module.rs b/dimos/navigation/nav_3d/mls_planner/rust/src/module.rs index 16f8289866f..4ad463f8abd 100644 --- a/dimos/navigation/nav_3d/mls_planner/rust/src/module.rs +++ b/dimos/navigation/nav_3d/mls_planner/rust/src/module.rs @@ -17,11 +17,17 @@ use std::time::{Duration, Instant, SystemTime, UNIX_EPOCH}; use crate::mls_planner::{Config, Planner, RegionBounds}; use crate::voxel::{surface_point_xyz, VoxelKey}; +use dimos_generated_messages::builtin_interfaces::msg::Time; +use dimos_generated_messages::dimos_msgs::msg::{LineSegment3D, LineSegments3D}; +use dimos_generated_messages::geometry_msgs::msg::{ + Point, PointStamped, Pose, PoseStamped, Quaternion, +}; +use dimos_generated_messages::nav_msgs::msg::Path; +use dimos_generated_messages::sensor_msgs::msg::{PointCloud2, PointField}; +use dimos_generated_messages::std_msgs::msg::Header; +use dimos_module::cdr; +use dimos_module::pointcloud::xyz_points as extract_xyz; use dimos_module::{error_throttled, warn_throttled, Input, Module, Output, Tf}; -use lcm_msgs::geometry_msgs::{Point, PointStamped, Pose, PoseStamped, Quaternion}; -use lcm_msgs::nav_msgs::Path; -use lcm_msgs::sensor_msgs::{PointCloud2, PointField}; -use lcm_msgs::std_msgs::{Header, Time}; use tokio::sync::Notify; use tracing::{debug, warn}; @@ -47,32 +53,31 @@ enum MapUpdate { #[derive(Module)] #[module(name = "mls_planner", setup = spawn_worker, teardown = stop_worker)] pub struct MlsPlanner { - #[input(decode = PointCloud2::decode, handler = on_global_map)] + #[input(decode = cdr::decode, handler = on_global_map)] global_map: Input, - #[input(decode = PointCloud2::decode, handler = on_local_map)] + #[input(decode = cdr::decode, handler = on_local_map)] local_map: Input, - #[input(decode = PoseStamped::decode, handler = on_region_bounds)] + #[input(decode = cdr::decode, handler = on_region_bounds)] region_bounds: Input, - #[input(decode = PointStamped::decode, handler = on_goal)] + #[input(decode = cdr::decode, handler = on_goal)] goal: Input, #[tf] tf: Tf, - #[output(encode = PointCloud2::encode)] + #[output(encode = cdr::encode)] surface_map: Output, - #[output(encode = PointCloud2::encode)] + #[output(encode = cdr::encode)] nodes: Output, - // The wire payload is a Path. dimos names the channel LineSegments3D. - #[output(encode = Path::encode, msg = "LineSegments3D")] - node_edges: Output, + #[output(encode = cdr::encode)] + node_edges: Output, - #[output(encode = Path::encode)] + #[output(encode = cdr::encode)] path: Output, #[config] @@ -174,7 +179,7 @@ struct Worker { config: Config, surface_map: Output, nodes: Output, - node_edges: Output, + node_edges: Output, path: Output, } @@ -216,7 +221,9 @@ impl Worker { if let Some((surface, node_cloud, edges)) = messages { publish_cloud(&self.surface_map, &surface).await; publish_cloud(&self.nodes, &node_cloud).await; - publish_path(&self.node_edges, &edges).await; + if let Err(error) = self.node_edges.publish(&edges).await { + warn!(%error, "edge segments failed to publish"); + } *last_viz_at = Some(now); } } @@ -286,7 +293,10 @@ impl Worker { true } - fn build_graph_messages(&self, planner: &Planner) -> (PointCloud2, PointCloud2, Path) { + fn build_graph_messages( + &self, + planner: &Planner, + ) -> (PointCloud2, PointCloud2, LineSegments3D) { let voxel_size = self.config.voxel_size; let frame = &self.config.world_frame; let graph = planner.graph(); @@ -304,7 +314,7 @@ impl Worker { let node_points: Vec = graph.nodes.iter().map(|n| n.pos).collect(); let node_cloud = build_pc2_xyz(&node_points, frame, now()); - let edges = build_segments_path(planner.edge_segments(), voxel_size, frame, now()); + let edges = build_segments(planner.edge_segments(), voxel_size, frame, now()); (surface, node_cloud, edges) } @@ -364,7 +374,7 @@ fn is_at_goal(start: Xyz, goal: Xyz, tol: f32) -> bool { } fn same_stamp(a: &Time, b: &Time) -> bool { - a.sec == b.sec && a.nsec == b.nsec + a.sec == b.sec && a.nanosec == b.nanosec } async fn publish_cloud(out: &Output, cloud: &PointCloud2) { @@ -395,13 +405,12 @@ fn now() -> Time { .unwrap_or_default(); Time { sec: dur.as_secs().min(i32::MAX as u64) as i32, - nsec: dur.subsec_nanos() as i32, + nanosec: dur.subsec_nanos(), } } fn header(frame_id: &str, stamp: Time) -> Header { Header { - seq: 0, stamp, frame_id: frame_id.into(), } @@ -448,30 +457,37 @@ fn build_path_from_waypoints(waypoints: &[(f32, f32, f32)], frame_id: &str, stam } } -/// Emit edges as alternating PoseStamped pairs with orientation.w carrying -/// the per-edge cost. -fn build_segments_path( +/// Emit explicit endpoints and weights in the dedicated custom message. +fn build_segments( segments: Vec<(VoxelKey, VoxelKey, f32)>, voxel_size: f32, frame_id: &str, stamp: Time, -) -> Path { - let mut poses: Vec = Vec::with_capacity(segments.len() * 2); - for (a, b, cost) in segments { - let pa = surface_point_xyz(a.0, a.1, a.2, voxel_size); - let pb = surface_point_xyz(b.0, b.1, b.2, voxel_size); - poses.push(pose_stamped(pa, cost as f64, frame_id, stamp.clone())); - poses.push(pose_stamped(pb, cost as f64, frame_id, stamp.clone())); - } - Path { +) -> LineSegments3D { + let point = |key: VoxelKey| { + let (x, y, z) = surface_point_xyz(key.0, key.1, key.2, voxel_size); + Point { + x: x as f64, + y: y as f64, + z: z as f64, + } + }; + LineSegments3D { header: header(frame_id, stamp), - poses, + segments: segments + .into_iter() + .map(|(a, b, cost)| LineSegment3D { + start: point(a), + end: point(b), + weight: cost as f64, + }) + .collect(), } } /// Like `build_pc2_xyz` plus an `intensity` float carrying the cell's wall clearance. fn build_pc2_xyzi(points: &[Xyzi], frame_id: &str, stamp: Time) -> PointCloud2 { - let n = points.len() as i32; + let n = points.len() as u32; let mut data = Vec::with_capacity(points.len() * 16); for &(x, y, z, i) in points { data.extend_from_slice(&x.to_le_bytes()); @@ -479,10 +495,10 @@ fn build_pc2_xyzi(points: &[Xyzi], frame_id: &str, stamp: Time) -> PointCloud2 { data.extend_from_slice(&z.to_le_bytes()); data.extend_from_slice(&i.to_le_bytes()); } - let make_field = |name: &str, off: i32| PointField { + let make_field = |name: &str, off: u32| PointField { name: name.into(), offset: off, - datatype: PointField::FLOAT32 as u8, + datatype: PointField::FLOAT32, count: 1, }; PointCloud2 { @@ -504,17 +520,17 @@ fn build_pc2_xyzi(points: &[Xyzi], frame_id: &str, stamp: Time) -> PointCloud2 { } fn build_pc2_xyz(points: &[(f32, f32, f32)], frame_id: &str, stamp: Time) -> PointCloud2 { - let n = points.len() as i32; + let n = points.len() as u32; let mut data = Vec::with_capacity(points.len() * 12); for &(x, y, z) in points { data.extend_from_slice(&x.to_le_bytes()); data.extend_from_slice(&y.to_le_bytes()); data.extend_from_slice(&z.to_le_bytes()); } - let make_field = |name: &str, off: i32| PointField { + let make_field = |name: &str, off: u32| PointField { name: name.into(), offset: off, - datatype: PointField::FLOAT32 as u8, + datatype: PointField::FLOAT32, count: 1, }; PointCloud2 { @@ -530,76 +546,40 @@ fn build_pc2_xyz(points: &[(f32, f32, f32)], frame_id: &str, stamp: Time) -> Poi } } -struct ExtractError(&'static str); -impl std::fmt::Display for ExtractError { - fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result { - f.write_str(self.0) - } -} - -fn extract_xyz(msg: &PointCloud2) -> Result, ExtractError> { - let mut x_off: Option = None; - let mut y_off: Option = None; - let mut z_off: Option = None; - for f in &msg.fields { - if f.datatype != PointField::FLOAT32 as u8 { - continue; - } - match f.name.as_str() { - "x" => x_off = Some(f.offset as usize), - "y" => y_off = Some(f.offset as usize), - "z" => z_off = Some(f.offset as usize), - _ => {} - } - } - let xo = x_off.ok_or(ExtractError("missing float32 x field"))?; - let yo = y_off.ok_or(ExtractError("missing float32 y field"))?; - let zo = z_off.ok_or(ExtractError("missing float32 z field"))?; - - let n = (msg.width as usize) * (msg.height as usize); - let step = msg.point_step as usize; - if step == 0 { - return Err(ExtractError("point_step is 0")); - } - if msg.data.len() < n * step { - return Err(ExtractError( - "data buffer shorter than width*height*point_step", - )); - } - if xo + 4 > step || yo + 4 > step || zo + 4 > step { - return Err(ExtractError( - "xyz field offsets do not fit within point_step", - )); - } - if msg.is_bigendian { - return Err(ExtractError("big-endian point data not supported")); - } - - let mut out = Vec::with_capacity(n); - for i in 0..n { - let base = i * step; - let x = read_f32_le(&msg.data, base + xo); - let y = read_f32_le(&msg.data, base + yo); - let z = read_f32_le(&msg.data, base + zo); - if x.is_finite() && y.is_finite() && z.is_finite() { - out.push((x, y, z)); - } - } - Ok(out) -} - -#[inline] -fn read_f32_le(buf: &[u8], off: usize) -> f32 { - let bytes: [u8; 4] = buf[off..off + 4] - .try_into() - .expect("bounds checked by caller"); - f32::from_le_bytes(bytes) -} - #[cfg(test)] mod tests { use super::*; + #[test] + fn graph_edges_encode_explicit_endpoints_and_cost() { + let stamp = Time { + sec: 1700000000, + nanosec: 123456789, + }; + let message = build_segments(vec![((0, 1, 2), (2, 3, 4), 3.5)], 0.5, "map", stamp.clone()); + let decoded: LineSegments3D = cdr::decode(&cdr::encode(&message).unwrap()).unwrap(); + assert_eq!(decoded.header.frame_id, "map"); + assert_eq!(decoded.header.stamp, stamp); + assert_eq!(decoded.segments.len(), 1); + assert_eq!( + decoded.segments[0].start, + Point { + x: 0.25, + y: 0.75, + z: 1.5 + } + ); + assert_eq!( + decoded.segments[0].end, + Point { + x: 1.25, + y: 1.75, + z: 2.5 + } + ); + assert_eq!(decoded.segments[0].weight, 3.5); + } + #[test] fn is_at_goal_respects_tolerance_and_ignores_z() { assert!(is_at_goal((0.0, 0.0, 0.0), (0.05, 0.0, 9.0), 0.1)); @@ -629,12 +609,12 @@ mod tests { #[test] fn stamps_paired_only_when_both_present_and_stamps_match() { - let s = Time { sec: 2, nsec: 3 }; + let s = Time { sec: 2, nanosec: 3 }; let b = bounds_at(s.clone()); let c = cloud_at(s); assert!(stamps_paired(Some(&b), Some(&c))); - let other = cloud_at(Time { sec: 2, nsec: 4 }); + let other = cloud_at(Time { sec: 2, nanosec: 4 }); assert!(!stamps_paired(Some(&b), Some(&other))); assert!(!stamps_paired(Some(&b), None)); diff --git a/dimos/navigation/nav_3d/mls_planner/start_relay.py b/dimos/navigation/nav_3d/mls_planner/start_relay.py index e55aeb5856a..04aedd9b93b 100644 --- a/dimos/navigation/nav_3d/mls_planner/start_relay.py +++ b/dimos/navigation/nav_3d/mls_planner/start_relay.py @@ -17,13 +17,13 @@ import time from typing import Any +from dimos_generated.geometry_msgs.msg import PoseStamped +from dimos_generated.tf2_msgs.msg import TFMessage from reactivex.disposable import Disposable from dimos.core.core import rpc from dimos.core.module import Module, ModuleConfig from dimos.core.stream import In, Out -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.tf2_msgs.TFMessage import TFMessage class StartRelayConfig(ModuleConfig): diff --git a/dimos/navigation/nav_3d/mls_planner/test_start_relay.py b/dimos/navigation/nav_3d/mls_planner/test_start_relay.py index 9c35385ac87..388df13f04c 100644 --- a/dimos/navigation/nav_3d/mls_planner/test_start_relay.py +++ b/dimos/navigation/nav_3d/mls_planner/test_start_relay.py @@ -14,11 +14,17 @@ from typing import Any -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Transform import Transform -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.msgs.tf2_msgs.TFMessage import TFMessage +from dimos_generated.builtin_interfaces.msg import Time +from dimos_generated.geometry_msgs.msg import ( + PoseStamped, + Quaternion, + Transform, + TransformStamped, + Vector3, +) +from dimos_generated.std_msgs.msg import Header +from dimos_generated.tf2_msgs.msg import TFMessage + from dimos.navigation.nav_3d.mls_planner.start_relay import StartRelay from dimos.protocol.tf.tf import MultiTBuffer @@ -40,7 +46,7 @@ def get( time_tolerance: float | None = None, *, forward_tolerance: float = 0.0, - ) -> Transform | None: + ) -> TransformStamped | None: self.gets += 1 return super().get( parent_frame, @@ -54,23 +60,19 @@ def dispose(self) -> None: pass -def _mount() -> Transform: - return Transform( - translation=Vector3(0.0, 0.0, MOUNT_Z), - rotation=Quaternion(0.0, 0.0, 0.0, 1.0), - frame_id="base_link", +def _mount() -> TransformStamped: + return TransformStamped( + header=Header(frame_id="base_link", stamp=Time(sec=1)), child_frame_id="mid360_link", - ts=1.0, + transform=Transform(translation=Vector3(z=MOUNT_Z), rotation=Quaternion(w=1.0)), ) -def _odom_edge() -> Transform: - return Transform( - translation=Vector3(1.0, 2.0, 3.0), - rotation=Quaternion(0.0, 0.0, 0.0, 1.0), - frame_id="odom", +def _odom_edge() -> TransformStamped: + return TransformStamped( + header=Header(frame_id="odom", stamp=Time(sec=2)), child_frame_id="mid360_link", - ts=2.0, + transform=Transform(translation=Vector3(x=1.0, y=2.0, z=3.0), rotation=Quaternion(w=1.0)), ) @@ -78,7 +80,7 @@ def _relay(tf: FakeTF, **config: Any) -> tuple[StartRelay, list[PoseStamped]]: module = StartRelay(**config) module._tf = tf captured: list[PoseStamped] = [] - module.start_pose.subscribe(captured.append) + module.start_pose.subscribe(lambda pose: captured.append(PoseStamped.decode(pose.encode()))) return module, captured @@ -91,9 +93,9 @@ def test_start_pose_is_the_tf_base_pose() -> None: module._on_tf(TFMessage()) # The base sits MOUNT_Z below the sensor along the mount leg. assert len(captured) == 1 - assert abs(captured[0].position.x - 1.0) < 1e-9 - assert abs(captured[0].position.y - 2.0) < 1e-9 - assert abs(captured[0].position.z - (3.0 - MOUNT_Z)) < 1e-9 + assert abs(captured[0].pose.position.x - 1.0) < 1e-9 + assert abs(captured[0].pose.position.y - 2.0) < 1e-9 + assert abs(captured[0].pose.position.z - (3.0 - MOUNT_Z)) < 1e-9 finally: module.stop() diff --git a/dimos/navigation/nav_3d/mls_planner/test_transformer.py b/dimos/navigation/nav_3d/mls_planner/test_transformer.py index 6513de50209..4d96f31c1b6 100644 --- a/dimos/navigation/nav_3d/mls_planner/test_transformer.py +++ b/dimos/navigation/nav_3d/mls_planner/test_transformer.py @@ -20,8 +20,13 @@ pytest.importorskip("dimos_mls_planner") +from dimos_generated.builtin_interfaces.msg import Time +from dimos_generated.nav_msgs.msg import Path +from dimos_generated.sensor_msgs.msg import PointCloud2 +from dimos_generated.std_msgs.msg import Header + from dimos.memory.type.observation import Observation -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 +from dimos.msgs.pointcloud import pointcloud_from_xyz from dimos.navigation.nav_3d.mls_planner.transformer import MLSPlan @@ -35,7 +40,12 @@ def _obs( ts=0.0, pose=pose, tags={"region_bounds": region_bounds}, - _data=PointCloud2.from_numpy(points), + _data=PointCloud2.decode( + pointcloud_from_xyz( + points, + header=Header(frame_id="world", stamp=Time(sec=1700000000, nanosec=123456789)), + ).encode() + ), ) @@ -55,6 +65,11 @@ def test_flat_floor_yields_populated_path_and_planned_true() -> None: [out] = list(MLSPlan(goal=(2.0, 2.0, 0.0), voxel_size=0.2, robot_height=1.0)(iter([obs]))) + decoded = Path.decode(out.data.encode()) + assert decoded.header.frame_id == "world" + assert decoded.header.stamp.sec == 1700000000 + assert decoded.header.stamp.nanosec == 123456789 + assert all(p.header.stamp.nanosec == 123456789 for p in decoded.poses) assert out.tags["planned"] is True assert len(out.data.poses) >= 2 assert out.tags["voxels"] > 0 @@ -70,7 +85,12 @@ def test_poseless_obs_is_skipped() -> None: ts=0.0, pose=None, tags={"region_bounds": (0.0, 0.0, 5.0, -1.0, 2.0)}, - _data=PointCloud2.from_numpy(points), + _data=PointCloud2.decode( + pointcloud_from_xyz( + points, + header=Header(frame_id="world", stamp=Time(sec=1700000000, nanosec=123456789)), + ).encode() + ), ) posed = _obs(points, pose=(-2.0, -2.0, 1.0), region_bounds=(0.0, 0.0, 5.0, -1.0, 2.0)) @@ -105,4 +125,4 @@ def test_no_route_yields_empty_path_with_planned_false() -> None: [out] = list(MLSPlan(goal=(100.0, 100.0, 100.0))(iter([obs]))) assert out.tags["planned"] is False - assert out.data.poses == [] + assert len(out.data.poses) == 0 diff --git a/dimos/navigation/nav_3d/mls_planner/test_viz.py b/dimos/navigation/nav_3d/mls_planner/test_viz.py new file mode 100644 index 00000000000..40f84dfb3eb --- /dev/null +++ b/dimos/navigation/nav_3d/mls_planner/test_viz.py @@ -0,0 +1,79 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Generated planner messages retain geometry through the Rerun adapters.""" + +import struct + +from dimos_generated.dimos_msgs.msg import LineSegment3D, LineSegments3D +from dimos_generated.geometry_msgs.msg import Point +from dimos_generated.sensor_msgs.msg import PointCloud2, PointField +from dimos_generated.std_msgs.msg import Header +import numpy as np + +from dimos.msgs.pointcloud import pointcloud_from_xyz +from dimos.navigation.nav_3d.mls_planner.viz import ( + render_node_edges, + render_nodes, + render_surface_map, +) + + +def test_surface_clearance_filter_after_cdr() -> None: + cloud = PointCloud2( + height=1, + width=2, + point_step=16, + row_step=32, + fields=[ + PointField(name=name, offset=i * 4, datatype=PointField.FLOAT32, count=1) + for i, name in enumerate(("x", "y", "z", "intensity")) + ], + data=struct.pack("<8f", 1, 2, 3, 0.1, 4, 5, 6, 0.8), + ) + decoded = PointCloud2.decode(cloud.encode()) + before = decoded.encode() + rendered = render_surface_map(decoded, wall_clearance_m=0.2) + assert rendered.positions.as_arrow_array().to_pylist() == [[4.0, 5.0, 6.0]] + assert decoded.encode() == before + + +def test_nodes_lift_and_surface_without_intensity() -> None: + cloud = pointcloud_from_xyz(np.array([[1.0, 2.0, 3.0]]), header=Header()) + before = cloud.encode() + np.testing.assert_allclose( + render_nodes(cloud).positions.as_arrow_array().to_pylist(), [[1, 2, 3.05]] + ) + assert render_surface_map(cloud).positions.as_arrow_array().to_pylist() == [[1, 2, 3]] + assert cloud.encode() == before + + +def test_weighted_edges_and_empty_messages() -> None: + message = LineSegments3D( + segments=[ + LineSegment3D(start=Point(), end=Point(x=1), weight=1), + LineSegment3D(start=Point(x=1), end=Point(x=2), weight=100), + ] + ) + decoded = LineSegments3D.decode(message.encode()) + rendered = render_node_edges(decoded) + np.testing.assert_allclose( + rendered.strips.as_arrow_array().to_pylist(), + [[[0, 0, 0.05], [1, 0, 0.05]], [[1, 0, 0.05], [2, 0, 0.05]]], + ) + assert rendered.colors.as_arrow_array().to_pylist() == [0x00FF3CDC, 0xFF003CDC] + assert decoded.segments[0].start.z == 0 + assert render_node_edges(LineSegments3D()).strips.as_arrow_array().to_pylist() == [] + empty = pointcloud_from_xyz(np.empty((0, 3)), header=Header()) + assert render_nodes(empty).positions.as_arrow_array().to_pylist() == [] diff --git a/dimos/navigation/nav_3d/mls_planner/transformer.py b/dimos/navigation/nav_3d/mls_planner/transformer.py index 411f24752a2..a275bf97dff 100644 --- a/dimos/navigation/nav_3d/mls_planner/transformer.py +++ b/dimos/navigation/nav_3d/mls_planner/transformer.py @@ -17,17 +17,20 @@ import time from typing import TYPE_CHECKING, Any +from dimos_generated.geometry_msgs.msg import Point, Pose, PoseStamped, Quaternion +from dimos_generated.nav_msgs.msg import Path +from dimos_generated.sensor_msgs.msg import PointCloud2 +from dimos_generated.std_msgs.msg import Header +import numpy as np + from dimos.memory.transform import Transformer -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.nav_msgs.Path import Path -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 +from dimos.msgs.pointcloud import pointcloud_xyz from dimos.navigation.nav_3d.mls_planner.mls_planner import MLSPlanner from dimos.utils.logging_config import setup_logger if TYPE_CHECKING: from collections.abc import Iterator - import numpy as np from numpy.typing import NDArray from dimos.memory.type.observation import Observation @@ -51,19 +54,20 @@ def __init__( self.robot_height = robot_height self._planner_kwargs = planner_kwargs - def _path_from_waypoints(self, waypoints: NDArray[np.float32] | None, ts: float) -> Path: + def _path_from_waypoints(self, waypoints: NDArray[np.float32] | None, header: Header) -> Path: poses: list[PoseStamped] = [] if waypoints is not None: for x, y, z in waypoints: poses.append( PoseStamped( - ts=ts, - frame_id="world", - position=(float(x), float(y), float(z)), - orientation=(0.0, 0.0, 0.0, 1.0), + header=header, + pose=Pose( + position=Point(x=float(x), y=float(y), z=float(z)), + orientation=Quaternion(w=1.0), + ), ) ) - return Path(ts=ts, frame_id="world", poses=poses) + return Path(header=header, poses=poses) def __call__( self, @@ -83,11 +87,18 @@ def __call__( ox, oy, radius, z_min, z_max = obs.tags["region_bounds"] t_update = time.perf_counter() - planner.update_region(obs.data.points_f32(), (ox, oy), radius, z_min, z_max, float(z)) + planner.update_region( + pointcloud_xyz(obs.data).astype(np.float32), + (ox, oy), + radius, + z_min, + z_max, + float(z), + ) t_plan = time.perf_counter() waypoints = planner.plan(start, self.goal) t_done = time.perf_counter() - path = self._path_from_waypoints(waypoints, obs.ts) + path = self._path_from_waypoints(waypoints, obs.data.header) timings = { "update_ms": (t_plan - t_update) * 1000, diff --git a/dimos/navigation/nav_3d/mls_planner/utils/plan_rrd.py b/dimos/navigation/nav_3d/mls_planner/utils/plan_rrd.py index c6d70e54344..79ebb502ac6 100644 --- a/dimos/navigation/nav_3d/mls_planner/utils/plan_rrd.py +++ b/dimos/navigation/nav_3d/mls_planner/utils/plan_rrd.py @@ -23,6 +23,9 @@ from time import perf_counter from typing import TYPE_CHECKING, Any, NamedTuple +from dimos_generated.geometry_msgs.msg import TransformStamped +from dimos_generated.sensor_msgs.msg import PointCloud2 +from dimos_generated.tf2_msgs.msg import TFMessage import numpy as np from numpy.typing import NDArray import typer @@ -34,9 +37,7 @@ from dimos.memory.transform import FnTransformer from dimos.memory.type.observation import Observation from dimos.memory.vis.utils import DEFAULT_RENDER_VOXEL, default_render_voxel -from dimos.msgs.geometry_msgs.Transform import Transform -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2, register_colormap_annotation -from dimos.msgs.tf2_msgs.TFMessage import TfFrameTree, TFMessage +from dimos.msgs.pointcloud import pointcloud_xyz from dimos.navigation.nav_3d.mls_planner.mls_planner import MLSPlanner from dimos.robot.unitree.go2.constants import ( BASE_LINK_HEIGHT, @@ -45,6 +46,7 @@ ROBOT_WIDTH, ) from dimos.utils.data import resolve_named_path +from dimos.visualization.rerun.message_helpers import register_colormap_annotation, tf_archetypes if TYPE_CHECKING: import rerun.blueprint as rrb @@ -127,7 +129,7 @@ def _pose_from_tf(tf: StreamTF, world_frame: str) -> FnTransformer[PointCloud2, def attach(obs: Observation[PointCloud2]) -> Observation[PointCloud2]: t = tf.get( world_frame, - obs.data.frame_id, + obs.data.header.frame_id, time_point=obs.ts, time_tolerance=TF_MATCH_TOLERANCE_S, ) @@ -181,14 +183,13 @@ class _TfSync: def __init__(self, tf: Stream[TFMessage] | None) -> None: self._pending = iter(tf) if tf is not None else iter(()) self._next = next(self._pending, None) - self._tree = TfFrameTree() def up_to(self, ts: float) -> None: import rerun as rr while self._next is not None and self._next.ts <= ts: rr.set_time(TIMELINE, timestamp=self._next.ts) - for path, archetype in self._next.data.to_rerun(self._tree): + for path, archetype in tf_archetypes(self._next.data): rr.log(path, archetype) self._next = next(self._pending, None) @@ -197,7 +198,7 @@ def _log_odometry( pose: tuple[float, ...], ts: float, trail: list[tuple[float, float, float]], - base: Transform | None, + base: TransformStamped | None, ) -> None: """Trace the sensor moving throughout the scene.""" import rerun as rr @@ -214,9 +215,18 @@ def _log_odometry( rr.log( "world/robot_body", rr.Transform3D( - translation=[base.translation.x, base.translation.y, base.translation.z], + translation=[ + base.transform.translation.x, + base.transform.translation.y, + base.transform.translation.z, + ], quaternion=rr.Quaternion( - xyzw=[base.rotation.x, base.rotation.y, base.rotation.z, base.rotation.w] + xyzw=[ + base.transform.rotation.x, + base.transform.rotation.y, + base.transform.rotation.z, + base.transform.rotation.w, + ] ), ), ) @@ -422,11 +432,13 @@ def _process_frame( bounds = ray_obs.tags["region_bounds"] ox, oy, radius, z_min, z_max = bounds - pts = ray_obs.data.points_f32() + pts = pointcloud_xyz(ray_obs.data).astype(np.float32) rr.set_time(TIMELINE, timestamp=ray_obs.ts) ref_timing: dict[str, float] = {} - surface = nodes = edges = np.empty((0,), dtype=np.float32) + surface: NDArray[np.float32] = np.empty((0,), dtype=np.float32) + nodes: NDArray[np.float32] = np.empty((0,), dtype=np.float32) + edges: NDArray[np.float32] = np.empty((0,), dtype=np.float32) for j, (label, color, planner) in enumerate(planners): t0 = perf_counter() planner.update_region(pts, (ox, oy), radius, z_min, z_max, sensor_z) @@ -666,11 +678,11 @@ def main( if base is None: continue start = ( - float(base.translation.x), - float(base.translation.y), - float(base.translation.z) - start_z_offset, + float(base.transform.translation.x), + float(base.transform.translation.y), + float(base.transform.translation.z) - start_z_offset, ) - sensor_z = float(base.translation.z) + sensor_z = float(base.transform.translation.z) ref_timing = _process_frame( ray_obs, planners, diff --git a/dimos/navigation/nav_3d/mls_planner/utils/test_plan_rrd.py b/dimos/navigation/nav_3d/mls_planner/utils/test_plan_rrd.py new file mode 100644 index 00000000000..4a96dd45854 --- /dev/null +++ b/dimos/navigation/nav_3d/mls_planner/utils/test_plan_rrd.py @@ -0,0 +1,58 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Recorded generated TF messages feed the offline planner viewer.""" + +from unittest.mock import patch + +from dimos_generated.geometry_msgs.msg import Quaternion, Transform, TransformStamped, Vector3 +from dimos_generated.std_msgs.msg import Header +from dimos_generated.tf2_msgs.msg import TFMessage + +from dimos.memory.type.observation import Observation +from dimos.navigation.nav_3d.mls_planner.utils.plan_rrd import _log_odometry, _TfSync +from dimos.visualization.rerun.message_helpers import register_colormap_annotation + + +def test_tf_sync_generated_edges_and_odometry() -> None: + edge = TransformStamped( + header=Header(frame_id="world"), + child_frame_id="base_link", + transform=Transform(translation=Vector3(x=2), rotation=Quaternion(w=1)), + ) + message = TFMessage.decode(TFMessage(transforms=[edge]).encode()) + sync = _TfSync([Observation(id=0, ts=1.0, _data=message)]) + with patch("rerun.log") as log: + sync.up_to(0.5) + log.assert_not_called() + sync.up_to(1.0) + entity, archetype = log.call_args.args + assert entity == "tf_links/base_link" + assert archetype.parent_frame.as_arrow_array().to_pylist() == ["tf#/world"] + assert archetype.child_frame.as_arrow_array().to_pylist() == ["tf#/base_link"] + assert archetype.translation.as_arrow_array().to_pylist() == [[2.0, 0.0, 0.0]] + sync.up_to(2.0) + assert log.call_count == 1 + _log_odometry((2, 0, 0, 0, 0, 0, 1), 1.0, [], edge) + assert log.call_args.args[0] == "world/robot_body" + assert log.call_args.args[1].translation.as_arrow_array().to_pylist() == [[2.0, 0.0, 0.0]] + + +def test_colormap_annotation_has_all_indices() -> None: + with patch("rerun.log") as log: + register_colormap_annotation() + assert log.call_args.args[0] == "/" + assert log.call_args.kwargs == {"static": True} + context = log.call_args.args[1].context.as_arrow_array().to_pylist() + assert len(context[0]) == 256 diff --git a/dimos/navigation/nav_3d/mls_planner/viz.py b/dimos/navigation/nav_3d/mls_planner/viz.py index e16593ae3b1..7eef67b2d9d 100644 --- a/dimos/navigation/nav_3d/mls_planner/viz.py +++ b/dimos/navigation/nav_3d/mls_planner/viz.py @@ -27,10 +27,11 @@ from functools import partial from typing import TYPE_CHECKING, Any +from dimos_generated.dimos_msgs.msg import LineSegments3D +from dimos_generated.sensor_msgs.msg import PointCloud2 import numpy as np -from dimos.msgs.nav_msgs.LineSegments3D import LineSegments3D -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 +from dimos.msgs.pointcloud import pointcloud_view, pointcloud_xyz if TYPE_CHECKING: from numpy.typing import NDArray @@ -63,15 +64,15 @@ def render_surface_map( """ import rerun as rr - pts = msg.points_f32() - clearance = msg.intensities_f32() + pts = pointcloud_xyz(msg) + records = pointcloud_view(msg) + clearance = records["intensity"].ravel() if "intensity" in (records.dtype.names or ()) else None if clearance is None or len(clearance) != len(pts): - return msg.to_rerun(voxel_size=voxel_size, colors=[40, 75, 130]) + return rr.Points3D(positions=pts, radii=voxel_size * 0.5, colors=[40, 75, 130]) passable = clearance >= wall_clearance_m - pts, clearance = pts[passable], clearance[passable] return rr.Points3D( - positions=pts, - colors=clearance_colors(clearance, clearance_clamp_m), + positions=pts[passable], + colors=clearance_colors(clearance[passable], clearance_clamp_m), radii=voxel_size * 0.5, ) @@ -79,8 +80,8 @@ def render_surface_map( def render_nodes(msg: PointCloud2) -> Archetype: import rerun as rr - pts, _ = msg.as_numpy() - if pts is None or len(pts) == 0: + pts = pointcloud_xyz(msg) + if len(pts) == 0: return rr.Points3D([]) pts = pts.copy() pts[:, 2] += _GRAPH_Z_LIFT @@ -88,7 +89,23 @@ def render_nodes(msg: PointCloud2) -> Archetype: def render_node_edges(msg: LineSegments3D) -> Archetype: - return msg.to_rerun(z_offset=_GRAPH_Z_LIFT, radii=0.01) + import rerun as rr + + if not msg.segments: + return rr.LineStrips3D([]) + strips = np.array( + [[[s.start.x, s.start.y, s.start.z], [s.end.x, s.end.y, s.end.z]] for s in msg.segments], + dtype=np.float32, + ) + strips[:, :, 2] += _GRAPH_Z_LIFT + weights = np.array([s.weight for s in msg.segments], dtype=np.float64) + log_weights = np.log10(np.maximum(weights, 1e-6)) + low, high = float(log_weights.min()), float(log_weights.max()) + norm = (log_weights - low) / (high - low) if high > low else np.zeros_like(log_weights) + red = (255 * norm).astype(np.uint8) + green = (255 * (1 - norm)).astype(np.uint8) + colors = np.column_stack([red, green, np.full_like(red, 60), np.full_like(red, 220)]) + return rr.LineStrips3D(strips, colors=colors, radii=0.01) def planner_visual_override( diff --git a/dimos/navigation/navigation_spec.py b/dimos/navigation/navigation_spec.py index d7aaab0ba7c..71b6697dc8b 100644 --- a/dimos/navigation/navigation_spec.py +++ b/dimos/navigation/navigation_spec.py @@ -14,7 +14,8 @@ from typing import Protocol -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped +from dimos_generated.geometry_msgs.msg import PoseStamped + from dimos.navigation.base import NavigationState from dimos.spec.utils import Spec diff --git a/dimos/navigation/patrolling/module.py b/dimos/navigation/patrolling/module.py index d6ef33c633f..9050cced92d 100644 --- a/dimos/navigation/patrolling/module.py +++ b/dimos/navigation/patrolling/module.py @@ -16,7 +16,9 @@ import asyncio from collections.abc import AsyncGenerator -from dimos_lcm.std_msgs import Bool +from dimos_generated.geometry_msgs.msg import PoseStamped +from dimos_generated.nav_msgs.msg import OccupancyGrid +from dimos_generated.std_msgs.msg import Bool from dimos.agents.annotation import skill from dimos.agents.capabilities import CAP_MOVEMENT @@ -24,8 +26,6 @@ from dimos.core.global_config import GlobalConfig, global_config from dimos.core.module import Module from dimos.core.stream import In, Out -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.nav_msgs.OccupancyGrid import OccupancyGrid from dimos.navigation.patrolling.constants import EXTRA_CLEARANCE from dimos.navigation.patrolling.create_patrol_router import create_patrol_router from dimos.navigation.patrolling.routers.patrol_router import PatrolRouter diff --git a/dimos/navigation/patrolling/routers/base_patrol_router.py b/dimos/navigation/patrolling/routers/base_patrol_router.py index eef90d81ed5..46a22a7461c 100644 --- a/dimos/navigation/patrolling/routers/base_patrol_router.py +++ b/dimos/navigation/patrolling/routers/base_patrol_router.py @@ -16,11 +16,11 @@ from threading import RLock import time +from dimos_generated.geometry_msgs.msg import PoseStamped +from dimos_generated.nav_msgs.msg import OccupancyGrid import numpy as np from numpy.typing import NDArray -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.nav_msgs.OccupancyGrid import OccupancyGrid from dimos.navigation.patrolling.routers.visitation_history import VisitationHistory @@ -62,7 +62,7 @@ def handle_odom(self, msg: PoseStamped) -> None: self._pose = msg if self._occupancy_grid is None: return - self._visitation.handle_odom(msg.position.x, msg.position.y) + self._visitation.handle_odom(msg.pose.position.x, msg.pose.position.y) def get_saturation(self) -> float: with self._lock: diff --git a/dimos/navigation/patrolling/routers/coverage_patrol_router.py b/dimos/navigation/patrolling/routers/coverage_patrol_router.py index a060868d9db..445060d1fbf 100644 --- a/dimos/navigation/patrolling/routers/coverage_patrol_router.py +++ b/dimos/navigation/patrolling/routers/coverage_patrol_router.py @@ -12,14 +12,14 @@ # See the License for the specific language governing permissions and # limitations under the License. +from dimos_generated.geometry_msgs.msg import PoseStamped +from dimos_generated.nav_msgs.msg import OccupancyGrid, Path import numpy as np from numpy.typing import NDArray from scipy.ndimage import binary_erosion from dimos.mapping.occupancy.gradient import gradient, voronoi_gradient -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.nav_msgs.OccupancyGrid import OccupancyGrid -from dimos.msgs.nav_msgs.Path import Path +from dimos.msgs.occupancy import grid_to_world, occupancy_view, world_to_grid from dimos.navigation.patrolling.routers.base_patrol_router import BasePatrolRouter from dimos.navigation.patrolling.utilities import point_to_pose_stamped from dimos.navigation.replanning_a_star.min_cost_astar import min_cost_astar @@ -48,7 +48,7 @@ def handle_occupancy_grid(self, msg: OccupancyGrid) -> None: # Precompute the safe mask (cells with enough clearance from obstacles). clearance_cells = self._visitation.clearance_radius_cells - free_mask = msg.grid == 0 + free_mask = occupancy_view(msg) == 0 structure = np.ones((2 * clearance_cells + 1, 2 * clearance_cells + 1), dtype=bool) self._safe_mask = binary_erosion(free_mask, structure=structure).astype(bool) @@ -57,7 +57,7 @@ def handle_occupancy_grid(self, msg: OccupancyGrid) -> None: # open areas. Low voronoi cost = on the skeleton (equidistant from # walls) = high sampling weight. voronoi = voronoi_gradient(msg, max_distance=1.5) - voronoi_cost = voronoi.grid.astype(np.float64) + voronoi_cost = occupancy_view(voronoi).astype(np.float64) # Invert: skeleton cells (cost 0) become weight 100, walls (100) become 0. # Clamp negatives (unknown = -1) to 0. weights = np.clip(100.0 - voronoi_cost, 0.0, 100.0) @@ -83,7 +83,7 @@ def next_goal(self) -> PoseStamped | None: if pose is None: return None - start = (pose.position.x, pose.position.y) + start = pose.pose.position # Get candidate points from unvisited safe cells. unvisited_safe = safe_mask & ~visited @@ -112,8 +112,8 @@ def next_goal(self) -> PoseStamped | None: best_point = None for row, col in chosen: - world = occupancy_grid.grid_to_world((col, row, 0)) - candidate = (world.x, world.y) + world = grid_to_world(occupancy_grid, (col, row)) + candidate = world path = min_cost_astar(costmap, candidate, start, unknown_penalty=1.0, use_cpp=True) if path is None: @@ -127,7 +127,7 @@ def next_goal(self) -> PoseStamped | None: if best_point is None: return None - return point_to_pose_stamped(best_point) + return point_to_pose_stamped(best_point, occupancy_grid.header) def _count_new_coverage( self, @@ -142,11 +142,11 @@ def _count_new_coverage( # Sample every few poses to avoid redundant work on dense paths. step = max(1, r) - poses = path.poses[::step] + poses = list(path.poses)[::step] for pose in poses: - grid = occupancy_grid.world_to_grid((pose.position.x, pose.position.y)) - col, row = int(grid.x), int(grid.y) + grid = world_to_grid(occupancy_grid, pose.pose.position) + col, row = int(np.floor(round(grid[0], 9))), int(np.floor(round(grid[1], 9))) r_min = max(0, row - r) r_max = min(h, row + r + 1) c_min = max(0, col - r) diff --git a/dimos/navigation/patrolling/routers/frontier_patrol_router.py b/dimos/navigation/patrolling/routers/frontier_patrol_router.py index ed1ec18dca1..4a26f4d0986 100644 --- a/dimos/navigation/patrolling/routers/frontier_patrol_router.py +++ b/dimos/navigation/patrolling/routers/frontier_patrol_router.py @@ -14,12 +14,13 @@ from typing import Any +from dimos_generated.geometry_msgs.msg import PoseStamped +from dimos_generated.nav_msgs.msg import OccupancyGrid import numpy as np from numpy.typing import NDArray from scipy.ndimage import binary_erosion, label -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.nav_msgs.OccupancyGrid import OccupancyGrid +from dimos.msgs.occupancy import grid_to_world, occupancy_view, world_to_grid from dimos.navigation.patrolling.routers.base_patrol_router import BasePatrolRouter from dimos.navigation.patrolling.utilities import point_to_pose_stamped @@ -49,7 +50,7 @@ def handle_occupancy_grid(self, msg: OccupancyGrid) -> None: return clearance_cells = self._visitation.clearance_radius_cells - free_mask = msg.grid == 0 + free_mask = occupancy_view(msg) == 0 structure = np.ones((2 * clearance_cells + 1, 2 * clearance_cells + 1), dtype=bool) self._safe_mask = binary_erosion(free_mask, structure=structure).astype(bool) @@ -66,8 +67,8 @@ def next_goal(self) -> PoseStamped | None: return None # Robot position in grid coordinates. - grid_pos = occupancy_grid.world_to_grid((pose.position.x, pose.position.y)) - robot_col, robot_row = grid_pos.x, grid_pos.y + grid_pos = world_to_grid(occupancy_grid, pose.pose.position) + robot_col, robot_row = grid_pos[0], grid_pos[1] # Unvisited safe cells. unvisited_safe = safe_mask & ~visited @@ -121,5 +122,5 @@ def next_goal(self) -> PoseStamped | None: dists_sq = cluster_dr * cluster_dr + cluster_dc * cluster_dc goal_row, goal_col = cluster_indices[np.argmax(dists_sq)] - world = occupancy_grid.grid_to_world((int(goal_col), int(goal_row), 0)) - return point_to_pose_stamped((world.x, world.y)) + world = grid_to_world(occupancy_grid, (int(goal_col), int(goal_row))) + return point_to_pose_stamped(world, occupancy_grid.header) diff --git a/dimos/navigation/patrolling/routers/patrol_router.py b/dimos/navigation/patrolling/routers/patrol_router.py index 19aff4ca34a..6209dade381 100644 --- a/dimos/navigation/patrolling/routers/patrol_router.py +++ b/dimos/navigation/patrolling/routers/patrol_router.py @@ -14,8 +14,8 @@ from typing import Protocol -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.nav_msgs.OccupancyGrid import OccupancyGrid +from dimos_generated.geometry_msgs.msg import PoseStamped +from dimos_generated.nav_msgs.msg import OccupancyGrid class PatrolRouter(Protocol): diff --git a/dimos/navigation/patrolling/routers/random_patrol_router.py b/dimos/navigation/patrolling/routers/random_patrol_router.py index 67e6f8e25c6..0b547a49c37 100644 --- a/dimos/navigation/patrolling/routers/random_patrol_router.py +++ b/dimos/navigation/patrolling/routers/random_patrol_router.py @@ -12,12 +12,13 @@ # See the License for the specific language governing permissions and # limitations under the License. +from dimos_generated.geometry_msgs.msg import Point, PoseStamped +from dimos_generated.nav_msgs.msg import OccupancyGrid import numpy as np from numpy.typing import NDArray from scipy.ndimage import binary_erosion -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.nav_msgs.OccupancyGrid import OccupancyGrid +from dimos.msgs.occupancy import grid_to_world, occupancy_view from dimos.navigation.patrolling.routers.base_patrol_router import BasePatrolRouter from dimos.navigation.patrolling.utilities import point_to_pose_stamped @@ -34,17 +35,17 @@ def next_goal(self) -> PoseStamped | None: ) if point is None: return None - return point_to_pose_stamped(point) + return point_to_pose_stamped(point, occupancy_grid.header) def _random_empty_spot( occupancy_grid: OccupancyGrid, clearance_m: float, visited: NDArray[np.bool_] | None = None, -) -> tuple[float, float] | None: - clearance_cells = int(np.ceil(clearance_m / occupancy_grid.resolution)) +) -> Point | None: + clearance_cells = int(np.ceil(clearance_m / occupancy_grid.info.resolution)) - free_mask = occupancy_grid.grid == 0 + free_mask = occupancy_view(occupancy_grid) == 0 if not np.any(free_mask): return None @@ -64,5 +65,5 @@ def _random_empty_spot( idx = safe_indices[np.random.randint(len(safe_indices))] row, col = idx - world = occupancy_grid.grid_to_world((col, row, 0)) - return (world.x, world.y) + world = grid_to_world(occupancy_grid, (col, row)) + return world diff --git a/dimos/navigation/patrolling/routers/visitation_history.py b/dimos/navigation/patrolling/routers/visitation_history.py index 939da19ddbb..e3d9a65e015 100644 --- a/dimos/navigation/patrolling/routers/visitation_history.py +++ b/dimos/navigation/patrolling/routers/visitation_history.py @@ -12,10 +12,12 @@ # See the License for the specific language governing permissions and # limitations under the License. +from dimos_generated.geometry_msgs.msg import Point +from dimos_generated.nav_msgs.msg import OccupancyGrid import numpy as np from numpy.typing import NDArray -from dimos.msgs.nav_msgs.OccupancyGrid import OccupancyGrid +from dimos.msgs.occupancy import occupancy_view, world_to_grid def _circular_disk(radius_cells: int) -> NDArray[np.bool_]: @@ -57,7 +59,7 @@ def clearance_disk(self) -> NDArray[np.bool_]: def update_grid(self, grid: OccupancyGrid) -> None: self._grid = grid - self._clearance_radius_cells = int(np.ceil(self._clearance_radius_m / grid.resolution)) + self._clearance_radius_cells = int(np.ceil(self._clearance_radius_m / grid.info.resolution)) self._clearance_disk = _circular_disk(self._clearance_radius_cells) self._rebuild() @@ -80,7 +82,7 @@ def get_saturation(self) -> float: visited = self._visited if grid is None or visited is None: return 0.0 - free_mask = grid.grid == 0 + free_mask = occupancy_view(grid) == 0 total = int(np.count_nonzero(free_mask)) if total == 0: return 0.0 @@ -96,7 +98,7 @@ def _rebuild(self) -> None: grid = self._grid if grid is None: return - self._visited = np.zeros((grid.height, grid.width), dtype=bool) + self._visited = np.zeros((grid.info.height, grid.info.width), dtype=bool) for x, y in self._points: self._stamp(x, y) @@ -106,14 +108,14 @@ def _stamp(self, x: float, y: float) -> None: if grid is None or visited is None: return r = self._clearance_radius_cells - grid_pos = grid.world_to_grid((x, y)) - col, row = int(grid_pos.x), int(grid_pos.y) - if row + r < 0 or row - r >= grid.height or col + r < 0 or col - r >= grid.width: + grid_pos = world_to_grid(grid, Point(x=x, y=y)) + col, row = int(np.floor(round(grid_pos[0], 9))), int(np.floor(round(grid_pos[1], 9))) + if row + r < 0 or row - r >= grid.info.height or col + r < 0 or col - r >= grid.info.width: return r_min = max(0, row - r) - r_max = min(grid.height, row + r + 1) + r_max = min(grid.info.height, row + r + 1) c_min = max(0, col - r) - c_max = min(grid.width, col + r + 1) + c_max = min(grid.info.width, col + r + 1) d_r_min = r_min - (row - r) d_r_max = d_r_min + (r_max - r_min) d_c_min = c_min - (col - r) diff --git a/dimos/navigation/patrolling/test_create_patrol_router.py b/dimos/navigation/patrolling/test_create_patrol_router.py index fa511695b7b..5572e6c72f8 100644 --- a/dimos/navigation/patrolling/test_create_patrol_router.py +++ b/dimos/navigation/patrolling/test_create_patrol_router.py @@ -16,6 +16,9 @@ import os import cv2 +from dimos_generated.geometry_msgs.msg import Point +from dimos_generated.nav_msgs.msg import OccupancyGrid +from dimos_generated.std_msgs.msg import Header import numpy as np import pytest @@ -24,8 +27,9 @@ from dimos.mapping.occupancy.visualizations import visualize_occupancy_grid from dimos.mapping.pointclouds.occupancy import height_cost_occupancy from dimos.mapping.pointclouds.util import read_pointcloud -from dimos.msgs.nav_msgs.OccupancyGrid import OccupancyGrid -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 +from dimos.msgs.image import image_view +from dimos.msgs.occupancy import world_to_grid +from dimos.msgs.pointcloud import pointcloud_from_xyz from dimos.navigation.patrolling.create_patrol_router import create_patrol_router from dimos.navigation.patrolling.utilities import point_to_pose_stamped from dimos.navigation.replanning_a_star.min_cost_astar import min_cost_astar @@ -35,7 +39,7 @@ @pytest.fixture def big_office() -> OccupancyGrid: data = read_pointcloud(get_data("big_office.ply")) - cloud = PointCloud2.from_numpy(np.asarray(data.points), frame_id="") + cloud = pointcloud_from_xyz(np.asarray(data.points), header=Header(frame_id="map")) return height_cost_occupancy(cloud) @@ -44,13 +48,13 @@ def big_office() -> OccupancyGrid: "router_name, saturation", [("random", 0.20), ("coverage", 0.30), ("frontier", 0.20)] ) def test_patrolling_coverage(router_name, saturation, big_office) -> None: - start = (-1.03, -13.48) + start = Point(x=-1.03, y=-13.48) robot_width = 0.4 multiplier = 1.5 big_office_gradient = gradient(big_office, max_distance=1.5) router = create_patrol_router(router_name, robot_width * multiplier) router.handle_occupancy_grid(big_office) - router.handle_odom(point_to_pose_stamped(start)) + router.handle_odom(point_to_pose_stamped(start, big_office.header)) all_poses: list = [] for _ in range(15): @@ -58,15 +62,15 @@ def test_patrolling_coverage(router_name, saturation, big_office) -> None: if goal is None: continue path = min_cost_astar( - big_office_gradient, goal.position, start, unknown_penalty=1.0, use_cpp=True + big_office_gradient, goal.pose.position, start, unknown_penalty=1.0, use_cpp=True ) if path is None: continue - path = smooth_resample_path(path, goal, 0.1) + path = smooth_resample_path(path, goal.pose, 0.1) for pose in path.poses: router.handle_odom(pose) all_poses.append(pose) - start = (path.poses[-1].position.x, path.poses[-1].position.y) + start = path.poses[-1].pose.position assert router.get_saturation() > saturation @@ -76,14 +80,15 @@ def test_patrolling_coverage(router_name, saturation, big_office) -> None: def _save_coverage_image(router_name, router, all_poses, big_office, big_office_gradient) -> None: image = visualize_occupancy_grid(big_office_gradient, "rainbow") - h, w = image.data.shape[:2] + pixels = image_view(image) + h, w = pixels.shape[:2] visit_counts = np.zeros((h, w), dtype=np.float32) - radius = int(np.ceil(router._clearance_radius_m / big_office.resolution)) + radius = int(np.ceil(router._clearance_radius_m / big_office.info.resolution)) stamp = np.zeros((h, w), dtype=np.uint8) for pose in all_poses: - grid = big_office.world_to_grid((pose.position.x, pose.position.y)) - gx, gy = int(grid.x), int(grid.y) + grid = world_to_grid(big_office, pose.pose.position) + gx, gy = int(grid[0]), int(grid[1]) if 0 <= gy < h and 0 <= gx < w: stamp[:] = 0 cv2.circle(stamp, (gx, gy), radius, 1, -1) @@ -93,9 +98,8 @@ def _save_coverage_image(router_name, router, all_poses, big_office, big_office_ mask = visit_counts > 0 blend = 1.0 - (1.0 - alpha) ** visit_counts - overlay = image.data.astype(np.float32) * 0.24 + overlay = pixels.astype(np.float32) * 0.24 for c in range(3): overlay[:, :, c][mask] = overlay[:, :, c][mask] * (1.0 - blend[mask]) + 255.0 * blend[mask] - image.data = overlay.astype(np.uint8) - image.save(f"patrolling_coverage_{router_name}.png") + cv2.imwrite(f"patrolling_coverage_{router_name}.png", overlay.astype(np.uint8)) diff --git a/dimos/navigation/patrolling/test_generated_patrol.py b/dimos/navigation/patrolling/test_generated_patrol.py new file mode 100644 index 00000000000..9a2b3717796 --- /dev/null +++ b/dimos/navigation/patrolling/test_generated_patrol.py @@ -0,0 +1,66 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + + +import math + +from dimos_generated.builtin_interfaces.msg import Time +from dimos_generated.geometry_msgs.msg import Point, Pose, PoseStamped +from dimos_generated.nav_msgs.msg import MapMetaData, OccupancyGrid +from dimos_generated.std_msgs.msg import Header +import numpy as np +import pytest + +from dimos.msgs.geometry import quaternion_from_euler +from dimos.msgs.occupancy import grid_to_world, occupancy_view, world_to_grid +from dimos.navigation.patrolling.create_patrol_router import create_patrol_router +from dimos.navigation.patrolling.utilities import point_to_pose_stamped + + +@pytest.mark.parametrize("router_name", ["random", "coverage", "frontier"]) +@pytest.mark.parametrize("yaw", [0, math.pi / 2]) +def test_generated_patrol_goals(router_name, yaw): + cells = np.zeros((24, 24), dtype=np.int8) + cells[[0, -1], :] = 100 + cells[:, [0, -1]] = 100 + grid = OccupancyGrid( + header=Header(frame_id="map", stamp=Time(sec=1700000000, nanosec=123456789)), + info=MapMetaData( + width=24, + height=24, + resolution=0.5, + origin=Pose( + position=Point(x=2, y=3, z=1), orientation=quaternion_from_euler(0, 0, yaw) + ), + ), + data=cells.ravel(), + ) + router = create_patrol_router(router_name, 0.5) + assert router.next_goal() is None + received = OccupancyGrid.decode(grid.encode()) + router.handle_occupancy_grid(received) + router.handle_odom(point_to_pose_stamped(grid_to_world(received, (12, 12)), received.header)) + initial = router.get_saturation() + goal = router.next_goal() + assert goal is not None + decoded = PoseStamped.decode(goal.encode()) + assert decoded.header == grid.header + assert decoded.pose.position.z == pytest.approx(1) + x, y = world_to_grid(received, decoded.pose.position) + assert occupancy_view(received)[round(y), round(x)] == 0 + router.handle_odom(decoded) + assert router.get_saturation() > initial + router.reset() + assert router.next_goal() is None + assert router.get_saturation() == 0 diff --git a/dimos/navigation/patrolling/utilities.py b/dimos/navigation/patrolling/utilities.py index d7caffaa9c4..df9805bc85e 100644 --- a/dimos/navigation/patrolling/utilities.py +++ b/dimos/navigation/patrolling/utilities.py @@ -12,15 +12,13 @@ # See the License for the specific language governing permissions and # limitations under the License. -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped +from dimos_generated.geometry_msgs.msg import Point, Pose, PoseStamped, Quaternion +from dimos_generated.std_msgs.msg import Header -def point_to_pose_stamped(point: tuple[float, float]) -> PoseStamped: - pose = PoseStamped() - pose.position.x = point[0] - pose.position.y = point[1] - return pose +def point_to_pose_stamped(point: Point, header: Header) -> PoseStamped: + return PoseStamped(header=header, pose=Pose(position=point, orientation=Quaternion(w=1))) def pose_stamped_to_point(pose: PoseStamped) -> tuple[float, float]: - return (pose.position.x, pose.position.y) + return (pose.pose.position.x, pose.pose.position.y) diff --git a/dimos/navigation/replanning_a_star/controllers.py b/dimos/navigation/replanning_a_star/controllers.py index c9902e278a4..1b7f269d096 100644 --- a/dimos/navigation/replanning_a_star/controllers.py +++ b/dimos/navigation/replanning_a_star/controllers.py @@ -15,13 +15,12 @@ import math from typing import Protocol +from dimos_generated.geometry_msgs.msg import PoseStamped, Twist, Vector3 import numpy as np from numpy.typing import NDArray from dimos.core.global_config import GlobalConfig -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.geometry_msgs.Twist import Twist -from dimos.msgs.geometry_msgs.Vector3 import Vector3 +from dimos.msgs.geometry import quaternion_euler from dimos.utils.trigonometry import angle_diff @@ -52,7 +51,7 @@ def __init__(self, global_config: GlobalConfig, speed: float, control_frequency: self._control_frequency = control_frequency def advance(self, lookahead_point: NDArray[np.float64], current_odom: PoseStamped) -> Twist: - current_pos = np.array([current_odom.position.x, current_odom.position.y]) + current_pos = np.array([current_odom.pose.position.x, current_odom.pose.position.y]) direction = lookahead_point - current_pos distance = np.linalg.norm(direction) @@ -60,7 +59,7 @@ def advance(self, lookahead_point: NDArray[np.float64], current_odom: PoseStampe # Robot is coincidentally at the lookahead point; skip this cycle. return Twist() - robot_yaw = current_odom.orientation.euler[2] + robot_yaw = quaternion_euler(current_odom.pose.orientation)[2] desired_yaw = np.arctan2(direction[1], direction[0]) yaw_error = angle_diff(desired_yaw, robot_yaw) @@ -75,8 +74,8 @@ def advance(self, lookahead_point: NDArray[np.float64], current_odom: PoseStampe linear_velocity = self._apply_min_velocity(linear_velocity, self._min_linear_velocity) return Twist( - linear=Vector3(linear_velocity, 0.0, 0.0), - angular=Vector3(0.0, 0.0, angular_velocity), + linear=Vector3(x=linear_velocity), + angular=Vector3(z=angular_velocity), ) def rotate(self, yaw_error: float) -> Twist: @@ -109,8 +108,8 @@ def _angular_twist(self, angular_velocity: float) -> Twist: angular_velocity = 0.8 * np.sign(angular_velocity) return Twist( - linear=Vector3(0.0, 0.0, 0.0), - angular=Vector3(0.0, 0.0, angular_velocity), + linear=Vector3(), + angular=Vector3(z=angular_velocity), ) diff --git a/dimos/navigation/replanning_a_star/global_planner.py b/dimos/navigation/replanning_a_star/global_planner.py index 7658589e302..5d33d6073bf 100644 --- a/dimos/navigation/replanning_a_star/global_planner.py +++ b/dimos/navigation/replanning_a_star/global_planner.py @@ -16,7 +16,9 @@ from threading import Event, RLock, Thread, current_thread import time -from dimos_lcm.std_msgs import Bool +from dimos_generated.geometry_msgs.msg import Point, PoseStamped, Twist +from dimos_generated.nav_msgs.msg import OccupancyGrid, Path +from dimos_generated.std_msgs.msg import Bool import numpy as np from reactivex import Subject from reactivex.disposable import CompositeDisposable @@ -25,11 +27,8 @@ from dimos.core.global_config import GlobalConfig from dimos.core.resource import Resource from dimos.mapping.occupancy.path_resampling import smooth_resample_path -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.geometry_msgs.Twist import Twist -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.msgs.nav_msgs.OccupancyGrid import CostValues, OccupancyGrid -from dimos.msgs.nav_msgs.Path import Path +from dimos.msgs.geometry import point_distance, quaternion_euler +from dimos.msgs.occupancy import occupancy_view, world_to_grid from dimos.navigation.base import NavigationState from dimos.navigation.replanning_a_star.goal_validator import find_safe_goal from dimos.navigation.replanning_a_star.local_planner import LocalPlanner, StopMessage @@ -167,7 +166,7 @@ def cancel_goal(self, *, but_will_try_again: bool = False, arrived: bool = False self._local_planner.stop_planning() if not but_will_try_again: - self.goal_reached.on_next(Bool(arrived)) + self.goal_reached.on_next(Bool(data=arrived)) def set_replanning_enabled(self, enabled: bool) -> None: with self._lock: @@ -221,9 +220,13 @@ def _thread_entrypoint(self) -> None: continue if ( - current_goal.position.distance(current_odom.position) < self._goal_tolerance + point_distance(current_goal.pose.position, current_odom.pose.position) + < self._goal_tolerance and abs( - angle_diff(current_goal.orientation.euler[2], current_odom.orientation.euler[2]) + angle_diff( + quaternion_euler(current_goal.pose.orientation)[2], + quaternion_euler(current_odom.pose.orientation)[2], + ) ) < self._rotation_tolerance ): @@ -293,7 +296,10 @@ def _replan_path(self) -> None: assert current_odom is not None assert current_goal is not None - if current_goal.position.distance(current_odom.position) < self._replan_goal_tolerance: + if ( + point_distance(current_goal.pose.position, current_odom.pose.position) + < self._replan_goal_tolerance + ): self.cancel_goal(arrived=True) return @@ -301,7 +307,7 @@ def _replan_path(self) -> None: self.cancel_goal() return - if not self._replan_limiter.can_retry(current_odom.position): + if not self._replan_limiter.can_retry(current_odom.pose.position): self.cancel_goal() return @@ -322,16 +328,18 @@ def _plan_path(self) -> None: logger.warning("Cannot handle goal request: missing odometry.") return - safe_goal = self._find_safe_goal(current_goal.position) + safe_goal = self._find_safe_goal(current_goal.pose.position) if not safe_goal: logger.warning( - "No safe goal found.", x=round(current_goal.x, 3), y=round(current_goal.y, 3) + "No safe goal found.", + x=round(current_goal.pose.position.x, 3), + y=round(current_goal.pose.position.y, 3), ) self.cancel_goal() return - path = self._find_wide_path(safe_goal, current_odom.position) + path = self._find_wide_path(safe_goal, current_odom.pose.position) if not path: logger.warning( @@ -340,18 +348,18 @@ def _plan_path(self) -> None: self.cancel_goal() return - resampled_path = smooth_resample_path(path, current_goal, 0.1) + resampled_path = smooth_resample_path(path, current_goal.pose, 0.1) self.path.on_next(resampled_path) self._local_planner.start_planning(resampled_path) - def _find_wide_path(self, goal: Vector3, robot_pos: Vector3) -> Path | None: + def _find_wide_path(self, goal: Point, robot_pos: Point) -> Path | None: # sizes_to_try: list[float] = [2.2, 1.7, 1.3, 1] sizes_to_try: list[float] = [1.1] for size in sizes_to_try: - distance = robot_pos.distance(goal) + distance = point_distance(robot_pos, goal) navigation_map = self._navigation_map if distance > 1.5 else self._navigation_map_near costmap = navigation_map.make_gradient_costmap(size) self._clear_robot_footprint(costmap, navigation_map.binary_costmap, robot_pos) @@ -363,7 +371,7 @@ def _find_wide_path(self, goal: Vector3, robot_pos: Vector3) -> Path | None: return None def _clear_robot_footprint( - self, costmap: OccupancyGrid, binary: OccupancyGrid, robot_pos: Vector3 + self, costmap: OccupancyGrid, binary: OccupancyGrid, robot_pos: Point ) -> None: """Make the cells under the robot passable for planning. @@ -376,39 +384,54 @@ def _clear_robot_footprint( the raw binary costmap (actually observed obstacle points) stay blocked. """ - if binary.grid.shape != costmap.grid.shape or binary.origin != costmap.origin: + if ( + occupancy_view(binary).shape != occupancy_view(costmap).shape + or binary.info.origin != costmap.info.origin + or binary.info.resolution != costmap.info.resolution + or binary.header.frame_id != costmap.header.frame_id + ): # A newer map update raced in between building the two grids; # skip clearing rather than misalign cells. return - center = costmap.world_to_grid(robot_pos) - center_x, center_y = int(center.x), int(center.y) - cells = int(self._global_config.robot_rotation_diameter / 2 / costmap.resolution) + 1 + center = world_to_grid(costmap, robot_pos) + center_x, center_y = int(np.floor(round(center[0], 9))), int(np.floor(round(center[1], 9))) + cells = int(self._global_config.robot_rotation_diameter / 2 / costmap.info.resolution) + 1 - height, width = costmap.grid.shape + height, width = occupancy_view(costmap).shape y0, y1 = max(0, center_y - cells), min(height, center_y + cells + 1) x0, x1 = max(0, center_x - cells), min(width, center_x + cells + 1) if y0 >= y1 or x0 >= x1: return - region = costmap.grid[y0:y1, x0:x1] - binary_region = binary.grid[y0:y1, x0:x1] + mutable_cells = occupancy_view(costmap).copy() + region = mutable_cells[y0:y1, x0:x1] + binary_region = occupancy_view(binary)[y0:y1, x0:x1] rows, columns = np.ogrid[y0:y1, x0:x1] disc = (rows - center_y) ** 2 + (columns - center_x) ** 2 <= cells**2 - clearable = disc & (region >= CostValues.OCCUPIED) & (binary_region < CostValues.OCCUPIED) - region[clearable] = CostValues.OCCUPIED - 1 - - def _find_safe_goal(self, goal: Vector3) -> Vector3 | None: + clearable = disc & (region >= 100) & (binary_region < 100) + region[clearable] = 99 + costmap.data = mutable_cells.ravel() + + @staticmethod + def _goal_is_unknown(costmap: OccupancyGrid, goal: Point) -> bool: + x, y = world_to_grid(costmap, goal) + gx, gy = math.floor(round(x, 9)), math.floor(round(y, 9)) + if not (0 <= gx < costmap.info.width and 0 <= gy < costmap.info.height): + return True + return bool(occupancy_view(costmap)[gy, gx] == -1) + + def _find_safe_goal(self, goal: Point) -> Point | None: costmap = self._navigation_map.binary_costmap - if costmap.cell_value(goal) == CostValues.UNKNOWN: + if self._goal_is_unknown(costmap, goal): return goal safe_goal = find_safe_goal( costmap, goal, algorithm="bfs_contiguous", - cost_threshold=CostValues.OCCUPIED, + cost_threshold=100, min_clearance=self._safe_goal_clearance, max_search_distance=self._safe_goal_tolerance, ) @@ -417,7 +440,7 @@ def _find_safe_goal(self, goal: Vector3) -> Vector3 | None: logger.warning("No safe goal found near requested target.") return None - goals_distance = safe_goal.distance(goal) + goals_distance = point_distance(safe_goal, goal) if goals_distance > 0.2: logger.warning(f"Travelling to goal {goals_distance}m away from requested goal.") diff --git a/dimos/navigation/replanning_a_star/goal_validator.py b/dimos/navigation/replanning_a_star/goal_validator.py index b717c762952..cb20eb7342a 100644 --- a/dimos/navigation/replanning_a_star/goal_validator.py +++ b/dimos/navigation/replanning_a_star/goal_validator.py @@ -14,21 +14,22 @@ from collections import deque +from dimos_generated.geometry_msgs.msg import Point +from dimos_generated.nav_msgs.msg import OccupancyGrid import numpy as np -from dimos.msgs.geometry_msgs.Vector3 import Vector3, VectorLike -from dimos.msgs.nav_msgs.OccupancyGrid import CostValues, OccupancyGrid +from dimos.msgs.occupancy import grid_to_world, occupancy_view, world_to_grid def find_safe_goal( costmap: OccupancyGrid, - goal: VectorLike, + goal: Point, algorithm: str = "bfs", cost_threshold: int = 50, min_clearance: float = 0.3, max_search_distance: float = 5.0, connectivity_check_radius: int = 3, -) -> Vector3 | None: +) -> Point | None: """ Find a safe goal position when the original goal is in collision or too close to obstacles. @@ -69,12 +70,12 @@ def find_safe_goal( def _find_safe_goal_bfs( costmap: OccupancyGrid, - goal: VectorLike, + goal: Point, cost_threshold: int, min_clearance: float, max_search_distance: float, connectivity_check_radius: int, -) -> Vector3 | None: +) -> Point | None: """ BFS-based search for nearest safe goal position. This guarantees finding the closest valid position. @@ -90,12 +91,12 @@ def _find_safe_goal_bfs( """ # Convert goal to grid coordinates - goal_grid = costmap.world_to_grid(goal) - gx, gy = int(goal_grid.x), int(goal_grid.y) + goal_grid = world_to_grid(costmap, goal) + gx, gy = int(np.floor(round(goal_grid[0], 9))), int(np.floor(round(goal_grid[1], 9))) # Convert distances to grid cells - clearance_cells = int(np.ceil(min_clearance / costmap.resolution)) - max_search_cells = int(np.ceil(max_search_distance / costmap.resolution)) + clearance_cells = int(np.ceil(min_clearance / costmap.info.resolution)) + max_search_cells = int(np.ceil(max_search_distance / costmap.info.resolution)) # BFS queue and visited set queue = deque([(gx, gy, 0)]) @@ -116,14 +117,14 @@ def _find_safe_goal_bfs( costmap, x, y, cost_threshold, clearance_cells, connectivity_check_radius ): # Convert back to world coordinates - return costmap.grid_to_world((x, y)) + return grid_to_world(costmap, (x, y)) # Add neighbors to queue for dx, dy in neighbors: nx, ny = x + dx, y + dy # Check bounds - if 0 <= nx < costmap.width and 0 <= ny < costmap.height: + if 0 <= nx < costmap.info.width and 0 <= ny < costmap.info.height: if (nx, ny) not in visited: visited.add((nx, ny)) queue.append((nx, ny, dist + 1)) @@ -133,12 +134,12 @@ def _find_safe_goal_bfs( def _find_safe_goal_bfs_contiguous( costmap: OccupancyGrid, - goal: VectorLike, + goal: Point, cost_threshold: int, min_clearance: float, max_search_distance: float, connectivity_check_radius: int, -) -> Vector3 | None: +) -> Point | None: """ BFS-based search for nearest safe goal position, only following passable cells. Unlike regular BFS, this only expands through cells with occupancy < 100, @@ -154,13 +155,15 @@ def _find_safe_goal_bfs_contiguous( - Slightly slower than regular BFS due to additional checks """ + grid = occupancy_view(costmap) + # Convert goal to grid coordinates - goal_grid = costmap.world_to_grid(goal) - gx, gy = int(goal_grid.x), int(goal_grid.y) + goal_grid = world_to_grid(costmap, goal) + gx, gy = int(np.floor(round(goal_grid[0], 9))), int(np.floor(round(goal_grid[1], 9))) # Convert distances to grid cells - clearance_cells = int(np.ceil(min_clearance / costmap.resolution)) - max_search_cells = int(np.ceil(max_search_distance / costmap.resolution)) + clearance_cells = int(np.ceil(min_clearance / costmap.info.resolution)) + max_search_cells = int(np.ceil(max_search_distance / costmap.info.resolution)) # BFS queue and visited set queue = deque([(gx, gy, 0)]) @@ -181,17 +184,17 @@ def _find_safe_goal_bfs_contiguous( costmap, x, y, cost_threshold, clearance_cells, connectivity_check_radius ): # Convert back to world coordinates - return costmap.grid_to_world((x, y)) + return grid_to_world(costmap, (x, y)) # Add neighbors to queue for dx, dy in neighbors: nx, ny = x + dx, y + dy # Check bounds - if 0 <= nx < costmap.width and 0 <= ny < costmap.height: + if 0 <= nx < costmap.info.width and 0 <= ny < costmap.info.height: if (nx, ny) not in visited: # Only expand through passable cells (occupancy < 100) - if costmap.grid[ny, nx] < 100: + if grid[ny, nx] < 100: visited.add((nx, ny)) queue.append((nx, ny, dist + 1)) @@ -220,22 +223,24 @@ def _is_position_safe( True if position is safe, False otherwise """ + grid = occupancy_view(costmap) + # Check bounds first - if not (0 <= x < costmap.width and 0 <= y < costmap.height): + if not (0 <= x < costmap.info.width and 0 <= y < costmap.info.height): return False # Check if position itself is free - if costmap.grid[y, x] >= cost_threshold or costmap.grid[y, x] == CostValues.UNKNOWN: + if grid[y, x] >= cost_threshold or grid[y, x] == -1: return False # Check clearance around position for dy in range(-clearance_cells, clearance_cells + 1): for dx in range(-clearance_cells, clearance_cells + 1): nx, ny = x + dx, y + dy - if 0 <= nx < costmap.width and 0 <= ny < costmap.height: + if 0 <= nx < costmap.info.width and 0 <= ny < costmap.info.height: # Check if within circular clearance if dx * dx + dy * dy <= clearance_cells * clearance_cells: - if costmap.grid[ny, nx] >= cost_threshold: + if grid[ny, nx] >= cost_threshold: return False # Check connectivity (not surrounded by obstacles) @@ -249,12 +254,9 @@ def _is_position_safe( continue nx, ny = x + dx, y + dy - if 0 <= nx < costmap.width and 0 <= ny < costmap.height: + if 0 <= nx < costmap.info.width and 0 <= ny < costmap.info.height: total_count += 1 - if ( - costmap.grid[ny, nx] < cost_threshold - and costmap.grid[ny, nx] != CostValues.UNKNOWN - ): + if grid[ny, nx] < cost_threshold and grid[ny, nx] != -1: free_count += 1 # Require at least 50% of neighbors to be free (not surrounded) diff --git a/dimos/navigation/replanning_a_star/local_planner.py b/dimos/navigation/replanning_a_star/local_planner.py index 451630414cb..753cbdbd219 100644 --- a/dimos/navigation/replanning_a_star/local_planner.py +++ b/dimos/navigation/replanning_a_star/local_planner.py @@ -13,20 +13,20 @@ # limitations under the License. import os -from threading import Event, RLock, Thread +from threading import Event, RLock, Thread, current_thread import time import traceback from typing import Literal, TypeAlias +from dimos_generated.geometry_msgs.msg import PoseStamped, Twist +from dimos_generated.nav_msgs.msg import OccupancyGrid, Path import numpy as np from reactivex import Subject +from dimos.constants import DEFAULT_THREAD_JOIN_TIMEOUT from dimos.core.global_config import GlobalConfig from dimos.core.resource import Resource -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.geometry_msgs.Twist import Twist -from dimos.msgs.nav_msgs.OccupancyGrid import OccupancyGrid -from dimos.msgs.nav_msgs.Path import Path +from dimos.msgs.geometry import point_distance, quaternion_euler from dimos.navigation.base import NavigationState from dimos.navigation.replanning_a_star.controllers import Controller, PController from dimos.navigation.replanning_a_star.navigation_map import NavigationMap @@ -120,13 +120,17 @@ def start_planning(self, path: Path) -> None: self._thread.start() def stop_planning(self) -> None: - self.cmd_vel.on_next(Twist()) self._stop_planning_event.set() - + with self._lock: + thread = self._thread + if thread is not None and thread is not current_thread(): + thread.join(DEFAULT_THREAD_JOIN_TIMEOUT) + if thread.is_alive(): + raise RuntimeError("LocalPlanner thread did not stop in time") with self._lock: self._thread = None - self._reset_state() + self.cmd_vel.on_next(Twist()) def get_state(self) -> NavigationState: with self._lock: @@ -176,14 +180,15 @@ def _loop(self) -> None: # Determine initial state: skip initial_rotation if already aligned. new_state: PlannerState = "initial_rotation" if current_odom is not None and len(path.poses) > 0: - first_yaw = path.poses[0].orientation.euler[2] - robot_yaw = current_odom.orientation.euler[2] + first_yaw = quaternion_euler(path.poses[0].pose.orientation)[2] + robot_yaw = quaternion_euler(current_odom.pose.orientation)[2] initial_yaw_error = angle_diff(first_yaw, robot_yaw) self._controller.reset_yaw_error(initial_yaw_error) angle_in_tolerance = abs(initial_yaw_error) < self._orientation_tolerance if angle_in_tolerance: position_in_tolerance = ( - path.poses[0].position.distance(current_odom.position) < 0.01 + point_distance(path.poses[-1].pose.position, current_odom.pose.position) + < self._goal_tolerance ) if position_in_tolerance: new_state = "final_rotation" @@ -241,8 +246,8 @@ def _compute_initial_rotation(self) -> Twist: assert current_odom is not None first_pose = path.poses[0] - first_yaw = first_pose.orientation.euler[2] - robot_yaw = current_odom.orientation.euler[2] + first_yaw = quaternion_euler(first_pose.pose.orientation)[2] + robot_yaw = quaternion_euler(current_odom.pose.orientation)[2] yaw_error = angle_diff(first_yaw, robot_yaw) if abs(yaw_error) < self._orientation_tolerance: @@ -260,7 +265,7 @@ def get_distance_to_path(self) -> float | None: if path_distancer is None or current_odom is None: return None - current_pos = np.array([current_odom.position.x, current_odom.position.y]) + current_pos = np.array([current_odom.pose.position.x, current_odom.pose.position.y]) return path_distancer.get_distance_to_path(current_pos) @@ -272,7 +277,7 @@ def _compute_path_following(self) -> Twist: assert path_distancer is not None assert current_odom is not None - current_pos = np.array([current_odom.position.x, current_odom.position.y]) + current_pos = np.array([current_odom.pose.position.x, current_odom.pose.position.y]) if path_distancer.distance_to_goal(current_pos) < self._goal_tolerance: logger.info("Reached goal position, starting final rotation") @@ -297,8 +302,8 @@ def _compute_final_rotation(self) -> Twist: assert path is not None assert current_odom is not None - goal_yaw = path.poses[-1].orientation.euler[2] - robot_yaw = current_odom.orientation.euler[2] + goal_yaw = quaternion_euler(path.poses[-1].pose.orientation)[2] + robot_yaw = quaternion_euler(current_odom.pose.orientation)[2] yaw_error = angle_diff(goal_yaw, robot_yaw) if abs(yaw_error) < self._orientation_tolerance: diff --git a/dimos/navigation/replanning_a_star/min_cost_astar.py b/dimos/navigation/replanning_a_star/min_cost_astar.py index 03212e87b80..ac6fd984ee9 100644 --- a/dimos/navigation/replanning_a_star/min_cost_astar.py +++ b/dimos/navigation/replanning_a_star/min_cost_astar.py @@ -13,12 +13,12 @@ # limitations under the License. import heapq +import math -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Vector3 import VectorLike -from dimos.msgs.nav_msgs.OccupancyGrid import CostValues, OccupancyGrid -from dimos.msgs.nav_msgs.Path import Path +from dimos_generated.geometry_msgs.msg import Point, Pose, PoseStamped, Quaternion +from dimos_generated.nav_msgs.msg import OccupancyGrid, Path + +from dimos.msgs.occupancy import grid_to_world, occupancy_view, world_to_grid from dimos.utils.logging_config import setup_logger # Try to import C++ extension for faster pathfinding @@ -65,88 +65,55 @@ def _reconstruct_path( parents: dict[tuple[int, int], tuple[int, int]], current: tuple[int, int], costmap: OccupancyGrid, - start_tuple: tuple[int, int], - goal_tuple: tuple[int, int], ) -> Path: - frame_id = costmap.frame_id - waypoints: list[PoseStamped] = [] + coordinates = [current] while current in parents: - world_point = costmap.grid_to_world(current) - pose = PoseStamped( - frame_id=frame_id, - position=[world_point.x, world_point.y, 0.0], - orientation=Quaternion(0, 0, 0, 1), # Identity quaternion - ) - waypoints.append(pose) current = parents[current] - - start_world_point = costmap.grid_to_world(start_tuple) - start_pose = PoseStamped( - frame_id=frame_id, - position=[start_world_point.x, start_world_point.y, 0.0], - orientation=Quaternion(0, 0, 0, 1), - ) - waypoints.append(start_pose) - - waypoints.reverse() - - # Add the goal position if it's not already included - goal_point = costmap.grid_to_world(goal_tuple) - - if ( - not waypoints - or (waypoints[-1].x - goal_point.x) ** 2 + (waypoints[-1].y - goal_point.y) ** 2 > 1e-10 - ): - goal_pose = PoseStamped( - frame_id=frame_id, - position=[goal_point.x, goal_point.y, 0.0], - orientation=Quaternion(0, 0, 0, 1), - ) - waypoints.append(goal_pose) - - return Path(frame_id=frame_id, poses=waypoints) + coordinates.append(current) + coordinates.reverse() + return _reconstruct_path_from_coords(coordinates, costmap) def _reconstruct_path_from_coords( path_coords: list[tuple[int, int]], costmap: OccupancyGrid, ) -> Path: - frame_id = costmap.frame_id - waypoints: list[PoseStamped] = [] - - for gx, gy in path_coords: - world_point = costmap.grid_to_world((gx, gy)) - pose = PoseStamped( - frame_id=frame_id, - position=[world_point.x, world_point.y, 0.0], - orientation=Quaternion(0, 0, 0, 1), - ) - waypoints.append(pose) - - return Path(frame_id=frame_id, poses=waypoints) + return Path( + header=costmap.header, + poses=[ + PoseStamped( + header=costmap.header, + pose=Pose(position=grid_to_world(costmap, coordinate), orientation=Quaternion(w=1)), + ) + for coordinate in path_coords + ], + ) def min_cost_astar( costmap: OccupancyGrid, - goal: VectorLike, - start: VectorLike = (0.0, 0.0), + goal: Point, + start: Point | None = None, cost_threshold: int = 100, unknown_penalty: float = 0.8, use_cpp: bool = True, ) -> Path | None: - start_vector = costmap.world_to_grid(start) - goal_vector = costmap.world_to_grid(goal) - - start_tuple = (int(start_vector.x), int(start_vector.y)) - goal_tuple = (int(goal_vector.x), int(goal_vector.y)) - - if not (0 <= goal_tuple[0] < costmap.width and 0 <= goal_tuple[1] < costmap.height): + cells = occupancy_view(costmap) + start_vector = world_to_grid(costmap, start if start is not None else Point()) + goal_vector = world_to_grid(costmap, goal) + # Ignore sub-nanocell rotation roundoff before assigning a point to its cell. + start_tuple = (math.floor(round(start_vector[0], 9)), math.floor(round(start_vector[1], 9))) + goal_tuple = (math.floor(round(goal_vector[0], 9)), math.floor(round(goal_vector[1], 9))) + + if not (0 <= goal_tuple[0] < costmap.info.width and 0 <= goal_tuple[1] < costmap.info.height): + return None + if not (0 <= start_tuple[0] < costmap.info.width and 0 <= start_tuple[1] < costmap.info.height): return None if use_cpp: if _USE_CPP: path_coords = _astar_cpp( - costmap.grid, + cells, start_tuple[0], start_tuple[1], goal_tuple[0], @@ -185,7 +152,7 @@ def min_cost_astar( continue if current == goal_tuple: - return _reconstruct_path(parents, current, costmap, start_tuple, goal_tuple) + return _reconstruct_path(parents, current, costmap) closed_set.add(current) @@ -193,22 +160,22 @@ def min_cost_astar( neighbor_x, neighbor_y = current_x + dx, current_y + dy neighbor = (neighbor_x, neighbor_y) - if not (0 <= neighbor_x < costmap.width and 0 <= neighbor_y < costmap.height): + if not (0 <= neighbor_x < costmap.info.width and 0 <= neighbor_y < costmap.info.height): continue if neighbor in closed_set: continue - neighbor_val = costmap.grid[neighbor_y, neighbor_x] + neighbor_val = cells[neighbor_y, neighbor_x] if neighbor_val >= cost_threshold: continue - if neighbor_val == CostValues.UNKNOWN: + if neighbor_val == -1: cell_cost = cost_threshold * unknown_penalty if cell_cost >= cost_threshold: continue - elif neighbor_val == CostValues.FREE: + elif neighbor_val == 0: cell_cost = 0.0 else: cell_cost = neighbor_val diff --git a/dimos/navigation/replanning_a_star/module.py b/dimos/navigation/replanning_a_star/module.py index 9ea8607bb27..984ab633ea9 100644 --- a/dimos/navigation/replanning_a_star/module.py +++ b/dimos/navigation/replanning_a_star/module.py @@ -15,18 +15,14 @@ import os from typing import Any -from dimos_lcm.std_msgs import Bool, String +from dimos_generated.geometry_msgs.msg import PointStamped, Pose, PoseStamped, Quaternion, Twist +from dimos_generated.nav_msgs.msg import OccupancyGrid, Odometry, Path +from dimos_generated.std_msgs.msg import Bool, String from reactivex.disposable import Disposable from dimos.core.core import rpc from dimos.core.module import Module, ModuleConfig from dimos.core.stream import In, Out -from dimos.msgs.geometry_msgs.PointStamped import PointStamped -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.geometry_msgs.Twist import Twist -from dimos.msgs.nav_msgs.OccupancyGrid import OccupancyGrid -from dimos.msgs.nav_msgs.Odometry import Odometry -from dimos.msgs.nav_msgs.Path import Path from dimos.navigation.base import NavigationInterface, NavigationState from dimos.navigation.replanning_a_star.global_planner import GlobalPlanner from dimos.utils.logging_config import setup_logger @@ -81,7 +77,9 @@ def start(self) -> None: self.register_disposable( Disposable( self.odometry.subscribe( - lambda msg: self._planner.handle_odom(msg.to_pose_stamped()) + lambda msg: self._planner.handle_odom( + PoseStamped(header=msg.header, pose=msg.pose.pose) + ) ) ) ) @@ -98,7 +96,12 @@ def start(self) -> None: self.register_disposable( Disposable( self.clicked_point.subscribe( - lambda pt: self._planner.handle_goal_request(pt.to_pose_stamped()) + lambda pt: self._planner.handle_goal_request( + PoseStamped( + header=pt.header, + pose=Pose(position=pt.point, orientation=Quaternion(w=1)), + ) + ) ) ) ) diff --git a/dimos/navigation/replanning_a_star/module_spec.py b/dimos/navigation/replanning_a_star/module_spec.py index c9ec73dc473..a930142b719 100644 --- a/dimos/navigation/replanning_a_star/module_spec.py +++ b/dimos/navigation/replanning_a_star/module_spec.py @@ -14,7 +14,8 @@ from typing import Protocol -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped +from dimos_generated.geometry_msgs.msg import PoseStamped + from dimos.navigation.base import NavigationState from dimos.spec.utils import Spec diff --git a/dimos/navigation/replanning_a_star/navigation_map.py b/dimos/navigation/replanning_a_star/navigation_map.py index fde75c0b0ee..25104c593c2 100644 --- a/dimos/navigation/replanning_a_star/navigation_map.py +++ b/dimos/navigation/replanning_a_star/navigation_map.py @@ -14,10 +14,11 @@ from threading import RLock +from dimos_generated.nav_msgs.msg import OccupancyGrid + from dimos.core.global_config import GlobalConfig from dimos.mapping.occupancy.gradient import GradientStrategy from dimos.mapping.occupancy.path_map import make_navigation_map -from dimos.msgs.nav_msgs.OccupancyGrid import OccupancyGrid class NavigationMap: diff --git a/dimos/navigation/replanning_a_star/path_clearance.py b/dimos/navigation/replanning_a_star/path_clearance.py index 7dc08d49e0c..5774f85f156 100644 --- a/dimos/navigation/replanning_a_star/path_clearance.py +++ b/dimos/navigation/replanning_a_star/path_clearance.py @@ -12,23 +12,25 @@ # See the License for the specific language governing permissions and # limitations under the License. +from copy import deepcopy from threading import RLock +from dimos_generated.nav_msgs.msg import MapMetaData, OccupancyGrid, Path import numpy as np from numpy.typing import NDArray from dimos.core.global_config import GlobalConfig from dimos.mapping.occupancy.path_mask import make_path_mask -from dimos.msgs.nav_msgs.OccupancyGrid import CostValues, OccupancyGrid -from dimos.msgs.nav_msgs.Path import Path +from dimos.msgs.geometry import point_distance +from dimos.msgs.occupancy import occupancy_view class PathClearance: _costmap: OccupancyGrid | None = None - _last_costmap: OccupancyGrid | None = None _path_lookup_distance: float = 3.0 _max_distance_cache: float = 1.0 - _last_used_shape: tuple[int, ...] | None = None + _last_info: MapMetaData | None = None + _last_frame: str | None = None _last_mask: NDArray[np.bool_] | None = None _last_used_pose: int | None = None _global_config: GlobalConfig @@ -61,7 +63,8 @@ def mask(self) -> NDArray[np.bool_]: if ( self._last_mask is not None and self._last_used_pose is not None - and costmap.grid.shape == self._last_used_shape + and costmap.info == self._last_info + and costmap.header.frame_id == self._last_frame and self._pose_distance(self._last_used_pose, pose_index) < self._max_distance_cache ): return self._last_mask @@ -74,7 +77,8 @@ def mask(self) -> NDArray[np.bool_]: max_length=self._path_lookup_distance, ) - self._last_used_shape = costmap.grid.shape + self._last_info = deepcopy(costmap.info) + self._last_frame = costmap.header.frame_id self._last_used_pose = pose_index return self._last_mask @@ -86,9 +90,9 @@ def is_obstacle_ahead(self) -> bool: if costmap is None: return True - return bool(np.any(costmap.grid[self.mask] == CostValues.OCCUPIED)) + return bool(np.any(occupancy_view(costmap)[self.mask] == 100)) def _pose_distance(self, index1: int, index2: int) -> float: - p1 = self._path.poses[index1].position - p2 = self._path.poses[index2].position - return p1.distance(p2) + p1 = self._path.poses[index1].pose.position + p2 = self._path.poses[index2].pose.position + return point_distance(p1, p2) diff --git a/dimos/navigation/replanning_a_star/path_distancer.py b/dimos/navigation/replanning_a_star/path_distancer.py index 9a2c28dad78..f5a481e9b6d 100644 --- a/dimos/navigation/replanning_a_star/path_distancer.py +++ b/dimos/navigation/replanning_a_star/path_distancer.py @@ -14,11 +14,10 @@ from typing import cast +from dimos_generated.nav_msgs.msg import Path import numpy as np from numpy.typing import NDArray -from dimos.msgs.nav_msgs.Path import Path - class PathDistancer: _lookahead_dist: float @@ -27,7 +26,7 @@ class PathDistancer: def __init__(self, path: Path, lookahead_dist: float = 0.5) -> None: self._lookahead_dist = lookahead_dist - self._path = np.array([[p.position.x, p.position.y] for p in path.poses]) + self._path = np.array([[p.pose.position.x, p.pose.position.y] for p in path.poses]) self._cumulative_dists = _make_cumulative_distance_array(self._path) def find_lookahead_point(self, start_idx: int) -> NDArray[np.float64]: diff --git a/dimos/navigation/replanning_a_star/position_tracker.py b/dimos/navigation/replanning_a_star/position_tracker.py index 7d8249b562b..61a56d12039 100644 --- a/dimos/navigation/replanning_a_star/position_tracker.py +++ b/dimos/navigation/replanning_a_star/position_tracker.py @@ -16,11 +16,10 @@ import time from typing import cast +from dimos_generated.geometry_msgs.msg import PoseStamped import numpy as np from numpy.typing import NDArray -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped - _max_points_per_second = 1000 @@ -51,7 +50,7 @@ def reset_data(self) -> None: def add_position(self, pose: PoseStamped) -> None: with self._lock: self._timestamps[self._index] = time.time() - self._positions[self._index] = (pose.position.x, pose.position.y) + self._positions[self._index] = (pose.pose.position.x, pose.pose.position.y) self._index = (self._index + 1) % self._max_points self._size = min(self._size + 1, self._max_points) diff --git a/dimos/navigation/replanning_a_star/replan_limiter.py b/dimos/navigation/replanning_a_star/replan_limiter.py index 8cc630f3dff..d8849b71823 100644 --- a/dimos/navigation/replanning_a_star/replan_limiter.py +++ b/dimos/navigation/replanning_a_star/replan_limiter.py @@ -14,7 +14,9 @@ from threading import RLock -from dimos.msgs.geometry_msgs.Vector3 import Vector3 +from dimos_generated.geometry_msgs.msg import Point + +from dimos.msgs.geometry import point_distance from dimos.utils.logging_config import setup_logger logger = setup_logger() @@ -28,7 +30,7 @@ class ReplanLimiter: _max_attempts: int = 6 _reset_distance: float = 2.0 - _attempt_pos: Vector3 | None = None + _attempt_pos: Point | None = None _lock: RLock _attempt: int @@ -37,13 +39,13 @@ def __init__(self) -> None: self._lock = RLock() self._attempt = 0 - def can_retry(self, position: Vector3) -> bool: + def can_retry(self, position: Point) -> bool: with self._lock: if self._attempt == 0: self._attempt_pos = position if self._attempt >= 1 and self._attempt_pos: - distance = self._attempt_pos.distance(position) + distance = point_distance(self._attempt_pos, position) if distance >= self._reset_distance: logger.info( "Traveled enough to reset attempts", diff --git a/dimos/navigation/replanning_a_star/test_generated_navigation.py b/dimos/navigation/replanning_a_star/test_generated_navigation.py new file mode 100644 index 00000000000..64e9fa054fa --- /dev/null +++ b/dimos/navigation/replanning_a_star/test_generated_navigation.py @@ -0,0 +1,158 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import math +from threading import Event + +from dimos_generated.builtin_interfaces.msg import Time +from dimos_generated.geometry_msgs.msg import Point, Pose, PoseStamped, Quaternion, Twist +from dimos_generated.nav_msgs.msg import MapMetaData, OccupancyGrid, Path +from dimos_generated.std_msgs.msg import Bool, Header +import numpy as np + +from dimos.core.global_config import GlobalConfig +from dimos.navigation.replanning_a_star.global_planner import GlobalPlanner +from dimos.navigation.replanning_a_star.local_planner import LocalPlanner +from dimos.navigation.replanning_a_star.navigation_map import NavigationMap +from dimos.navigation.replanning_a_star.path_clearance import PathClearance + + +def test_global_planner_reaches_goal_with_generated_cdr_feedback(): + header = Header(stamp=Time(sec=1700000000, nanosec=123456789), frame_id="map") + grid = OccupancyGrid( + header=header, + info=MapMetaData( + width=24, height=24, resolution=0.25, origin=Pose(orientation=Quaternion(w=1)) + ), + data=np.zeros(24 * 24, dtype=np.int8), + ) + planner = GlobalPlanner(GlobalConfig()) + planner._local_planner._control_frequency = 100 + arrived = Event() + commands: list[Twist] = [] + paths: list[Path] = [] + position = [1.0, 1.0, 0.0] + + def update_odom() -> None: + message = PoseStamped( + header=header, + pose=Pose( + position=Point(x=position[0], y=position[1]), + orientation=Quaternion(z=math.sin(position[2] / 2), w=math.cos(position[2] / 2)), + ), + ) + planner.handle_odom(PoseStamped.decode(message.encode())) + + def command(message: Twist) -> None: + decoded = Twist.decode(message.encode()) + commands.append(decoded) + position[0] += decoded.linear.x * math.cos(position[2]) * 0.1 + position[1] += decoded.linear.x * math.sin(position[2]) * 0.1 + position[2] += decoded.angular.z * 0.1 + update_odom() + + def goal_reached(message: Bool) -> None: + if Bool.decode(message.encode()).data: + arrived.set() + + subscriptions = [ + planner.cmd_vel.subscribe(command), + planner.goal_reached.subscribe(goal_reached), + planner.path.subscribe(lambda value: paths.append(Path.decode(value.encode()))), + ] + try: + planner.handle_global_costmap(OccupancyGrid.decode(grid.encode())) + update_odom() + planner.start() + planner.handle_goal_request( + PoseStamped( + header=header, pose=Pose(position=Point(x=3, y=1), orientation=Quaternion(w=1)) + ) + ) + assert arrived.wait(5), "planner did not report arrival" + assert abs(position[0] - 3) < 0.25 + assert abs(position[1] - 1) < 0.1 + assert any(command.linear.x > 0 for command in commands) + published = next(path for path in paths if len(path.poses)) + assert published.header == header + assert all(pose.header == header for pose in published.poses) + finally: + planner.stop() + for subscription in subscriptions: + subscription.dispose() + assert commands[-1] == Twist() + + +def test_clearance_mask_tracks_generated_map_geometry(): + path = Path(poses=[PoseStamped(pose=Pose(position=Point(x=x, y=2))) for x in [2, 3, 4]]) + clearance = PathClearance(GlobalConfig(robot_width=0.1), path) + grid = OccupancyGrid( + info=MapMetaData( + width=10, height=10, resolution=1, origin=Pose(orientation=Quaternion(w=1)) + ), + data=[0] * 100, + ) + clearance.update_costmap(grid) + original = clearance.mask.copy() + assert original[2, 2] + grid.info.origin.position.x = 1 + shifted = clearance.mask.copy() + assert shifted[2, 1] and not original[2, 1] + grid.info.resolution = 0.5 + scaled = clearance.mask + assert scaled[4, 2] and not shifted[4, 2] + + +def test_cancel_joins_control_loop_and_ends_with_generated_stop(): + config = GlobalConfig() + maps = NavigationMap(config, "gradient") + maps.update( + OccupancyGrid( + info=MapMetaData( + width=10, height=10, resolution=1, origin=Pose(orientation=Quaternion(w=1)) + ), + data=[0] * 100, + ) + ) + planner = LocalPlanner(config, maps, 0.2) + planner.handle_odom( + PoseStamped(pose=Pose(position=Point(x=1, y=1), orientation=Quaternion(w=1))) + ) + moving = Event() + commands = [] + + def command(message: Twist) -> None: + commands.append(Twist.decode(message.encode())) + if message.linear.x > 0: + moving.set() + + subscription = planner.cmd_vel.subscribe(command) + try: + planner.start_planning( + Path( + poses=[ + PoseStamped(pose=Pose(position=Point(x=x, y=1), orientation=Quaternion(w=1))) + for x in [1, 2, 3] + ] + ) + ) + thread = planner._thread + assert thread is not None + assert moving.wait(2) + planner.stop() + assert not thread.is_alive() + assert commands[-1] == Twist() + finally: + planner.stop() + subscription.dispose() diff --git a/dimos/navigation/replanning_a_star/test_generated_pipeline.py b/dimos/navigation/replanning_a_star/test_generated_pipeline.py new file mode 100644 index 00000000000..e2a5d179f78 --- /dev/null +++ b/dimos/navigation/replanning_a_star/test_generated_pipeline.py @@ -0,0 +1,109 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +# Copyright 2026 Dimensional Inc. +# SPDX-License-Identifier: Apache-2.0 + +import math + +from dimos_generated.builtin_interfaces.msg import Time +from dimos_generated.geometry_msgs.msg import Point, Pose, PoseStamped +from dimos_generated.nav_msgs.msg import MapMetaData, OccupancyGrid, Path +from dimos_generated.std_msgs.msg import Header +import numpy as np +import pytest + +from dimos.mapping.occupancy.path_mask import make_path_mask +from dimos.mapping.occupancy.path_resampling import simple_resample_path, smooth_resample_path +from dimos.msgs.geometry import quaternion_euler, quaternion_from_euler +from dimos.msgs.occupancy import grid_to_world, world_to_grid +from dimos.navigation.replanning_a_star.min_cost_astar import _USE_CPP, min_cost_astar + + +@pytest.mark.parametrize("use_cpp", [False, True]) +@pytest.mark.parametrize("yaw", [0.0, math.pi / 2]) +def test_generated_grid_to_path_and_mask(use_cpp, yaw): + if use_cpp: + assert _USE_CPP, "native A* extension must be built for this integration test" + cells = np.zeros((30, 30), dtype=np.int8) + cells[15, 0:20] = 100 + header = Header(stamp=Time(sec=1700000000, nanosec=123456789), frame_id="map") + grid = OccupancyGrid( + header=header, + info=MapMetaData( + width=30, + height=30, + resolution=0.25, + origin=Pose(position=Point(x=2, y=3), orientation=quaternion_from_euler(0, 0, yaw)), + ), + data=cells.ravel(), + ) + received = OccupancyGrid.decode(grid.encode()) + start = grid_to_world(received, (5, 5)) + goal = grid_to_world(received, (25, 25)) + path = min_cost_astar(received, goal, start, use_cpp=use_cpp) + assert path is not None + decoded = Path.decode(path.encode()) + assert decoded.header == header + assert world_to_grid(received, decoded.poses[0].pose.position) == pytest.approx((5, 5)) + assert world_to_grid(received, decoded.poses[-1].pose.position) == pytest.approx((25, 25)) + for pose in decoded.poses: + assert pose.header == header + x, y = world_to_grid(received, pose.pose.position) + assert cells[round(y), round(x)] == 0 + mask = make_path_mask(received, decoded, 0.1) + assert mask.any() + assert not np.any(mask & (cells == 100)) + assert world_to_grid(grid, start) == pytest.approx((5, 5)) + assert world_to_grid(grid, goal) == pytest.approx((25, 25)) + + +@pytest.mark.parametrize("resample", [simple_resample_path, smooth_resample_path]) +def test_resampling_preserves_headers_orientations_and_input(resample): + header = Header(stamp=Time(sec=123, nanosec=456), frame_id="map") + path = Path( + header=header, + poses=[ + PoseStamped( + header=header, + pose=Pose(position=Point(x=1, y=y), orientation=quaternion_from_euler(0, 0, 0)), + ) + for y in [1, 2, 3] + ], + ) + before = path.encode() + goal = Pose(orientation=quaternion_from_euler(0, 0, -math.pi / 2)) + actual = Path.decode(resample(path, goal, 0.1).encode()) + assert actual.header == header + assert len(actual.poses) > 3 + assert actual.poses[0].pose.position == path.poses[0].pose.position + assert actual.poses[-1].pose.position == path.poses[-1].pose.position + assert actual.poses[-1].pose.orientation == goal.orientation + for pose in list(actual.poses)[:-1]: + assert pose.header == header + assert quaternion_euler(pose.pose.orientation)[2] == pytest.approx(math.pi / 2) + assert path.encode() == before + + +@pytest.mark.parametrize("use_cpp", [False, True]) +def test_outside_start_and_goal_are_rejected(use_cpp): + grid = OccupancyGrid( + info=MapMetaData( + width=3, height=3, resolution=1, origin=Pose(orientation=quaternion_from_euler(0, 0, 0)) + ), + data=[0] * 9, + ) + assert min_cost_astar(grid, Point(x=1, y=1), Point(x=-0.1), use_cpp=use_cpp) is None + assert min_cost_astar(grid, Point(x=-0.1), Point(x=1, y=1), use_cpp=use_cpp) is None + assert min_cost_astar(grid, Point(x=3), use_cpp=use_cpp) is None diff --git a/dimos/navigation/replanning_a_star/test_global_planner.py b/dimos/navigation/replanning_a_star/test_global_planner.py index 9916874a7b0..74d9579730c 100644 --- a/dimos/navigation/replanning_a_star/test_global_planner.py +++ b/dimos/navigation/replanning_a_star/test_global_planner.py @@ -12,12 +12,12 @@ # See the License for the specific language governing permissions and # limitations under the License. +from dimos_generated.geometry_msgs.msg import Point, Pose, Quaternion +from dimos_generated.nav_msgs.msg import MapMetaData, OccupancyGrid +from dimos_generated.std_msgs.msg import Header import numpy as np from dimos.core.global_config import GlobalConfig -from dimos.msgs.geometry_msgs.Pose import Pose -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.msgs.nav_msgs.OccupancyGrid import OccupancyGrid from dimos.navigation.replanning_a_star.global_planner import GlobalPlanner @@ -29,16 +29,22 @@ def test_find_wide_path_with_start_inside_inflation() -> None: resolution = 0.05 grid = np.zeros((60, 60), dtype=np.int8) grid[20:40, 30] = 100 # wall at x=1.5m spanning y=1.0..2.0m - costmap = OccupancyGrid(grid=grid, resolution=resolution, origin=Pose(), frame_id="world") + costmap = OccupancyGrid( + header=Header(frame_id="world"), + info=MapMetaData( + width=60, height=60, resolution=resolution, origin=Pose(orientation=Quaternion(w=1)) + ), + data=grid.ravel(), + ) planner = GlobalPlanner(GlobalConfig()) planner.handle_global_costmap(costmap) # 7 cm in front of the wall: within the inflation radius # (robot_width * 1.1 / 2 = 0.165m), so the start cell is engulfed. - robot = Vector3(1.43, 1.5, 0) + robot = Point(x=1.43, y=1.5) # On the other side of the wall; the path must round a wall end. - goal = Vector3(2.75, 1.5, 0) + goal = Point(x=2.75, y=1.5) path = planner._find_wide_path(goal, robot) diff --git a/dimos/navigation/replanning_a_star/test_goal_validator.py b/dimos/navigation/replanning_a_star/test_goal_validator.py index 69c71476965..c135ac21643 100644 --- a/dimos/navigation/replanning_a_star/test_goal_validator.py +++ b/dimos/navigation/replanning_a_star/test_goal_validator.py @@ -12,42 +12,52 @@ # See the License for the specific language governing permissions and # limitations under the License. +from dimos_generated.geometry_msgs.msg import Point, Pose, Quaternion +from dimos_generated.nav_msgs.msg import MapMetaData, OccupancyGrid import numpy as np import pytest -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.msgs.nav_msgs.OccupancyGrid import CostValues, OccupancyGrid from dimos.navigation.replanning_a_star.goal_validator import find_safe_goal from dimos.utils.data import get_data @pytest.fixture def costmap() -> OccupancyGrid: - return OccupancyGrid(np.load(get_data("occupancy_simple.npy"))) + cells = np.load(get_data("occupancy_simple.npy")) + return OccupancyGrid( + info=MapMetaData( + width=cells.shape[1], + height=cells.shape[0], + resolution=0.05, + origin=Pose(orientation=Quaternion(w=1)), + ), + data=cells.astype(np.int8).ravel(), + ) @pytest.mark.parametrize( "input_pos,expected_pos", [ - # Identical. - ((6.15, 10.0), (6.15, 10.0)), + # The requested point falls in the preceding cell at ROS float32 resolution. + ((6.15, 10.0), (6.10, 9.95)), # Very slightly off. - ((6.0, 10.0), (6.05, 10.0)), + ((6.0, 10.0), (6.05, 9.95)), # Don't pick a spot that's the closest, but is actually on the other side of the wall. ((5.0, 9.0), (5.85, 9.6)), ], ) def test_find_safe_goal(costmap, input_pos, expected_pos) -> None: - goal = Vector3(input_pos[0], input_pos[1], 0.0) + goal = Point(x=input_pos[0], y=input_pos[1]) safe_goal = find_safe_goal( costmap, goal, algorithm="bfs_contiguous", - cost_threshold=CostValues.OCCUPIED, + cost_threshold=100, min_clearance=0.3, max_search_distance=5.0, connectivity_check_radius=0, ) - assert safe_goal == Vector3(expected_pos[0], expected_pos[1], 0.0) + assert safe_goal is not None + assert (safe_goal.x, safe_goal.y, safe_goal.z) == pytest.approx((*expected_pos, 0), abs=1e-6) diff --git a/dimos/navigation/replanning_a_star/test_min_cost_astar.py b/dimos/navigation/replanning_a_star/test_min_cost_astar.py index 1ea28d1cae3..e031fa6afb8 100644 --- a/dimos/navigation/replanning_a_star/test_min_cost_astar.py +++ b/dimos/navigation/replanning_a_star/test_min_cost_astar.py @@ -14,49 +14,62 @@ import time +import cv2 +from dimos_generated.geometry_msgs.msg import Point, Pose, Quaternion +from dimos_generated.nav_msgs.msg import MapMetaData, OccupancyGrid import numpy as np -from open3d.geometry import PointCloud import pytest from dimos.mapping.occupancy.gradient import gradient, voronoi_gradient from dimos.mapping.occupancy.visualizations import visualize_occupancy_grid -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.msgs.nav_msgs.OccupancyGrid import OccupancyGrid -from dimos.msgs.sensor_msgs.Image import Image +from dimos.msgs.image import image_view +from dimos.msgs.occupancy import grid_to_world, world_to_grid from dimos.navigation.replanning_a_star.min_cost_astar import min_cost_astar from dimos.utils.data import get_data +def _grid(cells, resolution=0.05): + return OccupancyGrid( + info=MapMetaData( + width=cells.shape[1], + height=cells.shape[0], + resolution=resolution, + origin=Pose(orientation=Quaternion(w=1)), + ), + data=cells.astype(np.int8).ravel(), + ) + + @pytest.fixture -def costmap() -> PointCloud: - return gradient(OccupancyGrid(np.load(get_data("occupancy_simple.npy"))), max_distance=1.5) +def costmap() -> OccupancyGrid: + return gradient(_grid(np.load(get_data("occupancy_simple.npy"))), max_distance=1.5) @pytest.fixture -def costmap_three_paths() -> PointCloud: - return voronoi_gradient(OccupancyGrid(np.load(get_data("three_paths.npy"))), max_distance=1.5) +def costmap_three_paths() -> OccupancyGrid: + return voronoi_gradient(_grid(np.load(get_data("three_paths.npy"))), max_distance=1.5) def test_astar(costmap) -> None: - start = Vector3(4.0, 2.0) - goal = Vector3(6.15, 10.0) - expected = Image.from_file(get_data("astar_min_cost.png")) + start = Point(x=4, y=2) + goal = Point(x=6.15, y=10) + expected = cv2.imread(str(get_data("astar_min_cost.png")), cv2.IMREAD_COLOR) path = min_cost_astar(costmap, goal, start, use_cpp=False) actual = visualize_occupancy_grid(costmap, "rainbow", path) - np.testing.assert_array_equal(actual.data, expected.data) + np.testing.assert_array_equal(image_view(actual), expected) def test_astar_corner(costmap_three_paths) -> None: - start = Vector3(2.8, 3.35) - goal = Vector3(6.35, 4.25) - expected = Image.from_file(get_data("astar_corner_min_cost.png")) + start = Point(x=2.8, y=3.35) + goal = Point(x=6.35, y=4.25) + expected = cv2.imread(str(get_data("astar_corner_min_cost.png")), cv2.IMREAD_COLOR) path = min_cost_astar(costmap_three_paths, goal, start, use_cpp=False) actual = visualize_occupancy_grid(costmap_three_paths, "rainbow", path) - np.testing.assert_array_equal(actual.data, expected.data) + np.testing.assert_array_equal(image_view(actual), expected) def test_astar_unknown_penalty_blocks_unknown_cells(costmap) -> None: @@ -69,10 +82,10 @@ def test_astar_unknown_penalty_blocks_unknown_cells(costmap) -> None: grid[10:90, 10] = 0 # left column grid[89, 10:90] = 0 # bottom row grid[10:90, 89] = 0 # right column - og = OccupancyGrid(grid, resolution=0.1) + og = _grid(grid, resolution=0.1) - start = og.grid_to_world((10, 10)) - goal = og.grid_to_world((89, 10)) + start = grid_to_world(og, (10, 10)) + goal = grid_to_world(og, (89, 10)) for use_cpp in [False, True]: path = min_cost_astar(og, goal, start, unknown_penalty=1.0, use_cpp=use_cpp) @@ -81,8 +94,8 @@ def test_astar_unknown_penalty_blocks_unknown_cells(costmap) -> None: continue # Verify no path cell lands on an unknown cell for pose in path.poses: - gp = og.world_to_grid((pose.position.x, pose.position.y)) - gx, gy = round(gp.x), round(gp.y) + gp = world_to_grid(og, pose.pose.position) + gx, gy = round(gp[0]), round(gp[1]) if 0 <= gx < 100 and 0 <= gy < 100: assert grid[gy, gx] != -1, ( f"Path traverses unknown cell at grid ({gx}, {gy}), use_cpp={use_cpp}" @@ -94,10 +107,10 @@ def test_astar_unknown_penalty_allows_with_low_penalty(costmap) -> None: grid = np.full((50, 50), -1, dtype=np.int8) # All unknown grid[5, 5] = 0 # start cell free grid[45, 45] = 0 # goal cell free - og = OccupancyGrid(grid, resolution=0.1) + og = _grid(grid, resolution=0.1) - start = og.grid_to_world((5, 5)) - goal = og.grid_to_world((45, 45)) + start = grid_to_world(og, (5, 5)) + goal = grid_to_world(og, (45, 45)) for use_cpp in [False, True]: path = min_cost_astar(og, goal, start, unknown_penalty=0.5, use_cpp=use_cpp) @@ -108,8 +121,8 @@ def test_astar_unknown_penalty_allows_with_low_penalty(costmap) -> None: def test_astar_python_and_cpp(costmap) -> None: - start = Vector3(4.0, 2.0, 0) - goal = Vector3(6.15, 10.0) + start = Point(x=4, y=2) + goal = Point(x=6.15, y=10) start_time = time.perf_counter() path_python = min_cost_astar(costmap, goal, start, use_cpp=False) @@ -130,7 +143,7 @@ def test_astar_python_and_cpp(costmap) -> None: # Assert that both implementations return almost identical points. np.testing.assert_allclose( - [(p.position.x, p.position.y) for p in path_python.poses], - [(p.position.x, p.position.y) for p in path_cpp.poses], + [(p.pose.position.x, p.pose.position.y) for p in path_python.poses], + [(p.pose.position.x, p.pose.position.y) for p in path_cpp.poses], atol=0.05001, ) diff --git a/dimos/navigation/test_generated_bbox_navigation.py b/dimos/navigation/test_generated_bbox_navigation.py new file mode 100644 index 00000000000..43ab745a57d --- /dev/null +++ b/dimos/navigation/test_generated_bbox_navigation.py @@ -0,0 +1,73 @@ +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from threading import Event +from uuid import uuid4 + +from dimos_generated.builtin_interfaces.msg import Time +from dimos_generated.geometry_msgs.msg import PoseStamped +from dimos_generated.sensor_msgs.msg import CameraInfo +from dimos_generated.std_msgs.msg import Header +from dimos_generated.vision_msgs.msg import ( + BoundingBox2D, + Detection2D, + Detection2DArray, + Point2D, + Pose2D, +) +import pytest + +from dimos.core.transport import LCMTransport +from dimos.navigation.bbox_navigation import BBoxNavigationModule + + +def test_generated_bbox_goal_preserves_header_and_projection(): + module = BBoxNavigationModule(goal_distance=2) + prefix = uuid4().hex[:8] + module.camera_info.transport = LCMTransport(f"/bc/{prefix}", CameraInfo) + module.detection2d.transport = LCMTransport(f"/bd/{prefix}", Detection2DArray) + goals = [] + ready = Event() + received = Event() + unsubscribe = module.goal_request.subscribe( + lambda value: (goals.append(PoseStamped.decode(value.encode())), received.set()) + ) + message = Detection2DArray( + header=Header(frame_id="base_link", stamp=Time(sec=1700000000, nanosec=123456789)), + detections=[Detection2D(bbox=BoundingBox2D(center=Pose2D(position=Point2D(x=420, y=290))))], + ) + try: + module.start() + module._on_detection(message) + assert not goals + camera = CameraInfo(k=[500, 0, 320, 0, 250, 240, 0, 0, 1]) + camera_unsubscribe = module.camera_info.subscribe(lambda _: ready.set()) + module.camera_info.transport.publish(CameraInfo.decode(camera.encode())) + assert ready.wait(2) + camera_unsubscribe() + module.detection2d.transport.publish(Detection2DArray.decode(message.encode())) + assert received.wait(2) + assert len(goals) == 1 + goal = goals[0] + assert goal.header == message.header + assert (goal.pose.position.x, goal.pose.position.y, goal.pose.position.z) == pytest.approx( + (2, -0.4, -0.4) + ) + module._on_detection(Detection2DArray()) + assert len(goals) == 1 + finally: + unsubscribe() + module.stop() + module.camera_info.transport.stop() + module.detection2d.transport.stop() diff --git a/dimos/navigation/visual/query.py b/dimos/navigation/visual/query.py index 16a6ca60ce8..5be1b953c55 100644 --- a/dimos/navigation/visual/query.py +++ b/dimos/navigation/visual/query.py @@ -12,9 +12,10 @@ # See the License for the specific language governing permissions and # limitations under the License. +from dimos_generated.sensor_msgs.msg import Image + from dimos.models.qwen.bbox import BBox from dimos.models.vl.base import VlModel -from dimos.msgs.sensor_msgs.Image import Image from dimos.utils.generic import extract_json_from_llm_response diff --git a/dimos/navigation/visual_servoing/detection_navigation.py b/dimos/navigation/visual_servoing/detection_navigation.py index 7430a23a883..16b66c692c8 100644 --- a/dimos/navigation/visual_servoing/detection_navigation.py +++ b/dimos/navigation/visual_servoing/detection_navigation.py @@ -12,15 +12,13 @@ # See the License for the specific language governing permissions and # limitations under the License. -from dimos_lcm.sensor_msgs import CameraInfo as DimosLcmCameraInfo +from dimos_generated.geometry_msgs.msg import TransformStamped, Twist, Vector3 +from dimos_generated.sensor_msgs.msg import CameraInfo, Image, PointCloud2 import numpy as np -from dimos.msgs.geometry_msgs.Transform import Transform -from dimos.msgs.geometry_msgs.Twist import Twist -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo -from dimos.msgs.sensor_msgs.Image import Image -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 +from dimos.msgs.geometry import quaternion_euler +from dimos.msgs.pointcloud import pointcloud_xyz +from dimos.msgs.time import to_seconds from dimos.perception.detection.type.detection2d.bbox import Detection2DBBox from dimos.perception.detection.type.detection3d.pointcloud import Detection3DPC from dimos.protocol.tf.tf import TFLookup @@ -59,22 +57,20 @@ def compute_twist_for_detection_3d( # Get transform from world frame to camera optical frame world_to_optical = self._tf.get( - "camera_optical", pointcloud.frame_id, image.ts, time_tolerance=1.0 + "camera_optical", + pointcloud.header.frame_id, + to_seconds(image.header.stamp), + time_tolerance=1.0, ) if world_to_optical is None: logger.warning("Could not get camera transform") return None - lcm_camera_info = DimosLcmCameraInfo() - lcm_camera_info.K = self._camera_info.K - lcm_camera_info.width = self._camera_info.width - lcm_camera_info.height = self._camera_info.height - # Project to 3D using the pointcloud detection_3d = Detection3DPC.from_2d( det=detection, world_pointcloud=pointcloud, - camera_info=lcm_camera_info, + camera_info=self._camera_info, world_to_optical_transform=world_to_optical, filters=[], # Skip filtering for faster processing in follow loop ) @@ -89,7 +85,7 @@ def compute_twist_for_detection_3d( logger.warning("Could not get robot transform") return None - robot_pos = robot_transform.translation + robot_pos = robot_transform.transform.translation # Compute robust target position using front-most points target_position = self._compute_robust_target_position(detection_3d.pointcloud, robot_pos) @@ -119,7 +115,7 @@ def _compute_robust_target_position( Vector3 position representing the front of the detected object, or None if not enough valid points. """ - points, _ = pointcloud.as_numpy() + points = pointcloud_xyz(pointcloud) if len(points) < 10: return None @@ -128,7 +124,7 @@ def _compute_robust_target_position( points = points[height_mask] if len(points) < 10: # Fall back to all points if height filtering removes too many - points, _ = pointcloud.as_numpy() + points = pointcloud_xyz(pointcloud) # Compute 2D distance (XY plane) from robot to each point dx = points[:, 0] - robot_pos.x @@ -152,9 +148,11 @@ def _compute_robust_target_position( # Compute centroid of front-most points centroid = front_points.mean(axis=0) - return Vector3(centroid[0], centroid[1], centroid[2]) + return Vector3(x=float(centroid[0]), y=float(centroid[1]), z=float(centroid[2])) - def _compute_twist_from_3d(self, target_position: Vector3, robot_transform: Transform) -> Twist: + def _compute_twist_from_3d( + self, target_position: Vector3, robot_transform: TransformStamped + ) -> Twist: """Compute twist command to navigate towards a 3D target position. Args: @@ -164,7 +162,7 @@ def _compute_twist_from_3d(self, target_position: Vector3, robot_transform: Tran Returns: Twist command for the robot. """ - robot_pos = robot_transform.translation + robot_pos = robot_transform.transform.translation # Compute vector from robot to target in world frame dx = target_position.x - robot_pos.x @@ -176,7 +174,7 @@ def _compute_twist_from_3d(self, target_position: Vector3, robot_transform: Tran angle_to_target = np.arctan2(dy, dx) # Get robot's current heading from transform - robot_yaw = robot_transform.rotation.to_euler().z + robot_yaw = quaternion_euler(robot_transform.transform.rotation)[2] # Angle error (how much to turn) angle_error = angle_to_target - robot_yaw @@ -207,6 +205,6 @@ def _compute_twist_from_3d(self, target_position: Vector3, robot_transform: Tran ) return Twist( - linear=Vector3(linear_x, 0.0, 0.0), - angular=Vector3(0.0, 0.0, angular_z), + linear=Vector3(x=linear_x), + angular=Vector3(z=angular_z), ) diff --git a/dimos/navigation/visual_servoing/test_visual_servoing_2d.py b/dimos/navigation/visual_servoing/test_visual_servoing_2d.py new file mode 100644 index 00000000000..d8b21b27e68 --- /dev/null +++ b/dimos/navigation/visual_servoing/test_visual_servoing_2d.py @@ -0,0 +1,40 @@ +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from dimos_generated.geometry_msgs.msg import Twist +from dimos_generated.sensor_msgs.msg import CameraInfo +import pytest + +from dimos.navigation.visual_servoing.visual_servoing_2d import VisualServoing2D + + +@pytest.mark.parametrize( + "bbox, linear, angular", + [ + ((270, 0, 370, 100), 0.5, 0), + ((245, 0, 395, 100), 0, 0), + ((120, 0, 520, 100), -0.3, 0), + ((370, 0, 470, 100), 0.36, -0.2), + ((320, 0, 320, 100), 0, 0), + ], +) +def test_generated_camera_to_twist(bbox, linear, angular): + camera = CameraInfo(width=640, height=480, k=[500, 0, 320, 0, 500, 240, 0, 0, 1]) + controller = VisualServoing2D(CameraInfo.decode(camera.encode())) + output = controller.compute_twist(bbox, 640) + decoded = Twist.decode(output.encode()) + assert decoded.linear.x == pytest.approx(linear) + assert decoded.angular.z == pytest.approx(angular) + assert decoded.linear.y == decoded.linear.z == 0 + assert decoded.angular.x == decoded.angular.y == 0 diff --git a/dimos/navigation/visual_servoing/visual_servoing_2d.py b/dimos/navigation/visual_servoing/visual_servoing_2d.py index f424b214661..fe209e7f95b 100644 --- a/dimos/navigation/visual_servoing/visual_servoing_2d.py +++ b/dimos/navigation/visual_servoing/visual_servoing_2d.py @@ -12,12 +12,10 @@ # See the License for the specific language governing permissions and # limitations under the License. +from dimos_generated.geometry_msgs.msg import Twist, Vector3 +from dimos_generated.sensor_msgs.msg import CameraInfo import numpy as np -from dimos.msgs.geometry_msgs.Twist import Twist -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo - class VisualServoing2D: """2D visual servoing controller for tracking objects using bounding boxes. @@ -91,7 +89,7 @@ def compute_twist( estimated_distance = self._estimate_distance(bbox) if estimated_distance is None: - return Twist.zero() + return Twist() # Calculate distance error (positive = too far, need to move forward) distance_error = estimated_distance - self._target_distance @@ -120,8 +118,8 @@ def compute_twist( linear_x = max(linear_x, 0.1) return Twist( - linear=Vector3(linear_x, 0.0, 0.0), - angular=Vector3(0.0, 0.0, angular_z), + linear=Vector3(x=linear_x), + angular=Vector3(z=angular_z), ) def _get_normalized_x(self, pixel_x: float) -> float: @@ -135,8 +133,8 @@ def _get_normalized_x(self, pixel_x: float) -> float: Returns: Normalized x coordinate (tan of angle from optical center) """ - fx = self._camera_info.K[0] # focal length x - cx = self._camera_info.K[2] # optical center x + fx = self._camera_info.k[0] # focal length x + cx = self._camera_info.k[2] # optical center x return (pixel_x - cx) / fx def _estimate_distance(self, bbox: tuple[float, float, float, float]) -> float | None: @@ -161,7 +159,7 @@ def _estimate_distance(self, bbox: tuple[float, float, float, float]) -> float | return None # Pinhole camera model: distance = (real_width * fx) / pixel_width - fx = self._camera_info.K[0] # focal length x in pixels + fx = self._camera_info.k[0] # focal length x in pixels estimated_distance = (self._assumed_object_width * fx) / bbox_width return estimated_distance diff --git a/dimos/perception/detection/conftest.py b/dimos/perception/detection/conftest.py index a34bcf914f8..ff065a0e1ea 100644 --- a/dimos/perception/detection/conftest.py +++ b/dimos/perception/detection/conftest.py @@ -14,18 +14,17 @@ from collections.abc import Callable, Generator import functools +import hashlib +import json from typing import TypedDict -from unittest import mock -from dimos_lcm.visualization_msgs.MarkerArray import MarkerArray +from dimos_generated.geometry_msgs.msg import PoseStamped, TransformStamped +from dimos_generated.sensor_msgs.msg import CameraInfo, Image, PointCloud2 +from dimos_generated.vision_msgs.msg import Detection2DArray +from dimos_generated.visualization_msgs.msg import MarkerArray import pytest from dimos.core.transport import LCMTransport -from dimos.msgs.geometry_msgs.Transform import Transform -from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo -from dimos.msgs.sensor_msgs.Image import Image -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 -from dimos.msgs.vision_msgs.Detection2DArray import Detection2DArray from dimos.perception.detection.module2D import Detection2DModule from dimos.perception.detection.module3D import Detection3DModule from dimos.perception.detection.type.detection2d.base import Detection2D @@ -35,17 +34,15 @@ from dimos.perception.experimental.moduleDB import ObjectDBModule from dimos.protocol.tf.tf import TF from dimos.robot.unitree.go2 import connection -from dimos.robot.unitree.type.odometry import Odometry from dimos.utils.data import get_data -from dimos.utils.testing.legacy_pickle import LegacyPickleStore class Moment(TypedDict, total=False): - odom_frame: Odometry + odom_frame: PoseStamped lidar_frame: PointCloud2 image_frame: Image camera_info: CameraInfo - transforms: list[Transform] + transforms: list[TransformStamped] tf: TF detections: ImageDetections3DPC | None markers: MarkerArray | None @@ -70,32 +67,23 @@ def tf(): def get_moment(tf): @functools.lru_cache(maxsize=1) def moment_provider(**kwargs) -> Moment: - print("MOMENT PROVIDER ARGS:", kwargs) seek = kwargs.get("seek", 10.0) - - data_dir = "unitree_go2_lidar_corrected" - get_data(data_dir) - - lidar_frame_result = LegacyPickleStore(f"{data_dir}/lidar").find_closest_seek(seek) - if lidar_frame_result is None: - raise ValueError("No lidar frame found") - lidar_frame: PointCloud2 = lidar_frame_result - - image_frame = LegacyPickleStore( - f"{data_dir}/video", - ).find_closest(lidar_frame.ts) - - if image_frame is None: - raise ValueError("No image frame found") - - image_frame.frame_id = "camera_optical" - - odom_frame = LegacyPickleStore(f"{data_dir}/odom", autocast=Odometry.from_msg).find_closest( - lidar_frame.ts - ) - - if odom_frame is None: - raise ValueError("No odom frame found") + data_dir = get_data("unitree_go2_detection_cdr") + manifest = json.loads((data_dir / "manifest.json").read_text()) + entry = next(item for item in manifest["moments"] if item["seek"] == seek) + + def payload(name): + stream = entry["streams"][name] + data = (data_dir / stream["file"]).read_bytes() + assert hashlib.sha256(data).hexdigest() == stream["cdr_sha256"] + return data + + lidar_frame = PointCloud2.decode(payload("lidar")) + image_frame = Image.decode(payload("video")) + image_frame.header.frame_id = "camera_optical" + odom_frame = PoseStamped.decode(payload("odom")) + # Match the Go2 connection's configured global parent for this recording. + odom_frame.header.frame_id = "world" transforms = connection.GO2Connection._odom_to_tf(odom_frame) @@ -105,7 +93,7 @@ def moment_provider(**kwargs) -> Moment: "odom_frame": odom_frame, "lidar_frame": lidar_frame, "image_frame": image_frame, - "camera_info": connection._camera_info_static(), + "camera_info": connection.front_camera_calibration(), "transforms": transforms, "tf": tf, } @@ -185,7 +173,7 @@ def detection3dpc(detections3dpc) -> Detection3DPC: def get_moment_2d(get_moment) -> Generator[Callable[[], Moment2D], None, None]: from dimos.perception.detection.detectors.yolo import Yolo2DDetector - c = mock.create_autospec(CameraInfo, spec_set=True, instance=True) + c = connection.front_camera_calibration() module = Detection2DModule(detector=lambda: Yolo2DDetector(device="cpu"), camera_info=c) @functools.lru_cache(maxsize=1) @@ -220,7 +208,7 @@ def moment_provider(**kwargs) -> Moment3D: if lidar_frame is None: raise ValueError("No lidar frame found") - camera_transform = moment["tf"].get("camera_optical", lidar_frame.frame_id) + camera_transform = moment["tf"].get("camera_optical", lidar_frame.header.frame_id) if camera_transform is None: raise ValueError("No camera_optical transform in tf") @@ -244,10 +232,10 @@ def object_db_module(get_moment): """Create and populate an ObjectDBModule with detections from multiple frames.""" from dimos.perception.detection.detectors.yolo import Yolo2DDetector - c = mock.create_autospec(CameraInfo, spec_set=True, instance=True) + c = connection.front_camera_calibration() module2d = Detection2DModule(detector=lambda: Yolo2DDetector(device="cpu"), camera_info=c) - module3d = Detection3DModule(camera_info=connection._camera_info_static()) - moduleDB = ObjectDBModule(camera_info=connection._camera_info_static()) + module3d = Detection3DModule(camera_info=connection.front_camera_calibration()) + moduleDB = ObjectDBModule(camera_info=connection.front_camera_calibration()) # Process 5 frames to build up object history for i in range(5): @@ -258,7 +246,7 @@ def object_db_module(get_moment): imageDetections2d = module2d.process_image_frame(moment["image_frame"]) # Get camera transform - camera_transform = moment["tf"].get("camera_optical", moment.get("lidar_frame").frame_id) + camera_transform = moment["tf"].get("camera_optical", moment["lidar_frame"].header.frame_id) # Process 3D detections imageDetections3d = module3d.process_frame( diff --git a/dimos/perception/detection/detectors/base.py b/dimos/perception/detection/detectors/base.py index 40aa82e5bdd..2de6d864b63 100644 --- a/dimos/perception/detection/detectors/base.py +++ b/dimos/perception/detection/detectors/base.py @@ -14,7 +14,8 @@ from abc import ABC, abstractmethod -from dimos.msgs.sensor_msgs.Image import Image +from dimos_generated.sensor_msgs.msg import Image + from dimos.perception.detection.type.detection2d.imageDetections2D import ImageDetections2D diff --git a/dimos/perception/detection/detectors/conftest.py b/dimos/perception/detection/detectors/conftest.py index bb9a47e0eb9..d7e42317792 100644 --- a/dimos/perception/detection/detectors/conftest.py +++ b/dimos/perception/detection/detectors/conftest.py @@ -14,7 +14,7 @@ import pytest -from dimos.msgs.sensor_msgs.Image import Image +from dimos.msgs.image import image_from_file from dimos.perception.detection.detectors.person.yolo import YoloPersonDetector from dimos.perception.detection.detectors.yolo import Yolo2DDetector from dimos.perception.detection.detectors.yoloe import Yoloe2DDetector, YoloePromptMode @@ -24,7 +24,7 @@ @pytest.fixture(scope="session") def test_image(): """Load the test image used for detector tests.""" - return Image.from_file(get_data("cafe.jpg")) + return image_from_file(get_data("cafe.jpg")) @pytest.fixture(scope="session") diff --git a/dimos/perception/detection/detectors/omdet.py b/dimos/perception/detection/detectors/omdet.py index aeee0f7433a..6ea3d71e42c 100644 --- a/dimos/perception/detection/detectors/omdet.py +++ b/dimos/perception/detection/detectors/omdet.py @@ -18,12 +18,14 @@ from functools import cached_property +from dimos_generated.sensor_msgs.msg import Image import numpy as np from PIL import Image as PILImage import torch from dimos.models.base import HuggingFaceModel, HuggingFaceModelConfig -from dimos.msgs.sensor_msgs.Image import Image +from dimos.msgs.image import image_to_rgb +from dimos.msgs.time import to_seconds from dimos.perception.detection.type.detection2d.bbox import Detection2DBBox from dimos.perception.detection.type.detection2d.imageDetections2D import ImageDetections2D @@ -79,7 +81,7 @@ def query_detections( ``confidence`` is the per-box score. ``class_id`` indexes into ``queries``. """ - pil = PILImage.fromarray(image.to_rgb().data) + pil = PILImage.fromarray(image_to_rgb(image)) with torch.inference_mode(): inputs = self._processor(images=pil, text=queries, return_tensors="pt").to( self.config.device @@ -105,7 +107,7 @@ def query_detections( class_id=int(label), confidence=float(score), name=queries[int(label)], - ts=image.ts, + ts=to_seconds(image.header.stamp), image=image, ) if det.is_valid(): @@ -135,7 +137,7 @@ def query_score_rows( from torchvision.ops.boxes import batched_nms # type: ignore[import-untyped] from transformers.image_transforms import center_to_corners_format - pil = PILImage.fromarray(image.to_rgb().data) + pil = PILImage.fromarray(image_to_rgb(image)) with torch.inference_mode(): inputs = self._processor( images=pil, text=queries, task="Detect all objects.", return_tensors="pt" diff --git a/dimos/perception/detection/detectors/owlv2.py b/dimos/perception/detection/detectors/owlv2.py index d88ee75b8d3..c408e7537a2 100644 --- a/dimos/perception/detection/detectors/owlv2.py +++ b/dimos/perception/detection/detectors/owlv2.py @@ -18,12 +18,14 @@ from functools import cached_property +from dimos_generated.sensor_msgs.msg import Image import numpy as np from PIL import Image as PILImage import torch from dimos.models.base import HuggingFaceModel, HuggingFaceModelConfig -from dimos.msgs.sensor_msgs.Image import Image +from dimos.msgs.image import image_to_rgb +from dimos.msgs.time import to_seconds from dimos.perception.detection.type.detection2d.bbox import Detection2DBBox from dimos.perception.detection.type.detection2d.imageDetections2D import ImageDetections2D @@ -74,7 +76,7 @@ def query_detections( ``confidence`` is the calibrated per-box score. ``class_id`` indexes into ``queries``. """ - pil = PILImage.fromarray(image.to_rgb().data) + pil = PILImage.fromarray(image_to_rgb(image)) with torch.inference_mode(): inputs = self._processor(text=[queries], images=pil, return_tensors="pt").to( self.config.device @@ -99,7 +101,7 @@ def query_detections( class_id=int(label), confidence=float(score), name=queries[int(label)], - ts=image.ts, + ts=to_seconds(image.header.stamp), image=image, ) if det.is_valid(): @@ -122,7 +124,7 @@ def query_score_rows( queries and refuse. Returns pixel ``(x1, y1, x2, y2)`` boxes and their score rows. """ - pil = PILImage.fromarray(image.to_rgb().data) + pil = PILImage.fromarray(image_to_rgb(image)) with torch.inference_mode(): inputs = self._processor(text=[queries], images=pil, return_tensors="pt").to( self.config.device diff --git a/dimos/perception/detection/detectors/person/yolo.py b/dimos/perception/detection/detectors/person/yolo.py index 635a3d119b7..649a1a489aa 100644 --- a/dimos/perception/detection/detectors/person/yolo.py +++ b/dimos/perception/detection/detectors/person/yolo.py @@ -12,7 +12,9 @@ # See the License for the specific language governing permissions and # limitations under the License. -from dimos.msgs.sensor_msgs.Image import Image +from dimos_generated.sensor_msgs.msg import Image + +from dimos.msgs.image import image_to_bgr from dimos.perception.detection.detectors.base import Detector from dimos.perception.detection.type.detection2d.imageDetections2D import ImageDetections2D from dimos.utils.data import get_data @@ -57,7 +59,7 @@ def process_image(self, image: Image) -> ImageDetections2D: ImageDetections2D containing Detection2DPerson objects with pose keypoints """ results = self.model.track( - source=image.to_opencv(), + source=image_to_bgr(image), verbose=False, conf=0.5, tracker=self.tracker, diff --git a/dimos/perception/detection/detectors/test_bbox_detectors.py b/dimos/perception/detection/detectors/test_bbox_detectors.py index 9ca3eca029a..d71abd7fe6a 100644 --- a/dimos/perception/detection/detectors/test_bbox_detectors.py +++ b/dimos/perception/detection/detectors/test_bbox_detectors.py @@ -12,10 +12,10 @@ # See the License for the specific language governing permissions and # limitations under the License. +from dimos_generated.sensor_msgs.msg import Image import pytest from dimos.core.transport import LCMTransport -from dimos.msgs.sensor_msgs.Image import Image from dimos.perception.detection.type.detection2d.base import Detection2D from dimos.perception.detection.type.detection2d.imageDetections2D import ImageDetections2D @@ -89,9 +89,8 @@ def test_detection_cropped_image(detections, test_image) -> None: assert cropped is not None # Cropped image should be smaller than original (usually) - if test_image.shape: - assert cropped.shape[0] <= test_image.shape[0] - assert cropped.shape[1] <= test_image.shape[1] + assert cropped.height <= test_image.height + assert cropped.width <= test_image.width def test_detection_ros_conversion(detections) -> None: diff --git a/dimos/perception/detection/detectors/yolo.py b/dimos/perception/detection/detectors/yolo.py index 20dd756ed4d..f9ad70d883a 100644 --- a/dimos/perception/detection/detectors/yolo.py +++ b/dimos/perception/detection/detectors/yolo.py @@ -12,7 +12,9 @@ # See the License for the specific language governing permissions and # limitations under the License. -from dimos.msgs.sensor_msgs.Image import Image +from dimos_generated.sensor_msgs.msg import Image + +from dimos.msgs.image import image_to_bgr from dimos.perception.detection.detectors.base import Detector from dimos.perception.detection.type.detection2d.imageDetections2D import ImageDetections2D from dimos.utils.data import get_data @@ -59,7 +61,7 @@ def process_image(self, image: Image) -> ImageDetections2D: ImageDetections2D containing all detected objects """ results = self.model.track( - source=image.to_opencv(), + source=image_to_bgr(image), device=self.device, conf=0.5, iou=0.6, diff --git a/dimos/perception/detection/detectors/yoloe.py b/dimos/perception/detection/detectors/yoloe.py index 4311f6783f7..856c6ab65f0 100644 --- a/dimos/perception/detection/detectors/yoloe.py +++ b/dimos/perception/detection/detectors/yoloe.py @@ -16,11 +16,12 @@ import threading from typing import Any +from dimos_generated.sensor_msgs.msg import Image import numpy as np from numpy.typing import NDArray import torch -from dimos.msgs.sensor_msgs.Image import Image +from dimos.msgs.image import image_to_bgr from dimos.perception.detection.detectors.base import Detector from dimos.perception.detection.type.detection2d.imageDetections2D import ImageDetections2D from dimos.utils.data import get_data @@ -144,7 +145,7 @@ def predict_image(self, image: Image) -> "ImageDetections2D[Any]": def _run(self, image: Image, *, track: bool) -> "ImageDetections2D[Any]": """Run tracking or prediction and apply common filtering.""" kwargs = { - "source": image.to_opencv(), + "source": image_to_bgr(image), "device": self.device, "conf": self.conf, "iou": 0.6, diff --git a/dimos/perception/detection/module2D.py b/dimos/perception/detection/module2D.py index 34e3e675915..696b16ae2a3 100644 --- a/dimos/perception/detection/module2D.py +++ b/dimos/perception/detection/module2D.py @@ -14,6 +14,11 @@ from collections.abc import Callable, Sequence from typing import Annotated, Any +from dimos_generated.geometry_msgs.msg import Quaternion, Transform, TransformStamped, Vector3 +from dimos_generated.sensor_msgs.msg import CameraInfo, Image +from dimos_generated.std_msgs.msg import Header +from dimos_generated.tf2_msgs.msg import TFMessage +from dimos_generated.vision_msgs.msg import Detection2DArray from pydantic.experimental.pipeline import validate_as from reactivex import operators as ops from reactivex.observable import Observable @@ -22,18 +27,14 @@ from dimos.core.core import rpc from dimos.core.module import Module, ModuleConfig from dimos.core.stream import IO, In, Out -from dimos.msgs.geometry_msgs.Transform import Transform -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo -from dimos.msgs.sensor_msgs.Image import Image, sharpness_barrier -from dimos.msgs.tf2_msgs.TFMessage import TFMessage -from dimos.msgs.vision_msgs.Detection2DArray import Detection2DArray +from dimos.msgs.image import image_sharpness +from dimos.msgs.time import to_seconds from dimos.perception.detection.detectors.base import Detector from dimos.perception.detection.detectors.yolo import Yolo2DDetector from dimos.perception.detection.type.detection2d.base import Filter2D from dimos.perception.detection.type.detection2d.imageDetections2D import ImageDetections2D from dimos.utils.decorators.decorators import simple_mcache -from dimos.utils.reactive import backpressure +from dimos.utils.reactive import backpressure, quality_barrier class Config(ModuleConfig): @@ -80,7 +81,7 @@ def process_image_frame(self, image: Image) -> ImageDetections2D: def sharp_image_stream(self) -> Observable[Image]: return backpressure( self.color_image.pure_observable().pipe( - sharpness_barrier(self.config.max_freq), + quality_barrier(image_sharpness, self.config.max_freq), ) ) @@ -89,7 +90,9 @@ def detection_stream_2d(self) -> Observable[ImageDetections2D]: return backpressure(self.sharp_image_stream().pipe(ops.map(self.process_image_frame))) def track(self, detections: ImageDetections2D) -> None: - sensor_frame = self.tfbuffer.get("sensor", "camera_optical", detections.image.ts, 5.0) + sensor_frame = self.tfbuffer.get( + "sensor", "camera_optical", to_seconds(detections.image.header.stamp), 5.0 + ) if not sensor_frame: return @@ -110,19 +113,19 @@ def track(self, detections: ImageDetections2D) -> None: detection = detections.detections[index] position_3d = self.pixel_to_3d( # type: ignore[attr-defined] detection.center_bbox, - self.config.camera_info, assumed_depth=1.0, ) else: # No detection at this index - publish zero transform - position_3d = Vector3(0.0, 0.0, 0.0) + position_3d = Vector3() transforms.append( - Transform( - frame_id=sensor_frame.child_frame_id, + TransformStamped( + header=Header( + frame_id=sensor_frame.child_frame_id, stamp=detections.image.header.stamp + ), child_frame_id=f"det_{index}", - ts=detections.image.ts, - translation=position_3d, + transform=Transform(translation=position_3d, rotation=Quaternion(w=1)), ) ) diff --git a/dimos/perception/detection/module3D.py b/dimos/perception/detection/module3D.py index 861dc14e873..f3a53e531ab 100644 --- a/dimos/perception/detection/module3D.py +++ b/dimos/perception/detection/module3D.py @@ -13,27 +13,36 @@ # limitations under the License. +from dimos_generated.geometry_msgs.msg import ( + Point, + Pose, + PoseStamped, + Quaternion, + TransformStamped, + Vector3, +) +from dimos_generated.sensor_msgs.msg import Image, PointCloud2 +from dimos_generated.std_msgs.msg import Header +from dimos_generated.vision_msgs.msg import Detection2DArray from reactivex import operators as ops from reactivex.observable import Observable from dimos.agents.annotation import skill from dimos.core.core import rpc from dimos.core.stream import In, Out -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Transform import Transform -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.msgs.sensor_msgs.Image import Image -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 -from dimos.msgs.vision_msgs.Detection2DArray import Detection2DArray +from dimos.msgs.time import to_seconds from dimos.perception.detection.module2D import Detection2DModule from dimos.perception.detection.type.detection2d.imageDetections2D import ImageDetections2D from dimos.perception.detection.type.detection3d.imageDetections3DPC import ImageDetections3DPC from dimos.perception.detection.type.detection3d.pointcloud import Detection3DPC -from dimos.types.timestamped import align_timestamped +from dimos.types.timestamped import TimestampedData, align_timestamped from dimos.utils.reactive import backpressure +def _timed_cloud(cloud: PointCloud2) -> TimestampedData[PointCloud2]: + return TimestampedData(cloud, to_seconds(cloud.header.stamp)) + + class Detection3DModule(Detection2DModule): color_image: In[Image] pointcloud: In[PointCloud2] @@ -56,7 +65,7 @@ def process_frame( self, detections: ImageDetections2D, pointcloud: PointCloud2, - transform: Transform | None, + transform: TransformStamped | None, ) -> ImageDetections3DPC: if not transform: return ImageDetections3DPC(detections.image, []) @@ -89,8 +98,8 @@ def pixel_to_3d( Vector3 position in camera optical frame coordinates """ # Extract camera intrinsics - fx, fy = self.config.camera_info.K[0], self.config.camera_info.K[4] - cx, cy = self.config.camera_info.K[2], self.config.camera_info.K[5] + fx, fy = self.config.camera_info.k[0], self.config.camera_info.k[4] + cx, cy = self.config.camera_info.k[2], self.config.camera_info.k[5] # Unproject pixel to normalized camera coordinates x_norm = (pixel[0] - cx) / fx @@ -98,7 +107,7 @@ def pixel_to_3d( # Create 3D point at assumed depth in camera optical frame # Camera optical frame: X right, Y down, Z forward - return Vector3(x_norm * assumed_depth, y_norm * assumed_depth, assumed_depth) + return Vector3(x=x_norm * assumed_depth, y=y_norm * assumed_depth, z=assumed_depth) @skill def ask_vlm(self, question: str) -> str: @@ -113,7 +122,7 @@ def ask_vlm(self, question: str) -> str: # @skill @rpc - def nav_vlm(self, question: str) -> str: + def nav_vlm(self, question: str) -> PoseStamped | None: """ query visual model about the view in front of the camera you can ask to mark objects like: @@ -131,17 +140,19 @@ def nav_vlm(self, question: str) -> str: print("VLM result:", result, "for", image, "and question", question) if isinstance(result, str) or not result or not len(result): - return None # type: ignore[return-value] + return None detections: ImageDetections2D = result print(detections) if not len(detections): print("No 2d detections") - return None # type: ignore[return-value] + return None pc = self.pointcloud.get_next() - transform = self.tfbuffer.get("camera_optical", pc.frame_id, detections.image.ts, 5.0) + transform = self.tfbuffer.get( + "camera_optical", pc.header.frame_id, to_seconds(detections.image.header.stamp), 5.0 + ) detections3d = self.process_frame(detections, pc, transform) @@ -150,11 +161,13 @@ def nav_vlm(self, question: str) -> str: print("No 3d detections, projecting 2d") center = detections[0].get_bbox_center() + position = self.pixel_to_3d(center, assumed_depth=1.5) return PoseStamped( - ts=detections.image.ts, - frame_id="world", - position=self.pixel_to_3d(center, assumed_depth=1.5), - orientation=Quaternion(0.0, 0.0, 0.0, 1.0), + header=Header(stamp=detections.image.header.stamp, frame_id="world"), + pose=Pose( + position=Point(x=position.x, y=position.y, z=position.z), + orientation=Quaternion(w=1), + ), ) @rpc @@ -162,13 +175,16 @@ def start(self) -> None: super().start() def detection2d_to_3d(args): # type: ignore[no-untyped-def] - detections, pc = args - transform = self.tfbuffer.get("camera_optical", pc.frame_id, detections.image.ts, 5.0) + detections, timed_pc = args + pc = timed_pc.value + transform = self.tfbuffer.get( + "camera_optical", pc.header.frame_id, to_seconds(detections.image.header.stamp), 5.0 + ) return self.process_frame(detections, pc, transform) self.detection_stream_3d = align_timestamped( # type: ignore[type-var] backpressure(self.detection_stream_2d()), - self.pointcloud.observable(), + self.pointcloud.observable().pipe(ops.map(_timed_cloud)), match_tolerance=0.25, buffer_size=20.0, ).pipe(ops.map(detection2d_to_3d)) diff --git a/dimos/perception/detection/person_tracker.py b/dimos/perception/detection/person_tracker.py index 1fb28dd29e7..0fb287856c4 100644 --- a/dimos/perception/detection/person_tracker.py +++ b/dimos/perception/detection/person_tracker.py @@ -15,24 +15,42 @@ from typing import Any +from dimos_generated.geometry_msgs.msg import Point, Pose, PoseStamped, Quaternion, Vector3 +from dimos_generated.sensor_msgs.msg import CameraInfo, Image +from dimos_generated.std_msgs.msg import Header +from dimos_generated.tf2_msgs.msg import TFMessage +from dimos_generated.vision_msgs.msg import Detection2DArray from reactivex import operators as ops from reactivex.observable import Observable from dimos.core.core import rpc from dimos.core.module import Module from dimos.core.stream import In, Out -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.geometry_msgs.Transform import Transform -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo -from dimos.msgs.sensor_msgs.Image import Image -from dimos.msgs.tf2_msgs.TFMessage import TFMessage -from dimos.msgs.vision_msgs.Detection2DArray import Detection2DArray +from dimos.msgs.geometry import compose_transforms, pose_from_transform, transform_from_pose +from dimos.msgs.time import to_seconds from dimos.perception.detection.type.detection2d.imageDetections2D import ImageDetections2D -from dimos.types.timestamped import align_timestamped +from dimos.types.timestamped import TimestampedData, align_timestamped from dimos.utils.reactive import backpressure +def _timed_image(message: Image) -> TimestampedData[Image]: + return TimestampedData(message, to_seconds(message.header.stamp)) + + +def _timed_detections(message: Detection2DArray) -> TimestampedData[Detection2DArray]: + return TimestampedData(message, to_seconds(message.header.stamp)) + + +def _has_detections(message: Detection2DArray) -> bool: + return len(message.detections) > 0 + + +def _paired_detections( + pair: tuple[TimestampedData[Image], TimestampedData[Detection2DArray]], +) -> ImageDetections2D: + return ImageDetections2D.from_ros_detection2d_array(pair[0].value, pair[1].value) + + class PersonTracker(Module): detections: In[Detection2DArray] color_image: In[Image] @@ -61,8 +79,8 @@ def center_to_3d( Vector3 position in camera_link frame coordinates (Z up, X forward) """ # Extract camera intrinsics - fx, fy = camera_info.K[0], camera_info.K[4] - cx, cy = camera_info.K[2], camera_info.K[5] + fx, fy = camera_info.k[0], camera_info.k[4] + cx, cy = camera_info.k[2], camera_info.k[5] # Unproject pixel to normalized camera coordinates x_norm = (pixel[0] - cx) / fx @@ -78,22 +96,18 @@ def center_to_3d( # Optical: X right, Y down, Z forward # Link: X forward, Y left, Z up # Transformation: x_link = z_optical, y_link = -x_optical, z_link = -y_optical - return Vector3(z_optical, -x_optical, -y_optical) + return Vector3(x=z_optical, y=-x_optical, z=-y_optical) def detections_stream(self) -> Observable[ImageDetections2D]: return backpressure( align_timestamped( - self.color_image.pure_observable(), + self.color_image.pure_observable().pipe(ops.map(_timed_image)), self.detections.pure_observable().pipe( - ops.filter(lambda d: d.detections_length > 0) # type: ignore[attr-defined] + ops.filter(_has_detections), ops.map(_timed_detections) ), match_tolerance=0.0, buffer_size=2.0, - ).pipe( - ops.map( - lambda pair: ImageDetections2D.from_ros_detection2d_array(*pair) # type: ignore[arg-type, misc] - ) - ) + ).pipe(ops.map(_paired_detections)) ) @rpc @@ -112,17 +126,18 @@ def track(self, detections2D: ImageDetections2D) -> None: vector = self.center_to_3d(target.center_bbox, self.camera_info, 2.0) pose_in_camera = PoseStamped( - ts=detections2D.ts, - position=vector, - frame_id="camera_link", + header=Header(frame_id="camera_link", stamp=detections2D.image.header.stamp), + pose=Pose( + position=Point(x=vector.x, y=vector.y, z=vector.z), orientation=Quaternion(w=1) + ), ) tf_world_to_camera = self.tfbuffer.get("world", "camera_link", detections2D.ts, 5.0) if not tf_world_to_camera: return - tf_camera_to_target = Transform.from_pose("target", pose_in_camera) - tf_world_to_target = tf_world_to_camera + tf_camera_to_target - pose_in_world = tf_world_to_target.to_pose(ts=detections2D.ts) + tf_camera_to_target = transform_from_pose(pose_in_camera, child_frame_id="target") + tf_world_to_target = compose_transforms(tf_world_to_camera, tf_camera_to_target) + pose_in_world = pose_from_transform(tf_world_to_target) self.target.publish(pose_in_world) diff --git a/dimos/perception/detection/project.py b/dimos/perception/detection/project.py index 1f9a8561ab1..d54e71e5422 100644 --- a/dimos/perception/detection/project.py +++ b/dimos/perception/detection/project.py @@ -18,20 +18,25 @@ from typing import TYPE_CHECKING, Any +from dimos_generated.geometry_msgs.msg import TransformStamped import numpy as np from dimos.memory.transform import Transformer -from dimos.msgs.geometry_msgs.Transform import Transform +from dimos.msgs.geometry import ( + compose_transforms, + inverse_transform, + transform_from_pose, + transform_matrix, +) +from dimos.msgs.image import image_view from dimos.perception.detection.type.detection3d.imageDetections3DPC import ImageDetections3DPC if TYPE_CHECKING: from collections.abc import Callable, Iterator - from dimos_lcm.sensor_msgs import CameraInfo + from dimos_generated.sensor_msgs.msg import CameraInfo, Image, PointCloud2 from dimos.memory.type.observation import Observation - from dimos.msgs.sensor_msgs.Image import Image - from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 from dimos.perception.detection.type.detection2d.imageDetections2D import ImageDetections2D from dimos.perception.detection.type.detection3d.pointcloud_filters import PointCloudFilter from dimos.protocol.tf.tf import TFLookup @@ -41,17 +46,20 @@ def _world_to_optical( obs: Observation[Any], world_frame: str, tf: TFLookup | None, - base_to_optical: Transform | None, + base_to_optical: TransformStamped | None, optical_frame: str, time_tolerance: float, -) -> Transform | None: +) -> TransformStamped | None: """Resolve the world→optical transform for an observation.""" if tf is not None: return tf.get(optical_frame, world_frame, obs.ts, time_tolerance) pose = obs.pose_stamped if base_to_optical is None or pose is None: return None - return -(Transform.from_pose("base_link", pose) + base_to_optical) + pose.header.frame_id = world_frame + return inverse_transform( + compose_transforms(transform_from_pose(pose, child_frame_id="base_link"), base_to_optical) + ) class ProjectDepthTo3D(Transformer["ImageDetections2D", ImageDetections3DPC]): @@ -67,7 +75,7 @@ def __init__( depth: Callable[[Observation[ImageDetections2D]], Image | None], camera_info: CameraInfo, tf: TFLookup | None = None, - base_to_optical: Transform | None = None, + base_to_optical: TransformStamped | None = None, world_frame: str = "world", optical_frame: str = "camera_optical", time_tolerance: float = 5.0, @@ -116,7 +124,7 @@ def sees( point: Any, camera_info: CameraInfo, tf: TFLookup | None = None, - base_to_optical: Transform | None = None, + base_to_optical: TransformStamped | None = None, world_frame: str = "world", optical_frame: str = "camera_optical", time_tolerance: float = 5.0, @@ -151,8 +159,8 @@ def sees( raise ValueError("sees needs either tf or base_to_optical") target = np.array([point.x, point.y, point.z]) if hasattr(point, "x") else np.asarray(point) - fx, fy = camera_info.K[0], camera_info.K[4] - cx, cy = camera_info.K[2], camera_info.K[5] + fx, fy = camera_info.k[0], camera_info.k[4] + cx, cy = camera_info.k[2], camera_info.k[5] width, height = float(camera_info.width), float(camera_info.height) if extent is not None: @@ -174,7 +182,7 @@ def _sees(obs: Observation[Any]) -> bool: ) if transform is None: return False - matrix = transform.to_matrix() + matrix = transform_matrix(transform.transform) pts = (matrix @ np.column_stack([samples, np.ones(len(samples))]).T).T[:, :3] in_front = pts[:, 2] > 0 if not in_front.any(): @@ -192,8 +200,9 @@ def _sees(obs: Observation[Any]) -> bool: if depth is not None and visible.any(): depth_frame = depth(obs) if depth_frame is not None: - depth_m = np.asarray(depth_frame.data, dtype=np.float32) - if depth_frame.data.dtype == np.uint16: + pixels = image_view(depth_frame) + depth_m = np.asarray(pixels, dtype=np.float32) + if pixels.dtype.kind == "u" and pixels.dtype.itemsize == 2: depth_m = depth_m * 0.001 rows = np.clip(v[visible].astype(int), 0, depth_m.shape[0] - 1) cols = np.clip(u[visible].astype(int), 0, depth_m.shape[1] - 1) @@ -225,7 +234,7 @@ def __init__( pointcloud: PointCloud2 | Callable[[Observation[ImageDetections2D]], PointCloud2 | None], camera_info: CameraInfo, tf: TFLookup | None = None, - base_to_optical: Transform | None = None, + base_to_optical: TransformStamped | None = None, optical_frame: str = "camera_optical", time_tolerance: float = 5.0, filters: list[PointCloudFilter] | None = None, @@ -249,7 +258,7 @@ def __call__( continue transform = _world_to_optical( obs, - pointcloud.frame_id, + pointcloud.header.frame_id, self.tf, self.base_to_optical, self.optical_frame, diff --git a/dimos/perception/detection/reid/module.py b/dimos/perception/detection/reid/module.py index c5420a42e23..e409e87f33f 100644 --- a/dimos/perception/detection/reid/module.py +++ b/dimos/perception/detection/reid/module.py @@ -12,21 +12,40 @@ # See the License for the specific language governing permissions and # limitations under the License. +from dimos_generated.sensor_msgs.msg import Image +from dimos_generated.vision_msgs.msg import Detection2DArray from reactivex import operators as ops from reactivex.observable import Observable from dimos.core.core import rpc from dimos.core.module import Module, ModuleConfig from dimos.core.stream import In -from dimos.msgs.sensor_msgs.Image import Image -from dimos.msgs.vision_msgs.Detection2DArray import Detection2DArray +from dimos.msgs.time import to_seconds from dimos.perception.detection.reid.embedding_id_system import EmbeddingIDSystem from dimos.perception.detection.reid.type import IDSystem from dimos.perception.detection.type.detection2d.imageDetections2D import ImageDetections2D -from dimos.types.timestamped import align_timestamped +from dimos.types.timestamped import TimestampedData, align_timestamped from dimos.utils.reactive import backpressure +def _timed_image(image: Image) -> TimestampedData[Image]: + return TimestampedData(image, to_seconds(image.header.stamp)) + + +def _timed_detections(detections: Detection2DArray) -> TimestampedData[Detection2DArray]: + return TimestampedData(detections, to_seconds(detections.header.stamp)) + + +def _has_detections(detections: Detection2DArray) -> bool: + return bool(detections.detections) + + +def _image_detections( + pair: tuple[TimestampedData[Image], TimestampedData[Detection2DArray]], +) -> ImageDetections2D: + return ImageDetections2D.from_ros_detection2d_array(pair[0].value, pair[1].value) + + class Config(ModuleConfig): idsystem: IDSystem @@ -37,7 +56,6 @@ class ReidModule(Module): image: In[Image] def __init__(self, idsystem: IDSystem | None = None, **kwargs) -> None: # type: ignore[no-untyped-def] - super().__init__(**kwargs) if idsystem is None: try: from dimos.models.embedding.treid import TorchReIDModel @@ -48,18 +66,19 @@ def __init__(self, idsystem: IDSystem | None = None, **kwargs) -> None: # type: "TorchReIDModel not available. Please install with: pip install dimos[torchreid]" ) from e + super().__init__(idsystem=idsystem, **kwargs) self.idsystem = idsystem def detections_stream(self) -> Observable[ImageDetections2D]: return backpressure( align_timestamped( - self.image.pure_observable(), + self.image.pure_observable().pipe(ops.map(_timed_image)), self.detections.pure_observable().pipe( - ops.filter(lambda d: d.detections_length > 0) # type: ignore[attr-defined] + ops.filter(_has_detections), ops.map(_timed_detections) ), match_tolerance=0.0, buffer_size=2.0, - ).pipe(ops.map(lambda pair: ImageDetections2D.from_ros_detection2d_array(*pair))) # type: ignore[arg-type, misc] + ).pipe(ops.map(_image_detections)) ) @rpc diff --git a/dimos/perception/detection/reid/test_embedding_id_system.py b/dimos/perception/detection/reid/test_embedding_id_system.py index 83bae94b88e..4f37e66ea82 100644 --- a/dimos/perception/detection/reid/test_embedding_id_system.py +++ b/dimos/perception/detection/reid/test_embedding_id_system.py @@ -15,7 +15,7 @@ import numpy as np import pytest -from dimos.msgs.sensor_msgs.Image import Image +from dimos.msgs.image import image_from_file from dimos.perception.detection.reid.embedding_id_system import EmbeddingIDSystem from dimos.utils.data import get_data @@ -43,7 +43,7 @@ def track_associator(mobileclip_model): @pytest.fixture(scope="session") def test_image(): """Load test image.""" - return Image.from_file(get_data("cafe.jpg")).to_rgb() + return image_from_file(get_data("cafe.jpg")) @pytest.mark.self_hosted diff --git a/dimos/perception/detection/reid/test_generated_module.py b/dimos/perception/detection/reid/test_generated_module.py new file mode 100644 index 00000000000..1443f2d0c9d --- /dev/null +++ b/dimos/perception/detection/reid/test_generated_module.py @@ -0,0 +1,91 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from threading import Event +from unittest.mock import MagicMock + +from dimos_generated.sensor_msgs.msg import Image +from dimos_generated.std_msgs.msg import Header +from dimos_generated.vision_msgs.msg import ( + BoundingBox2D, + Detection2D, + Detection2DArray, + Point2D, + Pose2D, +) +import numpy as np +import pytest +from reactivex.scheduler import ThreadPoolScheduler +from reactivex.subject import Subject + +from dimos.msgs.image import image_from_array +from dimos.msgs.time import time_from_seconds +from dimos.perception.detection.reid.module import ReidModule +from dimos.perception.detection.reid.type import PassthroughIDSystem + + +def test_generated_detection_alignment_uses_headers_and_filters_empty_arrays( + monkeypatch: pytest.MonkeyPatch, +) -> None: + scheduler = ThreadPoolScheduler(max_workers=2) + monkeypatch.setattr("dimos.utils.reactive.get_scheduler", lambda: scheduler) + module = ReidModule(idsystem=PassthroughIDSystem()) + images: Subject[Image] = Subject() + detections: Subject[Detection2DArray] = Subject() + module.image = MagicMock() + module.image.pure_observable.return_value = images + module.detections = MagicMock() + module.detections.pure_observable.return_value = detections + received = [] + errors = [] + ready = Event() + + def receive(value: object) -> None: + received.append(value) + ready.set() + + subscription = module.detections_stream().subscribe(receive, errors.append) + header = Header(stamp=time_from_seconds(1.25), frame_id="camera") + image = image_from_array(np.zeros((32, 32, 3), dtype=np.uint8), encoding="rgb8", header=header) + try: + detections.on_next(Detection2DArray(header=header)) + detections.on_next( + Detection2DArray( + header=header, + detections=[ + Detection2D( + header=header, + id="7", + bbox=BoundingBox2D( + center=Pose2D(position=Point2D(x=16, y=16)), size_x=8, size_y=8 + ), + ) + ], + ) + ) + images.on_next(image) + assert ready.wait(timeout=2.0) + assert not errors + (result,) = received + assert result.image is image + (detection,) = result.detections + assert detection.track_id == 7 + assert detection.bbox == (12, 12, 20, 20) + assert result.ts == 1.25 + finally: + subscription.dispose() + images.dispose() + detections.dispose() + module.stop() + scheduler.executor.shutdown(wait=True) diff --git a/dimos/perception/detection/type/detection2d/base.py b/dimos/perception/detection/type/detection2d/base.py index 7ee1d5bf1ad..0abc4a7329b 100644 --- a/dimos/perception/detection/type/detection2d/base.py +++ b/dimos/perception/detection/type/detection2d/base.py @@ -15,9 +15,9 @@ from abc import abstractmethod from collections.abc import Callable -from dimos_lcm.vision_msgs import Detection2D as ROSDetection2D +from dimos_generated.sensor_msgs.msg import Image +from dimos_generated.vision_msgs.msg import Detection2D as ROSDetection2D -from dimos.msgs.sensor_msgs.Image import Image from dimos.types.timestamped import Timestamped diff --git a/dimos/perception/detection/type/detection2d/bbox.py b/dimos/perception/detection/type/detection2d/bbox.py index 8facd3ac776..301fc1b4bf8 100644 --- a/dimos/perception/detection/type/detection2d/bbox.py +++ b/dimos/perception/detection/type/detection2d/bbox.py @@ -19,12 +19,11 @@ from typing import TYPE_CHECKING, Any if TYPE_CHECKING: + from dimos_generated.sensor_msgs.msg import Image from typing_extensions import Self from ultralytics.engine.results import Results - from dimos.msgs.sensor_msgs.Image import Image - -from dimos_lcm.vision_msgs import ( +from dimos_generated.vision_msgs.msg import ( BoundingBox2D, Detection2D as ROSDetection2D, ObjectHypothesis, @@ -32,12 +31,13 @@ Point2D, Pose2D, ) +import numpy as np from rich.console import Console from rich.text import Text -from dimos.msgs.std_msgs.Header import Header +from dimos.msgs.image import image_from_array, image_to_bgr, image_view +from dimos.msgs.time import to_seconds from dimos.perception.detection.type.detection2d.base import Detection2D -from dimos.types.timestamped import to_timestamp from dimos.utils.decorators.decorators import simple_mcache Bbox = tuple[float, float, float, float] @@ -94,7 +94,6 @@ def to_repr_dict(self) -> dict[str, Any]: def draw_on(self, img: Any, scale: float = 1.0) -> None: """Draw this detection's bbox and label onto a BGR numpy array (in-place).""" import cv2 - import numpy as np x1, y1, x2, y2 = map(int, self.bbox) @@ -127,11 +126,9 @@ def draw_on(self, img: Any, scale: float = 1.0) -> None: def annotated_image(self, scale: float = 1.0) -> Image: """Return the full image with this detection's bbox and label drawn on it.""" - img = self.image.to_opencv().copy() + img = image_to_bgr(self.image) self.draw_on(img, scale=scale) - from dimos.msgs.sensor_msgs.Image import Image - - return Image.from_opencv(img, ts=self.ts) + return image_from_array(img, encoding="bgr8", header=self.image.header) # return focused image, only on the bbox def cropped_image(self, padding: int = 20) -> Image: @@ -144,9 +141,12 @@ def cropped_image(self, padding: int = 20) -> Image: Cropped Image containing only the detection area plus padding """ x1, y1, x2, y2 = map(int, self.bbox) - return self.image.crop( - x1 - padding, y1 - padding, x2 - x1 + 2 * padding, y2 - y1 + 2 * padding - ) + pixels = image_view(self.image) + cropped = pixels[ + max(0, y1 - padding) : min(self.image.height, y2 + padding), + max(0, x1 - padding) : min(self.image.width, x2 + padding), + ] + return image_from_array(cropped, encoding=self.image.encoding, header=self.image.header) def __str__(self) -> str: console = Console(force_terminal=True, legacy_windows=False) @@ -202,8 +202,8 @@ def is_valid(self) -> bool: return False # Check if within image bounds (if image has shape) - if self.image.shape: - h, w = self.image.shape[:2] + if self.image.height and self.image.width: + h, w = self.image.height, self.image.width if not (0 <= x1 <= w and 0 <= y1 <= h and 0 <= x2 <= w and 0 <= y2 <= h): return False @@ -255,7 +255,7 @@ def from_ultralytics_result(cls, result: Results, idx: int, image: Image) -> Det class_id=class_id, confidence=confidence, name=name, - ts=image.ts, + ts=to_seconds(image.header.stamp), image=image, ) @@ -299,14 +299,14 @@ def from_ros_detection2d(cls, ros_det: ROSDetection2D, **kwargs) -> Self: # typ confidence = 0.0 if ros_det.results: hypothesis = ros_det.results[0].hypothesis - class_id = hypothesis.class_id + class_id = int(hypothesis.class_id) confidence = hypothesis.score # Extract track_id track_id = int(ros_det.id) if ros_det.id.isdigit() else 0 # Extract timestamp - ts = to_timestamp(ros_det.header.stamp) + ts = to_seconds(ros_det.header.stamp) name = kwargs.pop("name", f"class_{class_id}") @@ -322,12 +322,12 @@ def from_ros_detection2d(cls, ros_det: ROSDetection2D, **kwargs) -> Self: # typ def to_ros_detection2d(self) -> ROSDetection2D: return ROSDetection2D( - header=Header(self.ts, "camera_link"), + header=self.image.header, bbox=self.to_ros_bbox(), results=[ ObjectHypothesisWithPose( - ObjectHypothesis( - class_id=self.class_id, + hypothesis=ObjectHypothesis( + class_id=str(self.class_id), score=self.confidence, ) ) diff --git a/dimos/perception/detection/type/detection2d/imageDetections2D.py b/dimos/perception/detection/type/detection2d/imageDetections2D.py index 507125c333f..7b775cff544 100644 --- a/dimos/perception/detection/type/detection2d/imageDetections2D.py +++ b/dimos/perception/detection/type/detection2d/imageDetections2D.py @@ -25,11 +25,10 @@ from dimos.perception.detection.type.imageDetections import ImageDetections if TYPE_CHECKING: + from dimos_generated.sensor_msgs.msg import Image + from dimos_generated.vision_msgs.msg import Detection2DArray from ultralytics.engine.results import Results - from dimos.msgs.sensor_msgs.Image import Image - from dimos.msgs.vision_msgs.Detection2DArray import Detection2DArray - T2D = TypeVar("T2D", bound=Detection2D, default=Detection2DBBox) diff --git a/dimos/perception/detection/type/detection2d/person.py b/dimos/perception/detection/type/detection2d/person.py index 30100756e1e..94efb9e97b0 100644 --- a/dimos/perception/detection/type/detection2d/person.py +++ b/dimos/perception/detection/type/detection2d/person.py @@ -17,9 +17,10 @@ # Import for type checking only to avoid circular imports from typing import TYPE_CHECKING +from dimos_generated.sensor_msgs.msg import Image import numpy as np -from dimos.msgs.sensor_msgs.Image import Image +from dimos.msgs.time import to_seconds from dimos.perception.detection.type.detection2d.bbox import Bbox, Detection2DBBox from dimos.utils.decorators.decorators import simple_mcache @@ -152,7 +153,7 @@ def from_ultralytics_result( class_id=class_id, confidence=confidence, name=name, - ts=image.ts, + ts=to_seconds(image.header.stamp), image=image, # Person specific fields keypoints=keypoints, diff --git a/dimos/perception/detection/type/detection2d/point.py b/dimos/perception/detection/type/detection2d/point.py index 4e6d14ed27e..4042fd76ea0 100644 --- a/dimos/perception/detection/type/detection2d/point.py +++ b/dimos/perception/detection/type/detection2d/point.py @@ -17,7 +17,7 @@ from dataclasses import dataclass from typing import TYPE_CHECKING -from dimos_lcm.vision_msgs import ( +from dimos_generated.vision_msgs.msg import ( BoundingBox2D, Detection2D as ROSDetection2D, ObjectHypothesis, @@ -26,11 +26,11 @@ Pose2D, ) -from dimos.msgs.std_msgs.Header import Header +from dimos.msgs.image import image_from_array, image_view from dimos.perception.detection.type.detection2d.base import Detection2D if TYPE_CHECKING: - from dimos.msgs.sensor_msgs.Image import Image + from dimos_generated.sensor_msgs.msg import Image @dataclass @@ -65,17 +65,17 @@ def cropped_image(self, padding: int = 20) -> Image: Cropped Image containing the area around the point """ x, y = int(self.x), int(self.y) - return self.image.crop( - x - padding, - y - padding, - 2 * padding, - 2 * padding, - ) + pixels = image_view(self.image) + cropped = pixels[ + max(0, y - padding) : min(self.image.height, y + padding), + max(0, x - padding) : min(self.image.width, x + padding), + ] + return image_from_array(cropped, encoding=self.image.encoding, header=self.image.header) def to_ros_detection2d(self) -> ROSDetection2D: """Convert point to ROS Detection2D message (as zero-size bbox at point).""" return ROSDetection2D( - header=Header(self.ts, "camera_link"), + header=self.image.header, bbox=BoundingBox2D( center=Pose2D( position=Point2D(x=self.x, y=self.y), @@ -86,8 +86,8 @@ def to_ros_detection2d(self) -> ROSDetection2D: ), results=[ ObjectHypothesisWithPose( - ObjectHypothesis( - class_id=self.class_id, + hypothesis=ObjectHypothesis( + class_id=str(self.class_id), score=self.confidence, ) ) @@ -97,7 +97,7 @@ def to_ros_detection2d(self) -> ROSDetection2D: def is_valid(self) -> bool: """Check if the point is within image bounds.""" - if self.image.shape: - h, w = self.image.shape[:2] + if self.image.height and self.image.width: + h, w = self.image.height, self.image.width return bool(0 <= self.x <= w and 0 <= self.y <= h) return True diff --git a/dimos/perception/detection/type/detection2d/seg.py b/dimos/perception/detection/type/detection2d/seg.py index d96f2a8e7a1..852e0af12da 100644 --- a/dimos/perception/detection/type/detection2d/seg.py +++ b/dimos/perception/detection/type/detection2d/seg.py @@ -19,14 +19,14 @@ import numpy as np +from dimos.msgs.time import to_seconds from dimos.perception.detection.type.detection2d.bbox import Bbox, Detection2DBBox if TYPE_CHECKING: + from dimos_generated.sensor_msgs.msg import Image import torch from ultralytics.engine.results import Results - from dimos.msgs.sensor_msgs.Image import Image - @dataclass class Detection2DSeg(Detection2DBBox): @@ -90,7 +90,7 @@ def from_sam2_result( class_id=class_id, confidence=confidence, name=name, - ts=image.ts, + ts=to_seconds(image.header.stamp), image=image, mask=mask.astype(np.uint8), ) @@ -164,7 +164,7 @@ def from_ultralytics_result(cls, result: Results, idx: int, image: Image) -> Det class_id=class_id, confidence=confidence, name=name, - ts=image.ts, + ts=to_seconds(image.header.stamp), image=image, mask=mask, ) diff --git a/dimos/perception/detection/type/detection2d/test_bbox.py b/dimos/perception/detection/type/detection2d/test_bbox.py index cca4c8e59c2..312841ff60a 100644 --- a/dimos/perception/detection/type/detection2d/test_bbox.py +++ b/dimos/perception/detection/type/detection2d/test_bbox.py @@ -13,6 +13,8 @@ # limitations under the License. import pytest +from dimos.msgs.image import image_view + @pytest.mark.self_hosted def test_detection2d(detection2d) -> None: @@ -69,7 +71,7 @@ def test_detection2d(detection2d) -> None: # The actual cropped image is (260, 192, 3) assert cropped.width == 192 assert cropped.height == 260 - assert cropped.shape == (260, 192, 3) + assert image_view(cropped).shape == (260, 192, 3) # def test_to_ros_bbox(detection2d): """Test ROS bounding box conversion.""" diff --git a/dimos/perception/detection/type/detection2d/test_generated_bbox.py b/dimos/perception/detection/type/detection2d/test_generated_bbox.py new file mode 100644 index 00000000000..56648f90c71 --- /dev/null +++ b/dimos/perception/detection/type/detection2d/test_generated_bbox.py @@ -0,0 +1,101 @@ +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from dimos_generated.builtin_interfaces.msg import Time +from dimos_generated.std_msgs.msg import Header +from dimos_generated.vision_msgs.msg import Detection2D, Detection2DArray +import numpy as np + +from dimos.msgs.image import image_from_array, image_view +from dimos.perception.detection.type.detection2d.bbox import Detection2DBBox +from dimos.perception.detection.type.detection2d.imageDetections2D import ImageDetections2D +from dimos.perception.detection.type.detection2d.point import Detection2DPoint + + +def test_generated_bbox_image_and_wire_round_trip(): + header = Header(frame_id="camera_optical", stamp=Time(sec=1700000000, nanosec=123456789)) + pixels = np.zeros((40, 60, 3), dtype=np.uint8) + pixels[:, :, 0] = 250 + image = image_from_array(pixels, encoding="rgb8", header=header) + det = Detection2DBBox( + bbox=(10, 10, 30, 30), + track_id=7, + class_id=2, + confidence=0.9, + name="target", + ts=1.0, + image=image, + ) + assert det.is_valid() + crop = det.cropped_image(padding=2) + assert (crop.width, crop.height) == (24, 24) + assert crop.header == header + np.testing.assert_array_equal(image_view(crop), pixels[8:32, 8:32]) + annotated = det.annotated_image() + assert annotated.encoding == "bgr8" + assert annotated.header == header + np.testing.assert_array_equal(image_view(annotated)[39, 59], [0, 0, 250]) + np.testing.assert_array_equal(image_view(image), pixels) + wire = Detection2D.decode(det.to_ros_detection2d().encode()) + assert wire.header == header + assert wire.results[0].hypothesis.class_id == "2" + restored = Detection2DBBox.from_ros_detection2d(wire, image=image) + assert restored.bbox == det.bbox + assert restored.track_id == 7 + assert restored.class_id == 2 + + +def test_generated_detection_collection_round_trip(): + pixels = np.zeros((40, 60, 3), dtype=np.uint8) + image = image_from_array( + pixels, encoding="rgb8", header=Header(frame_id="camera", stamp=Time(nanosec=123)) + ) + det = Detection2DBBox( + bbox=(10, 10, 30, 30), + track_id=7, + class_id=2, + confidence=0.9, + name="target", + ts=0, + image=image, + ) + collection = ImageDetections2D(image=image, detections=[det]) + wire = Detection2DArray.decode(collection.to_ros_detection2d_array().encode()) + assert wire.header == image.header + assert len(wire.detections) == 1 + restored = ImageDetections2D.from_ros_detection2d_array(image, wire) + assert len(restored) == 1 + assert restored[0].bbox == det.bbox + annotated = collection.annotated_image() + assert annotated.header == image.header + assert annotated.encoding == "bgr8" + assert np.any(image_view(annotated)) + assert not np.any(image_view(image)) + + +def test_generated_point_detection_crop_and_cdr(): + pixels = np.arange(100, dtype=np.uint8).reshape(10, 10) + image = image_from_array( + pixels, encoding="mono8", header=Header(frame_id="camera", stamp=Time(nanosec=789)) + ) + point = Detection2DPoint(x=1, y=1, name="point", ts=0, image=image) + assert point.is_valid() + crop = point.cropped_image(padding=2) + np.testing.assert_array_equal(image_view(crop), pixels[:3, :3]) + assert crop.header == image.header + wire = Detection2D.decode(point.to_ros_detection2d().encode()) + assert wire.header == image.header + assert wire.bbox.center.position.x == wire.bbox.center.position.y == 1 + assert wire.bbox.size_x == wire.bbox.size_y == 0 + assert wire.results[0].hypothesis.class_id == "-1" diff --git a/dimos/perception/detection/type/detection2d/test_person.py b/dimos/perception/detection/type/detection2d/test_person.py index 3223380f4dc..59fc51e9a0d 100644 --- a/dimos/perception/detection/type/detection2d/test_person.py +++ b/dimos/perception/detection/type/detection2d/test_person.py @@ -18,14 +18,14 @@ def test_person_ros_confidence() -> None: """Test that Detection2DPerson preserves confidence when converting to ROS format.""" - from dimos.msgs.sensor_msgs.Image import Image + from dimos.msgs.image import image_from_file from dimos.perception.detection.detectors.person.yolo import YoloPersonDetector from dimos.perception.detection.type.detection2d.person import Detection2DPerson from dimos.utils.data import get_data # Load test image image_path = get_data("cafe.jpg") - image = Image.from_file(image_path) + image = image_from_file(image_path) # Run pose detection detector = YoloPersonDetector(device="cpu") diff --git a/dimos/perception/detection/type/detection3d/base.py b/dimos/perception/detection/type/detection3d/base.py index afe37aac6e2..f3b95df4d50 100644 --- a/dimos/perception/detection/type/detection3d/base.py +++ b/dimos/perception/detection/type/detection3d/base.py @@ -18,11 +18,12 @@ from dataclasses import dataclass, field from typing import TYPE_CHECKING -from dimos.msgs.geometry_msgs.Transform import Transform +from dimos_generated.geometry_msgs.msg import Quaternion, Transform, TransformStamped + from dimos.perception.detection.type.detection2d.bbox import Detection2DBBox if TYPE_CHECKING: - from dimos_lcm.sensor_msgs import CameraInfo + from dimos_generated.sensor_msgs.msg import CameraInfo @dataclass @@ -30,7 +31,9 @@ class Detection3D(Detection2DBBox): """Abstract base class for 3D detections.""" frame_id: str = "" - transform: Transform = field(default_factory=Transform.identity) + transform: TransformStamped = field( + default_factory=lambda: TransformStamped(transform=Transform(rotation=Quaternion(w=1))) + ) @classmethod @abstractmethod @@ -39,7 +42,7 @@ def from_2d( det: Detection2DBBox, distance: float, camera_info: CameraInfo, - world_to_optical_transform: Transform, + world_to_optical_transform: TransformStamped, ) -> Detection3D | None: """Create a 3D detection from a 2D detection.""" ... diff --git a/dimos/perception/detection/type/detection3d/bbox.py b/dimos/perception/detection/type/detection3d/bbox.py index be3b9b2f96d..e12078effe7 100644 --- a/dimos/perception/detection/type/detection3d/bbox.py +++ b/dimos/perception/detection/type/detection3d/bbox.py @@ -18,15 +18,23 @@ import functools from typing import Any -from dimos_lcm.vision_msgs import BoundingBox3D, ObjectHypothesis, ObjectHypothesisWithPose - -from dimos.msgs.geometry_msgs.Pose import Pose -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Transform import Transform -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.msgs.std_msgs.Header import Header -from dimos.msgs.vision_msgs.Detection3D import Detection3D +from dimos_generated.geometry_msgs.msg import ( + Point, + Pose, + PoseStamped, + Quaternion, + TransformStamped, + Vector3, +) +from dimos_generated.std_msgs.msg import Header +from dimos_generated.vision_msgs.msg import ( + BoundingBox3D, + Detection3D, + ObjectHypothesis, + ObjectHypothesisWithPose, +) + +from dimos.msgs.geometry import point_distance from dimos.perception.detection.type.detection2d.bbox import Detection2DBBox @@ -39,9 +47,9 @@ class Detection3DBBox(Detection2DBBox): center: Vector3 # Center point in world frame size: Vector3 # Width, height, depth - transform: Transform | None = None # Camera to world transform + transform: TransformStamped | None = None # Camera to world transform frame_id: str = "" # Frame ID (e.g., "world", "map") - orientation: Quaternion = field(default_factory=lambda: Quaternion(0.0, 0.0, 0.0, 1.0)) + orientation: Quaternion = field(default_factory=lambda: Quaternion(w=1)) @functools.cached_property def pose(self) -> PoseStamped: @@ -50,16 +58,17 @@ def pose(self) -> PoseStamped: Returns pose in world frame with the detection's orientation. """ return PoseStamped( - ts=self.ts, - frame_id=self.frame_id, - position=self.center, - orientation=self.orientation, + header=Header(stamp=self.image.header.stamp, frame_id=self.frame_id), + pose=Pose( + position=Point(x=self.center.x, y=self.center.y, z=self.center.z), + orientation=self.orientation, + ), ) def to_detection3d_msg(self) -> Detection3D: """Convert to ROS Detection3D message.""" msg = Detection3D() - msg.header = Header(self.ts, self.frame_id) + msg.header = Header(stamp=self.image.header.stamp, frame_id=self.frame_id) # Results msg.results = [ @@ -70,13 +79,10 @@ def to_detection3d_msg(self) -> Detection3D: ) ) ] - msg.results_length = len(msg.results) - # The generated LCM constructor reuses a shared default bbox object. - # Assign a new BoundingBox3D so this message does not share bbox state with others. msg.bbox = BoundingBox3D( center=Pose( - position=self.center, + position=Point(x=self.center.x, y=self.center.y, z=self.center.z), orientation=self.orientation, ), size=self.size, @@ -88,8 +94,11 @@ def to_repr_dict(self) -> dict[str, Any]: # Calculate distance from camera if self.transform is None: return super().to_repr_dict() - camera_pos = self.transform.translation - distance = (self.center - camera_pos).magnitude() + camera_pos = self.transform.transform.translation + distance = point_distance( + Point(x=self.center.x, y=self.center.y, z=self.center.z), + Point(x=camera_pos.x, y=camera_pos.y, z=camera_pos.z), + ) parent_dict = super().to_repr_dict() # Remove bbox key if present diff --git a/dimos/perception/detection/type/detection3d/imageDetections3D.py b/dimos/perception/detection/type/detection3d/imageDetections3D.py index 93b33d15660..c6e55afd9ed 100644 --- a/dimos/perception/detection/type/detection3d/imageDetections3D.py +++ b/dimos/perception/detection/type/detection3d/imageDetections3D.py @@ -16,10 +16,10 @@ from typing import Generic +from dimos_generated.std_msgs.msg import Header +from dimos_generated.vision_msgs.msg import Detection3DArray from typing_extensions import TypeVar -from dimos.msgs.std_msgs.Header import Header -from dimos.msgs.vision_msgs.Detection3DArray import Detection3DArray from dimos.perception.detection.type.detection3d.bbox import Detection3DBBox from dimos.perception.detection.type.imageDetections import ImageDetections @@ -36,7 +36,6 @@ def to_ros_detection3d_array(self, frame_id: str | None = None) -> Detection3DAr detections = [det.to_detection3d_msg() for det in self.detections] return Detection3DArray( - detections_length=len(detections), - header=Header(self.image.ts, resolved_frame_id), + header=Header(stamp=self.image.header.stamp, frame_id=resolved_frame_id), detections=detections, ) diff --git a/dimos/perception/detection/type/detection3d/imageDetections3DPC.py b/dimos/perception/detection/type/detection3d/imageDetections3DPC.py index e63d415b8aa..add2b42f3ea 100644 --- a/dimos/perception/detection/type/detection3d/imageDetections3DPC.py +++ b/dimos/perception/detection/type/detection3d/imageDetections3DPC.py @@ -20,11 +20,9 @@ from dimos.perception.detection.type.imageDetections import ImageDetections if TYPE_CHECKING: - from dimos_lcm.sensor_msgs import CameraInfo + from dimos_generated.geometry_msgs.msg import TransformStamped + from dimos_generated.sensor_msgs.msg import CameraInfo, Image, PointCloud2 - from dimos.msgs.geometry_msgs.Transform import Transform - from dimos.msgs.sensor_msgs.Image import Image - from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 from dimos.perception.detection.type.detection2d.imageDetections2D import ImageDetections2D from dimos.perception.detection.type.detection3d.pointcloud_filters import PointCloudFilter @@ -38,7 +36,7 @@ def from_2d( detections_2d: ImageDetections2D, world_pointcloud: PointCloud2, camera_info: CameraInfo, - world_to_optical_transform: Transform, + world_to_optical_transform: TransformStamped, filters: list[PointCloudFilter] | None = None, ) -> ImageDetections3DPC: """Project every 2D detection into 3D, dropping any that yield no valid points.""" @@ -64,7 +62,7 @@ def from_depth( detections_2d: ImageDetections2D, depth: Image, camera_info: CameraInfo, - world_to_optical_transform: Transform, + world_to_optical_transform: TransformStamped, filters: list[PointCloudFilter] | None = None, ) -> ImageDetections3DPC: """Unproject every detection's depth pixels into 3D, dropping empty results.""" diff --git a/dimos/perception/detection/type/detection3d/marker.py b/dimos/perception/detection/type/detection3d/marker.py index 9fd58cc6d85..f28104ddfa1 100644 --- a/dimos/perception/detection/type/detection3d/marker.py +++ b/dimos/perception/detection/type/detection3d/marker.py @@ -17,9 +17,9 @@ from dataclasses import dataclass, field from typing import Any +from dimos_generated.vision_msgs.msg import Detection3D import numpy as np -from dimos.msgs.vision_msgs.Detection3D import Detection3D from dimos.perception.detection.type.detection3d.bbox import Detection3DBBox @@ -35,7 +35,9 @@ class Detection3DMarker(Detection3DBBox): """ marker_id: int = -1 - corners_px: np.ndarray = field(default_factory=lambda: np.zeros((4, 2), dtype=np.float32)) + corners_px: np.ndarray[Any, np.dtype[np.float32]] = field( + default_factory=lambda: np.zeros((4, 2), dtype=np.float32) + ) dictionary: str = "" reprojection_error: float = 0.0 @@ -52,8 +54,9 @@ def to_detection3d_msg(self) -> Detection3D: msg = super().to_detection3d_msg() msg.id = str(self.marker_id) if msg.results: - msg.results[0].hypothesis.class_id = self.marker_label - msg.results_length = len(msg.results) + result = msg.results[0] + result.hypothesis.class_id = self.marker_label + msg.results[0] = result return msg def to_repr_dict(self) -> dict[str, Any]: diff --git a/dimos/perception/detection/type/detection3d/pointcloud.py b/dimos/perception/detection/type/detection3d/pointcloud.py index 691c7ae32af..21cbe0f43a3 100644 --- a/dimos/perception/detection/type/detection3d/pointcloud.py +++ b/dimos/perception/detection/type/detection3d/pointcloud.py @@ -18,12 +18,22 @@ import functools from typing import TYPE_CHECKING, Any +from dimos_generated.geometry_msgs.msg import ( + Point, + Pose, + PoseStamped, + Quaternion, + TransformStamped, + Vector3, +) +from dimos_generated.sensor_msgs.msg import PointCloud2 +from dimos_generated.std_msgs.msg import Header import numpy as np +from numpy.typing import NDArray -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.geometry_msgs.Transform import Transform -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 +from dimos.msgs.geometry import inverse_transform, transform_matrix +from dimos.msgs.image import image_view +from dimos.msgs.pointcloud import pointcloud_from_xyz, pointcloud_xyz, select_points from dimos.perception.detection.type.detection3d.base import Detection3D from dimos.perception.detection.type.detection3d.pointcloud_filters import ( PointCloudFilter, @@ -33,9 +43,9 @@ ) if TYPE_CHECKING: - from dimos_lcm.sensor_msgs import CameraInfo + from dimos_generated.sensor_msgs.msg import CameraInfo, Image + import open3d as o3d - from dimos.msgs.sensor_msgs.Image import Image from dimos.perception.detection.type.detection2d.bbox import Detection2DBBox @@ -45,7 +55,8 @@ class Detection3DPC(Detection3D): @functools.cached_property def center(self) -> Vector3: - return Vector3(*self.pointcloud.center) + center = pointcloud_xyz(self.pointcloud).mean(axis=0) + return Vector3(x=float(center[0]), y=float(center[1]), z=float(center[2])) @functools.cached_property def pose(self) -> PoseStamped: @@ -55,36 +66,53 @@ def pose(self) -> PoseStamped: The pointcloud is already in world frame. """ return PoseStamped( - ts=self.ts, - frame_id=self.frame_id, - position=self.center, - orientation=(0.0, 0.0, 0.0, 1.0), # Identity quaternion + header=self.pointcloud.header, + pose=Pose( + position=Point(x=self.center.x, y=self.center.y, z=self.center.z), + orientation=Quaternion(w=1), + ), ) - def get_bounding_box(self): # type: ignore[no-untyped-def] + def _open3d(self) -> o3d.geometry.PointCloud: + import open3d as o3d + + return o3d.geometry.PointCloud(o3d.utility.Vector3dVector(pointcloud_xyz(self.pointcloud))) + + def get_bounding_box(self) -> o3d.geometry.AxisAlignedBoundingBox: """Get axis-aligned bounding box of the detection's pointcloud.""" - return self.pointcloud.axis_aligned_bounding_box + return self._open3d().get_axis_aligned_bounding_box() - def get_oriented_bounding_box(self): # type: ignore[no-untyped-def] + def get_oriented_bounding_box(self) -> o3d.geometry.OrientedBoundingBox: """Get oriented bounding box of the detection's pointcloud.""" - return self.pointcloud.oriented_bounding_box + return self._open3d().get_oriented_bounding_box() def get_bounding_box_dimensions(self) -> tuple[float, float, float]: """Get dimensions (width, height, depth) of the detection's bounding box.""" - return self.pointcloud.bounding_box_dimensions + extent = self.get_bounding_box().get_extent() + return float(extent[0]), float(extent[1]), float(extent[2]) def bounding_box_intersects(self, other: Detection3DPC) -> bool: """Check if this detection's bounding box intersects with another's.""" - return self.pointcloud.bounding_box_intersects(other.pointcloud) + first, second = self.get_bounding_box(), other.get_bounding_box() + return bool( + np.all(first.get_min_bound() <= second.get_max_bound()) + and np.all(second.get_min_bound() <= first.get_max_bound()) + ) def to_repr_dict(self) -> dict[str, Any]: # Calculate distance from camera # The pointcloud is in world frame, and transform gives camera position in world center_world = self.center # Camera position in world frame is the translation part of the transform - camera_pos = self.transform.translation + camera_pos = inverse_transform(self.transform).transform.translation # Use Vector3 subtraction and magnitude - distance = (center_world - camera_pos).magnitude() + distance = np.linalg.norm( + [ + center_world.x - camera_pos.x, + center_world.y - camera_pos.y, + center_world.z - camera_pos.z, + ] + ) parent_dict = super().to_repr_dict() # Remove bbox key if present @@ -93,7 +121,7 @@ def to_repr_dict(self) -> dict[str, Any]: return { **parent_dict, "dist": f"{distance:.2f}m", - "points": str(len(self.pointcloud)), + "points": str(self.pointcloud.width * self.pointcloud.height), } @classmethod @@ -102,7 +130,7 @@ def from_depth( det: Detection2DBBox, depth: Image, camera_info: CameraInfo, - world_to_optical_transform: Transform, + world_to_optical_transform: TransformStamped, filters: list[PointCloudFilter] | None = None, max_depth: float = 10.0, depth_gap: float = 0.1, @@ -117,13 +145,16 @@ def from_depth( segmentation mask is eroded to ``mask_scale`` of its size first, since the bleed lives on the mask boundary. """ + import cv2 + # no radius_outlier: dense depth clouds make radius search expensive, # and the depth-gap cluster above already drops disconnected points if filters is None: filters = [statistical()] - depth_m = np.asarray(depth.data, dtype=np.float32) - if depth.data.dtype == np.uint16: + pixels = image_view(depth) + depth_m = pixels.astype(np.float32) + if pixels.dtype.kind == "u" and pixels.dtype.itemsize == 2: depth_m *= 0.001 height, width = depth_m.shape[:2] @@ -131,8 +162,6 @@ def from_depth( if seg_mask is not None: pixel_mask = seg_mask > 0 if mask_scale < 1.0: - import cv2 - radius = float(np.sqrt(pixel_mask.sum() / np.pi)) erode_px = round((1.0 - mask_scale) * radius) if erode_px > 0: @@ -168,12 +197,16 @@ def from_depth( rows, cols, z = rows[keep], cols[keep], z[keep] break - fx, fy = camera_info.K[0], camera_info.K[4] - cx, cy = camera_info.K[2], camera_info.K[5] + fx, fy = camera_info.k[0], camera_info.k[4] + cx, cy = camera_info.k[2], camera_info.k[5] points_optical = np.column_stack(((cols - cx) * z / fx, (rows - cy) * z / fy, z)) - detection_pc = PointCloud2.from_numpy(points_optical, timestamp=det.ts).transform( - -world_to_optical_transform + optical_to_world = inverse_transform(world_to_optical_transform) + matrix = transform_matrix(optical_to_world.transform) + points_world = points_optical @ matrix[:3, :3].T + matrix[:3, 3] + detection_pc = pointcloud_from_xyz( + points_world, + header=Header(stamp=depth.header.stamp, frame_id=optical_to_world.header.frame_id), ) for filter_func in filters: @@ -182,7 +215,7 @@ def from_depth( return None detection_pc = result - if len(detection_pc.pointcloud.points) == 0: + if detection_pc.width * detection_pc.height == 0: return None return cls( @@ -195,7 +228,7 @@ def from_depth( ts=det.ts, pointcloud=detection_pc, transform=world_to_optical_transform, - frame_id=detection_pc.frame_id, + frame_id=detection_pc.header.frame_id, ) @classmethod @@ -204,7 +237,7 @@ def from_2d( # type: ignore[override] det: Detection2DBBox, world_pointcloud: PointCloud2, camera_info: CameraInfo, - world_to_optical_transform: Transform, + world_to_optical_transform: TransformStamped, # filters are to be adjusted based on the sensor noise characteristics if feeding # sensor data directly filters: list[PointCloudFilter] | None = None, @@ -236,25 +269,27 @@ def from_2d( # type: ignore[override] ] # Extract camera parameters - fx, fy = camera_info.K[0], camera_info.K[4] - cx, cy = camera_info.K[2], camera_info.K[5] + fx, fy = camera_info.k[0], camera_info.k[4] + cx, cy = camera_info.k[2], camera_info.k[5] image_width = camera_info.width image_height = camera_info.height camera_matrix = np.array([[fx, 0, cx], [0, fy, cy], [0, 0, 1]]) # Convert pointcloud to numpy array - world_points, _ = world_pointcloud.as_numpy() + world_points = pointcloud_xyz(world_pointcloud) + indices: NDArray[np.int64] = np.arange(len(world_points), dtype=np.int64) # Project points to camera frame points_homogeneous = np.hstack([world_points, np.ones((world_points.shape[0], 1))]) - extrinsics_matrix = world_to_optical_transform.to_matrix() + extrinsics_matrix = transform_matrix(world_to_optical_transform.transform) points_camera = (extrinsics_matrix @ points_homogeneous.T).T # Filter out points behind the camera valid_mask = points_camera[:, 2] > 0 points_camera = points_camera[valid_mask] world_points = world_points[valid_mask] + indices = indices[valid_mask] if len(world_points) == 0: return None @@ -272,6 +307,7 @@ def from_2d( # type: ignore[override] ) points_2d = points_2d[in_image_mask] world_points = world_points[in_image_mask] + indices = indices[in_image_mask] if len(world_points) == 0: return None @@ -300,11 +336,9 @@ def from_2d( # type: ignore[override] return None # Create initial pointcloud for this detection - initial_pc = PointCloud2.from_numpy( - detection_points, - frame_id=world_pointcloud.frame_id, - timestamp=world_pointcloud.ts, - ) + keep = np.zeros(world_pointcloud.width * world_pointcloud.height, dtype=bool) + keep[indices[in_det_mask]] = True + initial_pc = select_points(world_pointcloud, keep) # Apply filters - each filter gets all arguments detection_pc = initial_pc @@ -315,7 +349,7 @@ def from_2d( # type: ignore[override] detection_pc = result # Final check for empty pointcloud - if len(detection_pc.pointcloud.points) == 0: + if detection_pc.width * detection_pc.height == 0: return None # Create Detection3D with filtered pointcloud @@ -329,5 +363,5 @@ def from_2d( # type: ignore[override] ts=det.ts, pointcloud=detection_pc, transform=world_to_optical_transform, - frame_id=world_pointcloud.frame_id, + frame_id=world_pointcloud.header.frame_id, ) diff --git a/dimos/perception/detection/type/detection3d/pointcloud_filters.py b/dimos/perception/detection/type/detection3d/pointcloud_filters.py index fdb2afeebb2..3e8598d64a0 100644 --- a/dimos/perception/detection/type/detection3d/pointcloud_filters.py +++ b/dimos/perception/detection/type/detection3d/pointcloud_filters.py @@ -15,34 +15,51 @@ from __future__ import annotations from collections.abc import Callable +from typing import TYPE_CHECKING -from dimos_lcm.sensor_msgs import CameraInfo +from dimos_generated.geometry_msgs.msg import TransformStamped +from dimos_generated.sensor_msgs.msg import CameraInfo, PointCloud2 +import numpy as np -from dimos.msgs.geometry_msgs.Transform import Transform -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 -from dimos.perception.detection.type.detection2d.bbox import Detection2DBBox +from dimos.msgs.geometry import inverse_transform +from dimos.msgs.pointcloud import pointcloud_xyz, select_points + +if TYPE_CHECKING: + import open3d as o3d + + from dimos.perception.detection.type.detection2d.bbox import Detection2DBBox -# Filters take Detection2DBBox, PointCloud2, CameraInfo, Transform and return filtered PointCloud2 or None PointCloudFilter = Callable[ - [Detection2DBBox, PointCloud2, CameraInfo, Transform], PointCloud2 | None + ["Detection2DBBox", PointCloud2, CameraInfo, TransformStamped], PointCloud2 | None ] +def _open3d_cloud(pc: PointCloud2) -> o3d.geometry.PointCloud: + import open3d as o3d + + return o3d.geometry.PointCloud(o3d.utility.Vector3dVector(pointcloud_xyz(pc))) + + +def _selected(pc: PointCloud2, indices: list[int]) -> PointCloud2: + keep = np.zeros(pc.width * pc.height, dtype=bool) + keep[indices] = True + return select_points(pc, keep) + + def height_filter(height: float = 0.1) -> PointCloudFilter: - return lambda det, pc, ci, tf: pc.filter_by_height(height) + return lambda det, pc, ci, tf: select_points(pc, pointcloud_xyz(pc)[:, 2] >= height) def statistical(nb_neighbors: int = 40, std_ratio: float = 0.5) -> PointCloudFilter: def filter_func( - det: Detection2DBBox, pc: PointCloud2, ci: CameraInfo, tf: Transform + det: Detection2DBBox, pc: PointCloud2, ci: CameraInfo, tf: TransformStamped ) -> PointCloud2 | None: try: - statistical, _removed = pc.pointcloud.remove_statistical_outlier( + _, indices = _open3d_cloud(pc).remove_statistical_outlier( nb_neighbors=nb_neighbors, std_ratio=std_ratio ) - return PointCloud2(statistical, pc.frame_id, pc.ts) - except Exception: - # print("statistical filter failed:", e) + return _selected(pc, indices) + except RuntimeError: return None return filter_func @@ -50,33 +67,27 @@ def filter_func( def raycast() -> PointCloudFilter: def filter_func( - det: Detection2DBBox, pc: PointCloud2, ci: CameraInfo, tf: Transform + det: Detection2DBBox, pc: PointCloud2, ci: CameraInfo, tf: TransformStamped ) -> PointCloud2 | None: try: - camera_pos = tf.inverse().translation - camera_pos_np = camera_pos.to_numpy() - _, visible_indices = pc.pointcloud.hidden_point_removal(camera_pos_np, radius=100.0) - visible_pcd = pc.pointcloud.select_by_index(visible_indices) - return PointCloud2(visible_pcd, pc.frame_id, pc.ts) - except Exception: - # print("raycast filter failed:", e) + camera_pos = inverse_transform(tf).transform.translation + _, indices = _open3d_cloud(pc).hidden_point_removal( + np.array([camera_pos.x, camera_pos.y, camera_pos.z]), radius=100.0 + ) + return _selected(pc, indices) + except RuntimeError: return None return filter_func def radius_outlier(min_neighbors: int = 20, radius: float = 0.3) -> PointCloudFilter: - """ - Remove isolated points: keep only points that have at least `min_neighbors` - neighbors within `radius` meters (same units as your point cloud). - """ + """Keep points with at least ``min_neighbors`` within ``radius`` meters.""" def filter_func( - det: Detection2DBBox, pc: PointCloud2, ci: CameraInfo, tf: Transform + det: Detection2DBBox, pc: PointCloud2, ci: CameraInfo, tf: TransformStamped ) -> PointCloud2 | None: - filtered_pcd, _removed = pc.pointcloud.remove_radius_outlier( - nb_points=min_neighbors, radius=radius - ) - return PointCloud2(filtered_pcd, pc.frame_id, pc.ts) + _, indices = _open3d_cloud(pc).remove_radius_outlier(nb_points=min_neighbors, radius=radius) + return _selected(pc, indices) return filter_func diff --git a/dimos/perception/detection/type/detection3d/test_generated_filters.py b/dimos/perception/detection/type/detection3d/test_generated_filters.py new file mode 100644 index 00000000000..7ae5a158ec2 --- /dev/null +++ b/dimos/perception/detection/type/detection3d/test_generated_filters.py @@ -0,0 +1,68 @@ +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from types import SimpleNamespace + +from dimos_generated.builtin_interfaces.msg import Time +from dimos_generated.geometry_msgs.msg import Quaternion, Transform, TransformStamped +from dimos_generated.sensor_msgs.msg import CameraInfo, PointCloud2, PointField +from dimos_generated.std_msgs.msg import Header +import numpy as np +import pytest + +from dimos.msgs.pointcloud import pointcloud_view +from dimos.perception.detection.type.detection3d.pointcloud_filters import ( + height_filter, + radius_outlier, + raycast, + statistical, +) + + +@pytest.mark.parametrize("make_filter", [height_filter, statistical, radius_outlier, raycast]) +def test_filters_preserve_custom_point_records(make_filter): + rng = np.random.default_rng(42) + values = np.zeros(101, dtype=[("x", " Image: - return Image( - data=np.zeros((80, 100, 3), dtype=np.uint8), - format=ImageFormat.BGR, - frame_id="camera_optical", - ts=ts, + return image_from_array( + np.zeros((80, 100, 3), dtype=np.uint8), + encoding="bgr8", + header=Header(frame_id="camera_optical", stamp=time_from_seconds(ts)), ) @@ -52,12 +53,12 @@ def _marker( class_id=marker_id, confidence=confidence, name="", - ts=image.ts, + ts=to_seconds(image.header.stamp), image=image, - center=Vector3(float(marker_id), 2.0, 3.0), - size=Vector3(0.16, 0.16, 0.0), + center=Vector3(x=float(marker_id), y=2.0, z=3.0), + size=Vector3(x=0.16, y=0.16, z=0.0), frame_id=frame_id, - orientation=Quaternion(0.0, 0.0, 0.0, 1.0), + orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0), marker_id=marker_id, corners_px=np.array( [[x1, y1], [x2, y1], [x2, y2], [x1, y2]], @@ -76,22 +77,22 @@ def test_to_ros_detection3d_array_serializes_plain_bbox_results() -> None: class_id=9, confidence=0.75, name="box", - ts=image.ts, + ts=to_seconds(image.header.stamp), image=image, - center=Vector3(1.0, 2.0, 3.0), - size=Vector3(0.4, 0.5, 0.6), + center=Vector3(x=1.0, y=2.0, z=3.0), + size=Vector3(x=0.4, y=0.5, z=0.6), frame_id="world", - orientation=Quaternion(0.0, 0.0, 0.0, 1.0), + orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0), ) msg = ImageDetections3D(image, [det]).to_ros_detection3d_array() - assert msg.detections_length == 1 - assert msg.detections[0].results_length == 1 + assert len(msg.detections) == 1 + assert len(msg.detections[0].results) == 1 - decoded = Detection3DArray.lcm_decode(msg.lcm_encode()) + decoded = Detection3DArray.decode(msg.encode()) decoded_det = decoded.detections[0] - assert decoded_det.results_length == 1 + assert len(decoded_det.results) == 1 assert decoded_det.results[0].hypothesis.class_id == "9" assert decoded_det.results[0].hypothesis.score == pytest.approx(0.75) assert decoded_det.bbox.center.position.x == pytest.approx(1.0) @@ -111,23 +112,23 @@ def test_to_ros_detection3d_array_preserves_marker_wire_identity() -> None: msg = detections.to_ros_detection3d_array() assert msg.header.frame_id == "world" - assert msg.ts == pytest.approx(image.ts) - assert msg.detections_length == 2 + assert to_seconds(msg.header.stamp) == pytest.approx(to_seconds(image.header.stamp)) + assert len(msg.detections) == 2 assert len(msg.detections) == 2 first = msg.detections[0] assert first.header.frame_id == "world" assert first.id == "7" - assert first.results_length == 1 + assert len(first.results) == 1 assert first.results[0].hypothesis.class_id == "DICT_APRILTAG_36h11:7" assert first.results[0].hypothesis.score == pytest.approx(1.0) assert first.bbox.center.position.x == pytest.approx(7.0) assert first.bbox.size.x == pytest.approx(0.16) assert first.bbox.size.z == pytest.approx(0.0) - decoded = Detection3DArray.lcm_decode(msg.lcm_encode()) + decoded = Detection3DArray.decode(msg.encode()) assert decoded.header.frame_id == "world" - assert decoded.detections_length == 2 + assert len(decoded.detections) == 2 assert decoded.detections[1].id == "42" assert decoded.detections[1].results[0].hypothesis.class_id == "DICT_APRILTAG_36h11:42" @@ -140,15 +141,15 @@ def test_to_ros_detection3d_array_uses_override_and_handles_empty_frames() -> No ) assert msg.header.frame_id == "map" - assert msg.ts == pytest.approx(image.ts) - assert msg.detections_length == 1 + assert to_seconds(msg.header.stamp) == pytest.approx(to_seconds(image.header.stamp)) + assert len(msg.detections) == 1 empty = ImageDetections3D(image, []).to_ros_detection3d_array(frame_id="world") assert empty.header.frame_id == "world" - assert empty.ts == pytest.approx(image.ts) - assert empty.detections_length == 0 - assert empty.detections == [] + assert to_seconds(empty.header.stamp) == pytest.approx(to_seconds(image.header.stamp)) + assert len(empty.detections) == 0 + assert len(empty.detections) == 0 def test_filter_and_annotated_image_work_for_3d_marker_detections() -> None: @@ -162,10 +163,28 @@ def test_filter_and_annotated_image_work_for_3d_marker_detections() -> None: assert isinstance(filtered, ImageDetections3D) assert filtered.detections == [keep] ros_msg = filtered.to_ros_detection3d_array() - assert ros_msg.detections_length == 1 + assert len(ros_msg.detections) == 1 assert ros_msg.detections[0].id == "5" annotated = filtered.annotated_image() - assert annotated.ts == pytest.approx(image.ts) - assert np.count_nonzero(annotated.data) > 0 - assert np.count_nonzero(image.data) == 0 + assert to_seconds(annotated.header.stamp) == pytest.approx(to_seconds(image.header.stamp)) + assert np.count_nonzero(image_view(annotated)) > 0 + assert np.count_nonzero(image_view(image)) == 0 + + +def test_marker_pose_and_array_keep_exact_source_stamp() -> None: + image = _image() + image.header.stamp.sec = 1700000000 + image.header.stamp.nanosec = 123456789 + marker = _marker(image, marker_id=42) + pose = marker.pose + message = marker.to_detection3d_msg() + array = ImageDetections3D(image, [marker]).to_ros_detection3d_array() + decoded = Detection3DArray.decode(array.encode()) + assert pose.header.stamp == image.header.stamp + assert message.header.stamp == image.header.stamp + assert decoded.header.stamp == image.header.stamp + assert decoded.detections[0].header.stamp == image.header.stamp + marker.center.x = 999 + assert pose.pose.position.x == 42 + assert decoded.detections[0].bbox.center.position.x == 42 diff --git a/dimos/perception/detection/type/detection3d/test_marker.py b/dimos/perception/detection/type/detection3d/test_marker.py index 79a71f0debb..6d9cb9e4e43 100644 --- a/dimos/perception/detection/type/detection3d/test_marker.py +++ b/dimos/perception/detection/type/detection3d/test_marker.py @@ -13,13 +13,14 @@ # limitations under the License. import cv2 +from dimos_generated.geometry_msgs.msg import Quaternion, Vector3 +from dimos_generated.std_msgs.msg import Header +from dimos_generated.vision_msgs.msg import Detection3D import numpy as np import pytest -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.msgs.sensor_msgs.Image import Image -from dimos.msgs.vision_msgs.Detection3D import Detection3D +from dimos.msgs.image import image_from_array +from dimos.msgs.time import time_from_seconds, to_seconds from dimos.perception.detection.type.detection3d.marker import Detection3DMarker from dimos.perception.fiducial.marker_pose import marker_reprojection_error @@ -41,9 +42,9 @@ def _project_synthetic_marker_corners( def test_marker_detection3d_msg_preserves_marker_identity_on_wire() -> None: - center = Vector3(1.0, 2.0, 3.0) - orientation = Quaternion(0.1, 0.2, 0.3, 0.9) - size = Vector3(0.16, 0.16, 0.0) + center = Vector3(x=1.0, y=2.0, z=3.0) + orientation = Quaternion(x=0.1, y=0.2, z=0.3, w=0.9) + size = Vector3(x=0.16, y=0.16, z=0.0) marker_length_m = 0.16 camera_matrix = np.array( [[420.0, 0.0, 60.0], [0.0, 420.0, 50.0], [0.0, 0.0, 1.0]], @@ -68,10 +69,10 @@ def test_marker_detection3d_msg_preserves_marker_identity_on_wire() -> None: rvec, tvec, ) - image = Image( - data=np.zeros((100, 120, 3), dtype=np.uint8), - frame_id="camera_optical", - ts=123.456, + image = image_from_array( + np.zeros((100, 120, 3), dtype=np.uint8), + encoding="bgr8", + header=Header(frame_id="camera_optical", stamp=time_from_seconds(123.456)), ) det = Detection3DMarker( @@ -80,7 +81,7 @@ def test_marker_detection3d_msg_preserves_marker_identity_on_wire() -> None: class_id=999, confidence=1.0, name="marker_42", - ts=image.ts, + ts=to_seconds(image.header.stamp), image=image, center=center, size=size, @@ -99,7 +100,7 @@ def test_marker_detection3d_msg_preserves_marker_identity_on_wire() -> None: assert msg.header.frame_id == "world" assert msg.id == "42" - assert msg.results_length == 1 + assert len(msg.results) == 1 assert msg.results[0].hypothesis.class_id == "DICT_APRILTAG_36h11:42" assert msg.results[0].hypothesis.score == pytest.approx(1.0) assert msg.bbox.center.position.x == pytest.approx(center.x) @@ -113,7 +114,7 @@ def test_marker_detection3d_msg_preserves_marker_identity_on_wire() -> None: assert msg.bbox.size.y == pytest.approx(size.y) assert msg.bbox.size.z == pytest.approx(size.z) - decoded = Detection3D.lcm_decode(msg.lcm_encode()) + decoded = Detection3D.decode(msg.encode()) assert decoded.id == "42" - assert decoded.results_length == 1 + assert len(decoded.results) == 1 assert decoded.results[0].hypothesis.class_id == "DICT_APRILTAG_36h11:42" diff --git a/dimos/perception/detection/type/detection3d/test_pointcloud.py b/dimos/perception/detection/type/detection3d/test_pointcloud.py index c9e62af6dea..16864a031d1 100644 --- a/dimos/perception/detection/type/detection3d/test_pointcloud.py +++ b/dimos/perception/detection/type/detection3d/test_pointcloud.py @@ -15,6 +15,8 @@ import numpy as np import pytest +from dimos.msgs.pointcloud import pointcloud_xyz + pytestmark = pytest.mark.self_hosted @@ -60,10 +62,10 @@ def test_detection3dpc(detection3dpc) -> None: # def test_point_cloud_properties(detection3dpc): """Test point cloud data and boundaries.""" - points, _ = detection3dpc.pointcloud.as_numpy() + points = pointcloud_xyz(detection3dpc.pointcloud) assert len(points) > 60 - assert detection3dpc.pointcloud.frame_id == "world", ( - f"Expected frame_id 'world', got '{detection3dpc.pointcloud.frame_id}'" + assert detection3dpc.pointcloud.header.frame_id == "world", ( + f"Expected frame_id 'world', got '{detection3dpc.pointcloud.header.frame_id}'" ) min_pt = np.min(points, axis=0) @@ -85,9 +87,9 @@ def test_detection3dpc(detection3dpc) -> None: # def test_detection_pose(detection3dpc): """Test detection pose and frame information.""" - assert detection3dpc.pose.x == pytest.approx(-3.327, abs=0.1) - assert detection3dpc.pose.y == pytest.approx(-0.202, abs=0.1) - assert detection3dpc.pose.z == pytest.approx(0.160, abs=0.1) - assert detection3dpc.pose.frame_id == "world", ( - f"Expected frame_id 'world', got '{detection3dpc.pose.frame_id}'" + assert detection3dpc.pose.pose.position.x == pytest.approx(-3.327, abs=0.1) + assert detection3dpc.pose.pose.position.y == pytest.approx(-0.202, abs=0.1) + assert detection3dpc.pose.pose.position.z == pytest.approx(0.160, abs=0.1) + assert detection3dpc.pose.header.frame_id == "world", ( + f"Expected frame_id 'world', got '{detection3dpc.pose.header.frame_id}'" ) diff --git a/dimos/perception/detection/type/imageDetections.py b/dimos/perception/detection/type/imageDetections.py index 98fd0e5388f..199e17690b3 100644 --- a/dimos/perception/detection/type/imageDetections.py +++ b/dimos/perception/detection/type/imageDetections.py @@ -22,15 +22,17 @@ else: from typing_extensions import Self -from dimos_lcm.vision_msgs import Detection2DArray +from dimos_generated.vision_msgs.msg import Detection2DArray -from dimos.msgs.std_msgs.Header import Header +from dimos.msgs.image import image_from_array, image_to_bgr +from dimos.msgs.time import to_seconds from dimos.perception.detection.type.utils import TableStr if TYPE_CHECKING: from collections.abc import Callable, Iterator - from dimos.msgs.sensor_msgs.Image import Image + from dimos_generated.sensor_msgs.msg import Image + from dimos.perception.detection.type.detection2d.base import Detection2D T = TypeVar("T", bound=Detection2D) @@ -46,14 +48,14 @@ class ImageDetections(Generic[T], TableStr): @property def ts(self) -> float: - return self.image.ts + return to_seconds(self.image.header.stamp) def __init__(self, image: Image, detections: list[T] | None = None) -> None: self.image = image self.detections = detections or [] for det in self.detections: if not det.ts: - det.ts = image.ts + det.ts = to_seconds(image.header.stamp) def __len__(self) -> int: return len(self.detections) @@ -80,18 +82,15 @@ def filter(self, *predicates: Callable[[T], bool]) -> Self: def to_ros_detection2d_array(self) -> Detection2DArray: return Detection2DArray( - detections_length=len(self.detections), - header=Header(self.image.ts, "camera_optical"), + header=self.image.header, detections=[det.to_ros_detection2d() for det in self.detections], ) def annotated_image(self, scale: float = 1.0) -> Image: """Return the image with all detection bboxes and labels drawn on it.""" - img = self.image.to_opencv().copy() + img = image_to_bgr(self.image) for det in self.detections: if hasattr(det, "draw_on"): det.draw_on(img, scale=scale) - from dimos.msgs.sensor_msgs.Image import Image as ImageMsg - - return ImageMsg.from_opencv(img, ts=self.image.ts) + return image_from_array(img, encoding="bgr8", header=self.image.header) diff --git a/dimos/perception/experimental/moduleDB.py b/dimos/perception/experimental/moduleDB.py index 305bca063a0..4f5d6ddc266 100644 --- a/dimos/perception/experimental/moduleDB.py +++ b/dimos/perception/experimental/moduleDB.py @@ -16,17 +16,16 @@ import time from typing import Any +from dimos_generated.geometry_msgs.msg import Point, Pose, PoseStamped, Quaternion, Vector3 +from dimos_generated.sensor_msgs.msg import Image, PointCloud2 +from dimos_generated.std_msgs.msg import Header +from dimos_generated.vision_msgs.msg import Detection2DArray +import numpy as np from reactivex.observable import Observable from dimos.core.core import rpc from dimos.core.stream import In, Out -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Transform import Transform -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.msgs.sensor_msgs.Image import Image -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 -from dimos.msgs.vision_msgs.Detection2DArray import Detection2DArray +from dimos.msgs.pointcloud import concatenate_clouds from dimos.perception.detection.module3D import Detection3DModule from dimos.perception.detection.type.detection3d.imageDetections3DPC import ImageDetections3DPC from dimos.perception.detection.type.detection3d.pointcloud import Detection3DPC @@ -45,7 +44,11 @@ def to_repr_dict(self) -> dict[str, Any]: center_str = "None" else: center_str = ( - "[" + ", ".join(list(map(lambda n: f"{n:1f}", self.center.to_list()))) + "]" + "[" + + ", ".join( + list(map(lambda n: f"{n:1f}", [self.center.x, self.center.y, self.center.z])) + ) + + "]" ) return { "object_id": self.track_id, @@ -82,9 +85,16 @@ def __add__(self, detection: Detection3DPC) -> "Object3D": new_object.class_id = self.class_id new_object.name = self.name new_object.transform = self.transform - new_object.pointcloud = self.pointcloud + detection.pointcloud + new_object.pointcloud = concatenate_clouds(self.pointcloud, detection.pointcloud) new_object.frame_id = self.frame_id - new_object.center = (self.center + detection.center) / 2 + if self.center is None: + raise ValueError("Cannot merge an object without a center") + center = detection.center + new_object.center = Vector3( + x=(self.center.x + center.x) / 2, + y=(self.center.y + center.y) / 2, + z=(self.center.z + center.z) / 2, + ) new_object.detections = self.detections + 1 if detection.bbox_2d_volume() > self.bbox_2d_volume(): @@ -113,26 +123,14 @@ def to_pose(self) -> PoseStamped: if self.best_detection is None or self.center is None: raise ValueError("Cannot compute pose without best_detection and center") - optical_inverse = Transform( - translation=Vector3(0.0, 0.0, 0.0), - rotation=Quaternion(-0.5, 0.5, -0.5, 0.5), - frame_id="camera_link", - child_frame_id="camera_optical", - ).inverse() - - print("transform is", self.best_detection.transform) - - global_transform = optical_inverse + self.best_detection.transform - - print("inverse optical is", global_transform) - - print("obj center is", self.center) - global_pose = global_transform.to_pose() - print("Global pose:", global_pose) - global_pose.frame_id = self.best_detection.frame_id - print("remap to", self.best_detection.frame_id) return PoseStamped( - position=self.center, orientation=Quaternion(), frame_id=self.best_detection.frame_id + header=Header( + frame_id=self.best_detection.frame_id, stamp=self.pointcloud.header.stamp + ), + pose=Pose( + position=Point(x=self.center.x, y=self.center.y, z=self.center.z), + orientation=Quaternion(w=1), + ), ) @@ -177,13 +175,26 @@ def __init__(self, *args, **kwargs) -> None: # type: ignore[no-untyped-def] def closest_object(self, detection: Detection3DPC) -> Object3D | None: # Filter objects to only those with matching names - matching_objects = [obj for obj in self.objects.values() if obj.name == detection.name] + matching_objects = [ + obj + for obj in self.objects.values() + if obj.name == detection.name and obj.center is not None + ] if not matching_objects: return None # Sort by distance - distances = sorted(matching_objects, key=lambda obj: detection.center.distance(obj.center)) + def distance(obj: Object3D) -> float: + assert obj.center is not None + center = detection.center + return float( + np.linalg.norm( + [center.x - obj.center.x, center.y - obj.center.y, center.z - obj.center.z] + ) + ) + + distances = sorted(matching_objects, key=distance) return distances[0] diff --git a/dimos/perception/experimental/object.py b/dimos/perception/experimental/object.py index 76f4cc425c1..2c568201481 100644 --- a/dimos/perception/experimental/object.py +++ b/dimos/perception/experimental/object.py @@ -19,25 +19,41 @@ from typing import TYPE_CHECKING, Any import uuid -from dimos_lcm.geometry_msgs import Pose -from dimos_lcm.vision_msgs import BoundingBox3D, ObjectHypothesis, ObjectHypothesisWithPose +from dimos_generated.geometry_msgs.msg import ( + Point, + Pose, + PoseStamped, + Quaternion, + TransformStamped, + Vector3, +) +from dimos_generated.sensor_msgs.msg import CameraInfo, Image, PointCloud2 +from dimos_generated.std_msgs.msg import Header +from dimos_generated.vision_msgs.msg import ( + BoundingBox3D, + Detection3D as ROSDetection3D, + Detection3DArray, + ObjectHypothesis, + ObjectHypothesisWithPose, +) import numpy as np -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Transform import Transform -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.msgs.sensor_msgs.Image import Image -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 -from dimos.msgs.std_msgs.Header import Header -from dimos.msgs.vision_msgs.Detection3D import Detection3D as ROSDetection3D -from dimos.msgs.vision_msgs.Detection3DArray import Detection3DArray +from dimos.msgs.geometry import quaternion_from_matrix, transform_matrix +from dimos.msgs.image import image_to_rgb, image_view +from dimos.msgs.pointcloud import ( + concatenate_clouds, + pointcloud_from_xyz, + pointcloud_from_xyz_rgb, + pointcloud_rgb, + pointcloud_to_open3d, + pointcloud_xyz, + voxel_downsample_cloud, +) +from dimos.msgs.time import time_from_seconds from dimos.perception.detection.type.detection2d.seg import Detection2DSeg from dimos.perception.detection.type.detection3d.base import Detection3D if TYPE_CHECKING: - from dimos_lcm.sensor_msgs import CameraInfo - from dimos.perception.detection.type.detection2d.imageDetections2D import ImageDetections2D @@ -54,7 +70,7 @@ class Object(Detection3D): size: Vector3 pose: PoseStamped pointcloud: PointCloud2 - camera_transform: Transform | None = None + camera_transform: TransformStamped | None = None mask: np.ndarray[Any, np.dtype[np.uint8]] | None = None detections_count: int = 1 visual_embedding: np.ndarray[Any, np.dtype[np.float32]] | None = None @@ -68,8 +84,8 @@ def __post_init__(self) -> None: def set_center(self, center: Vector3) -> None: """Update the canonical center and its pose representation together.""" - self.center = Vector3(center) - self.pose.position = self.center + self.center = Vector3(x=center.x, y=center.y, z=center.z) + self.pose.pose.position = Point(x=center.x, y=center.y, z=center.z) def update_object(self, other: Object, *, accumulate_pointcloud: bool = True) -> None: """Update this object with data from another detection. @@ -89,7 +105,7 @@ def update_object(self, other: Object, *, accumulate_pointcloud: bool = True) -> and other.camera_transform is not None and self.camera_transform is not None ): - self.pointcloud = self.pointcloud + other.pointcloud + self.pointcloud = concatenate_clouds(self.pointcloud, other.pointcloud) else: self.pointcloud = other.pointcloud @@ -115,20 +131,20 @@ def update_object(self, other: Object, *, accumulate_pointcloud: bool = True) -> def get_oriented_bounding_box(self) -> Any: """Get oriented bounding box of the pointcloud.""" - return self.pointcloud.oriented_bounding_box + return pointcloud_to_open3d(self.pointcloud).get_oriented_bounding_box() def _detection3d_bbox_components(self) -> tuple[Vector3, Quaternion, Vector3]: """Return canonical geometry without refitting the point cloud.""" center = self.center size = self.size - orientation = self.pose.orientation + orientation = self.pose.pose.orientation return ( - Vector3(center.x, center.y, center.z), - Quaternion(orientation.x, orientation.y, orientation.z, orientation.w), + Vector3(x=center.x, y=center.y, z=center.z), + Quaternion(x=orientation.x, y=orientation.y, z=orientation.z, w=orientation.w), Vector3( - max(float(size.x), 1e-3), - max(float(size.y), 1e-3), - max(float(size.z), 1e-3), + x=max(float(size.x), 1e-3), + y=max(float(size.y), 1e-3), + z=max(float(size.z), 1e-3), ), ) @@ -143,7 +159,7 @@ def to_detection3d_msg(self) -> ROSDetection3D: center, orientation, size = self._detection3d_bbox_components() msg = ROSDetection3D() - msg.header = Header(self.ts, self.frame_id) + msg.header = self.pose.header msg.id = self.object_id msg.results = [ ObjectHypothesisWithPose( @@ -153,10 +169,9 @@ def to_detection3d_msg(self) -> ROSDetection3D: ) ) ] - msg.results_length = len(msg.results) msg.bbox = BoundingBox3D( center=Pose( - position=center, + position=Point(x=center.x, y=center.y, z=center.z), orientation=orientation, ), size=size, @@ -179,6 +194,7 @@ def agent_encode(self) -> dict[str, Any]: def to_dict(self) -> dict[str, Any]: """Convert object to dictionary with all relevant data.""" + colors = pointcloud_rgb(self.pointcloud) return { "object_id": self.object_id, "track_id": self.track_id, @@ -188,8 +204,11 @@ def to_dict(self) -> dict[str, Any]: "identity_basis": self.identity_basis, "last_seen_ts": self.last_seen_ts, "mask": self.mask, - "pointcloud": self.pointcloud.as_numpy(), - "image": self.image.as_numpy() if self.image else None, + "pointcloud": ( + pointcloud_xyz(self.pointcloud), + None if colors is None else colors.astype(np.float64) / 255.0, + ), + "image": image_view(self.image) if self.image else None, } @classmethod @@ -199,7 +218,7 @@ def from_2d_to_list( color_image: Image, depth_image: Image, camera_info: CameraInfo, - camera_transform: Transform | None = None, + camera_transform: TransformStamped | None = None, depth_scale: float = 1.0, depth_trunc: float = 10.0, statistical_nb_neighbors: int = 10, @@ -241,16 +260,13 @@ def from_2d_to_list( import cv2 import open3d as o3d # type: ignore[import-untyped] - color_cv = color_image.to_opencv() - if color_cv.ndim == 3 and color_cv.shape[2] == 3: - color_cv = cv2.cvtColor(color_cv, cv2.COLOR_BGR2RGB) - - depth_cv = depth_image.to_opencv() + color_cv = image_to_rgb(color_image) + depth_cv = image_view(depth_image) h, w = depth_cv.shape[:2] # Build Open3D camera intrinsics - fx, fy = camera_info.K[0], camera_info.K[4] - cx, cy = camera_info.K[2], camera_info.K[5] + fx, fy = camera_info.k[0], camera_info.k[4] + cx, cy = camera_info.k[2], camera_info.k[5] intrinsic_o3d = o3d.camera.PinholeCameraIntrinsic(w, h, fx, fy, cx, cy) objects: list[Object] = [] @@ -289,56 +305,56 @@ def from_2d_to_list( ) pcd = o3d.geometry.PointCloud.create_from_rgbd_image(rgbd, intrinsic_o3d) - pc0 = PointCloud2( - pcd, - frame_id=depth_image.frame_id, - ts=depth_image.ts, - ).voxel_downsample(voxel_downsample) - - pcd_filtered, _ = pc0.pointcloud.remove_statistical_outlier( + initial = pointcloud_from_xyz_rgb( + np.asarray(pcd.points), + (np.asarray(pcd.colors) * 255).astype(np.uint8), + header=depth_image.header, + ) + pc0 = voxel_downsample_cloud(initial, voxel_downsample) + pcd_filtered, _ = pointcloud_to_open3d(pc0).remove_statistical_outlier( nb_neighbors=statistical_nb_neighbors, std_ratio=statistical_std_ratio, ) - if len(pcd_filtered.points) < 10: continue - - pc = PointCloud2( - pcd_filtered, - frame_id=depth_image.frame_id, - ts=depth_image.ts, - ) - - # Transform pointcloud to world frame if camera_transform is provided + header = depth_image.header if camera_transform is not None: - pc = pc.transform(camera_transform) - frame_id = camera_transform.frame_id - else: - frame_id = depth_image.frame_id + if camera_transform.child_frame_id != header.frame_id: + raise ValueError("Camera transform source frame does not match depth image") + pcd_filtered.transform(transform_matrix(camera_transform.transform)) + header = Header( + stamp=depth_image.header.stamp, frame_id=camera_transform.header.frame_id + ) + pc = pointcloud_from_xyz_rgb( + np.asarray(pcd_filtered.points), + (np.asarray(pcd_filtered.colors) * 255).astype(np.uint8), + header=header, + ) + frame_id = header.frame_id # Compute bounding box: AABB for stable upright obstacles, OBB for tighter fit if use_aabb: - aabb = pc.pointcloud.get_axis_aligned_bounding_box() + aabb = pcd_filtered.get_axis_aligned_bounding_box() aabb_center = (aabb.min_bound + aabb.max_bound) / 2.0 aabb_extent = aabb.max_bound - aabb.min_bound - center = Vector3(aabb_center[0], aabb_center[1], aabb_center[2]) + center = Vector3(x=aabb_center[0], y=aabb_center[1], z=aabb_center[2]) sx, sy, sz = float(aabb_extent[0]), float(aabb_extent[1]), float(aabb_extent[2]) - orientation = Quaternion(0.0, 0.0, 0.0, 1.0) + orientation = Quaternion(w=1.0) else: - obb = pc.pointcloud.get_oriented_bounding_box() - center = Vector3(obb.center[0], obb.center[1], obb.center[2]) + obb = pcd_filtered.get_oriented_bounding_box() + center = Vector3(x=obb.center[0], y=obb.center[1], z=obb.center[2]) sx, sy, sz = float(obb.extent[0]), float(obb.extent[1]), float(obb.extent[2]) - orientation = Quaternion.from_rotation_matrix(obb.R) + orientation = quaternion_from_matrix(np.asarray(obb.R)) if max_obstacle_width > 0: sx = min(sx, max_obstacle_width) sy = min(sy, max_obstacle_width) - size = Vector3(sx, sy, sz) + size = Vector3(x=sx, y=sy, z=sz) pose = PoseStamped( - ts=det.ts, - frame_id=frame_id, - position=center, - orientation=orientation, + header=Header(stamp=time_from_seconds(det.ts), frame_id=frame_id), + pose=Pose( + position=Point(x=center.x, y=center.y, z=center.z), orientation=orientation + ), ) # Skip objects too far from origin (background detections) @@ -380,52 +396,38 @@ def aggregate_pointclouds(objects: list[Object]) -> PointCloud2: Returns: Combined PointCloud2 with all points colored by object (empty if no points). """ - import open3d as o3d # type: ignore[import-untyped] - - if not objects: - return PointCloud2(pointcloud=o3d.geometry.PointCloud(), frame_id="", ts=0.0) - + header = ( + Header() + if not objects + else Header(frame_id=objects[0].frame_id, stamp=objects[0].pointcloud.header.stamp) + ) all_points = [] all_colors = [] - for obj in objects: - points, colors = obj.pointcloud.as_numpy() + if obj.frame_id != header.frame_id: + raise ValueError("Cannot aggregate object clouds in different frames") + points = pointcloud_xyz(obj.pointcloud) if len(points) == 0: continue - + colors = pointcloud_rgb(obj.pointcloud) try: seed = int(obj.object_id, 16) except (ValueError, TypeError): seed = abs(hash(obj.object_id)) rng = np.random.default_rng(abs(seed)) track_color = rng.integers(50, 255, 3) / 255.0 - - if colors is not None: - blended = np.clip(0.6 * colors + 0.4 * track_color, 0.0, 1.0) - else: - blended = np.tile(track_color, (len(points), 1)) - + blended = ( + np.clip(0.6 * colors.astype(np.float64) / 255.0 + 0.4 * track_color, 0.0, 1.0) + if colors is not None + else np.tile(track_color, (len(points), 1)) + ) all_points.append(points) all_colors.append(blended) - if not all_points: - return PointCloud2( - pointcloud=o3d.geometry.PointCloud(), frame_id=objects[0].frame_id, ts=objects[0].ts - ) - - combined_points = np.vstack(all_points) - combined_colors = np.vstack(all_colors) - - pc = PointCloud2.from_numpy( - combined_points, - frame_id=objects[0].frame_id, - timestamp=objects[0].ts, + return pointcloud_from_xyz(np.empty((0, 3), dtype=np.float32), header=header) + return pointcloud_from_xyz_rgb( + np.vstack(all_points), (np.vstack(all_colors) * 255).astype(np.uint8), header=header ) - pcd = pc.pointcloud - pcd.colors = o3d.utility.Vector3dVector(combined_colors) - pc.pointcloud = pcd - - return pc def to_detection3d_array( @@ -452,7 +454,6 @@ def to_detection3d_array( if resolved_ts is None: resolved_ts = objects[0].ts if objects else 0.0 return Detection3DArray( - detections_length=len(detections), - header=Header(resolved_ts, resolved_frame_id), + header=Header(stamp=time_from_seconds(resolved_ts), frame_id=resolved_frame_id), detections=detections, ) diff --git a/dimos/perception/experimental/objectDB.py b/dimos/perception/experimental/objectDB.py index d2d8e7e1bea..2dda55d4924 100644 --- a/dimos/perception/experimental/objectDB.py +++ b/dimos/perception/experimental/objectDB.py @@ -14,15 +14,19 @@ from __future__ import annotations +import math import threading import time from typing import TYPE_CHECKING, Any -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 +import numpy as np + +from dimos.msgs.pointcloud import pointcloud_from_xyz from dimos.utils.logging_config import setup_logger if TYPE_CHECKING: - from dimos.msgs.geometry_msgs.Vector3 import Vector3 + from dimos_generated.geometry_msgs.msg import Vector3 + from dimos.perception.experimental.object import Object logger = setup_logger() @@ -176,20 +180,19 @@ def find_nearest( if not candidates: return None - return min(candidates, key=lambda obj: position.distance(obj.center)) + return min( + candidates, + key=lambda obj: math.dist( + (position.x, position.y, position.z), (obj.center.x, obj.center.y, obj.center.z) + ), + ) def clear(self) -> None: """Clear all objects from the database.""" - import open3d as o3d # type: ignore[import-untyped] - with self._lock: - # Drop Open3D pointcloud references before clearing to reduce shutdown warnings. + # Release large generated point buffers, retaining metadata on held objects. for obj in list(self._pending_objects.values()) + list(self._objects.values()): - obj.pointcloud = PointCloud2( - pointcloud=o3d.geometry.PointCloud(), - frame_id=obj.pointcloud.frame_id, - ts=obj.pointcloud.ts, - ) + obj.pointcloud = pointcloud_from_xyz(np.empty((0, 3)), header=obj.pointcloud.header) self._pending_objects.clear() self._objects.clear() self._track_id_map.clear() @@ -283,13 +286,22 @@ def _match_by_distance(self, obj: Object) -> Object | None: candidates = [ o for o in all_objects - if o.center is not None and obj.center.distance(o.center) < self._distance_threshold + if o.center is not None + and math.dist( + (obj.center.x, obj.center.y, obj.center.z), (o.center.x, o.center.y, o.center.z) + ) + < self._distance_threshold ] if not candidates: return None - return min(candidates, key=lambda o: obj.center.distance(o.center)) + return min( + candidates, + key=lambda o: math.dist( + (obj.center.x, obj.center.y, obj.center.z), (o.center.x, o.center.y, o.center.z) + ), + ) def _prune_stale_pending(self, now: float) -> None: if self._pending_ttl_s <= 0: diff --git a/dimos/perception/experimental/object_scene_registration.py b/dimos/perception/experimental/object_scene_registration.py index 5e43cb490d0..82c6c3a7337 100644 --- a/dimos/perception/experimental/object_scene_registration.py +++ b/dimos/perception/experimental/object_scene_registration.py @@ -19,8 +19,13 @@ import time from typing import Any, Literal +from dimos_generated.geometry_msgs.msg import TransformStamped +from dimos_generated.sensor_msgs.msg import CameraInfo, Image, PointCloud2 +from dimos_generated.tf2_msgs.msg import TFMessage +from dimos_generated.vision_msgs.msg import Detection2DArray, Detection3DArray import numpy as np from numpy.typing import NDArray +from reactivex import operators as ops from dimos.agents.annotation import skill from dimos.core.core import rpc @@ -28,14 +33,9 @@ from dimos.core.stream import In, Out from dimos.models.segmentation.edge_tam import BoxPromptImageSegmenter from dimos.models.segmentation.yoloe import YoloeBoxSegmenter -from dimos.msgs.geometry_msgs.Transform import Transform -from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo -from dimos.msgs.sensor_msgs.Image import Image, ImageFormat -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 -from dimos.msgs.std_msgs.Header import Header -from dimos.msgs.tf2_msgs.TFMessage import TFMessage -from dimos.msgs.vision_msgs.Detection2DArray import Detection2DArray -from dimos.msgs.vision_msgs.Detection3DArray import Detection3DArray +from dimos.msgs.image import image_from_array, image_view +from dimos.msgs.pointcloud import pointcloud_from_xyz, transform_cloud +from dimos.msgs.time import to_seconds from dimos.perception.detection.detectors.yoloe import Yoloe2DDetector, YoloePromptMode from dimos.perception.detection.type.detection2d.imageDetections2D import ImageDetections2D from dimos.perception.experimental.object import ( @@ -45,13 +45,23 @@ to_detection3d_array, ) from dimos.perception.experimental.objectDB import ObjectDB -from dimos.types.timestamped import align_timestamped +from dimos.types.timestamped import TimestampedData, align_timestamped from dimos.utils.logging_config import setup_logger from dimos.utils.reactive import backpressure logger = setup_logger() +def _timed_image(image: Image) -> TimestampedData[Image]: + return TimestampedData(image, to_seconds(image.header.stamp)) + + +def _aligned_images( + pair: tuple[TimestampedData[Image], TimestampedData[Image]], +) -> tuple[Image, Image]: + return pair[0].value, pair[1].value + + class ObjectSceneRegistrationConfig(ModuleConfig): target_frame: str = "base_link" prompt_mode: YoloePromptMode = YoloePromptMode.LRPC @@ -89,7 +99,7 @@ class ObjectSceneRegistrationModule(Module): _processing_lock: threading.RLock # A tuple assignment/read is atomic, so depth and its transform cannot be # observed from different frames by get_full_scene_pointcloud(). - _latest_scene_snapshot: tuple[Image, Transform | None] | None = None + _latest_scene_snapshot: tuple[Image, TransformStamped | None] | None = None config: ObjectSceneRegistrationConfig @@ -155,11 +165,11 @@ def start(self) -> None: self.camera_info.subscribe(lambda msg: setattr(self, "_camera_info", msg)) aligned_frames = align_timestamped( - self.color_image.observable(), - self.depth_image.observable(), + self.color_image.observable().pipe(ops.map(_timed_image)), + self.depth_image.observable().pipe(ops.map(_timed_image)), buffer_size=2.0, match_tolerance=0.1, - ) + ).pipe(ops.map(_aligned_images)) backpressure(aligned_frames).subscribe(self._on_aligned_frames) def _create_segmenter(self) -> BoxPromptImageSegmenter | None: @@ -232,7 +242,7 @@ def scan_scene(self, text: list[str] | None = None) -> Detection3DArray: return to_detection3d_array( objects, frame_id=self._target_frame, - ts=frames[0].ts, + ts=to_seconds(frames[0].header.stamp), ) def _scan_scene_objects(self, frames: tuple[Image, Image] | None = None) -> list[DetObject]: @@ -275,7 +285,7 @@ def get_object_pointcloud_by_object_id(self, object_id: str) -> PointCloud2 | No logger.warning(f"No object found with object_id='{object_id}'") return None pc = obj.pointcloud - num_points = len(pc.pointcloud.points) if pc else 0 + num_points = pc.width * pc.height logger.info(f"Found object '{object_id}' ({obj.name}) with {num_points} points") return pc @@ -313,7 +323,7 @@ def get_full_scene_pointcloud( return None depth_image, camera_transform = scene_snapshot - depth_cv = depth_image.to_opencv() + depth_cv = image_view(depth_image) h, w = depth_cv.shape[:2] # Zero out excluded object's depth @@ -324,8 +334,8 @@ def get_full_scene_pointcloud( depth_cv[exclude_mask > 0] = 0 # Build pointcloud from depth - fx, fy = self._camera_info.K[0], self._camera_info.K[4] - cx, cy = self._camera_info.K[2], self._camera_info.K[5] + fx, fy = self._camera_info.k[0], self._camera_info.k[4] + cx, cy = self._camera_info.k[2], self._camera_info.k[5] intrinsic = o3d.camera.PinholeCameraIntrinsic(w, h, fx, fy, cx, cy) depth_o3d = o3d.geometry.Image(depth_cv.astype(np.float32)) @@ -338,14 +348,9 @@ def get_full_scene_pointcloud( pcd = pcd.voxel_down_sample(voxel_size) - pc = PointCloud2( - pcd, - frame_id=depth_image.frame_id, - ts=depth_image.ts, - ) - + pc = pointcloud_from_xyz(np.asarray(pcd.points), header=depth_image.header) if camera_transform is not None: - pc = pc.transform(camera_transform) + pc = transform_cloud(pc, camera_transform) return pc @@ -412,21 +417,19 @@ def _process_images(self, color_msg: Image, depth_msg: Image) -> list[DetObject] color_image = color_msg # Convert depth to meters (float32) - depth_cv = depth_msg.to_opencv() - if depth_msg.format == ImageFormat.DEPTH16: + depth_cv = image_view(depth_msg) + if depth_msg.encoding == "16UC1": depth_cv = depth_cv.astype(np.float32) / 1000.0 elif depth_cv.dtype != np.float32: depth_cv = depth_cv.astype(np.float32) - depth_image = Image( - data=depth_cv, format=ImageFormat.DEPTH, frame_id=depth_msg.frame_id, ts=depth_msg.ts - ) + depth_image = image_from_array(depth_cv, encoding="32FC1", header=depth_msg.header) camera_transform = None - if self._target_frame != color_image.frame_id: + if self._target_frame != color_image.header.frame_id: camera_transform = self.tfbuffer.get( self._target_frame, - color_image.frame_id, - color_image.ts, + color_image.header.frame_id, + to_seconds(color_image.header.stamp), 0.1, forward_tolerance=0.2, ) @@ -457,8 +460,7 @@ def _process_images(self, color_msg: Image, depth_msg: Image) -> list[DetObject] detections_2d = self._segmenter.segment(detections_2d) detections_2d_msg = Detection2DArray( - detections_length=len(detections_2d.detections), - header=Header(color_image.ts, color_image.frame_id or ""), + header=color_image.header, detections=[det.to_ros_detection2d() for det in detections_2d.detections], ) self.detections_2d.publish(detections_2d_msg) @@ -473,13 +475,13 @@ def _process_3d_detections( detections_2d: ImageDetections2D[Any], color_image: Image, depth_image: Image, - camera_transform: Transform | None, + camera_transform: TransformStamped | None, ) -> list[DetObject]: """Convert 2D detections to 3D and publish.""" if self._camera_info is None: return [] - if self._target_frame != color_image.frame_id and camera_transform is None: + if self._target_frame != color_image.header.frame_id and camera_transform is None: logger.warning("Failed to lookup transform from camera frame to target frame") return [] diff --git a/dimos/perception/experimental/object_scene_registration_spec.py b/dimos/perception/experimental/object_scene_registration_spec.py index 5f0cbbf8e1b..f8869c87364 100644 --- a/dimos/perception/experimental/object_scene_registration_spec.py +++ b/dimos/perception/experimental/object_scene_registration_spec.py @@ -14,8 +14,9 @@ from typing import Protocol -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 -from dimos.msgs.vision_msgs.Detection3DArray import Detection3DArray +from dimos_generated.sensor_msgs.msg import PointCloud2 +from dimos_generated.vision_msgs.msg import Detection3DArray + from dimos.spec.utils import Spec diff --git a/dimos/perception/experimental/object_tracker.py b/dimos/perception/experimental/object_tracker.py index 2331dbfd8ca..045619a24dd 100644 --- a/dimos/perception/experimental/object_tracker.py +++ b/dimos/perception/experimental/object_tracker.py @@ -16,31 +16,38 @@ import time from typing import Any -# Import LCM messages -from dimos_lcm.vision_msgs import ( +from dimos_generated.geometry_msgs.msg import ( + Point, + Pose, + Quaternion, + Transform, + TransformStamped, + Vector3, +) +from dimos_generated.sensor_msgs.msg import CameraInfo, Image +from dimos_generated.std_msgs.msg import Header +from dimos_generated.tf2_msgs.msg import TFMessage + +# Generated detection values +from dimos_generated.vision_msgs.msg import ( Detection2D, + Detection2DArray, Detection3D, + Detection3DArray, ObjectHypothesisWithPose, ) import numpy as np from numpy.typing import NDArray +from reactivex import operators as ops from reactivex.disposable import Disposable from dimos.constants import DEFAULT_THREAD_JOIN_TIMEOUT from dimos.core.core import rpc from dimos.core.module import Module, ModuleConfig from dimos.core.stream import In, Out -from dimos.msgs.geometry_msgs.Pose import Pose -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Transform import Transform -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo -from dimos.msgs.sensor_msgs.Image import Image, ImageFormat -from dimos.msgs.std_msgs.Header import Header -from dimos.msgs.tf2_msgs.TFMessage import TFMessage -from dimos.msgs.vision_msgs.Detection2DArray import Detection2DArray -from dimos.msgs.vision_msgs.Detection3DArray import Detection3DArray -from dimos.types.timestamped import align_timestamped +from dimos.msgs.image import image_from_array, image_to_rgb, image_view +from dimos.msgs.time import time_from_seconds, to_seconds +from dimos.types.timestamped import TimestampedData, align_timestamped from dimos.utils.logging_config import setup_logger from dimos.utils.transform_utils import ( euler_to_quaternion, @@ -48,6 +55,11 @@ yaw_towards_point, ) + +def _timed_image(image: Image) -> TimestampedData[Image]: + return TimestampedData(image, to_seconds(image.header.stamp)) + + logger = setup_logger() @@ -128,20 +140,20 @@ def start(self) -> None: # Subscribe to aligned rgb and depth streams def on_aligned_frames(frames_tuple) -> None: # type: ignore[no-untyped-def] - rgb_msg, depth_msg = frames_tuple + rgb_msg, depth_msg = (item.value for item in frames_tuple) with self._frame_lock: - self._latest_rgb_frame = rgb_msg.data + self._latest_rgb_frame = image_to_rgb(rgb_msg) - depth_data = depth_msg.data + depth_data = image_view(depth_msg) # Convert from millimeters to meters if depth is DEPTH16 format - if depth_msg.format == ImageFormat.DEPTH16: + if depth_msg.encoding in ("16UC1", "mono16"): depth_data = depth_data.astype(np.float32) / 1000.0 self._latest_depth_frame = depth_data # Create aligned observable for RGB and depth aligned_frames = align_timestamped( - self.color_image.observable(), - self.depth.observable(), + self.color_image.pure_observable().pipe(ops.map(_timed_image)), + self.depth.pure_observable().pipe(ops.map(_timed_image)), buffer_size=2.0, # 2 second buffer match_tolerance=0.5, # 500ms tolerance ) @@ -154,10 +166,10 @@ def on_camera_info(camera_info_msg: CameraInfo) -> None: # Extract intrinsics from camera info K matrix # K is a 3x3 matrix in row-major order: [fx, 0, cx, 0, fy, cy, 0, 0, 1] self.camera_intrinsics = [ # type: ignore[assignment] - camera_info_msg.K[0], - camera_info_msg.K[4], - camera_info_msg.K[2], - camera_info_msg.K[5], + camera_info_msg.k[0], + camera_info_msg.k[4], + camera_info_msg.k[2], + camera_info_msg.k[5], ] unsub = self.camera_info.subscribe(on_camera_info) # type: ignore[assignment] @@ -309,8 +321,8 @@ def _reset_tracking_state(self) -> None: self.tracking_frame_count = 0 # Reset frame counter # Publish empty detections to clear any visualizations - empty_2d = Detection2DArray(detections_length=0, header=Header(), detections=[]) - empty_3d = Detection3DArray(detections_length=0, header=Header(), detections=[]) + empty_2d = Detection2DArray(header=Header(), detections=[]) + empty_3d = Detection3DArray(header=Header(), detections=[]) self._latest_detection2d = empty_2d self._latest_detection3d = empty_3d self._detection_event.clear() @@ -409,9 +421,9 @@ def _process_tracking(self) -> None: return # Create detections if tracking succeeded - header = Header(self.frame_id) - detection2darray = Detection2DArray(detections_length=0, header=header, detections=[]) - detection3darray = Detection3DArray(detections_length=0, header=header, detections=[]) + header = Header(stamp=time_from_seconds(time.time()), frame_id=self.config.frame_id) + detection2darray = Detection2DArray(header=header, detections=[]) + detection3darray = Detection3DArray(header=header, detections=[]) if final_success and current_bbox_x1y1x2y2 is not None: x1, y1, x2, y2 = current_bbox_x1y1x2y2 @@ -423,7 +435,6 @@ def _process_tracking(self) -> None: # Create Detection2D detection_2d = Detection2D() detection_2d.id = "0" - detection_2d.results_length = 1 detection_2d.header = header # Create hypothesis @@ -440,7 +451,6 @@ def _process_tracking(self) -> None: detection_2d.bbox.size_y = height detection2darray = Detection2DArray() - detection2darray.detections_length = 1 detection2darray.header = header detection2darray.detections = [detection_2d] @@ -462,15 +472,15 @@ def _process_tracking(self) -> None: # Create pose in optical frame optical_pose = Pose() - optical_pose.position = Vector3(x_optical, y_optical, z_optical) - optical_pose.orientation = Quaternion(0.0, 0.0, 0.0, 1.0) # Identity for now + optical_pose.position = Point(x=x_optical, y=y_optical, z=z_optical) + optical_pose.orientation = Quaternion(w=1.0) # Identity for now # Convert to robot frame robot_pose = optical_to_robot_frame(optical_pose) # Calculate orientation: object facing towards camera (origin) yaw = yaw_towards_point(robot_pose.position) - euler = Vector3(0.0, 0.0, yaw) # Only yaw, no roll/pitch + euler = Vector3(z=yaw) # Only yaw, no roll/pitch robot_pose.orientation = euler_to_quaternion(euler) # Estimate object size in meters @@ -481,7 +491,6 @@ def _process_tracking(self) -> None: # Create Detection3D detection_3d = Detection3D() detection_3d.id = "0" - detection_3d.results_length = 1 detection_3d.header = header # Reuse hypothesis from 2D @@ -491,30 +500,34 @@ def _process_tracking(self) -> None: detection_3d.bbox.center = Pose() detection_3d.bbox.center.position = robot_pose.position detection_3d.bbox.center.orientation = robot_pose.orientation - detection_3d.bbox.size = Vector3(size_x, size_y, size_z) + detection_3d.bbox.size = Vector3(x=size_x, y=size_y, z=size_z) detection3darray = Detection3DArray() - detection3darray.detections_length = 1 detection3darray.header = header detection3darray.detections = [detection_3d] # Publish transform for tracked object # The optical pose is in camera optical frame, so publish it relative to the camera frame - tracked_object_tf = Transform( - translation=robot_pose.position, - rotation=robot_pose.orientation, - frame_id=self.frame_id, # Use configured camera frame + tracked_object_tf = TransformStamped( + header=header, child_frame_id="tracked_object", - ts=header.ts, + transform=Transform( + translation=Vector3( + x=robot_pose.position.x, + y=robot_pose.position.y, + z=robot_pose.position.z, + ), + rotation=robot_pose.orientation, + ), ) - self.tf.publish(TFMessage(tracked_object_tf)) + self.tf.publish(TFMessage(transforms=[tracked_object_tf])) # Store latest detections for RPC access self._latest_detection2d = detection2darray self._latest_detection3d = detection3darray # Signal that new detections are available - if detection2darray.detections_length > 0 or detection3darray.detections_length > 0: + if bool(detection2darray.detections) or bool(detection3darray.detections): self._detection_event.set() # Publish detections @@ -524,12 +537,8 @@ def _process_tracking(self) -> None: # Create and publish visualization if tracking is active if self.tracking_initialized: # Convert single detection to list for visualization - detections_3d = ( - detection3darray.detections if detection3darray.detections_length > 0 else [] - ) - detections_2d = ( - detection2darray.detections if detection2darray.detections_length > 0 else [] - ) + detections_3d = detection3darray.detections if bool(detection3darray.detections) else [] + detections_2d = detection2darray.detections if bool(detection2darray.detections) else [] if detections_3d and detections_2d: # Extract 2D bbox for visualization @@ -555,7 +564,9 @@ def _process_tracking(self) -> None: viz_image = self._draw_reid_matches(viz_image) # Convert to Image message and publish - viz_msg = Image.from_numpy(viz_image) + viz_msg = image_from_array( + viz_image, encoding="rgb8", header=self._latest_detection2d.header + ) self.tracked_overlay.publish(viz_msg) def _draw_reid_matches(self, image: NDArray[np.uint8]) -> NDArray[np.uint8]: diff --git a/dimos/perception/experimental/object_tracker_2d.py b/dimos/perception/experimental/object_tracker_2d.py index 385d0ff98d8..abb4b413729 100644 --- a/dimos/perception/experimental/object_tracker_2d.py +++ b/dimos/perception/experimental/object_tracker_2d.py @@ -17,10 +17,14 @@ import time from typing import Any -# Import LCM messages -from dimos_lcm.vision_msgs import ( +from dimos_generated.sensor_msgs.msg import Image +from dimos_generated.std_msgs.msg import Header + +# Generated detection values +from dimos_generated.vision_msgs.msg import ( BoundingBox2D, Detection2D, + Detection2DArray, ObjectHypothesis, ObjectHypothesisWithPose, Point2D, @@ -34,9 +38,8 @@ from dimos.core.core import rpc from dimos.core.module import Module, ModuleConfig from dimos.core.stream import In, Out -from dimos.msgs.sensor_msgs.Image import Image, ImageFormat -from dimos.msgs.std_msgs.Header import Header -from dimos.msgs.vision_msgs.Detection2DArray import Detection2DArray +from dimos.msgs.image import image_from_array, image_to_rgb +from dimos.msgs.time import time_from_seconds from dimos.utils.logging_config import setup_logger logger = setup_logger(level=logging.INFO) @@ -91,7 +94,7 @@ def start(self) -> None: def on_frame(frame_msg: Image) -> None: arrival_time = time.perf_counter() with self._frame_lock: - self._latest_rgb_frame = frame_msg.data + self._latest_rgb_frame = image_to_rgb(frame_msg) self._frame_arrival_time = arrival_time unsub = self.color_image.subscribe(on_frame) @@ -176,7 +179,8 @@ def _reset_tracking_state(self) -> None: # Publish empty detection empty_2d = Detection2DArray( - detections_length=0, header=Header(time.time(), self.frame_id), detections=[] + header=Header(stamp=time_from_seconds(time.time()), frame_id=self.config.frame_id), + detections=[], ) self._latest_detection2d = empty_2d self.detection2darray.publish(empty_2d) @@ -260,12 +264,11 @@ def _process_tracking(self) -> None: height = float(y2 - y1) # Create 2D detection header - header = Header(time.time(), self.frame_id) + header = Header(stamp=time_from_seconds(time.time()), frame_id=self.config.frame_id) # Create Detection2D with all fields in constructors detection_2d = Detection2D( id="0", - results_length=1, header=header, bbox=BoundingBox2D( center=Pose2D(position=Point2D(x=center_x, y=center_y), theta=0.0), @@ -279,9 +282,7 @@ def _process_tracking(self) -> None: ], ) - detection2darray = Detection2DArray( - detections_length=1, header=header, detections=[detection_2d] - ) + detection2darray = Detection2DArray(header=header, detections=[detection_2d]) # Store and publish self._latest_detection2d = detection2darray @@ -290,7 +291,7 @@ def _process_tracking(self) -> None: # Create visualization viz_image = self._draw_visualization(frame, current_bbox_x1y1x2y2) viz_copy = viz_image.copy() # Force copy needed to prevent frame reuse - viz_msg = Image.from_numpy(viz_copy, format=ImageFormat.RGB) + viz_msg = image_from_array(viz_copy, encoding="rgb8", header=header) self.tracked_overlay.publish(viz_msg) def _draw_visualization(self, image: NDArray[np.uint8], bbox: list[int]) -> NDArray[np.uint8]: diff --git a/dimos/perception/experimental/object_tracker_3d.py b/dimos/perception/experimental/object_tracker_3d.py index f2f5a116b10..5377f259874 100644 --- a/dimos/perception/experimental/object_tracker_3d.py +++ b/dimos/perception/experimental/object_tracker_3d.py @@ -13,27 +13,33 @@ # limitations under the License. -# Import LCM messages -from dimos_lcm.sensor_msgs import CameraInfo -from dimos_lcm.vision_msgs import ( +# Generated detection values +from dimos_generated.geometry_msgs.msg import ( + Point, + Pose, + Quaternion, + Transform, + TransformStamped, + Vector3, +) +from dimos_generated.sensor_msgs.msg import CameraInfo, Image +from dimos_generated.std_msgs.msg import Header +from dimos_generated.tf2_msgs.msg import TFMessage +from dimos_generated.vision_msgs.msg import ( + Detection2DArray, Detection3D, + Detection3DArray, ObjectHypothesisWithPose, ) import numpy as np +from reactivex import operators as ops from dimos.core.core import rpc from dimos.core.stream import In, Out -from dimos.msgs.geometry_msgs.Pose import Pose -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Transform import Transform -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.msgs.sensor_msgs.Image import Image, ImageFormat -from dimos.msgs.std_msgs.Header import Header -from dimos.msgs.tf2_msgs.TFMessage import TFMessage -from dimos.msgs.vision_msgs.Detection2DArray import Detection2DArray -from dimos.msgs.vision_msgs.Detection3DArray import Detection3DArray +from dimos.msgs.image import image_from_array, image_to_rgb, image_view +from dimos.msgs.time import to_seconds from dimos.perception.experimental.object_tracker_2d import ObjectTracker2D -from dimos.types.timestamped import align_timestamped +from dimos.types.timestamped import TimestampedData, align_timestamped from dimos.utils.logging_config import setup_logger from dimos.utils.transform_utils import ( euler_to_quaternion, @@ -41,6 +47,11 @@ yaw_towards_point, ) + +def _timed_image(image: Image) -> TimestampedData[Image]: + return TimestampedData(image, to_seconds(image.header.stamp)) + + logger = setup_logger() @@ -78,20 +89,20 @@ def start(self) -> None: # Subscribe to aligned RGB and depth streams def on_aligned_frames(frames_tuple) -> None: # type: ignore[no-untyped-def] - rgb_msg, depth_msg = frames_tuple + rgb_msg, depth_msg = (item.value for item in frames_tuple) with self._frame_lock: - self._latest_rgb_frame = rgb_msg.data + self._latest_rgb_frame = image_to_rgb(rgb_msg) - depth_data = depth_msg.data + depth_data = image_view(depth_msg) # Convert from millimeters to meters if depth is DEPTH16 format - if depth_msg.format == ImageFormat.DEPTH16: + if depth_msg.encoding in ("16UC1", "mono16"): depth_data = depth_data.astype(np.float32) / 1000.0 self._latest_depth_frame = depth_data # Create aligned observable for RGB and depth aligned_frames = align_timestamped( - self.color_image.observable(), - self.depth.observable(), + self.color_image.pure_observable().pipe(ops.map(_timed_image)), + self.depth.pure_observable().pipe(ops.map(_timed_image)), buffer_size=2.0, # 2 second buffer match_tolerance=0.5, # 500ms tolerance ) @@ -103,10 +114,10 @@ def on_camera_info(camera_info_msg: CameraInfo) -> None: self._latest_camera_info = camera_info_msg # Extract intrinsics: K is [fx, 0, cx, 0, fy, cy, 0, 0, 1] self.camera_intrinsics = [ # type: ignore[assignment] - camera_info_msg.K[0], - camera_info_msg.K[4], - camera_info_msg.K[2], - camera_info_msg.K[5], + camera_info_msg.k[0], + camera_info_msg.k[4], + camera_info_msg.k[2], + camera_info_msg.k[5], ] self.camera_info.subscribe(on_camera_info) @@ -127,7 +138,7 @@ def _process_tracking(self) -> None: # Enhance with 3D if we have depth and a valid 2D detection if ( self._latest_detection2d - and self._latest_detection2d.detections_length > 0 + and bool(self._latest_detection2d.detections) and self._latest_depth_frame is not None and self.camera_intrinsics is not None ): @@ -161,12 +172,14 @@ def _process_tracking(self) -> None: if self.last_good_matches and self.last_roi_kps and self.last_roi_bbox: viz_image = self._draw_reid_overlay(viz_image) - viz_msg = Image.from_numpy(viz_image) + viz_msg = image_from_array( + viz_image, encoding="rgb8", header=self._latest_detection2d.header + ) self.tracked_overlay.publish(viz_msg) def _create_detection3d_from_2d(self, detection2d: Detection2DArray) -> Detection3DArray | None: """Create 3D detection from 2D detection using depth.""" - if detection2d.detections_length == 0: + if not detection2d.detections: return None det_2d = detection2d.detections[0] @@ -198,15 +211,15 @@ def _create_detection3d_from_2d(self, detection2d: Detection2DArray) -> Detectio # Create pose in optical frame optical_pose = Pose() - optical_pose.position = Vector3(x_optical, y_optical, z_optical) - optical_pose.orientation = Quaternion(0.0, 0.0, 0.0, 1.0) + optical_pose.position = Point(x=x_optical, y=y_optical, z=z_optical) + optical_pose.orientation = Quaternion(w=1.0) # Convert to robot frame robot_pose = optical_to_robot_frame(optical_pose) # Calculate orientation: object facing towards camera yaw = yaw_towards_point(robot_pose.position) - euler = Vector3(0.0, 0.0, yaw) + euler = Vector3(z=yaw) robot_pose.orientation = euler_to_quaternion(euler) # Estimate object size in meters @@ -215,10 +228,9 @@ def _create_detection3d_from_2d(self, detection2d: Detection2DArray) -> Detectio size_z = 0.1 # Default depth size # Create Detection3D - header = Header(self.frame_id) + header = Header(stamp=detection2d.header.stamp, frame_id=self.config.frame_id) detection_3d = Detection3D() detection_3d.id = "0" - detection_3d.results_length = 1 detection_3d.header = header # Create hypothesis @@ -231,22 +243,24 @@ def _create_detection3d_from_2d(self, detection2d: Detection2DArray) -> Detectio detection_3d.bbox.center = Pose() detection_3d.bbox.center.position = robot_pose.position detection_3d.bbox.center.orientation = robot_pose.orientation - detection_3d.bbox.size = Vector3(size_x, size_y, size_z) + detection_3d.bbox.size = Vector3(x=size_x, y=size_y, z=size_z) detection3darray = Detection3DArray() - detection3darray.detections_length = 1 detection3darray.header = header detection3darray.detections = [detection_3d] # Publish TF for tracked object - tracked_object_tf = Transform( - translation=robot_pose.position, - rotation=robot_pose.orientation, - frame_id=self.frame_id, + tracked_object_tf = TransformStamped( + header=header, child_frame_id="tracked_object", - ts=header.ts, + transform=Transform( + translation=Vector3( + x=robot_pose.position.x, y=robot_pose.position.y, z=robot_pose.position.z + ), + rotation=robot_pose.orientation, + ), ) - self.tf.publish(TFMessage(tracked_object_tf)) + self.tf.publish(TFMessage(transforms=[tracked_object_tf])) return detection3darray diff --git a/dimos/perception/experimental/perceive_loop_skill.py b/dimos/perception/experimental/perceive_loop_skill.py index 4f1c6b1bf4c..1b0e0ab2d50 100644 --- a/dimos/perception/experimental/perceive_loop_skill.py +++ b/dimos/perception/experimental/perceive_loop_skill.py @@ -14,21 +14,24 @@ from __future__ import annotations +import base64 from datetime import datetime, timezone import json import os from threading import RLock from typing import TYPE_CHECKING, Any +from dimos_generated.sensor_msgs.msg import Image + from dimos.agents.agent_spec import AgentSpec from dimos.agents.annotation import skill from dimos.core.core import rpc from dimos.core.module import Module from dimos.core.stream import In from dimos.models.vl.create import create -from dimos.msgs.sensor_msgs.Image import Image, sharpness_window +from dimos.msgs.image import image_sharpness, image_to_bgr, image_to_jpeg from dimos.utils.logging_config import setup_logger -from dimos.utils.reactive import backpressure +from dimos.utils.reactive import backpressure, quality_barrier if TYPE_CHECKING: from reactivex.abc import DisposableBase @@ -109,7 +112,9 @@ def look_out_for( ) sharpest = backpressure( - sharpness_window(1.0 / self._period, self.color_image.pure_observable()) + self.color_image.pure_observable().pipe( + quality_barrier(image_sharpness, 1.0 / self._period) + ) ) self._vl_model.start() self._model_started = True @@ -176,7 +181,7 @@ def _on_image(self, image: Image) -> None: continuation_context: dict[str, Any] = { "bbox": list(best.bbox), "label": best.name, - "image": image.to_base64(quality=70), + "image": base64.b64encode(image_to_jpeg(image, quality=70)).decode("ascii"), } logger.info( "Lookout matched, dispatching continuation", @@ -203,7 +208,7 @@ def _write_debug_image(image: Image, detections: ImageDetections2D[Detection2DBB import cv2 try: - debug_img = image.to_opencv().copy() + debug_img = image_to_bgr(image) for det in detections.detections: x1, y1, x2, y2 = (int(v) for v in det.bbox) cv2.rectangle(debug_img, (x1, y1), (x2, y2), (0, 255, 0), 2) diff --git a/dimos/perception/experimental/spatial_perception.py b/dimos/perception/experimental/spatial_perception.py index a34745a765a..63aa2f29b48 100644 --- a/dimos/perception/experimental/spatial_perception.py +++ b/dimos/perception/experimental/spatial_perception.py @@ -19,9 +19,12 @@ from datetime import datetime import os import time -from typing import TYPE_CHECKING, Any +from typing import Any import uuid +from dimos_generated.geometry_msgs.msg import Point +from dimos_generated.sensor_msgs.msg import Image +from dimos_generated.tf2_msgs.msg import TFMessage import numpy as np from reactivex import Observable, interval, operators as ops from reactivex.disposable import Disposable @@ -30,17 +33,14 @@ from dimos.core.core import rpc from dimos.core.module import Module, ModuleConfig from dimos.core.stream import In -from dimos.msgs.sensor_msgs.Image import Image -from dimos.msgs.tf2_msgs.TFMessage import TFMessage +from dimos.msgs.geometry import pose_from_transform, quaternion_euler +from dimos.msgs.image import image_to_bgr from dimos.perception.experimental.image_embedding import ImageEmbeddingProvider from dimos.perception.experimental.spatial_vector_db import SpatialVectorDB from dimos.perception.experimental.visual_memory import VisualMemory from dimos.types.robot_location import RobotLocation from dimos.utils.logging_config import setup_logger -if TYPE_CHECKING: - from dimos.msgs.geometry_msgs.Vector3 import Vector3 - _OUTPUT_DIR = DIMOS_PROJECT_ROOT / "assets" / "output" _MEMORY_DIR = _OUTPUT_DIR / "memory" _SPATIAL_MEMORY_DIR = _MEMORY_DIR / "spatial_memory" @@ -162,7 +162,7 @@ def __init__(self, **kwargs: Any) -> None: embedding_provider=self.embedding_provider, ) - self.last_position: Vector3 | None = None + self.last_position: Point | None = None self.last_record_time: float | None = None self.frame_count: int = 0 @@ -177,19 +177,11 @@ def __init__(self, **kwargs: Any) -> None: @rpc def start(self) -> None: - import cv2 - super().start() # Subscribe to LCM streams def set_video(image_msg: Image) -> None: - # Convert Image message to numpy array - if hasattr(image_msg, "data"): - frame = image_msg.data - frame = cv2.cvtColor(frame, cv2.COLOR_RGB2BGR) - self._latest_video_frame = frame - else: - logger.warning("Received image message without data attribute") + self._latest_video_frame = image_to_bgr(image_msg) self.register_disposable(Disposable(self.color_image.subscribe(set_video))) @@ -219,7 +211,7 @@ def _process_frame(self) -> None: return # Create Pose object with position and orientation - current_pose = tf.to_pose() + current_pose = pose_from_transform(tf).pose # Process the frame directly try: @@ -250,16 +242,16 @@ def _process_frame(self) -> None: frame_id = f"frame_{datetime.now().strftime('%Y%m%d_%H%M%S')}_{uuid.uuid4().hex[:8]}" # Get euler angles from quaternion orientation for metadata - euler = tf.rotation.to_euler() + roll, pitch, yaw = quaternion_euler(tf.transform.rotation) # Create metadata dictionary with primitive types only metadata = { "pos_x": float(current_pose.position.x), "pos_y": float(current_pose.position.y), "pos_z": float(current_pose.position.z), - "rot_x": float(euler.x), - "rot_y": float(euler.y), - "rot_z": float(euler.z), + "rot_x": roll, + "rot_y": pitch, + "rot_z": yaw, "timestamp": current_time, "frame_id": frame_id, } @@ -279,7 +271,7 @@ def _process_frame(self) -> None: logger.info( f"Stored frame at position ({current_pose.position.x:.2f}, {current_pose.position.y:.2f}, {current_pose.position.z:.2f}), " - f"rotation ({euler.x:.2f}, {euler.y:.2f}, {euler.z:.2f}) " + f"rotation ({roll:.2f}, {pitch:.2f}, {yaw:.2f}) " f"stored {self.stored_frame_count}/{self.frame_count} frames" ) @@ -378,7 +370,7 @@ def process_combined_data(data): # type: ignore[no-untyped-def] vector_id=frame_id, image=frame, embedding=frame_embedding, metadata=metadata ) - self.last_position = position_v3 + self.last_position = Point(x=position_v3.x, y=position_v3.y, z=position_v3.z) self.last_record_time = current_time self.stored_frame_count += 1 @@ -481,11 +473,12 @@ def add_named_location( return False # Create RobotLocation object - location = RobotLocation( # type: ignore[call-arg] + translation = tf.transform.translation + location = RobotLocation( name=name, - position=tf.translation, - rotation=tf.rotation.to_euler(), - description=description or f"Location: {name}", + position=(translation.x, translation.y, translation.z), + rotation=quaternion_euler(tf.transform.rotation), + metadata={"description": description or f"Location: {name}"}, timestamp=time.time(), ) diff --git a/dimos/perception/experimental/temporal_memory/clip_filter.py b/dimos/perception/experimental/temporal_memory/clip_filter.py index cb89c31c55d..19b2a142113 100644 --- a/dimos/perception/experimental/temporal_memory/clip_filter.py +++ b/dimos/perception/experimental/temporal_memory/clip_filter.py @@ -16,9 +16,10 @@ from typing import Any +from dimos_generated.sensor_msgs.msg import Image import numpy as np -from dimos.msgs.sensor_msgs.Image import Image +from dimos.msgs.image import image_view from dimos.utils.logging_config import setup_logger logger = setup_logger() @@ -36,9 +37,7 @@ def _get_image_data(image: Image) -> np.ndarray[Any, Any]: """Extract numpy array from Image.""" - if not hasattr(image, "data"): - raise AttributeError(f"Image missing .data attribute: {type(image)}") - return image.data + return image_view(image) if CLIP_AVAILABLE: diff --git a/dimos/perception/experimental/temporal_memory/entity_graph_db.py b/dimos/perception/experimental/temporal_memory/entity_graph_db.py index 5e51aa5f6ad..ac7139b43a0 100644 --- a/dimos/perception/experimental/temporal_memory/entity_graph_db.py +++ b/dimos/perception/experimental/temporal_memory/entity_graph_db.py @@ -34,8 +34,9 @@ from .temporal_utils.prompts import build_batch_distance_estimation_prompt if TYPE_CHECKING: + from dimos_generated.sensor_msgs.msg import Image + from dimos.models.vl.base import VlModel - from dimos.msgs.sensor_msgs.Image import Image logger = setup_logger() diff --git a/dimos/perception/experimental/temporal_memory/frame_window_accumulator.py b/dimos/perception/experimental/temporal_memory/frame_window_accumulator.py index 4c910a1b88a..674c23caaee 100644 --- a/dimos/perception/experimental/temporal_memory/frame_window_accumulator.py +++ b/dimos/perception/experimental/temporal_memory/frame_window_accumulator.py @@ -24,8 +24,10 @@ import threading from typing import TYPE_CHECKING +from dimos.msgs.time import to_seconds + if TYPE_CHECKING: - from dimos.msgs.sensor_msgs.Image import Image + from dimos_generated.sensor_msgs.msg import Image @dataclass @@ -87,10 +89,7 @@ def add_frame(self, image: Image, wall_time: float) -> None: with self._lock: if self._video_start_wall_time is None: return - if image.ts is not None: - timestamp_s = image.ts - self._video_start_wall_time - else: - timestamp_s = wall_time - self._video_start_wall_time + timestamp_s = to_seconds(image.header.stamp) - self._video_start_wall_time frame = Frame( frame_index=self._frame_count, timestamp_s=timestamp_s, diff --git a/dimos/perception/experimental/temporal_memory/temporal_memory.py b/dimos/perception/experimental/temporal_memory/temporal_memory.py index ce4b7eb3768..a3f1377ef7b 100644 --- a/dimos/perception/experimental/temporal_memory/temporal_memory.py +++ b/dimos/perception/experimental/temporal_memory/temporal_memory.py @@ -29,6 +29,9 @@ import time from typing import Any +from dimos_generated.dimos_msgs.msg import EntityMarker, EntityMarkers +from dimos_generated.geometry_msgs.msg import Point, PoseStamped +from dimos_generated.sensor_msgs.msg import Image from reactivex import Subject, interval from reactivex.disposable import Disposable @@ -38,10 +41,10 @@ from dimos.core.module import Module, ModuleConfig from dimos.core.stream import In, Out from dimos.models.vl.base import VlModel -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.sensor_msgs.Image import Image, sharpness_barrier -from dimos.msgs.visualization_msgs.EntityMarkers import EntityMarkers, Marker +from dimos.msgs.image import image_sharpness +from dimos.msgs.time import header_now from dimos.utils.logging_config import get_run_log_dir, setup_logger +from dimos.utils.reactive import quality_barrier from .clip_filter import CLIP_AVAILABLE, adaptive_keyframes from .entity_graph_db import EntityGraphDB @@ -248,7 +251,7 @@ def _publish_entity_markers(self) -> None: if not all_entities: return - markers: list[Marker] = [] + markers: list[EntityMarker] = [] for e in all_entities: meta = e.get("metadata") or {} x = meta.get("world_x") @@ -257,18 +260,18 @@ def _publish_entity_markers(self) -> None: if x is None or y is None: continue markers.append( - Marker( + EntityMarker( entity_id=e["entity_id"], label=(e.get("descriptor") or "")[:40], entity_type=e.get("entity_type", "object"), - x=x, - y=y, - z=(z or 0.0) + 0.3, # Offset up so labels float above ground + position=Point(x=x, y=y, z=(z or 0.0) + 0.3), # Offset labels above ground ) ) if markers: - self.entity_markers.publish(EntityMarkers(markers=markers)) + self.entity_markers.publish( + EntityMarkers(header=header_now("world"), markers=markers) + ) logger.info(f"[temporal-memory] published {len(markers)} entity markers to Rerun") except Exception as e: logger.debug(f"entity marker publish error: {e}") @@ -295,7 +298,9 @@ def _on_frame(img: Image) -> None: ) self.register_disposable( - frame_subject.pipe(sharpness_barrier(self.config.fps)).subscribe(_on_frame) + frame_subject.pipe(quality_barrier(image_sharpness, self.config.fps)).subscribe( + _on_frame + ) ) unsub_image = self.color_image.subscribe(frame_subject.on_next) self.register_disposable(Disposable(unsub_image)) @@ -303,9 +308,9 @@ def _on_frame(img: Image) -> None: # Odometry tracking for entity world positioning (optional — # module works without it, entities just won't have world positions) def _on_odom(msg: PoseStamped) -> None: - self._robot_x = msg.position.x - self._robot_y = msg.position.y - self._robot_z = msg.position.z + self._robot_x = msg.pose.position.x + self._robot_y = msg.pose.position.y + self._robot_z = msg.pose.position.z self._odom_count += 1 if self.odom.transport is not None: diff --git a/dimos/perception/experimental/temporal_memory/temporal_utils/helpers.py b/dimos/perception/experimental/temporal_memory/temporal_utils/helpers.py index 88ddee1157e..aa6f2fadc41 100644 --- a/dimos/perception/experimental/temporal_memory/temporal_utils/helpers.py +++ b/dimos/perception/experimental/temporal_memory/temporal_utils/helpers.py @@ -18,6 +18,8 @@ import numpy as np +from dimos.msgs.image import image_to_rgb + if TYPE_CHECKING: from ..frame_window_accumulator import Frame @@ -68,7 +70,9 @@ def is_scene_stale(frames: list["Frame"], stale_threshold: float = 5.0) -> bool: last_img = frames[-1].image if first_img is None or last_img is None: return False - if not hasattr(first_img, "data") or not hasattr(last_img, "data"): + first = image_to_rgb(first_img) + last = image_to_rgb(last_img) + if first.shape != last.shape: return False - diff = np.abs(first_img.data.astype(float) - last_img.data.astype(float)) + diff = np.abs(first.astype(float) - last.astype(float)) return bool(diff.mean() < stale_threshold) diff --git a/dimos/perception/experimental/temporal_memory/test_temporal_memory_module.py b/dimos/perception/experimental/temporal_memory/test_temporal_memory_module.py index c072c5f026b..1eaed121bc0 100644 --- a/dimos/perception/experimental/temporal_memory/test_temporal_memory_module.py +++ b/dimos/perception/experimental/temporal_memory/test_temporal_memory_module.py @@ -23,6 +23,9 @@ from typing import TYPE_CHECKING, Any from unittest.mock import MagicMock, create_autospec, patch +from dimos_generated.dimos_msgs.msg import EntityMarker, EntityMarkers +from dimos_generated.geometry_msgs.msg import Point +from dimos_generated.sensor_msgs.msg import Image import numpy as np import pytest from reactivex import operators as ops @@ -33,7 +36,8 @@ from dimos.core.stream import Out from dimos.core.transport import LCMTransport from dimos.models.vl.base import VlModel -from dimos.msgs.sensor_msgs.Image import Image +from dimos.msgs.image import image_from_array +from dimos.msgs.time import time_from_seconds from dimos.perception.experimental.temporal_memory.entity_graph_db import EntityGraphDB from dimos.perception.experimental.temporal_memory.frame_window_accumulator import ( Frame, @@ -47,6 +51,7 @@ extract_time_window, ) from dimos.utils.logging_config import setup_logger +from dimos.visualization.rerun.message_helpers import entity_points if TYPE_CHECKING: from pathlib import Path @@ -60,7 +65,7 @@ def _make_image(value: int = 128, shape: tuple[int, ...] = (64, 64, 3)) -> Image: data = np.full(shape, value, dtype=np.uint8) - return Image.from_numpy(data) + return image_from_array(data, encoding="rgb8") class TestFrameWindowAccumulator: @@ -69,7 +74,7 @@ def test_bounded_buffer(self) -> None: acc.set_start_time(0.0) for i in range(10): img = _make_image(i * 25) - img.ts = float(i) + img.header.stamp = time_from_seconds(float(i)) acc.add_frame(img, float(i)) assert acc.buffer_size == 5 assert acc.frame_count == 10 @@ -82,7 +87,7 @@ def test_window_extraction(self) -> None: # Add 3 frames for i in range(3): img = _make_image() - img.ts = float(i) + img.header.stamp = time_from_seconds(float(i)) acc.add_frame(img, float(i)) frames = acc.try_extract_window() assert frames is not None @@ -93,7 +98,7 @@ def test_stride_guard(self) -> None: acc.set_start_time(0.0) for i in range(3): img = _make_image() - img.ts = float(i) + img.header.stamp = time_from_seconds(float(i)) acc.add_frame(img, float(i)) # First extraction should succeed frames = acc.try_extract_window() @@ -112,7 +117,7 @@ def test_clear(self) -> None: acc = FrameWindowAccumulator(max_buffer_frames=50, window_s=1.0, stride_s=1.0, fps=1.0) acc.set_start_time(0.0) img = _make_image() - img.ts = 0.0 + img.header.stamp = time_from_seconds(0.0) acc.add_frame(img, 0.0) assert acc.buffer_size == 1 acc.clear() @@ -426,20 +431,28 @@ def test_publish_entity_markers(self, tmp_path: Path) -> None: ids = {m.entity_id for m in msg.markers} assert ids == {"E1", "E2"} e1 = next(m for m in msg.markers if m.entity_id == "E1") - assert e1.x == 1.0 - assert e1.y == 2.0 + assert e1.position.x == 1.0 + assert e1.position.y == 2.0 tm.stop() def test_markers_to_rerun(self) -> None: - from dimos.msgs.visualization_msgs.EntityMarkers import EntityMarkers, Marker - markers = EntityMarkers( markers=[ - Marker("E1", "person walking", "person", 1.0, 2.0, 0.3), - Marker("E2", "wooden table", "object", 3.0, 4.0, 0.3), + EntityMarker( + entity_id="E1", + label="person walking", + entity_type="person", + position=Point(x=1.0, y=2.0, z=0.3), + ), + EntityMarker( + entity_id="E2", + label="wooden table", + entity_type="object", + position=Point(x=3.0, y=4.0, z=0.3), + ), ] ) - archetype = markers.to_rerun() + archetype = entity_points(EntityMarkers.decode(markers.encode())) # Should return rr.Points3D import rerun as rr @@ -463,7 +476,7 @@ def test_analyze_window_calls_vlm(self) -> None: analyzer = WindowAnalyzer(mock_vlm) img = _make_image() - img.ts = 0.0 + img.header.stamp = time_from_seconds(0.0) frame = Frame(frame_index=0, timestamp_s=0.0, image=img) state_dict = {"entity_roster": [], "rolling_summary": ""} @@ -481,7 +494,7 @@ def test_analyze_window_vlm_error(self) -> None: analyzer = WindowAnalyzer(mock_vlm) img = _make_image() - img.ts = 0.0 + img.header.stamp = time_from_seconds(0.0) frame = Frame(frame_index=0, timestamp_s=0.0, image=img) result = analyzer.analyze_window([frame], {}, 0.0, 2.0) @@ -530,7 +543,7 @@ def start(self) -> None: def emit_frames(observer, scheduler): for i in range(self.num_frames): img = _make_image(value=min(50 + i * 30, 255)) # Varying brightness - img.ts = time.time() + img.header.stamp = time_from_seconds(time.time()) observer.on_next(img) time.sleep(0.5) observer.on_completed() @@ -642,3 +655,17 @@ def test_frames_flow_and_query( video_module.stop() temporal_memory_module.stop() + + +def test_scene_staleness_compares_pixels_not_padding(): + from dimos.perception.experimental.temporal_memory.frame_window_accumulator import Frame + from dimos.perception.experimental.temporal_memory.temporal_utils.helpers import is_scene_stale + + first = Image( + width=2, height=2, encoding="rgb8", step=8, data=[10] * 6 + [0, 0] + [10] * 6 + [0, 0] + ) + last = Image.decode(first.encode()) + last.data = [10] * 6 + [255, 255] + [10] * 6 + [255, 255] + assert is_scene_stale([Frame(0, 0, first), Frame(1, 1, last)], stale_threshold=1) + last.data = [30] * 6 + [255, 255] + [30] * 6 + [255, 255] + assert not is_scene_stale([Frame(0, 0, first), Frame(1, 1, last)], stale_threshold=1) diff --git a/dimos/perception/experimental/temporal_memory/window_analyzer.py b/dimos/perception/experimental/temporal_memory/window_analyzer.py index ccf194e9a4f..56352681ea6 100644 --- a/dimos/perception/experimental/temporal_memory/window_analyzer.py +++ b/dimos/perception/experimental/temporal_memory/window_analyzer.py @@ -34,8 +34,9 @@ ) if TYPE_CHECKING: + from dimos_generated.sensor_msgs.msg import Image + from dimos.models.vl.base import VlModel - from dimos.msgs.sensor_msgs.Image import Image from .frame_window_accumulator import Frame diff --git a/dimos/perception/experimental/test_generated_tracker.py b/dimos/perception/experimental/test_generated_tracker.py new file mode 100644 index 00000000000..3d8895afe80 --- /dev/null +++ b/dimos/perception/experimental/test_generated_tracker.py @@ -0,0 +1,105 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from unittest.mock import MagicMock + +from dimos_generated.sensor_msgs.msg import Image +from dimos_generated.std_msgs.msg import Header +from dimos_generated.tf2_msgs.msg import TFMessage +from dimos_generated.vision_msgs.msg import ( + BoundingBox2D, + Detection2D, + Detection2DArray, + Detection3DArray, + Point2D, + Pose2D, +) +import numpy as np +import pytest + +from dimos.msgs.image import image_view +from dimos.msgs.time import time_from_seconds +from dimos.perception.experimental.object_tracker_2d import ObjectTracker2D +from dimos.perception.experimental.object_tracker_3d import ObjectTracker3D + + +@pytest.mark.parametrize("succeeded", [True, False]) +def test_tracker_publishes_generated_detection_and_clears_failed_track(succeeded: bool) -> None: + module = ObjectTracker2D(frame_id="camera") + module.tracker = MagicMock() + module.tracker.update.return_value = (succeeded, (10, 12, 20, 16)) + module.tracking_initialized = True + module._latest_rgb_frame = np.zeros((64, 64, 3), dtype=np.uint8) + module.detection2darray = MagicMock() + module.tracked_overlay = MagicMock() + try: + module._process_tracking() + result = Detection2DArray.decode(module.detection2darray.publish.call_args.args[0].encode()) + assert result.header.frame_id == "camera" + if not succeeded: + assert not result.detections + assert not module.tracking_initialized + module.tracked_overlay.publish.assert_not_called() + return + (detection,) = result.detections + assert (detection.bbox.center.position.x, detection.bbox.center.position.y) == (20, 20) + assert (detection.bbox.size_x, detection.bbox.size_y) == (20, 16) + assert detection.results[0].hypothesis.class_id == "tracked_object" + overlay = Image.decode(module.tracked_overlay.publish.call_args.args[0].encode()) + assert overlay.header == result.header + assert image_view(overlay).shape == (64, 64, 3) + assert overlay.encoding == "rgb8" + finally: + module.stop() + + +@pytest.mark.parametrize("depth", [2.0, 0.0, float("nan")]) +def test_tracker3d_generated_projection_preserves_depth_filter_and_source_stamp( + depth: float, +) -> None: + module = ObjectTracker3D(frame_id="camera") + module.camera_intrinsics = [100.0, 100.0, 32.0, 32.0] + module._latest_depth_frame = np.full((64, 64), depth, dtype=np.float32) + module.tf = MagicMock() + module.detection2darray = MagicMock() + header = Header(stamp=time_from_seconds(12.25), frame_id="camera") + detection = Detection2DArray( + header=header, + detections=[ + Detection2D( + header=header, + bbox=BoundingBox2D( + center=Pose2D(position=Point2D(x=32, y=32)), size_x=20, size_y=16 + ), + ) + ], + ) + try: + output = module._create_detection3d_from_2d(detection) + if not np.isfinite(depth) or depth <= 0: + assert output is None + module.tf.publish.assert_not_called() + return + result = Detection3DArray.decode(output.encode()) + assert result.header == header + box = result.detections[0].bbox + assert (box.center.position.x, box.center.position.y, box.center.position.z) == (2, 0, 0) + assert np.allclose([box.size.x, box.size.y, box.size.z], [0.4, 0.32, 0.1]) + transform = TFMessage.decode(module.tf.publish.call_args.args[0].encode()).transforms[0] + assert transform.header == header + assert transform.child_frame_id == "tracked_object" + assert transform.transform.translation.x == 2.0 + assert transform.transform.rotation.w == 1.0 + finally: + module.stop() diff --git a/dimos/perception/experimental/test_object_generated.py b/dimos/perception/experimental/test_object_generated.py new file mode 100644 index 00000000000..be288947c6d --- /dev/null +++ b/dimos/perception/experimental/test_object_generated.py @@ -0,0 +1,96 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Object geometry and deduplication use generated ROS values.""" + +from dimos_generated.builtin_interfaces.msg import Time +from dimos_generated.geometry_msgs.msg import PoseStamped, Vector3 +from dimos_generated.sensor_msgs.msg import Image, PointCloud2 +from dimos_generated.std_msgs.msg import Header +from dimos_generated.vision_msgs.msg import Detection3D +import numpy as np + +from dimos.msgs.image import image_from_array +from dimos.msgs.pointcloud import pointcloud_from_xyz_rgb, pointcloud_rgb, pointcloud_xyz +from dimos.perception.experimental.object import Object, aggregate_pointclouds +from dimos.perception.experimental.objectDB import ObjectDB + + +def _object(object_id, x, timestamp): + return Object( + object_id=object_id, + name="cup", + center=Vector3(x=x), + size=Vector3(x=0.1, y=0.2, z=0.3), + pose=PoseStamped( + header=Header(frame_id="world", stamp=Time(sec=1700000000, nanosec=123456789)) + ), + pointcloud=PointCloud2(), + bbox=(0.0, 0.0, 1.0, 1.0), + track_id=-1, + class_id=0, + confidence=0.9, + ts=timestamp, + frame_id="world", + image=Image(), + ) + + +def test_detection_cdr_preserves_source_pose_header_and_current_center(): + obj = _object("cup-1", 0.2, 1.0) + obj.set_center(Vector3(x=0.4, y=0.5, z=0.6)) + decoded = Detection3D.decode(obj.to_detection3d_msg().encode()) + assert decoded.header == obj.pose.header + assert decoded.bbox.center == obj.pose.pose + assert decoded.bbox.size == obj.size + assert decoded.results[0].hypothesis.class_id == "cup" + assert decoded.results[0].hypothesis.score == 0.9 + decoded.bbox.center.position.x = 99 + assert obj.center.x == 0.4 + assert obj.pose.pose.position.x == 0.4 + + +def test_spatial_deduplication_and_nearest_lookup_use_generated_centers(): + database = ObjectDB(distance_threshold=0.2, min_detections_for_permanent=1) + first = _object("first", 0.0, 1.0) + nearby = _object("nearby", 0.1, 2.0) + distant = _object("distant", 2.0, 3.0) + assert database.add_objects([first]) == [first] + assert database.add_objects([nearby]) == [first] + assert first.center.x == 0.1 + assert database.add_objects([distant]) == [distant] + assert database.find_nearest(Vector3(x=1.8)) is distant + assert database.find_nearest(Vector3(x=0.2)) is first + + +def test_generated_object_cloud_aggregation_and_dictionary_preserve_values(): + obj = _object("abc", 0.2, 1.0) + stamp = Time(sec=1700000000, nanosec=987654321) + obj.pointcloud = pointcloud_from_xyz_rgb( + np.array([[1.0, 2.0, 3.0]], dtype=np.float32), + np.array([[255, 128, 0]], dtype=np.uint8), + header=Header(frame_id="world", stamp=stamp), + ) + pixels = np.zeros((2, 3, 3), dtype=np.uint8) + obj.image = image_from_array(pixels, encoding="rgb8", header=obj.pointcloud.header) + exported = obj.to_dict() + np.testing.assert_array_equal(exported["pointcloud"][0], [[1.0, 2.0, 3.0]]) + np.testing.assert_allclose(exported["pointcloud"][1], [[1.0, 128 / 255.0, 0.0]]) + np.testing.assert_array_equal(exported["image"], pixels) + merged = aggregate_pointclouds([obj]) + decoded = PointCloud2.decode(merged.encode()) + assert decoded.header.stamp == stamp and decoded.header.frame_id == "world" + np.testing.assert_array_equal(pointcloud_xyz(decoded), [[1.0, 2.0, 3.0]]) + assert pointcloud_rgb(decoded) is not None + assert aggregate_pointclouds([]).width == 0 diff --git a/dimos/perception/experimental/test_object_scene_registration_temporal.py b/dimos/perception/experimental/test_object_scene_registration_temporal.py index 82f14417f9c..4aa1bade0e8 100644 --- a/dimos/perception/experimental/test_object_scene_registration_temporal.py +++ b/dimos/perception/experimental/test_object_scene_registration_temporal.py @@ -20,11 +20,18 @@ from typing import Any from unittest.mock import ANY, MagicMock, call +from dimos_generated.geometry_msgs.msg import Quaternion, Transform, TransformStamped, Vector3 +from dimos_generated.sensor_msgs.msg import Image +from dimos_generated.std_msgs.msg import Header +from dimos_generated.vision_msgs.msg import Detection3DArray import numpy as np import pytest -from dimos.msgs.sensor_msgs.Image import Image, ImageFormat -from dimos.msgs.vision_msgs.Detection3DArray import Detection3DArray +from dimos.msgs.camera_info import camera_info_from_intrinsics +from dimos.msgs.image import image_from_array +from dimos.msgs.pointcloud import pointcloud_xyz +from dimos.msgs.time import time_from_seconds +from dimos.perception.detection.type.detection2d.bbox import Detection2DBBox from dimos.perception.detection.type.detection2d.imageDetections2D import ImageDetections2D from dimos.perception.experimental.object_scene_registration import ObjectSceneRegistrationModule from dimos.perception.experimental.objectDB import ObjectDB @@ -44,18 +51,19 @@ def dispose(self) -> None: def _image(timestamp: float) -> Image: - return Image( - data=np.ones((2, 2), dtype=np.float32), - format=ImageFormat.DEPTH, - frame_id="camera", - ts=timestamp, + return image_from_array( + np.ones((2, 2), dtype=np.float32), + encoding="32FC1", + header=Header(frame_id="camera", stamp=time_from_seconds(timestamp)), ) @pytest.fixture def module() -> Iterator[ObjectSceneRegistrationModule]: module = ObjectSceneRegistrationModule(target_frame="map") - module._camera_info = MagicMock(K=[1.0, 0.0, 0.0, 0.0, 1.0, 0.0]) + module._camera_info = camera_info_from_intrinsics( + 1, 1, 0, 0, 2, 2, header=Header(frame_id="camera") + ) module._latest_scene_snapshot = None yield module module.stop() @@ -78,11 +86,10 @@ def get(self, *args: Any, **kwargs: Any) -> Any: module._detector = MagicMock() module._text_prompts = ["cup"] module.detections_2d = MagicMock() - color = Image( - data=np.zeros((2, 2, 3), dtype=np.uint8), - format=ImageFormat.BGR, - frame_id="camera", - ts=12.5, + color = image_from_array( + np.zeros((2, 2, 3), dtype=np.uint8), + encoding="bgr8", + header=Header(frame_id="camera", stamp=time_from_seconds(12.5)), ) detections = ImageDetections2D(color, []) @@ -138,35 +145,30 @@ def test_full_scene_pointcloud_uses_one_coherent_scene_snapshot( monkeypatch: Any, module: ObjectSceneRegistrationModule ) -> None: depth = _image(3.0) - transform = MagicMock(name="transform") + transform = TransformStamped( + header=Header(frame_id="map"), + child_frame_id="camera", + transform=Transform(translation=Vector3(x=1), rotation=Quaternion(w=1)), + ) module._tf = _FakeTF(transform) # type: ignore[assignment] module._latest_scene_snapshot = (depth, transform) class _PointCloud: - points = list(range(100)) + points = np.zeros((100, 3), dtype=np.float64) def voxel_down_sample(self, voxel_size: float) -> _PointCloud: return self - pointcloud = _PointCloud() fake_o3d = MagicMock() - fake_o3d.camera.PinholeCameraIntrinsic.return_value = MagicMock() - fake_o3d.geometry.Image.return_value = MagicMock() - fake_o3d.geometry.PointCloud.create_from_depth_image.return_value = pointcloud - # open3d is imported inside the method under test, so swap the module itself + fake_o3d.geometry.PointCloud.create_from_depth_image.return_value = _PointCloud() + # Only the native depth projection is mocked; generated output and transformation are real. monkeypatch.setitem(sys.modules, "open3d", fake_o3d) - - result = MagicMock() - result.transform.side_effect = lambda used_transform: ( - result if used_transform is transform else pytest.fail("mixed scene snapshot") - ) - monkeypatch.setattr( - "dimos.perception.experimental.object_scene_registration.PointCloud2", - lambda *_args, **_kwargs: result, - ) - - module.get_full_scene_pointcloud() - result.transform.assert_called_once_with(transform) + result = module.get_full_scene_pointcloud() + assert result is not None + assert result.header.frame_id == "map" + assert result.header.stamp == depth.header.stamp + np.testing.assert_array_equal(pointcloud_xyz(result), np.tile([1, 0, 0], (100, 1))) + assert module._tf.calls == [] # The cached coherent snapshot supplied the transform. def test_owlv2_queries_configured_prompts(monkeypatch: Any) -> None: @@ -177,11 +179,10 @@ def test_owlv2_queries_configured_prompts(monkeypatch: Any) -> None: module._detector = MagicMock() module._text_prompts = ["mug"] module.detections_2d = MagicMock() - color = Image( - data=np.zeros((2, 2, 3), dtype=np.uint8), - format=ImageFormat.BGR, - frame_id="camera", - ts=4.0, + color = image_from_array( + np.zeros((2, 2, 3), dtype=np.uint8), + encoding="bgr8", + header=Header(frame_id="camera", stamp=time_from_seconds(4.0)), ) detections = ImageDetections2D(color, []) module._detector.query_detections.return_value = detections @@ -219,27 +220,30 @@ def test_owlv2_yolo_constructs_box_prompt_segmenter(mocker: Any) -> None: def test_moondream_queries_each_configured_prompt(monkeypatch: Any) -> None: module = ObjectSceneRegistrationModule(target_frame="camera", detector_backend="moondream") module._camera_info = MagicMock() - module._detector = MagicMock() + detector = MagicMock() + module._detector = detector module._text_prompts = ["cup", "bottle"] module.detections_2d = MagicMock() - color = Image( - data=np.zeros((2, 2, 3), dtype=np.uint8), - format=ImageFormat.BGR, - frame_id="camera", - ts=4.0, + color = image_from_array( + np.zeros((2, 2, 3), dtype=np.uint8), + encoding="bgr8", + header=Header(frame_id="camera", stamp=time_from_seconds(4.0)), ) - cup = MagicMock(track_id=0, class_id=-1) - bottle = MagicMock(track_id=0, class_id=-1) - module._detector.query_detections.side_effect = [ + cup = Detection2DBBox((0, 0, 1, 1), 0, -1, 0.9, "cup", 4.0, color) + bottle = Detection2DBBox((1, 1, 2, 2), 0, -1, 0.8, "bottle", 4.0, color) + detector.query_detections.side_effect = [ ImageDetections2D(color, [cup]), ImageDetections2D(color, [bottle]), ] process_3d = MagicMock() monkeypatch.setattr(module, "_process_3d_detections", process_3d) - module._process_images(color, _image(4.0)) + try: + module._process_images(color, _image(4.0)) + finally: + module.stop() - assert module._detector.query_detections.call_args_list == [ + assert detector.query_detections.call_args_list == [ call(color, "cup"), call(color, "bottle"), ] @@ -247,17 +251,15 @@ def test_moondream_queries_each_configured_prompt(monkeypatch: Any) -> None: assert combined.detections == [cup, bottle] assert (cup.track_id, cup.class_id) == (-1, 0) assert (bottle.track_id, bottle.class_id) == (-1, 1) - module.stop() def test_edgetam_refines_detector_output(monkeypatch: Any) -> None: module = ObjectSceneRegistrationModule(target_frame="camera", segmentation_backend="edgetam") module._camera_info = MagicMock() - color = Image( - data=np.zeros((2, 2, 3), dtype=np.uint8), - format=ImageFormat.BGR, - frame_id="camera", - ts=4.0, + color = image_from_array( + np.zeros((2, 2, 3), dtype=np.uint8), + encoding="bgr8", + header=Header(frame_id="camera", stamp=time_from_seconds(4.0)), ) raw_detections = ImageDetections2D(color, []) segmented_detections = ImageDetections2D(color, []) @@ -281,11 +283,10 @@ def test_request_driven_scan_processes_latest_aligned_frame(monkeypatch: Any) -> target_frame="camera", detector_backend="owlv2", detect_on_request=True ) module._object_db = MagicMock() - color = Image( - data=np.zeros((2, 2, 3), dtype=np.uint8), - format=ImageFormat.BGR, - frame_id="camera", - ts=4.0, + color = image_from_array( + np.zeros((2, 2, 3), dtype=np.uint8), + encoding="bgr8", + header=Header(frame_id="camera", stamp=time_from_seconds(4.0)), ) depth = _image(4.0) module._on_aligned_frames((color, depth)) @@ -378,11 +379,10 @@ def test_concurrent_request_scans_do_not_overlap(monkeypatch: Any) -> None: target_frame="camera", detector_backend="owlv2", detect_on_request=True ) module._object_db = MagicMock() - color = Image( - data=np.zeros((2, 2, 3), dtype=np.uint8), - format=ImageFormat.BGR, - frame_id="camera", - ts=4.0, + color = image_from_array( + np.zeros((2, 2, 3), dtype=np.uint8), + encoding="bgr8", + header=Header(frame_id="camera", stamp=time_from_seconds(4.0)), ) module._on_aligned_frames((color, _image(4.0))) first_started = Event() @@ -454,10 +454,9 @@ def test_object_db_counts_each_source_frame_once(monkeypatch: Any) -> None: last_seen_ts=None, detections_count=1, ) - first.center = MagicMock() + first.center = Vector3() duplicate = MagicMock(object_id="duplicate-id", track_id=-1, ts=4.0) - duplicate.center = MagicMock() - duplicate.center.distance.return_value = 0.0 + duplicate.center = Vector3() observed = object_db.add_objects([first, duplicate]) @@ -466,8 +465,7 @@ def test_object_db_counts_each_source_frame_once(monkeypatch: Any) -> None: assert first.last_seen_ts == 1000.0 newer = MagicMock(object_id="newer-id", track_id=-1, ts=5.0) - newer.center = MagicMock() - newer.center.distance.return_value = 0.0 + newer.center = Vector3() first.update_object.side_effect = lambda _: setattr(first, "detections_count", 2) now[0] = 1001.0 diff --git a/dimos/perception/experimental/test_spatial_memory.py b/dimos/perception/experimental/test_spatial_memory.py index a330394460b..b8c3126a472 100644 --- a/dimos/perception/experimental/test_spatial_memory.py +++ b/dimos/perception/experimental/test_spatial_memory.py @@ -17,12 +17,12 @@ import tempfile import time +from dimos_generated.geometry_msgs.msg import Vector3 import numpy as np import pytest from reactivex import operators as ops from reactivex.scheduler import ThreadPoolScheduler -from dimos.msgs.geometry_msgs.Pose import Pose from dimos.perception.experimental.spatial_perception import SpatialMemory from dimos.stream.video_provider import VideoProvider @@ -110,7 +110,7 @@ def process_frame(frame): nonlocal frame_counter # Generate a unique position for this frame to ensure minimum distance threshold is met - pos = Pose(frame_counter * 0.5, frame_counter * 0.5, 0) + pos = Vector3(x=frame_counter * 0.5, y=frame_counter * 0.5) transform = {"position": pos, "timestamp": time.time()} frame_counter += 1 diff --git a/dimos/perception/experimental/test_spatial_memory_module.py b/dimos/perception/experimental/test_spatial_memory_module.py index 0bdef987335..39e8f241003 100644 --- a/dimos/perception/experimental/test_spatial_memory_module.py +++ b/dimos/perception/experimental/test_spatial_memory_module.py @@ -17,6 +17,10 @@ import time from typing import Any +from dimos_generated.geometry_msgs.msg import PoseStamped +from dimos_generated.sensor_msgs.msg import Image +from dimos_generated.std_msgs.msg import Header +from dimos_generated.tf2_msgs.msg import TFMessage import pytest from reactivex import operators as ops @@ -25,11 +29,10 @@ from dimos.core.module import Module, ModuleConfig from dimos.core.stream import Out from dimos.core.transport_factory import make_transport -from dimos.msgs.geometry_msgs.Transform import Transform -from dimos.msgs.sensor_msgs.Image import Image -from dimos.msgs.tf2_msgs.TFMessage import TFMessage +from dimos.msgs.geometry import transform_from_pose +from dimos.msgs.image import image_from_array from dimos.perception.experimental.spatial_perception import SpatialMemory -from dimos.robot.unitree.type.odometry import Odometry +from dimos.robot.unitree.type.odometry import pose_from_webrtc_odometry from dimos.utils.data import get_data from dimos.utils.logging_config import setup_logger from dimos.utils.testing.legacy_pickle import LegacyPickleStore @@ -53,7 +56,10 @@ class VideoReplayModule(Module): def start(self) -> None: """Start replaying video data.""" # Use LegacyPickleStore to replay video frames - video_replay = LegacyPickleStore(self.config.video_path, autocast=Image.from_numpy) + video_replay = LegacyPickleStore( + self.config.video_path, + autocast=lambda pixels: image_from_array(pixels, encoding="bgr8", header=Header()), + ) # Subscribe to the replay stream and publish to LCM self._subscription = ( @@ -86,15 +92,17 @@ def __init__(self, odom_path: str, **kwargs: Any) -> None: self.odom_path = odom_path self._subscription = None - def _publish_tf(self, odom: Odometry) -> None: + def _publish_tf(self, odom: PoseStamped) -> None: """Convert odometry to TF transforms and publish.""" - self.tf.publish(TFMessage(Transform.from_pose("base_link", odom))) + self.tf.publish( + TFMessage(transforms=[transform_from_pose(odom, child_frame_id="base_link")]) + ) @rpc def start(self) -> None: """Start replaying odometry data.""" # Use LegacyPickleStore to replay odometry - odom_replay = LegacyPickleStore(self.odom_path, autocast=Odometry.from_msg) + odom_replay = LegacyPickleStore(self.odom_path, autocast=pose_from_webrtc_odometry) # Subscribe to the replay stream and publish to tf self._subscription = ( diff --git a/dimos/perception/fiducial/blueprints/desk_marker_tf.py b/dimos/perception/fiducial/blueprints/desk_marker_tf.py index 0a63f90ca57..2367589457c 100644 --- a/dimos/perception/fiducial/blueprints/desk_marker_tf.py +++ b/dimos/perception/fiducial/blueprints/desk_marker_tf.py @@ -20,6 +20,12 @@ import threading import time +from dimos_generated.geometry_msgs.msg import Quaternion, Transform, TransformStamped, Vector3 +from dimos_generated.sensor_msgs.msg import CameraInfo +from dimos_generated.std_msgs.msg import Header +from dimos_generated.tf2_msgs.msg import TFMessage +from dimos_generated.vision_msgs.msg import Detection3DArray + from dimos.core.coordination.blueprints import autoconnect from dimos.core.core import rpc from dimos.core.module import Module, ModuleConfig @@ -27,12 +33,9 @@ from dimos.core.transport import LCMTransport from dimos.hardware.sensors.camera.module import CameraModule from dimos.hardware.sensors.camera.webcam import Webcam -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Transform import Transform -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo -from dimos.msgs.tf2_msgs.TFMessage import TFMessage -from dimos.msgs.vision_msgs.Detection3DArray import Detection3DArray +from dimos.msgs.camera_info import camera_info_from_yaml +from dimos.msgs.geometry import quaternion_from_euler +from dimos.msgs.time import time_from_nanoseconds from dimos.perception.fiducial.marker_detection_stream_module import MarkerDetectionStreamModule from dimos.perception.fiducial.marker_tf_module import MarkerTfModule @@ -61,8 +64,9 @@ def create_desk_webcam( def create_desk_camera_info( camera_info_yaml: str | Path = DEFAULT_DESK_CAMERA_INFO_YAML, ) -> CameraInfo: - camera_info = CameraInfo.from_yaml(str(camera_info_yaml)) - camera_info.frame_id = DESK_CAMERA_FRAME_ID + camera_info = camera_info_from_yaml( + camera_info_yaml, header=Header(frame_id=DESK_CAMERA_FRAME_ID) + ) return camera_info @@ -124,28 +128,27 @@ def stop(self) -> None: super().stop() def publish_static_chain(self) -> None: - ts = time.time() - self._last_publish_ts = ts + stamp = time_from_nanoseconds(time.time_ns()) + self._last_publish_ts = stamp.sec + stamp.nanosec / 1e9 roll, pitch, yaw = self.config.camera_rotation_rpy_rad x, y, z = self.config.camera_translation_m - self.tf.publish( TFMessage( - Transform( - translation=Vector3(0.0, 0.0, 0.0), - rotation=Quaternion(0.0, 0.0, 0.0, 1.0), - frame_id=self.config.world_frame, - child_frame_id=self.config.base_frame, - ts=ts, - ), - Transform( - # Default desk camera pose: about 25 cm forward and 15 cm above base_link. - translation=Vector3(x, y, z), - rotation=Quaternion.from_euler(Vector3(roll, pitch, yaw)), - frame_id=self.config.base_frame, - child_frame_id=self.config.camera_optical_frame, - ts=ts, - ), + transforms=[ + TransformStamped( + header=Header(stamp=stamp, frame_id=self.config.world_frame), + child_frame_id=self.config.base_frame, + transform=Transform(rotation=Quaternion(w=1)), + ), + TransformStamped( + header=Header(stamp=stamp, frame_id=self.config.base_frame), + child_frame_id=self.config.camera_optical_frame, + transform=Transform( + translation=Vector3(x=x, y=y, z=z), + rotation=quaternion_from_euler(roll, pitch, yaw), + ), + ), + ] ) ) diff --git a/dimos/perception/fiducial/blueprints/test_desk_marker_tf.py b/dimos/perception/fiducial/blueprints/test_desk_marker_tf.py index fc7e4ecf639..2d820d94fa4 100644 --- a/dimos/perception/fiducial/blueprints/test_desk_marker_tf.py +++ b/dimos/perception/fiducial/blueprints/test_desk_marker_tf.py @@ -41,7 +41,9 @@ def test_desk_marker_tf_blueprint_declares_static_tf_module() -> None: assert desk_marker_tf.blueprints[2].module is MarkerDetectionStreamModule assert desk_marker_tf.blueprints[2].kwargs["marker_length_m"] == DESK_MARKER_LENGTH_M assert desk_marker_tf.blueprints[2].kwargs["aruco_dictionary"] == DESK_MARKER_ARUCO_DICTIONARY - assert desk_marker_tf.blueprints[2].kwargs["camera_info"].frame_id == DESK_CAMERA_FRAME_ID + assert ( + desk_marker_tf.blueprints[2].kwargs["camera_info"].header.frame_id == DESK_CAMERA_FRAME_ID + ) assert desk_marker_tf.blueprints[3].module is MarkerTfModule assert ( desk_marker_tf.blueprints[3].kwargs["marker_namespace_prefix"] @@ -87,10 +89,10 @@ def test_create_desk_webcam_loads_camera_info_yaml(tmp_path: Path) -> None: assert camera.config.width == 1920 assert camera.config.height == 1080 assert camera.config.fps == 7.5 - assert camera.config.camera_info.frame_id == DESK_CAMERA_FRAME_ID + assert camera.config.camera_info.header.frame_id == DESK_CAMERA_FRAME_ID camera_info = create_desk_camera_info(camera_info_yaml) - assert camera_info.frame_id == DESK_CAMERA_FRAME_ID + assert camera_info.header.frame_id == DESK_CAMERA_FRAME_ID assert camera_info.width == 1920 @@ -106,10 +108,11 @@ def test_desk_static_tf_module_publishes_world_to_camera_optical_chain() -> None world_camera = view.get("world", "camera_optical", mod._last_publish_ts, 1.0) assert world_camera is not None - assert world_camera.frame_id == "world" + assert world_camera.header.frame_id == "world" assert world_camera.child_frame_id == "camera_optical" - assert world_camera.translation.x == 0.3 - assert world_camera.translation.y == 0.0 - assert world_camera.translation.z == 0.2 + assert world_camera.transform.translation.x == 0.3 + assert world_camera.transform.translation.y == 0.0 + assert world_camera.transform.translation.z == 0.2 finally: + view.dispose() mod.stop() diff --git a/dimos/perception/fiducial/marker_detect.py b/dimos/perception/fiducial/marker_detect.py index 9abb07ff334..c002a5ba4fe 100644 --- a/dimos/perception/fiducial/marker_detect.py +++ b/dimos/perception/fiducial/marker_detect.py @@ -18,12 +18,14 @@ from typing import Any +from dimos_generated.geometry_msgs.msg import TransformStamped, Vector3 +from dimos_generated.sensor_msgs.msg import CameraInfo, Image +from dimos_generated.std_msgs.msg import Header import numpy as np -from dimos.msgs.geometry_msgs.Transform import Transform -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo -from dimos.msgs.sensor_msgs.Image import Image +from dimos.msgs.geometry import compose_transforms +from dimos.msgs.image import image_to_bgr +from dimos.msgs.time import to_seconds from dimos.perception.detection.type.detection3d.marker import Detection3DMarker from dimos.perception.fiducial.marker_pose import ( camera_info_to_cv_matrices, @@ -40,16 +42,18 @@ def detect_markers_in_image( image: Image, *, camera_info: CameraInfo, - world_T_optical: Transform, + world_T_optical: TransformStamped, marker_length_m: float, aruco_dictionary: str, world_frame: str = "world", detect_inverted: bool = False, detector: Any | None = None, - camera_matrix: np.ndarray | None = None, - dist_coeffs: np.ndarray | None = None, + camera_matrix: np.ndarray[Any, np.dtype[Any]] | None = None, + dist_coeffs: np.ndarray[Any, np.dtype[Any]] | None = None, ) -> list[Detection3DMarker]: """Detect markers in one image and return rich world-frame 3D detections.""" + import cv2 + if marker_length_m <= 0: raise ValueError(f"marker_length_m must be > 0, got {marker_length_m}") if ( @@ -66,20 +70,18 @@ def detect_markers_in_image( if camera_matrix is None or dist_coeffs is None: camera_matrix, dist_coeffs = camera_info_to_cv_matrices(camera_info) - gray = image.to_grayscale().as_numpy() + gray = cv2.cvtColor(image_to_bgr(image), cv2.COLOR_BGR2GRAY) corners, ids, _ = detector.detectMarkers(gray) if ids is None or len(ids) == 0: return [] optical_frame = camera_optical_frame_id(image, camera_info) - t_world_optical = Transform( - translation=world_T_optical.translation, - rotation=world_T_optical.rotation, - frame_id=world_frame, + t_world_optical = TransformStamped( + header=Header(stamp=image.header.stamp, frame_id=world_frame), child_frame_id=optical_frame, - ts=image.ts, + transform=world_T_optical.transform, ) - marker_size = Vector3(marker_length_m, marker_length_m, 0.0) + marker_size = Vector3(x=marker_length_m, y=marker_length_m) detections: list[Detection3DMarker] = [] for corner_set, mid_arr in zip(corners, ids, strict=True): @@ -98,11 +100,10 @@ def detect_markers_in_image( t_optical_marker = rvec_tvec_to_transform( rvec, tvec, - frame_id=optical_frame, + header=Header(stamp=image.header.stamp, frame_id=optical_frame), child_frame_id=f"marker_{mid}", - ts=image.ts, ) - t_world_marker = t_world_optical + t_optical_marker + t_world_marker = compose_transforms(t_world_optical, t_optical_marker) corners_2d = corner_set.reshape(4, 2).astype(np.float32) bbox = marker_corners_to_bbox(corners_2d) @@ -123,13 +124,13 @@ def detect_markers_in_image( class_id=mid, confidence=1.0, name="", - ts=image.ts, + ts=to_seconds(image.header.stamp), image=image, - center=t_world_marker.translation, + center=t_world_marker.transform.translation, size=marker_size, transform=t_world_optical, frame_id=world_frame, - orientation=t_world_marker.rotation, + orientation=t_world_marker.transform.rotation, marker_id=mid, corners_px=corners_2d, dictionary=aruco_dictionary, diff --git a/dimos/perception/fiducial/marker_detection_stream_module.py b/dimos/perception/fiducial/marker_detection_stream_module.py index e200326c75a..2158daa5c11 100644 --- a/dimos/perception/fiducial/marker_detection_stream_module.py +++ b/dimos/perception/fiducial/marker_detection_stream_module.py @@ -21,9 +21,11 @@ from __future__ import annotations -import time from typing import Any, cast +from dimos_generated.sensor_msgs.msg import CameraInfo, Image +from dimos_generated.tf2_msgs.msg import TFMessage +from dimos_generated.vision_msgs.msg import Detection3DArray from pydantic import Field from reactivex.disposable import Disposable @@ -34,10 +36,8 @@ from dimos.memory.store.null import NullStore from dimos.memory.stream import Stream from dimos.memory.transform import QualityWindow, SpeedLimit -from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo -from dimos.msgs.sensor_msgs.Image import Image -from dimos.msgs.tf2_msgs.TFMessage import TFMessage -from dimos.msgs.vision_msgs.Detection3DArray import Detection3DArray +from dimos.msgs.image import image_sharpness +from dimos.msgs.time import to_seconds from dimos.perception.detection.type.detection3d.marker import Detection3DMarker from dimos.perception.fiducial.marker_pose import camera_optical_frame_id, is_fisheye_model from dimos.perception.fiducial.marker_transformer import DetectMarkers, MarkersPerFrame @@ -78,7 +78,7 @@ def __init__(self, **kwargs: Any) -> None: def pipeline(self, stream: Stream[Image]) -> Stream[Detection3DArray]: result: Stream[Any] = stream.transform( - QualityWindow(lambda img: img.sharpness, window=self.config.quality_window_s) + QualityWindow(image_sharpness, window=self.config.quality_window_s) ) if self.config.speed_limit_enabled: result = result.transform( @@ -121,7 +121,7 @@ def _append_image_with_pose(self, stream: Stream[Image], image: Image) -> None: logger.debug("MarkerDetectionStreamModule: no CameraInfo yet; skipping frame") return - ts = getattr(image, "ts", None) or time.time() + ts = to_seconds(image.header.stamp) optical = camera_optical_frame_id(image, info) t_world_optical = self.tfbuffer.get( self.config.world_frame, @@ -142,13 +142,13 @@ def _append_image_with_pose(self, stream: Stream[Image], image: Image) -> None: image, ts=ts, pose=( - t_world_optical.translation.x, - t_world_optical.translation.y, - t_world_optical.translation.z, - t_world_optical.rotation.x, - t_world_optical.rotation.y, - t_world_optical.rotation.z, - t_world_optical.rotation.w, + t_world_optical.transform.translation.x, + t_world_optical.transform.translation.y, + t_world_optical.transform.translation.z, + t_world_optical.transform.rotation.x, + t_world_optical.transform.rotation.y, + t_world_optical.transform.rotation.z, + t_world_optical.transform.rotation.w, ), ) diff --git a/dimos/perception/fiducial/marker_pose.py b/dimos/perception/fiducial/marker_pose.py index bfe7f115e18..6e7c9b838ed 100644 --- a/dimos/perception/fiducial/marker_pose.py +++ b/dimos/perception/fiducial/marker_pose.py @@ -16,15 +16,14 @@ from __future__ import annotations -from typing import TYPE_CHECKING +from typing import TYPE_CHECKING, Any +from dimos_generated.geometry_msgs.msg import Transform, TransformStamped, Vector3 +from dimos_generated.sensor_msgs.msg import CameraInfo, Image +from dimos_generated.std_msgs.msg import Header import numpy as np -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Transform import Transform -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo -from dimos.msgs.sensor_msgs.Image import Image +from dimos.msgs.geometry import quaternion_from_matrix if TYPE_CHECKING: import cv2 @@ -38,26 +37,28 @@ def is_fisheye_model(distortion_model: str | None) -> bool: return (distortion_model or "").strip().lower() in _FISHEYE_MODELS -def camera_info_to_cv_matrices(camera_info: CameraInfo) -> tuple[np.ndarray, np.ndarray]: +def camera_info_to_cv_matrices( + camera_info: CameraInfo, +) -> tuple[np.ndarray[Any, np.dtype[Any]], np.ndarray[Any, np.dtype[Any]]]: """Build OpenCV ``cameraMatrix`` and ``distCoeffs`` from ``CameraInfo``.""" - k = np.array(camera_info.K, dtype=np.float64).reshape(3, 3) - d = np.array(camera_info.D if camera_info.D else [], dtype=np.float64).reshape(-1, 1) + k = np.array(camera_info.k, dtype=np.float64).reshape(3, 3) + d = np.array(camera_info.d if camera_info.d else [], dtype=np.float64).reshape(-1, 1) return k, d def camera_optical_frame_id(image: Image, camera_info: CameraInfo) -> str: """Frame in which image pixels and intrinsics apply (optical convention in ROS). - Prefer ``Image.frame_id`` so TF lookups match the stream that produced the - pixels. Fall back to ``CameraInfo.frame_id``, then a conventional default. + Prefer ``Image.header.frame_id`` so TF lookups match the stream that produced the + pixels. Fall back to ``CameraInfo.header.frame_id``, then a conventional default. """ - for fid in (image.frame_id, camera_info.frame_id): + for fid in (image.header.frame_id, camera_info.header.frame_id): if fid and fid.strip(): return fid.strip() return "camera_optical" -def _aruco_marker_object_points(marker_length_m: float) -> np.ndarray: +def _aruco_marker_object_points(marker_length_m: float) -> np.ndarray[Any, np.dtype[Any]]: """Corner order matches OpenCV ArUco / solvePnP convention (planar square, Z=0).""" h = marker_length_m / 2.0 return np.array( @@ -72,13 +73,13 @@ def _aruco_marker_object_points(marker_length_m: float) -> np.ndarray: def estimate_marker_pose( - corners_px: np.ndarray, + corners_px: np.ndarray[Any, np.dtype[Any]], marker_length_m: float, - camera_matrix: np.ndarray, - dist_coeffs: np.ndarray, + camera_matrix: np.ndarray[Any, np.dtype[Any]], + dist_coeffs: np.ndarray[Any, np.dtype[Any]], *, distortion_model: str | None = None, -) -> tuple[np.ndarray, np.ndarray] | None: +) -> tuple[np.ndarray[Any, np.dtype[Any]], np.ndarray[Any, np.dtype[Any]]] | None: """Return ``(rvec, tvec)`` for camera optical <- marker from undistorted solvePnP. For fisheye/equidistant intrinsics, corners are first undistorted into the @@ -88,17 +89,17 @@ def estimate_marker_pose( import cv2 obj = _aruco_marker_object_points(marker_length_m) - img: np.ndarray = corners_px.reshape(4, 1, 2).astype(np.float32) + img: np.ndarray[Any, np.dtype[Any]] = corners_px.reshape(4, 1, 2).astype(np.float32) if is_fisheye_model(distortion_model): d_flat = np.asarray(dist_coeffs, dtype=np.float64).reshape(-1) if d_flat.size < 4: raise ValueError( f"Fisheye/equidistant distortion model requires at least 4 coefficients; " - f"got {d_flat.size}. Check CameraInfo.D." + f"got {d_flat.size}. Check CameraInfo.d." ) d_fisheye = d_flat[:4].reshape(4, 1) img = cv2.fisheye.undistortPoints(img, camera_matrix, d_fisheye, P=camera_matrix) - solve_dist: np.ndarray = np.zeros((0, 1), dtype=np.float64) + solve_dist: np.ndarray[Any, np.dtype[Any]] = np.zeros((0, 1), dtype=np.float64) else: solve_dist = dist_coeffs ok, rvec, tvec = cv2.solvePnP( @@ -114,25 +115,24 @@ def estimate_marker_pose( def rvec_tvec_to_transform( - rvec: np.ndarray, - tvec: np.ndarray, + rvec: np.ndarray[Any, np.dtype[Any]], + tvec: np.ndarray[Any, np.dtype[Any]], *, - frame_id: str, + header: Header, child_frame_id: str, - ts: float, -) -> Transform: +) -> TransformStamped: """Build ``Transform`` for ``frame_id`` <- ``child_frame_id`` (camera <- marker).""" import cv2 rot_mat, _ = cv2.Rodrigues(rvec) - quat = Quaternion.from_rotation_matrix(rot_mat) + quat = quaternion_from_matrix(np.asarray(rot_mat, dtype=np.float64)) tx, ty, tz = tvec.reshape(3) - return Transform( - translation=Vector3(float(tx), float(ty), float(tz)), - rotation=quat, - frame_id=frame_id, + return TransformStamped( + header=header, child_frame_id=child_frame_id, - ts=ts, + transform=Transform( + translation=Vector3(x=float(tx), y=float(ty), z=float(tz)), rotation=quat + ), ) @@ -150,7 +150,9 @@ def create_aruco_detector( return cv2.aruco.ArucoDetector(dictionary, parameters) -def marker_corners_to_bbox(corners_px: np.ndarray) -> tuple[float, float, float, float]: +def marker_corners_to_bbox( + corners_px: np.ndarray[Any, np.dtype[Any]], +) -> tuple[float, float, float, float]: """Return the axis-aligned image bbox around a marker's four pixel corners.""" corners_2d = np.asarray(corners_px, dtype=np.float32).reshape(4, 2) xy_min = corners_2d.min(axis=0) @@ -159,12 +161,12 @@ def marker_corners_to_bbox(corners_px: np.ndarray) -> tuple[float, float, float, def marker_reprojection_error( - corners_px: np.ndarray, + corners_px: np.ndarray[Any, np.dtype[Any]], marker_length_m: float, - camera_matrix: np.ndarray, - dist_coeffs: np.ndarray, - rvec: np.ndarray, - tvec: np.ndarray, + camera_matrix: np.ndarray[Any, np.dtype[Any]], + dist_coeffs: np.ndarray[Any, np.dtype[Any]], + rvec: np.ndarray[Any, np.dtype[Any]], + tvec: np.ndarray[Any, np.dtype[Any]], *, distortion_model: str | None = None, ) -> float: @@ -175,7 +177,9 @@ def marker_reprojection_error( """ import cv2 - observed: np.ndarray = np.asarray(corners_px, dtype=np.float32).reshape(4, 1, 2) + observed: np.ndarray[Any, np.dtype[Any]] = np.asarray(corners_px, dtype=np.float32).reshape( + 4, 1, 2 + ) project_dist = dist_coeffs if is_fisheye_model(distortion_model): @@ -183,7 +187,7 @@ def marker_reprojection_error( if d_flat.size < 4: raise ValueError( f"Fisheye/equidistant distortion model requires at least 4 coefficients; " - f"got {d_flat.size}. Check CameraInfo.D." + f"got {d_flat.size}. Check CameraInfo.d." ) d_fisheye = d_flat[:4].reshape(4, 1) observed = cv2.fisheye.undistortPoints( diff --git a/dimos/perception/fiducial/marker_tf_module.py b/dimos/perception/fiducial/marker_tf_module.py index f5184ab9430..573245a21bc 100644 --- a/dimos/perception/fiducial/marker_tf_module.py +++ b/dimos/perception/fiducial/marker_tf_module.py @@ -40,17 +40,15 @@ from __future__ import annotations +from dimos_generated.geometry_msgs.msg import Quaternion, Transform, TransformStamped, Vector3 +from dimos_generated.std_msgs.msg import Header +from dimos_generated.tf2_msgs.msg import TFMessage +from dimos_generated.vision_msgs.msg import Detection3D, Detection3DArray from reactivex.disposable import Disposable from dimos.core.core import rpc from dimos.core.module import Module, ModuleConfig from dimos.core.stream import In, Out -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Transform import Transform -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.msgs.tf2_msgs.TFMessage import TFMessage -from dimos.msgs.vision_msgs.Detection3D import Detection3D -from dimos.msgs.vision_msgs.Detection3DArray import Detection3DArray from dimos.utils.logging_config import setup_logger logger = setup_logger() @@ -83,22 +81,16 @@ def _marker_child_frame(self, marker_id: str) -> str: return f"{p}/{name}" if p else name def _process_detections(self, detections: Detection3DArray) -> None: - if detections.detections_length == 0: + if not detections.detections: return - - marker_detections = detections.detections[: detections.detections_length] - if not marker_detections: - return - + marker_detections = detections.detections markers_parent = self._markers_parent_frame() - ts = detections.ts - out: list[Transform] = [ - Transform( - translation=Vector3(0.0, 0.0, 0.0), - rotation=Quaternion(0.0, 0.0, 0.0, 1.0), - frame_id=self.config.world_frame, + stamp = detections.header.stamp + out = [ + TransformStamped( + header=Header(stamp=stamp, frame_id=self.config.world_frame), child_frame_id=markers_parent, - ts=ts, + transform=Transform(rotation=Quaternion(w=1)), ) ] @@ -110,12 +102,15 @@ def _process_detections(self, detections: Detection3DArray) -> None: pose = detection.bbox.center out.append( - Transform( - translation=pose.position, - rotation=pose.orientation, - frame_id=markers_parent, + TransformStamped( + header=Header(stamp=stamp, frame_id=markers_parent), child_frame_id=self._marker_child_frame(marker_id), - ts=ts, + transform=Transform( + translation=Vector3( + x=pose.position.x, y=pose.position.y, z=pose.position.z + ), + rotation=pose.orientation, + ), ) ) @@ -123,7 +118,7 @@ def _process_detections(self, detections: Detection3DArray) -> None: # In that case, skip TF entirely rather than publishing only the # namespace anchor without any marker child frames. if len(out) > 1: - self.tf.publish(TFMessage(*out)) + self.tf.publish(TFMessage(transforms=out)) @staticmethod def _marker_id_from_detection(detection: Detection3D) -> str | None: @@ -131,7 +126,7 @@ def _marker_id_from_detection(detection: Detection3D) -> str | None: if marker_id: return marker_id - for result in detection.results[: detection.results_length]: + for result in detection.results: class_id = str(result.hypothesis.class_id).strip() if ":" not in class_id: continue diff --git a/dimos/perception/fiducial/marker_transformer.py b/dimos/perception/fiducial/marker_transformer.py index 0a0f7bfd036..a2b8a447644 100644 --- a/dimos/perception/fiducial/marker_transformer.py +++ b/dimos/perception/fiducial/marker_transformer.py @@ -32,17 +32,15 @@ from collections.abc import Callable import dataclasses import math -from typing import TYPE_CHECKING, Any, cast +from typing import TYPE_CHECKING, Any, TypeAlias, cast +from dimos_generated.geometry_msgs.msg import Quaternion, Transform, TransformStamped, Vector3 +from dimos_generated.sensor_msgs.msg import CameraInfo, Image +from dimos_generated.std_msgs.msg import Header +from dimos_generated.vision_msgs.msg import Detection3DArray import numpy as np from dimos.memory.transform import Transformer -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Transform import Transform -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo -from dimos.msgs.sensor_msgs.Image import Image -from dimos.msgs.vision_msgs.Detection3DArray import Detection3DArray from dimos.perception.detection.type.detection3d.imageDetections3D import ImageDetections3D from dimos.perception.detection.type.detection3d.marker import Detection3DMarker from dimos.perception.fiducial.marker_detect import ( @@ -64,7 +62,7 @@ logger = setup_logger() -CameraInfoSource = CameraInfo | Callable[[], CameraInfo | None] | None +CameraInfoSource: TypeAlias = CameraInfo | Callable[[], CameraInfo | None] | None def _camera_info_key(info: CameraInfo) -> tuple[Any, ...]: @@ -73,25 +71,24 @@ def _camera_info_key(info: CameraInfo) -> tuple[Any, ...]: info.width, info.height, info.distortion_model, - tuple(info.K), - tuple(info.D), + tuple(info.k), + tuple(info.d), ) def _pose_tuple_to_transform( pose: tuple[float, float, float, float, float, float, float], *, - frame_id: str, + header: Header, child_frame_id: str, - ts: float, -) -> Transform: +) -> TransformStamped: x, y, z, qx, qy, qz, qw = pose - return Transform( - translation=Vector3(x, y, z), - rotation=Quaternion(qx, qy, qz, qw), - frame_id=frame_id, + return TransformStamped( + header=header, child_frame_id=child_frame_id, - ts=ts, + transform=Transform( + translation=Vector3(x=x, y=y, z=z), rotation=Quaternion(x=qx, y=qy, z=qz, w=qw) + ), ) @@ -124,10 +121,10 @@ def _average_marker_pose( norm = math.sqrt(qsx * qsx + qsy * qsy + qsz * qsz + qsw * qsw) if norm < 1e-12: # Signs cancelled exactly — pathological, fall back to the hemisphere ref. - return (Vector3(cx, cy, cz), ref) + return (Vector3(x=cx, y=cy, z=cz), ref) return ( - Vector3(cx, cy, cz), - Quaternion(qsx / norm, qsy / norm, qsz / norm, qsw / norm), + Vector3(x=cx, y=cy, z=cz), + Quaternion(x=qsx / norm, y=qsy / norm, z=qsz / norm, w=qsw / norm), ) @@ -158,8 +155,8 @@ def __init__( self._detector = create_aruco_detector(aruco_dictionary, detect_inverted=detect_inverted) self._camera_info_key: tuple[Any, ...] | None = None self._resolved_camera_info: CameraInfo | None = None - self._cam_mtx: np.ndarray | None = None - self._dist: np.ndarray | None = None + self._cam_mtx: np.ndarray[Any, np.dtype[Any]] | None = None + self._dist: np.ndarray[Any, np.dtype[Any]] | None = None # Per marker_id sliding-window buffer of raw detections, used to emit # smoothed pose updates when ``smoothing_window > 0``. self._buffers: dict[int, TimestampedBufferCollection[Detection3DMarker]] = {} @@ -220,9 +217,8 @@ def __call__( optical_frame = camera_optical_frame_id(image, info) t_world_optical = _pose_tuple_to_transform( pose_tuple, - frame_id=self.world_frame, + header=Header(stamp=image.header.stamp, frame_id=self.world_frame), child_frame_id=optical_frame, - ts=obs.ts, ) detections = _detect_markers_in_image( @@ -264,12 +260,10 @@ def __call__( track_id = -1 det = dataclasses.replace(det, track_id=track_id) - yielded_pose = Transform( - translation=det.center, - rotation=det.orientation, - frame_id=self.world_frame, + yielded_pose = TransformStamped( + header=Header(stamp=image.header.stamp, frame_id=self.world_frame), child_frame_id=f"marker_{mid}", - ts=obs.ts, + transform=Transform(translation=det.center, rotation=det.orientation), ) yielded_det = det @@ -289,12 +283,10 @@ def __call__( yielded_det = dataclasses.replace( det, center=avg_center, orientation=avg_orient, transform=None ) - yielded_pose = Transform( - translation=avg_center, - rotation=avg_orient, - frame_id=self.world_frame, + yielded_pose = TransformStamped( + header=Header(stamp=image.header.stamp, frame_id=self.world_frame), child_frame_id=f"marker_{mid}", - ts=obs.ts, + transform=Transform(translation=avg_center, rotation=avg_orient), ) yield obs.derive(data=yielded_det, pose=yielded_pose).tag( @@ -401,12 +393,12 @@ def _source_pose( if detections and detections[0].transform is not None: transform = detections[0].transform return ( - transform.translation.x, - transform.translation.y, - transform.translation.z, - transform.rotation.x, - transform.rotation.y, - transform.rotation.z, - transform.rotation.w, + transform.transform.translation.x, + transform.transform.translation.y, + transform.transform.translation.z, + transform.transform.rotation.x, + transform.transform.rotation.y, + transform.transform.rotation.z, + transform.transform.rotation.w, ) return obs.pose diff --git a/dimos/perception/fiducial/test_fixture_verification.py b/dimos/perception/fiducial/test_fixture_verification.py index 295fd1ef375..35047537e7b 100644 --- a/dimos/perception/fiducial/test_fixture_verification.py +++ b/dimos/perception/fiducial/test_fixture_verification.py @@ -15,14 +15,15 @@ from __future__ import annotations import cv2 +from dimos_generated.geometry_msgs.msg import Quaternion, Transform, TransformStamped +from dimos_generated.std_msgs.msg import Header import numpy as np import pytest -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Transform import Transform -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo -from dimos.msgs.sensor_msgs.Image import Image +from dimos.msgs.camera_info import camera_info_from_intrinsics +from dimos.msgs.geometry import transform_matrix +from dimos.msgs.image import image_from_array +from dimos.msgs.time import time_from_seconds from dimos.perception.detection.type.detection3d.imageDetections3D import ImageDetections3D from dimos.perception.fiducial.fixture_verification import ( BoardLayout, @@ -159,24 +160,15 @@ def test_marker_tf_replay_synthetic_packed_board_publishes_twelve_markers( width, height = 1920, 1080 bgr = _synthetic_packed_apriltag_board_bgr(layout, width=width, height=height) ts = 10_000.0 - cam_info = CameraInfo.from_intrinsics( - 1400.0, - 1400.0, - width / 2.0, - height / 2.0, - width, - height, - frame_id="camera_optical", + header = Header(stamp=time_from_seconds(ts), frame_id="camera_optical") + cam_info = camera_info_from_intrinsics( + fx=1400, fy=1400, cx=width / 2, cy=height / 2, width=width, height=height, header=header ) - cam_info.ts = ts - - image = Image.from_opencv(bgr, frame_id="camera_optical", ts=ts) - world_T_optical = Transform( - translation=Vector3(0.0, 0.0, 0.0), - rotation=Quaternion(0.0, 0.0, 0.0, 1.0), - frame_id="world", + image = image_from_array(bgr, encoding="bgr8", header=header) + world_T_optical = TransformStamped( + header=Header(stamp=header.stamp, frame_id="world"), child_frame_id="camera_optical", - ts=ts, + transform=Transform(rotation=Quaternion(w=1)), ) detections = detect_markers_in_image( image, @@ -199,8 +191,9 @@ def test_marker_tf_replay_synthetic_packed_board_publishes_twelve_markers( for tag_id in range(12): tr = view.get(marker_parent, f"fixture/marker_{tag_id}", ts, 0.1) assert tr is not None, f"missing marker {tag_id}" - assert np.all(np.isfinite(tr.to_matrix())) + assert np.all(np.isfinite(transform_matrix(tr.transform))) finally: + view.dispose() mod.stop() diff --git a/dimos/perception/fiducial/test_generated_marker_detect.py b/dimos/perception/fiducial/test_generated_marker_detect.py new file mode 100644 index 00000000000..178185eed28 --- /dev/null +++ b/dimos/perception/fiducial/test_generated_marker_detect.py @@ -0,0 +1,55 @@ +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import cv2 +from dimos_generated.builtin_interfaces.msg import Time +from dimos_generated.geometry_msgs.msg import Quaternion, Transform, TransformStamped, Vector3 +from dimos_generated.sensor_msgs.msg import CameraInfo, Image +from dimos_generated.std_msgs.msg import Header +from dimos_generated.vision_msgs.msg import Detection3D +import numpy as np +import pytest + +from dimos.msgs.image import image_from_array +from dimos.perception.fiducial.marker_detect import detect_markers_in_image + + +def test_generated_image_detects_real_aruco_and_composes_pose(): + dictionary = cv2.aruco.getPredefinedDictionary(cv2.aruco.DICT_4X4_50) + pixels = np.full((400, 400), 255, dtype=np.uint8) + pixels[100:300, 100:300] = cv2.aruco.generateImageMarker(dictionary, 7, 200) + header = Header(frame_id="camera", stamp=Time(sec=1700000000, nanosec=123456789)) + image = image_from_array(pixels, encoding="mono8", header=header) + image = Image.decode(image.encode()) + camera = CameraInfo(width=400, height=400, k=[400, 0, 200, 0, 400, 200, 0, 0, 1]) + tf = TransformStamped( + header=Header(frame_id="world"), + child_frame_id="camera", + transform=Transform(translation=Vector3(x=2), rotation=Quaternion(w=1)), + ) + found = detect_markers_in_image( + image, + camera_info=camera, + world_T_optical=tf, + marker_length_m=0.2, + aruco_dictionary="DICT_4X4_50", + ) + assert len(found) == 1 + wire = Detection3D.decode(found[0].to_detection3d_msg().encode()) + assert wire.id == "7" + assert wire.results[0].hypothesis.class_id == "DICT_4X4_50:7" + assert wire.header.stamp == header.stamp + assert wire.header.frame_id == "world" + assert wire.bbox.center.position.x == pytest.approx(2, abs=0.01) + assert wire.bbox.center.position.z == pytest.approx(0.4, abs=0.01) diff --git a/dimos/perception/fiducial/test_helpers.py b/dimos/perception/fiducial/test_helpers.py index 12b4f890450..bf2449aa042 100644 --- a/dimos/perception/fiducial/test_helpers.py +++ b/dimos/perception/fiducial/test_helpers.py @@ -17,35 +17,31 @@ from __future__ import annotations import cv2 +from dimos_generated.geometry_msgs.msg import Quaternion, Transform, TransformStamped, Vector3 +from dimos_generated.sensor_msgs.msg import CameraInfo, Image +from dimos_generated.std_msgs.msg import Header import numpy as np -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Transform import Transform -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo -from dimos.msgs.sensor_msgs.Image import Image, ImageFormat +from dimos.msgs.image import image_from_array +from dimos.msgs.time import time_from_seconds def camera_info(ts: float = 10.0) -> CameraInfo: - info = CameraInfo.from_intrinsics( - fx=600.0, - fy=600.0, - cx=320.0, - cy=240.0, + info = CameraInfo( width=640, height=480, - frame_id="camera_optical", + k=[600, 0, 320, 0, 600, 240, 0, 0, 1], + header=Header(frame_id="camera_optical"), ) - info.ts = ts + info.header.stamp = time_from_seconds(ts) return info def blank_image(ts: float = 10.0) -> Image: - return Image( - data=np.full((480, 640, 3), 255, dtype=np.uint8), - format=ImageFormat.BGR, - frame_id="camera_optical", - ts=ts, + return image_from_array( + np.full((480, 640, 3), 255, dtype=np.uint8), + encoding="bgr8", + header=Header(frame_id="camera_optical", stamp=time_from_seconds(ts)), ) @@ -70,19 +66,19 @@ def synthetic_marker_image(marker_id: int = 7, ts: float = 10.0, inverted: bool y0 = (canvas.shape[0] - side_px) // 2 x0 = (canvas.shape[1] - side_px) // 2 canvas[y0 : y0 + side_px, x0 : x0 + side_px] = tile - return Image( - data=cv2.cvtColor(canvas, cv2.COLOR_GRAY2BGR), - format=ImageFormat.BGR, - frame_id="camera_optical", - ts=ts, + return image_from_array( + cv2.cvtColor(canvas, cv2.COLOR_GRAY2BGR), + encoding="bgr8", + header=Header(frame_id="camera_optical", stamp=time_from_seconds(ts)), ) -def world_T_optical(ts: float = 10.0) -> Transform: - return Transform( - translation=Vector3(1.0, 2.0, 3.0), - rotation=Quaternion(0.0, 0.0, 0.0, 1.0), - frame_id="world", +def world_T_optical(ts: float = 10.0) -> TransformStamped: + return TransformStamped( + header=Header(frame_id="world", stamp=time_from_seconds(ts)), child_frame_id="camera_optical", - ts=ts, + transform=Transform( + translation=Vector3(x=1.0, y=2.0, z=3.0), + rotation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0), + ), ) diff --git a/dimos/perception/fiducial/test_marker_detection_stream_module.py b/dimos/perception/fiducial/test_marker_detection_stream_module.py index 59779c5fc24..402ba2a789c 100644 --- a/dimos/perception/fiducial/test_marker_detection_stream_module.py +++ b/dimos/perception/fiducial/test_marker_detection_stream_module.py @@ -16,16 +16,17 @@ from typing import Any +from dimos_generated.geometry_msgs.msg import Quaternion, Transform, TransformStamped, Vector3 +from dimos_generated.sensor_msgs.msg import Image +from dimos_generated.std_msgs.msg import Header +from dimos_generated.vision_msgs.msg import Detection3DArray import numpy as np import pytest from dimos.memory.store.memory import MemoryStore +from dimos.memory.transform import SpeedLimit from dimos.memory.type.observation import Observation -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Transform import Transform -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.msgs.sensor_msgs.Image import Image -from dimos.msgs.vision_msgs.Detection3DArray import Detection3DArray +from dimos.msgs.time import time_from_seconds, to_seconds from dimos.perception.detection.type.detection3d.marker import Detection3DMarker from dimos.perception.fiducial.marker_detection_stream_module import MarkerDetectionStreamModule from dimos.perception.fiducial.marker_transformer import MarkersPerFrame @@ -43,19 +44,22 @@ def _marker(image: Image, marker_id: int) -> Detection3DMarker: class_id=marker_id, confidence=1.0, name="", - ts=image.ts, + ts=to_seconds(image.header.stamp), image=image, - center=Vector3(float(marker_id), 2.0, 3.0), - size=Vector3(0.18, 0.18, 0.0), - transform=Transform( - translation=Vector3(1.0, 2.0, 3.0), - rotation=Quaternion(0.0, 0.0, 0.0, 1.0), - frame_id="world", + center=Vector3(x=float(marker_id), y=2.0, z=3.0), + size=Vector3(x=0.18, y=0.18, z=0.0), + transform=TransformStamped( + header=Header( + frame_id="world", stamp=time_from_seconds(to_seconds(image.header.stamp)) + ), child_frame_id="camera_optical", - ts=image.ts, + transform=Transform( + translation=Vector3(x=1.0, y=2.0, z=3.0), + rotation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0), + ), ), frame_id="world", - orientation=Quaternion(0.0, 0.0, 0.0, 1.0), + orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0), marker_id=marker_id, corners_px=np.array( [[10.0, 20.0], [40.0, 20.0], [40.0, 50.0], [10.0, 50.0]], @@ -82,7 +86,7 @@ def _marker_obs( tags["marker_frame_index"] = marker_index return Observation( id=obs_id, - ts=image.ts, + ts=to_seconds(image.header.stamp), data_type=Detection3DMarker if marker is not None else type(None), pose=(1.0, 2.0, 3.0, 0.0, 0.0, 0.0, 1.0), tags=tags, @@ -120,15 +124,15 @@ def test_markers_per_frame_groups_markers_and_preserves_empty_frames() -> None: assert len(outputs) == 2 first = outputs[0].data assert first.header.frame_id == "world" - assert first.ts == pytest.approx(image.ts) - assert first.detections_length == 2 + assert to_seconds(first.header.stamp) == pytest.approx(to_seconds(image.header.stamp)) + assert len(first.detections) == 2 assert [det.id for det in first.detections] == ["7", "42"] assert outputs[0].pose_tuple == pytest.approx((1.0, 2.0, 3.0, 0.0, 0.0, 0.0, 1.0)) empty = outputs[1].data assert empty.header.frame_id == "world" - assert empty.ts == pytest.approx(empty_image.ts) - assert empty.detections_length == 0 + assert to_seconds(empty.header.stamp) == pytest.approx(to_seconds(empty_image.header.stamp)) + assert len(empty.detections) == 0 assert empty.detections == [] @@ -140,7 +144,7 @@ def test_marker_detection_stream_pipeline_outputs_arrays_for_marker_and_empty_fr module = MarkerDetectionStreamModule( marker_length_m=marker_length_m, - camera_info=camera_info(marker_image.ts), + camera_info=camera_info(to_seconds(marker_image.header.stamp)), quality_window_s=0.01, ) try: @@ -148,12 +152,12 @@ def test_marker_detection_stream_pipeline_outputs_arrays_for_marker_and_empty_fr stream = store.stream("color_image", Image) stream.append( marker_image, - ts=marker_image.ts, + ts=to_seconds(marker_image.header.stamp), pose=(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0), ) stream.append( empty_image, - ts=empty_image.ts, + ts=to_seconds(empty_image.header.stamp), pose=(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0), ) @@ -162,15 +166,17 @@ def test_marker_detection_stream_pipeline_outputs_arrays_for_marker_and_empty_fr module.stop() assert len(outputs) == 2 - assert outputs[0].detections_length == 1 + assert len(outputs[0].detections) == 1 assert outputs[0].detections[0].id == str(marker_id) assert outputs[0].detections[0].results[0].hypothesis.class_id == ( f"DICT_APRILTAG_36h11:{marker_id}" ) assert outputs[0].detections[0].bbox.size.x == pytest.approx(marker_length_m) - assert outputs[1].ts == pytest.approx(empty_image.ts) - assert outputs[1].detections_length == 0 + assert to_seconds(outputs[1].header.stamp) == pytest.approx( + to_seconds(empty_image.header.stamp) + ) + assert len(outputs[1].detections) == 0 assert outputs[1].detections == [] @@ -199,7 +205,7 @@ def run_pipeline(*, speed_limit_enabled: bool) -> list[Detection3DArray]: with MemoryStore() as store: stream = store.stream("color_image", Image) for image, pose in zip(images, poses, strict=True): - stream.append(image, ts=image.ts, pose=pose) + stream.append(image, ts=to_seconds(image.header.stamp), pose=pose) return [obs.data for obs in module.pipeline(stream).to_list()] finally: module.stop() @@ -207,21 +213,23 @@ def run_pipeline(*, speed_limit_enabled: bool) -> list[Detection3DArray]: disabled = run_pipeline(speed_limit_enabled=False) enabled = run_pipeline(speed_limit_enabled=True) - assert [msg.ts for msg in disabled] == pytest.approx([10.0, 11.0, 12.0]) - assert all(msg.detections_length == 0 for msg in disabled) - assert [msg.ts for msg in enabled] == pytest.approx([12.0]) - assert enabled[0].detections_length == 0 + assert [to_seconds(msg.header.stamp) for msg in disabled] == pytest.approx([10.0, 11.0, 12.0]) + assert all(len(msg.detections) == 0 for msg in disabled) + assert [to_seconds(msg.header.stamp) for msg in enabled] == pytest.approx([12.0]) + assert len(enabled[0].detections) == 0 -def test_append_image_with_pose_uses_camera_optical_tf_without_recomputing_pose() -> None: - image = blank_image(ts=12.0) - info = camera_info(image.ts) - t_world_optical = Transform( - translation=Vector3(4.0, 5.0, 6.0), - rotation=Quaternion(0.0, 0.0, 0.0, 1.0), - frame_id="world", +@pytest.mark.parametrize("timestamp", [0.0, 12.0]) +def test_append_image_with_pose_uses_camera_optical_tf_without_recomputing_pose(timestamp) -> None: + image = blank_image(ts=timestamp) + info = camera_info(to_seconds(image.header.stamp)) + t_world_optical = TransformStamped( + header=Header(frame_id="world", stamp=time_from_seconds(to_seconds(image.header.stamp))), child_frame_id="camera_optical", - ts=image.ts, + transform=Transform( + translation=Vector3(x=4.0, y=5.0, z=6.0), + rotation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0), + ), ) class FakeTf: @@ -234,7 +242,7 @@ def get( child_frame: str, time_point: float | None = None, time_tolerance: float | None = None, - ) -> Transform: + ) -> TransformStamped: self.calls.append((parent_frame, child_frame, time_point, time_tolerance)) return t_world_optical @@ -258,10 +266,10 @@ def dispose(self) -> None: finally: module.stop() - assert fake_tf.calls == [("world", "camera_optical", image.ts, 0.25)] + assert fake_tf.calls == [("world", "camera_optical", to_seconds(image.header.stamp), 0.25)] assert len(observations) == 1 assert observations[0].data is image - assert observations[0].ts == pytest.approx(image.ts) + assert observations[0].ts == pytest.approx(to_seconds(image.header.stamp)) assert observations[0].pose_tuple == pytest.approx((4.0, 5.0, 6.0, 0.0, 0.0, 0.0, 1.0)) @@ -289,7 +297,9 @@ def dispose(self) -> None: pass missing_tf = MissingTf() - module = MarkerDetectionStreamModule(marker_length_m=0.18, camera_info=camera_info(image.ts)) + module = MarkerDetectionStreamModule( + marker_length_m=0.18, camera_info=camera_info(to_seconds(image.header.stamp)) + ) module._tf = missing_tf try: with MemoryStore() as store: @@ -300,3 +310,14 @@ def dispose(self) -> None: module.stop() assert missing_tf.calls == 1 + + +def test_generated_speed_limit_handles_quaternion_sign_and_rotation(): + image = blank_image() + poses = [(0, 0, 0, 0, 0, 0, 1), (0, 0, 0, 0, 0, 0, -1), (0, 0, 0, 0, 0, 1, 0)] + observations = [ + Observation(id=i, ts=float(i), data_type=Image, _data=image, pose=pose) + for i, pose in enumerate(poses) + ] + accepted = list(SpeedLimit(max_mps=1, max_dps=15)(iter(observations))) + assert [obs.id for obs in accepted] == [1] diff --git a/dimos/perception/fiducial/test_marker_pose.py b/dimos/perception/fiducial/test_marker_pose.py index 78c22751805..31f64f6121e 100644 --- a/dimos/perception/fiducial/test_marker_pose.py +++ b/dimos/perception/fiducial/test_marker_pose.py @@ -13,10 +13,11 @@ # limitations under the License. import cv2 +from dimos_generated.sensor_msgs.msg import CameraInfo +from dimos_generated.std_msgs.msg import Header import numpy as np -from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo -from dimos.msgs.sensor_msgs.Image import Image, ImageFormat +from dimos.msgs.image import image_from_array from dimos.perception.fiducial.marker_pose import ( camera_optical_frame_id, estimate_marker_pose, @@ -26,25 +27,14 @@ def test_camera_optical_frame_id_resolution() -> None: - ts = 1.0 - fx, fy, cx, cy = 600.0, 600.0, 320.0, 240.0 - info_named = CameraInfo.from_intrinsics(fx, fy, cx, cy, 640, 480, frame_id="cam_info_optical") - info_named.ts = ts - info_empty = CameraInfo.from_intrinsics(fx, fy, cx, cy, 640, 480) - info_empty.ts = ts - img_custom = Image( - data=np.zeros((480, 640, 3), dtype=np.uint8), - format=ImageFormat.BGR, - ts=ts, - frame_id="custom_optical", + info_named = CameraInfo(header=Header(frame_id="cam_info_optical")) + info_empty = CameraInfo() + pixels = np.zeros((480, 640, 3), dtype=np.uint8) + img_custom = image_from_array(pixels, encoding="bgr8", header=Header(frame_id="custom_optical")) + img_whitespace = image_from_array( + pixels, encoding="bgr8", header=Header(frame_id=" custom_optical ") ) - img_whitespace = Image( - data=np.zeros((480, 640, 3), dtype=np.uint8), - format=ImageFormat.BGR, - ts=ts, - frame_id=" custom_optical ", - ) - img_empty = Image(data=np.zeros((480, 640, 3), dtype=np.uint8), format=ImageFormat.BGR, ts=ts) + img_empty = image_from_array(pixels, encoding="bgr8") assert camera_optical_frame_id(img_custom, info_named) == "custom_optical" assert camera_optical_frame_id(img_whitespace, info_named) == "custom_optical" diff --git a/dimos/perception/fiducial/test_marker_tf_integration.py b/dimos/perception/fiducial/test_marker_tf_integration.py index 458edd22e67..43fa7fbe453 100644 --- a/dimos/perception/fiducial/test_marker_tf_integration.py +++ b/dimos/perception/fiducial/test_marker_tf_integration.py @@ -19,25 +19,28 @@ import time import uuid -from dimos_lcm.vision_msgs import BoundingBox3D, ObjectHypothesis, ObjectHypothesisWithPose +from dimos_generated.geometry_msgs.msg import Point, Pose, Quaternion, Vector3 +from dimos_generated.std_msgs.msg import Header +from dimos_generated.tf2_msgs.msg import TFMessage +from dimos_generated.vision_msgs.msg import ( + BoundingBox3D, + Detection3D, + Detection3DArray, + ObjectHypothesis, + ObjectHypothesisWithPose, +) import pytest from dimos.core.transport import LCMTransport from dimos.core.transport_factory import make_transport -from dimos.msgs.geometry_msgs.Pose import Pose -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.msgs.std_msgs.Header import Header -from dimos.msgs.tf2_msgs.TFMessage import TFMessage -from dimos.msgs.vision_msgs.Detection3D import Detection3D -from dimos.msgs.vision_msgs.Detection3DArray import Detection3DArray +from dimos.msgs.time import time_from_seconds from dimos.perception.fiducial.marker_tf_module import MarkerTfModule from dimos.protocol.tf.tf import TF def _marker_detection_array(ts: float) -> Detection3DArray: det = Detection3D() - det.header = Header(ts, "world") + det.header = Header(stamp=time_from_seconds(ts), frame_id="world") det.id = "11" det.results = [ ObjectHypothesisWithPose( @@ -47,18 +50,16 @@ def _marker_detection_array(ts: float) -> Detection3DArray: ) ) ] - det.results_length = len(det.results) det.bbox = BoundingBox3D( center=Pose( - position=Vector3(0.5, -0.2, 1.25), - orientation=Quaternion(0.0, 0.0, 0.0, 1.0), + position=Point(x=0.5, y=-0.2, z=1.25), + orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0), ), - size=Vector3(0.18, 0.18, 0.0), + size=Vector3(x=0.18, y=0.18, z=0.0), ) return Detection3DArray( - header=Header(ts, "world"), + header=Header(stamp=time_from_seconds(ts), frame_id="world"), detections=[det], - detections_length=1, ) @@ -89,13 +90,13 @@ def test_marker_tf_consumes_detection_array_over_lcm() -> None: assert w_markers is not None, "Timed out waiting for world -> marker_tf/markers" assert w_marker is not None, "Timed out waiting for world -> marker_tf/marker_11" - assert w_markers.frame_id == "world" + assert w_markers.header.frame_id == "world" assert w_markers.child_frame_id == "marker_tf/markers" - assert w_marker.frame_id == "world" + assert w_marker.header.frame_id == "world" assert w_marker.child_frame_id == "marker_tf/marker_11" - assert w_marker.translation.x == pytest.approx(0.5) - assert w_marker.translation.y == pytest.approx(-0.2) - assert w_marker.translation.z == pytest.approx(1.25) + assert w_marker.transform.translation.x == pytest.approx(0.5) + assert w_marker.transform.translation.y == pytest.approx(-0.2) + assert w_marker.transform.translation.z == pytest.approx(1.25) finally: host_tf.dispose() host_transport.stop() diff --git a/dimos/perception/fiducial/test_marker_tf_module.py b/dimos/perception/fiducial/test_marker_tf_module.py index 90d2c10ff36..2c4aeb3c261 100644 --- a/dimos/perception/fiducial/test_marker_tf_module.py +++ b/dimos/perception/fiducial/test_marker_tf_module.py @@ -14,15 +14,20 @@ from unittest.mock import patch -from dimos_lcm.vision_msgs import BoundingBox3D, ObjectHypothesis, ObjectHypothesisWithPose +from dimos_generated.geometry_msgs.msg import Point, Pose, Quaternion, Vector3 +from dimos_generated.std_msgs.msg import Header +from dimos_generated.tf2_msgs.msg import TFMessage +from dimos_generated.vision_msgs.msg import ( + BoundingBox3D, + Detection3D, + Detection3DArray, + ObjectHypothesis, + ObjectHypothesisWithPose, +) import pytest -from dimos.msgs.geometry_msgs.Pose import Pose -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.msgs.std_msgs.Header import Header -from dimos.msgs.vision_msgs.Detection3D import Detection3D -from dimos.msgs.vision_msgs.Detection3DArray import Detection3DArray +from dimos.msgs.geometry import quaternion_from_euler +from dimos.msgs.time import time_from_seconds from dimos.perception.fiducial.marker_tf_module import MarkerTfModule from dimos.protocol.tf.tf import TF @@ -32,16 +37,16 @@ def _detection_array( ts: float, marker_id: str = "0", class_id: str = "DICT_APRILTAG_36h11:0", - center: Vector3 | None = None, + center: Point | None = None, orientation: Quaternion | None = None, ) -> Detection3DArray: if center is None: - center = Vector3(1.2, -0.3, 0.8) + center = Point(x=1.2, y=-0.3, z=0.8) if orientation is None: - orientation = Quaternion(0.0, 0.0, 0.0, 1.0) + orientation = Quaternion(x=0.0, y=0.0, z=0.0, w=1.0) det = Detection3D() - det.header = Header(ts, "world") + det.header = Header(stamp=time_from_seconds(ts), frame_id="world") det.id = marker_id det.results = [ ObjectHypothesisWithPose( @@ -51,25 +56,23 @@ def _detection_array( ) ) ] - det.results_length = len(det.results) det.bbox = BoundingBox3D( center=Pose( position=center, orientation=orientation, ), - size=Vector3(0.18, 0.18, 0.0), + size=Vector3(x=0.18, y=0.18, z=0.0), ) return Detection3DArray( - header=Header(ts, "world"), + header=Header(stamp=time_from_seconds(ts), frame_id="world"), detections=[det], - detections_length=1, ) def test_marker_tf_module_publishes_world_markers_chain() -> None: ts = 1_000_000.0 - center = Vector3(1.3, -0.2, 0.4) - orientation = Quaternion(0.1, 0.2, 0.3, 0.9) + center = Point(x=1.3, y=-0.2, z=0.4) + orientation = quaternion_from_euler(0.1, 0.2, 0.3) mod = MarkerTfModule() view = TF(mod.tf) @@ -86,21 +89,22 @@ def test_marker_tf_module_publishes_world_markers_chain() -> None: wm = view.get("world", "markers", ts, 1.0) assert wm is not None - assert abs(wm.translation.x) < 1e-6 - assert abs(wm.translation.y) < 1e-6 - assert abs(wm.translation.z) < 1e-6 + assert abs(wm.transform.translation.x) < 1e-6 + assert abs(wm.transform.translation.y) < 1e-6 + assert abs(wm.transform.translation.z) < 1e-6 w_m7 = view.get("world", "marker_7", ts, 1.0) assert w_m7 is not None - assert w_m7.translation.x == pytest.approx(center.x) - assert w_m7.translation.y == pytest.approx(center.y) - assert w_m7.translation.z == pytest.approx(center.z) - assert w_m7.rotation.x == pytest.approx(orientation.x) - assert w_m7.rotation.y == pytest.approx(orientation.y) - assert w_m7.rotation.z == pytest.approx(orientation.z) - assert w_m7.rotation.w == pytest.approx(orientation.w) + assert w_m7.transform.translation.x == pytest.approx(center.x) + assert w_m7.transform.translation.y == pytest.approx(center.y) + assert w_m7.transform.translation.z == pytest.approx(center.z) + assert w_m7.transform.rotation.x == pytest.approx(orientation.x) + assert w_m7.transform.rotation.y == pytest.approx(orientation.y) + assert w_m7.transform.rotation.z == pytest.approx(orientation.z) + assert w_m7.transform.rotation.w == pytest.approx(orientation.w) assert view.get("world", "marker_99", ts, 0.1) is None finally: + view.dispose() mod.stop() @@ -116,6 +120,7 @@ def test_marker_tf_parses_class_id_when_detection_id_empty() -> None: assert view.get("world", "marker_42", ts, 1.0) is not None finally: + view.dispose() mod.stop() @@ -126,14 +131,14 @@ def test_marker_tf_empty_array_skips_publication() -> None: try: mod._process_detections( Detection3DArray( - header=Header(ts, "world"), + header=Header(stamp=time_from_seconds(ts), frame_id="world"), detections=[], - detections_length=0, ) ) assert view.get("world", "markers", ts, 0.1) is None finally: + view.dispose() mod.stop() @@ -146,6 +151,7 @@ def test_marker_tf_non_empty_array_without_marker_id_skips_publication() -> None assert view.get("world", "markers", ts, 0.1) is None finally: + view.dispose() mod.stop() @@ -160,6 +166,7 @@ def test_marker_tf_does_not_recompute_marker_pose() -> None: mock_estimate.assert_not_called() assert view.get("world", "marker_4", ts, 1.0) is not None finally: + view.dispose() mod.stop() @@ -174,4 +181,22 @@ def test_marker_namespace_prefix_child_frames() -> None: assert view.get("world", "r1/markers", ts, 1.0) is not None assert view.get("world", "r1/marker_0", ts, 1.0) is not None finally: + view.dispose() mod.stop() + + +def test_marker_tf_wire_preserves_exact_timestamp(): + module = MarkerTfModule() + outputs = [] + unsubscribe = module.tf.subscribe(lambda msg: outputs.append(TFMessage.decode(msg.encode()))) + detections = _detection_array(ts=0) + detections.header.stamp.sec = 1700000000 + detections.header.stamp.nanosec = 123456789 + try: + module._process_detections(detections) + assert len(outputs) == 1 + assert len(outputs[0].transforms) == 2 + assert all(t.header.stamp == detections.header.stamp for t in outputs[0].transforms) + finally: + unsubscribe() + module.stop() diff --git a/dimos/perception/fiducial/test_marker_transformer.py b/dimos/perception/fiducial/test_marker_transformer.py index 16830e35561..cef20524a59 100644 --- a/dimos/perception/fiducial/test_marker_transformer.py +++ b/dimos/perception/fiducial/test_marker_transformer.py @@ -12,12 +12,14 @@ # See the License for the specific language governing permissions and # limitations under the License. +from dimos_generated.sensor_msgs.msg import CameraInfo, Image +from dimos_generated.std_msgs.msg import Header import numpy as np import pytest from dimos.memory.type.observation import Observation -from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo -from dimos.msgs.sensor_msgs.Image import Image, ImageFormat +from dimos.msgs.image import image_from_array +from dimos.msgs.time import time_from_seconds, to_seconds from dimos.perception.fiducial.marker_detect import detect_markers_in_image from dimos.perception.fiducial.marker_transformer import DetectMarkers from dimos.perception.fiducial.test_helpers import ( @@ -31,12 +33,12 @@ def test_detect_markers_in_image_builds_rich_marker_detection() -> None: marker_id = 7 marker_length_m = 0.18 image = synthetic_marker_image(marker_id) - info = camera_info(image.ts) + info = camera_info(to_seconds(image.header.stamp)) detections = detect_markers_in_image( image, camera_info=info, - world_T_optical=world_T_optical(image.ts), + world_T_optical=world_T_optical(to_seconds(image.header.stamp)), marker_length_m=marker_length_m, aruco_dictionary="DICT_APRILTAG_36h11", ) @@ -65,11 +67,10 @@ def test_detect_markers_in_image_builds_rich_marker_detection() -> None: def test_detect_markers_in_image_returns_empty_for_no_marker_frame() -> None: ts = 11.0 - image = Image( - data=np.full((480, 640, 3), 255, dtype=np.uint8), - format=ImageFormat.BGR, - frame_id="camera_optical", - ts=ts, + image = image_from_array( + np.full((480, 640, 3), 255, dtype=np.uint8), + encoding="bgr8", + header=Header(frame_id="camera_optical", stamp=time_from_seconds(ts)), ) detections = detect_markers_in_image( @@ -86,8 +87,8 @@ def test_detect_markers_in_image_returns_empty_for_no_marker_frame() -> None: def test_detect_markers_in_image_needs_detect_inverted_for_negative_tags() -> None: image = synthetic_marker_image(7, inverted=True) kwargs = { - "camera_info": camera_info(image.ts), - "world_T_optical": world_T_optical(image.ts), + "camera_info": camera_info(to_seconds(image.header.stamp)), + "world_T_optical": world_T_optical(to_seconds(image.header.stamp)), "marker_length_m": 0.18, "aruco_dictionary": "DICT_APRILTAG_36h11", } @@ -104,13 +105,13 @@ def test_detect_markers_transformer_preserves_observation_context_and_tags() -> image = synthetic_marker_image(marker_id, ts=12.0) obs = Observation[Image]( id=42, - ts=image.ts, + ts=to_seconds(image.header.stamp), data_type=Image, pose=(1.0, 2.0, 3.0, 0.0, 0.0, 0.0, 1.0), _data=image, ) transformer = DetectMarkers( - camera_info=camera_info(image.ts), + camera_info=camera_info(to_seconds(image.header.stamp)), marker_length_m=0.18, aruco_dictionary="DICT_APRILTAG_36h11", ) @@ -130,21 +131,20 @@ def test_detect_markers_transformer_preserves_observation_context_and_tags() -> def test_detect_markers_transformer_can_emit_empty_frame_sentinel() -> None: - image = Image( - data=np.full((480, 640, 3), 255, dtype=np.uint8), - format=ImageFormat.BGR, - frame_id="camera_optical", - ts=13.0, + image = image_from_array( + np.full((480, 640, 3), 255, dtype=np.uint8), + encoding="bgr8", + header=Header(frame_id="camera_optical", stamp=time_from_seconds(13.0)), ) obs = Observation[Image]( id=43, - ts=image.ts, + ts=to_seconds(image.header.stamp), data_type=Image, pose=(1.0, 2.0, 3.0, 0.0, 0.0, 0.0, 1.0), _data=image, ) transformer = DetectMarkers( - camera_info=camera_info(image.ts), + camera_info=camera_info(to_seconds(image.header.stamp)), marker_length_m=0.18, aruco_dictionary="DICT_APRILTAG_36h11", emit_empty_frames=True, @@ -165,7 +165,7 @@ def test_detect_markers_transformer_uses_callablecamera_info_source() -> None: image = synthetic_marker_image(marker_id=7, ts=14.0) obs = Observation[Image]( id=44, - ts=image.ts, + ts=to_seconds(image.header.stamp), data_type=Image, pose=(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0), _data=image, @@ -180,7 +180,7 @@ def test_detect_markers_transformer_uses_callablecamera_info_source() -> None: assert list(transformer(iter([obs]))) == [] - latest_info = camera_info(image.ts) + latest_info = camera_info(to_seconds(image.header.stamp)) results = list(transformer(iter([obs]))) assert len(results) == 1 @@ -193,24 +193,23 @@ def test_detect_markers_rebuilds_intrinsics_without_resetting_smoothing_track() marker_length_m = 0.18 image_a = synthetic_marker_image(marker_id=marker_id, ts=15.0) image_b = synthetic_marker_image(marker_id=marker_id, ts=15.2) - info_a = camera_info(image_a.ts) - info_b = camera_info(image_b.ts) - info_b.K = info_b.K.copy() - info_b.P = info_b.P.copy() - info_b.K[0] = info_b.K[4] = 900.0 - info_b.P[0] = info_b.P[5] = 900.0 + info_a = camera_info(to_seconds(image_a.header.stamp)) + info_b = camera_info(to_seconds(image_b.header.stamp)) + + info_b.k[0] = info_b.k[4] = 900.0 + info_b.p[0] = info_b.p[5] = 900.0 latest_info: CameraInfo | None = info_a obs_a = Observation[Image]( id=45, - ts=image_a.ts, + ts=to_seconds(image_a.header.stamp), data_type=Image, pose=(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0), _data=image_a, ) obs_b = Observation[Image]( id=46, - ts=image_b.ts, + ts=to_seconds(image_b.header.stamp), data_type=Image, pose=(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0), _data=image_b, diff --git a/dimos/perception/memory/dandetect.py b/dimos/perception/memory/dandetect.py index 081cb52e610..096093eb186 100644 --- a/dimos/perception/memory/dandetect.py +++ b/dimos/perception/memory/dandetect.py @@ -122,7 +122,7 @@ def embed( tf_tolerance=tf_tolerance, ) - from dimos.msgs.sensor_msgs.Image import Image + from dimos_generated.sensor_msgs.msg import Image tf = StreamTF.from_store(store) if tf is None: diff --git a/dimos/perception/memory/gates.py b/dimos/perception/memory/gates.py index 99ade4ccdd6..fcf813e6959 100644 --- a/dimos/perception/memory/gates.py +++ b/dimos/perception/memory/gates.py @@ -38,10 +38,14 @@ import numpy as np +from dimos.msgs.geometry import inverse_transform, point_distance, pose_from_transform +from dimos.msgs.image import image_to_bgr + if TYPE_CHECKING: + from dimos_generated.geometry_msgs.msg import PoseStamped + from dimos_generated.sensor_msgs.msg import Image + from dimos.memory.type.observation import Observation - from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped - from dimos.msgs.sensor_msgs.Image import Image from dimos.protocol.tf.tf import TFLookup OPTICAL_FRAME = "camera_color_optical_frame" @@ -69,7 +73,7 @@ def camera_pose( ) -> PoseStamped | None: """World pose of the camera optical frame at ts - it rides the wrist.""" transform = tf.get(optical_frame, world_frame, ts, tolerance) - return (-transform).to_pose() if transform is not None else None + return pose_from_transform(inverse_transform(transform)) if transform is not None else None def camera_speed( @@ -85,7 +89,7 @@ def camera_speed( b = camera_pose(tf, ts + dt, optical_frame, world_frame, tolerance) if a is None or b is None: return None - return float((b.position - a.position).magnitude() / (2 * dt)) + return point_distance(b.pose.position, a.pose.position) / (2 * dt) def camera_still( @@ -160,7 +164,7 @@ def _gray_small(store: Any, ts: float, gray: dict[float, np.ndarray | None]) -> except LookupError: frame = None if frame is not None: - img = frame.to_opencv() + img = image_to_bgr(frame) h = int(img.shape[0] * DIFF_WIDTH / img.shape[1]) small = cv2.resize(img, (DIFF_WIDTH, h), interpolation=cv2.INTER_AREA) small_gray = cv2.cvtColor(small, cv2.COLOR_BGR2GRAY) diff --git a/dimos/perception/memory/inventory.py b/dimos/perception/memory/inventory.py index f256701ee19..276585c1161 100644 --- a/dimos/perception/memory/inventory.py +++ b/dimos/perception/memory/inventory.py @@ -39,6 +39,8 @@ import numpy as np from dimos.memory.tf import StreamTF +from dimos.msgs.geometry import transform_matrix +from dimos.msgs.pointcloud import concatenate_clouds, pointcloud_xyz, select_points from dimos.perception.detection.type.detection3d.imageDetections3DPC import ImageDetections3DPC from dimos.perception.memory import gates from dimos.perception.memory.gates import ( @@ -58,7 +60,8 @@ from dimos.utils.logging_config import setup_logger if TYPE_CHECKING: - from dimos_lcm.sensor_msgs import CameraInfo + from dimos_generated.geometry_msgs.msg import TransformStamped + from dimos_generated.sensor_msgs.msg import CameraInfo from dimos.models.segmentation.edge_tam import EdgeTAMImageSegmenter from dimos.perception.detection.detectors.owlv2 import Owlv2Detector @@ -132,7 +135,7 @@ class _Track: def add(self, obs: SupportObservation, frame_key: float) -> None: self.members.append(obs) self.frame_ts.add(frame_key) - points = np.asarray(obs.cloud.pointcloud.points) + points = pointcloud_xyz(obs.cloud) self.support_pts = np.vstack([self.support_pts, points[:: max(1, len(points) // 400)]]) @property @@ -189,7 +192,7 @@ def _proposal_passes_2d(det: Detection2DSeg, image_area: float, policy: Inventor def _split_oversized( points: np.ndarray, plane: SupportPlane | None, policy: InventoryPolicy ) -> list[np.ndarray]: - """Re-segment a mask-bled cloud by 3D connectivity. + """Return source-point masks for a mask-bled cloud split by 3D connectivity. Automatic masks occasionally bleed across an object onto the table and its neighbors; the lifted cloud then violates single-object bounds. The @@ -199,40 +202,45 @@ def _split_oversized( """ extent = points.max(axis=0) - points.min(axis=0) if float(extent.max()) <= SPLIT_EXTENT_M and float(extent[2]) <= SPLIT_HEIGHT_M: - return [points] + return [np.ones(len(points), dtype=np.bool_)] if plane is None: - return [points] + return [np.ones(len(points), dtype=np.bool_)] heights = plane.height_above(points) if float((np.abs(heights) <= 0.003).mean()) < 0.15: # No appreciable support-surface content: this is one oversized body, # not a mask that bled across the table. Leave it to the extent cap. - return [points] + return [np.ones(len(points), dtype=np.bool_)] above = heights > 0.002 - body = points[above] if above.sum() >= policy.min_depth_points else points + body_mask = ( + above if above.sum() >= policy.min_depth_points else np.ones(len(points), dtype=np.bool_) + ) + body = points[body_mask] import open3d as o3d cloud = o3d.geometry.PointCloud() cloud.points = o3d.utility.Vector3dVector(body) labels = np.asarray(cloud.cluster_dbscan(eps=SPLIT_EPS_M, min_points=20)) - clusters = [ - body[labels == label] - for label in range(labels.max() + 1) - if (labels == label).sum() >= policy.min_depth_points - ] - return clusters if clusters else [body] + clusters = [] + for label in range(labels.max() + 1): + chosen = labels == label + if chosen.sum() >= policy.min_depth_points: + mask = np.zeros(len(points), dtype=np.bool_) + mask[body_mask] = chosen + clusters.append(mask) + return clusters if clusters else [body_mask] def _pixel_bbox( - points: np.ndarray, camera_info: CameraInfo, transform: Any + points: np.ndarray, camera_info: CameraInfo, transform: TransformStamped ) -> tuple[float, float, float, float]: """Project world points back into the frame for a sub-observation's bbox.""" - matrix = transform.to_matrix() + matrix = transform_matrix(transform.transform) optical = (matrix[:3, :3] @ points.T).T + matrix[:3, 3] z = np.maximum(optical[:, 2], 1e-6) - fx, fy = camera_info.K[0], camera_info.K[4] - cx, cy = camera_info.K[2], camera_info.K[5] + fx, fy = camera_info.k[0], camera_info.k[4] + cx, cy = camera_info.k[2], camera_info.k[5] u = fx * optical[:, 0] / z + cx v = fy * optical[:, 1] / z + cy return (float(u.min()), float(v.min()), float(u.max()), float(v.max())) @@ -252,8 +260,6 @@ def _lift_frame( plane: SupportPlane | None = None, ) -> tuple[list[SupportObservation], list[Detection2DSeg]]: """Depth-lift accepted proposals of one frame; returns (grounded, ungrounded).""" - from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 - depth = gates.depth_at(store, obs_ts) transform = tf.get(optical_frame, world_frame, obs_ts, tf_tolerance) if depth is None or transform is None: @@ -266,12 +272,16 @@ def _lift_frame( lifted_by_track = {det3d.track_id: det3d for det3d in lifted} for det2d in detections_2d: det3d = lifted_by_track.get(det2d.track_id) - if det3d is None or len(det3d.pointcloud) < policy.min_depth_points: + if ( + det3d is None + or det3d.pointcloud.width * det3d.pointcloud.height < policy.min_depth_points + ): ungrounded.append(det2d) continue - points = np.asarray(det3d.pointcloud.pointcloud.points) + points = pointcloud_xyz(det3d.pointcloud) mask_area = int((det2d.mask > 0).sum()) - for piece in _split_oversized(points, plane, policy): + for mask in _split_oversized(points, plane, policy): + piece = points[mask] aabb_min, aabb_max = piece.min(axis=0), piece.max(axis=0) extent = aabb_max - aabb_min if float(extent.max()) > policy.max_object_extent_m: @@ -283,9 +293,7 @@ def _lift_frame( grounded.append( SupportObservation( ts=obs_ts, - cloud=det3d.pointcloud - if whole - else PointCloud2.from_numpy(piece, frame_id="world", timestamp=obs_ts), + cloud=det3d.pointcloud if whole else select_points(det3d.pointcloud, mask), centroid=piece.mean(axis=0), aabb_min=aabb_min, aabb_max=aabb_max, @@ -301,11 +309,11 @@ def _lift_frame( def _in_scope(obs: SupportObservation, plane: SupportPlane | None, policy: InventoryPolicy) -> bool: """Support-plane scope: in a band above the plane, footprint on the workspace.""" if policy.in_scope is not None: - points = np.asarray(obs.cloud.pointcloud.points) + points = pointcloud_xyz(obs.cloud) return bool(policy.in_scope(points)) if plane is None: return True - points = np.asarray(obs.cloud.pointcloud.points) + points = pointcloud_xyz(obs.cloud) heights = plane.height_above(points) low, high = float(np.quantile(heights, 0.05)), float(np.quantile(heights, 0.95)) band_lo, band_hi = policy.band_above_plane_m @@ -337,8 +345,8 @@ def _cloud_gap(a: SupportObservation, b: SupportObservation) -> float: return np.inf from scipy.spatial import cKDTree - pa = np.asarray(a.cloud.pointcloud.points) - pb = np.asarray(b.cloud.pointcloud.points) + pa = pointcloud_xyz(a.cloud) + pb = pointcloud_xyz(b.cloud) pa = pa[:: max(1, len(pa) // 800)] pb = pb[:: max(1, len(pb) // 800)] distances, _ = cKDTree(pa).query(pb, k=1) @@ -358,8 +366,8 @@ def _support_explained(points: np.ndarray, support: np.ndarray, pad: float) -> f def _absorb_into(target: SupportObservation, obs: SupportObservation) -> None: target.aabb_min = np.minimum(target.aabb_min, obs.aabb_min) target.aabb_max = np.maximum(target.aabb_max, obs.aabb_max) - target.cloud = target.cloud + obs.cloud - points = np.asarray(target.cloud.pointcloud.points) + target.cloud = concatenate_clouds(target.cloud, obs.cloud) + points = pointcloud_xyz(target.cloud) target.centroid = points.mean(axis=0) target.n_points = len(points) target.mask_area_px = max(target.mask_area_px, obs.mask_area_px) @@ -423,7 +431,7 @@ def _associate( cost = np.full((len(observations), len(tracks)), forbidden) for i, obs in enumerate(observations): - obs_points = np.asarray(obs.cloud.pointcloud.points) + obs_points = pointcloud_xyz(obs.cloud) for j, track in enumerate(tracks): distance = float(np.linalg.norm(obs.centroid - track.centroid)) if distance > policy.search_radius_m: @@ -862,7 +870,9 @@ def inventory( f"skip=no_pose {perf_counter() - t_frame:.1f}s" ) continue - camera_position = np.array([pose.position.x, pose.position.y, pose.position.z]) + camera_position = np.array( + [pose.pose.position.x, pose.pose.position.y, pose.pose.position.z] + ) for j, det in enumerate(accepted): det.track_id = j diff --git a/dimos/perception/memory/localize.py b/dimos/perception/memory/localize.py index e6182fd8d65..bea10007d3d 100644 --- a/dimos/perception/memory/localize.py +++ b/dimos/perception/memory/localize.py @@ -38,6 +38,7 @@ from dimos.memory.embed import EmbedImages from dimos.memory.tf import StreamTF from dimos.memory.transform import throttle +from dimos.msgs.pointcloud import pointcloud_xyz from dimos.perception.detection.project import sees from dimos.perception.detection.type.detection2d.imageDetections2D import ImageDetections2D from dimos.perception.detection.type.detection3d.imageDetections3DPC import ImageDetections3DPC @@ -47,7 +48,7 @@ from dimos.utils.logging_config import setup_logger if TYPE_CHECKING: - from dimos_lcm.sensor_msgs import CameraInfo + from dimos_generated.sensor_msgs.msg import CameraInfo from dimos.memory.stream import Stream from dimos.models.embedding.siglip import SigLIPModel @@ -104,7 +105,7 @@ def n_views(self) -> int: @property def extent(self) -> np.ndarray: - points = np.concatenate([np.asarray(o.cloud.pointcloud.points) for o in self.observations]) + points = np.concatenate([pointcloud_xyz(o.cloud) for o in self.observations]) extent: np.ndarray = points.max(axis=0) - points.min(axis=0) return extent @@ -208,12 +209,12 @@ def _lift( pose = gates.camera_pose(tf, detections.ts, optical_frame, world_frame, tf_tolerance) if pose is None: return [] - camera = np.array([pose.position.x, pose.position.y, pose.position.z]) + camera = np.array([pose.pose.position.x, pose.pose.position.y, pose.pose.position.z]) lifted = ImageDetections3DPC.from_depth(detections, depth, camera_info, transform) valid: list[tuple[Detection3DPC, np.ndarray]] = [] for det3d in lifted: - points = np.asarray(det3d.pointcloud.pointcloud.points) + points = pointcloud_xyz(det3d.pointcloud) if len(points) < policy.min_depth_points: continue extent = points.max(axis=0) - points.min(axis=0) @@ -443,7 +444,7 @@ def _absorb(frame_obs: Any, is_verify: bool) -> None: observation = _ClusterObservation( ts=det3d.ts, score=det3d.confidence, - centroid=np.asarray(det3d.pointcloud.pointcloud.points).mean(axis=0), + centroid=pointcloud_xyz(det3d.pointcloud).mean(axis=0), cloud=det3d.pointcloud, camera_position=camera, detection=det3d, @@ -545,10 +546,14 @@ def _absorb(frame_obs: Any, is_verify: bool) -> None: reason = "ambiguous_between_coexisting_candidates" if margin < policy.refusal_margin else None latest = winner.latest - points = np.asarray(latest.cloud.pointcloud.points) + points = pointcloud_xyz(latest.cloud) aabb_min, aabb_max = points.min(axis=0), points.max(axis=0) try: - orientation = _quaternion_from_matrix(np.asarray(latest.cloud.oriented_bounding_box.R)) + import open3d as o3d + + native = o3d.geometry.PointCloud() + native.points = o3d.utility.Vector3dVector(points) + orientation = _quaternion_from_matrix(np.asarray(native.get_oriented_bounding_box().R)) except Exception: orientation = (0.0, 0.0, 0.0, 1.0) center = (aabb_min + aabb_max) / 2 diff --git a/dimos/perception/memory/support_plane.py b/dimos/perception/memory/support_plane.py index f3382f74e9d..84cdc8c313e 100644 --- a/dimos/perception/memory/support_plane.py +++ b/dimos/perception/memory/support_plane.py @@ -27,15 +27,15 @@ import numpy as np -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 +from dimos.msgs.geometry import inverse_transform +from dimos.msgs.pointcloud import pointcloud_from_rgbd, pointcloud_xyz, transform_cloud from dimos.perception.memory import gates from dimos.utils.logging_config import setup_logger if TYPE_CHECKING: - from dimos_lcm.sensor_msgs import CameraInfo + from dimos_generated.sensor_msgs.msg import CameraInfo, Image from dimos.memory.type.observation import Observation - from dimos.msgs.sensor_msgs.Image import Image from dimos.protocol.tf.tf import TFLookup logger = setup_logger() @@ -104,17 +104,21 @@ def fit_support_plane( transform = tf.get(optical_frame, world_frame, obs.ts, tf_tolerance) if depth is None or transform is None: continue - cloud = PointCloud2.from_rgbd( - obs.data, depth, camera_info, depth_scale=0.001, depth_trunc=BACKDROP_DEPTH_TRUNC - ).transform(-transform) - clouds.append(cloud.voxel_downsample(0.01)) + cloud = transform_cloud( + pointcloud_from_rgbd( + obs.data, depth, camera_info, depth_scale=0.001, depth_trunc=BACKDROP_DEPTH_TRUNC + ), + inverse_transform(transform), + ) + import open3d as o3d + + native = o3d.geometry.PointCloud() + native.points = o3d.utility.Vector3dVector(pointcloud_xyz(cloud)) + clouds.append(native.voxel_down_sample(0.01)) if not clouds: return None - merged = clouds[0] - for cloud in clouds[1:]: - merged = merged + cloud - points = np.asarray(merged.pointcloud.points) + points = np.concatenate([np.asarray(cloud.points) for cloud in clouds]) if len(points) < 500: return None diff --git a/dimos/perception/memory/test_generated_inventory.py b/dimos/perception/memory/test_generated_inventory.py new file mode 100644 index 00000000000..8c3223fc4b8 --- /dev/null +++ b/dimos/perception/memory/test_generated_inventory.py @@ -0,0 +1,73 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from dimos_generated.geometry_msgs.msg import Quaternion, Transform, TransformStamped, Vector3 +from dimos_generated.std_msgs.msg import Header +import numpy as np + +from dimos.msgs.camera_info import camera_info_from_intrinsics +from dimos.msgs.pointcloud import pointcloud_from_xyz_rgb, pointcloud_rgb, pointcloud_xyz +from dimos.msgs.time import time_from_nanoseconds, to_nanoseconds +from dimos.perception.memory.inventory import _absorb_into, _pixel_bbox, _split_oversized +from dimos.perception.memory.types import InventoryPolicy, SupportObservation + + +def _observation(offset: float, timestamp_ns: int) -> SupportObservation: + points = np.array([[offset, 0, 1], [offset + 0.1, 0, 1]]) + cloud = pointcloud_from_xyz_rgb( + points, + np.array([[1, 2, 3], [4, 5, 6]], dtype=np.uint8), + header=Header(frame_id="world", stamp=time_from_nanoseconds(timestamp_ns)), + ) + return SupportObservation( + ts=timestamp_ns / 1e9, + cloud=cloud, + centroid=points.mean(axis=0), + aabb_min=points.min(axis=0), + aabb_max=points.max(axis=0), + n_points=2, + mask_area_px=2, + camera_position=np.zeros(3), + ) + + +def test_generated_support_merge_preserves_colors_and_source_time(): + first = _observation(0, 1700000000123456789) + second = _observation(0.2, 1700000000123456790) + _absorb_into(first, second) + assert first.n_points == 4 + assert to_nanoseconds(first.cloud.header.stamp) == 1700000000123456790 + np.testing.assert_array_equal( + pointcloud_rgb(first.cloud), [[1, 2, 3], [4, 5, 6], [1, 2, 3], [4, 5, 6]] + ) + np.testing.assert_allclose(first.centroid, pointcloud_xyz(first.cloud).mean(axis=0)) + assert second.n_points == 2 and second.cloud.width == 2 + + +def test_generated_support_projection_reads_nested_transform_and_intrinsics(): + calibration = camera_info_from_intrinsics(10, 20, 5, 6, 100, 100, header=Header()) + transform = TransformStamped( + header=Header(frame_id="optical"), + child_frame_id="world", + transform=Transform(translation=Vector3(x=-1), rotation=Quaternion(w=1)), + ) + assert _pixel_bbox(np.array([[1, 0, 2], [2, 1, 2]]), calibration, transform) == (5, 6, 10, 16) + + +def test_unsplit_support_returns_source_point_mask(): + points = np.array([[0, 0, 0], [0.01, 0.01, 0.01]]) + masks = _split_oversized(points, None, InventoryPolicy()) + assert len(masks) == 1 + assert masks[0].dtype == np.bool_ + np.testing.assert_array_equal(points[masks[0]], points) diff --git a/dimos/perception/memory/tool_inventory.py b/dimos/perception/memory/tool_inventory.py index dc8c20afafe..bbaf45fba48 100644 --- a/dimos/perception/memory/tool_inventory.py +++ b/dimos/perception/memory/tool_inventory.py @@ -96,8 +96,15 @@ def render(out: str, store: Any, instances: list[Instance], t0: float, t1: float import rerun.blueprint as rrb from dimos.memory.vis.color import Color - from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 + from dimos.msgs.geometry import inverse_transform + from dimos.msgs.pointcloud import pointcloud_from_rgbd, transform_cloud, voxel_downsample_cloud from dimos.visualization.rerun.init import rerun_init + from dimos.visualization.rerun.message_helpers import ( + camera_pinhole, + cloud_archetype, + image_archetype, + navigation_archetype, + ) tf = StreamTF.from_store(store) assert tf is not None @@ -143,13 +150,18 @@ def at(ts: float) -> None: depth = gates.depth_at(store, backdrop_ts) transform = tf.get(gates.OPTICAL_FRAME, gates.WORLD_FRAME, backdrop_ts, gates.TF_TOLERANCE) assert depth is not None and transform is not None - backdrop = PointCloud2.from_rgbd(color, depth, camera_info, depth_scale=0.001).transform( - -transform + backdrop = transform_cloud( + pointcloud_from_rgbd(color, depth, camera_info, depth_scale=0.001), + inverse_transform(transform), + ) + rr.log( + "map", + cloud_archetype(voxel_downsample_cloud(backdrop, 0.01), voxel_size=POINT_SIZE), + static=True, ) - rr.log("map", backdrop.voxel_downsample(0.01).to_rerun(voxel_size=POINT_SIZE), static=True) # live camera feed + frustum; the empty box clears the overlay off non-keyframes - rr.log("camera", camera_info.to_rerun(), static=True) + rr.log("camera/image", camera_pinhole(camera_info), static=True) feed_throttle = 0.1 if (t1 - t0) <= 160 else 0.4 feed = store.streams.color_image.after(t0).before(t1).transform(throttle(feed_throttle)) for obs in feed: @@ -157,8 +169,8 @@ def at(ts: float) -> None: if pose is None: continue at(obs.ts) - rr.log("camera/image", obs.data.to_rerun()) - rr.log("camera", pose.to_rerun()) + rr.log("camera/image", image_archetype(obs.data)) + rr.log("camera", navigation_archetype(pose)) rr.log("camera/image/instances", rr.Boxes2D(array=[], array_format=rr.Box2DFormat.XYXY)) # keyframes, logged after the feed so their boxes win the shared timestamps @@ -166,8 +178,8 @@ def at(ts: float) -> None: at(ts) keyframe_pose = gates.camera_pose(tf, ts) assert keyframe_pose is not None - rr.log("camera/image", store.streams.color_image.at(ts, 0.05).first().data.to_rerun()) - rr.log("camera", keyframe_pose.to_rerun()) + rr.log("camera/image", image_archetype(store.streams.color_image.at(ts, 0.05).first().data)) + rr.log("camera", navigation_archetype(keyframe_pose)) rr.log( "camera/image/instances", rr.Boxes2D( @@ -180,7 +192,7 @@ def at(ts: float) -> None: ), ) for i, member in entries: - rr.log(paths[i], member.cloud.to_rerun(voxel_size=POINT_SIZE, colors=colors[i])) + rr.log(paths[i], cloud_archetype(member.cloud, voxel_size=POINT_SIZE, colors=colors[i])) # the reported instance: one labeled box, static so it holds over the whole timeline for i, instance in enumerate(grounded): diff --git a/dimos/perception/memory/tool_localize.py b/dimos/perception/memory/tool_localize.py index b1ec9e91907..2b21d667078 100644 --- a/dimos/perception/memory/tool_localize.py +++ b/dimos/perception/memory/tool_localize.py @@ -55,8 +55,15 @@ def render( import rerun as rr import rerun.blueprint as rrb - from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 + from dimos.msgs.geometry import inverse_transform + from dimos.msgs.pointcloud import pointcloud_from_rgbd, transform_cloud, voxel_downsample_cloud from dimos.visualization.rerun.init import rerun_init + from dimos.visualization.rerun.message_helpers import ( + camera_pinhole, + cloud_archetype, + image_archetype, + navigation_archetype, + ) tf = cast("StreamTF", StreamTF.from_store(store)) camera_info = store.streams.camera_info.first().data @@ -91,19 +98,20 @@ def at(ts: float) -> None: gates.OPTICAL_FRAME, gates.WORLD_FRAME, backdrop_ts, gates.TF_TOLERANCE ) if depth is not None and transform is not None: - backdrop = PointCloud2.from_rgbd( - color, depth, camera_info, depth_scale=0.001 - ).transform(-transform) + backdrop = transform_cloud( + pointcloud_from_rgbd(color, depth, camera_info, depth_scale=0.001), + inverse_transform(transform), + ) rr.log( "map", - backdrop.voxel_downsample(0.01).to_rerun(voxel_size=POINT_SIZE), + cloud_archetype(voxel_downsample_cloud(backdrop, 0.01), voxel_size=POINT_SIZE), static=True, ) except LookupError: pass # live camera feed + frustum tracking the wrist along the timeline - rr.log("camera", camera_info.to_rerun(), static=True) + rr.log("camera/image", camera_pinhole(camera_info), static=True) feed_throttle = 0.1 if (t1 - t0) <= 160 else 0.4 feed = store.streams.color_image.after(t0).before(t1).transform(throttle(feed_throttle)) for obs in feed: @@ -111,8 +119,8 @@ def at(ts: float) -> None: if pose is None: continue at(obs.ts) - rr.log("camera/image", obs.data.to_rerun()) - rr.log("camera", pose.to_rerun()) + rr.log("camera/image", image_archetype(obs.data)) + rr.log("camera", navigation_archetype(pose)) for query, trace in traces: root = f"detections/{query.replace(' ', '_')}" @@ -124,11 +132,11 @@ def at(ts: float) -> None: continue at(obs.ts) annotated = obs.data.annotated_image() - rr.log("camera/image", annotated.to_rerun()) + rr.log("camera/image", image_archetype(annotated)) frame = f"{root}/frames/{i}" - rr.log(frame, pose.to_rerun()) - rr.log(frame, camera_info.to_rerun()) - rr.log(f"{frame}/image", annotated.to_rerun()) + rr.log(frame, navigation_archetype(pose)) + rr.log(f"{frame}/image", camera_pinhole(camera_info)) + rr.log(f"{frame}/image", image_archetype(annotated)) # 3d detections: green = matched candidates, red = cross-view re-detections for tag, entries, rgb in [ @@ -139,7 +147,7 @@ def at(ts: float) -> None: at(ts) rr.log( f"{root}/{tag}/{i}_{det.name.replace(' ', '_')}", - det.pointcloud.to_rerun(voxel_size=POINT_SIZE, colors=rgb), + cloud_archetype(det.pointcloud, voxel_size=POINT_SIZE, colors=rgb), ) # the answer: always blue, whatever the query @@ -147,7 +155,7 @@ def at(ts: float) -> None: at(trace.answer.ts) rr.log( f"{root}/answer", - trace.answer.pointcloud.to_rerun(voxel_size=POINT_SIZE, colors=BLUE), + cloud_archetype(trace.answer.pointcloud, voxel_size=POINT_SIZE, colors=BLUE), ) @@ -164,7 +172,9 @@ def report(query: str, hit: Localization | None, lo: float) -> bool: ) return True x, y, z = hit.position_world_xyz - cloud_points = len(hit.point_cloud) if hit.point_cloud is not None else 0 + cloud_points = ( + hit.point_cloud.width * hit.point_cloud.height if hit.point_cloud is not None else 0 + ) print( f"hit {query!r}: position=({x:.3f}, {y:.3f}, {z:.3f}) frame={hit.frame_id} " f"ts_offset={offset:.1f}s points={cloud_points} views={hit.n_views} " @@ -232,6 +242,7 @@ def main() -> int: require_pose=not args.allow_no_pose, trace=qtraces, ) + assert isinstance(results, list) for query, qtrace, hit in zip(queries, qtraces, results, strict=True): if report(query, hit, lo): hits += 1 @@ -239,14 +250,15 @@ def main() -> int: else: for query in queries: trace = LocalizeTrace() - hit = dan.localize( + single_hit = dan.localize( store, query, index=index, require_pose=not args.allow_no_pose, trace=trace, ) - if report(query, hit, lo): + assert not isinstance(single_hit, list) + if report(query, single_hit, lo): hits += 1 traces.append((query, trace)) diff --git a/dimos/perception/memory/types.py b/dimos/perception/memory/types.py index 328590d71d7..60ce2c8a157 100644 --- a/dimos/perception/memory/types.py +++ b/dimos/perception/memory/types.py @@ -28,7 +28,7 @@ import numpy as np if TYPE_CHECKING: - from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 + from dimos_generated.sensor_msgs.msg import PointCloud2 @dataclass(frozen=True) diff --git a/dimos/protocol/pubsub/benchmark/test_replay_bench.py b/dimos/protocol/pubsub/benchmark/test_replay_bench.py index 4e1d772ed29..6d663567950 100644 --- a/dimos/protocol/pubsub/benchmark/test_replay_bench.py +++ b/dimos/protocol/pubsub/benchmark/test_replay_bench.py @@ -20,17 +20,17 @@ import time from types import SimpleNamespace +from dimos_generated.sensor_msgs.msg import CompressedImage, Image +from dimos_generated.std_msgs.msg import Header import numpy as np import pytest from dimos.core.transport import LCMTransport, ZenohTransport -from dimos.msgs.sensor_msgs.CompressedImage import CompressedImage -from dimos.msgs.sensor_msgs.Image import Image, ImageFormat +from dimos.msgs.image import image_from_array, image_to_jpeg, image_view +from dimos.msgs.time import time_from_seconds from dimos.protocol.pubsub.benchmark.tool_replay_bench import CompressedCodec from dimos.protocol.service.zenohservice import ZenohSessionPool -pytestmark = pytest.mark.skipif_no_turbojpeg - def make_image(width: int = 1280, height: int = 720) -> Image: """720p-ish frame: gradient + noise, roughly what a real camera compresses like.""" @@ -38,7 +38,9 @@ def make_image(width: int = 1280, height: int = 720) -> Image: gradient = np.broadcast_to(np.linspace(0, 255, width, dtype=np.uint8), (height, width)) noise = rng.randint(0, 60, (height, width), dtype=np.uint8) data = np.stack([gradient, np.minimum(gradient, 128) + noise // 2, noise], axis=-1) - return Image(data=data, format=ImageFormat.RGB, frame_id="cam", ts=42.125) + return image_from_array( + data, encoding="rgb8", header=Header(frame_id="cam", stamp=time_from_seconds(42.125)) + ) @pytest.fixture() @@ -60,30 +62,38 @@ def session_pool(): pool.close_all() -def test_roundtrip_over_lcm(retry_until, collector) -> None: - t = CompressedCodec(LCMTransport("dimos/test/codec_lcm", CompressedImage)) +@pytest.fixture +def codec_transports(): + transports = [] + yield transports + for transport in transports: + transport.stop() + + +def test_roundtrip_over_lcm(retry_until, collector, codec_transports, lcm_url) -> None: + t = CompressedCodec(LCMTransport("dimos/test/codec_lcm", CompressedImage, url=lcm_url)) + codec_transports.append(t) t.subscribe(collector.callback) src = make_image(320, 240) retry_until(collector.event, lambda: t.broadcast(None, src)) img = collector.received[0] assert isinstance(img, Image) - assert img.frame_id == "cam" - assert abs(img.ts - src.ts) < 1e-6 - assert img.shape == src.shape - t.stop() + assert img.header.frame_id == "cam" + assert img.header.stamp == src.header.stamp + assert image_view(img).shape == image_view(src).shape -def test_roundtrip_over_zenoh(retry_until, collector, session_pool) -> None: +def test_roundtrip_over_zenoh(retry_until, collector, session_pool, codec_transports) -> None: t = CompressedCodec( ZenohTransport("dimos/test/codec_zenoh", CompressedImage, session_pool=session_pool) ) + codec_transports.append(t) t.subscribe(collector.callback) src = make_image(320, 240) retry_until(collector.event, lambda: t.broadcast(None, src)) img = collector.received[0] assert isinstance(img, Image) - assert abs(img.ts - src.ts) < 1e-6 - t.stop() + assert img.header.stamp == src.header.stamp def test_pickle_roundtrip() -> None: @@ -184,8 +194,10 @@ def inner(topic: str, typ: type): return LCMTransport(topic, typ, url=f"{lcm_url}&recv_buf_size={2 * 1024 * 1024}") return ZenohTransport(topic, typ, session_pool=session_pool) - raw_wire = len(frame.lcm_encode()) - jpeg_wire = len(CompressedImage.from_image(frame).lcm_encode()) + raw_wire = len(frame.encode()) + jpeg_wire = len( + CompressedImage(header=frame.header, format="jpeg", data=image_to_jpeg(frame)).encode() + ) raw = _bench(inner(f"dimos/bench/{proto}_raw", Image), frame, n, raw_wire) codec = _bench( diff --git a/dimos/protocol/pubsub/benchmark/testdata.py b/dimos/protocol/pubsub/benchmark/testdata.py index ab2190d7a7a..70d24e46899 100644 --- a/dimos/protocol/pubsub/benchmark/testdata.py +++ b/dimos/protocol/pubsub/benchmark/testdata.py @@ -18,10 +18,11 @@ import os from typing import TYPE_CHECKING, Any +from dimos_generated.sensor_msgs.msg import Image import numpy as np import pytest -from dimos.msgs.sensor_msgs.Image import Image, ImageFormat +from dimos.msgs.image import image_from_array from dimos.protocol.pubsub.benchmark.type import Case from dimos.protocol.pubsub.impl.zenohpubsub import Topic as ZenohTopic, Zenoh from dimos.protocol.pubsub.spec import PubSub @@ -38,7 +39,7 @@ from dimos.protocol.pubsub.impl.memory import Memory from dimos.protocol.pubsub.impl.shmpubsub import ( BytesSharedMemory, - LCMSharedMemory, + CDRSharedMemory, PickleSharedMemory, ) @@ -59,7 +60,7 @@ def make_data_image(size: int) -> Image: height = max(1, int(pixels**0.5)) width = pixels // height data = padded_data[: height * width * 3].reshape(height, width, 3) - return Image(data=data, format=ImageFormat.RGB) + return image_from_array(data, encoding="rgb8") testcases: list[Case[Any, Any]] = [] @@ -74,7 +75,7 @@ def lcm_pubsub_channel() -> Generator[LCM, None, None]: def lcm_msggen(size: int) -> tuple[LCMTopic, Image]: - topic = LCMTopic(topic="benchmark/lcm", lcm_type=Image) + topic = LCMTopic(topic="benchmark/lcm", msg_type=Image) return (topic, make_data_image(size)) @@ -172,9 +173,9 @@ def shm_bytes_msggen(size: int) -> tuple[str, bytes]: @contextmanager -def shm_lcm_pubsub_channel() -> Generator[LCMSharedMemory, None, None]: - """SharedMemory with LCM binary encoding - no pickle overhead.""" - shm_pubsub = LCMSharedMemory(prefer="cpu", default_capacity=12 * 1024 * 1024) +def shm_lcm_pubsub_channel() -> Generator[CDRSharedMemory, None, None]: + """SharedMemory with CDR encoding - no pickle overhead.""" + shm_pubsub = CDRSharedMemory(prefer="cpu", default_capacity=12 * 1024 * 1024) shm_pubsub.start() yield shm_pubsub shm_pubsub.stop() diff --git a/dimos/protocol/pubsub/benchmark/tool_replay_bench.py b/dimos/protocol/pubsub/benchmark/tool_replay_bench.py index ee80e5854e2..da4796d0eee 100644 --- a/dimos/protocol/pubsub/benchmark/tool_replay_bench.py +++ b/dimos/protocol/pubsub/benchmark/tool_replay_bench.py @@ -35,21 +35,23 @@ from pathlib import Path import threading import time -from typing import Any, TypeVar +from typing import Any + +from dimos_generated.sensor_msgs.msg import CompressedImage, Image +import numpy as np from dimos.core.core import rpc from dimos.core.module import Module -from dimos.core.stream import In, Out, Stream +from dimos.core.stream import In, Stream from dimos.core.transport import PubSubTransport -from dimos.msgs.sensor_msgs.Image import Image +from dimos.msgs.image import image_from_array, image_to_jpeg, image_view +from dimos.msgs.time import to_seconds from dimos.utils.logging_config import setup_logger logger = setup_logger() -T = TypeVar("T") - -class CompressedCodec(PubSubTransport[T]): +class CompressedCodec(PubSubTransport[Image]): """Image→CompressedImage jpeg codec over any inner transport. Bench-only utility: rejected as public API in #2831 (transports shouldn't @@ -58,7 +60,7 @@ class CompressedCodec(PubSubTransport[T]): Subscribers always receive a decoded Image. """ - def __init__(self, inner: PubSubTransport[Any], quality: int = 75) -> None: + def __init__(self, inner: PubSubTransport[CompressedImage], quality: int = 75) -> None: super().__init__(inner.topic) self.inner = inner self.quality = quality @@ -66,21 +68,30 @@ def __init__(self, inner: PubSubTransport[Any], quality: int = 75) -> None: def __reduce__(self): # type: ignore[no-untyped-def] return (CompressedCodec, (self.inner, self.quality)) - def broadcast(self, stream: Out[T] | None, msg: T) -> None: - from dimos.msgs.sensor_msgs.CompressedImage import ( - CompressedImage, - ) # deferred to avoid pulling in cv2/rerun - - if not isinstance(msg, CompressedImage): - msg = CompressedImage.from_image(msg, quality=self.quality) # type: ignore[assignment, arg-type] - self.inner.broadcast(stream, msg) + def broadcast(self, stream: Stream[Image] | None, msg: Image | CompressedImage) -> None: + compressed = ( + msg + if isinstance(msg, CompressedImage) + else CompressedImage( + header=msg.header, format="jpeg", data=image_to_jpeg(msg, quality=self.quality) + ) + ) + self.inner.broadcast(None, compressed) def subscribe( - self, callback: Callable[[T], Any], selfstream: Stream[T] | None = None + self, callback: Callable[[Image], Any], selfstream: Stream[Image] | None = None ) -> Callable[[], None]: - return self.inner.subscribe( # type: ignore[no-any-return] - lambda m: callback(m.decode()), selfstream - ) + def decoded(message: CompressedImage) -> Any: + import cv2 + + pixels = cv2.imdecode( + np.frombuffer(message.data.view(), dtype=np.uint8), cv2.IMREAD_COLOR + ) + if pixels is None: + raise ValueError("JPEG benchmark received an invalid compressed image") + return callback(image_from_array(pixels, encoding="bgr8", header=message.header)) + + return self.inner.subscribe(decoded) def start(self) -> None: self.inner.start() @@ -116,13 +127,14 @@ def stop(self) -> None: def _on_image(self, msg: Image) -> None: now = time.time() self._count += 1 - rec = {"t": now, "ts": msg.ts, "age": now - msg.ts, "i": self._count} + ts = to_seconds(msg.header.stamp) + rec = {"t": now, "ts": ts, "age": now - ts, "i": self._count} if self._work_ms > 0: import cv2 deadline = time.perf_counter() + self._work_ms / 1000 while time.perf_counter() < deadline: - cv2.GaussianBlur(msg.data, (31, 31), 5) + cv2.GaussianBlur(image_view(msg), (31, 31), 5) rec["done_t"] = time.time() with self._lock: self._buf.append(json.dumps(rec)) @@ -202,7 +214,6 @@ def main() -> None: from dimos.core.coordination.blueprints import autoconnect from dimos.core.coordination.module_coordinator import ModuleCoordinator from dimos.core.transport import LCMTransport - from dimos.msgs.sensor_msgs.CompressedImage import CompressedImage from dimos.robot.get_all_blueprints import get_blueprint_by_name bp = get_blueprint_by_name(args.blueprint) @@ -220,7 +231,7 @@ def main() -> None: } ) - coordinator = ModuleCoordinator.build(bp, {"g": {"replay": True, "viewer": "none"}}) + coordinator = ModuleCoordinator.build(bp.global_config(replay=True, viewer="none")) logger.info("benchmark run started", mode=args.mode, blueprint=args.blueprint) stop = threading.Event() diff --git a/dimos/protocol/pubsub/encoders.py b/dimos/protocol/pubsub/encoders.py index ff5c78a2f52..3ef7845d39e 100644 --- a/dimos/protocol/pubsub/encoders.py +++ b/dimos/protocol/pubsub/encoders.py @@ -94,33 +94,29 @@ def decode(self, msg: bytes, _: TopicT) -> MsgT: return cast("MsgT", pickle.loads(msg)) -class LCMTopicProto(Protocol): - """Protocol for topics usable with LCM encoders.""" +class TypedTopicProto(Protocol): + """A topic's explicit generated message type, independent of its transport.""" topic: str # At decode time, always concrete string - lcm_type: type[DimosMsg] | None + msg_type: type[DimosMsg] | None -class LCMEncoderMixin(PubSubEncoderMixin[LCMTopicProto, DimosMsg, bytes]): - """Encoder mixin for DimosMsg using LCM binary encoding.""" +class CDREncoderMixin(PubSubEncoderMixin[TypedTopicProto, DimosMsg, bytes]): + """Encode typed streams as standard encapsulated CDR.""" - def subscribe( - self, topic: LCMTopicProto, callback: Callable[[DimosMsg, LCMTopicProto], None] - ) -> Callable[[], None]: - # Import the type's heavy decode dependencies now, on the subscriber's - # thread. Left to the first decode, the import would run on the LCM - # handler thread and block it for seconds, dropping messages meanwhile. - warmup = getattr(topic.lcm_type, "lcm_warmup", None) - if warmup is not None: - warmup() - return super().subscribe(topic, callback) - - def encode(self, msg: DimosMsg | bytes, _: LCMTopicProto) -> bytes: + def encode(self, msg: DimosMsg | bytes, topic: TypedTopicProto) -> bytes: if isinstance(msg, bytes): return msg - return msg.lcm_encode() - - def decode(self, msg: bytes, topic: LCMTopicProto) -> DimosMsg: - if topic.lcm_type is None: - raise DecodingError(f"Cannot decode: topic {topic.topic!r} has no lcm_type") - return topic.lcm_type.lcm_decode(msg) + if topic.msg_type is not None and msg.msg_name != topic.msg_type.msg_name: + raise ValueError( + f"Message {msg.msg_name} does not match topic type {topic.msg_type.msg_name}" + ) + return msg.encode() + + def decode(self, msg: bytes, topic: TypedTopicProto) -> DimosMsg: + if topic.msg_type is None: + raise DecodingError(f"Cannot decode: topic {topic.topic!r} has no msg_type") + try: + return topic.msg_type.decode(msg) + except ValueError as exc: + raise DecodingError(f"Invalid CDR on topic {topic.topic!r}") from exc diff --git a/dimos/protocol/pubsub/impl/jpeg_lcm.py b/dimos/protocol/pubsub/impl/jpeg_lcm.py index b87905584ee..bafb414a22f 100644 --- a/dimos/protocol/pubsub/impl/jpeg_lcm.py +++ b/dimos/protocol/pubsub/impl/jpeg_lcm.py @@ -21,25 +21,29 @@ from __future__ import annotations -from typing import cast +from dimos_generated.sensor_msgs.msg import CompressedImage, Image -from dimos.msgs.sensor_msgs.Image import Image -from dimos.protocol.pubsub.encoders import DecodingError, LCMTopicProto, PubSubEncoderMixin +from dimos.msgs.image import compressed_image_from_image, image_from_compressed +from dimos.protocol.pubsub.encoders import DecodingError, PubSubEncoderMixin, TypedTopicProto from dimos.protocol.pubsub.impl.lcmpubsub import LCMPubSubBase -class JpegEncoderMixin(PubSubEncoderMixin[LCMTopicProto, Image, bytes]): - """Encoder mixin for DimosMsg using JPEG encoding (for images).""" +class JpegEncoderMixin(PubSubEncoderMixin[TypedTopicProto, Image, bytes]): + """Transcode generated Image values using CompressedImage CDR on the wire.""" - def encode(self, msg: Image, _: LCMTopicProto) -> bytes: - return msg.lcm_jpeg_encode() + def encode(self, msg: Image, topic: TypedTopicProto) -> bytes: + if topic.msg_type is not CompressedImage: + raise ValueError("JPEG wire topics must declare generated CompressedImage") + return compressed_image_from_image(msg).encode() - def decode(self, msg: bytes, topic: LCMTopicProto) -> Image: + def decode(self, msg: bytes, topic: TypedTopicProto) -> Image: if topic.topic == "LCM_SELF_TEST": raise DecodingError("Ignoring LCM_SELF_TEST topic") - if topic.lcm_type is None: - raise DecodingError(f"Cannot decode: topic {topic.topic!r} has no lcm_type") - return cast("type[Image]", topic.lcm_type).lcm_jpeg_decode(msg) + if topic.msg_type is None: + raise DecodingError(f"Cannot decode: topic {topic.topic!r} has no msg_type") + if topic.msg_type is not CompressedImage: + raise DecodingError("JPEG wire topics must declare generated CompressedImage") + return image_from_compressed(CompressedImage.decode(msg)) class JpegLCM( # type: ignore[misc] diff --git a/dimos/protocol/pubsub/impl/jpeg_shm.py b/dimos/protocol/pubsub/impl/jpeg_shm.py index 649da4f9c9c..fb6f63cfbcc 100644 --- a/dimos/protocol/pubsub/impl/jpeg_shm.py +++ b/dimos/protocol/pubsub/impl/jpeg_shm.py @@ -14,14 +14,15 @@ from typing import Any -from dimos.msgs.sensor_msgs.Image import Image, ImageFormat +from dimos_generated.sensor_msgs.msg import CompressedImage, Image + +from dimos.msgs.image import compressed_image_from_image, image_from_compressed from dimos.protocol.pubsub.encoders import PubSubEncoderMixin from dimos.protocol.pubsub.impl.shmpubsub import SharedMemoryPubSubBase -from dimos.utils.turbojpeg import get_turbojpeg class JpegSharedMemoryEncoderMixin(PubSubEncoderMixin[str, Image, bytes]): - def __init__(self, quality: int = 75, **kwargs) -> None: # type: ignore[no-untyped-def] + def __init__(self, quality: int = 75, **kwargs: Any) -> None: super().__init__(**kwargs) self.quality = quality @@ -29,12 +30,10 @@ def encode(self, msg: Any, _topic: str) -> bytes: if not isinstance(msg, Image): raise ValueError("Can only encode images.") - bgr_image = msg.to_bgr().to_opencv() - return get_turbojpeg().encode(bgr_image, quality=self.quality) # type: ignore[no-any-return] + return compressed_image_from_image(msg, quality=self.quality).encode() def decode(self, msg: bytes, _topic: str) -> Image: - bgr_array = get_turbojpeg().decode(msg) - return Image(data=bgr_array, format=ImageFormat.BGR) + return image_from_compressed(CompressedImage.decode(msg)) class JpegSharedMemory(JpegSharedMemoryEncoderMixin, SharedMemoryPubSubBase): # type: ignore[misc] diff --git a/dimos/protocol/pubsub/impl/lcmpubsub.py b/dimos/protocol/pubsub/impl/lcmpubsub.py index f47280f35cb..34f28d4ca03 100644 --- a/dimos/protocol/pubsub/impl/lcmpubsub.py +++ b/dimos/protocol/pubsub/impl/lcmpubsub.py @@ -20,9 +20,10 @@ import threading from typing import Any +from dimos.msgs.helpers import resolve_msg_type from dimos.msgs.protocol import DimosMsg from dimos.protocol.pubsub.encoders import ( - LCMEncoderMixin, + CDREncoderMixin, PickleEncoderMixin, ) from dimos.protocol.pubsub.patterns import Glob @@ -36,7 +37,7 @@ @dataclass class Topic: topic: str | re.Pattern[str] | Glob - lcm_type: type[DimosMsg] | None = None + msg_type: type[DimosMsg] | None = None @property def is_pattern(self) -> bool: @@ -51,25 +52,23 @@ def pattern(self) -> str: return self.topic def __str__(self) -> str: - if self.lcm_type is None: + if self.msg_type is None: return self.pattern - return f"{self.pattern}#{self.lcm_type.msg_name}" + return f"{self.pattern}#{self.msg_type.msg_name}" @staticmethod - def from_channel_str(channel: str, default_lcm_type: type[DimosMsg] | None = None) -> Topic: + def from_channel_str(channel: str, default_msg_type: type[DimosMsg] | None = None) -> Topic: """Create Topic from channel string. - Channel format: /topic#module.ClassName - Falls back to default_lcm_type if type cannot be parsed. + Channel format: /topic#package/msg/Type. + A supplied type applies only to channels without a type suffix. """ - from dimos.msgs.helpers import resolve_msg_type - if "#" not in channel: - return Topic(topic=channel, lcm_type=default_lcm_type) + return Topic(topic=channel, msg_type=default_msg_type) topic_str, type_name = channel.rsplit("#", 1) - lcm_type = resolve_msg_type(type_name) - return Topic(topic=topic_str, lcm_type=lcm_type or default_lcm_type) + msg_type = resolve_msg_type(type_name) + return Topic(topic=topic_str, msg_type=msg_type) class LCMPubSubBase(LCMService, AllPubSub[Topic, Any]): @@ -89,6 +88,10 @@ def publish(self, topic: Topic | str, message: bytes) -> None: return topic_str = str(topic) if isinstance(topic, Topic) else topic + if "\0" in topic_str or len(topic_str.encode("utf-8")) > 63: + raise ValueError( + "LCM channel (including type suffix) must fit in 63 bytes and contain no NUL" + ) self.l.publish(topic_str, message) def subscribe_all(self, callback: Callable[[bytes, Topic], Any]) -> Callable[[], None]: @@ -115,11 +118,11 @@ def noop() -> None: def handler(channel: str, msg: bytes) -> None: if not alive or channel == "LCM_SELF_TEST": return - callback(msg, Topic.from_channel_str(channel, topic.lcm_type)) + callback(msg, Topic.from_channel_str(channel, topic.msg_type)) pattern_str = str(topic) - if not pattern_str.endswith("*"): - pattern_str = f"{pattern_str}(#.*)?" + if topic.msg_type is None: + pattern_str = f"(?:{pattern_str})(#.*)?" lcm_subscription = self.l.subscribe(pattern_str, handler) else: @@ -149,7 +152,7 @@ def unsubscribe() -> None: class LCM( # type: ignore[misc] - LCMEncoderMixin, + CDREncoderMixin, LCMPubSubBase, ): ... diff --git a/dimos/protocol/pubsub/impl/rospubsub_conversion.py b/dimos/protocol/pubsub/impl/rospubsub_conversion.py index 6853c57ca91..16438801eda 100644 --- a/dimos/protocol/pubsub/impl/rospubsub_conversion.py +++ b/dimos/protocol/pubsub/impl/rospubsub_conversion.py @@ -12,353 +12,48 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""Conversion functions between dimos messages and ROS messages. - -This module provides conversion functions between dimos message types and ROS messages. -It handles three categories of types: - -1. Complex types (different internal representation) - use LCM roundtrip -2. Simple types (field structures match) - use direct field copy -3. No dimos.msgs equivalent - return dimos_lcm type -""" +"""Convert generated DimOS and ROS2 messages through their shared CDR format.""" from __future__ import annotations import importlib -import re -from typing import TYPE_CHECKING, Any, cast +from typing import TYPE_CHECKING, Any, TypeVar, cast -from dimos.msgs.helpers import lcm_msg_type +try: + import rclpy.serialization as ros_serialization +except ImportError: + ros_serialization = None # type: ignore[assignment] if TYPE_CHECKING: from dimos.msgs.protocol import DimosMsg from dimos.protocol.pubsub.impl.rospubsub import ROSMessage -# Complex types that need LCM roundtrip (explicit list) -# These types have different internal representations in dimos vs ROS/LCM -COMPLEX_TYPES: set[str] = { - "sensor_msgs.PointCloud2", - "sensor_msgs.Image", - "sensor_msgs.CompressedImage", - "sensor_msgs.CameraInfo", - "geometry_msgs.PoseStamped", -} - -# Cache for dynamic imports of dimos types -_dimos_type_cache: dict[str, type[DimosMsg] | None] = {} - -# Cache for LCM type derivation -_lcm_type_cache: dict[str, type[Any]] = {} - -# Field name mappings between ROS and LCM (ROS name -> LCM name) -# This is some mixup in dimos_lcm having ROS1 and ROS2 message definitions? -# Would be good to clarify later, but this works for now -_ROS_TO_LCM_FIELD_MAP: dict[str, str] = { - "nanosec": "nsec", # ROS2 Time.nanosec -> LCM Time.nsec -} - -# Reverse mapping (LCM name -> ROS name) -_LCM_TO_ROS_FIELD_MAP: dict[str, str] = {v: k for k, v in _ROS_TO_LCM_FIELD_MAP.items()} - - -def get_dimos_type(msg_name: str) -> type[DimosMsg] | None: - """Try to import dimos.msgs type, return None if not found. Cached. - - Args: - msg_name: Message name in format "package.MessageName" (e.g., "geometry_msgs.Vector3") - - Returns: - The dimos message type, or None if not found - """ - if msg_name in _dimos_type_cache: - return _dimos_type_cache[msg_name] - - try: - package, name = msg_name.split(".") - module = importlib.import_module(f"dimos.msgs.{package}.{name}") - dimos_type = cast("type[DimosMsg]", getattr(module, name)) - _dimos_type_cache[msg_name] = dimos_type - return dimos_type - except (ImportError, AttributeError, ValueError): - _dimos_type_cache[msg_name] = None - return None - - -def derive_lcm_type(dimos_type: type[DimosMsg]) -> type[Any]: - """Derive the LCM message type from a dimos message type. - - Args: - dimos_type: A dimos message type (e.g., dimos.msgs.sensor_msgs.PointCloud2) - - Returns: - The corresponding LCM message type (e.g., dimos_lcm.sensor_msgs.PointCloud2) - """ - msg_name = dimos_type.msg_name # e.g., "sensor_msgs.PointCloud2" - - if msg_name in _lcm_type_cache: - return _lcm_type_cache[msg_name] - - parts = msg_name.split(".") - if len(parts) != 2: - raise ValueError(f"Invalid msg_name format: {msg_name}, expected 'package.MessageName'") - - lcm_type = lcm_msg_type(msg_name) - _lcm_type_cache[msg_name] = lcm_type - return lcm_type +MessageT = TypeVar("MessageT", bound="DimosMsg") def derive_ros_type(dimos_type: type[DimosMsg]) -> type[ROSMessage]: - """Derive the ROS message type from a dimos message type. - - Args: - dimos_type: A dimos message type (e.g., dimos.msgs.geometry_msgs.Vector3) - - Returns: - The corresponding ROS message type (e.g., geometry_msgs.msg.Vector3) - - Example: - msg_name = "geometry_msgs.Vector3" -> geometry_msgs.msg.Vector3 - """ - msg_name = dimos_type.msg_name # e.g., "geometry_msgs.Vector3" - parts = msg_name.split(".") - if len(parts) != 2: - raise ValueError(f"Invalid msg_name format: {msg_name}, expected 'package.MessageName'") - - package, message_name = parts - ros_module = importlib.import_module(f"{package}.msg") - return cast("type[ROSMessage]", getattr(ros_module, message_name)) - - -def _copy_ros_to_lcm_recursive(ros_msg: Any, lcm_msg: Any) -> None: - """Recursively copy fields from ROS message to LCM message. - - Handles nested messages, arrays, and primitive types. - - Args: - ros_msg: Source ROS message - lcm_msg: Target LCM message (modified in place) - """ - if not hasattr(ros_msg, "get_fields_and_field_types"): - raise TypeError(f"Expected ROS message, got {type(ros_msg).__name__}") - - field_types = ros_msg.get_fields_and_field_types() - for ros_field_name in field_types: - # Map ROS field name to LCM field name - lcm_field_name = _ROS_TO_LCM_FIELD_MAP.get(ros_field_name, ros_field_name) - - if not hasattr(lcm_msg, lcm_field_name): - continue - - ros_value = getattr(ros_msg, ros_field_name) - lcm_value = getattr(lcm_msg, lcm_field_name) - - # Handle nested messages - if hasattr(ros_value, "get_fields_and_field_types"): - _copy_ros_to_lcm_recursive(ros_value, lcm_value) - # Handle arrays of messages - elif isinstance(ros_value, (list, tuple)) and len(ros_value) > 0: - if hasattr(ros_value[0], "get_fields_and_field_types"): - # Array of nested messages - create LCM instances - lcm_array = [] - for ros_item in ros_value: - # Get the LCM element type from the first lcm_value element if available - # Otherwise try to derive from ros item - if isinstance(lcm_value, list) and len(lcm_value) > 0: - lcm_item = type(lcm_value[0])() - else: - # Try to create matching LCM type - lcm_item = _create_lcm_instance_for_ros_msg(ros_item) - _copy_ros_to_lcm_recursive(ros_item, lcm_item) - lcm_array.append(lcm_item) - setattr(lcm_msg, lcm_field_name, lcm_array) - else: - # Array of primitives - direct copy - setattr(lcm_msg, lcm_field_name, list(ros_value)) - # Handle bytes/data fields - elif isinstance(ros_value, (bytes, bytearray)): - setattr(lcm_msg, lcm_field_name, bytes(ros_value)) - # Handle array.array (ROS uses this for data fields) - elif hasattr(ros_value, "tobytes"): - setattr(lcm_msg, lcm_field_name, ros_value.tobytes()) - else: - # Primitive type - direct copy - setattr(lcm_msg, lcm_field_name, ros_value) - - # Update length fields if present (LCM convention: field_name_length) - length_field = f"{lcm_field_name}_length" - if hasattr(lcm_msg, length_field): - value = getattr(lcm_msg, lcm_field_name) - if isinstance(value, (list, tuple, bytes, bytearray)): - setattr(lcm_msg, length_field, len(value)) - - -def _copy_lcm_to_ros_recursive(lcm_msg: Any, ros_msg: Any) -> None: - """Recursively copy fields from LCM message to ROS message. - - Handles nested messages, arrays, and primitive types. - - Args: - lcm_msg: Source LCM message - ros_msg: Target ROS message (modified in place) - """ - if not hasattr(ros_msg, "get_fields_and_field_types"): - raise TypeError(f"Expected ROS message, got {type(ros_msg).__name__}") - - field_types = ros_msg.get_fields_and_field_types() - for ros_field_name in field_types: - # Map ROS field name to LCM field name - lcm_field_name = _ROS_TO_LCM_FIELD_MAP.get(ros_field_name, ros_field_name) - - if not hasattr(lcm_msg, lcm_field_name): - continue - - lcm_value = getattr(lcm_msg, lcm_field_name) - ros_value = getattr(ros_msg, ros_field_name) - - # Handle nested messages - if hasattr(ros_value, "get_fields_and_field_types"): - _copy_lcm_to_ros_recursive(lcm_value, ros_value) - # Handle arrays of messages - elif isinstance(lcm_value, (list, tuple)) and len(lcm_value) > 0: - if hasattr(lcm_value[0], "lcm_encode"): - # Array of nested LCM messages - ros_array = [] - for lcm_item in lcm_value: - ros_item = _create_ros_instance_for_lcm_msg( - lcm_item, field_types[ros_field_name] - ) - _copy_lcm_to_ros_recursive(lcm_item, ros_item) - ros_array.append(ros_item) - setattr(ros_msg, ros_field_name, ros_array) - else: - # Array of primitives - direct copy - setattr(ros_msg, ros_field_name, list(lcm_value)) - # Handle bytes/data fields - elif isinstance(lcm_value, (bytes, bytearray)): - # ROS data fields might expect array.array - if hasattr(ros_value, "frombytes"): - import array - - arr = array.array("B") - arr.frombytes(lcm_value) - setattr(ros_msg, ros_field_name, arr) - else: - setattr(ros_msg, ros_field_name, bytes(lcm_value)) - else: - # Primitive type - direct copy - setattr(ros_msg, ros_field_name, lcm_value) - - -def _create_lcm_instance_for_ros_msg(ros_msg: Any) -> Any: - """Create an LCM message instance that matches the ROS message type. - - Args: - ros_msg: ROS message to match - - Returns: - New LCM message instance - """ - # Get the ROS type name (e.g., "std_msgs.msg.Header" -> "std_msgs.Header") - ros_type = type(ros_msg) - module_name = ros_type.__module__ # e.g., "std_msgs.msg" - class_name = ros_type.__name__ # e.g., "Header" - - # std_msgs.msg.Header -> dimos_lcm.std_msgs.Header - package = module_name.split(".")[0] # e.g., "std_msgs" - return lcm_msg_type(f"{package}.{class_name}")() - - -def _create_ros_instance_for_lcm_msg(lcm_msg: Any, ros_type_hint: str) -> Any: - """Create a ROS message instance that matches the LCM message type. - - Args: - lcm_msg: LCM message to match - ros_type_hint: ROS type hint string (e.g., "sequence") - - Returns: - New ROS message instance - """ - # Parse the type hint to get the message type - # e.g., "sequence" -> "sensor_msgs", "PointField" - # e.g., "sensor_msgs/PointField" -> "sensor_msgs", "PointField" - - match = re.search(r"(\w+)/(\w+)", ros_type_hint) - if match: - package, class_name = match.groups() - ros_module = importlib.import_module(f"{package}.msg") - ros_type = getattr(ros_module, class_name) - return ros_type() - - # Fallback: try to derive from LCM type - lcm_type = type(lcm_msg) - module_name = lcm_type.__module__ # e.g., "dimos_lcm.std_msgs.Header" - class_name = lcm_type.__name__ - parts = module_name.split(".") - if len(parts) >= 2: - package = parts[1] # e.g., "std_msgs" - ros_module = importlib.import_module(f"{package}.msg") - ros_type = getattr(ros_module, class_name) - return ros_type() - - raise ValueError(f"Cannot determine ROS type for LCM message: {lcm_type}") + """Resolve the matching installed ROS type by canonical package/msg/Type name.""" + parts = dimos_type.msg_name.split("/") + if len(parts) != 3 or parts[1] != "msg" or not all(part.isidentifier() for part in parts): + raise ValueError(f"Invalid message name {dimos_type.msg_name!r}; expected package/msg/Type") + package, _, name = parts + return cast("type[ROSMessage]", getattr(importlib.import_module(f"{package}.msg"), name)) def dimos_to_ros(msg: DimosMsg, ros_type: type[ROSMessage]) -> ROSMessage: - """Convert a dimos message to a ROS message. - - For complex types (PointCloud2, Image, CameraInfo), uses LCM roundtrip - to properly convert internal representations. For simple types, uses - direct field copy. - - Args: - msg: Dimos message instance - ros_type: Target ROS message type - - Returns: - ROS message instance - """ - msg_name = type(msg).msg_name - - if msg_name in COMPLEX_TYPES: - # Complex: dimos → encode → decode LCM → copy to ROS - lcm_type = derive_lcm_type(type(msg)) - lcm_bytes = msg.lcm_encode() - lcm_msg = lcm_type.lcm_decode(lcm_bytes) - ros_msg = ros_type() - _copy_lcm_to_ros_recursive(lcm_msg, ros_msg) - return ros_msg - - # Simple: recursive field copy (handles nested messages) - ros_msg = ros_type() - _copy_lcm_to_ros_recursive(msg, ros_msg) - return ros_msg - - -def ros_to_dimos(msg: Any, dimos_type: type[DimosMsg]) -> DimosMsg: - """Convert a ROS message to a dimos message. - - For complex types (PointCloud2, Image, CameraInfo), uses LCM roundtrip - to properly build the dimos internal representation. For simple types, - uses direct field copy. - - Args: - msg: ROS message instance - dimos_type: Target dimos message type - - Returns: - Dimos message instance - """ - msg_name = dimos_type.msg_name - - if msg_name in COMPLEX_TYPES: - # Complex: ROS → LCM → encode → decode → dimos - lcm_type = derive_lcm_type(dimos_type) - lcm_msg = lcm_type() - _copy_ros_to_lcm_recursive(msg, lcm_msg) - return dimos_type.lcm_decode(lcm_msg.lcm_encode()) - - # Simple type: recursive field copy (handles nested messages) - dimos_msg = dimos_type() - _copy_ros_to_lcm_recursive(msg, dimos_msg) - return dimos_msg + """Deserialize generated CDR with ROS's own type support, preserving every field.""" + if ros_serialization is None: + raise ImportError("ROS message conversion requires rclpy; install and source ROS 2.") + if derive_ros_type(type(msg)) is not ros_type: + raise TypeError(f"ROS target does not match {msg.msg_name}") + return cast("ROSMessage", ros_serialization.deserialize_message(msg.encode(), ros_type)) + + +def ros_to_dimos(msg: Any, dimos_type: type[MessageT]) -> MessageT: + """Decode ROS CDR with the generated codec; no LCM or field-copy adapter is needed.""" + if ros_serialization is None: + raise ImportError("ROS message conversion requires rclpy; install and source ROS 2.") + if type(msg) is not derive_ros_type(dimos_type): + raise TypeError(f"ROS source does not match {dimos_type.msg_name}") + return dimos_type.decode(ros_serialization.serialize_message(msg)) diff --git a/dimos/protocol/pubsub/impl/shmpubsub.py b/dimos/protocol/pubsub/impl/shmpubsub.py index a0421f30d1d..a7749824c54 100644 --- a/dimos/protocol/pubsub/impl/shmpubsub.py +++ b/dimos/protocol/pubsub/impl/shmpubsub.py @@ -29,7 +29,7 @@ import numpy as np import numpy.typing as npt -from dimos.protocol.pubsub.encoders import LCMEncoderMixin, PickleEncoderMixin +from dimos.protocol.pubsub.encoders import CDREncoderMixin, PickleEncoderMixin from dimos.protocol.pubsub.impl.lcmpubsub import Topic from dimos.protocol.pubsub.shm.ipc_factory import CpuShmChannel, FrameChannel from dimos.protocol.pubsub.spec import PubSub @@ -332,7 +332,7 @@ class PickleSharedMemory( ... -class LCMSharedMemoryPubSubBase(PubSub[Topic, Any]): +class TypedSharedMemoryPubSubBase(PubSub[Topic, Any]): """SharedMemory pubsub that uses LCM Topic type, delegating to SharedMemoryPubSubBase.""" def __init__(self, **kwargs: Any) -> None: @@ -360,10 +360,10 @@ def reconfigure(self, topic: Topic, *, capacity: int) -> dict: # type: ignore[t return self._shm.reconfigure(str(topic), capacity=capacity) -class LCMSharedMemory( # type: ignore[misc] - LCMEncoderMixin, - LCMSharedMemoryPubSubBase, +class CDRSharedMemory( # type: ignore[misc] + CDREncoderMixin, + TypedSharedMemoryPubSubBase, ): - """SharedMemory pubsub that uses LCM binary encoding (no pickle overhead).""" + """SharedMemory pubsub that uses CDR encoding (no pickle overhead).""" ... diff --git a/dimos/protocol/pubsub/impl/test_jpeg_cdr.py b/dimos/protocol/pubsub/impl/test_jpeg_cdr.py new file mode 100644 index 00000000000..b1827a2abf9 --- /dev/null +++ b/dimos/protocol/pubsub/impl/test_jpeg_cdr.py @@ -0,0 +1,76 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +# Copyright 2026 Dimensional Inc. +# SPDX-License-Identifier: Apache-2.0 + +import pickle +from types import SimpleNamespace + +from dimos_generated.builtin_interfaces.msg import Time +from dimos_generated.sensor_msgs.msg import CompressedImage, Image +from dimos_generated.std_msgs.msg import Header +import numpy as np +import pytest + +from dimos.core.transport import JpegLcmTransport +from dimos.msgs.image import image_from_array, image_from_compressed, image_view +from dimos.protocol.pubsub.encoders import DecodingError +from dimos.protocol.pubsub.impl.jpeg_lcm import JpegEncoderMixin +from dimos.protocol.pubsub.impl.jpeg_shm import JpegSharedMemoryEncoderMixin + + +@pytest.mark.parametrize("codec", [JpegEncoderMixin(), JpegSharedMemoryEncoderMixin(quality=90)]) +def test_jpeg_wire_is_standard_cdr_and_preserves_exact_image_header(codec): + header = Header(frame_id="optical", stamp=Time(sec=-1, nanosec=987654321)) + image = image_from_array( + np.full((16, 16, 3), [20, 80, 140], dtype=np.uint8), encoding="rgb8", header=header + ) + topic = SimpleNamespace(topic="/image", msg_type=CompressedImage) + before = image.encode() + encoded = codec.encode(image, topic) + wire = CompressedImage.decode(encoded) + assert encoded[:4] == b"\x00\x01\x00\x00" + assert wire.header == header + assert wire.format == "rgb8; jpeg compressed bgr8" + decoded = codec.decode(encoded, topic) + assert decoded.encoding == "bgr8" and decoded.header == header + np.testing.assert_allclose(image_view(decoded)[0, 0], [140, 80, 20], atol=3) + assert image.encode() == before + + +def test_jpeg_lcm_requires_truthful_compressed_wire_type(): + codec = JpegEncoderMixin() + with pytest.raises(DecodingError, match="CompressedImage"): + codec.decode(b"", SimpleNamespace(topic="/image", msg_type=Image)) + with pytest.raises(DecodingError, match="SELF_TEST"): + codec.decode(b"", SimpleNamespace(topic="LCM_SELF_TEST", msg_type=CompressedImage)) + + +def test_jpeg_transport_wire_metadata_and_pickle_keep_separate_application_type(monkeypatch): + monkeypatch.setattr( + "dimos.protocol.pubsub.impl.jpeg_lcm.JpegLCM", lambda **kwargs: SimpleNamespace() + ) + transport = JpegLcmTransport("/image", Image) + assert transport.topic.msg_type is CompressedImage + restored = pickle.loads(pickle.dumps(transport)) + assert restored.image_type is Image + assert restored.topic.msg_type is CompressedImage + + +def test_compressed_image_rejects_invalid_data_and_unknown_format(): + with pytest.raises(ValueError, match="invalid compressed"): + image_from_compressed(CompressedImage(format="jpeg", data=b"not jpeg")) + with pytest.raises(ValueError, match="unsupported compressed"): + image_from_compressed(CompressedImage(format="h264", data=b"not jpeg")) diff --git a/dimos/protocol/pubsub/impl/test_lcmpubsub.py b/dimos/protocol/pubsub/impl/test_lcmpubsub.py index 4ab7030b107..92a827d0289 100644 --- a/dimos/protocol/pubsub/impl/test_lcmpubsub.py +++ b/dimos/protocol/pubsub/impl/test_lcmpubsub.py @@ -13,14 +13,12 @@ # limitations under the License. from collections.abc import Iterator -import threading from typing import Any +from dimos_generated.geometry_msgs.msg import Point, Pose, Quaternion, Vector3 +from dimos_generated.std_msgs.msg import String import pytest -from dimos.msgs.geometry_msgs.Pose import Pose -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Vector3 import Vector3 from dimos.protocol.pubsub.impl.lcmpubsub import ( LCM, LCMPubSubBase, @@ -54,34 +52,15 @@ def lcm(lcm_url: str) -> Iterator[LCM]: lcm.stop() -class MockLCMMessage: - """Mock LCM message for testing""" - - msg_name = "geometry_msgs.Mock" - - def __init__(self, data: Any) -> None: - self.data = data - - def lcm_encode(self) -> bytes: - return str(self.data).encode("utf-8") - - @classmethod - def lcm_decode(cls, data: bytes) -> "MockLCMMessage": - return cls(data.decode("utf-8")) - - def __eq__(self, other: object) -> bool: - return isinstance(other, MockLCMMessage) and self.data == other.data - - def test_LCMPubSubBase_pubsub(lcm_pub_sub_base: LCMPubSubBase) -> None: lcm = lcm_pub_sub_base collector = CallbackCollector(1) - topic = Topic(topic="/test_topic", lcm_type=MockLCMMessage) - test_message = MockLCMMessage("test_data") + topic = Topic(topic="/test_topic", msg_type=String) + test_message = String(data="test_data") lcm.subscribe(topic, collector) - lcm.publish(topic, test_message.lcm_encode()) + lcm.publish(topic, test_message.encode()) collector.wait() assert len(collector.results) == 1 @@ -90,31 +69,17 @@ def test_LCMPubSubBase_pubsub(lcm_pub_sub_base: LCMPubSubBase) -> None: received_topic = collector.results[0][1] assert isinstance(received_data, bytes) - assert received_data.decode() == "test_data" + assert String.decode(received_data).data == "test_data" assert isinstance(received_topic, Topic) assert received_topic == topic -def test_subscribe_calls_lcm_warmup(lcm: LCM) -> None: - warmup_threads = [] - - class WarmupMessage(MockLCMMessage): - @classmethod - def lcm_warmup(cls) -> None: - warmup_threads.append(threading.current_thread()) - - lcm.subscribe(Topic(topic="/warmup", lcm_type=WarmupMessage), lambda msg, topic: None) - - # Warmed synchronously on the subscriber's thread, not the LCM handler thread. - assert warmup_threads == [threading.current_thread()] - - def test_lcm_autodecoder_pubsub(lcm: LCM) -> None: collector = CallbackCollector(1) - topic = Topic(topic="/test_topic", lcm_type=MockLCMMessage) - test_message = MockLCMMessage("test_data") + topic = Topic(topic="/test_topic", msg_type=String) + test_message = String(data="test_data") lcm.subscribe(topic, collector) lcm.publish(topic, test_message) @@ -125,17 +90,47 @@ def test_lcm_autodecoder_pubsub(lcm: LCM) -> None: received_data = collector.results[0][0] received_topic = collector.results[0][1] - assert isinstance(received_data, MockLCMMessage) - assert received_data == test_message + assert isinstance(received_data, String) + assert received_data.encode() == test_message.encode() assert isinstance(received_topic, Topic) assert received_topic == topic +def test_invalid_cdr_does_not_stop_the_subscriber(lcm: LCM) -> None: + topic = Topic("/test_corrupt", String) + collector = CallbackCollector(1) + lcm.subscribe(topic, collector) + + lcm.publish(topic, b"invalid CDR") + lcm.publish(topic, String(data="after malformed payload")) + collector.wait() + + assert [message.data for message, _ in collector.results] == ["after malformed payload"] + + +def test_different_message_types_cannot_be_published_on_a_typed_topic(lcm: LCM) -> None: + with pytest.raises(ValueError, match="does not match"): + lcm.publish(Topic("/point", Vector3), Quaternion(w=1)) + + +def test_explicit_unknown_type_does_not_use_default_decoder() -> None: + topic = Topic.from_channel_str("/point#unknown_msgs/msg/Point", Vector3) + + assert topic.topic == "/point" + assert topic.msg_type is None + + +@pytest.mark.parametrize("channel", ["x" * 64, "invalid\0channel", "é" * 32]) +def test_invalid_lcm_channel_fails_before_sending(lcm: LCM, channel: str) -> None: + with pytest.raises(ValueError, match="63 bytes"): + lcm.publish(Topic(channel), b"payload") + + test_msgs = [ - (Vector3(1, 2, 3)), - (Quaternion(1, 2, 3, 4)), - (Pose(Vector3(1, 2, 3), Quaternion(0, 0, 0, 1))), + (Vector3(x=1, y=2, z=3)), + (Quaternion(x=1, y=2, z=3, w=4)), + (Pose(position=Point(x=1, y=2, z=3), orientation=Quaternion(w=1))), ] @@ -144,7 +139,7 @@ def test_lcm_autodecoder_pubsub(lcm: LCM) -> None: def test_lcm_geometry_msgs_pubsub(test_message: Any, lcm: LCM) -> None: collector = CallbackCollector(1) - topic = Topic(topic="/test_topic", lcm_type=test_message.__class__) + topic = Topic(topic="/test_topic", msg_type=test_message.__class__) lcm.subscribe(topic, collector) lcm.publish(topic, test_message) @@ -156,7 +151,7 @@ def test_lcm_geometry_msgs_pubsub(test_message: Any, lcm: LCM) -> None: received_topic = collector.results[0][1] assert isinstance(received_data, test_message.__class__) - assert received_data == test_message + assert received_data.encode() == test_message.encode() assert isinstance(received_topic, Topic) assert received_topic == topic @@ -180,7 +175,7 @@ def test_lcm_geometry_msgs_autopickle_pubsub(test_message: Any, pickle_lcm: Pick received_topic = collector.results[0][1] assert isinstance(received_data, test_message.__class__) - assert received_data == test_message + assert received_data.encode() == test_message.encode() assert isinstance(received_topic, Topic) assert received_topic == topic diff --git a/dimos/protocol/pubsub/impl/test_rospubsub.py b/dimos/protocol/pubsub/impl/test_rospubsub.py index 89be14cd296..48878767833 100644 --- a/dimos/protocol/pubsub/impl/test_rospubsub.py +++ b/dimos/protocol/pubsub/impl/test_rospubsub.py @@ -12,237 +12,73 @@ # See the License for the specific language governing permissions and # limitations under the License. +"""Generated CDR streams through the optional ROS2 pub/sub bridge.""" + from collections.abc import Generator +import threading +import uuid -from dimos_lcm.geometry_msgs import PointStamped -import numpy as np +from dimos_generated.builtin_interfaces.msg import Time +from dimos_generated.geometry_msgs.msg import Point, PointStamped, PoseStamped, Twist, Vector3 +from dimos_generated.sensor_msgs.msg import Image, PointCloud2, PointField +from dimos_generated.std_msgs.msg import Header import pytest -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.geometry_msgs.Twist import Twist -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 -from dimos.protocol.pubsub.impl.rospubsub import DimosROS, ROSTopic +from dimos.protocol.pubsub.impl.rospubsub import ROS_AVAILABLE, DimosROS, ROSTopic -# Add msg_name to LCM PointStamped for testing nested message conversion -PointStamped.msg_name = "geometry_msgs.PointStamped" -from dimos.utils.testing.collector import CallbackCollector -from dimos.utils.testing.replay import TimedSensorReplay +pytestmark = pytest.mark.skipif(not ROS_AVAILABLE, reason="requires ROS 2") -def ros_node(): - ros = DimosROS() - ros.start() +@pytest.fixture() +def nodes() -> Generator[tuple[DimosROS, DimosROS], None, None]: + publisher, subscriber = DimosROS(), DimosROS() try: - yield ros + publisher.start() + subscriber.start() + yield publisher, subscriber finally: - ros.stop() - - -@pytest.fixture() -def publisher() -> Generator[DimosROS, None, None]: - yield from ros_node() - - -@pytest.fixture() -def subscriber() -> Generator[DimosROS, None, None]: - yield from ros_node() - - -@pytest.mark.skipif_no_ros -def test_basic_conversion(publisher, subscriber): - """Test Vector3 publish/subscribe through ROS. - - Simple flat dimos.msgs type with no nesting (just x/y/z floats). - """ - topic = ROSTopic("/test_ros_topic", Vector3) - collector = CallbackCollector(1) - - subscriber.subscribe(topic, collector) - publisher.publish(topic, Vector3(1.0, 2.0, 3.0)) - - collector.wait() - assert len(collector.results) == 1 - msg = collector.results[0][0] - assert msg.x == 1.0 - assert msg.y == 2.0 - assert msg.z == 3.0 - - -@pytest.mark.skipif_no_ros -@pytest.mark.self_hosted -def test_pointcloud2_pubsub(publisher, subscriber): - """Test PointCloud2 publish/subscribe through ROS. - - COMPLEX_TYPE - has non-standard attributes (numpy arrays, custom accessors) - that can't be treated like a standard message with direct field copy. - Uses LCM encode/decode roundtrip to properly convert internal representation. - """ - # Load real lidar data from replay (5 seconds in) - replay = TimedSensorReplay("go2_bigoffice/lidar") - original = replay.find_closest_seek(5.0) - - assert original is not None, "Failed to load lidar data from replay" - assert len(original) > 0, "Loaded empty pointcloud" - - topic = ROSTopic("/test_pointcloud2", PointCloud2) - collector = CallbackCollector(1, timeout=5.0) - - subscriber.subscribe(topic, collector) - publisher.publish(topic, original) - - collector.wait() - assert len(collector.results) == 1 - - converted = collector.results[0][0] - - # Verify point cloud data is preserved - original_points, _ = original.as_numpy() - converted_points, _ = converted.as_numpy() - - assert len(original_points) == len(converted_points), ( - f"Point count mismatch: {len(original_points)} vs {len(converted_points)}" - ) - - np.testing.assert_allclose( - original_points, - converted_points, - rtol=1e-5, - atol=1e-5, - err_msg="Points don't match after ROS pubsub roundtrip", - ) - - # Verify frame_id is preserved - assert converted.frame_id == original.frame_id - - # Verify timestamp is preserved (within 1ms tolerance) - assert abs(original.ts - converted.ts) < 0.001 - - -@pytest.mark.skipif_no_ros -def test_pointcloud2_empty_pubsub(publisher, subscriber): - """Test empty PointCloud2 publish/subscribe. - - Edge case for COMPLEX_TYPE with zero points. - """ - original = PointCloud2.from_numpy( - np.array([]).reshape(0, 3), - frame_id="empty_frame", - timestamp=1234567890.0, - ) - - topic = ROSTopic("/test_empty_pointcloud", PointCloud2) - collector = CallbackCollector(1) - - subscriber.subscribe(topic, collector) - publisher.publish(topic, original) - - collector.wait() - assert len(collector.results) == 1 - assert len(collector.results[0][0]) == 0 - - -@pytest.mark.skipif_no_ros -def test_posestamped_pubsub(publisher, subscriber): - """Test PoseStamped publish/subscribe through ROS. - - COMPLEX_TYPE with custom dimos.msgs implementation and nested messages - (Header, Pose containing Point and Quaternion). Uses LCM roundtrip. - """ - original = PoseStamped( - ts=1234567890.123456, - frame_id="base_link", - position=[1.0, 2.0, 3.0], - orientation=[0.0, 0.0, 0.7071068, 0.7071068], # 90 degree yaw - ) - - topic = ROSTopic("/test_posestamped", PoseStamped) - collector = CallbackCollector(1) - - subscriber.subscribe(topic, collector) - publisher.publish(topic, original) - - collector.wait() - assert len(collector.results) == 1 - - converted = collector.results[0][0] - - # Verify all fields preserved - assert converted.frame_id == original.frame_id - assert abs(converted.ts - original.ts) < 0.001 # 1ms tolerance - assert converted.x == original.x - assert converted.y == original.y - assert converted.z == original.z - np.testing.assert_allclose(converted.orientation.z, original.orientation.z, rtol=1e-5) - np.testing.assert_allclose(converted.orientation.w, original.orientation.w, rtol=1e-5) - - -@pytest.mark.skipif_no_ros -def test_pointstamped_pubsub(publisher, subscriber): - """Test PointStamped publish/subscribe through ROS. - - Raw LCM type with nested messages (Header, Point) but NO custom dimos.msgs - implementation. Tests recursive field copy for non-COMPLEX_TYPES. - """ - original = PointStamped() - original.header.stamp.sec = 1234567890 - original.header.stamp.nsec = 123456000 - original.header.frame_id = "map" - original.point.x = 1.5 - original.point.y = 2.5 - original.point.z = 3.5 - - topic = ROSTopic("/test_pointstamped", PointStamped) - collector = CallbackCollector(1) - - subscriber.subscribe(topic, collector) - publisher.publish(topic, original) - - collector.wait() - assert len(collector.results) == 1 - - converted = collector.results[0][0] - - # Verify nested header fields are preserved - assert converted.header.frame_id == original.header.frame_id - assert converted.header.stamp.sec == original.header.stamp.sec - assert converted.header.stamp.nsec == original.header.stamp.nsec - - # Verify point coordinates are preserved - assert converted.point.x == original.point.x - assert converted.point.y == original.point.y - assert converted.point.z == original.point.z - - -@pytest.mark.skipif_no_ros -def test_twist_pubsub(publisher, subscriber): - """Test Twist publish/subscribe through ROS. - - dimos.msgs type with nested Vector3 messages (linear, angular). - Tests recursive field copy with custom dimos.msgs nested types. - """ - original = Twist( - linear=[1.0, 2.0, 3.0], - angular=[0.1, 0.2, 0.3], - ) - - topic = ROSTopic("/test_twist", Twist) - collector = CallbackCollector(1) - - subscriber.subscribe(topic, collector) - publisher.publish(topic, original) - - collector.wait() - assert len(collector.results) == 1 - - converted = collector.results[0][0] - - # Verify linear velocity preserved - assert converted.linear.x == original.linear.x - assert converted.linear.y == original.linear.y - assert converted.linear.z == original.linear.z - - # Verify angular velocity preserved - assert converted.angular.x == original.angular.x - assert converted.angular.y == original.angular.y - assert converted.angular.z == original.angular.z + subscriber.stop() + publisher.stop() + + +@pytest.mark.parametrize( + "original", + [ + Vector3(x=1.0, y=2.0, z=3.0), + Twist(linear=Vector3(x=1.0), angular=Vector3(z=-0.5)), + PoseStamped(header=Header(stamp=Time(sec=1700000000, nanosec=123456789), frame_id="map")), + PointStamped(header=Header(stamp=Time(sec=-1, nanosec=999999999)), point=Point(x=1.5)), + PointCloud2(), + PointCloud2( + height=1, + width=1, + fields=[PointField(name="x", datatype=7, count=1)], + point_step=4, + row_step=4, + data=bytes([0, 0, 128, 63]), + ), + Image(height=1, width=2, encoding="rgb8", step=6, data=bytes([0, 1, 2, 253, 254, 255])), + ], + ids=lambda msg: msg.msg_name, +) +def test_generated_pubsub(nodes, original): + publisher, subscriber = nodes + topic = ROSTopic(f"/dimos_cdr_test_{uuid.uuid4().hex}", type(original)) + received = threading.Event() + values = [] + + def collect(message, _topic): + values.append(message) + received.set() + + unsubscribe = subscriber.subscribe(topic, collect) + node = publisher._raw._node + assert node is not None + # A live periodic source tolerates asynchronous DDS discovery without a fixed sleep. + timer = node.create_timer(0.05, lambda: publisher.publish(topic, original)) + try: + assert received.wait(10), "ROS subscription did not receive the generated message" + assert values[0] == original + finally: + node.destroy_timer(timer) + unsubscribe() diff --git a/dimos/protocol/pubsub/impl/test_rospubsub_conversion.py b/dimos/protocol/pubsub/impl/test_rospubsub_conversion.py new file mode 100644 index 00000000000..c9d025e947f --- /dev/null +++ b/dimos/protocol/pubsub/impl/test_rospubsub_conversion.py @@ -0,0 +1,133 @@ +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Conformance with installed ROS type support; skipped when ROS is absent.""" + +from dimos_generated.builtin_interfaces.msg import Time +from dimos_generated.geometry_msgs.msg import ( + Point, + PointStamped, + Pose, + PoseStamped, + Quaternion, + TransformStamped, + Vector3, +) +from dimos_generated.sensor_msgs.msg import ( + CameraInfo, + CompressedImage, + Image, + PointCloud2, + PointField, +) +from dimos_generated.std_msgs.msg import Header +from dimos_generated.tf2_msgs.msg import TFMessage +import pytest + +from dimos.protocol.pubsub.impl import rospubsub_conversion as conversion + +requires_ros = pytest.mark.skipif(conversion.ros_serialization is None, reason="requires ROS 2") + + +def messages(): + header = Header(stamp=Time(sec=1700000000, nanosec=123456789), frame_id="map/相機") + return [ + Point(x=1.25, y=-2.5, z=3.75), + PointStamped(header=header, point=Point(x=2.0)), + PoseStamped(header=header, pose=Pose(orientation=Quaternion(z=0.6, w=0.8))), + Image( + header=header, + height=1, + width=2, + encoding="rgb8", + step=6, + data=bytes([0, 1, 2, 253, 254, 255]), + ), + CompressedImage(header=header, format="jpeg", data=bytes([0, 255, 127])), + CameraInfo( + header=header, + width=640, + height=480, + distortion_model="plumb_bob", + d=[0.1, -0.2], + k=[1.0, 0.0, 3.0, 0.0, 2.0, 4.0, 0.0, 0.0, 1.0], + ), + PointCloud2( + header=header, + height=1, + width=1, + fields=[PointField(name="x", offset=4, datatype=7, count=1)], + is_bigendian=True, + point_step=8, + row_step=12, + data=bytes([9, 8, 7, 6, 0x3F, 0x80, 0, 0, 5, 4, 3, 2]), + ), + PointCloud2(header=header), + TFMessage(), + TFMessage(transforms=[TransformStamped(header=header, child_frame_id="camera")]), + ] + + +@requires_ros +@pytest.mark.parametrize("original", messages(), ids=lambda msg: msg.msg_name) +def test_generated_fields_survive_ros_serialization(original): + ros_type = conversion.derive_ros_type(type(original)) + ros_message = conversion.dimos_to_ros(original, ros_type) + converted = conversion.ros_to_dimos(ros_message, type(original)) + assert converted == original + + +@requires_ros +@pytest.mark.parametrize("sec,nanosec", [(0, 0), (-1, 999999999), (1700000000, 123456789)]) +def test_ros_origin_timestamp_and_nested_fields_are_exact(sec, nanosec): + ros_type = conversion.derive_ros_type(PointStamped) + source = ros_type() + source.header.stamp.sec = sec + source.header.stamp.nanosec = nanosec + source.header.frame_id = "odom" + source.point.x, source.point.y, source.point.z = 1.5, -2.5, 3.5 + + converted = conversion.ros_to_dimos(source, PointStamped) + assert converted == PointStamped( + header=Header(stamp=Time(sec=sec, nanosec=nanosec), frame_id="odom"), + point=Point(x=1.5, y=-2.5, z=3.5), + ) + assert conversion.dimos_to_ros(converted, ros_type) == source + + +@requires_ros +def test_matching_layout_does_not_allow_wrong_type(): + from_type = conversion.derive_ros_type(Point) + to_type = conversion.derive_ros_type(Vector3) + with pytest.raises(TypeError, match="target does not match"): + conversion.dimos_to_ros(Point(), to_type) + with pytest.raises(TypeError, match="source does not match"): + conversion.ros_to_dimos(from_type(), Vector3) + + +def test_missing_ros_is_explicit_and_does_not_affect_generated_messages(monkeypatch): + monkeypatch.setattr(conversion, "ros_serialization", None) + message = Point(x=1.0) + assert Point.decode(message.encode()) == message + with pytest.raises(ImportError, match="requires rclpy"): + conversion.dimos_to_ros(message, Point) + with pytest.raises(ImportError, match="requires rclpy"): + conversion.ros_to_dimos(message, Point) + + +@pytest.mark.parametrize("name", ["geometry_msgs.Point", "geometry_msgs/srv/Point", "../msg/Point"]) +def test_invalid_type_name_fails_before_import(name): + invalid = type("Invalid", (), {"msg_name": name}) + with pytest.raises(ValueError, match="expected package/msg/Type"): + conversion.derive_ros_type(invalid) diff --git a/dimos/protocol/pubsub/impl/test_zenohpubsub.py b/dimos/protocol/pubsub/impl/test_zenohpubsub.py index 6c179740283..0908540afdc 100644 --- a/dimos/protocol/pubsub/impl/test_zenohpubsub.py +++ b/dimos/protocol/pubsub/impl/test_zenohpubsub.py @@ -22,10 +22,10 @@ pytest.importorskip("zenoh") +from dimos_generated.geometry_msgs.msg import Twist +from dimos_generated.sensor_msgs.msg import Image import zenoh -from dimos.msgs.geometry_msgs.Twist import Twist -from dimos.msgs.sensor_msgs.Image import Image from dimos.protocol.pubsub.impl.lcmpubsub import Topic as LCMTopic from dimos.protocol.pubsub.impl.zenohpubsub import ( Topic, @@ -152,7 +152,7 @@ def race() -> None: def test_concrete_subscription_passes_topic_through(self, pubsub, retry_until) -> None: received: list[Topic] = [] event = threading.Event() - topic = Topic("dimos/test/passthrough", lcm_type=Twist) + topic = Topic("dimos/test/passthrough", msg_type=Twist) def callback(msg: bytes, t: Topic) -> None: received.append(t) @@ -167,7 +167,7 @@ def test_untyped_topic_with_dotted_segment_round_trips(self, pubsub, retry_until # must not re-parse it into base+type on receive. received: list[Topic] = [] event = threading.Event() - topic = Topic("dimos/test/geometry_msgs.Twist") + topic = Topic("dimos/test/geometry_msgs/msg/Twist") def callback(msg: bytes, t: Topic) -> None: received.append(t) @@ -270,9 +270,9 @@ class TestTopicKeyExprConversion: """Tests for _topic_to_key_expr and _key_expr_to_topic round-trip.""" def test_typed_topic_to_key_expr(self) -> None: - topic = Topic("dimos/cmd_vel", lcm_type=Twist) + topic = Topic("dimos/cmd_vel", msg_type=Twist) key = _topic_to_key_expr(topic) - assert key == "dimos/cmd_vel/geometry_msgs.Twist" + assert key == "dimos/cmd_vel/geometry_msgs/msg/Twist" def test_untyped_topic_to_key_expr(self) -> None: topic = Topic("dimos/data") @@ -280,36 +280,36 @@ def test_untyped_topic_to_key_expr(self) -> None: assert key == "dimos/data" def test_key_expr_to_topic_with_known_type(self) -> None: - topic = _key_expr_to_topic("dimos/cmd_vel/geometry_msgs.Twist") + topic = _key_expr_to_topic("dimos/cmd_vel/geometry_msgs/msg/Twist") assert topic.topic == "dimos/cmd_vel" - assert topic.lcm_type is Twist + assert topic.msg_type is Twist def test_key_expr_to_topic_with_unknown_type(self) -> None: - topic = _key_expr_to_topic("dimos/data/unknown.FooBar") - # Last segment doesn't resolve — entire string becomes the topic - assert topic.topic == "dimos/data/unknown.FooBar" - assert topic.lcm_type is None + topic = _key_expr_to_topic("dimos/data/unknown/msg/FooBar", default_msg_type=Twist) + # Unknown declared types cannot fall back to a subscriber codec + assert topic.topic == "dimos/data" + assert topic.msg_type is None def test_key_expr_to_topic_with_no_slash(self) -> None: topic = _key_expr_to_topic("simple_topic") assert topic.topic == "simple_topic" - assert topic.lcm_type is None + assert topic.msg_type is None def test_key_expr_to_topic_uses_default_type(self) -> None: - topic = _key_expr_to_topic("dimos/data", default_lcm_type=Twist) + topic = _key_expr_to_topic("dimos/data", default_msg_type=Twist) assert topic.topic == "dimos/data" - assert topic.lcm_type is Twist + assert topic.msg_type is Twist def test_round_trip_typed(self) -> None: - original = Topic("dimos/color_image", lcm_type=Image) + original = Topic("dimos/color_image", msg_type=Image) key = _topic_to_key_expr(original) reconstructed = _key_expr_to_topic(key) assert reconstructed.topic == original.topic - assert reconstructed.lcm_type is original.lcm_type + assert reconstructed.msg_type is original.msg_type def test_round_trip_untyped(self) -> None: original = Topic("dimos/gps_location") key = _topic_to_key_expr(original) reconstructed = _key_expr_to_topic(key) assert reconstructed.topic == original.topic - assert reconstructed.lcm_type is None + assert reconstructed.msg_type is None diff --git a/dimos/protocol/pubsub/impl/webrtc/providers/broker.py b/dimos/protocol/pubsub/impl/webrtc/providers/broker.py index 2b733c58894..1b8d10dfde1 100644 --- a/dimos/protocol/pubsub/impl/webrtc/providers/broker.py +++ b/dimos/protocol/pubsub/impl/webrtc/providers/broker.py @@ -104,7 +104,7 @@ class BrokerProvider(AsyncProviderBase): Inbound (operator → robot): ``cmd_unreliable`` + ``state_reliable``; subscribers get the bytes of the channel matching their topic, and typed - demux by LCM fingerprint happens at the transport layer. Outbound + demux by explicit frame type happens at the transport layer. Outbound (robot → operator): ``publish()`` on ``state_reliable_back`` / ``map_unreliable``; while no operator is connected the channel doesn't exist and messages drop, which is normal pubsub behaviour. Media rides the @@ -654,7 +654,7 @@ def set_video_frame(self, img: Any) -> None: def subscribe(self, topic: str, callback: Callable[[bytes, str], None]) -> Callable[[], None]: """Subscribers receive the bytes of the inbound channel matching - their topic; the transport layer filters by LCM fingerprint.""" + their topic; the transport layer filters by explicit frame channel and type.""" if not self.is_connected: self.start() with self._lock: diff --git a/dimos/protocol/pubsub/impl/webrtc/providers/test_video_track_generated.py b/dimos/protocol/pubsub/impl/webrtc/providers/test_video_track_generated.py new file mode 100644 index 00000000000..375a635b1cf --- /dev/null +++ b/dimos/protocol/pubsub/impl/webrtc/providers/test_video_track_generated.py @@ -0,0 +1,44 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +# Copyright 2026 Dimensional Inc. +# SPDX-License-Identifier: Apache-2.0 + +import asyncio + +import numpy as np +import pytest + +from dimos.msgs.image import image_from_array +from dimos.protocol.pubsub.impl.webrtc.providers.video_track import CameraVideoTrack + + +@pytest.mark.asyncio +@pytest.mark.parametrize( + "encoding,channels", [("rgb8", 3), ("bgr8", 3), ("rgba8", 4), ("bgra8", 4), ("mono8", 1)] +) +async def test_video_track_uses_generated_encoding_without_network(encoding, channels): + pixels = np.full((4, 4) if channels == 1 else (4, 4, channels), 127, dtype=np.uint8) + image = image_from_array(pixels, encoding=encoding) + original = image.encode() + track = CameraVideoTrack(asyncio.get_running_loop()) + try: + track.arm() + track.set_latest(image) + frame = await asyncio.wait_for(track.recv(), timeout=1) + assert frame.width == 4 and frame.height == 4 + assert frame.pts == 0 + assert image.encode() == original + finally: + track.stop() diff --git a/dimos/protocol/pubsub/impl/webrtc/providers/video_track.py b/dimos/protocol/pubsub/impl/webrtc/providers/video_track.py index 87034ec205c..e2b9adc2579 100644 --- a/dimos/protocol/pubsub/impl/webrtc/providers/video_track.py +++ b/dimos/protocol/pubsub/impl/webrtc/providers/video_track.py @@ -25,15 +25,16 @@ from aiortc.mediastreams import VIDEO_CLOCK_RATE, VIDEO_TIME_BASE, VideoStreamTrack import av +from dimos_generated.sensor_msgs.msg import Image -from dimos.msgs.sensor_msgs.Image import Image, ImageFormat +from dimos.msgs.image import image_view _AV_FORMAT_MAP = { - ImageFormat.BGR: "bgr24", - ImageFormat.RGB: "rgb24", - ImageFormat.BGRA: "bgra", - ImageFormat.RGBA: "rgba", - ImageFormat.GRAY: "gray", + "bgr8": "bgr24", + "rgb8": "rgb24", + "bgra8": "bgra", + "rgba8": "rgba", + "mono8": "gray", } @@ -99,7 +100,9 @@ async def recv(self) -> av.VideoFrame: self._first_mono = now pts = int((now - self._first_mono) * VIDEO_CLOCK_RATE) - frame = av.VideoFrame.from_ndarray(img.data, format=_AV_FORMAT_MAP.get(img.format, "bgr24")) + if img.encoding not in _AV_FORMAT_MAP: + raise ValueError(f"unsupported WebRTC video encoding {img.encoding!r}") + frame = av.VideoFrame.from_ndarray(image_view(img), format=_AV_FORMAT_MAP[img.encoding]) frame.pts = pts frame.time_base = VIDEO_TIME_BASE return frame diff --git a/dimos/protocol/pubsub/impl/webrtc/test_transport.py b/dimos/protocol/pubsub/impl/webrtc/test_transport.py index d2a81dd5103..c3bc29b1302 100644 --- a/dimos/protocol/pubsub/impl/webrtc/test_transport.py +++ b/dimos/protocol/pubsub/impl/webrtc/test_transport.py @@ -18,8 +18,8 @@ from collections.abc import Callable import pickle -import struct +from dimos_generated.geometry_msgs.msg import Point, TwistStamped, Vector3 from pydantic import ValidationError import pytest @@ -28,7 +28,6 @@ from dimos.core.coordination.blueprints import Blueprint from dimos.core.coordination.module_coordinator import _materialize_transports from dimos.core.transport import WebRTCTransport -from dimos.msgs.geometry_msgs.TwistStamped import TwistStamped from dimos.protocol.pubsub.impl.webrtc.providers import spec from dimos.protocol.pubsub.impl.webrtc.providers.broker import BrokerConfig from dimos.protocol.pubsub.impl.webrtc.providers.spec import ( @@ -36,6 +35,7 @@ ProviderConfig, ) from dimos.protocol.pubsub.impl.webrtc.webrtcpubsub import WebRTCPubSub +from dimos.web.relay_bridge.protocol import FrameHeader, decode_data_frame, encode_data_frame # ─── Mock provider ─────────────────────────────────────────────────── @@ -85,51 +85,6 @@ class MockTransport(WebRTCTransport): _config_cls = MockConfig -# ─── Fake LCM messages ─────────────────────────────────────────────── - - -class FakeLCMMsg: - msg_name = "test.FakeLCMMsg" - _FINGERPRINT = b"\x01\x02\x03\x04\x05\x06\x07\x08" - - def __init__(self, value: float = 0.0): - self.value = value - - @classmethod - def _get_packed_fingerprint(cls) -> bytes: - return cls._FINGERPRINT - - def lcm_encode(self) -> bytes: - return self._FINGERPRINT + struct.pack(" FakeLCMMsg: - if data[:8] != cls._FINGERPRINT: # like real LCM generated code - raise ValueError("Decode error") - return cls(struct.unpack(" bytes: - return cls._FINGERPRINT - - def lcm_encode(self) -> bytes: - return self._FINGERPRINT + self.text.encode() - - @classmethod - def lcm_decode(cls, data: bytes) -> OtherLCMMsg: - if data[:8] != cls._FINGERPRINT: # like real LCM generated code - raise ValueError("Decode error") - return cls(data[8:].decode()) - - # ─── Transport modes ───────────────────────────────────────────────── @@ -142,46 +97,71 @@ def test_raw_bytes_mode() -> None: def test_typed_encode_decode() -> None: - transport = MockTransport("cmd_unreliable", FakeLCMMsg, name="typed") - received: list[FakeLCMMsg] = [] + transport = MockTransport("cmd_unreliable", Point, name="typed") + received: list[Point] = [] transport.subscribe(lambda msg: received.append(msg)) - transport.broadcast(None, FakeLCMMsg(3.14)) + transport.broadcast(None, Point(x=3.14)) assert len(received) == 1 - assert abs(received[0].value - 3.14) < 1e-9 + assert abs(received[0].x - 3.14) < 1e-9 def test_multiple_types_multiplexed() -> None: """Typed transports sharing one channel each receive only their own type.""" - t1 = MockTransport("cmd_unreliable", FakeLCMMsg, name="mux") - t2 = MockTransport("cmd_unreliable", OtherLCMMsg, name="mux") - r1: list[FakeLCMMsg] = [] - r2: list[OtherLCMMsg] = [] + t1 = MockTransport("cmd_unreliable", Point, name="mux") + t2 = MockTransport("cmd_unreliable", Vector3, name="mux") + r1: list[Point] = [] + r2: list[Vector3] = [] t1.subscribe(lambda msg: r1.append(msg)) t2.subscribe(lambda msg: r2.append(msg)) - t1.broadcast(None, FakeLCMMsg(1.0)) - t2.broadcast(None, OtherLCMMsg("world")) - assert [m.value for m in r1] == [1.0] - assert [m.text for m in r2] == ["world"] - + t1.broadcast(None, Point(x=1.0)) + t2.broadcast(None, Vector3(x=2.0)) + assert [m.x for m in r1] == [1.0] + assert [m.x for m in r2] == [2.0] -def test_wire_fingerprint_matches_encoding() -> None: - """Demux must follow the wire format, not _get_packed_fingerprint(). - TwistStamped inherits Twist's fingerprint but encodes as LCM TwistStamped — - any filter keyed on the class fingerprint would drop every real message. - The try-decode demux delegates the check to lcm_decode, which gets it right. - """ - transport = MockTransport("cmd_unreliable", TwistStamped, name="wire") - received: list[TwistStamped] = [] - transport.subscribe(lambda msg: received.append(msg)) - - wire = TwistStamped(linear=[0.5, 0, 0], angular=[0, 0, 0.1], frame_id="keyboard").lcm_encode() - assert wire[:8] != TwistStamped._get_packed_fingerprint() +def test_generated_frames_declare_type_and_sequence() -> None: + publisher = MockTransport("data", Point, name="framed") + raw = MockTransport("data", name="framed") + received: list[bytes] = [] + raw.subscribe(received.append) + publisher.broadcast(None, Point(x=1)) + publisher.broadcast(None, Point(x=2)) + frames = [decode_data_frame(data) for data in received] + assert [frame.header.seq for frame in frames] == [0, 1] + assert all(frame.header.ch == "data" for frame in frames) + assert all(frame.header.meta == {"type": Point.msg_name, "encoding": "cdr"} for frame in frames) + assert [Point.decode(frame.payload).x for frame in frames] == [1, 2] + + +@pytest.mark.parametrize("bad", ["type", "topic", "encoding", "truncated", "trailing", "raw"]) +def test_invalid_frame_does_not_poison_subscription(bad: str) -> None: + transport = MockTransport("cmd_unreliable", Point, name=f"invalid-{bad}") + received: list[Point] = [] + transport.subscribe(received.append) + header = FrameHeader( + ch="cmd_unreliable", + seq=1, + ts=0, + delivery="latest", + meta={"type": Point.msg_name, "encoding": "cdr"}, + ) + if bad == "topic": + header.ch = "other" + elif bad in ("type", "encoding"): + assert header.meta is not None + header.meta[bad] = "unknown" + wire = encode_data_frame(header, Point(x=1).encode()) + if bad == "truncated": + wire = wire[:-1] + elif bad == "trailing": + wire += b"extra" + elif bad == "raw": + wire = Point(x=1).encode() transport._config.provider().publish("cmd_unreliable", wire) - assert len(received) == 1 - assert abs(received[0].linear.x - 0.5) < 1e-9 - assert received[0].frame_id == "keyboard" + assert received == [] + transport.broadcast(None, Point(x=2)) + assert received == [Point(x=2)] # ─── Pickling + provider sharing ───────────────────────────────────── @@ -190,20 +170,20 @@ def test_wire_fingerprint_matches_encoding() -> None: def test_pickle_roundtrip_preserves_everything() -> None: """Transports are pickled into module worker processes; topic, type, and provider config must all survive.""" - t1 = MockTransport("cmd_unreliable", FakeLCMMsg, name="pickled") + t1 = MockTransport("cmd_unreliable", Point, name="pickled") t2 = pickle.loads(pickle.dumps(t1)) assert type(t2) is MockTransport assert t2.topic == t1.topic - assert t2._msg_type is FakeLCMMsg + assert t2._msg_type is Point assert t2._config == t1._config # Same config → same per-process provider, so the two halves interoperate. - received: list[FakeLCMMsg] = [] + received: list[Point] = [] t2.subscribe(lambda msg: received.append(msg)) - t1.broadcast(None, FakeLCMMsg(42.0)) + t1.broadcast(None, Point(x=42.0)) assert len(received) == 1 - assert abs(received[0].value - 42.0) < 1e-9 + assert abs(received[0].x - 42.0) < 1e-9 def test_provider_singleton_per_config() -> None: @@ -261,7 +241,7 @@ def test_provider_config_is_frozen_and_hashable() -> None: def test_blueprint_config_exposes_transport_fields() -> None: """Each unique `_config_cls` becomes a `transports.` config section.""" - bp = Blueprint(blueprints=()).transports({("topic", FakeLCMMsg): MockTransport.spec("topic")}) + bp = Blueprint(blueprints=()).transports({("topic", Point): MockTransport.spec("topic")}) parser = BlueprintConfigParser(bp) parsed = parser.parse(environ={}, overrides={"transports": {"mock": {"name": "override"}}}) assert parsed.transport_overrides() == {"mock": {"name": "override"}} @@ -273,8 +253,8 @@ def test_blueprint_config_exposes_transport_fields() -> None: # Multiple transports sharing one `_config_cls` collapse to one section. bp_shared = Blueprint(blueprints=()).transports( { - ("a", FakeLCMMsg): MockTransport.spec("a"), - ("b", FakeLCMMsg): MockTransport.spec("b"), + ("a", Point): MockTransport.spec("a"), + ("b", Point): MockTransport.spec("b"), } ) shared = BlueprintConfigParser(bp_shared).parse( @@ -285,32 +265,32 @@ def test_blueprint_config_exposes_transport_fields() -> None: def test_transport_overrides_apply_and_survive_pickle() -> None: """Materialization builds each transport with its resolved config; pickle-safe.""" - bp = Blueprint(blueprints=()).transports({("topic", FakeLCMMsg): MockTransport.spec("topic")}) + bp = Blueprint(blueprints=()).transports({("topic", Point): MockTransport.spec("topic")}) - transport = _materialize_transports(bp, {"mock": {"name": "overridden"}})[("topic", FakeLCMMsg)] + transport = _materialize_transports(bp, {"mock": {"name": "overridden"}})[("topic", Point)] assert transport._config.name == "overridden" assert pickle.loads(pickle.dumps(transport))._config.name == "overridden" # No override → config built from the spec's defaults. - default = _materialize_transports(bp, {})[("topic", FakeLCMMsg)] + default = _materialize_transports(bp, {})[("topic", Point)] assert default._config.name == "default" def test_materialize_uses_resolved_config() -> None: """The config reaching the transport is built straight from the overrides, not a default instance that is later mutated.""" - bp = Blueprint(blueprints=()).transports({("topic", FakeLCMMsg): MockTransport.spec("topic")}) + bp = Blueprint(blueprints=()).transports({("topic", Point): MockTransport.spec("topic")}) - transport = _materialize_transports(bp, {"mock": {"name": "resolved"}})[("topic", FakeLCMMsg)] + transport = _materialize_transports(bp, {"mock": {"name": "resolved"}})[("topic", Point)] assert transport._config == MockConfig(name="resolved") def test_transport_overrides_coerce_string_values() -> None: """CLI/env overrides arrive as raw strings; non-str fields must coerce, not pass through.""" - bp = Blueprint(blueprints=()).transports({("topic", FakeLCMMsg): MockTransport.spec("topic")}) - transport = _materialize_transports(bp, {"mock": {"count": "5"}})[("topic", FakeLCMMsg)] + bp = Blueprint(blueprints=()).transports({("topic", Point): MockTransport.spec("topic")}) + transport = _materialize_transports(bp, {"mock": {"count": "5"}})[("topic", Point)] assert transport._config.count == 5 @@ -323,11 +303,11 @@ def test_raw_transport_pins_still_work() -> None: # Pinned on the active backend: a plain LCM/Zenoh pin that does not match it # is deliberately rebuilt by the backend switch, which is a different path. - raw = make_transport("/raw_topic", FakeLCMMsg) + raw = make_transport("/raw_topic", Point) bp = Blueprint(blueprints=()).transports( { - ("raw", FakeLCMMsg): raw, - ("speced", FakeLCMMsg): MockTransport.spec("topic"), + ("raw", Point): raw, + ("speced", Point): MockTransport.spec("topic"), } ) # Raw pins contribute no transports.* config sections; the spec still does. @@ -337,8 +317,8 @@ def test_raw_transport_pins_still_work() -> None: assert parsed.transport_overrides() == {"mock": {"name": "x"}} materialized = _materialize_transports(bp, {}) - assert materialized[("raw", FakeLCMMsg)] is raw - assert isinstance(materialized[("speced", FakeLCMMsg)], MockTransport) + assert materialized[("raw", Point)] is raw + assert isinstance(materialized[("speced", Point)], MockTransport) # ─── Broker credential validation ──────────────────────────────────── diff --git a/dimos/protocol/pubsub/impl/webrtc/tool_blueprint_e2e.py b/dimos/protocol/pubsub/impl/webrtc/tool_blueprint_e2e.py index 286e2a3b264..c00d5d53df3 100644 --- a/dimos/protocol/pubsub/impl/webrtc/tool_blueprint_e2e.py +++ b/dimos/protocol/pubsub/impl/webrtc/tool_blueprint_e2e.py @@ -27,6 +27,7 @@ import os import time +from dimos_generated.geometry_msgs.msg import Twist, TwistStamped, Vector3 import pytest from dimos.core.coordination.blueprint_config.parser import BlueprintConfigParser @@ -35,7 +36,6 @@ from dimos.core.module import Module from dimos.core.stream import In, Out from dimos.core.transport import WebRTCTransport -from dimos.msgs.geometry_msgs.TwistStamped import TwistStamped from dimos.protocol.pubsub.impl.webrtc.providers.cloudflare import CloudflareConfig from dimos.protocol.pubsub.impl.webrtc.providers.spec import WEBRTC_AVAILABLE @@ -54,7 +54,7 @@ class TwistSource(Module): @rpc def send(self, x: float) -> None: - self.cmd_webrtc.publish(TwistStamped(linear=[x, 0, 0], angular=[0, 0, 0])) + self.cmd_webrtc.publish(TwistStamped(twist=Twist(linear=Vector3(x=x)))) class TwistSink(Module): @@ -64,7 +64,7 @@ class TwistSink(Module): def start(self) -> None: super().start() self._received: list[float] = [] - self.cmd_webrtc.subscribe(lambda msg: self._received.append(msg.linear.x)) + self.cmd_webrtc.subscribe(lambda msg: self._received.append(msg.twist.linear.x)) @rpc def received(self) -> list[float]: diff --git a/dimos/protocol/pubsub/impl/webrtc/tool_broker_e2e.py b/dimos/protocol/pubsub/impl/webrtc/tool_broker_e2e.py index 20c79310488..d82d7ae03d2 100644 --- a/dimos/protocol/pubsub/impl/webrtc/tool_broker_e2e.py +++ b/dimos/protocol/pubsub/impl/webrtc/tool_broker_e2e.py @@ -83,12 +83,13 @@ def test_operator_to_transport_e2e() -> None: RTCPeerConnection, RTCSessionDescription, ) + from dimos_generated.geometry_msgs.msg import Twist, TwistStamped, Vector3 import numpy as np from dimos.core.transport import CloudflareTransport, CloudflareVideoTransport - from dimos.msgs.geometry_msgs.TwistStamped import TwistStamped - from dimos.msgs.sensor_msgs.Image import Image, ImageFormat + from dimos.msgs.image import image_from_array from dimos.protocol.pubsub.impl.webrtc.providers.spec import wait_connected, wait_open + from dimos.web.relay_bridge.protocol import FrameHeader, decode_data_frame, encode_data_frame # The TELEOP_* env fallback inside BrokerProvider was removed when # transport config moved to the blueprint flow — pass the key explicitly. @@ -128,7 +129,7 @@ async def operator() -> int: # from flowing RTP, so (as with a real camera stream) frames must be # arriving before the operator bridges, or the pull offer comes back # without a usable video m-line. - frame = Image(data=np.full((120, 160, 3), 128, dtype=np.uint8), format=ImageFormat.BGR) + frame = image_from_array(np.full((120, 160, 3), 128, dtype=np.uint8), encoding="bgr8") feeding = True async def _feed() -> None: @@ -230,16 +231,27 @@ def _on_back(payload: object) -> None: # Robot → operator: telemetry through the broker-bridged back channel. for i in range(10): - back_transport.broadcast(None, TwistStamped(linear=[0.0, 0.0, 1.0 + i])) + back_transport.broadcast(None, TwistStamped(twist=Twist(linear=Vector3(z=1.0 + i)))) await asyncio.sleep(0.05) await _wait_for(lambda: bool(back_bytes), 5.0, "robot->operator telemetry") - back_msg = TwistStamped.lcm_decode(back_bytes[-1]) - assert back_msg.linear.z >= 1.0, back_msg.linear + back_msg = TwistStamped.decode(decode_data_frame(back_bytes[-1]).payload) + assert back_msg.twist.linear.z >= 1.0, back_msg.twist.linear sent = 0 for i in range(40): - msg = TwistStamped(linear=[0.5, 0.0, 0.0], angular=[0.0, 0.0, i * 0.01]) - ch.send(msg.lcm_encode()) + msg = TwistStamped(twist=Twist(linear=Vector3(x=0.5), angular=Vector3(z=i * 0.01))) + ch.send( + encode_data_frame( + FrameHeader( + ch="cmd_unreliable", + seq=i, + ts=time.time(), + delivery="latest", + meta={"type": TwistStamped.msg_name, "encoding": "cdr"}, + ), + msg.encode(), + ) + ) sent += 1 await asyncio.sleep(0.05) # Unreliable channel: wait for the pass condition itself (early exit), @@ -261,7 +273,7 @@ def _on_back(payload: object) -> None: assert len(received) >= sent * 0.8, f"sent={sent} received={len(received)}" sample = received[-1] assert isinstance(sample, TwistStamped) - assert abs(sample.linear.x - 0.5) < 1e-9 + assert abs(sample.twist.linear.x - 0.5) < 1e-9 finally: transport.stop() back_transport.stop() diff --git a/dimos/protocol/pubsub/impl/webrtc/webrtcpubsub.py b/dimos/protocol/pubsub/impl/webrtc/webrtcpubsub.py index fa05e47ddf0..6ec24323fc4 100644 --- a/dimos/protocol/pubsub/impl/webrtc/webrtcpubsub.py +++ b/dimos/protocol/pubsub/impl/webrtc/webrtcpubsub.py @@ -38,7 +38,7 @@ class WebRTCPubSub(AllPubSub[str, bytes]): """AllPubSub[str, bytes] over a WebRTC DataChannel provider. WebRTC DataChannels are inherently "receive all" — messages arrive on a - shared multiplexed channel and are demuxed by topic/fingerprint. This + shared multiplexed channel and are demuxed by frame channel/type. This matches LCM multicast semantics, hence AllPubSub. """ diff --git a/dimos/protocol/pubsub/impl/zenohpubsub.py b/dimos/protocol/pubsub/impl/zenohpubsub.py index cb83837df89..b39a2abfcc1 100644 --- a/dimos/protocol/pubsub/impl/zenohpubsub.py +++ b/dimos/protocol/pubsub/impl/zenohpubsub.py @@ -23,7 +23,7 @@ import zenoh from dimos.msgs.helpers import resolve_msg_type -from dimos.protocol.pubsub.encoders import LCMEncoderMixin, PickleEncoderMixin +from dimos.protocol.pubsub.encoders import CDREncoderMixin, PickleEncoderMixin from dimos.protocol.pubsub.impl.lcmpubsub import Topic as LCMTopic from dimos.protocol.pubsub.spec import AllPubSub from dimos.protocol.service.zenohservice import ZenohService @@ -85,14 +85,14 @@ class Topic(LCMTopic): @property def key_expr(self) -> str: - """The Zenoh key expression for this topic, embedding lcm_type after a '/'. + """The Zenoh key expression for this topic, embedding msg_type after a '/'. Examples: - Topic("dimos/cmd_vel", Twist) -> "dimos/cmd_vel/geometry_msgs.Twist" + Topic("dimos/cmd_vel", Twist) -> "dimos/cmd_vel/geometry_msgs/msg/Twist" Topic("dimos/data") -> "dimos/data" """ - if self.lcm_type is not None: - return f"{self.pattern}/{self.lcm_type.msg_name}" + if self.msg_type is not None: + return f"{self.pattern}/{self.msg_type.msg_name}" return self.pattern @@ -100,32 +100,22 @@ def _topic_to_key_expr(topic: LCMTopic) -> str: """Convert any LCM-compatible Topic to a Zenoh key expression.""" if isinstance(topic, Topic): return topic.key_expr - return Topic(topic=topic.topic, lcm_type=topic.lcm_type).key_expr + return Topic(topic=topic.topic, msg_type=topic.msg_type).key_expr @lru_cache(maxsize=1024) -def _key_expr_to_topic(key_expr: str, default_lcm_type: type | None = None) -> Topic: - """Reconstruct a Topic from a Zenoh key expression. +def _key_expr_to_topic(key_expr: str, default_msg_type: type | None = None) -> Topic: + """Resolve a qualified package/msg/Type suffix; never guess from payload bytes. - Parses the last '/' segment and attempts to resolve it as a DimosMsg - type via resolve_msg_type(). If resolution succeeds, the segment is - treated as the type suffix and the remainder as the base topic. - - Results are cached; callers must treat the returned Topic as immutable. - - Examples: - "dimos/cmd_vel/geometry_msgs.Twist" -> Topic("dimos/cmd_vel", Twist) - "dimos/data" -> Topic("dimos/data", default_lcm_type) - "dimos/data/unknown.Foo" -> Topic("dimos/data/unknown.Foo", default_lcm_type) + Untyped keys retain their complete path. An explicit but unknown type suffix + cannot fall back to the subscriber's type. """ - # Try to resolve the last segment as a message type - parts = key_expr.rsplit("/", 1) - if len(parts) == 2: - base, maybe_type = parts - lcm_type = resolve_msg_type(maybe_type) - if lcm_type is not None: - return Topic(topic=base, lcm_type=lcm_type) - return Topic(topic=key_expr, lcm_type=default_lcm_type) + parts = key_expr.rsplit("/", 3) + if len(parts) == 4 and parts[2] == "msg": + base, package, _, name = parts + msg_type = resolve_msg_type(f"{package}/msg/{name}") + return Topic(topic=base, msg_type=msg_type) + return Topic(topic=key_expr, msg_type=default_msg_type) class ZenohPubSubBase(ZenohService, AllPubSub[Topic, bytes]): @@ -211,7 +201,7 @@ def on_sample(sample: zenoh.Sample) -> None: if sample_key == key_expr: recv_topic = topic else: - recv_topic = _key_expr_to_topic(sample_key, topic.lcm_type) + recv_topic = _key_expr_to_topic(sample_key, topic.msg_type) callback(data, recv_topic) sub = self.session.declare_subscriber(key_expr, on_sample) @@ -306,10 +296,10 @@ def stop(self) -> None: class Zenoh( # type: ignore[misc] - LCMEncoderMixin, + CDREncoderMixin, ZenohPubSubBase, ): - """Zenoh pub/sub with LCM encoding for typed DimosMsg.""" + """Zenoh pub/sub with CDR encoding for typed DimosMsg.""" ... diff --git a/dimos/protocol/pubsub/registry.py b/dimos/protocol/pubsub/registry.py index 82afd2cef61..291dad41786 100644 --- a/dimos/protocol/pubsub/registry.py +++ b/dimos/protocol/pubsub/registry.py @@ -76,10 +76,10 @@ def _make_pshm(topic: str, msg_type: type | None) -> Any: def _make_shm(topic: str, msg_type: type | None) -> Any: - # raw-bytes shared memory: subscribers receive bytes; caller decodes. + # Explicit type selects CDR; an untyped channel carries raw bytes. from dimos.core.transport import SHMTransport - return SHMTransport(topic) + return SHMTransport(topic, msg_type) def _make_jpeg_shm(topic: str, msg_type: type | None) -> Any: diff --git a/dimos/protocol/pubsub/test_pattern_sub.py b/dimos/protocol/pubsub/test_pattern_sub.py index b1d4827c37a..10bc1d5290a 100644 --- a/dimos/protocol/pubsub/test_pattern_sub.py +++ b/dimos/protocol/pubsub/test_pattern_sub.py @@ -22,11 +22,9 @@ import time from typing import Any, Generic, TypeVar +from dimos_generated.geometry_msgs.msg import Point, Pose, Quaternion, Vector3 import pytest -from dimos.msgs.geometry_msgs.Pose import Pose -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Vector3 import Vector3 from dimos.protocol.pubsub.impl.lcmpubsub import LCM, LCMPubSubBase, Topic from dimos.protocol.pubsub.patterns import Glob from dimos.protocol.pubsub.spec import AllPubSub, PubSub @@ -83,9 +81,12 @@ def lcm_bytes_context(url: str) -> Iterator[tuple[LCMPubSubBase, LCMPubSubBase]] name="lcm_typed", pubsub_context=lcm_typed_context, topic_values=[ - (Topic("/sensor/position", Vector3), Vector3(1, 2, 3)), - (Topic("/sensor/orientation", Quaternion), Quaternion(0, 0, 0, 1)), - (Topic("/robot/arm", Pose), Pose(Vector3(4, 5, 6), Quaternion(0, 0, 0, 1))), + (Topic("/sensor/position", Vector3), Vector3(x=1, y=2, z=3)), + (Topic("/sensor/orientation", Quaternion), Quaternion(w=1)), + ( + Topic("/robot/arm", Pose), + Pose(position=Point(x=4, y=5, z=6), orientation=Quaternion(w=1)), + ), ], tags={"all", "glob", "regex"}, glob_patterns=[ diff --git a/dimos/protocol/pubsub/test_registry.py b/dimos/protocol/pubsub/test_registry.py index 9d7796c2ce2..1200b10c5f3 100644 --- a/dimos/protocol/pubsub/test_registry.py +++ b/dimos/protocol/pubsub/test_registry.py @@ -14,6 +14,7 @@ from __future__ import annotations +from dimos_generated.sensor_msgs.msg import Image import pytest from dimos.core.transport import ( @@ -24,7 +25,6 @@ pLCMTransport, pSHMTransport, ) -from dimos.msgs.sensor_msgs.Image import Image from dimos.protocol.pubsub.registry import ( make_pubsub_transport, parse_pubsub_uri, @@ -50,8 +50,8 @@ def test_supported_protos_includes_known_set() -> None: ("lcm", "/odom", "nav_msgs.Odometry"), ), ( - "jpeg_lcm:/color_image#sensor_msgs.Image", - ("jpeg_lcm", "/color_image", "sensor_msgs.Image"), + "jpeg_lcm:/color_image#sensor_msgs/msg/Image", + ("jpeg_lcm", "/color_image", "sensor_msgs/msg/Image"), ), ], ) @@ -127,7 +127,7 @@ def test_make_pubsub_transport_typed_proto_without_msg_type_raises() -> None: def test_make_pubsub_transport_uri_suffix_resolves_msg_type() -> None: """The '#' suffix is resolved via resolve_msg_type and used for typed protos.""" - t = make_pubsub_transport("lcm:/color_image#sensor_msgs.Image") + t = make_pubsub_transport("lcm:/color_image#sensor_msgs/msg/Image") assert isinstance(t, LCMTransport) diff --git a/dimos/protocol/pubsub/test_spec.py b/dimos/protocol/pubsub/test_spec.py index a6ef319c641..f9eb3079f28 100644 --- a/dimos/protocol/pubsub/test_spec.py +++ b/dimos/protocol/pubsub/test_spec.py @@ -21,9 +21,9 @@ import time from typing import Any +from dimos_generated.geometry_msgs.msg import Vector3 import pytest -from dimos.msgs.geometry_msgs.Vector3 import Vector3 from dimos.protocol.pubsub.impl.lcmpubsub import LCM, Topic from dimos.protocol.pubsub.impl.memory import Memory from dimos.protocol.pubsub.impl.zenohpubsub import PickleZenoh, Zenoh @@ -122,8 +122,12 @@ def lcm_context() -> Generator[LCM, None, None]: testdata.append( ( lcm_context, - Topic(topic="/test_topic", lcm_type=Vector3), - [Vector3(1, 2, 3), Vector3(4, 5, 6), Vector3(7, 8, 9)], # Using Vector3 as mock data, + Topic(topic="/test_topic", msg_type=Vector3), + [ + Vector3(x=1, y=2, z=3), + Vector3(x=4, y=5, z=6), + Vector3(x=7, y=8, z=9), + ], # Using Vector3 as mock data, ) ) @@ -183,8 +187,8 @@ def zenoh_lcm_context() -> Generator[Zenoh, None, None]: testdata.append( ( zenoh_lcm_context, - Topic(topic="dimos/test/spec", lcm_type=Vector3), - [Vector3(1, 2, 3), Vector3(4, 5, 6), Vector3(7, 8, 9)], + Topic(topic="dimos/test/spec", msg_type=Vector3), + [Vector3(x=1, y=2, z=3), Vector3(x=4, y=5, z=6), Vector3(x=7, y=8, z=9)], ) ) diff --git a/dimos/protocol/service/test_lcmservice.py b/dimos/protocol/service/test_lcmservice.py index dfe6f1ea8d1..c0e3dc508c6 100644 --- a/dimos/protocol/service/test_lcmservice.py +++ b/dimos/protocol/service/test_lcmservice.py @@ -119,7 +119,7 @@ def test_str_without_lcm_type(self) -> None: def test_str_with_lcm_type(self) -> None: mock_type = MagicMock() mock_type.msg_name = "TestMessage" - topic = Topic(topic="my_topic", lcm_type=mock_type) + topic = Topic(topic="my_topic", msg_type=mock_type) assert str(topic) == "my_topic#TestMessage" diff --git a/dimos/protocol/tf/static_tf_publisher.py b/dimos/protocol/tf/static_tf_publisher.py index 0f5b6d12625..587fae4349b 100644 --- a/dimos/protocol/tf/static_tf_publisher.py +++ b/dimos/protocol/tf/static_tf_publisher.py @@ -27,15 +27,16 @@ import asyncio import time +from dimos_generated.geometry_msgs.msg import Transform, TransformStamped, Vector3 +from dimos_generated.std_msgs.msg import Header +from dimos_generated.tf2_msgs.msg import TFMessage from pydantic import Field from dimos.core.core import rpc from dimos.core.module import Module, ModuleConfig from dimos.core.stream import Out -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Transform import Transform -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.msgs.tf2_msgs.TFMessage import TFMessage +from dimos.msgs.geometry import quaternion_from_euler +from dimos.msgs.time import time_from_nanoseconds from dimos.utils.logging_config import setup_logger logger = setup_logger() @@ -44,23 +45,25 @@ FrameSpec = tuple[str, str | None, tuple[float, float, float], tuple[float, float, float]] -def frames_to_edge_transforms(frames: list[FrameSpec]) -> list[Transform]: - """Build a ``parent -> child`` Transform for each non-root edge of a frame tree. +def frames_to_edge_transforms(frames: list[FrameSpec]) -> list[TransformStamped]: + """Build a ``parent -> child`` TransformStamped for each non-root edge of a frame tree. This is the static mount tree (the rigid sensor offsets); a tf buffer composes these edges to answer any ``world <- frame`` query once odometry supplies the moving ``world <- root`` edge. """ - transforms: list[Transform] = [] + transforms: list[TransformStamped] = [] for name, parent, translation, rpy in frames: if parent is None: continue transforms.append( - Transform( - translation=Vector3(*translation), - rotation=Quaternion.from_euler(Vector3(*rpy)), - frame_id=parent, + TransformStamped( + header=Header(frame_id=parent), child_frame_id=name, + transform=Transform( + translation=Vector3(x=translation[0], y=translation[1], z=translation[2]), + rotation=quaternion_from_euler(*rpy), + ), ) ) return transforms @@ -77,9 +80,9 @@ class StaticTfPublisher(Module): tf: Out[TFMessage] _running: bool = False - _transforms: list[Transform] = [] + _transforms: list[TransformStamped] = [] - def transforms(self) -> list[Transform]: + def transforms(self) -> list[TransformStamped]: """The static transforms to publish. Override in a rig-specific subclass.""" raise NotImplementedError( f"{type(self).__name__} must override transforms() with its mount frames" @@ -104,10 +107,10 @@ def start(self) -> None: async def _publish_loop(self) -> None: period = 1.0 / self.config.publish_hz while self._running: - now = time.time() + now = time_from_nanoseconds(time.time_ns()) for transform in self._transforms: - transform.ts = now - self.tf.publish(TFMessage(*self._transforms)) + transform.header.stamp = now + self.tf.publish(TFMessage(transforms=self._transforms)) await asyncio.sleep(period) @rpc diff --git a/dimos/protocol/tf/test_static_tf_publisher.py b/dimos/protocol/tf/test_static_tf_publisher.py new file mode 100644 index 00000000000..d274f6bcc54 --- /dev/null +++ b/dimos/protocol/tf/test_static_tf_publisher.py @@ -0,0 +1,47 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Static frame definitions become generated ROS TF edges without convenience types.""" + +import math + +from dimos_generated.tf2_msgs.msg import TFMessage +import pytest + +from dimos.msgs.geometry import compose_transforms +from dimos.protocol.tf.static_tf_publisher import frames_to_edge_transforms + + +def test_frame_tree_skips_root_and_composes_rotated_mounts(): + edges = frames_to_edge_transforms( + [ + ("world", None, (0, 0, 0), (0, 0, 0)), + ("body", "world", (1, 0, 0), (0, 0, math.pi / 2)), + ("camera", "body", (2, 0, 3), (0, 0, 0)), + ] + ) + assert [(edge.header.frame_id, edge.child_frame_id) for edge in edges] == [ + ("world", "body"), + ("body", "camera"), + ] + decoded = TFMessage.decode(TFMessage(transforms=edges).encode()) + combined = compose_transforms(*decoded.transforms) + translation = combined.transform.translation + assert (translation.x, translation.y, translation.z) == pytest.approx((1, 2, 3)) + assert combined.header.stamp.sec == 0 + assert combined.header.stamp.nanosec == 0 + + +def test_empty_frame_tree_produces_no_edges(): + assert frames_to_edge_transforms([]) == [] diff --git a/dimos/protocol/tf/test_tf.py b/dimos/protocol/tf/test_tf.py index b44d99c1f11..dff9e89bc1c 100644 --- a/dimos/protocol/tf/test_tf.py +++ b/dimos/protocol/tf/test_tf.py @@ -18,17 +18,18 @@ import threading import time +from dimos_generated.builtin_interfaces.msg import Time +from dimos_generated.geometry_msgs.msg import Quaternion, Transform, TransformStamped, Vector3 +from dimos_generated.std_msgs.msg import Header +from dimos_generated.tf2_msgs.msg import TFMessage import pytest from dimos.core.transport_factory import make_transport from dimos.memory.store.memory import MemoryStore from dimos.memory.store.sqlite import SqliteStore from dimos.memory.tf import StreamTF -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Transform import Transform -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.msgs.tf2_msgs.TFMessage import TFMessage +from dimos.msgs.geometry import quaternion_from_euler +from dimos.msgs.time import time_from_seconds, to_seconds from dimos.protocol.tf.tf import TF, MultiTBuffer, TBuffer, TFLookup @@ -36,20 +37,22 @@ def test_tf_ros_example() -> None: tf = TF() - base_link_to_arm = Transform( - translation=Vector3(1.0, -1.0, 0.0), - rotation=Quaternion.from_euler(Vector3(0, 0, math.pi / 6)), - frame_id="base_link", + base_link_to_arm = TransformStamped( + header=Header(frame_id="base_link", stamp=time_from_seconds(time.time())), child_frame_id="arm", - ts=time.time(), + transform=Transform( + translation=Vector3(x=1.0, y=-1.0, z=0.0), + rotation=quaternion_from_euler(0, 0, math.pi / 6), + ), ) - arm_to_end = Transform( - translation=Vector3(1.0, 1.0, 0.0), - rotation=Quaternion(0.0, 0.0, 0.0, 1.0), # Identity rotation - frame_id="arm", + arm_to_end = TransformStamped( + header=Header(frame_id="arm", stamp=time_from_seconds(time.time())), child_frame_id="end_effector", - ts=time.time(), + transform=Transform( + translation=Vector3(x=1.0, y=1.0, z=0.0), + rotation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0), + ), ) tf.publish(base_link_to_arm, arm_to_end) @@ -58,8 +61,8 @@ def test_tf_ros_example() -> None: end_effector_global_pose = tf.get("base_link", "end_effector") assert end_effector_global_pose is not None - assert end_effector_global_pose.translation.x == pytest.approx(1.366, abs=1e-3) - assert end_effector_global_pose.translation.y == pytest.approx(0.366, abs=1e-3) + assert end_effector_global_pose.transform.translation.x == pytest.approx(1.366, abs=1e-3) + assert end_effector_global_pose.transform.translation.y == pytest.approx(0.366, abs=1e-3) tf.dispose() @@ -79,20 +82,21 @@ def test_tf_main() -> None: # Create a transform from world to robot current_time = time.time() - world_to_charger = Transform( - translation=Vector3(2.0, -2.0, 0.0), - rotation=Quaternion.from_euler(Vector3(0, 0, 2)), - frame_id="world", + world_to_charger = TransformStamped( + header=Header(frame_id="world", stamp=time_from_seconds(current_time)), child_frame_id="charger", - ts=current_time, + transform=Transform( + translation=Vector3(x=2.0, y=-2.0, z=0.0), rotation=quaternion_from_euler(0, 0, 2) + ), ) - world_to_robot = Transform( - translation=Vector3(1.0, 2.0, 3.0), - rotation=Quaternion(0.0, 0.0, 0.0, 1.0), # Identity rotation - frame_id="world", + world_to_robot = TransformStamped( + header=Header(frame_id="world", stamp=time_from_seconds(current_time)), child_frame_id="robot", - ts=current_time, + transform=Transform( + translation=Vector3(x=1.0, y=2.0, z=3.0), + rotation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0), + ), ) # Broadcast the transform @@ -107,12 +111,13 @@ def test_tf_main() -> None: assert "robot" in frames # Add another transform in the chain - robot_to_sensor = Transform( - translation=Vector3(0.5, 0.0, 0.2), - rotation=Quaternion(0.0, 0.0, 0.707107, 0.707107), # 90 degrees around Z - frame_id="robot", + robot_to_sensor = TransformStamped( + header=Header(frame_id="robot", stamp=time_from_seconds(current_time)), child_frame_id="sensor", - ts=current_time, + transform=Transform( + translation=Vector3(x=0.5, y=0.0, z=0.2), + rotation=Quaternion(x=0.0, y=0.0, z=0.707107, w=0.707107), + ), ) broadcaster.publish(robot_to_sensor) @@ -128,27 +133,17 @@ def test_tf_main() -> None: # The chain should compose: world->robot (1,2,3) + robot->sensor (0.5,0,0.2) # Expected translation: (1.5, 2.0, 3.2) assert chain_transform is not None - assert abs(chain_transform.translation.x - 1.5) < 0.001 - assert abs(chain_transform.translation.y - 2.0) < 0.001 - assert abs(chain_transform.translation.z - 3.2) < 0.001 - - # we see something on camera - random_object_in_view = PoseStamped( - frame_id="random_object", - position=Vector3(1, 0, 0), + assert abs(chain_transform.transform.translation.x - 1.5) < 0.001 + assert abs(chain_transform.transform.translation.y - 2.0) < 0.001 + assert abs(chain_transform.transform.translation.z - 3.2) < 0.001 + + # A perceived object is expressed as a stamped sensor -> object transform. + random_t = TransformStamped( + header=Header(frame_id="sensor", stamp=time_from_seconds(time.time())), + child_frame_id="random_object", + transform=Transform(translation=Vector3(x=1, y=0, z=0)), ) - print("Random obj", random_object_in_view) - - # random_object is perceived by the sensor - # we create a transform pointing from sensor to object - random_t = random_object_in_view.new_transform_from("sensor") - - # we could have also done - assert random_t == random_object_in_view.new_transform_to("sensor").inverse() - - print("randm t", random_t) - # we broadcast our object location broadcaster.publish(random_t) @@ -176,21 +171,21 @@ def test_tf_main() -> None: print(broadcaster.graph()) assert world_object is not None - assert abs(world_object.translation.x - 1.5) < 0.001 - assert abs(world_object.translation.y - 3.0) < 0.001 - assert abs(world_object.translation.z - 3.2) < 0.001 + assert abs(world_object.transform.translation.x - 1.5) < 0.001 + assert abs(world_object.transform.translation.y - 3.0) < 0.001 + assert abs(world_object.transform.translation.z - 3.2) < 0.001 # this doesn't work atm robot_to_charger = broadcaster.get("robot", "charger") assert robot_to_charger is not None # Expected: robot->world->charger - print(f"robot_to_charger translation: {robot_to_charger.translation}") - print(f"robot_to_charger rotation: {robot_to_charger.rotation}") + print(f"robot_to_charger translation: {robot_to_charger.transform.translation}") + print(f"robot_to_charger rotation: {robot_to_charger.transform.rotation}") - assert abs(robot_to_charger.translation.x - 1.0) < 0.001 - assert abs(robot_to_charger.translation.y - (-4.0)) < 0.001 - assert abs(robot_to_charger.translation.z - (-3.0)) < 0.001 + assert abs(robot_to_charger.transform.translation.x - 1.0) < 0.001 + assert abs(robot_to_charger.transform.translation.y - (-4.0)) < 0.001 + assert abs(robot_to_charger.transform.translation.z - (-3.0)) < 0.001 # Stop services (they were autostarted but don't know how to autostop) broadcaster.dispose() @@ -202,12 +197,13 @@ def test_tf_main() -> None: class TestTBuffer: def test_add_transform(self) -> None: buffer = TBuffer(buffer_size=10.0) - transform = Transform( - translation=Vector3(1.0, 2.0, 3.0), - rotation=Quaternion(0.0, 0.0, 0.0, 1.0), - frame_id="world", + transform = TransformStamped( + header=Header(frame_id="world", stamp=time_from_seconds(time.time())), child_frame_id="robot", - ts=time.time(), + transform=Transform( + translation=Vector3(x=1.0, y=2.0, z=3.0), + rotation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0), + ), ) buffer.add(transform) @@ -220,23 +216,22 @@ def test_get(self) -> None: # Add transforms at different times for i in range(3): - transform = Transform( - translation=Vector3(float(i), 0.0, 0.0), - frame_id="world", + transform = TransformStamped( + header=Header(frame_id="world", stamp=time_from_seconds(base_time + i * 0.5)), child_frame_id="robot", - ts=base_time + i * 0.5, + transform=Transform(translation=Vector3(x=float(i), y=0.0, z=0.0)), ) buffer.add(transform) # Test getting latest transform latest = buffer.get() assert latest is not None - assert latest.translation.x == 2.0 + assert latest.transform.translation.x == 2.0 # Test getting transform at specific time middle = buffer.get(time_point=base_time + 0.75) assert middle is not None - assert middle.translation.x == 2.0 # Closest to i=1 + assert middle.transform.translation.x == 2.0 # Closest to i=1 # Test time tolerance result = buffer.get(time_point=base_time + 10.0, time_tolerance=0.1) @@ -247,20 +242,18 @@ def test_buffer_pruning(self) -> None: # Add old transform old_time = time.time() - 2.0 - old_transform = Transform( - translation=Vector3(1.0, 0.0, 0.0), - frame_id="world", + old_transform = TransformStamped( + header=Header(frame_id="world", stamp=time_from_seconds(old_time)), child_frame_id="robot", - ts=old_time, + transform=Transform(translation=Vector3(x=1.0, y=0.0, z=0.0)), ) buffer.add(old_transform) # Add recent transform - recent_transform = Transform( - translation=Vector3(2.0, 0.0, 0.0), - frame_id="world", + recent_transform = TransformStamped( + header=Header(frame_id="world", stamp=time_from_seconds(time.time())), child_frame_id="robot", - ts=time.time(), + transform=Transform(translation=Vector3(x=2.0, y=0.0, z=0.0)), ) buffer.add(recent_transform) @@ -268,7 +261,7 @@ def test_buffer_pruning(self) -> None: assert len(buffer) == 1 first = buffer.first() assert first is not None - assert first.translation.x == 2.0 + assert first.transform.translation.x == 2.0 class TestMultiTBuffer: @@ -276,18 +269,16 @@ def test_multiple_frame_pairs(self) -> None: ttbuffer = MultiTBuffer(buffer_size=10.0) # Add transforms for different frame pairs - transform1 = Transform( - translation=Vector3(1.0, 0.0, 0.0), - frame_id="world", + transform1 = TransformStamped( + header=Header(frame_id="world", stamp=time_from_seconds(time.time())), child_frame_id="robot1", - ts=time.time(), + transform=Transform(translation=Vector3(x=1.0, y=0.0, z=0.0)), ) - transform2 = Transform( - translation=Vector3(2.0, 0.0, 0.0), - frame_id="world", + transform2 = TransformStamped( + header=Header(frame_id="world", stamp=time_from_seconds(time.time())), child_frame_id="robot2", - ts=time.time(), + transform=Transform(translation=Vector3(x=2.0, y=0.0, z=0.0)), ) ttbuffer.receive_transform(transform1, transform2) @@ -301,18 +292,16 @@ def test_graph(self) -> None: ttbuffer = MultiTBuffer(buffer_size=10.0) # Add transforms for different frame pairs - transform1 = Transform( - translation=Vector3(1.0, 0.0, 0.0), - frame_id="world", + transform1 = TransformStamped( + header=Header(frame_id="world", stamp=time_from_seconds(time.time())), child_frame_id="robot1", - ts=time.time(), + transform=Transform(translation=Vector3(x=1.0, y=0.0, z=0.0)), ) - transform2 = Transform( - translation=Vector3(2.0, 0.0, 0.0), - frame_id="world", + transform2 = TransformStamped( + header=Header(frame_id="world", stamp=time_from_seconds(time.time())), child_frame_id="robot2", - ts=time.time(), + transform=Transform(translation=Vector3(x=2.0, y=0.0, z=0.0)), ) ttbuffer.receive_transform(transform1, transform2) @@ -326,11 +315,10 @@ def test_forward_tolerance_returns_when_buffer_fills(self) -> None: def publish_after_delay() -> None: time.sleep(0.05) ttbuffer.receive_transform( - Transform( - translation=Vector3(1.0, 0.0, 0.0), - frame_id="world", + TransformStamped( + header=Header(frame_id="world", stamp=time_from_seconds(base_time)), child_frame_id="robot", - ts=base_time, + transform=Transform(translation=Vector3(x=1.0, y=0.0, z=0.0)), ) ) @@ -345,7 +333,7 @@ def publish_after_delay() -> None: publisher.join() assert result is not None - assert result.translation.x == 1.0 + assert result.transform.translation.x == 1.0 assert elapsed < 0.5 def test_forward_tolerance_times_out(self) -> None: @@ -360,11 +348,10 @@ def test_forward_tolerance_fast_path_when_already_available(self) -> None: ttbuffer = MultiTBuffer() base_time = time.time() ttbuffer.receive_transform( - Transform( - translation=Vector3(2.0, 0.0, 0.0), - frame_id="world", + TransformStamped( + header=Header(frame_id="world", stamp=time_from_seconds(base_time)), child_frame_id="robot", - ts=base_time, + transform=Transform(translation=Vector3(x=2.0, y=0.0, z=0.0)), ) ) @@ -373,7 +360,7 @@ def test_forward_tolerance_fast_path_when_already_available(self) -> None: elapsed = time.monotonic() - t0 assert result is not None - assert result.translation.x == 2.0 + assert result.transform.translation.x == 2.0 assert elapsed < 0.05 def test_forward_tolerance_wakes_on_chain_completion(self) -> None: @@ -381,22 +368,20 @@ def test_forward_tolerance_wakes_on_chain_completion(self) -> None: base_time = time.time() ttbuffer.receive_transform( - Transform( - translation=Vector3(1.0, 0.0, 0.0), - frame_id="world", + TransformStamped( + header=Header(frame_id="world", stamp=time_from_seconds(base_time)), child_frame_id="robot", - ts=base_time, + transform=Transform(translation=Vector3(x=1.0, y=0.0, z=0.0)), ) ) def publish_after_delay() -> None: time.sleep(0.05) ttbuffer.receive_transform( - Transform( - translation=Vector3(0.0, 2.0, 0.0), - frame_id="robot", + TransformStamped( + header=Header(frame_id="robot", stamp=time_from_seconds(base_time)), child_frame_id="sensor", - ts=base_time, + transform=Transform(translation=Vector3(x=0.0, y=2.0, z=0.0)), ) ) @@ -409,19 +394,18 @@ def publish_after_delay() -> None: publisher.join() assert result is not None - assert result.translation.x == 1.0 - assert result.translation.y == 2.0 + assert result.transform.translation.x == 1.0 + assert result.transform.translation.y == 2.0 def test_get_transform_search_direct(self) -> None: ttbuffer = MultiTBuffer() base_time = time.time() # Add direct transform - transform = Transform( - translation=Vector3(1.0, 0.0, 0.0), - frame_id="world", + transform = TransformStamped( + header=Header(frame_id="world", stamp=time_from_seconds(base_time)), child_frame_id="robot", - ts=base_time, + transform=Transform(translation=Vector3(x=1.0, y=0.0, z=0.0)), ) ttbuffer.receive_transform(transform) @@ -429,24 +413,22 @@ def test_get_transform_search_direct(self) -> None: result = ttbuffer.get_transform_search("world", "robot") assert result is not None assert len(result) == 1 - assert result[0].translation.x == 1.0 + assert result[0].transform.translation.x == 1.0 def test_get_transform_search_chain(self) -> None: ttbuffer = MultiTBuffer() base_time = time.time() # Create transform chain: world -> robot -> sensor - transform1 = Transform( - translation=Vector3(1.0, 0.0, 0.0), - frame_id="world", + transform1 = TransformStamped( + header=Header(frame_id="world", stamp=time_from_seconds(base_time)), child_frame_id="robot", - ts=base_time, + transform=Transform(translation=Vector3(x=1.0, y=0.0, z=0.0)), ) - transform2 = Transform( - translation=Vector3(0.0, 2.0, 0.0), - frame_id="robot", + transform2 = TransformStamped( + header=Header(frame_id="robot", stamp=time_from_seconds(base_time)), child_frame_id="sensor", - ts=base_time, + transform=Transform(translation=Vector3(x=0.0, y=2.0, z=0.0)), ) ttbuffer.receive_transform(transform1, transform2) @@ -454,8 +436,8 @@ def test_get_transform_search_chain(self) -> None: result = ttbuffer.get_transform_search("world", "sensor") assert result is not None assert len(result) == 2 - assert result[0].translation.x == 1.0 # world -> robot - assert result[1].translation.y == 2.0 # robot -> sensor + assert result[0].transform.translation.x == 1.0 # world -> robot + assert result[1].transform.translation.y == 2.0 # robot -> sensor def test_get_transform_search_complex_chain(self) -> None: ttbuffer = MultiTBuffer() @@ -465,35 +447,30 @@ def test_get_transform_search_complex_chain(self) -> None: # world -> base -> arm -> hand # \-> robot -> sensor transforms = [ - Transform( - frame_id="world", + TransformStamped( + header=Header(frame_id="world", stamp=time_from_seconds(base_time)), child_frame_id="base", - translation=Vector3(1.0, 0.0, 0.0), - ts=base_time, + transform=Transform(translation=Vector3(x=1.0, y=0.0, z=0.0)), ), - Transform( - frame_id="base", + TransformStamped( + header=Header(frame_id="base", stamp=time_from_seconds(base_time)), child_frame_id="arm", - translation=Vector3(0.0, 1.0, 0.0), - ts=base_time, + transform=Transform(translation=Vector3(x=0.0, y=1.0, z=0.0)), ), - Transform( - frame_id="arm", + TransformStamped( + header=Header(frame_id="arm", stamp=time_from_seconds(base_time)), child_frame_id="hand", - translation=Vector3(0.0, 0.0, 1.0), - ts=base_time, + transform=Transform(translation=Vector3(x=0.0, y=0.0, z=1.0)), ), - Transform( - frame_id="world", + TransformStamped( + header=Header(frame_id="world", stamp=time_from_seconds(base_time)), child_frame_id="robot", - translation=Vector3(2.0, 0.0, 0.0), - ts=base_time, + transform=Transform(translation=Vector3(x=2.0, y=0.0, z=0.0)), ), - Transform( - frame_id="robot", + TransformStamped( + header=Header(frame_id="robot", stamp=time_from_seconds(base_time)), child_frame_id="sensor", - translation=Vector3(0.0, 2.0, 0.0), - ts=base_time, + transform=Transform(translation=Vector3(x=0.0, y=2.0, z=0.0)), ), ] @@ -513,8 +490,16 @@ def test_get_transform_search_no_path(self) -> None: base_time = time.time() # Create disconnected transforms - transform1 = Transform(frame_id="world", child_frame_id="robot", ts=base_time) - transform2 = Transform(frame_id="base", child_frame_id="sensor", ts=base_time) + transform1 = TransformStamped( + header=Header(frame_id="world", stamp=time_from_seconds(base_time)), + child_frame_id="robot", + transform=Transform(), + ) + transform2 = TransformStamped( + header=Header(frame_id="base", stamp=time_from_seconds(base_time)), + child_frame_id="sensor", + transform=Transform(), + ) ttbuffer.receive_transform(transform1, transform2) # No path exists @@ -526,24 +511,22 @@ def test_get_transform_search_with_time(self) -> None: base_time = time.time() # Add transforms at different times - old_transform = Transform( - frame_id="world", + old_transform = TransformStamped( + header=Header(frame_id="world", stamp=time_from_seconds(base_time - 10.0)), child_frame_id="robot", - translation=Vector3(1.0, 0.0, 0.0), - ts=base_time - 10.0, + transform=Transform(translation=Vector3(x=1.0, y=0.0, z=0.0)), ) - new_transform = Transform( - frame_id="world", + new_transform = TransformStamped( + header=Header(frame_id="world", stamp=time_from_seconds(base_time)), child_frame_id="robot", - translation=Vector3(2.0, 0.0, 0.0), - ts=base_time, + transform=Transform(translation=Vector3(x=2.0, y=0.0, z=0.0)), ) ttbuffer.receive_transform(old_transform, new_transform) # Search at specific time result = ttbuffer.get_transform_search("world", "robot", time_point=base_time) assert result is not None - assert result[0].translation.x == 2.0 + assert result[0].transform.translation.x == 2.0 # Search with time tolerance result = ttbuffer.get_transform_search( @@ -559,10 +542,26 @@ def test_get_transform_search_shortest_path(self) -> None: # world -> A -> B -> target (3 hops) # world -> target (direct, 1 hop) transforms = [ - Transform(frame_id="world", child_frame_id="A", ts=base_time), - Transform(frame_id="A", child_frame_id="B", ts=base_time), - Transform(frame_id="B", child_frame_id="target", ts=base_time), - Transform(frame_id="world", child_frame_id="target", ts=base_time), + TransformStamped( + header=Header(frame_id="world", stamp=time_from_seconds(base_time)), + child_frame_id="A", + transform=Transform(), + ), + TransformStamped( + header=Header(frame_id="A", stamp=time_from_seconds(base_time)), + child_frame_id="B", + transform=Transform(), + ), + TransformStamped( + header=Header(frame_id="B", stamp=time_from_seconds(base_time)), + child_frame_id="target", + transform=Transform(), + ), + TransformStamped( + header=Header(frame_id="world", stamp=time_from_seconds(base_time)), + child_frame_id="target", + transform=Transform(), + ), ] for t in transforms: @@ -586,11 +585,10 @@ def test_string_representations(self) -> None: buffer = TBuffer() base_time = time.time() for i in range(3): - transform = Transform( - translation=Vector3(float(i), 0.0, 0.0), - frame_id="world", + transform = TransformStamped( + header=Header(frame_id="world", stamp=time_from_seconds(base_time + i * 0.1)), child_frame_id="robot", - ts=base_time + i * 0.1, + transform=Transform(translation=Vector3(x=float(i), y=0.0, z=0.0)), ) buffer.add(transform) @@ -602,9 +600,21 @@ def test_string_representations(self) -> None: # Test MultiTBuffer with multiple frame pairs ttbuffer = MultiTBuffer() transforms = [ - Transform(frame_id="world", child_frame_id="robot1", ts=base_time), - Transform(frame_id="world", child_frame_id="robot2", ts=base_time + 0.5), - Transform(frame_id="robot1", child_frame_id="sensor", ts=base_time + 1.0), + TransformStamped( + header=Header(frame_id="world", stamp=time_from_seconds(base_time)), + child_frame_id="robot1", + transform=Transform(), + ), + TransformStamped( + header=Header(frame_id="world", stamp=time_from_seconds(base_time + 0.5)), + child_frame_id="robot2", + transform=Transform(), + ), + TransformStamped( + header=Header(frame_id="robot1", stamp=time_from_seconds(base_time + 1.0)), + child_frame_id="sensor", + transform=Transform(), + ), ] for t in transforms: @@ -626,12 +636,11 @@ def test_string_representations(self) -> None: T0 = 1_700_000_000.0 -def _t(parent: str, child: str, x: float, ts: float) -> Transform: - return Transform( - frame_id=parent, +def _t(parent: str, child: str, x: float, ts: float) -> TransformStamped: + return TransformStamped( + header=Header(frame_id=parent, stamp=time_from_seconds(ts)), child_frame_id=child, - translation=Vector3(x, 0.0, 0.0), - ts=ts, + transform=Transform(translation=Vector3(x=x, y=0.0, z=0.0)), ) @@ -648,7 +657,7 @@ def make_tf(request, tmp_path): # type: ignore[no-untyped-def] """Builder fixture: feed it transforms, get back a TFLookup over them.""" stores = [] - def build(*transforms: Transform) -> TFLookup: + def build(*transforms: TransformStamped) -> TFLookup: if request.param == "live": buf = MultiTBuffer() buf.receive_transform(*transforms) @@ -661,7 +670,7 @@ def build(*transforms: Transform) -> TFLookup: stores.append(store) stream = store.stream("tf", TFMessage) for t in transforms: - stream.append(TFMessage(t), ts=t.ts, pose=None) + stream.append(TFMessage(transforms=[t]), ts=to_seconds(t.header.stamp), pose=None) return StreamTF(store.stream("tf", TFMessage)) yield build @@ -676,20 +685,20 @@ def test_latest(self, make_tf) -> None: # type: ignore[no-untyped-def] tf = make_tf(*(_t("world", "robot", float(i), T0 + i * 0.1) for i in range(3))) got = tf.get("world", "robot") assert got is not None - assert got.translation.x == 2.0 + assert got.transform.translation.x == 2.0 def test_nearest_in_time(self, make_tf) -> None: # type: ignore[no-untyped-def] tf = make_tf(*(_t("world", "robot", float(i), T0 + i * 0.5) for i in range(5))) got = tf.get("world", "robot", time_point=T0 + 1.25) assert got is not None # Equidistant between i=2 (t=1.0) and i=3 (t=1.5) — the later one wins. - assert got.translation.x == 3.0 + assert got.transform.translation.x == 3.0 def test_inverse(self, make_tf) -> None: # type: ignore[no-untyped-def] tf = make_tf(_t("world", "robot", 5.0, T0)) got = tf.get("robot", "world", time_point=T0) assert got is not None - assert got.translation.x == pytest.approx(-5.0) + assert got.transform.translation.x == pytest.approx(-5.0) def test_time_tolerance(self, make_tf) -> None: # type: ignore[no-untyped-def] tf = make_tf(_t("world", "robot", 1.0, T0)) @@ -700,10 +709,10 @@ def test_same_frame_identity(self, make_tf) -> None: # type: ignore[no-untyped- tf = make_tf(_t("world", "robot", 1.0, T0)) got = tf.get("world", "world", time_point=T0) assert got is not None - assert got.frame_id == "world" + assert got.header.frame_id == "world" assert got.child_frame_id == "world" - assert got.translation.x == 0.0 - assert got.rotation.w == 1.0 + assert got.transform.translation.x == 0.0 + assert got.transform.rotation.w == 1.0 def test_unknown_frame(self, make_tf) -> None: # type: ignore[no-untyped-def] tf = make_tf(_t("world", "robot", 1.0, T0)) @@ -713,29 +722,33 @@ def test_chain_composition(self, make_tf) -> None: # type: ignore[no-untyped-de # world -> robot: translate (1, 0, 0); robot -> sensor: translate # (0, 2, 0) and rotate 90° around Z. tf = make_tf( - Transform( - translation=Vector3(1.0, 0.0, 0.0), - rotation=Quaternion(0.0, 0.0, 0.0, 1.0), - frame_id="world", + TransformStamped( + header=Header(frame_id="world", stamp=time_from_seconds(T0)), child_frame_id="robot", - ts=T0, + transform=Transform( + translation=Vector3(x=1.0, y=0.0, z=0.0), + rotation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0), + ), ), - Transform( - translation=Vector3(0.0, 2.0, 0.0), - rotation=Quaternion(0.0, 0.0, math.sin(math.pi / 4), math.cos(math.pi / 4)), - frame_id="robot", + TransformStamped( + header=Header(frame_id="robot", stamp=time_from_seconds(T0)), child_frame_id="sensor", - ts=T0, + transform=Transform( + translation=Vector3(x=0.0, y=2.0, z=0.0), + rotation=Quaternion( + x=0.0, y=0.0, z=math.sin(math.pi / 4), w=math.cos(math.pi / 4) + ), + ), ), ) result = tf.get("world", "sensor", time_point=T0) assert result is not None - assert result.translation.x == pytest.approx(1.0) - assert result.translation.y == pytest.approx(2.0) - assert result.translation.z == pytest.approx(0.0) - assert result.rotation.z == pytest.approx(math.sin(math.pi / 4)) - assert result.rotation.w == pytest.approx(math.cos(math.pi / 4)) - assert result.frame_id == "world" + assert result.transform.translation.x == pytest.approx(1.0) + assert result.transform.translation.y == pytest.approx(2.0) + assert result.transform.translation.z == pytest.approx(0.0) + assert result.transform.rotation.z == pytest.approx(math.sin(math.pi / 4)) + assert result.transform.rotation.w == pytest.approx(math.cos(math.pi / 4)) + assert result.header.frame_id == "world" assert result.child_frame_id == "sensor" def test_chain_with_sparse_static_edge(self, make_tf) -> None: # type: ignore[no-untyped-def] @@ -747,7 +760,7 @@ def test_chain_with_sparse_static_edge(self, make_tf) -> None: # type: ignore[n ) got = tf.get("world", "base", time_point=T0 + 9.0) assert got is not None - assert got.translation.x == pytest.approx(109.0) + assert got.transform.translation.x == pytest.approx(109.0) def test_conforms_to_tf_lookup(self, make_tf) -> None: # type: ignore[no-untyped-def] assert isinstance(make_tf(_t("world", "robot", 1.0, T0)), TFLookup) @@ -762,10 +775,10 @@ def store(self): # type: ignore[no-untyped-def] stream = store.stream("tf", TFMessage) # Startup burst: a static stamped exactly at the stream start (regression: # strict `ts >` range queries used to drop it), then dynamic map→base. - stream.append(TFMessage(_t("world", "map", 100.0, T0)), ts=T0, pose=None) + stream.append(TFMessage(transforms=[_t("world", "map", 100.0, T0)]), ts=T0, pose=None) for i in range(100): ts = T0 + i / 10 - stream.append(TFMessage(_t("map", "base", i / 10, ts)), ts=ts, pose=None) + stream.append(TFMessage(transforms=[_t("map", "base", i / 10, ts)]), ts=ts, pose=None) yield store store.stop() @@ -802,7 +815,7 @@ def test_get_pose(self, store) -> None: # type: ignore[no-untyped-def] tf = StreamTF(store.stream("tf", TFMessage)) pose = tf.get_pose("map", "base", time_point=T0 + 5.0) assert pose is not None - assert pose.position.x == pytest.approx(5.0) + assert pose.pose.position.x == pytest.approx(5.0) def test_cache_prefetch_and_eviction(self) -> None: # 40 s of data with a small cache_span: a miss caches the query window @@ -812,25 +825,69 @@ def test_cache_prefetch_and_eviction(self) -> None: stream = store.stream("tf", TFMessage) for i in range(400): ts = T0 + i / 10 - stream.append(TFMessage(_t("map", "base", i / 10, ts)), ts=ts, pose=None) + stream.append(TFMessage(transforms=[_t("map", "base", i / 10, ts)]), ts=ts, pose=None) try: tf = StreamTF(store.stream("tf", TFMessage), cache_span=2.0) got = tf.get("map", "base", time_point=T0 + 35.0, time_tolerance=0.5) assert got is not None - assert got.translation.x == pytest.approx(35.0) + assert got.transform.translation.x == pytest.approx(35.0) covered = tf._covered assert covered == pytest.approx((T0 + 34.5, T0 + 37.5)) # Inside the prefetched span: served from cache, no re-query. got = tf.get("map", "base", time_point=T0 + 37.0, time_tolerance=0.5) assert got is not None - assert got.translation.x == pytest.approx(37.0) + assert got.transform.translation.x == pytest.approx(37.0) assert tf._covered == covered # Past the span: evict and re-cache around the new query. got = tf.get("map", "base", time_point=T0 + 39.0, time_tolerance=0.5) assert got is not None - assert got.translation.x == pytest.approx(39.0) + assert got.transform.translation.x == pytest.approx(39.0) assert tf._covered == pytest.approx((T0 + 38.5, T0 + 41.5)) # Bounded: the buffer holds the re-cached span, not the stream. assert len(tf.buffers[("map", "base")]) < 100 finally: store.stop() + + +def test_buffer_retains_distinct_nanoseconds_and_owns_its_snapshots(): + buffer = TBuffer() + first = TransformStamped( + header=Header(frame_id="a", stamp=Time(sec=1700000000, nanosec=123456789)), + child_frame_id="b", + ) + second = TransformStamped( + header=Header(frame_id="a", stamp=Time(sec=1700000000, nanosec=123456790)), + child_frame_id="b", + ) + buffer.add(first) + buffer.add(second) + first.header.stamp.nanosec = 0 + assert len(buffer) == 2 + stored_first = buffer.first() + assert stored_first is not None + assert stored_first.header.stamp.nanosec == 123456789 + stored_first.header.stamp.nanosec = 1 + unchanged = buffer.first() + assert unchanged is not None + assert unchanged.header.stamp.nanosec == 123456789 + latest = buffer.get() + assert latest is not None + assert latest.header.stamp.nanosec == 123456790 + + +def test_buffer_drops_out_of_order_data_older_than_retained_window(): + buffer = TBuffer(buffer_size=1) + for sec in (10, 11, 1): + buffer.add(TransformStamped(header=Header(stamp=Time(sec=sec)))) + assert len(buffer) == 2 + first = buffer.first() + assert first is not None + assert first.header.stamp.sec == 10 + + +def test_buffer_preserves_negative_stamp(): + buffer = TBuffer() + buffer.add(TransformStamped(header=Header(stamp=Time(sec=-1, nanosec=500000000)))) + result = buffer.get(time_point=-0.5, time_tolerance=0) + assert result is not None + assert result.header.stamp == Time(sec=-1, nanosec=500000000) diff --git a/dimos/protocol/tf/tf.py b/dimos/protocol/tf/tf.py index 03a4706deda..87afac6b4cc 100644 --- a/dimos/protocol/tf/tf.py +++ b/dimos/protocol/tf/tf.py @@ -15,17 +15,23 @@ # limitations under the License. from collections import deque +from copy import copy from functools import reduce +import math +import subprocess import threading import time -from typing import TYPE_CHECKING, Any, Protocol, runtime_checkable +from typing import TYPE_CHECKING, Any, Protocol, cast, runtime_checkable -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.geometry_msgs.Transform import Transform -from dimos.msgs.tf2_msgs.TFMessage import TFMessage +from dimos_generated.geometry_msgs.msg import PoseStamped, TransformStamped +from dimos_generated.std_msgs.msg import Header +from dimos_generated.tf2_msgs.msg import TFMessage +from sortedcontainers import SortedDict # type: ignore[import-untyped] + +from dimos.msgs.geometry import compose_transforms, inverse_transform, pose_from_transform +from dimos.msgs.time import time_from_seconds, to_nanoseconds, to_seconds from dimos.types.timestamped import to_human_readable from dimos.utils.logging_config import setup_logger -from dimos.utils.timeseries.inmemory import InMemoryStore if TYPE_CHECKING: from collections.abc import Callable @@ -48,45 +54,68 @@ def get( child_frame: str, time_point: float | None = None, time_tolerance: float | None = None, - ) -> Transform | None: ... + ) -> TransformStamped | None: ... + +class TBuffer: + """A bounded time index of generated transforms, keyed by integer nanoseconds.""" -class TBuffer(InMemoryStore[Transform]): def __init__(self, buffer_size: float = 10.0) -> None: - super().__init__() + if math.isnan(buffer_size) or buffer_size <= 0: + raise ValueError("buffer_size must be positive") self.buffer_size = buffer_size + self._entries: SortedDict = SortedDict() + + def __len__(self) -> int: + return len(self._entries) + + def add(self, transform: TransformStamped) -> None: + stamp = to_nanoseconds(transform.header.stamp) + self._entries[stamp] = TransformStamped( + header=transform.header, + child_frame_id=transform.child_frame_id, + transform=transform.transform, + ) + if math.isfinite(self.buffer_size): + oldest = self._entries.peekitem(-1)[0] - round(self.buffer_size * 1_000_000_000) + while self._entries and self._entries.peekitem(0)[0] < oldest: + self._entries.popitem(0) + + def first(self) -> TransformStamped | None: + return copy(self._entries.peekitem(0)[1]) if self._entries else None - def add(self, transform: Transform) -> None: - self.save(transform) - self.prune_old(transform.ts - self.buffer_size) + def last(self) -> TransformStamped | None: + return copy(self._entries.peekitem(-1)[1]) if self._entries else None - def get(self, time_point: float | None = None, time_tolerance: float = 1.0) -> Transform | None: - """Get transform at specified time or latest if no time given.""" + def get( + self, time_point: float | None = None, time_tolerance: float = 1.0 + ) -> TransformStamped | None: if time_point is None: return self.last() - return self.find_closest(time_point, time_tolerance) + stamp = to_nanoseconds(time_from_seconds(time_point)) + if math.isnan(time_tolerance) or time_tolerance < 0: + raise ValueError("time_tolerance must be nonnegative") + index = self._entries.bisect_left(stamp) + candidates = [ + self._entries.peekitem(i) for i in (index - 1, index) if 0 <= i < len(self._entries) + ] + if not candidates: + return None + nearest, result = min(candidates, key=lambda item: (abs(item[0] - stamp), -item[0])) + if abs(nearest - stamp) / 1_000_000_000 > time_tolerance: + return None + return copy(cast("TransformStamped", result)) def __str__(self) -> str: - if len(self) == 0: + first, last = self.first(), self.last() + if first is None or last is None: return "TBuffer(empty)" - - first_item = self.first() - time_range = self.time_range() - if time_range and first_item: - start_time = to_human_readable(time_range[0]) - end_time = to_human_readable(time_range[1]) - duration = time_range[1] - time_range[0] - - frame_str = f"{first_item.frame_id} -> {first_item.child_frame_id}" - - return ( - f"TBuffer(" - f"{frame_str}, " - f"{len(self)} msgs, " - f"{duration:.2f}s [{start_time} - {end_time}])" - ) - - return f"TBuffer({len(self)} msgs)" + start, end = to_seconds(first.header.stamp), to_seconds(last.header.stamp) + return ( + f"TBuffer({first.header.frame_id} -> {first.child_frame_id}, " + f"{len(self)} msgs, {end - start:.2f}s " + f"[{to_human_readable(start)} - {to_human_readable(end)}])" + ) # stores multiple transform buffers @@ -97,10 +126,10 @@ def __init__(self, buffer_size: float = 10.0) -> None: self.buffer_size = buffer_size self._cv = threading.Condition() - def receive_transform(self, *args: Transform) -> None: + def receive_transform(self, *args: TransformStamped) -> None: with self._cv: for transform in args: - key = (transform.frame_id, transform.child_frame_id) + key = (transform.header.frame_id, transform.child_frame_id) if key not in self.buffers: self.buffers[key] = TBuffer(self.buffer_size) self.buffers[key].add(transform) @@ -134,12 +163,14 @@ def get_transform( child_frame: str, time_point: float | None = None, time_tolerance: float | None = None, - ) -> Transform | None: + ) -> TransformStamped | None: if parent_frame == child_frame: - return Transform( - frame_id=parent_frame, + return TransformStamped( + header=Header( + frame_id=parent_frame, + stamp=time_from_seconds(time_point if time_point is not None else time.time()), + ), child_frame_id=child_frame, - ts=time_point if time_point is not None else time.time(), ) # No explicit tolerance means "anything still buffered" — the buffer @@ -156,7 +187,7 @@ def get_transform( reverse_key = (child_frame, parent_frame) if reverse_key in self.buffers: transform = self.buffers[reverse_key].get(time_point, tolerance) - return transform.inverse() if transform else None + return inverse_transform(transform) if transform else None return None @@ -166,7 +197,7 @@ def _get( child_frame: str, time_point: float | None = None, time_tolerance: float | None = None, - ) -> Transform | None: + ) -> TransformStamped | None: with self._cv: simple = self.get_transform(parent_frame, child_frame, time_point, time_tolerance) @@ -180,7 +211,7 @@ def _get( if complex is None: return None - return reduce(lambda t1, t2: t1 + t2, complex) + return reduce(compose_transforms, complex) def _wait_get( self, @@ -189,7 +220,7 @@ def _wait_get( time_point: float | None, time_tolerance: float | None, forward_tolerance: float, - ) -> Transform | None: + ) -> TransformStamped | None: deadline = time.monotonic() + forward_tolerance with self._cv: while True: @@ -210,7 +241,7 @@ def get( *, forward_tolerance: float = 0.0, warn: bool = True, - ) -> Transform | None: + ) -> TransformStamped | None: result = self._get(parent_frame, child_frame, time_point, time_tolerance) if result is None and forward_tolerance > 0: result = self._wait_get( @@ -240,7 +271,7 @@ def get_pose( ) if not tf: return None - return tf.to_pose() + return pose_from_transform(tf) def get_transform_search( self, @@ -248,7 +279,7 @@ def get_transform_search( child_frame: str, time_point: float | None = None, time_tolerance: float | None = None, - ) -> list[Transform] | None: + ) -> list[TransformStamped] | None: """Search for shortest transform chain between parent and child frames using BFS.""" with self._cv: # Check if direct transform exists (already checked in get_transform, but for clarity) @@ -257,7 +288,7 @@ def get_transform_search( return [direct] # BFS to find shortest path - queue: deque[tuple[str, list[Transform]]] = deque([(parent_frame, [])]) + queue: deque[tuple[str, list[TransformStamped]]] = deque([(parent_frame, [])]) visited = {parent_frame} while queue: @@ -283,8 +314,6 @@ def get_transform_search( return None def graph(self) -> str: - import subprocess - def connection_str(connection: tuple[str, str]) -> str: (frame_from, frame_to) = connection return f"{frame_from} -> {frame_to}" @@ -335,10 +364,10 @@ def __init__(self, stream: Any | None = None, buffer_size: float = 10.0) -> None if stream is not None: self._unsubscribe = stream.subscribe(self.receive_tfmessage) - def publish(self, *transforms: Transform) -> None: + def publish(self, *transforms: TransformStamped) -> None: self.receive_transform(*transforms) if self._stream is not None: - self._stream.publish(TFMessage(*transforms)) + self._stream.publish(TFMessage(transforms=list(transforms))) def dispose(self) -> None: if self._unsubscribe is not None: diff --git a/dimos/robot/assembly/mid360_realsense_30.py b/dimos/robot/assembly/mid360_realsense_30.py index 50956d1318b..2039d31ea79 100644 --- a/dimos/robot/assembly/mid360_realsense_30.py +++ b/dimos/robot/assembly/mid360_realsense_30.py @@ -46,6 +46,9 @@ import math +from dimos_generated.geometry_msgs.msg import TransformStamped +from dimos_generated.sensor_msgs.msg import CameraInfo, Image, PointCloud2 + from dimos.core.coordination.blueprints import autoconnect from dimos.core.stream import In from dimos.hardware.sensors.camera.realsense.camera import RealSenseCamera @@ -53,10 +56,6 @@ from dimos.hardware.sensors.lidar.pointlio.module import PointLio from dimos.hardware.sensors.lidar.pointlio.recorder import PointlioRecorder from dimos.hardware.sensors.lidar.virtual_mid360.recorder import Mid360PcapRecorder -from dimos.msgs.geometry_msgs.Transform import Transform -from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo -from dimos.msgs.sensor_msgs.Image import Image -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 from dimos.protocol.tf.static_tf_publisher import ( FrameSpec, StaticTfPublisher, @@ -92,7 +91,7 @@ class Mid360RealsenseStaticTf(StaticTfPublisher): """Publishes the RealSense/Mid-360 mount tree onto tf on a fixed interval.""" - def transforms(self) -> list[Transform]: + def transforms(self) -> list[TransformStamped]: return frames_to_edge_transforms(FRAMES) diff --git a/dimos/robot/deeprobotics/m20/blueprints/m20_kronknav.py b/dimos/robot/deeprobotics/m20/blueprints/m20_kronknav.py index 163e41f6c5d..64ddc4dc15c 100644 --- a/dimos/robot/deeprobotics/m20/blueprints/m20_kronknav.py +++ b/dimos/robot/deeprobotics/m20/blueprints/m20_kronknav.py @@ -16,12 +16,13 @@ from typing import Any +from dimos_generated.foxglove_msgs.msg import CompressedVideo +from dimos_generated.sensor_msgs.msg import CameraInfo + from dimos.core.coordination.blueprints import autoconnect from dimos.core.global_config import global_config from dimos.core.transport import ZenohTransport from dimos.mapping.ray_tracing.module import RayTracingVoxelMap -from dimos.msgs.foxglove_msgs.CompressedVideo import CompressedVideo -from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo from dimos.navigation.dannav.holonomic_tc.module import DanHolonomicTC from dimos.navigation.dannav.local_planner.module import DanLocalPlanner from dimos.navigation.movement_manager.movement_manager import MovementManager @@ -37,6 +38,7 @@ PLANNING_HEIGHT_M, ROTATION_DIAMETER_M, ) +from dimos.visualization.rerun.message_helpers import camera_pinhole, video_archetype from dimos.visualization.vis_module import vis_module VOXEL_SIZE_M = 0.1 @@ -50,23 +52,15 @@ def _render_path(msg: Any) -> Any: def _render_h265(msg: CompressedVideo) -> Any: - import rerun as rr - - return rr.VideoStream(codec=rr.VideoCodec.H265, sample=msg.data.tobytes()) + return video_archetype(msg) def _render_front_camera_info(msg: CameraInfo) -> Any: - return msg.to_rerun( - image_topic="world/front_camera", - optical_frame="front_camera_optical", - ) + return [("world/front_camera", camera_pinhole(msg))] def _render_rear_camera_info(msg: CameraInfo) -> Any: - return msg.to_rerun( - image_topic="world/rear_camera", - optical_frame="rear_camera_optical", - ) + return [("world/rear_camera", camera_pinhole(msg))] def _static_robot_body(rr: Any) -> list[Any]: diff --git a/dimos/robot/deeprobotics/m20/camera.py b/dimos/robot/deeprobotics/m20/camera.py index 4e0b18957f1..c7db9a6d594 100644 --- a/dimos/robot/deeprobotics/m20/camera.py +++ b/dimos/robot/deeprobotics/m20/camera.py @@ -19,17 +19,18 @@ from typing import Any import av +from dimos_generated.foxglove_msgs.msg import CompressedVideo +from dimos_generated.geometry_msgs.msg import Quaternion, Transform, TransformStamped, Vector3 +from dimos_generated.sensor_msgs.msg import CameraInfo +from dimos_generated.std_msgs.msg import Header +from dimos_generated.tf2_msgs.msg import TFMessage from dimos.constants import DEFAULT_THREAD_JOIN_TIMEOUT from dimos.core.core import rpc from dimos.core.module import Module from dimos.core.stream import Out -from dimos.msgs.foxglove_msgs.CompressedVideo import CompressedVideo -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Transform import Transform -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo -from dimos.msgs.tf2_msgs.TFMessage import TFMessage +from dimos.msgs.camera_info import camera_info_from_intrinsics, camera_info_with_stamp +from dimos.msgs.time import time_from_seconds from dimos.robot.deeprobotics.m20.constants import ( FRONT_CAMERA_RTSP_URL, REAR_CAMERA_RTSP_URL, @@ -44,27 +45,27 @@ # factory calibration. _FRONT_CAMERA_XYZ = (0.37646, 0.0, 0.03738) _REAR_CAMERA_XYZ = (-0.37646, 0.0, 0.03738) -_OPTICAL_ROT = Quaternion(-0.5, 0.5, -0.5, 0.5) +_OPTICAL_ROT = Quaternion(x=-0.5, y=0.5, z=-0.5, w=0.5) _CAMERA_WIDTH = 800 _CAMERA_HEIGHT = 600 _CAMERA_FOCAL_LENGTH = 607.0 * _CAMERA_WIDTH / 1280.0 -_FRONT_CAMERA_INFO = CameraInfo.from_intrinsics( +_FRONT_CAMERA_INFO = camera_info_from_intrinsics( fx=_CAMERA_FOCAL_LENGTH, fy=_CAMERA_FOCAL_LENGTH, cx=_CAMERA_WIDTH * 0.5, cy=_CAMERA_HEIGHT * 0.5, width=_CAMERA_WIDTH, height=_CAMERA_HEIGHT, - frame_id="front_camera_optical", + header=Header(frame_id="front_camera_optical"), ) -_REAR_CAMERA_INFO = CameraInfo.from_intrinsics( +_REAR_CAMERA_INFO = camera_info_from_intrinsics( fx=_CAMERA_FOCAL_LENGTH, fy=_CAMERA_FOCAL_LENGTH, cx=_CAMERA_WIDTH * 0.5, cy=_CAMERA_HEIGHT * 0.5, width=_CAMERA_WIDTH, height=_CAMERA_HEIGHT, - frame_id="rear_camera_optical", + header=Header(frame_id="rear_camera_optical"), ) @@ -115,35 +116,54 @@ def _publish_camera_metadata(self) -> None: now = time.time() self.tf.publish( TFMessage( - Transform( - translation=Vector3(*_FRONT_CAMERA_XYZ), - frame_id="base_link", - child_frame_id="front_camera_link", - ts=now, - ), - Transform( - rotation=_OPTICAL_ROT, - frame_id="front_camera_link", - child_frame_id="front_camera_optical", - ts=now, - ), - Transform( - translation=Vector3(*_REAR_CAMERA_XYZ), - rotation=Quaternion(0.0, 0.0, 1.0, 0.0), - frame_id="base_link", - child_frame_id="rear_camera_link", - ts=now, - ), - Transform( - rotation=_OPTICAL_ROT, - frame_id="rear_camera_link", - child_frame_id="rear_camera_optical", - ts=now, - ), + transforms=[ + TransformStamped( + header=Header(frame_id="base_link", stamp=time_from_seconds(now)), + child_frame_id="front_camera_link", + transform=Transform( + translation=Vector3( + x=_FRONT_CAMERA_XYZ[0], + y=_FRONT_CAMERA_XYZ[1], + z=_FRONT_CAMERA_XYZ[2], + ), + rotation=Quaternion(w=1.0), + ), + ), + TransformStamped( + header=Header( + frame_id="front_camera_link", stamp=time_from_seconds(now) + ), + child_frame_id="front_camera_optical", + transform=Transform(rotation=_OPTICAL_ROT), + ), + TransformStamped( + header=Header(frame_id="base_link", stamp=time_from_seconds(now)), + child_frame_id="rear_camera_link", + transform=Transform( + translation=Vector3( + x=_REAR_CAMERA_XYZ[0], + y=_REAR_CAMERA_XYZ[1], + z=_REAR_CAMERA_XYZ[2], + ), + rotation=Quaternion(z=1.0), + ), + ), + TransformStamped( + header=Header( + frame_id="rear_camera_link", stamp=time_from_seconds(now) + ), + child_frame_id="rear_camera_optical", + transform=Transform(rotation=_OPTICAL_ROT), + ), + ] ) ) - self.front_camera_info.publish(_FRONT_CAMERA_INFO.with_ts(now)) - self.rear_camera_info.publish(_REAR_CAMERA_INFO.with_ts(now)) + self.front_camera_info.publish( + camera_info_with_stamp(_FRONT_CAMERA_INFO, time_from_seconds(now)) + ) + self.rear_camera_info.publish( + camera_info_with_stamp(_REAR_CAMERA_INFO, time_from_seconds(now)) + ) self._stop_event.wait(1.0) def _relay(self, url: str, output: Out[CompressedVideo], camera_name: str) -> None: @@ -168,10 +188,10 @@ def _relay(self, url: str, output: Out[CompressedVideo], camera_name: str) -> No continue output.publish( CompressedVideo( - bytes(packet), + data=bytes(packet), format="h265", frame_id="", - ts=time.time(), + timestamp=time_from_seconds(time.time()), ) ) except (av.FFmpegError, IndexError, ValueError) as exc: diff --git a/dimos/robot/deeprobotics/m20/connection.py b/dimos/robot/deeprobotics/m20/connection.py index f820df24fc4..2e9c076b1c6 100644 --- a/dimos/robot/deeprobotics/m20/connection.py +++ b/dimos/robot/deeprobotics/m20/connection.py @@ -23,18 +23,15 @@ from threading import Condition, RLock from typing import Any, Literal +from dimos_generated.geometry_msgs.msg import PoseStamped, Twist +from dimos_generated.nav_msgs.msg import Odometry +from dimos_generated.std_msgs.msg import Bool, Int32, UInt32 from pydantic import Field from reactivex.disposable import Disposable from dimos.core.core import rpc from dimos.core.module import Module, ModuleConfig from dimos.core.stream import In, Out -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.geometry_msgs.Twist import Twist -from dimos.msgs.nav_msgs.Odometry import Odometry -from dimos.msgs.std_msgs.Bool import Bool -from dimos.msgs.std_msgs.Int32 import Int32 -from dimos.msgs.std_msgs.UInt32 import UInt32 from dimos.utils.logging_config import setup_logger from dimos.utils.sequential_ids import SequentialIds @@ -139,7 +136,7 @@ def move(self, twist: Twist, duration: float = 0.0) -> bool: @rpc def stop_movement(self) -> None: """Publish an immediate zero velocity without disabling future commands.""" - self.cmd_vel.publish(Twist.zero()) + self.cmd_vel.publish(Twist()) @rpc def standup(self) -> bool: @@ -169,7 +166,7 @@ def standup(self) -> bool: def liedown(self) -> bool: """Disable velocity output and command the M20 to its Sit/prone state.""" self.stop_movement() - self.motion_state_cmd.publish(Int32(MOTION_SIT)) + self.motion_state_cmd.publish(Int32(data=MOTION_SIT)) return True @rpc @@ -221,7 +218,7 @@ def _on_gait_state(self, msg: UInt32) -> None: self._state_condition.notify_all() def _on_odometry(self, odometry: Odometry) -> None: - self.odom.publish(odometry.to_pose_stamped()) + self.odom.publish(PoseStamped(header=odometry.header, pose=odometry.pose.pose)) def _ensure_rl_control(self) -> bool: with self._lock: @@ -229,18 +226,18 @@ def _ensure_rl_control(self) -> bool: if motion_state == MOTION_RL_CONTROL: return True if motion_state != MOTION_STAND: - self.motion_state_cmd.publish(Int32(MOTION_STAND)) + self.motion_state_cmd.publish(Int32(data=MOTION_STAND)) if not self._wait_for_motion_state(MOTION_STAND, self.config.stand_timeout_s): logger.error("M20 did not confirm Stand before the transition timeout") return False - self.motion_state_cmd.publish(Int32(MOTION_RL_CONTROL)) + self.motion_state_cmd.publish(Int32(data=MOTION_RL_CONTROL)) if not self._wait_for_motion_state(MOTION_RL_CONTROL, self.config.rl_control_timeout_s): logger.error("M20 did not confirm RL Control after standing") return False return True def _set_gait_and_wait(self, gait: int) -> bool: - self.gait_cmd.publish(UInt32(gait)) + self.gait_cmd.publish(UInt32(data=gait)) return self._wait_for_gait_state(gait, self.config.gait_timeout_s) def _enter_navigation_mode(self) -> bool: diff --git a/dimos/robot/deeprobotics/m20/test_camera_messages.py b/dimos/robot/deeprobotics/m20/test_camera_messages.py new file mode 100644 index 00000000000..89a9a8694c3 --- /dev/null +++ b/dimos/robot/deeprobotics/m20/test_camera_messages.py @@ -0,0 +1,52 @@ +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from threading import Event +from types import SimpleNamespace + +from dimos_generated.sensor_msgs.msg import CameraInfo +from dimos_generated.tf2_msgs.msg import TFMessage + +from dimos.robot.deeprobotics.m20.camera import M20CameraRelay + + +class _OneIteration(Event): + def wait(self, timeout=None): + self.set() + return True + + +def test_camera_metadata_uses_generated_wire_values_without_connecting_streams(): + transforms, front, rear = [], [], [] + proxy = SimpleNamespace( + _stop_event=_OneIteration(), + tf=SimpleNamespace(publish=transforms.append), + front_camera_info=SimpleNamespace(publish=front.append), + rear_camera_info=SimpleNamespace(publish=rear.append), + ) + M20CameraRelay._publish_camera_metadata(proxy) + tf = TFMessage.decode(transforms[0].encode()) + assert len(tf.transforms) == 4 + assert [t.child_frame_id for t in tf.transforms] == [ + "front_camera_link", + "front_camera_optical", + "rear_camera_link", + "rear_camera_optical", + ] + first, second = CameraInfo.decode(front[0].encode()), CameraInfo.decode(rear[0].encode()) + assert first.header.frame_id == "front_camera_optical" + assert second.header.frame_id == "rear_camera_optical" + assert first.header.stamp == second.header.stamp == tf.transforms[0].header.stamp + assert first.width == second.width == 800 + assert first.height == second.height == 600 diff --git a/dimos/robot/deeprobotics/m20/test_connection_messages.py b/dimos/robot/deeprobotics/m20/test_connection_messages.py new file mode 100644 index 00000000000..1c60f925b38 --- /dev/null +++ b/dimos/robot/deeprobotics/m20/test_connection_messages.py @@ -0,0 +1,53 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +# Copyright 2026 Dimensional Inc. +# SPDX-License-Identifier: Apache-2.0 + +from dimos_generated.builtin_interfaces.msg import Time +from dimos_generated.geometry_msgs.msg import ( + Point, + Pose, + PoseWithCovariance, + Quaternion, + Twist, + Vector3, +) +from dimos_generated.nav_msgs.msg import Odometry +from dimos_generated.std_msgs.msg import Bool, Header + +from dimos.robot.deeprobotics.m20.connection import M20Connection + + +def test_generated_forwarding_preserves_readiness_pose_clock_and_zero_stop(monkeypatch): + module = M20Connection() + velocities, poses = [], [] + monkeypatch.setattr(module.cmd_vel, "publish", velocities.append) + monkeypatch.setattr(module.odom, "publish", poses.append) + command = Twist(linear=Vector3(x=0.1), angular=Vector3(z=0.2)) + try: + assert module.move(command) is False + module._on_command_ready(Bool(data=True)) + assert module.move(command) is True + source = Odometry( + header=Header(frame_id="map", stamp=Time(sec=1, nanosec=123456789)), + pose=PoseWithCovariance(pose=Pose(position=Point(x=2), orientation=Quaternion(w=1))), + ) + module._on_odometry(source) + assert poses[0].header == source.header and poses[0].pose == source.pose.pose + module.stop_movement() + assert velocities[-1] == Twist() + assert Twist.decode(velocities[0].encode()) == command + finally: + module.stop() diff --git a/dimos/robot/diy/alfred/config.py b/dimos/robot/diy/alfred/config.py index cef7bf0e7b9..09c6295cd77 100644 --- a/dimos/robot/diy/alfred/config.py +++ b/dimos/robot/diy/alfred/config.py @@ -18,9 +18,7 @@ from pathlib import Path from typing import Any -from dimos.msgs.geometry_msgs.Pose import Pose -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Vector3 import Vector3 +from dimos_generated.geometry_msgs.msg import Point, Pose DEFAULT_ADDRESS = "172.6.2.20:11323" ALFRED_URDF = Path(__file__).resolve().parent / "alfred.urdf" @@ -52,6 +50,6 @@ class AlfredConfig: d455_serial="260922302422", internal_odom_offsets={ # Mid-360 lidar: a bit forward, and a bit to the right of base center, above ground. - "mid360_link": Pose(0.20, -0.20, 0.30, *Quaternion.from_euler(Vector3(0, 0, 0))), + "mid360_link": Pose(position=Point(x=0.20, y=-0.20, z=0.30)), }, ) diff --git a/dimos/robot/diy/alfred/effector_high_level.py b/dimos/robot/diy/alfred/effector_high_level.py index aeff8a199ed..07619405d43 100644 --- a/dimos/robot/diy/alfred/effector_high_level.py +++ b/dimos/robot/diy/alfred/effector_high_level.py @@ -36,6 +36,16 @@ import time from typing import TYPE_CHECKING, Any +from dimos_generated.geometry_msgs.msg import ( + Point, + Pose, + PoseWithCovariance, + Twist, + TwistWithCovariance, + Vector3, +) +from dimos_generated.nav_msgs.msg import Odometry +from dimos_generated.std_msgs.msg import Header import numpy as np from pydantic import Field, FiniteFloat @@ -44,11 +54,8 @@ from dimos.core.core import rpc from dimos.core.module import Module, ModuleConfig from dimos.core.stream import In, Out -from dimos.msgs.geometry_msgs.Pose import Pose -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Twist import Twist -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.msgs.nav_msgs.Odometry import Odometry +from dimos.msgs.geometry import quaternion_from_euler +from dimos.msgs.time import time_from_seconds from dimos.robot.diy.alfred.config import DEFAULT_ADDRESS from dimos.utils.logging_config import setup_logger @@ -171,7 +178,7 @@ async def move_velocity( self, x: float, y: float = 0.0, yaw: float = 0.0, duration: float = 0.0 ) -> str: """Move the Alfred at the given velocity for ``duration`` seconds.""" - twist = Twist(linear=Vector3(x, y, 0), angular=Vector3(0, 0, yaw)) + twist = Twist(linear=Vector3(x=x, y=y), angular=Vector3(z=yaw)) await self.move(twist, duration=duration) return f"Started moving with velocity=({x}, {y}, {yaw}) for {duration} seconds" @@ -218,21 +225,24 @@ async def _poll_wheel_odometry(self, client: portal.Client) -> None: forward = math.cos(heading) * dx + math.sin(heading) * dy left = -math.sin(heading) * dx + math.cos(heading) * dy twist = Twist( - linear=Vector3(forward / dt, left / dt, 0.0), - angular=Vector3(0.0, 0.0, turn / dt), + linear=Vector3(x=forward / dt, y=left / dt), + angular=Vector3(z=turn / dt), ) previous = (ts, x, y, yaw) self.wheel_odometry.publish( Odometry( - ts=ts, - frame_id=self.config.wheel_odom_frame_id, + header=Header( + stamp=time_from_seconds(ts), frame_id=self.config.wheel_odom_frame_id + ), child_frame_id=self.config.base_frame_id, - pose=Pose( - position=Vector3(x, y, 0.0), - orientation=Quaternion.from_euler(Vector3(0.0, 0.0, yaw)), + pose=PoseWithCovariance( + pose=Pose( + position=Point(x=x, y=y), + orientation=quaternion_from_euler(0.0, 0.0, yaw), + ) ), - twist=twist, + twist=TwistWithCovariance(twist=twist), ) ) except asyncio.CancelledError: diff --git a/dimos/robot/diy/alfred/mount_tf.py b/dimos/robot/diy/alfred/mount_tf.py index 660ffe15591..47962f957c0 100644 --- a/dimos/robot/diy/alfred/mount_tf.py +++ b/dimos/robot/diy/alfred/mount_tf.py @@ -25,14 +25,15 @@ from xml.etree import ElementTree -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Transform import Transform -from dimos.msgs.geometry_msgs.Vector3 import Vector3 +from dimos_generated.geometry_msgs.msg import Transform, TransformStamped, Vector3 +from dimos_generated.std_msgs.msg import Header + +from dimos.msgs.geometry import quaternion_from_euler from dimos.protocol.tf.static_tf_publisher import StaticTfPublisher from dimos.robot.diy.alfred.config import ALFRED_URDF -def mount_transforms() -> list[Transform]: +def mount_transforms() -> list[TransformStamped]: """One transform per fixed joint of the urdf, minus the imager frames. The drivers publish their own imager offsets from the factory extrinsics read @@ -52,11 +53,13 @@ def mount_transforms() -> list[Transform]: translation = [float(value) for value in origin.attrib["xyz"].split()] rpy = [float(value) for value in origin.attrib["rpy"].split()] transforms.append( - Transform( - translation=Vector3(*translation), - rotation=Quaternion.from_euler(Vector3(*rpy)), - frame_id=parent.attrib["link"], + TransformStamped( + header=Header(frame_id=parent.attrib["link"]), child_frame_id=child_link, + transform=Transform( + translation=Vector3(x=translation[0], y=translation[1], z=translation[2]), + rotation=quaternion_from_euler(*rpy), + ), ) ) return transforms @@ -65,5 +68,5 @@ def mount_transforms() -> list[Transform]: class AlfredMountTf(StaticTfPublisher): """Publishes Alfred's urdf mount tree onto tf on a fixed interval.""" - def transforms(self) -> list[Transform]: + def transforms(self) -> list[TransformStamped]: return mount_transforms() diff --git a/dimos/robot/diy/alfred/test_alfred.py b/dimos/robot/diy/alfred/test_alfred.py index f4b32d360a3..56025344ba8 100644 --- a/dimos/robot/diy/alfred/test_alfred.py +++ b/dimos/robot/diy/alfred/test_alfred.py @@ -26,7 +26,7 @@ def test_every_camera_frame_dim_slam_wants_is_rooted_at_base_link(): """cuVSLAM places no camera at all until every configured frame resolves against base_link, so one missing mount edge drops every image instead of degrading.""" - parents = {t.child_frame_id: t.frame_id for t in mount_transforms()} + parents = {t.child_frame_id: t.header.frame_id for t in mount_transforms()} for frame in [*IR_ENTITY_BY_FRAME, DEPTH_FRAME]: # The driver publishes the imager leaves off its own link; the mount tree owes # that link a path to base_link. diff --git a/dimos/robot/diy/alfred/test_generated_effector.py b/dimos/robot/diy/alfred/test_generated_effector.py new file mode 100644 index 00000000000..2bc95b58835 --- /dev/null +++ b/dimos/robot/diy/alfred/test_generated_effector.py @@ -0,0 +1,76 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from types import SimpleNamespace +from unittest.mock import MagicMock + +from dimos_generated.nav_msgs.msg import Odometry +import pytest + +from dimos.msgs.geometry import yaw +from dimos.robot.diy.alfred.effector_high_level import AlfredHighLevel + + +@pytest.mark.asyncio +async def test_wheel_odometry_generated_frame_inversion_and_local_velocity( + monkeypatch: pytest.MonkeyPatch, +) -> None: + module = AlfredHighLevel() + readings = iter( + [ + {"translation": [0.0, 0.0], "rotation": 0.0}, + {"translation": [1.0, -2.0], "rotation": 0.0}, + ] + ) + client = MagicMock() + + def get_odometry(_request: object) -> SimpleNamespace: + reading = next(readings) + return SimpleNamespace(done=lambda: True, result=lambda: reading) + + client.get_odometry.side_effect = get_odometry + stamps = iter([100.0, 101.0]) + monkeypatch.setattr( + "dimos.robot.diy.alfred.effector_high_level.time", + SimpleNamespace(time=lambda: next(stamps)), + ) + received: list[Odometry] = [] + + def publish(value: Odometry) -> None: + received.append(Odometry.decode(value.encode())) + if len(received) == 2: + module._odometry_stop.set() + + module.wheel_odometry = MagicMock() + module.wheel_odometry.publish.side_effect = publish + + async def wait_or_stop(_seconds: float) -> None: + return None + + monkeypatch.setattr(module, "_wait_or_stop", wait_or_stop) + await module._poll_wheel_odometry(client) + assert len(received) == 2 + first, last = received + assert first.header.stamp.sec == 100 + assert last.header.stamp.sec == 101 + assert last.header.frame_id == "wheel_odom" + assert last.child_frame_id == "base_link" + assert (last.pose.pose.position.x, last.pose.pose.position.y) == (1.0, 2.0) + assert yaw(last.pose.pose.orientation) == 0.0 + assert (last.twist.twist.linear.x, last.twist.twist.linear.y, last.twist.twist.angular.z) == ( + 1.0, + 2.0, + 0.0, + ) + client.set_target_velocity.assert_not_called() diff --git a/dimos/robot/diy/alfred/test_mount_tf.py b/dimos/robot/diy/alfred/test_mount_tf.py new file mode 100644 index 00000000000..758c3a1a37e --- /dev/null +++ b/dimos/robot/diy/alfred/test_mount_tf.py @@ -0,0 +1,44 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""URDF mount extraction emits generated transforms and excludes driver-owned frames.""" + +from dimos_generated.tf2_msgs.msg import TFMessage + +from dimos.robot.diy.alfred import mount_tf + + +def test_mounts_preserve_parent_and_translation_and_skip_imager_frames(tmp_path, monkeypatch): + urdf = tmp_path / "robot.urdf" + urdf.write_text(""" + + + + + + + + + """) + monkeypatch.setattr(mount_tf, "ALFRED_URDF", urdf) + + edges = mount_tf.mount_transforms() + decoded = TFMessage.decode(TFMessage(transforms=edges).encode()) + + assert len(decoded.transforms) == 1 + edge = decoded.transforms[0] + assert (edge.header.frame_id, edge.child_frame_id) == ("base", "lidar_link") + translation = edge.transform.translation + assert (translation.x, translation.y, translation.z) == (1, 2, 3) + assert edge.transform.rotation.w == 1 diff --git a/dimos/robot/drone/camera_module.py b/dimos/robot/drone/camera_module.py index 8f2b11631f0..453a1e1ad02 100644 --- a/dimos/robot/drone/camera_module.py +++ b/dimos/robot/drone/camera_module.py @@ -21,16 +21,18 @@ import time from typing import Any -from dimos_lcm.sensor_msgs import CameraInfo +from dimos_generated.geometry_msgs.msg import PoseStamped +from dimos_generated.sensor_msgs.msg import CameraInfo, Image +from dimos_generated.std_msgs.msg import Header +from dimos_generated.tf2_msgs.msg import TFMessage from dimos.constants import DEFAULT_THREAD_JOIN_TIMEOUT from dimos.core.core import rpc from dimos.core.module import Module from dimos.core.stream import In, Out -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.sensor_msgs.Image import Image -from dimos.msgs.std_msgs.Header import Header -from dimos.msgs.tf2_msgs.TFMessage import TFMessage +from dimos.msgs.geometry import pose_from_transform +from dimos.msgs.image import image_view +from dimos.msgs.time import to_seconds from dimos.utils.logging_config import setup_logger logger = setup_logger() @@ -140,10 +142,10 @@ def _processing_loop(self) -> None: self._latest_frame = None # Get numpy array from Image - img_array = frame.data + img_array = image_view(frame) # Create header - header = Header(self.camera_frame_id) + header = Header(stamp=frame.header.stamp, frame_id=self.camera_frame_id) # Publish camera info self._publish_camera_info(header, img_array.shape) @@ -177,15 +179,14 @@ def _publish_camera_info(self, header: Header, shape: tuple[int, ...]) -> None: P = [fx, 0, cx, 0, 0, fy, cy, 0, 0, 0, 1, 0] msg = CameraInfo( - D_length=len(D), header=header, height=height, width=width, distortion_model="plumb_bob", - D=D, - K=K, - R=R, - P=P, + d=D, + k=K, + r=R, + p=P, binning_x=0, binning_y=0, ) @@ -201,17 +202,13 @@ def _publish_camera_pose(self, header: Header) -> None: transform = self.tfbuffer.get( parent_frame=self.world_frame_id, child_frame=self.camera_frame_id, - time_point=header.ts, + time_point=to_seconds(header.stamp), time_tolerance=1.0, ) if transform: - pose_msg = PoseStamped( - ts=header.ts, - frame_id=self.camera_frame_id, - position=transform.translation, - orientation=transform.rotation, - ) + pose_msg = pose_from_transform(transform) + pose_msg.header.stamp = header.stamp self.camera_pose.publish(pose_msg) except Exception as e: diff --git a/dimos/robot/drone/connection_module.py b/dimos/robot/drone/connection_module.py index e823ca8defa..e2074d287e4 100644 --- a/dimos/robot/drone/connection_module.py +++ b/dimos/robot/drone/connection_module.py @@ -21,7 +21,17 @@ import time from typing import Any -from dimos_lcm.std_msgs import String +from dimos_generated.geometry_msgs.msg import ( + Pose, + PoseStamped, + Transform, + TransformStamped, + Twist, + Vector3, +) +from dimos_generated.sensor_msgs.msg import Image +from dimos_generated.std_msgs.msg import Header, String +from dimos_generated.tf2_msgs.msg import TFMessage from reactivex.disposable import Disposable from dimos.agents.annotation import skill @@ -30,13 +40,7 @@ from dimos.core.module import Module, ModuleConfig from dimos.core.stream import In, Out from dimos.mapping.models import LatLon -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Transform import Transform -from dimos.msgs.geometry_msgs.Twist import Twist -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.msgs.sensor_msgs.Image import Image -from dimos.msgs.tf2_msgs.TFMessage import TFMessage +from dimos.msgs.time import time_from_seconds from dimos.robot.drone.dji_video_stream import DJIDroneVideoStream from dimos.robot.drone.mavlink_connection import MavlinkConnection from dimos.utils.logging_config import setup_logger @@ -141,7 +145,7 @@ def start(self) -> None: ) # Subscribe to movement commands - self.register_disposable(Disposable(self.movecmd.subscribe(self.move))) + self.register_disposable(Disposable(self.movecmd.subscribe(self._on_move_command))) # Subscribe to Twist movement commands if self.movecmd_twist.transport: @@ -173,34 +177,32 @@ def _publish_tf(self, msg: PoseStamped) -> None: self.odom.publish(msg) # Publish base_link transform - base_link = Transform( - translation=msg.position, - rotation=msg.orientation, - frame_id="world", + position = msg.pose.position + base_link = TransformStamped( + header=msg.header, child_frame_id="base_link", - ts=msg.ts if hasattr(msg, "ts") else time.time(), + transform=Transform( + translation=Vector3(x=position.x, y=position.y, z=position.z), + rotation=msg.pose.orientation, + ), ) - - # Publish camera_link transform (camera mounted on front of drone, no gimbal factored in yet) - camera_link = Transform( - translation=Vector3(0.1, 0.0, -0.05), # 10cm forward, 5cm down - rotation=Quaternion(0.0, 0.0, 0.0, 1.0), # No rotation relative to base - frame_id="base_link", + camera_link = TransformStamped( + header=Header(stamp=time_from_seconds(time.time()), frame_id="base_link"), child_frame_id="camera_link", - ts=time.time(), + transform=Transform(translation=Vector3(x=0.1, z=-0.05)), ) - self.tf.publish(TFMessage(base_link, camera_link)) + self.tf.publish(TFMessage(transforms=[base_link, camera_link])) def _publish_status(self, status: dict[str, Any]) -> None: """Publish drone status as JSON string.""" self._status = status - status_str = String(json.dumps(status)) + status_str = String(data=json.dumps(status)) self.status.publish(status_str) def _publish_telemetry(self, telemetry: dict[str, Any]) -> None: """Publish full telemetry as JSON string.""" - telemetry_str = String(json.dumps(telemetry)) + telemetry_str = String(data=json.dumps(telemetry)) self.telemetry.publish(telemetry_str) self._latest_telemetry = telemetry @@ -222,10 +224,8 @@ def _telemetry_loop(self) -> None: if self._odom is None: # Publish default pose default_pose = PoseStamped( - position=Vector3(0, 0, 0), - orientation=Quaternion(0, 0, 0, 1), - frame_id="world", - ts=time.time(), + header=Header(stamp=time_from_seconds(time.time()), frame_id="world"), + pose=Pose(), ) self._publish_tf(default_pose) logger.debug("Publishing default odometry") @@ -265,7 +265,7 @@ def move(self, x: float = 0.0, y: float = 0.0, z: float = 0.0, duration: float = duration: How long to move (0 = continuous) """ if self.connection: - self.connection.move(Vector3(x, y, z), duration) + self.connection.move(Vector3(x=x, y=y, z=z), duration) @skill def takeoff(self, altitude: float = 3.0) -> bool: @@ -385,7 +385,7 @@ def follow_object( duration (float, optional): How long to track for. Defaults to 120.0. """ msg = {"object_description": object_description, "duration": duration} - self.follow_object_cmd.publish(String(json.dumps(msg))) + self.follow_object_cmd.publish(String(data=json.dumps(msg))) yield "Started trying to track. First, trying to find the object." @@ -416,6 +416,10 @@ def follow_object( else: yield f"Stopped tracking '{object_description}'" + def _on_move_command(self, message: Vector3) -> None: + """Unpack generated movement vectors at the scalar skill boundary.""" + self.move(message.x, message.y, message.z) + def _on_move_twist(self, msg: Twist) -> None: """Handle Twist movement commands from tracking/navigation. diff --git a/dimos/robot/drone/dji_video_stream.py b/dimos/robot/drone/dji_video_stream.py index 92afc1ce507..54229da7480 100644 --- a/dimos/robot/drone/dji_video_stream.py +++ b/dimos/robot/drone/dji_video_stream.py @@ -23,10 +23,13 @@ import time from typing import Any +from dimos_generated.sensor_msgs.msg import Image +from dimos_generated.std_msgs.msg import Header import numpy as np from reactivex import Observable, Subject -from dimos.msgs.sensor_msgs.Image import Image, ImageFormat +from dimos.msgs.image import image_from_array +from dimos.msgs.time import time_from_seconds from dimos.utils.logging_config import setup_logger logger = setup_logger() @@ -137,7 +140,9 @@ def _capture_loop(self) -> None: frame = frame.reshape((self.height, self.width, channels)) # Create Image message (RGB format - matches GStreamer pipeline output) - img_msg = Image.from_numpy(frame, format=ImageFormat.RGB) + img_msg = image_from_array( + frame, encoding="rgb8", header=Header(stamp=time_from_seconds(time.time())) + ) # Publish self._video_subject.on_next(img_msg) @@ -217,8 +222,8 @@ def get_stream(self) -> Observable[Image]: # type: ignore[override] from dimos.utils.testing.legacy_pickle import LegacyPickleStore def _fix_format(img: Image) -> Image: - if img.format == ImageFormat.BGR: - img.format = ImageFormat.RGB + if img.encoding == "bgr8": + img.encoding = "rgb8" return img logger.info("Creating video replay stream") diff --git a/dimos/robot/drone/drone_tracking_module.py b/dimos/robot/drone/drone_tracking_module.py index ec18323ad3f..e22be472c9d 100644 --- a/dimos/robot/drone/drone_tracking_module.py +++ b/dimos/robot/drone/drone_tracking_module.py @@ -20,7 +20,9 @@ import time from typing import Any -from dimos_lcm.std_msgs import String +from dimos_generated.geometry_msgs.msg import Twist, Vector3 +from dimos_generated.sensor_msgs.msg import Image +from dimos_generated.std_msgs.msg import Header, String import numpy as np from numpy.typing import NDArray @@ -29,9 +31,8 @@ from dimos.core.module import Module from dimos.core.stream import In, Out from dimos.models.qwen.video_query import get_bbox_from_qwen_frame -from dimos.msgs.geometry_msgs.Twist import Twist -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.msgs.sensor_msgs.Image import Image, ImageFormat +from dimos.msgs.image import image_from_array, image_view +from dimos.msgs.time import time_from_seconds from dimos.robot.drone.drone_visual_servoing_controller import ( DroneVisualServoingController, PIDParams, @@ -114,7 +115,7 @@ def _get_latest_frame(self) -> np.ndarray[Any, np.dtype[Any]] | None: if self._latest_frame is None: return None # Convert Image to numpy array - data: np.ndarray[Any, np.dtype[Any]] = self._latest_frame.data + data: np.ndarray[Any, np.dtype[Any]] = image_view(self._latest_frame) return data @rpc @@ -270,8 +271,8 @@ def _visual_servoing_loop(self, tracker: Any, duration: float) -> None: # Publish velocity command via LCM if self.cmd_vel.transport: twist = Twist() - twist.linear = Vector3(vx, vy, 0) - twist.angular = Vector3(0, 0, 0) # No rotation for now + twist.linear = Vector3(x=vx, y=vy) + twist.angular = Vector3() # No rotation for now self.cmd_vel.publish(twist) # Publish visualization if transport is set @@ -279,7 +280,11 @@ def _visual_servoing_loop(self, tracker: Any, duration: float) -> None: overlay = self._draw_tracking_overlay( frame, (int(x), int(y), int(w), int(h)), (int(current_x), int(current_y)) ) - overlay_msg = Image.from_numpy(overlay, format=ImageFormat.BGR) + overlay_msg = image_from_array( + overlay, + encoding="bgr8", + header=Header(stamp=time_from_seconds(time.time())), + ) self.tracking_overlay.publish(overlay_msg) # Publish status @@ -304,8 +309,8 @@ def _visual_servoing_loop(self, tracker: Any, duration: float) -> None: # Stop movement by publishing zero velocity if self.cmd_vel.transport: stop_twist = Twist() - stop_twist.linear = Vector3(0, 0, 0) - stop_twist.angular = Vector3(0, 0, 0) + stop_twist.linear = Vector3() + stop_twist.angular = Vector3() self.cmd_vel.publish(stop_twist) self._tracking_active = False logger.info(f"Visual servoing loop ended after {frame_count} frames") @@ -366,7 +371,7 @@ def _publish_status(self, status: dict[str, Any]) -> None: status: Status dictionary """ if self.tracking_status.transport: - status_msg = String(json.dumps(status)) + status_msg = String(data=json.dumps(status)) self.tracking_status.publish(status_msg) def _stop_tracking(self) -> None: @@ -378,8 +383,8 @@ def _stop_tracking(self) -> None: # Send stop command via LCM if self.cmd_vel.transport: stop_twist = Twist() - stop_twist.linear = Vector3(0, 0, 0) - stop_twist.angular = Vector3(0, 0, 0) + stop_twist.linear = Vector3() + stop_twist.angular = Vector3() self.cmd_vel.publish(stop_twist) self._publish_status({"status": "stopped", "object": self._current_object}) diff --git a/dimos/robot/drone/mavlink_connection.py b/dimos/robot/drone/mavlink_connection.py index e908be4ab03..1bb15870c63 100644 --- a/dimos/robot/drone/mavlink_connection.py +++ b/dimos/robot/drone/mavlink_connection.py @@ -20,13 +20,13 @@ import time from typing import Any +from dimos_generated.geometry_msgs.msg import Point, Pose, PoseStamped, Twist, Vector3 +from dimos_generated.std_msgs.msg import Header from pymavlink import mavutil # type: ignore[import-untyped] from reactivex import Subject -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Twist import Twist -from dimos.msgs.geometry_msgs.Vector3 import Vector3 +from dimos.msgs.geometry import quaternion_from_euler +from dimos.msgs.time import time_from_seconds from dimos.utils.logging_config import setup_logger logger = setup_logger(level=logging.INFO) @@ -167,7 +167,7 @@ def _publish_odom(self) -> None: # MAVLink --> ROS conversion # MAVLink: positive pitch = nose up, positive yaw = clockwise # ROS: positive pitch = nose down, positive yaw = counter-clockwise - quaternion = Quaternion.from_euler(Vector3(roll, -pitch, -yaw)) + quaternion = quaternion_from_euler(roll, -pitch, -yaw) if not hasattr(self, "_position"): self._position = {"x": 0.0, "y": 0.0, "z": 0.0} @@ -229,10 +229,8 @@ def _publish_odom(self) -> None: ) pose = PoseStamped( - position=Vector3(self._position["x"], self._position["y"], self._position["z"]), - orientation=quaternion, - frame_id="world", - ts=current_time, + header=Header(stamp=time_from_seconds(current_time), frame_id="world"), + pose=Pose(position=Point(**self._position), orientation=quaternion), ) self._odom_subject.on_next(pose) diff --git a/dimos/robot/drone/test_drone.py b/dimos/robot/drone/test_drone.py index 2e7336c2c7d..ce30006ea4f 100644 --- a/dimos/robot/drone/test_drone.py +++ b/dimos/robot/drone/test_drone.py @@ -23,12 +23,13 @@ import unittest from unittest.mock import MagicMock, patch +from dimos_generated.geometry_msgs.msg import PoseStamped, Vector3 +from dimos_generated.sensor_msgs.msg import Image +from dimos_generated.std_msgs.msg import Header import numpy as np import pytest -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.msgs.sensor_msgs.Image import Image, ImageFormat +from dimos.msgs.image import image_from_array, image_view from dimos.robot.drone.connection_module import DroneConnectionModule from dimos.robot.drone.dji_video_stream import FakeDJIVideoStream @@ -85,7 +86,7 @@ def recv_side_effect(*args, **kwargs): # Verify NED to ROS conversion happened # ROS uses different conventions: positive pitch = nose down, positive yaw = counter-clockwise # So we expect sign flips in the quaternion conversion - self.assertIsNotNone(pose.orientation) + self.assertIsNotNone(pose.pose.orientation) def test_position_integration(self) -> None: """Test velocity integration for indoor flight positioning.""" @@ -221,7 +222,10 @@ def test_fake_mavlink_connection(self) -> None: def test_fake_video_stream_no_throttling(self) -> None: """Test FakeDJIVideoStream returns replay stream with format fix.""" - with patch("dimos.utils.testing.legacy_pickle.LegacyPickleStore") as mock_replay: + with ( + patch("dimos.utils.data.get_data") as mock_get_data, + patch("dimos.utils.testing.legacy_pickle.LegacyPickleStore") as mock_replay, + ): mock_stream = MagicMock() mock_replay.return_value.stream.return_value = mock_stream @@ -229,6 +233,7 @@ def test_fake_video_stream_no_throttling(self) -> None: stream.get_stream() # Verify replay store was created and stream was piped (for BGR→RGB fix) + mock_get_data.assert_called_once_with("drone") mock_replay.assert_called_once_with("drone/video") mock_replay.return_value.stream.assert_called_once() mock_stream.pipe.assert_called_once() @@ -357,16 +362,14 @@ def replay_side_effect(store_name: str): publish=lambda x: ( published_odom.append(x), print( - f"[TEST] Published odom: position=({x.position.x:.2f}, {x.position.y:.2f}, {x.position.z:.2f})" + f"[TEST] Published odom: position=({x.pose.position.x:.2f}, {x.pose.position.y:.2f}, {x.pose.position.z:.2f})" ), ) ) module.video = MagicMock( publish=lambda x: ( published_video.append(x), - print( - f"[TEST] Published video frame with shape: {x.data.shape if hasattr(x, 'data') else 'unknown'}" - ), + print(f"[TEST] Published video frame with shape: {image_view(x).shape}"), ) ) module.status = MagicMock( @@ -505,7 +508,7 @@ def test_video_stream_replay(self, mock_get_data, mock_replay) -> None: def subscribe_side_effect(callback) -> None: for frame in video_frames: - img = Image(data=frame, format=ImageFormat.BGR) + img = image_from_array(frame, encoding="bgr8", header=Header()) callback(img) received_frames.append(img) @@ -517,7 +520,7 @@ def subscribe_side_effect(callback) -> None: def piped_subscribe(callback): for frame in video_frames: - img = Image(data=frame, format=ImageFormat.RGB) # After format fix + img = image_from_array(frame, encoding="rgb8", header=Header()) # After format fix callback(img) piped_captured.append(img) @@ -537,8 +540,8 @@ def piped_subscribe(callback): self.assertEqual(len(piped_captured), 2) for _i, frame in enumerate(piped_captured): self.assertIsInstance(frame, Image) - self.assertEqual(frame.data.shape, (360, 640, 3)) - self.assertEqual(frame.format, ImageFormat.RGB) # Format should be corrected + self.assertEqual(image_view(frame).shape, (360, 640, 3)) + self.assertEqual(frame.encoding, "rgb8") # Format should be corrected class TestDroneMovementAndOdometry(unittest.TestCase): @@ -556,7 +559,7 @@ def test_movement_command_conversion(self, mock_get_data, mock_replay) -> None: # Test movement in ROS frame # ROS: X=forward, Y=left, Z=up - velocity_ros = Vector3(2.0, -1.0, 0.5) # Forward 2m/s, right 1m/s, up 0.5m/s + velocity_ros = Vector3(x=2.0, y=-1.0, z=0.5) # Forward 2m/s, right 1m/s, up 0.5m/s result = conn.move(velocity_ros, duration=1.0) self.assertTrue(result) @@ -608,8 +611,8 @@ def replay_stream_subscribe(callback) -> None: # Check odometry message odom = published_odom[0] self.assertIsInstance(odom, PoseStamped) - self.assertIsNotNone(odom.orientation) - self.assertEqual(odom.frame_id, "world") + self.assertIsNotNone(odom.pose.orientation) + self.assertEqual(odom.header.frame_id, "world") @patch("dimos.utils.testing.legacy_pickle.LegacyPickleStore") @patch("dimos.utils.data.get_data") diff --git a/dimos/robot/galaxea/r1pro/blueprints/basic/r1pro_coordinator.py b/dimos/robot/galaxea/r1pro/blueprints/basic/r1pro_coordinator.py index 96927e59a48..6ea1584b327 100644 --- a/dimos/robot/galaxea/r1pro/blueprints/basic/r1pro_coordinator.py +++ b/dimos/robot/galaxea/r1pro/blueprints/basic/r1pro_coordinator.py @@ -28,21 +28,17 @@ from collections.abc import Sequence from typing import Any +from dimos_generated.dimos_msgs.msg import MotorCommandArray +from dimos_generated.geometry_msgs.msg import PoseStamped, Twist +from dimos_generated.nav_msgs.msg import Odometry +from dimos_generated.sensor_msgs.msg import CompressedImage, Image, Imu, JointState, PointCloud2 + from dimos.control.components import HardwareComponent, HardwareType, make_twist_base_joints from dimos.control.coordinator import ControlCoordinator, TaskConfig from dimos.control.tasks.trajectory_task.trajectory_task import joint_trajectory_task from dimos.core.coordination.blueprints import Blueprint, TransportSpec, autoconnect from dimos.core.global_config import global_config from dimos.core.transport import ZenohTransport -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.geometry_msgs.Twist import Twist -from dimos.msgs.nav_msgs.Odometry import Odometry -from dimos.msgs.sensor_msgs.CompressedImage import CompressedImage -from dimos.msgs.sensor_msgs.Image import Image -from dimos.msgs.sensor_msgs.Imu import Imu -from dimos.msgs.sensor_msgs.JointState import JointState -from dimos.msgs.sensor_msgs.MotorCommandArray import MotorCommandArray -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 from dimos.protocol.pubsub.impl.zenohpubsub import QOS_LATEST_WINS, Topic as ZenohTopic, Zenoh from dimos.robot.galaxea.r1pro.connection import R1PRO_UPPER_BODY_JOINTS, R1ProConnection from dimos.visualization.rerun.bridge import RerunBridgeModule diff --git a/dimos/robot/galaxea/r1pro/blueprints/manipulation/test_r1pro_planar_preview.py b/dimos/robot/galaxea/r1pro/blueprints/manipulation/test_r1pro_planar_preview.py index fdc0692dd1e..d7967235bbb 100644 --- a/dimos/robot/galaxea/r1pro/blueprints/manipulation/test_r1pro_planar_preview.py +++ b/dimos/robot/galaxea/r1pro/blueprints/manipulation/test_r1pro_planar_preview.py @@ -14,6 +14,7 @@ """R1 Pro real-hardware and planar-preview blueprint contracts.""" +from dimos_generated.trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint import pytest from dimos.control.components import HardwareType, make_twist_base_joints @@ -22,9 +23,8 @@ from dimos.core.coordination.blueprint_config.parser import BlueprintConfigParser from dimos.manipulation.manipulation_module import ManipulationModule, ManipulationModuleConfig from dimos.manipulation.planning.spec.validation import prepare_robot_model -from dimos.msgs.trajectory_msgs.JointTrajectory import JointTrajectory -from dimos.msgs.trajectory_msgs.TrajectoryPoint import TrajectoryPoint -from dimos.msgs.trajectory_msgs.TrajectoryStatus import TrajectoryState +from dimos.msgs.time import duration_from_seconds, header_now +from dimos.msgs.trajectory import TrajectoryState from dimos.robot.galaxea.r1pro.blueprints.basic.r1pro_coordinator import r1pro_control from dimos.robot.galaxea.r1pro.blueprints.manipulation.r1pro_planar_preview import ( r1pro_planar_preview, @@ -44,11 +44,18 @@ def _coordinator_config(blueprint) -> ControlCoordinatorConfig: def _forward(distance: float, duration: float) -> JointTrajectory: velocity = [distance / duration, 0.0, 0.0] return JointTrajectory( + header=header_now(), joint_names=["x", "y", "yaw"], points=[ - TrajectoryPoint(time_from_start=0.0, positions=[0.0, 0.0, 0.0], velocities=velocity), - TrajectoryPoint( - time_from_start=duration, positions=[distance, 0.0, 0.0], velocities=velocity + JointTrajectoryPoint( + time_from_start=duration_from_seconds(0.0), + positions=[0.0, 0.0, 0.0], + velocities=velocity, + ), + JointTrajectoryPoint( + time_from_start=duration_from_seconds(duration), + positions=[distance, 0.0, 0.0], + velocities=velocity, ), ], ) @@ -87,7 +94,7 @@ def test_planar_preview_drives_its_mock_base_from_a_base_trajectory(wait_until) assert accepted.status is TrajectoryExecutionStatus.ACCEPTED wait_until( lambda: coordinator.task_invoke("base_trajectory", "get_status", {}).state - is TrajectoryState.COMPLETED, + == TrajectoryState.COMPLETED, timeout=5.0, message="the base trajectory never completed", ) diff --git a/dimos/robot/galaxea/r1pro/connection.py b/dimos/robot/galaxea/r1pro/connection.py index 3275427ce81..e83e587e767 100644 --- a/dimos/robot/galaxea/r1pro/connection.py +++ b/dimos/robot/galaxea/r1pro/connection.py @@ -37,6 +37,24 @@ import time from typing import TYPE_CHECKING, Any +from dimos_generated.dimos_msgs.msg import MotorCommandArray +from dimos_generated.geometry_msgs.msg import ( + Point, + Pose, + PoseStamped, + PoseWithCovariance, + Quaternion, + Transform, + TransformStamped, + Twist, + TwistStamped, + TwistWithCovariance, + Vector3, +) +from dimos_generated.nav_msgs.msg import Odometry +from dimos_generated.sensor_msgs.msg import CompressedImage, Image, Imu, JointState, PointCloud2 +from dimos_generated.std_msgs.msg import Header +from dimos_generated.tf2_msgs.msg import TFMessage from pydantic import Field from reactivex.disposable import Disposable @@ -48,17 +66,9 @@ from dimos.core.module import Module, ModuleConfig from dimos.core.stream import In, Out from dimos.hardware.whole_body.spec import VEL_STOP -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.geometry_msgs.Transform import Transform -from dimos.msgs.geometry_msgs.Twist import Twist -from dimos.msgs.nav_msgs.Odometry import Odometry -from dimos.msgs.sensor_msgs.CompressedImage import CompressedImage -from dimos.msgs.sensor_msgs.Image import Image -from dimos.msgs.sensor_msgs.Imu import Imu -from dimos.msgs.sensor_msgs.JointState import JointState -from dimos.msgs.sensor_msgs.MotorCommandArray import MotorCommandArray -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 -from dimos.msgs.tf2_msgs.TFMessage import TFMessage +from dimos.msgs.geometry import transform_from_pose +from dimos.msgs.time import header_now, time_from_nanoseconds +from dimos.protocol.pubsub.impl.rospubsub_conversion import dimos_to_ros, ros_to_dimos from dimos.robot.galaxea.r1pro.joints import UPPER_BODY_JOINTS, coordinator_name from dimos.utils.logging_config import setup_logger @@ -118,18 +128,9 @@ def _make_qos() -> Any: ) -def _stamp_secs(msg: Any) -> float: - """Header stamp in seconds, falling back to wall clock if the driver left it 0.""" - ts = msg.header.stamp.sec + msg.header.stamp.nanosec * 1e-9 - return ts if ts > 0 else time.time() - - -def _ros_stamp_now() -> Any: - from builtin_interfaces.msg import Time as RosTime - - t = time.time() - sec = int(t) - return RosTime(sec=sec, nanosec=int((t - sec) * 1e9)) +def _stamp_nanoseconds(msg: Any) -> int: + """Preserve the driver's source stamp, including zero, as integer nanoseconds.""" + return int(msg.header.stamp.sec) * 1_000_000_000 + int(msg.header.stamp.nanosec) class R1ProConnectionConfig(ModuleConfig): @@ -206,9 +207,9 @@ def __init__(self, **kwargs: Any) -> None: self._latest_right_dq: list[float] = [0.0] * 7 self._latest_right_eff: list[float] = [0.0] * 7 self._torso_seen = False - self._ts_torso = 0.0 - self._ts_left = 0.0 - self._ts_right = 0.0 + self._ts_torso = 0 + self._ts_left = 0 + self._ts_right = 0 self._left_seen = False self._right_seen = False self._latest_imu_chassis: Imu | None = None @@ -218,7 +219,7 @@ def __init__(self, **kwargs: Any) -> None: self._odom_x = 0.0 self._odom_y = 0.0 self._odom_yaw = 0.0 - self._odom_last_ts: float | None = None + self._odom_last_ns: int | None = None # Sensor decode pipeline. self._sensor_stop = threading.Event() @@ -510,16 +511,14 @@ def sensor_stats(self) -> dict[str, Any]: # Control input handlers def _on_motor_command(self, msg: MotorCommandArray) -> None: - if msg.num_joints != _NUM_MOTORS: - logger.warning(f"Expected {_NUM_MOTORS} motor commands, got {msg.num_joints}; ignoring") + if any(len(values) != _NUM_MOTORS for values in (msg.q, msg.dq, msg.kp, msg.kd, msg.tau)): + logger.warning(f"Expected {_NUM_MOTORS} values in each motor-command array; ignoring") return - from sensor_msgs.msg import JointState as RosJointState - ros = self._ros if ros is None: return # pre-start / post-stop - stamp = _ros_stamp_now() + header = header_now() for topic, sl in ( (self._cmd_torso_topic, _TORSO_SLICE), @@ -528,13 +527,14 @@ def _on_motor_command(self, msg: MotorCommandArray) -> None: ): if topic is None: continue - cmd = RosJointState() - cmd.header.stamp = stamp - cmd.name = [""] - cmd.position = list(msg.q[sl]) - cmd.velocity = self._tracking_velocities(msg.dq[sl]) - cmd.effort = [0.0] - ros.publish(topic, cmd) + cmd = JointState( + header=header, + name=[""], + position=list(msg.q)[sl], + velocity=self._tracking_velocities(list(msg.dq)[sl]), + effort=[0.0], + ) + ros.publish(topic, dimos_to_ros(cmd, topic.ros_type)) def _tracking_velocities(self, dqs: list[float]) -> list[float]: """Map MotorCommand.dq to ROS tracking velocity (0/sentinel → configured).""" @@ -542,18 +542,16 @@ def _tracking_velocities(self, dqs: list[float]) -> list[float]: return [speed if (v == 0.0 or v == VEL_STOP) else float(v) for v in dqs] def _on_cmd_vel(self, msg: Twist) -> None: - from geometry_msgs.msg import TwistStamped - ros = self._ros if ros is None or self._speed_topic is None: return - - cmd = TwistStamped() - cmd.header.stamp = _ros_stamp_now() - cmd.twist.linear.x = msg.linear.x - cmd.twist.linear.y = msg.linear.y - cmd.twist.angular.z = msg.angular.z - ros.publish(self._speed_topic, cmd) + cmd = TwistStamped( + header=header_now(), + twist=Twist( + linear=Vector3(x=msg.linear.x, y=msg.linear.y), angular=Vector3(z=msg.angular.z) + ), + ) + ros.publish(self._speed_topic, dimos_to_ros(cmd, self._speed_topic.ros_type)) # Control feedback callbacks (3 segments) @@ -562,7 +560,7 @@ def _on_feedback_torso(self, msg: Any, _topic: Any) -> None: self._copy_segment( msg, self._latest_torso_q, self._latest_torso_dq, self._latest_torso_eff ) - self._ts_torso = _stamp_secs(msg) + self._ts_torso = _stamp_nanoseconds(msg) self._torso_seen = True def _on_feedback_left(self, msg: Any, _topic: Any) -> None: @@ -570,7 +568,7 @@ def _on_feedback_left(self, msg: Any, _topic: Any) -> None: self._copy_segment( msg, self._latest_left_q, self._latest_left_dq, self._latest_left_eff ) - self._ts_left = _stamp_secs(msg) + self._ts_left = _stamp_nanoseconds(msg) self._left_seen = True def _on_feedback_right(self, msg: Any, _topic: Any) -> None: @@ -578,7 +576,7 @@ def _on_feedback_right(self, msg: Any, _topic: Any) -> None: self._copy_segment( msg, self._latest_right_q, self._latest_right_dq, self._latest_right_eff ) - self._ts_right = _stamp_secs(msg) + self._ts_right = _stamp_nanoseconds(msg) self._right_seen = True @staticmethod @@ -586,23 +584,23 @@ def _copy_segment( msg: Any, q_dst: list[float], dq_dst: list[float], eff_dst: list[float] ) -> None: n = min(len(msg.position), len(q_dst)) - q_dst[:n] = msg.position[:n] + q_dst[:n] = list(msg.position)[:n] if msg.velocity: nv = min(len(msg.velocity), len(dq_dst)) - dq_dst[:nv] = msg.velocity[:nv] + dq_dst[:nv] = list(msg.velocity)[:nv] if msg.effort: ne = min(len(msg.effort), len(eff_dst)) - eff_dst[:ne] = msg.effort[:ne] + eff_dst[:ne] = list(msg.effort)[:ne] # Wheel odometry (integrated from executed chassis speed) def _on_chassis_speed(self, msg: Any, _topic: Any) -> None: - now = msg.header.stamp.sec + msg.header.stamp.nanosec * 1e-9 - if self._odom_last_ts is None: - self._odom_last_ts = now + now = _stamp_nanoseconds(msg) + if self._odom_last_ns is None: + self._odom_last_ns = now return - dt = now - self._odom_last_ts - self._odom_last_ts = now + dt = (now - self._odom_last_ns) / 1_000_000_000 + self._odom_last_ns = now if dt <= 0.0 or dt > 1.0: # Clock jump or first-tick anomaly — keep position, refresh ts. return @@ -614,37 +612,42 @@ def _on_chassis_speed(self, msg: Any, _topic: Any) -> None: self._odom_y += (sy * vx + cy * vy) * dt self._odom_yaw += wz * dt - from dimos.msgs.geometry_msgs.Pose import Pose - from dimos.msgs.geometry_msgs.Quaternion import Quaternion - from dimos.msgs.geometry_msgs.Vector3 import Vector3 - half = self._odom_yaw * 0.5 - position = Vector3(self._odom_x, self._odom_y, 0.0) - orientation = Quaternion(0.0, 0.0, math.sin(half), math.cos(half)) - frame_id = self.config.odom_frame_id + header = Header(stamp=time_from_nanoseconds(now), frame_id=self.config.odom_frame_id) base = self.config.frame_id - pose = PoseStamped(ts=now, frame_id=frame_id, position=position, orientation=orientation) + pose = PoseStamped( + header=header, + pose=Pose( + position=Point(x=self._odom_x, y=self._odom_y), + orientation=Quaternion(z=math.sin(half), w=math.cos(half)), + ), + ) self.odom.publish(pose) self.odometry.publish( Odometry( - ts=now, - frame_id=frame_id, + header=header, child_frame_id=base, - pose=Pose(position, orientation), - twist=Twist(Vector3(vx, vy, 0.0), Vector3(0.0, 0.0, wz)), + pose=PoseWithCovariance(pose=pose.pose), + twist=TwistWithCovariance( + twist=Twist(linear=Vector3(x=vx, y=vy), angular=Vector3(z=wz)) + ), ) ) - # Both edges at the odom stamp: the voxel map matches each against the - # cloud stamp independently + # Both edges share the source stamp so clouds can resolve their full chain. self.tf.publish( TFMessage( - Transform.from_pose(base, pose), - Transform( - translation=Vector3(*_LIDAR_MOUNT_XYZ), - frame_id=base, - child_frame_id=self.config.lidar_frame_id, - ts=now, - ), + transforms=[ + transform_from_pose(pose, child_frame_id=base), + TransformStamped( + header=Header(stamp=header.stamp, frame_id=base), + child_frame_id=self.config.lidar_frame_id, + transform=Transform( + translation=Vector3( + x=_LIDAR_MOUNT_XYZ[0], y=_LIDAR_MOUNT_XYZ[1], z=_LIDAR_MOUNT_XYZ[2] + ) + ), + ), + ] ) ) @@ -690,8 +693,7 @@ def _publish_loop(self) -> None: if bootstrapped: self.motor_states.publish( JointState( - ts=ts, - frame_id=frame_id, + header=Header(stamp=time_from_nanoseconds(ts), frame_id=frame_id), name=R1PRO_UPPER_BODY_JOINTS, position=positions, # type: ignore[arg-type] velocity=velocities, @@ -706,7 +708,7 @@ def _publish_loop(self) -> None: next_tick += period sleep_for = next_tick - time.perf_counter() if sleep_for > 0: - time.sleep(sleep_for) + self._stop_event.wait(sleep_for) else: next_tick = time.perf_counter() @@ -729,16 +731,7 @@ def _compressed_image_loop(self, stream: str, q: queue.Queue[Any]) -> None: continue last_pub = t0 try: - stamp = msg.header.stamp - ts = stamp.sec + stamp.nanosec * 1e-9 - out.publish( - CompressedImage( - data=bytes(msg.data), - format="jpeg", - frame_id=msg.header.frame_id or stream, - ts=ts if ts > 0 else time.time(), - ) - ) + out.publish(ros_to_dimos(msg, CompressedImage)) self._record_decode(stream, (time.perf_counter() - t0) * 1e3, ok=True) except Exception: self._record_decode(stream, (time.perf_counter() - t0) * 1e3, ok=False) @@ -746,8 +739,6 @@ def _compressed_image_loop(self, stream: str, q: queue.Queue[Any]) -> None: def _convert_loop(self, stream: str, q: queue.Queue[Any], dimos_type: type) -> None: """ros_to_dimos passthrough worker (depth images, lidar).""" - from dimos.protocol.pubsub.impl.rospubsub_conversion import ros_to_dimos - out: Out[Any] = getattr(self, stream) while not self._sensor_stop.is_set(): try: @@ -766,8 +757,6 @@ def _convert_loop(self, stream: str, q: queue.Queue[Any], dimos_type: type) -> N def _imu_loop(self, stream: str, q: queue.Queue[Any]) -> None: """Store the latest converted IMU; re-emitted by the publish loop.""" - from dimos.protocol.pubsub.impl.rospubsub_conversion import ros_to_dimos - target_attr = "_latest_imu_chassis" if stream == "imu_chassis" else "_latest_imu_torso" while not self._sensor_stop.is_set(): try: diff --git a/dimos/robot/galaxea/r1pro/test_connection_messages.py b/dimos/robot/galaxea/r1pro/test_connection_messages.py new file mode 100644 index 00000000000..fd1670c06d8 --- /dev/null +++ b/dimos/robot/galaxea/r1pro/test_connection_messages.py @@ -0,0 +1,157 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import math + +from dimos_generated.builtin_interfaces.msg import Time +from dimos_generated.dimos_msgs.msg import MotorCommandArray +from dimos_generated.geometry_msgs.msg import Twist, TwistStamped, Vector3 +from dimos_generated.sensor_msgs.msg import JointState +from dimos_generated.std_msgs.msg import Header +import pytest + +from dimos.hardware.whole_body.spec import VEL_STOP +from dimos.protocol.pubsub.impl.rospubsub import RawROS, RawROSTopic +from dimos.robot.galaxea.r1pro import connection + + +@pytest.fixture() +def robot(): + robot = connection.R1ProConnection() + try: + yield robot + finally: + robot.stop() + + +def speed(sec, nanosec, *, vx=1.0, vy=0.0, wz=0.0): + return TwistStamped( + header=Header(stamp=Time(sec=sec, nanosec=nanosec)), + twist=Twist(linear=Vector3(x=vx, y=vy), angular=Vector3(z=wz)), + ) + + +def test_integrated_pose_odometry_and_tf_preserve_nanoseconds(robot, monkeypatch): + outputs = {name: [] for name in ("odom", "odometry", "tf")} + for name, values in outputs.items(): + monkeypatch.setattr(getattr(robot, name), "publish", values.append) + robot._on_chassis_speed(speed(1700000000, 123456789), None) + robot._on_chassis_speed(speed(1700000000, 223456789, vy=0.5, wz=0.2), None) + + pose, odometry, tf = [outputs[name][0] for name in outputs] + assert pose.pose.position.x == pytest.approx(0.1) + assert pose.pose.position.y == pytest.approx(0.05) + assert pose.pose.orientation.z == pytest.approx(math.sin(0.01)) + assert odometry.pose.pose == pose.pose + assert odometry.twist.twist.linear == Vector3(x=1.0, y=0.5) + assert [edge.child_frame_id for edge in tf.transforms] == [ + robot.config.frame_id, + robot.config.lidar_frame_id, + ] + assert tf.transforms[1].header.frame_id == robot.config.frame_id + assert tf.transforms[1].transform.rotation.w == 1.0 + for message in (pose, odometry, *tf.transforms): + assert message.header.stamp == Time(sec=1700000000, nanosec=223456789) + for message in (pose, odometry, tf): + assert type(message).decode(message.encode()) == message + + +@pytest.mark.parametrize("second_stamp", [(0, 0), (-1, 0), (2, 1)]) +def test_clock_jump_does_not_move_base(robot, monkeypatch, second_stamp): + values = [] + monkeypatch.setattr(robot.odom, "publish", values.append) + robot._on_chassis_speed(speed(0, 0), None) + robot._on_chassis_speed(speed(*second_stamp), None) + assert values == [] + assert robot._odom_x == 0.0 + + +@pytest.mark.parametrize("oldest", [Time(), Time(sec=1700000000, nanosec=123456788)]) +def test_joint_snapshot_uses_oldest_exact_stamp_and_all_segments(robot, monkeypatch, oldest): + for callback, count, stamp, value in [ + (robot._on_feedback_torso, 4, oldest, 1.0), + (robot._on_feedback_left, 7, Time(sec=1700000000, nanosec=123456789), 2.0), + (robot._on_feedback_right, 7, Time(sec=1700000000, nanosec=123456790), 3.0), + ]: + callback( + JointState( + header=Header(stamp=stamp), + position=[value] * count, + velocity=[0.5] * count, + effort=[0.2] * count, + ), + None, + ) + values = [] + + def collect(message): + values.append(message) + robot._stop_event.set() + + monkeypatch.setattr(robot.motor_states, "publish", collect) + robot._publish_loop() + state = values[0] + assert list(state.position) == [1.0] * 4 + [2.0] * 7 + [3.0] * 7 + assert list(state.velocity) == [0.5] * 18 + assert list(state.name) == connection.R1PRO_UPPER_BODY_JOINTS + assert state.header.stamp == oldest + assert JointState.decode(state.encode()) == state + + +@pytest.fixture() +def command_sink(robot, monkeypatch, mocker): + ros = mocker.Mock(spec=RawROS) + monkeypatch.setattr(robot, "_ros", ros) + monkeypatch.setattr(connection, "dimos_to_ros", lambda msg, _: msg) + monkeypatch.setattr(connection, "header_now", lambda: Header(stamp=Time(sec=9, nanosec=123))) + for name in ("_cmd_torso_topic", "_cmd_left_topic", "_cmd_right_topic", "_speed_topic"): + monkeypatch.setattr(robot, name, RawROSTopic(name, JointState)) + return ros.publish + + +def motor_command(): + return MotorCommandArray( + q=list(range(18)), dq=[0.0, VEL_STOP, 0.2] * 6, kp=[0.0] * 18, kd=[0.0] * 18, tau=[0.0] * 18 + ) + + +def test_motor_command_splits_generated_sequences_and_tracking_speed(robot, command_sink): + robot._on_motor_command(motor_command()) + messages = [call.args[1] for call in command_sink.call_args_list] + assert [list(msg.position) for msg in messages] == [ + list(range(4)), + list(range(4, 11)), + list(range(11, 18)), + ] + assert list(messages[0].velocity) == [0.5, 0.5, 0.2, 0.5] + assert all(msg.header.stamp == Time(sec=9, nanosec=123) for msg in messages) + + +@pytest.mark.parametrize("field", ["q", "dq", "kp", "kd", "tau"]) +def test_malformed_motor_array_never_reaches_ros(robot, command_sink, field): + message = motor_command() + setattr(message, field, [0.0]) + robot._on_motor_command(message) + command_sink.assert_not_called() + + +def test_base_command_only_forwards_supported_axes(robot, command_sink): + robot._on_cmd_vel( + Twist(linear=Vector3(x=1.0, y=2.0, z=3.0), angular=Vector3(x=4.0, y=5.0, z=6.0)) + ) + message = command_sink.call_args.args[1] + assert message == TwistStamped( + header=Header(stamp=Time(sec=9, nanosec=123)), + twist=Twist(linear=Vector3(x=1.0, y=2.0), angular=Vector3(z=6.0)), + ) diff --git a/dimos/robot/manipulators/common/coordinators.py b/dimos/robot/manipulators/common/coordinators.py index 78c5d360726..6e824d4e1c5 100644 --- a/dimos/robot/manipulators/common/coordinators.py +++ b/dimos/robot/manipulators/common/coordinators.py @@ -23,10 +23,10 @@ from __future__ import annotations +from dimos_generated.geometry_msgs.msg import PoseStamped, TwistStamped + from dimos.control.coordinator import ControlCoordinator from dimos.core.stream import In -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.geometry_msgs.TwistStamped import TwistStamped class ArmPoseCoordinator(ControlCoordinator): diff --git a/dimos/robot/manipulators/common/mock.py b/dimos/robot/manipulators/common/mock.py index 2420591c42a..9da13b93c64 100644 --- a/dimos/robot/manipulators/common/mock.py +++ b/dimos/robot/manipulators/common/mock.py @@ -16,11 +16,12 @@ from __future__ import annotations +from dimos_generated.sensor_msgs.msg import JointState + from dimos.control.components import HardwareComponent, HardwareType, make_joints from dimos.control.coordinator import ControlCoordinator from dimos.control.tasks.trajectory_task.trajectory_task import joint_trajectory_task from dimos.core.stream import Out -from dimos.msgs.sensor_msgs.JointState import JointState _mock_hw = HardwareComponent( hardware_id="arm", diff --git a/dimos/robot/manipulators/dual_openyam/test_blueprints.py b/dimos/robot/manipulators/dual_openyam/test_blueprints.py index 0633b071043..ab7129ed98a 100644 --- a/dimos/robot/manipulators/dual_openyam/test_blueprints.py +++ b/dimos/robot/manipulators/dual_openyam/test_blueprints.py @@ -14,6 +14,9 @@ from typing import Any, cast +from dimos_generated.geometry_msgs.msg import Point, Pose, PoseStamped +from dimos_generated.sensor_msgs.msg import JointState +from dimos_generated.std_msgs.msg import Float32, Header import pytest from pytest_mock import MockerFixture @@ -22,9 +25,6 @@ from dimos.core.coordination.blueprint_config.parser import BlueprintConfigParser from dimos.core.coordination.blueprints import Blueprint from dimos.hardware.whole_body.spec import WholeBodyAdapter -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.sensor_msgs.JointState import JointState -from dimos.msgs.std_msgs.Float32 import Float32 from dimos.robot.manipulators.dual_openyam.blueprints.basic import ( DualOpenYamCoordinator, ) @@ -72,8 +72,8 @@ def test_mock_webxr_coordinator_commands_both_arms_and_grippers( DualOpenYamPinkPoseTargetSolver, "frame_poses", return_value={ - "left_grasp_frame": PoseStamped(position=[0.5, 0.2, 0.4]), - "right_grasp_frame": PoseStamped(position=[0.5, -0.2, 0.4]), + "left_grasp_frame": PoseStamped(pose=Pose(position=Point(x=0.5, y=0.2, z=0.4))), + "right_grasp_frame": PoseStamped(pose=Pose(position=Point(x=0.5, y=-0.2, z=0.4))), }, ) mocker.patch.object( @@ -91,16 +91,22 @@ def test_mock_webxr_coordinator_commands_both_arms_and_grippers( buttons = Buttons() buttons.left_grip = True buttons.right_grip = True - coordinator._dispatch("teleop_buttons", buttons) + coordinator._dispatch("teleop_buttons", buttons.to_message()) coordinator._dispatch("left_gripper_command", Float32(data=0.75)) coordinator._dispatch("right_gripper_command", Float32(data=0.25)) coordinator._dispatch( "left_cartesian_command", - PoseStamped(frame_id=DUAL_OPENYAM_WEBXR_TASK_NAME, position=[1.0, 0.0, 0.0]), + PoseStamped( + header=Header(frame_id=DUAL_OPENYAM_WEBXR_TASK_NAME), + pose=Pose(position=Point(x=1.0)), + ), ) coordinator._dispatch( "right_cartesian_command", - PoseStamped(frame_id=DUAL_OPENYAM_WEBXR_TASK_NAME, position=[-1.0, 0.0, 0.0]), + PoseStamped( + header=Header(frame_id=DUAL_OPENYAM_WEBXR_TASK_NAME), + pose=Pose(position=Point(x=-1.0)), + ), ) assert coordinator._tick_loop is not None coordinator._tick_loop._tick() diff --git a/dimos/robot/manipulators/dual_openyam/test_teleop_ik.py b/dimos/robot/manipulators/dual_openyam/test_teleop_ik.py index 103f620a797..104e24945fa 100644 --- a/dimos/robot/manipulators/dual_openyam/test_teleop_ik.py +++ b/dimos/robot/manipulators/dual_openyam/test_teleop_ik.py @@ -14,12 +14,12 @@ """Objective tests for Dual OpenYAM WebXR teleoperation.""" +from dimos_generated.geometry_msgs.msg import Point, Pose, PoseStamped +from dimos_generated.sensor_msgs.msg import JointState import numpy as np import pytest from dimos.control.tasks.pose_target_ik import PoseTargetIKTaskConfig -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.sensor_msgs.JointState import JointState from dimos.robot.manipulators.dual_openyam.blueprints.teleop import ( _dual_openyam_webxr_task, ) @@ -76,9 +76,13 @@ def test_webxr_solver_matches_a1z_target_tracking_speed() -> None: initial = solver.frame_poses(state, _TARGET_FRAMES) targets = { frame_name: PoseStamped( - frame_id=pose.frame_id, - position=[pose.position.x, pose.position.y, pose.position.z + 0.1], - orientation=pose.orientation, + header=pose.header, + pose=Pose( + position=Point( + x=pose.pose.position.x, y=pose.pose.position.y, z=pose.pose.position.z + 0.1 + ), + orientation=pose.pose.orientation, + ), ) for frame_name, pose in initial.items() } @@ -91,7 +95,7 @@ def test_webxr_solver_matches_a1z_target_tracking_speed() -> None: current = solver.frame_poses(state, _TARGET_FRAMES) progress = np.mean( [ - current[frame_name].position.z - initial[frame_name].position.z + current[frame_name].pose.position.z - initial[frame_name].pose.position.z for frame_name in _TARGET_FRAMES ] ) diff --git a/dimos/robot/manipulators/openarm/test_openarm_teleop.py b/dimos/robot/manipulators/openarm/test_openarm_teleop.py index 5fd7bb203ca..c242f34773f 100644 --- a/dimos/robot/manipulators/openarm/test_openarm_teleop.py +++ b/dimos/robot/manipulators/openarm/test_openarm_teleop.py @@ -16,6 +16,9 @@ from typing import Any, cast +from dimos_generated.geometry_msgs.msg import Point, Pose, PoseStamped +from dimos_generated.sensor_msgs.msg import JointState +from dimos_generated.std_msgs.msg import Float32, Header import numpy as np import pytest from pytest_mock import MockerFixture @@ -28,9 +31,6 @@ from dimos.hardware.whole_body.spec import WholeBodyAdapter from dimos.manipulation.planning.kinematics.config import PinkKinematicsConfig from dimos.manipulation.planning.spec.config import RobotModelConfig -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.sensor_msgs.JointState import JointState -from dimos.msgs.std_msgs.Float32 import Float32 from dimos.robot.manipulators.openarm.blueprints.basic import openarm_planner_coordinator from dimos.robot.manipulators.openarm.blueprints.teleop import ( OPENARM_WEBXR_TASK_NAME, @@ -160,8 +160,12 @@ def test_openarm_webxr_commands_both_arms_and_grippers_through_coordinator( OpenArmPinkPoseTargetSolver, "frame_poses", return_value={ - "openarm_left_grasp_frame": PoseStamped(position=[0.5, 0.2, 0.4]), - "openarm_right_grasp_frame": PoseStamped(position=[0.5, -0.2, 0.4]), + "openarm_left_grasp_frame": PoseStamped( + header=Header(frame_id=""), pose=Pose(position=Point(x=0.5, y=0.2, z=0.4)) + ), + "openarm_right_grasp_frame": PoseStamped( + header=Header(frame_id=""), pose=Pose(position=Point(x=0.5, y=-0.2, z=0.4)) + ), }, ) step = mocker.patch.object( @@ -194,16 +198,22 @@ def test_openarm_webxr_commands_both_arms_and_grippers_through_coordinator( buttons.left_grip = True buttons.right_grip = True buttons.pack_analog_triggers(left=0.25, right=0.75) - coordinator._dispatch("teleop_buttons", buttons) + coordinator._dispatch("teleop_buttons", buttons.to_message()) coordinator._dispatch("left_gripper_command", Float32(data=0.75)) coordinator._dispatch("right_gripper_command", Float32(data=0.25)) coordinator._dispatch( "left_cartesian_command", - PoseStamped(frame_id=OPENARM_WEBXR_TASK_NAME, position=[1.0, 0.0, 0.0]), + PoseStamped( + header=Header(frame_id=OPENARM_WEBXR_TASK_NAME), + pose=Pose(position=Point(x=1.0, y=0.0, z=0.0)), + ), ) coordinator._dispatch( "right_cartesian_command", - PoseStamped(frame_id=OPENARM_WEBXR_TASK_NAME, position=[-1.0, 0.0, 0.0]), + PoseStamped( + header=Header(frame_id=OPENARM_WEBXR_TASK_NAME), + pose=Pose(position=Point(x=-1.0, y=0.0, z=0.0)), + ), ) assert coordinator._tick_loop is not None @@ -223,7 +233,7 @@ def test_openarm_webxr_commands_both_arms_and_grippers_through_coordinator( released = Buttons() released.right_primary = True - coordinator._dispatch("teleop_buttons", released) + coordinator._dispatch("teleop_buttons", released.to_message()) coordinator._tick_loop._tick() step.assert_called_once() finally: @@ -291,9 +301,13 @@ def test_openarm_bimanual_pink_steps_from_canonical_zero_with_bounded_updates() initial = ik.frame_poses(seed, frames) targets = { name: PoseStamped( - frame_id=pose.frame_id, - position=[pose.position.x, pose.position.y, pose.position.z + 0.01], - orientation=pose.orientation, + header=Header(frame_id=pose.header.frame_id), + pose=Pose( + position=Point( + x=pose.pose.position.x, y=pose.pose.position.y, z=pose.pose.position.z + 0.01 + ), + orientation=pose.pose.orientation, + ), ) for name, pose in initial.items() } @@ -311,12 +325,12 @@ def test_openarm_bimanual_pink_steps_from_canonical_zero_with_bounded_updates() final = ik.frame_poses(seed, frames) errors = [ np.linalg.norm( - np.array([target.position.x, target.position.y, target.position.z]) + np.array([target.pose.position.x, target.pose.position.y, target.pose.position.z]) - np.array( [ - final[name].position.x, - final[name].position.y, - final[name].position.z, + final[name].pose.position.x, + final[name].pose.position.y, + final[name].pose.position.z, ] ) ) diff --git a/dimos/robot/manipulators/openyam/test_teleop_ik.py b/dimos/robot/manipulators/openyam/test_teleop_ik.py index 945ec7d8935..fcc0cf193d3 100644 --- a/dimos/robot/manipulators/openyam/test_teleop_ik.py +++ b/dimos/robot/manipulators/openyam/test_teleop_ik.py @@ -14,13 +14,12 @@ """Objective tests for single-arm OpenYAM Quest teleoperation.""" +from dimos_generated.geometry_msgs.msg import Pose, PoseStamped, Quaternion +from dimos_generated.sensor_msgs.msg import JointState import pytest +from scipy.spatial.transform import Rotation from dimos.control.tasks.pose_target_ik import PinkPoseTargetSolver, PoseTargetIKTaskConfig -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.msgs.sensor_msgs.JointState import JointState from dimos.robot.manipulators.openyam.blueprints.teleop import teleop_webxr_openyam from dimos.robot.manipulators.openyam.config import OPENYAM_ARM_JOINTS, OPENYAM_HOME_JOINTS from dimos.robot.manipulators.openyam.teleop_ik import OpenYamPinkPoseTargetSolver @@ -70,10 +69,16 @@ def _orientation_target_motion( solver = _solver(solver_type) initial = JointState(name=OPENYAM_ARM_JOINTS, position=OPENYAM_HOME_JOINTS) pose = solver.frame_poses(initial, (_TARGET_FRAME,))[_TARGET_FRAME] + q = pose.pose.orientation + rotated = ( + Rotation.from_euler("xyz", [0.0, 0.2, 0.0]) * Rotation.from_quat([q.x, q.y, q.z, q.w]) + ).as_quat() target = PoseStamped( - frame_id=pose.frame_id, - position=pose.position, - orientation=Quaternion.from_euler(Vector3(0.0, 0.2, 0.0)) * pose.orientation, + header=pose.header, + pose=Pose( + position=pose.pose.position, + orientation=Quaternion(x=rotated[0], y=rotated[1], z=rotated[2], w=rotated[3]), + ), ) state = initial for _ in range(10): diff --git a/dimos/robot/manipulators/xarm/blueprints/grasp.py b/dimos/robot/manipulators/xarm/blueprints/grasp.py index 8e34d32a397..197ce2544fd 100644 --- a/dimos/robot/manipulators/xarm/blueprints/grasp.py +++ b/dimos/robot/manipulators/xarm/blueprints/grasp.py @@ -27,6 +27,16 @@ from __future__ import annotations +from dimos_generated.geometry_msgs.msg import ( + Pose, + PoseStamped, + Quaternion, + Transform, + TransformStamped, + Vector3, +) +from dimos_generated.std_msgs.msg import Header + from dimos.control.coordinator import TaskConfig from dimos.core.coordination.blueprints import Blueprint, autoconnect from dimos.core.global_config import global_config @@ -38,10 +48,6 @@ from dimos.manipulation.pick_and_place_module import PickAndPlaceModule from dimos.manipulation.planning.utils.point_cloud_self_filter import PointCloudSelfFilter from dimos.mapping.ray_tracing.module import RayTracingVoxelMap -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Transform import Transform -from dimos.msgs.geometry_msgs.Vector3 import Vector3 from dimos.perception.experimental.object_scene_registration import ObjectSceneRegistrationModule from dimos.robot.manipulators.common.blueprints import coordinator, trajectory_task from dimos.robot.manipulators.xarm.config import ( @@ -108,11 +114,13 @@ # resolves into world, and every cloud the camera produces is silently unusable. # link7 is a frame the model already publishes, so ManipulationModule emits the # whole chain from one loop at one rate. -XARM_WRIST_CAMERA_TRANSFORM = Transform( - translation=Vector3(x=0.06693724, y=-0.0309563, z=0.00691482), - rotation=Quaternion(0.70513398, 0.00535696, 0.70897578, -0.01052180), # xyzw - frame_id="link7", +XARM_WRIST_CAMERA_TRANSFORM = TransformStamped( + header=Header(frame_id="link7"), child_frame_id="camera_link", + transform=Transform( + translation=Vector3(x=0.06693724, y=-0.0309563, z=0.00691482), + rotation=Quaternion(x=0.70513398, y=0.00535696, z=0.70897578, w=-0.01052180), + ), ) @@ -121,7 +129,7 @@ # of the 12 cm pedestal data/xarm7 uses. Inheriting that offset would put the # planning model 12 cm above the arm MuJoCo simulates. _model = make_xarm7_sim_robot_config( - base_pose=PoseStamped(frame_id="world"), + base_pose=PoseStamped(header=Header(frame_id="world"), pose=Pose()), # The self filter needs a capture-time transform for every collision link # and drops the whole cloud when one is missing. tf_extra_links=XARM7_COLLISION_LINKS, @@ -131,7 +139,7 @@ _model = make_xarm7_model_config( add_gripper=True, gripper_hardware_id="arm", - base_pose=PoseStamped(frame_id="world"), + base_pose=PoseStamped(header=Header(frame_id="world"), pose=Pose()), # The self filter needs a capture-time transform for every collision link # and drops the whole cloud when one is missing. tf_extra_links=XARM7_COLLISION_LINKS, diff --git a/dimos/robot/manipulators/xarm/config.py b/dimos/robot/manipulators/xarm/config.py index 627c78f77ba..cba42cfb1cd 100644 --- a/dimos/robot/manipulators/xarm/config.py +++ b/dimos/robot/manipulators/xarm/config.py @@ -20,6 +20,9 @@ from pathlib import Path from typing import Any +from dimos_generated.geometry_msgs.msg import Point, Pose, PoseStamped +from dimos_generated.std_msgs.msg import Header + from dimos.control.components import ( HardwareComponent, HardwareType, @@ -28,8 +31,6 @@ from dimos.hardware.spec import JointLimits from dimos.manipulation.planning.groups.models import PlanningGroupDefinition from dimos.manipulation.planning.spec.config import RobotModelConfig -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.geometry_msgs.Vector3 import Vector3 from dimos.robot.assets.model import RobotModel from dimos.robot.assets.source import RobotDescriptionSource from dimos.robot.manipulators._modeling import joint_names @@ -66,7 +67,9 @@ # The sim scene stands the arm on a pedestal: xarm7.xml mounts link_base at # z=0.12. Place the planning model to match, or the planner solves poses 12cm # below the arm it is driving and every grasp closes on air. -XARM7_SIM_BASE_POSE = PoseStamped(frame_id="world", position=Vector3(z=0.12)) +XARM7_SIM_BASE_POSE = PoseStamped( + header=Header(frame_id="world"), pose=Pose(position=Point(z=0.12)) +) # Every link the xArm7-with-gripper URDF gives collision geometry. A point-cloud # self filter needs a capture-time transform for each one, and drops the whole # cloud if any is missing, so publish them all as TF when one is composed in. @@ -340,7 +343,9 @@ def make_xarm_model_config( package_paths=XARM_PACKAGE_PATHS, xacro_args=xacro_args, ).with_default_joint_acceleration_limit(2.0), - base_pose=base_pose if base_pose is not None else PoseStamped(), + base_pose=base_pose + if base_pose is not None + else PoseStamped(header=Header(frame_id=""), pose=Pose()), joint_names=model_joint_names, base_link=f"{prefix}link_base", planning_groups=[ diff --git a/dimos/robot/raw_robot_bridge.py b/dimos/robot/raw_robot_bridge.py index 48163cc1bd7..f7698b41b65 100644 --- a/dimos/robot/raw_robot_bridge.py +++ b/dimos/robot/raw_robot_bridge.py @@ -32,6 +32,8 @@ import time from typing import Any +from dimos_generated.geometry_msgs.msg import PoseStamped, Twist, Vector3 +from dimos_generated.sensor_msgs.msg import CameraInfo, Image, PointCloud2 import numpy as np from PIL import Image as PILImage import zenoh @@ -48,11 +50,9 @@ RAW_MAX_LINEAR_MPS, RAW_TOPIC_PREFIX, ) -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.geometry_msgs.Twist import Twist -from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo -from dimos.msgs.sensor_msgs.Image import Image -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 +from dimos.msgs.image import image_to_rgb +from dimos.msgs.pointcloud import pointcloud_xyz +from dimos.msgs.time import to_seconds def zenoh_config(endpoint: str, *, listen: bool) -> zenoh.Config: @@ -104,19 +104,28 @@ def close(self) -> None: def jpeg_bytes(image: Image, quality: int = 90) -> bytes: buf = io.BytesIO() - PILImage.fromarray(image.to_rgb().as_numpy()).save(buf, format="JPEG", quality=quality) + PILImage.fromarray(image_to_rgb(image)).save(buf, format="JPEG", quality=quality) return buf.getvalue() def xyz_f32(cloud: PointCloud2) -> bytes: - points, _ = cloud.as_numpy() + points = pointcloud_xyz(cloud) return np.ascontiguousarray(points, dtype=" str: - p, q = pose.position, pose.orientation + p, q = pose.pose.position, pose.pose.orientation return json.dumps( - {"t": pose.ts, "x": p.x, "y": p.y, "z": p.z, "qx": q.x, "qy": q.y, "qz": q.z, "qw": q.w} + { + "t": to_seconds(pose.header.stamp), + "x": p.x, + "y": p.y, + "z": p.z, + "qx": q.x, + "qy": q.y, + "qz": q.z, + "qw": q.w, + } ) @@ -190,13 +199,17 @@ def start(self) -> None: self._stop = threading.Event() self._topics = RawTopics(self.config.endpoint, self.config.prefix, listen=True) q = self.config.jpeg_quality - self.color_image.subscribe(lambda img: self._put("camera/jpeg", jpeg_bytes(img, q), img.ts)) - self.lidar.subscribe(lambda cloud: self._put("lidar/xyz_f32", xyz_f32(cloud), cloud.ts)) + self.color_image.subscribe( + lambda img: self._put("camera/jpeg", jpeg_bytes(img, q), to_seconds(img.header.stamp)) + ) + self.lidar.subscribe( + lambda cloud: self._put("lidar/xyz_f32", xyz_f32(cloud), to_seconds(cloud.header.stamp)) + ) self.odom.subscribe(lambda pose: self._put("odom/json", odom_json(pose))) self.camera_info.subscribe( lambda info: self._put( "camera_info/json", - json.dumps({"width": info.width, "height": info.height, "K": info.K}), + json.dumps({"width": info.width, "height": info.height, "K": list(info.k)}), ) ) self._cmd_sub = self._topics.subscribe("cmd_vel/json", self._on_command) @@ -227,7 +240,7 @@ def _drive(self) -> None: while not self._stop.wait(1.0 / self.config.drive_hz): vx, vy, wz = self._deadman.current() if (vx, vy, wz) != (0.0, 0.0, 0.0): - self.cmd_vel.publish(Twist(linear=(vx, vy, 0.0), angular=(0.0, 0.0, wz))) + self.cmd_vel.publish(Twist(linear=Vector3(x=vx, y=vy), angular=Vector3(z=wz))) active = True elif active: self.cmd_vel.publish(Twist()) diff --git a/dimos/robot/test_raw_robot_bridge.py b/dimos/robot/test_raw_robot_bridge.py index e407754be5f..01eef21b9b9 100644 --- a/dimos/robot/test_raw_robot_bridge.py +++ b/dimos/robot/test_raw_robot_bridge.py @@ -17,13 +17,15 @@ import json import time +from dimos_generated.geometry_msgs.msg import Point, Pose, PoseStamped, Quaternion +from dimos_generated.std_msgs.msg import Header import numpy as np from PIL import Image as PILImage import pytest -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.sensor_msgs.Image import Image -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 +from dimos.msgs.image import image_from_array +from dimos.msgs.pointcloud import pointcloud_from_xyz +from dimos.msgs.time import time_from_seconds from dimos.robot.raw_robot_bridge import Deadman, RawTopics, jpeg_bytes, odom_json, xyz_f32 ENDPOINT = "tcp/127.0.0.1:17448" @@ -58,7 +60,10 @@ def test_client_receives_sensor_topics_and_robot_receives_commands( robot.subscribe("cmd_vel/json", lambda b, _ts: commands.append(b)) time.sleep(0.5) # subscriptions propagate over the link - pose = PoseStamped(position=(1.0, 2.0, 0.5), orientation=(0.0, 0.0, 0.0, 1.0), ts=3.5) + pose = PoseStamped( + header=Header(stamp=time_from_seconds(3.5)), + pose=Pose(position=Point(x=1, y=2, z=0.5), orientation=Quaternion(w=1)), + ) robot.put("odom/json", odom_json(pose)) robot.put("camera/jpeg", b"\xff\xd8jpeg", ts=4.25) client.put("cmd_vel/json", json.dumps({"vx": 0.3, "vy": 0.0, "wz": 0.1, "t": 1.0})) @@ -73,15 +78,27 @@ def test_client_receives_sensor_topics_and_robot_receives_commands( def test_encoders_are_plain_formats() -> None: frame = np.zeros((6, 8, 3), dtype=np.uint8) frame[:, :, 0] = 200 - decoded = PILImage.open(io.BytesIO(jpeg_bytes(Image.from_numpy(frame, ts=1.0)))) + decoded = PILImage.open( + io.BytesIO( + jpeg_bytes( + image_from_array( + frame, encoding="rgb8", header=Header(stamp=time_from_seconds(1.0)) + ) + ) + ) + ) assert decoded.format == "JPEG" and decoded.size == (8, 6) points = np.arange(12, dtype=np.float32).reshape(4, 3) - raw = xyz_f32(PointCloud2.from_numpy(points, timestamp=2.0)) + raw = xyz_f32(pointcloud_from_xyz(points, header=Header(stamp=time_from_seconds(2.0)))) assert np.frombuffer(raw, dtype=" None: This is called automatically when messages arrive on cmd_vel input. """ # Extract Twist from TwistStamped - twist = Twist(linear=twist_stamped.linear, angular=twist_stamped.angular) + twist = twist_stamped.twist logger.debug( f"Received cmd_vel: linear=({twist.linear.x:.3f}, {twist.linear.y:.3f}, {twist.linear.z:.3f}), angular=({twist.angular.x:.3f}, {twist.angular.y:.3f}, {twist.angular.z:.3f})" @@ -309,10 +307,8 @@ def _publish_odom_pose(self, msg: Odometry) -> None: """ if self.odom_pose: pose_stamped = PoseStamped( - ts=msg.ts, - frame_id=msg.frame_id, - position=msg.pose.pose.position, - orientation=msg.pose.pose.orientation, + header=msg.header, + pose=msg.pose.pose, ) self.odom_pose.publish(pose_stamped) diff --git a/dimos/robot/unitree/b1/joystick_module.py b/dimos/robot/unitree/b1/joystick_module.py index 4040e818ecd..7c9e578895d 100644 --- a/dimos/robot/unitree/b1/joystick_module.py +++ b/dimos/robot/unitree/b1/joystick_module.py @@ -25,14 +25,14 @@ import time +from dimos_generated.geometry_msgs.msg import Twist, TwistStamped, Vector3 +from dimos_generated.std_msgs.msg import Header, Int32 + from dimos.constants import DEFAULT_THREAD_JOIN_TIMEOUT from dimos.core.core import rpc from dimos.core.module import Module from dimos.core.stream import Out -from dimos.msgs.geometry_msgs.Twist import Twist -from dimos.msgs.geometry_msgs.TwistStamped import TwistStamped -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.msgs.std_msgs.Int32 import Int32 +from dimos.msgs.time import time_from_seconds class JoystickModule(Module): @@ -80,10 +80,8 @@ def stop(self) -> None: # Send stop command stop_twist = Twist() stop_twist_stamped = TwistStamped( - ts=time.time(), - frame_id="base_link", - linear=stop_twist.linear, - angular=stop_twist.angular, + header=Header(stamp=time_from_seconds(time.time()), frame_id="base_link"), + twist=Twist(linear=stop_twist.linear, angular=stop_twist.angular), ) self.twist_out.publish(stop_twist_stamped) @@ -145,13 +143,13 @@ def _pygame_loop(self) -> None: self.mode_out.publish(mode_msg) # Also send zero twist stop_twist = Twist() - stop_twist.linear = Vector3(0, 0, 0) - stop_twist.angular = Vector3(0, 0, 0) + stop_twist.linear = Vector3(x=0, y=0, z=0) + stop_twist.angular = Vector3(x=0, y=0, z=0) stop_twist_stamped = TwistStamped( - ts=time.time(), - frame_id="base_link", - linear=stop_twist.linear, - angular=stop_twist.angular, + header=Header( + stamp=time_from_seconds(time.time()), frame_id="base_link" + ), + twist=Twist(linear=stop_twist.linear, angular=stop_twist.angular), ) self.twist_out.publish(stop_twist_stamped) print("EMERGENCY STOP!") @@ -164,8 +162,8 @@ def _pygame_loop(self) -> None: # Generate Twist message from held keys twist = Twist() - twist.linear = Vector3(0, 0, 0) - twist.angular = Vector3(0, 0, 0) + twist.linear = Vector3(x=0, y=0, z=0) + twist.angular = Vector3(x=0, y=0, z=0) # Apply controls based on mode if self.current_mode == 2: # WALK mode - movement control @@ -213,7 +211,8 @@ def _pygame_loop(self) -> None: twist.angular.y = -1.0 # Pitch backward twist_stamped = TwistStamped( - ts=time.time(), frame_id="base_link", linear=twist.linear, angular=twist.angular + header=Header(stamp=time_from_seconds(time.time()), frame_id="base_link"), + twist=Twist(linear=twist.linear, angular=twist.angular), ) self.twist_out.publish(twist_stamped) diff --git a/dimos/robot/unitree/b1/test_cdr_forwarding.py b/dimos/robot/unitree/b1/test_cdr_forwarding.py new file mode 100644 index 00000000000..0456fa1e8b0 --- /dev/null +++ b/dimos/robot/unitree/b1/test_cdr_forwarding.py @@ -0,0 +1,87 @@ +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from types import SimpleNamespace +from unittest.mock import Mock + +from dimos_generated.builtin_interfaces.msg import Time +from dimos_generated.geometry_msgs.msg import ( + Point, + Pose, + PoseStamped, + PoseWithCovariance, + Quaternion, + Twist, + TwistStamped, + Vector3, +) +from dimos_generated.nav_msgs.msg import Odometry +from dimos_generated.std_msgs.msg import Header, Int32 +import pytest + +from dimos.robot.unitree.b1.connection import B1ConnectionModule, MockB1ConnectionModule, RobotMode +from dimos.robot.unitree.b1.joystick_module import JoystickModule + + +def test_b1_odometry_forwarding_preserves_generated_pose_and_exact_header(): + source = Odometry( + header=Header(frame_id="map", stamp=Time(sec=1700000000, nanosec=123456789)), + pose=PoseWithCovariance( + pose=Pose(position=Point(x=1, y=2, z=3), orientation=Quaternion(w=1)) + ), + ) + received = [] + proxy = SimpleNamespace(odom_pose=SimpleNamespace(publish=received.append)) + B1ConnectionModule._publish_odom_pose(proxy, source) + result = PoseStamped.decode(received[0].encode()) + assert result.header == source.header + assert result.pose == source.pose.pose + + +@pytest.mark.parametrize("mode", [RobotMode.WALK, RobotMode.STAND]) +def test_b1_generated_command_mapping_preserves_mode_clamps_and_source(mode): + module = MockB1ConnectionModule() + try: + module.handle_mode(Int32.decode(Int32(data=mode).encode())) + message = TwistStamped( + header=Header(frame_id="base_link", stamp=Time(sec=1700000000, nanosec=123456789)), + twist=Twist(linear=Vector3(x=2, y=-2, z=2), angular=Vector3(x=2, y=-2, z=4)), + ) + before = message.encode() + module.handle_twist_stamped(TwistStamped.decode(before)) + command = module._current_cmd + assert module.current_mode == mode and command.mode == mode + assert command.lx == -1 and command.ly == 1 + assert command.rx == (1 if mode == RobotMode.WALK else -1) + assert command.ry == (0 if mode == RobotMode.WALK else -1) + assert message.encode() == before + assert module.socket is None + finally: + module.stop() + + +def test_b1_joystick_stop_publishes_generated_zero_without_starting_loop(monkeypatch): + module = JoystickModule() + thread = Mock() + module._thread = thread + received = [] + monkeypatch.setattr(module.twist_out, "publish", received.append) + monkeypatch.setattr("dimos.robot.unitree.b1.joystick_module.time.time", lambda: 1700000000.25) + module.stop() + assert len(received) == 1 + value = TwistStamped.decode(received[0].encode()) + assert value.header.frame_id == "base_link" + assert value.header.stamp == Time(sec=1700000000, nanosec=250000000) + assert value.twist == Twist() + thread.join.assert_called_once() diff --git a/dimos/robot/unitree/b1/test_connection.py b/dimos/robot/unitree/b1/test_connection.py index 9c7a2867fa2..ef12f2c37ce 100644 --- a/dimos/robot/unitree/b1/test_connection.py +++ b/dimos/robot/unitree/b1/test_connection.py @@ -26,11 +26,13 @@ import threading import time +from dimos_generated.geometry_msgs.msg import Twist, TwistStamped, Vector3 +from dimos_generated.std_msgs.msg import Header, Int32 + +from dimos.msgs.time import time_from_seconds + _IS_MACOS = sys.platform == "darwin" -from dimos.msgs.geometry_msgs.TwistStamped import TwistStamped -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.msgs.std_msgs.Int32 import Int32 from .connection import MockB1ConnectionModule @@ -50,10 +52,8 @@ def test_watchdog_actually_zeros_commands(self) -> None: # Send a forward command twist_stamped = TwistStamped( - ts=time.time(), - frame_id="base_link", - linear=Vector3(1.0, 0, 0), - angular=Vector3(0, 0, 0), + header=Header(stamp=time_from_seconds(time.time()), frame_id="base_link"), + twist=Twist(linear=Vector3(x=1.0, y=0, z=0), angular=Vector3(x=0, y=0, z=0)), ) conn.handle_twist_stamped(twist_stamped) @@ -98,10 +98,8 @@ def test_watchdog_resets_on_new_command(self) -> None: # Send commands in rapid succession — each resets the 200ms watchdog for val in [1.0, 0.8, 0.6, 0.5]: twist = TwistStamped( - ts=time.time(), - frame_id="base_link", - linear=Vector3(val, 0, 0), - angular=Vector3(0, 0, 0), + header=Header(stamp=time_from_seconds(time.time()), frame_id="base_link"), + twist=Twist(linear=Vector3(x=val, y=0, z=0), angular=Vector3(x=0, y=0, z=0)), ) conn.handle_twist_stamped(twist) time.sleep(0.02) # 20ms between commands, well under timeout @@ -132,10 +130,8 @@ def test_watchdog_thread_efficiency(self) -> None: # Send many commands rapidly (would create many Timer threads in old implementation) for i in range(50): twist = TwistStamped( - ts=time.time(), - frame_id="base_link", - linear=Vector3(i * 0.01, 0, 0), - angular=Vector3(0, 0, 0), + header=Header(stamp=time_from_seconds(time.time()), frame_id="base_link"), + twist=Twist(linear=Vector3(x=i * 0.01, y=0, z=0), angular=Vector3(x=0, y=0, z=0)), ) conn.handle_twist_stamped(twist) time.sleep(0.01) # 100Hz command rate @@ -175,10 +171,8 @@ def blocking_send_loop() -> None: try: # Send command twist = TwistStamped( - ts=time.time(), - frame_id="base_link", - linear=Vector3(1.0, 0, 0), - angular=Vector3(0, 0, 0), + header=Header(stamp=time_from_seconds(time.time()), frame_id="base_link"), + twist=Twist(linear=Vector3(x=1.0, y=0, z=0), angular=Vector3(x=0, y=0, z=0)), ) conn.handle_twist_stamped(twist) assert conn._current_cmd.ly == 1.0 @@ -218,10 +212,8 @@ def test_continuous_commands_prevent_timeout(self) -> None: commands_sent = 0 while time.time() - start < 1.0: twist = TwistStamped( - ts=time.time(), - frame_id="base_link", - linear=Vector3(0.5, 0, 0), - angular=Vector3(0, 0, 0), + header=Header(stamp=time_from_seconds(time.time()), frame_id="base_link"), + twist=Twist(linear=Vector3(x=0.5, y=0, z=0), angular=Vector3(x=0, y=0, z=0)), ) conn.handle_twist_stamped(twist) commands_sent += 1 @@ -253,10 +245,8 @@ def test_watchdog_timing_accuracy(self) -> None: # Send command and record time start_time = time.time() twist = TwistStamped( - ts=time.time(), - frame_id="base_link", - linear=Vector3(1.0, 0, 0), - angular=Vector3(0, 0, 0), + header=Header(stamp=time_from_seconds(time.time()), frame_id="base_link"), + twist=Twist(linear=Vector3(x=1.0, y=0, z=0), angular=Vector3(x=0, y=0, z=0)), ) conn.handle_twist_stamped(twist) @@ -297,10 +287,8 @@ def test_mode_changes_with_watchdog(self) -> None: # Send walk command twist = TwistStamped( - ts=time.time(), - frame_id="base_link", - linear=Vector3(1.0, 0, 0), - angular=Vector3(0, 0, 0), + header=Header(stamp=time_from_seconds(time.time()), frame_id="base_link"), + twist=Twist(linear=Vector3(x=1.0, y=0, z=0), angular=Vector3(x=0, y=0, z=0)), ) conn.handle_twist_stamped(twist) assert conn.current_mode == 2 @@ -338,10 +326,8 @@ def test_watchdog_stops_movement_when_commands_stop(self) -> None: # Simulate sending movement commands for a while for _i in range(5): twist = TwistStamped( - ts=time.time(), - frame_id="base_link", - linear=Vector3(1.0, 0, 0), - angular=Vector3(0, 0, 0.5), # Forward and turning + header=Header(stamp=time_from_seconds(time.time()), frame_id="base_link"), + twist=Twist(linear=Vector3(x=1.0, y=0, z=0), angular=Vector3(x=0, y=0, z=0.5)), ) conn.handle_twist_stamped(twist) time.sleep(0.05) # Send at 20Hz @@ -369,10 +355,8 @@ def test_watchdog_stops_movement_when_commands_stop(self) -> None: # Verify recovery works - send new command twist = TwistStamped( - ts=time.time(), - frame_id="base_link", - linear=Vector3(0.5, 0, 0), - angular=Vector3(0, 0, 0), + header=Header(stamp=time_from_seconds(time.time()), frame_id="base_link"), + twist=Twist(linear=Vector3(x=0.5, y=0, z=0), angular=Vector3(x=0, y=0, z=0)), ) conn.handle_twist_stamped(twist) @@ -409,10 +393,10 @@ def test_rapid_command_thread_safety(self) -> None: def send_commands(thread_id) -> None: for _i in range(10): twist = TwistStamped( - ts=time.time(), - frame_id="base_link", - linear=Vector3(thread_id * 0.1, 0, 0), - angular=Vector3(0, 0, 0), + header=Header(stamp=time_from_seconds(time.time()), frame_id="base_link"), + twist=Twist( + linear=Vector3(x=thread_id * 0.1, y=0, z=0), angular=Vector3(x=0, y=0, z=0) + ), ) conn.handle_twist_stamped(twist) time.sleep(0.01) diff --git a/dimos/robot/unitree/b1/unitree_b1.py b/dimos/robot/unitree/b1/unitree_b1.py index 46924f3edc0..d05c5ae3d6e 100644 --- a/dimos/robot/unitree/b1/unitree_b1.py +++ b/dimos/robot/unitree/b1/unitree_b1.py @@ -23,15 +23,15 @@ import logging import os +from dimos_generated.geometry_msgs.msg import PoseStamped, TwistStamped +from dimos_generated.nav_msgs.msg import Odometry +from dimos_generated.std_msgs.msg import Int32 +from dimos_generated.tf2_msgs.msg import TFMessage + from dimos.core.coordination.module_coordinator import ModuleCoordinator from dimos.core.resource import Resource from dimos.core.transport import ROSTransport from dimos.core.transport_factory import make_transport -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.geometry_msgs.TwistStamped import TwistStamped -from dimos.msgs.nav_msgs.Odometry import Odometry -from dimos.msgs.std_msgs.Int32 import Int32 -from dimos.msgs.tf2_msgs.TFMessage import TFMessage from dimos.robot.robot import Robot from dimos.robot.unitree.b1.connection import ( B1ConnectionModule, diff --git a/dimos/robot/unitree/connection.py b/dimos/robot/unitree/connection.py index 761dff99475..97f05488832 100644 --- a/dimos/robot/unitree/connection.py +++ b/dimos/robot/unitree/connection.py @@ -14,12 +14,13 @@ import asyncio from dataclasses import dataclass -import functools import json import threading import time -from typing import Any, TypeAlias, TypeVar +from typing import Any, TypeAlias +from dimos_generated.geometry_msgs.msg import PoseStamped, TransformStamped, Twist +from dimos_generated.sensor_msgs.msg import Image, PointCloud2 import numpy as np from numpy.typing import NDArray from reactivex import operators as ops @@ -38,18 +39,15 @@ from dimos.constants import DEFAULT_THREAD_JOIN_TIMEOUT from dimos.core.resource import Resource -from dimos.msgs.geometry_msgs.Pose import Pose -from dimos.msgs.geometry_msgs.Transform import Transform -from dimos.msgs.geometry_msgs.Twist import Twist -from dimos.msgs.sensor_msgs.Image import Image, ImageFormat -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 +from dimos.msgs.geometry import transform_from_pose +from dimos.msgs.image import image_from_array +from dimos.msgs.time import header_now, time_from_nanoseconds from dimos.robot.unitree.type.lidar import ( RawLidarMsg, pointcloud2_from_webrtc_lidar, ) from dimos.robot.unitree.type.lowstate import LowStateMsg -from dimos.robot.unitree.type.odometry import Odometry -from dimos.types.timestamped import Timestamped +from dimos.robot.unitree.type.odometry import RawOdometryMessage, pose_from_webrtc_odometry from dimos.utils.decorators.decorators import simple_mcache from dimos.utils.logging_config import setup_logger from dimos.utils.reactive import backpressure, callback_to_observable @@ -60,14 +58,6 @@ logger = setup_logger() -_T = TypeVar("_T", bound=Timestamped) - - -def time_is_now(x: _T) -> _T: - x.ts = time.time() - return x - - @dataclass class SerializableVideoFrame: """Pickleable wrapper for av.VideoFrame with all metadata""" @@ -283,34 +273,29 @@ def raw_lidar_stream(self) -> Observable[RawLidarMsg]: return backpressure(self.unitree_sub_stream(RTC_TOPIC["ULIDAR_ARRAY"])) @simple_mcache - def raw_odom_stream(self) -> Observable[Pose]: + def raw_odom_stream(self) -> Observable[RawOdometryMessage]: return backpressure(self.unitree_sub_stream(RTC_TOPIC["ROBOTODOM"])) @simple_mcache def lidar_stream(self) -> Observable[PointCloud2]: - return backpressure( - self.raw_lidar_stream().pipe( - ops.map(pointcloud2_from_webrtc_lidar), - ops.map(time_is_now), - # repair_stale_ts(), - ) - ) + def convert(raw: RawLidarMsg) -> PointCloud2: + return pointcloud2_from_webrtc_lidar(raw, stamp=time_from_nanoseconds(time.time_ns())) + + return backpressure(self.raw_lidar_stream().pipe(ops.map(convert))) @simple_mcache - def tf_stream(self) -> Observable[Transform]: - base_link = functools.partial(Transform.from_pose, "base_link") - return backpressure(self.odom_stream().pipe(ops.map(base_link))) + def tf_stream(self) -> Observable[TransformStamped]: + def convert(pose: PoseStamped) -> TransformStamped: + return transform_from_pose(pose, child_frame_id="base_link") + + return backpressure(self.odom_stream().pipe(ops.map(convert))) @simple_mcache - def odom_stream(self) -> Observable[Pose]: - return backpressure( - self.raw_odom_stream().pipe( - ops.map( - Odometry.from_msg, - ), - ops.map(time_is_now), - ) - ) + def odom_stream(self) -> Observable[PoseStamped]: + def convert(raw: RawOdometryMessage) -> PoseStamped: + return pose_from_webrtc_odometry(raw, header=header_now("world")) + + return backpressure(self.raw_odom_stream().pipe(ops.map(convert))) @simple_mcache def video_stream(self) -> Observable[Image]: @@ -318,14 +303,13 @@ def video_stream(self) -> Observable[Image]: self.raw_video_stream().pipe( ops.filter(lambda frame: frame is not None), ops.map( - lambda frame: Image.from_numpy( + lambda frame: image_from_array( # np.ascontiguousarray(frame.to_ndarray("rgb24")), frame.to_ndarray(format="rgb24"), # type: ignore[attr-defined] - format=ImageFormat.RGB, # Frame is RGB24, not BGR - frame_id="camera_optical", + encoding="rgb8", + header=header_now("camera_optical"), ), ), - ops.map(time_is_now), ) ) diff --git a/dimos/robot/unitree/dimsim_connection.py b/dimos/robot/unitree/dimsim_connection.py index ffa52d2196a..d7bd90ac92c 100644 --- a/dimos/robot/unitree/dimsim_connection.py +++ b/dimos/robot/unitree/dimsim_connection.py @@ -16,20 +16,24 @@ import functools from typing import Any +from dimos_generated.geometry_msgs.msg import ( + PoseStamped, + Quaternion, + Transform, + TransformStamped, + Twist, + Vector3, +) +from dimos_generated.sensor_msgs.msg import CameraInfo, Image, PointCloud2 +from dimos_generated.std_msgs.msg import Header +from dimos_generated.tf2_msgs.msg import TFMessage from reactivex import Observable, Subject from dimos.core.global_config import GlobalConfig from dimos.core.transport import PubSubTransport from dimos.core.transport_factory import make_transport -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Transform import Transform -from dimos.msgs.geometry_msgs.Twist import Twist -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo -from dimos.msgs.sensor_msgs.Image import Image -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 -from dimos.msgs.tf2_msgs.TFMessage import TFMessage +from dimos.msgs.camera_info import camera_info_from_fov +from dimos.msgs.geometry import transform_from_pose from dimos.simulation.dimsim.dimsim_process import DimSimProcess from dimos.utils.logging_config import setup_logger @@ -41,12 +45,12 @@ class DimSimConnection: - camera_info_static: CameraInfo = CameraInfo.from_fov( - fov_deg=_FOV_DEG, + camera_info_static: CameraInfo = camera_info_from_fov( + fov_degrees=_FOV_DEG, width=_WIDTH, height=_HEIGHT, axis="horizontal", - frame_id="camera_optical", + header=Header(frame_id="camera_optical"), ) def __init__(self, global_config: GlobalConfig) -> None: @@ -117,41 +121,25 @@ def publish_request(self, topic: str, data: dict[str, Any]) -> dict[Any, Any]: return {} def _handle_odom(self, msg: PoseStamped) -> None: - self._tf_transport.publish(TFMessage(*_odom_to_tf(msg))) + self._tf_transport.publish(TFMessage(transforms=_odom_to_tf(msg))) -def _odom_to_tf(odom: PoseStamped) -> list[Transform]: - """Build transform chain from odometry pose. - - Transform tree: world -> base_link -> {camera_link -> camera_optical, lidar_link} - """ - camera_link = Transform( - translation=Vector3(0.3, 0.0, 0.0), # camera 30cm forward - rotation=Quaternion(0.0, 0.0, 0.0, 1.0), - frame_id="base_link", - child_frame_id="camera_link", - ts=odom.ts, - ) - - camera_optical = Transform( - translation=Vector3(0.0, 0.0, 0.0), - rotation=Quaternion(-0.5, 0.5, -0.5, 0.5), - frame_id="camera_link", - child_frame_id="camera_optical", - ts=odom.ts, - ) - - lidar_link = Transform( - translation=Vector3(0.0, 0.0, 0.0), - rotation=Quaternion(0.0, 0.0, 0.0, 1.0), - frame_id="base_link", - child_frame_id="lidar_link", - ts=odom.ts, - ) - +def _odom_to_tf(odom: PoseStamped) -> list[TransformStamped]: + """Build world → base_link → camera/lidar transforms at the source pose stamp.""" return [ - Transform.from_pose("base_link", odom), - camera_link, - camera_optical, - lidar_link, + transform_from_pose(odom, child_frame_id="base_link"), + TransformStamped( + header=Header(stamp=odom.header.stamp, frame_id="base_link"), + child_frame_id="camera_link", + transform=Transform(translation=Vector3(x=0.3)), + ), + TransformStamped( + header=Header(stamp=odom.header.stamp, frame_id="camera_link"), + child_frame_id="camera_optical", + transform=Transform(rotation=Quaternion(x=-0.5, y=0.5, z=-0.5, w=0.5)), + ), + TransformStamped( + header=Header(stamp=odom.header.stamp, frame_id="base_link"), + child_frame_id="lidar_link", + ), ] diff --git a/dimos/robot/unitree/g1/blueprints/basic/unitree_g1_coordinator.py b/dimos/robot/unitree/g1/blueprints/basic/unitree_g1_coordinator.py index 35408e812ba..5d8e5b556a0 100644 --- a/dimos/robot/unitree/g1/blueprints/basic/unitree_g1_coordinator.py +++ b/dimos/robot/unitree/g1/blueprints/basic/unitree_g1_coordinator.py @@ -22,15 +22,15 @@ import os +from dimos_generated.dimos_msgs.msg import MotorCommandArray +from dimos_generated.sensor_msgs.msg import Imu, JointState + from dimos.control.components import HardwareComponent, HardwareType, make_humanoid_joints from dimos.control.coordinator import ControlCoordinator from dimos.control.tasks.trajectory_task.trajectory_task import joint_trajectory_task from dimos.core.coordination.blueprints import autoconnect from dimos.core.stream import Out from dimos.core.transport import LCMTransport -from dimos.msgs.sensor_msgs.Imu import Imu -from dimos.msgs.sensor_msgs.JointState import JointState -from dimos.msgs.sensor_msgs.MotorCommandArray import MotorCommandArray from dimos.robot.unitree.g1.wholebody_connection import G1WholeBodyConnection _g1_joints = make_humanoid_joints("g1") diff --git a/dimos/robot/unitree/g1/blueprints/basic/unitree_g1_groot_wbc.py b/dimos/robot/unitree/g1/blueprints/basic/unitree_g1_groot_wbc.py index 236498e6c88..c8ee29b62a0 100644 --- a/dimos/robot/unitree/g1/blueprints/basic/unitree_g1_groot_wbc.py +++ b/dimos/robot/unitree/g1/blueprints/basic/unitree_g1_groot_wbc.py @@ -49,6 +49,11 @@ from pathlib import Path from typing import Any, cast +from dimos_generated.dimos_msgs.msg import MotorCommandArray +from dimos_generated.geometry_msgs.msg import Twist +from dimos_generated.nav_msgs.msg import Odometry, Path as NavPath +from dimos_generated.sensor_msgs.msg import Imu, JointState + from dimos.control.components import HardwareComponent, HardwareType from dimos.control.coordinator import TaskConfig from dimos.control.tasks.g1_groot_wbc_task.g1_groot_wbc_task import ( @@ -69,11 +74,7 @@ from dimos.manipulation.planning.spec.config import RobotModelConfig from dimos.mapping.costmapper import CostMapper from dimos.mapping.pointclouds.occupancy import HeightCostConfig -from dimos.msgs.geometry_msgs.Twist import Twist -from dimos.msgs.nav_msgs.Path import Path as NavPath -from dimos.msgs.sensor_msgs.Imu import Imu -from dimos.msgs.sensor_msgs.JointState import JointState -from dimos.msgs.sensor_msgs.MotorCommandArray import MotorCommandArray +from dimos.msgs.geometry import pose_matrix, quaternion_from_matrix from dimos.navigation.movement_manager.movement_manager import MovementManager from dimos.navigation.replanning_a_star.module import ReplanningAStarPlanner from dimos.robot.unitree.g1.config import G1 @@ -87,6 +88,7 @@ from dimos.robot.unitree.g1.teleop_ik import G1PinkPoseTargetSolver from dimos.simulation.scene_assets.spec import ScenePackage from dimos.utils.data import LfsPath +from dimos.visualization.rerun.message_helpers import navigation_archetype from dimos.visualization.rerun.scene_package import scene_package_static_entities from dimos.visualization.vis_module import vis_module @@ -375,7 +377,7 @@ def _g1_groot_rerun_blueprint() -> Any: def _g1_nav_path(path: NavPath) -> Any: - return path.to_rerun(z_offset=0.3) + return navigation_archetype(path, z_offset=0.3) # Mesh root: sim roots under the /odom transform; real hw under the LIO's @@ -415,20 +417,17 @@ def _g1_pelvis_to_mid360() -> Any: return _g1_pelvis_mid360_cache[0] -def _g1_real_odometry_root(odom: Any) -> Any: +def _g1_real_odometry_root(odom: Odometry) -> Any: """Robot-mesh root: pelvis pose from the LIO's mid360 odometry (rest offset).""" import numpy as np import rerun as rr - from dimos.msgs.geometry_msgs.Quaternion import Quaternion - - t_world_mid360 = np.eye(4) + t_world_mid360 = pose_matrix(odom.pose.pose) # The MID-360 is mounted upside down (the URDF doesn't carry the flip): # un-roll by Rx(pi) == diag(1, -1, -1). - t_world_mid360[:3, :3] = odom.orientation.to_rotation_matrix() @ np.diag([1.0, -1.0, -1.0]) - t_world_mid360[:3, 3] = (odom.x, odom.y, odom.z) + t_world_mid360[:3, :3] = t_world_mid360[:3, :3] @ np.diag([1.0, -1.0, -1.0]) t_world_pelvis = t_world_mid360 @ np.linalg.inv(_g1_pelvis_to_mid360()) - q = Quaternion.from_rotation_matrix(t_world_pelvis[:3, :3]) + q = quaternion_from_matrix(t_world_pelvis[:3, :3]) return rr.Transform3D( translation=t_world_pelvis[:3, 3].tolist(), rotation=rr.Quaternion(xyzw=[q.x, q.y, q.z, q.w]), diff --git a/dimos/robot/unitree/g1/blueprints/basic/unitree_g1_record.py b/dimos/robot/unitree/g1/blueprints/basic/unitree_g1_record.py index ef077ed3f39..56dc69f6029 100644 --- a/dimos/robot/unitree/g1/blueprints/basic/unitree_g1_record.py +++ b/dimos/robot/unitree/g1/blueprints/basic/unitree_g1_record.py @@ -22,17 +22,19 @@ from typing import Any +from dimos_generated.sensor_msgs.msg import CameraInfo + from dimos.core.coordination.blueprints import autoconnect from dimos.core.coordination.module_coordinator import ModuleCoordinator from dimos.core.global_config import global_config from dimos.hardware.sensors.camera.realsense.camera import RealSenseCamera from dimos.hardware.sensors.lidar.pointlio.module import PointLio from dimos.memory.module import default_recording_dir -from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo from dimos.navigation.movement_manager.movement_manager import MovementManager from dimos.robot.unitree.g1.effectors.high_level.dds_sdk import G1HighLevelDdsSdk from dimos.robot.unitree.g1.g1_recorder import G1Recorder from dimos.robot.unitree.g1.g1_tf_publisher import G1TfPublisher +from dimos.visualization.rerun.message_helpers import camera_pinhole from dimos.visualization.vis_module import vis_module _RECORDING_DIR = default_recording_dir() @@ -70,18 +72,22 @@ def _static_robot_body(rr: Any) -> list[Any]: def _convert_camera_info(camera_info: CameraInfo) -> Any: - return camera_info.to_rerun( - image_topic="/world/color_image", - optical_frame="d435_color_optical_frame", - ) + return [ + ( + "/world/color_image", + camera_pinhole(camera_info, optical_frame="d435_color_optical_frame"), + ) + ] def _convert_depth_camera_info(camera_info: CameraInfo) -> Any: # Depth is aligned to color, so it shares the color optical frame. - return camera_info.to_rerun( - image_topic="/world/realsense_depth_image", - optical_frame="d435_color_optical_frame", - ) + return [ + ( + "/world/realsense_depth_image", + camera_pinhole(camera_info, optical_frame="d435_color_optical_frame"), + ) + ] _record_vis = vis_module( diff --git a/dimos/robot/unitree/g1/blueprints/basic/unitree_g1_teleop.py b/dimos/robot/unitree/g1/blueprints/basic/unitree_g1_teleop.py index 012b8547389..87baa4161a7 100644 --- a/dimos/robot/unitree/g1/blueprints/basic/unitree_g1_teleop.py +++ b/dimos/robot/unitree/g1/blueprints/basic/unitree_g1_teleop.py @@ -49,6 +49,9 @@ from datetime import datetime +from dimos_generated.geometry_msgs.msg import PoseStamped +from dimos_generated.sensor_msgs.msg import Image + from dimos.constants import DEFAULT_CAPACITY_COLOR_IMAGE, STATE_DIR from dimos.core.coordination.blueprints import Blueprint, autoconnect from dimos.core.global_config import global_config @@ -58,8 +61,6 @@ from dimos.imitation.collection.recorder import CollectionRecorder from dimos.manipulation.manipulation_module import ManipulationModule from dimos.manipulation.visualization.viser.config import ViserVisualizationConfig -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.sensor_msgs.Image import Image from dimos.robot.unitree.g1.blueprints.basic.unitree_g1_groot_wbc import ( _G1GrootCoordinator, _unitree_g1_groot_wbc_core, diff --git a/dimos/robot/unitree/g1/blueprints/perceptive/unitree_g1_detection.py b/dimos/robot/unitree/g1/blueprints/perceptive/unitree_g1_detection.py index ec72cc10b23..ec472fc7b26 100644 --- a/dimos/robot/unitree/g1/blueprints/perceptive/unitree_g1_detection.py +++ b/dimos/robot/unitree/g1/blueprints/perceptive/unitree_g1_detection.py @@ -17,13 +17,13 @@ from typing import Any +from dimos_generated.geometry_msgs.msg import PoseStamped +from dimos_generated.sensor_msgs.msg import Image, PointCloud2 +from dimos_generated.vision_msgs.msg import Detection2DArray + from dimos.core.coordination.blueprints import autoconnect from dimos.core.transport import LCMTransport from dimos.hardware.sensors.camera.zed import compat as zed -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.sensor_msgs.Image import Image -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 -from dimos.msgs.vision_msgs.Detection2DArray import Detection2DArray from dimos.perception.detection.detectors.person.yolo import YoloPersonDetector from dimos.perception.detection.module3D import Detection3DModule from dimos.perception.detection.person_tracker import PersonTracker diff --git a/dimos/robot/unitree/g1/blueprints/perceptive/unitree_g1_shm.py b/dimos/robot/unitree/g1/blueprints/perceptive/unitree_g1_shm.py index 37727eaf9b2..a24be2c596c 100644 --- a/dimos/robot/unitree/g1/blueprints/perceptive/unitree_g1_shm.py +++ b/dimos/robot/unitree/g1/blueprints/perceptive/unitree_g1_shm.py @@ -15,11 +15,12 @@ """G1 stack with shared memory image transport.""" +from dimos_generated.sensor_msgs.msg import Image + from dimos.constants import DEFAULT_CAPACITY_COLOR_IMAGE from dimos.core.coordination.blueprints import autoconnect from dimos.core.global_config import global_config from dimos.core.transport import pSHMTransport -from dimos.msgs.sensor_msgs.Image import Image from dimos.robot.unitree.g1.blueprints.perceptive.unitree_g1 import unitree_g1 from dimos.visualization.vis_module import vis_module diff --git a/dimos/robot/unitree/g1/blueprints/primitive/test_unitree_g1_vis.py b/dimos/robot/unitree/g1/blueprints/primitive/test_unitree_g1_vis.py new file mode 100644 index 00000000000..8386659f8a6 --- /dev/null +++ b/dimos/robot/unitree/g1/blueprints/primitive/test_unitree_g1_vis.py @@ -0,0 +1,43 @@ +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from dimos_generated.geometry_msgs.msg import Point, PointStamped +from dimos_generated.std_msgs.msg import Header +import numpy as np +import pytest + +from dimos.msgs.pointcloud import pointcloud_from_xyz +from dimos.robot.unitree.g1.blueprints.primitive.unitree_g1_vis import ( + _global_map_colors, + _goal_colors, + _waypoint_colors, +) + + +def test_global_map_uses_generated_xyz_and_height_colors(): + cloud = pointcloud_from_xyz(np.array([[1.0, 2.0, 0.0], [3.0, 4.0, 2.0]]), header=Header()) + result = _global_map_colors(type(cloud).decode(cloud.encode())) + np.testing.assert_array_equal( + result.positions.as_arrow_array().to_pylist(), [[1, 2, 0], [3, 4, 2]] + ) + assert result.colors.as_arrow_array().to_pylist() == [0x1E50C8FF, 0x3BDB64FF] + + +@pytest.mark.parametrize("render", [_goal_colors, _waypoint_colors]) +def test_generated_stamped_targets_keep_position_and_reject_nonfinite(render): + target = PointStamped(header=Header(frame_id="world"), point=Point(x=1, y=2, z=3)) + result = render(PointStamped.decode(target.encode())) + np.testing.assert_allclose(result.positions.as_arrow_array().to_pylist(), [[1, 2, 3.3]]) + target.point.x = float("nan") + assert render(target) is None diff --git a/dimos/robot/unitree/g1/blueprints/primitive/unitree_g1_primitive_no_nav.py b/dimos/robot/unitree/g1/blueprints/primitive/unitree_g1_primitive_no_nav.py index 8fc4b906339..c0ea5c42dfb 100644 --- a/dimos/robot/unitree/g1/blueprints/primitive/unitree_g1_primitive_no_nav.py +++ b/dimos/robot/unitree/g1/blueprints/primitive/unitree_g1_primitive_no_nav.py @@ -17,7 +17,16 @@ from typing import Any -from dimos_lcm.sensor_msgs import CameraInfo +from dimos_generated.geometry_msgs.msg import ( + PoseStamped, + Transform, + TransformStamped, + Twist, + Vector3, +) +from dimos_generated.nav_msgs.msg import Odometry, Path +from dimos_generated.sensor_msgs.msg import CameraInfo, Image, PointCloud2 +from dimos_generated.std_msgs.msg import Bool, Header from dimos.core.coordination.blueprints import autoconnect from dimos.core.global_config import global_config @@ -27,28 +36,17 @@ from dimos.hardware.sensors.camera.zed import compat as zed from dimos.mapping.costmapper import CostMapper from dimos.mapping.voxels.module import VoxelGridMapper -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Transform import Transform -from dimos.msgs.geometry_msgs.Twist import Twist -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.msgs.nav_msgs.Odometry import Odometry -from dimos.msgs.nav_msgs.Path import Path -from dimos.msgs.sensor_msgs.Image import Image -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 -from dimos.msgs.std_msgs.Bool import Bool +from dimos.msgs.geometry import quaternion_from_euler from dimos.navigation.frontier_exploration.wavefront_frontier_goal_selector import ( WavefrontFrontierExplorer, ) from dimos.robot.unitree.g1.g1_rerun import g1_costmap +from dimos.visualization.rerun.message_helpers import camera_pinhole from dimos.visualization.vis_module import vis_module -def _convert_camera_info(camera_info: Any) -> Any: - return camera_info.to_rerun( - image_topic="/world/color_image", - optical_frame="camera_optical", - ) +def _convert_camera_info(camera_info: CameraInfo) -> Any: + return camera_pinhole(camera_info) def _static_base_link(rr: Any) -> list[Any]: @@ -109,11 +107,13 @@ def _create_webcam() -> Webcam: _camera = ( autoconnect( CameraModule.blueprint( - transform=Transform( - translation=Vector3(0.05, 0.0, 0.6), # height of camera on G1 robot - rotation=Quaternion.from_euler(Vector3(0.0, 0.2, 0.0)), - frame_id="sensor", + transform=TransformStamped( + header=Header(frame_id="sensor"), child_frame_id="camera_link", + transform=Transform( + translation=Vector3(x=0.05, z=0.6), + rotation=quaternion_from_euler(0.0, 0.2, 0.0), + ), ), hardware=_create_webcam, ), diff --git a/dimos/robot/unitree/g1/blueprints/primitive/unitree_g1_vis.py b/dimos/robot/unitree/g1/blueprints/primitive/unitree_g1_vis.py index 8c352e5017c..236e03dd560 100644 --- a/dimos/robot/unitree/g1/blueprints/primitive/unitree_g1_vis.py +++ b/dimos/robot/unitree/g1/blueprints/primitive/unitree_g1_vis.py @@ -18,10 +18,13 @@ import math from typing import Any +from dimos_generated.geometry_msgs.msg import PointStamped +from dimos_generated.nav_msgs.msg import Path +from dimos_generated.sensor_msgs.msg import PointCloud2 import numpy as np from dimos.core.global_config import global_config -from dimos.msgs.nav_msgs.Path import Path +from dimos.msgs.pointcloud import pointcloud_xyz from dimos.robot.unitree.g1.g1_rerun import g1_costmap, g1_odometry_tf_override, g1_static_robot from dimos.visualization.vis_module import vis_module @@ -51,14 +54,17 @@ def _g1_path_colors(path: Path) -> Any: if not path.poses: return rr.LineStrips3D([]) - points = [[pose.x, pose.y, pose.z + _PATH_Z_LIFT] for pose in path.poses] + points = [ + [pose.pose.position.x, pose.pose.position.y, pose.pose.position.z + _PATH_Z_LIFT] + for pose in path.poses + ] return rr.LineStrips3D([points], colors=[_PATH_COLOR_RGBA], radii=_PATH_RADIUS_METERS) -def _global_map_colors(cloud: Any) -> Any: +def _global_map_colors(cloud: PointCloud2) -> Any: import rerun as rr - points, _ = cloud.as_numpy() + points = pointcloud_xyz(cloud) if len(points) == 0: return None @@ -76,27 +82,29 @@ def _global_map_colors(cloud: Any) -> Any: return rr.Points3D(positions=points[:, :3], colors=colors, radii=0.03) -def _waypoint_colors(waypoint: Any) -> Any: +def _waypoint_colors(waypoint: PointStamped) -> Any: import rerun as rr - if not all(math.isfinite(value) for value in (waypoint.x, waypoint.y, waypoint.z)): + if not all( + math.isfinite(value) for value in (waypoint.point.x, waypoint.point.y, waypoint.point.z) + ): return None return rr.Points3D( - positions=[[waypoint.x, waypoint.y, waypoint.z + _VIS_LIFT]], + positions=[[waypoint.point.x, waypoint.point.y, waypoint.point.z + _VIS_LIFT]], colors=[(255, 140, 0)], radii=0.22, ) -def _goal_colors(goal: Any) -> Any: +def _goal_colors(goal: PointStamped) -> Any: import rerun as rr - if not all(math.isfinite(value) for value in (goal.x, goal.y, goal.z)): + if not all(math.isfinite(value) for value in (goal.point.x, goal.point.y, goal.point.z)): return None return rr.Points3D( - positions=[[goal.x, goal.y, goal.z + _VIS_LIFT]], + positions=[[goal.point.x, goal.point.y, goal.point.z + _VIS_LIFT]], colors=[(180, 60, 220)], radii=0.3, ) diff --git a/dimos/robot/unitree/g1/config.py b/dimos/robot/unitree/g1/config.py index fae7d212e8a..4a423fc835e 100644 --- a/dimos/robot/unitree/g1/config.py +++ b/dimos/robot/unitree/g1/config.py @@ -20,9 +20,7 @@ from pathlib import Path from typing import Any -from dimos.msgs.geometry_msgs.Pose import Pose -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Vector3 import Vector3 +from dimos_generated.geometry_msgs.msg import Point, Pose @dataclass(frozen=True) @@ -43,6 +41,6 @@ class G1Config: width_clearance=0.6, internal_odom_offsets={ # Mid-360 lidar: 1.2 m above ground. - "mid360_link": Pose(0.0, 0.0, 1.2, *Quaternion.from_euler(Vector3(0, 0, 0))), + "mid360_link": Pose(position=Point(x=0.0, y=0.0, z=1.2)), }, ) diff --git a/dimos/robot/unitree/g1/connection.py b/dimos/robot/unitree/g1/connection.py index ed2c490e495..5dfc966598e 100644 --- a/dimos/robot/unitree/g1/connection.py +++ b/dimos/robot/unitree/g1/connection.py @@ -15,13 +15,13 @@ from abc import ABC, abstractmethod from typing import Any +from dimos_generated.geometry_msgs.msg import Twist from pydantic import Field from reactivex.disposable import Disposable from dimos.core.core import rpc from dimos.core.module import Module, ModuleConfig from dimos.core.stream import In -from dimos.msgs.geometry_msgs.Twist import Twist from dimos.robot.unitree.connection import UnitreeWebRTCConnection from dimos.utils.logging_config import setup_logger diff --git a/dimos/robot/unitree/g1/connection_spec.py b/dimos/robot/unitree/g1/connection_spec.py index ed2d572df27..cfb5b93fc55 100644 --- a/dimos/robot/unitree/g1/connection_spec.py +++ b/dimos/robot/unitree/g1/connection_spec.py @@ -14,7 +14,8 @@ from typing import Any, Protocol -from dimos.msgs.geometry_msgs.Twist import Twist +from dimos_generated.geometry_msgs.msg import Twist + from dimos.spec.utils import Spec diff --git a/dimos/robot/unitree/g1/debug/demo_arrow_control.py b/dimos/robot/unitree/g1/debug/demo_arrow_control.py index 7224275ecf3..10dc3c11b18 100755 --- a/dimos/robot/unitree/g1/debug/demo_arrow_control.py +++ b/dimos/robot/unitree/g1/debug/demo_arrow_control.py @@ -20,8 +20,8 @@ import traceback from typing import Any -from dimos.msgs.geometry_msgs.Twist import Twist -from dimos.msgs.geometry_msgs.Vector3 import Vector3 +from dimos_generated.geometry_msgs.msg import Twist, Vector3 + from dimos.robot.unitree.g1.effectors.high_level.dds_sdk import G1HighLevelDdsSdk @@ -113,26 +113,39 @@ def main(stdscr: Any) -> None: # Arrow keys if key == curses.KEY_UP or key_char == "w": - twist = Twist(linear=Vector3(linear_speed, 0, 0), angular=Vector3(0, 0, 0)) + twist = Twist( + linear=Vector3(x=linear_speed, y=0, z=0), angular=Vector3(x=0, y=0, z=0) + ) action = "Moving forward..." elif key == curses.KEY_DOWN or key_char == "s": - twist = Twist(linear=Vector3(-linear_speed, 0, 0), angular=Vector3(0, 0, 0)) + twist = Twist( + linear=Vector3(x=-linear_speed, y=0, z=0), angular=Vector3(x=0, y=0, z=0) + ) action = "Moving backward..." elif key == curses.KEY_LEFT or key_char == "a": - twist = Twist(linear=Vector3(0, 0, 0), angular=Vector3(0, 0, angular_speed)) + twist = Twist( + linear=Vector3(x=0, y=0, z=0), angular=Vector3(x=0, y=0, z=angular_speed) + ) action = "Rotating left..." elif key == curses.KEY_RIGHT or key_char == "d": - twist = Twist(linear=Vector3(0, 0, 0), angular=Vector3(0, 0, -angular_speed)) + twist = Twist( + linear=Vector3(x=0, y=0, z=0), angular=Vector3(x=0, y=0, z=-angular_speed) + ) action = "Rotating right..." elif key_char == "q": - twist = Twist(linear=Vector3(0, linear_speed, 0), angular=Vector3(0, 0, 0)) + twist = Twist( + linear=Vector3(x=0, y=linear_speed, z=0), angular=Vector3(x=0, y=0, z=0) + ) action = "Strafing left..." elif key_char == "e": - twist = Twist(linear=Vector3(0, -linear_speed, 0), angular=Vector3(0, 0, 0)) + twist = Twist( + linear=Vector3(x=0, y=-linear_speed, z=0), angular=Vector3(x=0, y=0, z=0) + ) action = "Strafing right..." elif key_char == " ": conn.move( - Twist(linear=Vector3(0, 0, 0), angular=Vector3(0, 0, 0)), duration=move_duration + Twist(linear=Vector3(x=0, y=0, z=0), angular=Vector3(x=0, y=0, z=0)), + duration=move_duration, ) action = "🛑 Stopped" last_cmd_time = current_time diff --git a/dimos/robot/unitree/g1/debug/demo_arrow_control_cmd_vel.py b/dimos/robot/unitree/g1/debug/demo_arrow_control_cmd_vel.py index bd284428589..fab4326f17e 100644 --- a/dimos/robot/unitree/g1/debug/demo_arrow_control_cmd_vel.py +++ b/dimos/robot/unitree/g1/debug/demo_arrow_control_cmd_vel.py @@ -20,18 +20,17 @@ import traceback from typing import Any +from dimos_generated.geometry_msgs.msg import Twist, Vector3 import lcm from dimos.core.transport import LCMTransport -from dimos.msgs.geometry_msgs.Twist import Twist -from dimos.msgs.geometry_msgs.Vector3 import Vector3 from dimos.robot.unitree.g1.effectors.high_level.dds_sdk import G1HighLevelDdsSdk -CMD_VEL_CHANNEL = "/cmd_vel#geometry_msgs.Twist" +CMD_VEL_CHANNEL = "/cmd_vel#geometry_msgs/msg/Twist" def publish_twist(lc: lcm.LCM, twist: Twist) -> None: - lc.publish(CMD_VEL_CHANNEL, twist.lcm_encode()) + lc.publish(CMD_VEL_CHANNEL, twist.encode()) def draw_ui(stdscr: Any, state_text: str = "Not connected") -> None: @@ -116,25 +115,37 @@ def main(stdscr: Any) -> None: action = None if key == curses.KEY_UP or key_char == "w": - twist = Twist(linear=Vector3(linear_speed, 0, 0), angular=Vector3(0, 0, 0)) + twist = Twist( + linear=Vector3(x=linear_speed, y=0, z=0), angular=Vector3(x=0, y=0, z=0) + ) action = "Moving forward..." elif key == curses.KEY_DOWN or key_char == "s": - twist = Twist(linear=Vector3(-linear_speed, 0, 0), angular=Vector3(0, 0, 0)) + twist = Twist( + linear=Vector3(x=-linear_speed, y=0, z=0), angular=Vector3(x=0, y=0, z=0) + ) action = "Moving backward..." elif key == curses.KEY_LEFT or key_char == "a": - twist = Twist(linear=Vector3(0, 0, 0), angular=Vector3(0, 0, angular_speed)) + twist = Twist( + linear=Vector3(x=0, y=0, z=0), angular=Vector3(x=0, y=0, z=angular_speed) + ) action = "Rotating left..." elif key == curses.KEY_RIGHT or key_char == "d": - twist = Twist(linear=Vector3(0, 0, 0), angular=Vector3(0, 0, -angular_speed)) + twist = Twist( + linear=Vector3(x=0, y=0, z=0), angular=Vector3(x=0, y=0, z=-angular_speed) + ) action = "Rotating right..." elif key_char == "q": - twist = Twist(linear=Vector3(0, linear_speed, 0), angular=Vector3(0, 0, 0)) + twist = Twist( + linear=Vector3(x=0, y=linear_speed, z=0), angular=Vector3(x=0, y=0, z=0) + ) action = "Strafing left..." elif key_char == "e": - twist = Twist(linear=Vector3(0, -linear_speed, 0), angular=Vector3(0, 0, 0)) + twist = Twist( + linear=Vector3(x=0, y=-linear_speed, z=0), angular=Vector3(x=0, y=0, z=0) + ) action = "Strafing right..." elif key_char == " ": - stop = Twist(linear=Vector3(0, 0, 0), angular=Vector3(0, 0, 0)) + stop = Twist(linear=Vector3(x=0, y=0, z=0), angular=Vector3(x=0, y=0, z=0)) publish_twist(lc, stop) action = "Stopped" last_cmd_time = current_time @@ -160,7 +171,7 @@ def main(stdscr: Any) -> None: pass finally: draw_ui(stdscr, "Stopping...") - stop = Twist(linear=Vector3(0, 0, 0), angular=Vector3(0, 0, 0)) + stop = Twist(linear=Vector3(x=0, y=0, z=0), angular=Vector3(x=0, y=0, z=0)) publish_twist(lc, stop) time.sleep(0.5) conn.disconnect() diff --git a/dimos/robot/unitree/g1/effectors/high_level/dds_sdk.py b/dimos/robot/unitree/g1/effectors/high_level/dds_sdk.py index df62f9c0b42..72849d2f9c4 100644 --- a/dimos/robot/unitree/g1/effectors/high_level/dds_sdk.py +++ b/dimos/robot/unitree/g1/effectors/high_level/dds_sdk.py @@ -20,6 +20,7 @@ import time from typing import Any +from dimos_generated.geometry_msgs.msg import Twist, Vector3 from reactivex.disposable import Disposable from unitree_sdk2py.comm.motion_switcher.motion_switcher_client import ( # type: ignore[import-not-found] MotionSwitcherClient, @@ -37,8 +38,6 @@ from dimos.core.global_config import GlobalConfig, global_config from dimos.core.module import Module, ModuleConfig from dimos.core.stream import In -from dimos.msgs.geometry_msgs.Twist import Twist -from dimos.msgs.geometry_msgs.Vector3 import Vector3 from dimos.robot.unitree.g1.effectors.high_level.commands import ( ARM_API_ID, ARM_COMMANDS, @@ -297,7 +296,7 @@ def move_velocity( self, x: float, y: float = 0.0, yaw: float = 0.0, duration: float = 0.0 ) -> str: """Move the robot at the given velocity for ``duration`` seconds.""" - twist = Twist(linear=Vector3(x, y, 0), angular=Vector3(0, 0, yaw)) + twist = Twist(linear=Vector3(x=x, y=y), angular=Vector3(z=yaw)) self.move(twist, duration=duration) return f"Started moving with velocity=({x}, {y}, {yaw}) for {duration} seconds" diff --git a/dimos/robot/unitree/g1/effectors/high_level/high_level_spec.py b/dimos/robot/unitree/g1/effectors/high_level/high_level_spec.py index 01dde2ab248..146b734c527 100644 --- a/dimos/robot/unitree/g1/effectors/high_level/high_level_spec.py +++ b/dimos/robot/unitree/g1/effectors/high_level/high_level_spec.py @@ -16,8 +16,9 @@ from typing import Any, Protocol +from dimos_generated.geometry_msgs.msg import Twist + from dimos.core.stream import In -from dimos.msgs.geometry_msgs.Twist import Twist from dimos.spec.utils import Spec diff --git a/dimos/robot/unitree/g1/effectors/high_level/webrtc.py b/dimos/robot/unitree/g1/effectors/high_level/webrtc.py index 7b9743f4d02..24acc0f20db 100644 --- a/dimos/robot/unitree/g1/effectors/high_level/webrtc.py +++ b/dimos/robot/unitree/g1/effectors/high_level/webrtc.py @@ -14,6 +14,7 @@ from typing import Any +from dimos_generated.geometry_msgs.msg import Twist, Vector3 from pydantic import Field from reactivex.disposable import Disposable @@ -22,8 +23,6 @@ from dimos.core.global_config import GlobalConfig, global_config from dimos.core.module import Module, ModuleConfig from dimos.core.stream import In -from dimos.msgs.geometry_msgs.Twist import Twist -from dimos.msgs.geometry_msgs.Vector3 import Vector3 from dimos.robot.unitree.connection import UnitreeWebRTCConnection from dimos.robot.unitree.g1.effectors.high_level.commands import ( ARM_API_ID, @@ -116,7 +115,7 @@ def move_velocity( args = { "x": 0.5, "y": 0.0, "yaw": 0.0, "duration": 2.0 } move_velocity(**args) """ - twist = Twist(linear=Vector3(x, y, 0), angular=Vector3(0, 0, yaw)) + twist = Twist(linear=Vector3(x=x, y=y), angular=Vector3(z=yaw)) self.move(twist, duration=duration) return f"Started moving with velocity=({x}, {y}, {yaw}) for {duration} seconds" diff --git a/dimos/robot/unitree/g1/g1_recorder.py b/dimos/robot/unitree/g1/g1_recorder.py index d6f8883d676..d80152d9f44 100644 --- a/dimos/robot/unitree/g1/g1_recorder.py +++ b/dimos/robot/unitree/g1/g1_recorder.py @@ -16,18 +16,17 @@ from __future__ import annotations +from dimos_generated.sensor_msgs.msg import CameraInfo, Image from pydantic import Field from dimos.core.stream import In from dimos.hardware.sensors.lidar.pointlio.recorder import PointlioRecorder, PointlioRecorderConfig -from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo -from dimos.msgs.sensor_msgs.Image import Image class G1RecorderConfig(PointlioRecorderConfig): - # don't compress + # Preserve depth pixels; lossless compression wraps generated CDR. stream_codecs: dict[str, str] = Field( - default_factory=lambda: {"realsense_depth_image": "lz4+lcm"} + default_factory=lambda: {"realsense_depth_image": "lz4+cdr"} ) diff --git a/dimos/robot/unitree/g1/g1_tf_publisher.py b/dimos/robot/unitree/g1/g1_tf_publisher.py index 7237a76218e..aba566175ea 100644 --- a/dimos/robot/unitree/g1/g1_tf_publisher.py +++ b/dimos/robot/unitree/g1/g1_tf_publisher.py @@ -26,16 +26,17 @@ import time from typing import Any +from dimos_generated.geometry_msgs.msg import Transform, TransformStamped, Vector3 +from dimos_generated.std_msgs.msg import Header +from dimos_generated.tf2_msgs.msg import TFMessage from pydantic import Field from dimos.constants import DEFAULT_THREAD_JOIN_TIMEOUT from dimos.core.core import rpc from dimos.core.module import Module, ModuleConfig from dimos.core.stream import Out -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Transform import Transform -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.msgs.tf2_msgs.TFMessage import TFMessage +from dimos.msgs.geometry import compose_transforms, inverse_transform, quaternion_from_euler +from dimos.msgs.time import time_from_nanoseconds from dimos.utils.logging_config import setup_logger logger = setup_logger() @@ -51,25 +52,32 @@ _WAIST_ROLL_ORIGIN = (-0.0039635, 0.0, 0.044) -def torso_to_mid360() -> Transform: - """torso_link -> the Mid-360's own frame: URDF mount pitch plus the upside-down roll.""" - return Transform( - translation=Vector3(*_TORSO_MID360_XYZ), - rotation=Quaternion.from_euler(Vector3(0.0, MID360_PITCH, 0.0)) - * Quaternion.from_euler(Vector3(math.pi, 0.0, 0.0)), - frame_id="torso_link", - child_frame_id="mid360_link", +def _mount( + parent: str, + child: str, + xyz: tuple[float, float, float] = (0.0, 0.0, 0.0), + roll: float = 0.0, + pitch: float = 0.0, + yaw: float = 0.0, +) -> TransformStamped: + return TransformStamped( + header=Header(frame_id=parent), + child_frame_id=child, + transform=Transform( + translation=Vector3(x=xyz[0], y=xyz[1], z=xyz[2]), + rotation=quaternion_from_euler(roll, pitch, yaw), + ), ) -def torso_to_d435() -> Transform: +def torso_to_mid360() -> TransformStamped: + """URDF mount pitch composed after the sensor's upside-down roll.""" + return _mount("torso_link", "mid360_link", _TORSO_MID360_XYZ, roll=math.pi, pitch=MID360_PITCH) + + +def torso_to_d435() -> TransformStamped: """torso_link -> d435_link, the URDF mount.""" - return Transform( - translation=Vector3(*_TORSO_D435_XYZ), - rotation=Quaternion.from_euler(Vector3(0.0, D435_PITCH, 0.0)), - frame_id="torso_link", - child_frame_id="d435_link", - ) + return _mount("torso_link", "d435_link", _TORSO_D435_XYZ, pitch=D435_PITCH) # rt/lowstate motor indices, ordering from make_humanoid_joints("g1"). @@ -78,34 +86,21 @@ def torso_to_d435() -> Transform: _WAIST_PITCH_IDX = 14 -def base_to_torso(waist_yaw: float, waist_roll: float, waist_pitch: float) -> Transform: +def base_to_torso(waist_yaw: float, waist_roll: float, waist_pitch: float) -> TransformStamped: """base_link -> torso_link through the g1.urdf waist chain.""" - yaw = Transform( - rotation=Quaternion.from_euler(Vector3(0.0, 0.0, waist_yaw)), - frame_id="base_link", - child_frame_id="waist_yaw_link", - ) - roll = Transform( - translation=Vector3(*_WAIST_ROLL_ORIGIN), - rotation=Quaternion.from_euler(Vector3(waist_roll, 0.0, 0.0)), - frame_id="waist_yaw_link", - child_frame_id="waist_roll_link", - ) - pitch = Transform( - rotation=Quaternion.from_euler(Vector3(0.0, waist_pitch, 0.0)), - frame_id="waist_roll_link", - child_frame_id="torso_link", - ) - return yaw + roll + pitch + yaw = _mount("base_link", "waist_yaw_link", yaw=waist_yaw) + roll = _mount("waist_yaw_link", "waist_roll_link", _WAIST_ROLL_ORIGIN, roll=waist_roll) + pitch = _mount("waist_roll_link", "torso_link", pitch=waist_pitch) + return compose_transforms(compose_transforms(yaw, roll), pitch) def mount_transforms( waist_yaw: float = 0.0, waist_roll: float = 0.0, waist_pitch: float = 0.0 -) -> list[Transform]: +) -> list[TransformStamped]: """The mount tree as published: rooted at mid360_link.""" return [ - -torso_to_mid360(), - -base_to_torso(waist_yaw, waist_roll, waist_pitch), + inverse_transform(torso_to_mid360()), + inverse_transform(base_to_torso(waist_yaw, waist_roll, waist_pitch)), torso_to_d435(), ] @@ -212,8 +207,8 @@ async def _publish_loop(self) -> None: with self._waist_lock: waist_yaw, waist_roll, waist_pitch = self._waist transforms = mount_transforms(waist_yaw, waist_roll, waist_pitch) - now = time.time() + now = time.time_ns() for transform in transforms: - transform.ts = now - self.tf.publish(TFMessage(*transforms)) + transform.header.stamp = time_from_nanoseconds(now) + self.tf.publish(TFMessage(transforms=transforms)) await asyncio.sleep(period) diff --git a/dimos/robot/unitree/g1/mujoco_sim.py b/dimos/robot/unitree/g1/mujoco_sim.py index 174c5042e87..f9320f4bfb4 100644 --- a/dimos/robot/unitree/g1/mujoco_sim.py +++ b/dimos/robot/unitree/g1/mujoco_sim.py @@ -12,11 +12,23 @@ # See the License for the specific language governing permissions and # limitations under the License. +import copy import threading from threading import Thread import time from typing import Any +from dimos_generated.geometry_msgs.msg import ( + PoseStamped, + Quaternion, + Transform, + TransformStamped, + Twist, + Vector3, +) +from dimos_generated.sensor_msgs.msg import CameraInfo, Image, PointCloud2 +from dimos_generated.std_msgs.msg import Header +from dimos_generated.tf2_msgs.msg import TFMessage from pydantic import Field from reactivex.disposable import Disposable @@ -24,18 +36,10 @@ from dimos.core.core import rpc from dimos.core.module import ModuleConfig from dimos.core.stream import In, Out -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Transform import Transform -from dimos.msgs.geometry_msgs.Twist import Twist -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo -from dimos.msgs.sensor_msgs.Image import Image -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 -from dimos.msgs.tf2_msgs.TFMessage import TFMessage +from dimos.msgs.geometry import transform_from_pose +from dimos.msgs.time import time_from_nanoseconds from dimos.robot.unitree.g1.connection import G1ConnectionBase from dimos.robot.unitree.mujoco_connection import MujocoConnection -from dimos.robot.unitree.type.odometry import Odometry as SimOdometry from dimos.utils.logging_config import setup_logger logger = setup_logger() @@ -64,14 +68,12 @@ def __init__(self, **kwargs: Any) -> None: def start(self) -> None: super().start() - from dimos.robot.unitree.mujoco_connection import MujocoConnection - self.connection = MujocoConnection(self.config.g) assert self.connection is not None self.connection.start() self.register_disposable(Disposable(self.cmd_vel.subscribe(self.move))) - self.register_disposable(self.connection.odom_stream().subscribe(self._publish_sim_odom)) + self.register_disposable(self.connection.odom_stream().subscribe(self._publish_tf)) self.register_disposable(self.connection.lidar_stream().subscribe(self.lidar.publish)) self.register_disposable(self.connection.video_stream().subscribe(self.color_image.publish)) @@ -92,52 +94,32 @@ def stop(self) -> None: def _publish_camera_info_loop(self) -> None: assert self.connection is not None - info = self.connection.camera_info_static while not self._stop_event.is_set(): + info = copy.copy(self.connection.camera_info_static) + info.header.stamp = time_from_nanoseconds(time.time_ns()) self.camera_info.publish(info) self._stop_event.wait(1.0) def _publish_tf(self, msg: PoseStamped) -> None: self.odom.publish(msg) - - base_link = Transform.from_pose("base_link", msg) - - # Publish camera_link and camera_optical transforms - camera_link = Transform( - translation=Vector3(0.05, 0.0, 0.6), - rotation=Quaternion(0.0, 0.0, 0.0, 1.0), - frame_id="base_link", - child_frame_id="camera_link", - ts=time.time(), - ) - - camera_optical = Transform( - translation=Vector3(0.0, 0.0, 0.0), - rotation=Quaternion(-0.5, 0.5, -0.5, 0.5), - frame_id="camera_link", - child_frame_id="camera_optical", - ts=time.time(), - ) - - map_to_world = Transform( - translation=Vector3(0.0, 0.0, 0.0), - rotation=Quaternion(0.0, 0.0, 0.0, 1.0), - frame_id="map", - child_frame_id="world", - ts=time.time(), - ) - - self.tf.publish(TFMessage(base_link, camera_link, camera_optical, map_to_world)) - - def _publish_sim_odom(self, msg: SimOdometry) -> None: - self._publish_tf( - PoseStamped( - ts=msg.ts, - frame_id=msg.frame_id, - position=msg.position, - orientation=msg.orientation, - ) - ) + transforms = [ + transform_from_pose(msg, child_frame_id="base_link"), + TransformStamped( + header=Header(stamp=msg.header.stamp, frame_id="base_link"), + child_frame_id="camera_link", + transform=Transform(translation=Vector3(x=0.05, z=0.6)), + ), + TransformStamped( + header=Header(stamp=msg.header.stamp, frame_id="camera_link"), + child_frame_id="camera_optical", + transform=Transform(rotation=Quaternion(x=-0.5, y=0.5, z=-0.5, w=0.5)), + ), + TransformStamped( + header=Header(stamp=msg.header.stamp, frame_id="map"), + child_frame_id="world", + ), + ] + self.tf.publish(TFMessage(transforms=transforms)) @rpc def move(self, twist: Twist, duration: float = 0.0) -> None: diff --git a/dimos/robot/unitree/g1/skill_container.py b/dimos/robot/unitree/g1/skill_container.py index c3825ed29cc..68f8327fe5e 100644 --- a/dimos/robot/unitree/g1/skill_container.py +++ b/dimos/robot/unitree/g1/skill_container.py @@ -19,11 +19,11 @@ import difflib +from dimos_generated.geometry_msgs.msg import Twist, Vector3 + from dimos.agents.annotation import skill from dimos.core.core import rpc from dimos.core.module import Module -from dimos.msgs.geometry_msgs.Twist import Twist -from dimos.msgs.geometry_msgs.Vector3 import Vector3 from dimos.robot.unitree.g1.connection_spec import G1ConnectionSpec from dimos.utils.logging_config import setup_logger @@ -89,7 +89,7 @@ def move(self, x: float, y: float = 0.0, yaw: float = 0.0, duration: float = 0.0 duration: How long to move (seconds) """ - twist = Twist(linear=Vector3(x, y, 0), angular=Vector3(0, 0, yaw)) + twist = Twist(linear=Vector3(x=x, y=y), angular=Vector3(z=yaw)) self._connection.move(twist, duration=duration) return f"Started moving with velocity=({x}, {y}, {yaw}) for {duration} seconds" diff --git a/dimos/robot/unitree/g1/test_g1_manipulation_integration.py b/dimos/robot/unitree/g1/test_g1_manipulation_integration.py index c7ab78abbdf..53f9b309451 100644 --- a/dimos/robot/unitree/g1/test_g1_manipulation_integration.py +++ b/dimos/robot/unitree/g1/test_g1_manipulation_integration.py @@ -18,12 +18,13 @@ pytest.importorskip("roboplan.core") +from dimos_generated.sensor_msgs.msg import JointState + from dimos.manipulation.planning.planners import roboplan_planner as roboplan_planner_module from dimos.manipulation.planning.planners.roboplan_config import RoboPlanPlannerConfig from dimos.manipulation.planning.spec.enums import PlanningStatus from dimos.manipulation.planning.spec.validation import prepare_robot_model from dimos.manipulation.planning.world import roboplan_world as roboplan_world_module -from dimos.msgs.sensor_msgs.JointState import JointState from dimos.robot.unitree.g1.manip_config import ( G1_LEFT_ARM_JOINTS, G1_RIGHT_ARM_JOINTS, diff --git a/dimos/robot/unitree/g1/test_g1_tf_publisher.py b/dimos/robot/unitree/g1/test_g1_tf_publisher.py index 222e26d1b51..df689179879 100644 --- a/dimos/robot/unitree/g1/test_g1_tf_publisher.py +++ b/dimos/robot/unitree/g1/test_g1_tf_publisher.py @@ -16,8 +16,10 @@ import math -from dimos.msgs.geometry_msgs.Transform import Transform -from dimos.msgs.geometry_msgs.Vector3 import Vector3 +from dimos_generated.geometry_msgs.msg import Transform, TransformStamped, Vector3 +from dimos_generated.std_msgs.msg import Header + +from dimos.msgs.geometry import inverse_transform, quaternion_euler, transform_matrix from dimos.protocol.tf.tf import MultiTBuffer from dimos.robot.unitree.g1.g1_tf_publisher import ( D435_PITCH, @@ -40,7 +42,7 @@ def _buffer( waist_yaw: float = 0.0, waist_roll: float = 0.0, waist_pitch: float = 0.0, - live: Transform | None = None, + live: TransformStamped | None = None, ) -> MultiTBuffer: buffer = MultiTBuffer() buffer.receive_transform(*mount_transforms(waist_yaw, waist_roll, waist_pitch)) @@ -53,34 +55,35 @@ def test_rest_pose_offsets_match_urdf() -> None: """The lidar sits MOUNT_Z above base_link, the offset every ground projection uses.""" leg = _buffer().get("mid360_link", "base_link") assert leg is not None - base_to_sensor = -leg - assert abs(base_to_sensor.translation.z - MOUNT_Z) < 1e-6 - assert abs(base_to_sensor.translation.x - MOUNT_X) < 1e-6 + base_to_sensor = inverse_transform(leg) + assert abs(base_to_sensor.transform.translation.z - MOUNT_Z) < 1e-6 + assert abs(base_to_sensor.transform.translation.x - MOUNT_X) < 1e-6 def test_mid360_frame_is_upside_down() -> None: """The inverted mount points the sensor's z axis at the floor in the base frame.""" leg = _buffer().get("base_link", "mid360_link") assert leg is not None - z_axis = leg.rotation.rotate_vector(Vector3(0.0, 0.0, 1.0)) - assert z_axis.z < -0.99 + z_axis = transform_matrix(leg.transform)[:3, 2] + assert z_axis[2] < -0.99 def test_flipped_level_sensor_yields_level_base() -> None: """A standing robot reports a flipped sensor pose. base_link must come out level, below it.""" - live = Transform( - translation=Vector3(0.0, 0.0, LIDAR_HEIGHT), - rotation=torso_to_mid360().rotation, - frame_id="world", + live = TransformStamped( + header=Header(frame_id="world"), child_frame_id="mid360_link", + transform=Transform( + translation=Vector3(z=LIDAR_HEIGHT), rotation=torso_to_mid360().transform.rotation + ), ) base = _buffer(live=live).get("world", "base_link") assert base is not None - euler = base.rotation.euler - assert abs(euler.x) < 1e-6 - assert abs(euler.y) < 1e-6 - assert abs(euler.z) < 1e-6 - assert math.isclose(base.translation.z, LIDAR_HEIGHT - MOUNT_Z, abs_tol=1e-6) + euler = quaternion_euler(base.transform.rotation) + assert abs(euler[0]) < 1e-6 + assert abs(euler[1]) < 1e-6 + assert abs(euler[2]) < 1e-6 + assert math.isclose(base.transform.translation.z, LIDAR_HEIGHT - MOUNT_Z, abs_tol=1e-6) def test_waist_yaw_rotates_base_link_against_the_torso() -> None: @@ -88,32 +91,32 @@ def test_waist_yaw_rotates_base_link_against_the_torso() -> None: yaw = math.pi / 4 leg = _buffer(waist_yaw=yaw).get("torso_link", "base_link") assert leg is not None - assert abs(leg.rotation.euler.z - (-yaw)) < 1e-6 + assert abs(quaternion_euler(leg.transform.rotation)[2] - (-yaw)) < 1e-6 def test_waist_pitch_rotates_base_link_against_the_torso() -> None: pitch = 0.3 leg = _buffer(waist_pitch=pitch).get("torso_link", "base_link") assert leg is not None - assert abs(leg.rotation.euler.y - (-pitch)) < 1e-6 + assert abs(quaternion_euler(leg.transform.rotation)[1] - (-pitch)) < 1e-6 def test_rest_pose_base_to_torso_matches_urdf_offsets() -> None: rest = base_to_torso(0.0, 0.0, 0.0) - assert abs(rest.translation.x - PELVIS_TORSO_X) < 1e-6 - assert abs(rest.translation.z - PELVIS_TORSO_Z) < 1e-6 - assert abs(rest.rotation.euler.x) < 1e-6 - assert abs(rest.rotation.euler.y) < 1e-6 - assert abs(rest.rotation.euler.z) < 1e-6 + assert abs(rest.transform.translation.x - PELVIS_TORSO_X) < 1e-6 + assert abs(rest.transform.translation.z - PELVIS_TORSO_Z) < 1e-6 + assert abs(quaternion_euler(rest.transform.rotation)[0]) < 1e-6 + assert abs(quaternion_euler(rest.transform.rotation)[1]) < 1e-6 + assert abs(quaternion_euler(rest.transform.rotation)[2]) < 1e-6 def test_d435_hangs_off_base_link() -> None: """The tree is rooted at mid360_link, so the camera edge is reachable by composition.""" camera = _buffer().get("base_link", "d435_link") assert camera is not None - assert abs(camera.translation.x - (PELVIS_TORSO_X + 0.0576235)) < 1e-6 - assert abs(camera.translation.z - (PELVIS_TORSO_Z + 0.42987)) < 1e-6 - assert abs(camera.rotation.euler.y - D435_PITCH) < 1e-6 + assert abs(camera.transform.translation.x - (PELVIS_TORSO_X + 0.0576235)) < 1e-6 + assert abs(camera.transform.translation.z - (PELVIS_TORSO_Z + 0.42987)) < 1e-6 + assert abs(quaternion_euler(camera.transform.rotation)[1] - D435_PITCH) < 1e-6 def test_pelvis_height_matches_config_note() -> None: diff --git a/dimos/robot/unitree/g1/test_wholebody_connection.py b/dimos/robot/unitree/g1/test_wholebody_connection.py index 35c41badabc..4a6067981e9 100644 --- a/dimos/robot/unitree/g1/test_wholebody_connection.py +++ b/dimos/robot/unitree/g1/test_wholebody_connection.py @@ -17,13 +17,16 @@ from collections.abc import Iterator from types import SimpleNamespace +from dimos_generated.builtin_interfaces.msg import Time +from dimos_generated.dimos_msgs.msg import MotorCommandArray +from dimos_generated.std_msgs.msg import Header from pydantic import ValidationError import pytest -from dimos.msgs.sensor_msgs.MotorCommandArray import MotorCommandArray from dimos.robot.unitree.g1.wholebody_connection import ( _NUM_MOTOR_SLOTS, _NUM_MOTORS, + G1LowStateSnapshot, G1WholeBodyConnection, G1WholeBodyConnectionConfig, ) @@ -130,3 +133,38 @@ def test_non_finite_soft_start_is_rejected(value): # and no way back to the commanded gains. with pytest.raises(ValidationError): G1WholeBodyConnectionConfig(soft_start_seconds=value) + + +def test_generated_feedback_has_one_exact_header_and_ros_quaternion(connection, monkeypatch): + joints, imu = [], [] + monkeypatch.setattr(connection.motor_states, "publish", joints.append) + monkeypatch.setattr(connection.imu, "publish", imu.append) + header = Header(stamp=Time(sec=1700000000, nanosec=123456789), frame_id="g1_pelvis") + sample = G1LowStateSnapshot( + positions=[0.1] * 29, + velocities=[0.2] * 29, + efforts=[0.3] * 29, + quaternion=(0.8, 0.0, 0.0, 0.6), + gyroscope=(1.0, 2.0, 3.0), + accelerometer=(4.0, 5.0, 6.0), + ) + connection._publish_motor_state_and_imu(header=header, sample=sample) + assert joints[0].header == header == imu[0].header + assert list(joints[0].position) == [0.1] * 29 + assert imu[0].orientation.w == 0.8 + assert imu[0].orientation.z == 0.6 + assert imu[0].angular_velocity.x == 1.0 + assert imu[0].linear_acceleration.z == 6.0 + for message in [joints[0], imu[0]]: + assert type(message).decode(message.encode()) == message + header.stamp.nanosec = 0 + assert joints[0].header.stamp.nanosec == 123456789 + + +@pytest.mark.parametrize("field", ["dq", "kp", "kd", "tau"]) +def test_inconsistent_motor_array_is_dropped(connection, field): + publisher = _wire(connection, soft_start_seconds=0.0) + command = _command() + setattr(command, field, [0.0]) + connection._on_motor_command(command) + assert publisher.frames == [] diff --git a/dimos/robot/unitree/g1/wholebody_connection.py b/dimos/robot/unitree/g1/wholebody_connection.py index 2f185d62e8b..2139ae6c3eb 100644 --- a/dimos/robot/unitree/g1/wholebody_connection.py +++ b/dimos/robot/unitree/g1/wholebody_connection.py @@ -27,6 +27,10 @@ import time from typing import TYPE_CHECKING, Any +from dimos_generated.dimos_msgs.msg import MotorCommandArray +from dimos_generated.geometry_msgs.msg import Quaternion, Vector3 +from dimos_generated.sensor_msgs.msg import Imu, JointState +from dimos_generated.std_msgs.msg import Header from pydantic import Field from reactivex.disposable import Disposable @@ -41,11 +45,7 @@ from dimos.core.module import Module, ModuleConfig from dimos.core.stream import In, Out from dimos.hardware.whole_body.spec import POS_STOP, VEL_STOP -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.msgs.sensor_msgs.Imu import Imu -from dimos.msgs.sensor_msgs.JointState import JointState -from dimos.msgs.sensor_msgs.MotorCommandArray import MotorCommandArray +from dimos.msgs.time import header_now from dimos.utils.logging_config import setup_logger logger = setup_logger() @@ -69,16 +69,14 @@ def _imu_from_unitree_wxyz( gyroscope: tuple[float, float, float], accelerometer: tuple[float, float, float], *, - frame_id: str, - ts: float, + header: Header, ) -> Imu: _w, x, y, z = quaternion return Imu( - orientation=Quaternion(x, y, z, _w), - angular_velocity=Vector3(*gyroscope), - linear_acceleration=Vector3(*accelerometer), - frame_id=frame_id, - ts=ts, + orientation=Quaternion(x=x, y=y, z=z, w=_w), + angular_velocity=Vector3(x=gyroscope[0], y=gyroscope[1], z=gyroscope[2]), + linear_acceleration=Vector3(x=accelerometer[0], y=accelerometer[1], z=accelerometer[2]), + header=header, ) @@ -336,13 +334,10 @@ def _snapshot_motor_imu(self) -> G1LowStateSnapshot | None: ), ) - def _publish_motor_state_and_imu( - self, now: float, frame_id: str, sample: G1LowStateSnapshot - ) -> None: + def _publish_motor_state_and_imu(self, header: Header, sample: G1LowStateSnapshot) -> None: self.motor_states.publish( JointState( - ts=now, - frame_id=frame_id, + header=header, name=G1_JOINT_NAMES, position=sample.positions, velocity=sample.velocities, @@ -355,8 +350,7 @@ def _publish_motor_state_and_imu( sample.quaternion, sample.gyroscope, sample.accelerometer, - frame_id=frame_id, - ts=now, + header=header, ) ) @@ -369,12 +363,12 @@ def _publish_loop(self) -> None: self._drain_low_state() sample = self._snapshot_motor_imu() if sample is not None: - self._publish_motor_state_and_imu(now=time.time(), frame_id=frame_id, sample=sample) + self._publish_motor_state_and_imu(header=header_now(frame_id), sample=sample) next_tick += period sleep_for = next_tick - time.perf_counter() if sleep_for > 0: - time.sleep(sleep_for) + self._stop_event.wait(sleep_for) else: next_tick = time.perf_counter() @@ -396,8 +390,8 @@ def _soft_start_scale(self, now: float) -> float: return scale def _on_motor_command(self, msg: MotorCommandArray) -> None: - if msg.num_joints != _NUM_MOTORS: - logger.warning(f"Expected {_NUM_MOTORS} motor commands, got {msg.num_joints}; ignoring") + if any(len(values) != _NUM_MOTORS for values in (msg.q, msg.dq, msg.kp, msg.kd, msg.tau)): + logger.warning(f"Expected {_NUM_MOTORS} values in each motor-command array; ignoring") return with self._lock: diff --git a/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_coordinator.py b/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_coordinator.py index d2a4c0454a8..d43a4af6ad3 100644 --- a/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_coordinator.py +++ b/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_coordinator.py @@ -22,12 +22,12 @@ from __future__ import annotations +from dimos_generated.geometry_msgs.msg import PoseStamped, Twist + from dimos.control.components import HardwareComponent, HardwareType, make_twist_base_joints from dimos.control.coordinator import ControlCoordinator, TaskConfig from dimos.core.coordination.blueprints import autoconnect from dimos.core.transport import LCMTransport -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.geometry_msgs.Twist import Twist from dimos.robot.unitree.go2.connection import GO2Connection _go2_joints = make_twist_base_joints("go2") diff --git a/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_holonomic_benchmark.py b/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_holonomic_benchmark.py index 632b46a5dcf..86d0fec2601 100644 --- a/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_holonomic_benchmark.py +++ b/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_holonomic_benchmark.py @@ -37,11 +37,12 @@ from typing import Any +from dimos_generated.geometry_msgs.msg import PoseStamped + from dimos.control.benchmarking.benchmark import Benchmarker from dimos.core.coordination.blueprints import TransportSpec, autoconnect from dimos.core.stream import Transport from dimos.core.transport import LCMTransport -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped from dimos.robot.unitree.go2.blueprints.basic.unitree_go2_holonomic_controller import ( unitree_go2_holonomic_controller, ) diff --git a/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_holonomic_controller.py b/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_holonomic_controller.py index 762727023e3..f19d5b7d26c 100644 --- a/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_holonomic_controller.py +++ b/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_holonomic_controller.py @@ -49,18 +49,17 @@ from __future__ import annotations +from dimos_generated.geometry_msgs.msg import PoseStamped, Twist +from dimos_generated.nav_msgs.msg import Path +from dimos_generated.sensor_msgs.msg import JointState +from dimos_generated.std_msgs.msg import Float32, Int8 + from dimos.control.components import HardwareComponent, HardwareType, make_twist_base_joints from dimos.control.coordinator import TaskConfig from dimos.control.path_following_coordinator import PathFollowingCoordinator from dimos.core.coordination.blueprints import autoconnect from dimos.core.stream import Out from dimos.core.transport import LCMTransport -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.geometry_msgs.Twist import Twist -from dimos.msgs.nav_msgs.Path import Path -from dimos.msgs.sensor_msgs.JointState import JointState -from dimos.msgs.std_msgs.Float32 import Float32 -from dimos.msgs.std_msgs.Int8 import Int8 from dimos.robot.unitree.go2.connection import GO2Connection from dimos.robot.unitree.keyboard_teleop import KeyboardTeleop diff --git a/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_rpp_benchmark.py b/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_rpp_benchmark.py index 965f845b232..2e0cd11ab00 100644 --- a/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_rpp_benchmark.py +++ b/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_rpp_benchmark.py @@ -50,11 +50,12 @@ from typing import Any +from dimos_generated.geometry_msgs.msg import PoseStamped + from dimos.control.benchmarking.benchmark import Benchmarker from dimos.core.coordination.blueprints import TransportSpec, autoconnect from dimos.core.stream import Transport from dimos.core.transport import LCMTransport -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped from dimos.robot.unitree.go2.blueprints.basic.unitree_go2_rpp_controller import ( unitree_go2_rpp_controller, ) diff --git a/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_rpp_controller.py b/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_rpp_controller.py index fce12dff27f..eb24b6b4583 100644 --- a/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_rpp_controller.py +++ b/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_rpp_controller.py @@ -52,18 +52,17 @@ from __future__ import annotations +from dimos_generated.geometry_msgs.msg import PoseStamped, Twist +from dimos_generated.nav_msgs.msg import Path +from dimos_generated.sensor_msgs.msg import JointState +from dimos_generated.std_msgs.msg import Float32, Int8 + from dimos.control.components import HardwareComponent, HardwareType, make_twist_base_joints from dimos.control.coordinator import TaskConfig from dimos.control.path_following_coordinator import PathFollowingCoordinator from dimos.core.coordination.blueprints import autoconnect from dimos.core.stream import Out from dimos.core.transport import LCMTransport -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.geometry_msgs.Twist import Twist -from dimos.msgs.nav_msgs.Path import Path -from dimos.msgs.sensor_msgs.JointState import JointState -from dimos.msgs.std_msgs.Float32 import Float32 -from dimos.msgs.std_msgs.Int8 import Int8 from dimos.robot.unitree.go2.connection import GO2Connection from dimos.robot.unitree.keyboard_teleop import KeyboardTeleop diff --git a/dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_nav_3d.py b/dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_nav_3d.py index 92907d4513f..f4c16a1d8ad 100644 --- a/dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_nav_3d.py +++ b/dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_nav_3d.py @@ -20,6 +20,9 @@ from pathlib import Path from typing import Any +from dimos_generated.geometry_msgs.msg import PoseStamped +from dimos_generated.sensor_msgs.msg import PointCloud2 + from dimos.constants import RECORDINGS_DIR from dimos.core.coordination.blueprints import autoconnect from dimos.core.global_config import global_config @@ -29,8 +32,6 @@ from dimos.hardware.sensors.lidar.virtual_mid360.recorder import Mid360PcapRecorder from dimos.mapping.ray_tracing.module import RayTracingVoxelMap from dimos.memory.module import pose_setter_for -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 from dimos.navigation.basic_path_follower.module import BasicPathFollower from dimos.navigation.movement_manager.movement_manager import MovementManager from dimos.navigation.nav_3d.mls_planner.mls_planner_native import MLSPlannerNative diff --git a/dimos/robot/unitree/go2/blueprints/smart/test_unitree_go2_markers.py b/dimos/robot/unitree/go2/blueprints/smart/test_unitree_go2_markers.py index bf33a2e0888..213ffb1125f 100644 --- a/dimos/robot/unitree/go2/blueprints/smart/test_unitree_go2_markers.py +++ b/dimos/robot/unitree/go2/blueprints/smart/test_unitree_go2_markers.py @@ -29,7 +29,7 @@ def test_unitree_go2_markers_uses_detector_backed_tf_stack() -> None: bp for bp in unitree_go2_markers.blueprints if bp.module is MarkerDetectionStreamModule ) assert detector.kwargs["marker_length_m"] == 0.1 - assert detector.kwargs["camera_info"].frame_id == "camera_optical" + assert detector.kwargs["camera_info"].header.frame_id == "camera_optical" assert ( unitree_go2_markers.transport_map[("detections", MarkerDetectionStreamModule)].topic.topic == "/marker_detection/detections" diff --git a/dimos/robot/unitree/go2/blueprints/smart/unitree_go2.py b/dimos/robot/unitree/go2/blueprints/smart/unitree_go2.py index 392258b3b4b..21976def079 100644 --- a/dimos/robot/unitree/go2/blueprints/smart/unitree_go2.py +++ b/dimos/robot/unitree/go2/blueprints/smart/unitree_go2.py @@ -15,6 +15,10 @@ from pathlib import Path +from dimos_generated.geometry_msgs.msg import Pose, PoseStamped +from dimos_generated.sensor_msgs.msg import CameraInfo, Image, PointCloud2 +from dimos_generated.vision_msgs.msg import Detection3DArray + from dimos.core.coordination.blueprints import autoconnect from dimos.core.stream import In from dimos.core.transport import LCMTransport @@ -22,12 +26,6 @@ from dimos.mapping.relocalization.go2.module import Go2Relocalization from dimos.mapping.voxels.module import VoxelGridMapper from dimos.memory.module import Recorder, RecorderConfig, pose_setter_for -from dimos.msgs.geometry_msgs.Pose import Pose -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo -from dimos.msgs.sensor_msgs.Image import Image -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 -from dimos.msgs.vision_msgs.Detection3DArray import Detection3DArray from dimos.navigation.frontier_exploration.wavefront_frontier_goal_selector import ( WavefrontFrontierExplorer, ) @@ -65,7 +63,7 @@ class Go2Memory(Recorder): @pose_setter_for("odom") async def _odom_pose(self, msg: PoseStamped) -> Pose | None: - self._last_odom_pose = msg + self._last_odom_pose = msg.pose return self._last_odom_pose @pose_setter_for("lidar") diff --git a/dimos/robot/unitree/go2/blueprints/smart/unitree_go2_detection.py b/dimos/robot/unitree/go2/blueprints/smart/unitree_go2_detection.py index 2d021657a2c..1c94bd5b410 100644 --- a/dimos/robot/unitree/go2/blueprints/smart/unitree_go2_detection.py +++ b/dimos/robot/unitree/go2/blueprints/smart/unitree_go2_detection.py @@ -13,11 +13,11 @@ # See the License for the specific language governing permissions and # limitations under the License. +from dimos_generated.sensor_msgs.msg import Image, PointCloud2 +from dimos_generated.vision_msgs.msg import Detection2DArray + from dimos.core.coordination.blueprints import autoconnect from dimos.core.transport import LCMTransport -from dimos.msgs.sensor_msgs.Image import Image -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 -from dimos.msgs.vision_msgs.Detection2DArray import Detection2DArray from dimos.perception.detection.module3D import Detection3DModule from dimos.robot.unitree.go2.blueprints.smart.unitree_go2 import unitree_go2 from dimos.robot.unitree.go2.connection import GO2Connection diff --git a/dimos/robot/unitree/go2/blueprints/smart/unitree_go2_ros.py b/dimos/robot/unitree/go2/blueprints/smart/unitree_go2_ros.py index ba35872bd98..2a05bd91680 100644 --- a/dimos/robot/unitree/go2/blueprints/smart/unitree_go2_ros.py +++ b/dimos/robot/unitree/go2/blueprints/smart/unitree_go2_ros.py @@ -13,10 +13,10 @@ # See the License for the specific language governing permissions and # limitations under the License. +from dimos_generated.geometry_msgs.msg import PoseStamped +from dimos_generated.sensor_msgs.msg import Image, PointCloud2 + from dimos.core.transport import ROSTransport -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.sensor_msgs.Image import Image -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 from dimos.robot.unitree.go2.blueprints.smart.unitree_go2 import unitree_go2 unitree_go2_ros = unitree_go2.transports( diff --git a/dimos/robot/unitree/go2/blueprints/teleop_webxr.py b/dimos/robot/unitree/go2/blueprints/teleop_webxr.py index 7d8325a52fb..c170213f605 100644 --- a/dimos/robot/unitree/go2/blueprints/teleop_webxr.py +++ b/dimos/robot/unitree/go2/blueprints/teleop_webxr.py @@ -15,11 +15,12 @@ """WebXR velocity teleoperation and headset video for Go2.""" +from dimos_generated.geometry_msgs.msg import Twist +from dimos_generated.sensor_msgs.msg import Image + from dimos.constants import DEFAULT_CAPACITY_COLOR_IMAGE from dimos.core.coordination.blueprints import autoconnect from dimos.core.transport import LCMTransport, pSHMTransport -from dimos.msgs.geometry_msgs.Twist import Twist -from dimos.msgs.sensor_msgs.Image import Image from dimos.robot.unitree.go2.connection import GO2Connection from dimos.teleop.webxr.extensions import Go2TeleopModule diff --git a/dimos/robot/unitree/go2/blueprints/test_tf_topology.py b/dimos/robot/unitree/go2/blueprints/test_tf_topology.py index eef20aa954c..e6693b0a18e 100644 --- a/dimos/robot/unitree/go2/blueprints/test_tf_topology.py +++ b/dimos/robot/unitree/go2/blueprints/test_tf_topology.py @@ -22,11 +22,12 @@ mid360_link so it never writes the frame PointLio owns. """ +from dimos_generated.geometry_msgs.msg import PoseStamped +from dimos_generated.std_msgs.msg import Header import pytest from dimos.core.coordination.blueprints import Blueprint from dimos.hardware.sensors.lidar.pointlio.module import PointLio -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped from dimos.robot.unitree.go2.blueprints.basic.unitree_go2_mid360_record import ( unitree_go2_mid360_record, ) @@ -44,7 +45,7 @@ def _tf_children_by_publisher(blueprint: Blueprint) -> dict[str, set[str]]: """Child frames each tf publisher the blueprint actually enables will write.""" - odom = PoseStamped(ts=1.0, frame_id="go2_odom") + odom = PoseStamped(header=Header(frame_id="go2_odom")) children: dict[str, set[str]] = {} for atom in blueprint.blueprints: if atom.module is GO2Connection and atom.kwargs.get("publish_tf", True): diff --git a/dimos/robot/unitree/go2/camera_calibration.py b/dimos/robot/unitree/go2/camera_calibration.py new file mode 100644 index 00000000000..7a79c10b353 --- /dev/null +++ b/dimos/robot/unitree/go2/camera_calibration.py @@ -0,0 +1,28 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""The packaged Go2 front-camera calibration, independent of its robot connection.""" + +from importlib import resources + +from dimos_generated.sensor_msgs.msg import CameraInfo +from dimos_generated.std_msgs.msg import Header + +from dimos.msgs.camera_info import camera_info_from_yaml + + +def front_camera_calibration() -> CameraInfo: + resource = resources.files("dimos.robot.unitree.go2").joinpath("front_camera_720.yaml") + with resources.as_file(resource) as path: + return camera_info_from_yaml(path, header=Header(frame_id="camera_optical")) diff --git a/dimos/robot/unitree/go2/connection.py b/dimos/robot/unitree/go2/connection.py index c6d132f4acb..2a6a0c51997 100644 --- a/dimos/robot/unitree/go2/connection.py +++ b/dimos/robot/unitree/go2/connection.py @@ -14,12 +14,22 @@ import copy from enum import Enum -from importlib import resources import sys from threading import Thread import time from typing import Any, Protocol +from dimos_generated.geometry_msgs.msg import ( + PoseStamped, + Quaternion, + Transform, + TransformStamped, + Twist, + Vector3, +) +from dimos_generated.sensor_msgs.msg import CameraInfo, Image, PointCloud2 +from dimos_generated.std_msgs.msg import Header +from dimos_generated.tf2_msgs.msg import TFMessage from pydantic import Field from reactivex import empty from reactivex.disposable import Disposable @@ -34,16 +44,10 @@ from dimos.core.stream import In, Out from dimos.memory.replay import Replay, ReplayStream, resolve_db_path from dimos.memory.store.sqlite import SqliteStore -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Transform import Transform -from dimos.msgs.geometry_msgs.Twist import Twist -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo -from dimos.msgs.sensor_msgs.Image import Image -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 -from dimos.msgs.tf2_msgs.TFMessage import TFMessage +from dimos.msgs.geometry import compose_transforms, transform_from_pose +from dimos.msgs.time import time_from_nanoseconds from dimos.robot.unitree.connection import UnitreeWebRTCConnection +from dimos.robot.unitree.go2.camera_calibration import front_camera_calibration from dimos.robot.unitree.type.lowstate import LowStateMsg from dimos.spec.perception import Camera, Pointcloud from dimos.utils.decorators.decorators import cached_property, simple_mcache @@ -102,16 +106,6 @@ def switch_joystick(self, enable: bool = True) -> bool: ... def publish_request(self, topic: str, data: dict) -> dict: ... # type: ignore[type-arg] -_FRONT_CAMERA_720_YAML = resources.files("dimos.robot.unitree.go2").joinpath( - "front_camera_720.yaml" -) - - -def _camera_info_static() -> CameraInfo: - with resources.as_file(_FRONT_CAMERA_720_YAML) as yaml_path: - return CameraInfo.from_yaml(str(yaml_path)) - - def _prefixed(prefix: str | None, name: str) -> str: """Apply a TF namespace prefix (ModuleConfig.frame_id_prefix) to a frame name.""" if not prefix or not name: @@ -121,16 +115,17 @@ def _prefixed(prefix: str | None, name: str) -> str: # Static camera mount chain: base_link -> camera_link -> camera_optical. # TODO we need a standardized way to specify this for all cameras in dimos -BASE_TO_OPTICAL: Transform = Transform( - translation=Vector3(0.3, 0.0, 0.0), - rotation=Quaternion(0.0, 0.0, 0.0, 1.0), - frame_id="base_link", - child_frame_id="camera_link", -) + Transform( - translation=Vector3(0.0, 0.0, 0.0), - rotation=Quaternion(-0.5, 0.5, -0.5, 0.5), - frame_id="camera_link", - child_frame_id="camera_optical", +BASE_TO_OPTICAL = compose_transforms( + TransformStamped( + header=Header(frame_id="base_link"), + child_frame_id="camera_link", + transform=Transform(translation=Vector3(x=0.3)), + ), + TransformStamped( + header=Header(frame_id="camera_link"), + child_frame_id="camera_optical", + transform=Transform(rotation=Quaternion(x=-0.5, y=0.5, z=-0.5, w=0.5)), + ), ) @@ -279,7 +274,7 @@ class GO2Connection(Module, Camera, Pointcloud): tf: Out[TFMessage] connection: Go2ConnectionProtocol - camera_info_static: CameraInfo = _camera_info_static() + camera_info_static: CameraInfo = front_camera_calibration() _camera_info_thread: Thread | None = None _latest_lowstate: LowStateMsg | None = None @@ -307,11 +302,11 @@ def __init__(self, **kwargs: Any) -> None: if hasattr(self.connection, "camera_info_static"): self.camera_info_static = self.connection.camera_info_static - if self.config.frame_id_prefix and self.camera_info_static.frame_id: + if self.config.frame_id_prefix and self.camera_info_static.header.frame_id: # Copy so the class-level default is not mutated. self.camera_info_static = copy.copy(self.camera_info_static) - self.camera_info_static.frame_id = _prefixed( - self.config.frame_id_prefix, self.camera_info_static.frame_id + self.camera_info_static.header.frame_id = _prefixed( + self.config.frame_id_prefix, self.camera_info_static.header.frame_id ) @rpc @@ -322,7 +317,7 @@ def start(self) -> None: self.connection.start() def onimage(image: Image) -> None: - image.frame_id = _prefixed(self.config.frame_id_prefix, image.frame_id) + image.header.frame_id = _prefixed(self.config.frame_id_prefix, image.header.frame_id) self.color_image.publish(image) if self.config.lidar: @@ -371,42 +366,36 @@ def stop(self) -> None: super().stop() @classmethod - def _odom_to_tf(cls, odom: PoseStamped, prefix: str = "") -> list[Transform]: - # The odom parent frame (odom.frame_id) stays unprefixed so namespaced - # robots still hang off one shared tree root. - camera_link = Transform( - translation=Vector3(0.3, 0.0, 0.0), - rotation=Quaternion(0.0, 0.0, 0.0, 1.0), - frame_id=_prefixed(prefix, "base_link"), - child_frame_id=_prefixed(prefix, "camera_link"), - ts=odom.ts, - ) - - camera_optical = Transform( - translation=Vector3(0.0, 0.0, 0.0), - rotation=Quaternion(-0.5, 0.5, -0.5, 0.5), - frame_id=_prefixed(prefix, "camera_link"), - child_frame_id=_prefixed(prefix, "camera_optical"), - ts=odom.ts, - ) - + def _odom_to_tf(cls, odom: PoseStamped, prefix: str = "") -> list[TransformStamped]: + # The odometry parent stays global; only robot-local frames get a prefix. return [ - Transform.from_pose(_prefixed(prefix, "base_link"), odom), - camera_link, - camera_optical, + transform_from_pose(odom, child_frame_id=_prefixed(prefix, "base_link")), + TransformStamped( + header=Header(stamp=odom.header.stamp, frame_id=_prefixed(prefix, "base_link")), + child_frame_id=_prefixed(prefix, "camera_link"), + transform=Transform(translation=Vector3(x=0.3)), + ), + TransformStamped( + header=Header(stamp=odom.header.stamp, frame_id=_prefixed(prefix, "camera_link")), + child_frame_id=_prefixed(prefix, "camera_optical"), + transform=Transform(rotation=Quaternion(x=-0.5, y=0.5, z=-0.5, w=0.5)), + ), ] def _publish_tf(self, msg: PoseStamped) -> None: - msg.frame_id = self.config.odom_frame_id + message = copy.copy(msg) + message.header.frame_id = self.config.odom_frame_id if self.config.publish_tf: - transforms = self._odom_to_tf(msg, prefix=self.config.frame_id_prefix or "") - self.tf.publish(TFMessage(*transforms)) + transforms = self._odom_to_tf(message, prefix=self.config.frame_id_prefix or "") + self.tf.publish(TFMessage(transforms=transforms)) if self.odom.transport: - self.odom.publish(msg) + self.odom.publish(message) def publish_camera_info(self) -> None: while True: - self.camera_info.publish(self.camera_info_static.with_ts(time.time())) + message = copy.copy(self.camera_info_static) + message.header.stamp = time_from_nanoseconds(time.time_ns()) + self.camera_info.publish(message) time.sleep(1.0) @rpc diff --git a/dimos/robot/unitree/go2/dds/cli/render.py b/dimos/robot/unitree/go2/dds/cli/render.py index 7711e9cf0b7..0eae19fbb10 100644 --- a/dimos/robot/unitree/go2/dds/cli/render.py +++ b/dimos/robot/unitree/go2/dds/cli/render.py @@ -34,26 +34,42 @@ import subprocess from typing import TYPE_CHECKING, Any, cast +from dimos_generated.geometry_msgs.msg import ( + Point, + Pose, + PoseStamped, + Quaternion, + TransformStamped, + Twist, + TwistStamped, + Vector3, +) +from dimos_generated.nav_msgs.msg import Path +from dimos_generated.std_msgs.msg import Header import numpy as np +from scipy.spatial.transform import Rotation import typer from dimos.memory.transform import throttle from dimos.memory.utils.progress import progress -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.geometry_msgs.Transform import Transform -from dimos.msgs.geometry_msgs.TwistStamped import TwistStamped -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.msgs.nav_msgs.Path import Path +from dimos.msgs.geometry import compose_transforms, transform_from_matrix +from dimos.msgs.pointcloud import transform_cloud +from dimos.msgs.time import time_from_seconds from dimos.robot.unitree.go2.dds.extrinsics import LIDAR_TO_BASE from dimos.robot.unitree.go2.dds.msgs.SportModeState import SportModeState from dimos.robot.unitree.go2.dds.store import Go2McapStore +from dimos.visualization.rerun.message_helpers import ( + cloud_archetype, + image_archetype, + navigation_archetype, +) if TYPE_CHECKING: from collections.abc import Iterator + from dimos_generated.sensor_msgs.msg import Image, PointCloud2 + from dimos.memory.type.observation import Observation - from dimos.msgs.sensor_msgs.Image import Image - from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 WORLD = "world" @@ -61,8 +77,8 @@ def world_accel(obs: Observation[Any]) -> np.ndarray: """IMU linear acceleration rotated into the world frame (includes gravity).""" la = obs.data.linear_acceleration - a = obs.data.orientation.rotate_vector(Vector3(la.x, la.y, la.z)) - return np.array([float(a.x), float(a.y), float(a.z)]) + q = obs.data.orientation + return np.asarray(Rotation.from_quat([q.x, q.y, q.z, q.w]).apply([la.x, la.y, la.z])) def gravity_bias( @@ -109,10 +125,11 @@ def sportmode_pose(obs: Observation[SportModeState]) -> PoseStamped: sm = obs.data w, x, y, z = (float(v) for v in sm.imu_state.quaternion) # Unitree order: wxyz return PoseStamped( - ts=obs.ts, - frame_id=WORLD, - position=[float(v) for v in sm.position], - orientation=[x, y, z, w], + header=Header(frame_id=WORLD, stamp=time_from_seconds(obs.ts)), + pose=Pose( + position=Point(x=sm.position[0], y=sm.position[1], z=sm.position[2]), + orientation=Quaternion(x=x, y=y, z=z, w=w), + ), ) @@ -127,7 +144,8 @@ def step(state: Any, obs: Observation[Any]) -> tuple[Any, TwistStamped]: if prev is not None: vel = vel + a_world * (obs.ts - prev) twist = TwistStamped( - ts=obs.ts, frame_id=WORLD, linear=vel.tolist(), angular=[0.0, 0.0, 0.0] + header=Header(frame_id=WORLD, stamp=time_from_seconds(obs.ts)), + twist=Twist(linear=Vector3(x=vel[0], y=vel[1], z=vel[2])), ) return (vel, obs.ts), twist @@ -137,20 +155,21 @@ def step(state: Any, obs: Observation[Any]) -> tuple[Any, TwistStamped]: def integrate_position(state: Any, obs: Observation[Any]) -> tuple[Any, PoseStamped]: """scan_data: TwistStamped -> PoseStamped (velocity integrated to position).""" pos, prev = state - v = obs.data.linear + v = obs.data.twist.linear if prev is not None: pos = pos + np.array([v.x, v.y, v.z]) * (obs.ts - prev) pose = PoseStamped( - ts=obs.ts, frame_id=WORLD, position=pos.tolist(), orientation=[0.0, 0.0, 0.0, 1.0] + header=Header(frame_id=WORLD, stamp=time_from_seconds(obs.ts)), + pose=Pose(position=Point(x=pos[0], y=pos[1], z=pos[2]), orientation=Quaternion(w=1)), ) return (pos, obs.ts), pose def accumulate_path(upstream: Iterator[Observation[PoseStamped]]) -> Iterator[Observation[Path]]: """transform: yield the growing nav_msgs/Path as each pose streams in.""" - path = Path(frame_id=WORLD) + path = Path(header=Header(frame_id=WORLD)) for obs in upstream: - path = path.push(obs.data) + path = Path(header=obs.data.header, poses=[*path.poses, obs.data]) yield obs.derive(data=path) @@ -160,11 +179,13 @@ def leg_odom(store: Go2McapStore, seconds: float | None) -> None: def log_pose(obs: Observation[PoseStamped]) -> None: rr.set_time("time", timestamp=obs.ts) - rr.log("world/leg_odom", obs.data.to_rerun(), rr.TransformAxes3D(axis_length=0.2)) + rr.log( + "world/leg_odom", navigation_archetype(obs.data), rr.TransformAxes3D(axis_length=0.2) + ) def log_path(obs: Observation[Path]) -> None: rr.set_time("time", timestamp=obs.ts) - rr.log("world/leg_odom_path", obs.data.to_rerun()) + rr.log("world/leg_odom_path", navigation_archetype(obs.data)) src = store.streams.sportmodestate.to_time(seconds) with progress(src.count(), "leg_odom") as bar: @@ -185,7 +206,7 @@ def imu_odom(store: Go2McapStore, seconds: float | None) -> None: def log_path(obs: Observation[Path]) -> None: rr.set_time("time", timestamp=obs.ts) - rr.log("world/imu_odom_path", obs.data.to_rerun(color=(220, 90, 90))) + rr.log("world/imu_odom_path", navigation_archetype(obs.data, color=(220, 90, 90))) gravity = gravity_bias(store) # calibrate gravity+bias on the stationary start src = store.streams.imu.to_time(seconds) @@ -207,10 +228,17 @@ def lidar(store: Go2McapStore, seconds: float | None) -> None: def log_lidar(obs: Observation[PointCloud2]) -> None: rr.set_time("time", timestamp=obs.ts) - rr.log("world/leg_odom/lidar", obs.data.to_rerun()) + rr.log("world/leg_odom/lidar", cloud_archetype(obs.data)) src = store.streams.lidar.to_time(seconds) - rr.log("world/leg_odom/lidar", LIDAR_TO_BASE.to_rerun(frameless=True), static=True) + t, q = LIDAR_TO_BASE.transform.translation, LIDAR_TO_BASE.transform.rotation + rr.log( + "world/leg_odom/lidar", + rr.Transform3D( + translation=[t.x, t.y, t.z], quaternion=rr.Quaternion(xyzw=[q.x, q.y, q.z, q.w]) + ), + static=True, + ) with progress(src.count(), "lidar") as bar: src.tap(bar).tap(log_lidar).drain() @@ -248,15 +276,19 @@ def world_lidar(store: Go2McapStore, seconds: float | None) -> None: def to_world(obs: Observation[PointCloud2]) -> PointCloud2: p, q = _interp_pose(tt, pos, quat, obs.ts) - b2w = Transform.from_pose( - WORLD, - PoseStamped(ts=obs.ts, frame_id=WORLD, position=p.tolist(), orientation=q.tolist()), + matrix = np.eye(4) + matrix[:3, :3] = Rotation.from_quat(q).as_matrix() + matrix[:3, 3] = p + b2w = TransformStamped( + header=Header(frame_id=WORLD, stamp=obs.data.header.stamp), + child_frame_id=ext.header.frame_id, + transform=transform_from_matrix(matrix), ) - return obs.data.transform(b2w.apply(ext)) # lidar -> base -> world + return transform_cloud(obs.data, compose_transforms(b2w, ext)) def log_voxels(obs: Observation[PointCloud2]) -> None: rr.set_time("time", timestamp=obs.ts) - rr.log("world/world_lidar", obs.data.to_rerun()) + rr.log("world/world_lidar", cloud_archetype(obs.data)) src = store.streams.lidar.to_time(seconds) with progress(src.count(), "world_lidar") as bar: @@ -281,7 +313,7 @@ def log_image(obs: Observation[Image]) -> None: if obs.data is None: # truncated/corrupt frame return rr.set_time("time", timestamp=obs.ts) - rr.log("world/camera", obs.data.to_rerun()) + rr.log("world/camera", image_archetype(obs.data)) src = store.streams.color_image.to_time(seconds) with progress(src.count(), "camera") as bar: diff --git a/dimos/robot/unitree/go2/dds/cli/test_render_generated.py b/dimos/robot/unitree/go2/dds/cli/test_render_generated.py new file mode 100644 index 00000000000..864a5565d67 --- /dev/null +++ b/dimos/robot/unitree/go2/dds/cli/test_render_generated.py @@ -0,0 +1,106 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +# Copyright 2026 Dimensional Inc. +# SPDX-License-Identifier: Apache-2.0 + +import math +from types import SimpleNamespace + +from dimos_generated.geometry_msgs.msg import Point, Pose, PoseStamped, Quaternion, Vector3 +from dimos_generated.sensor_msgs.msg import Imu +from dimos_generated.std_msgs.msg import Header +import numpy as np + +from dimos.memory.type.observation import Observation +from dimos.msgs.geometry import transform_matrix +from dimos.msgs.pointcloud import pointcloud_from_xyz, pointcloud_xyz, transform_cloud +from dimos.msgs.time import time_from_seconds +from dimos.robot.unitree.go2.dds.cli.render import ( + accumulate_path, + integrate_position, + integrate_velocity, + sportmode_pose, + world_accel, +) +from dimos.robot.unitree.go2.dds.extrinsics import EXT_R, EXT_T, LIDAR_TO_BASE + + +def test_world_acceleration_and_integrators_keep_world_axes_and_clock(): + imu = Imu( + orientation=Quaternion(z=math.sqrt(0.5), w=math.sqrt(0.5)), + linear_acceleration=Vector3(x=1, z=9.81), + ) + observations = [Observation(ts=ts, _data=imu) for ts in (2.0, 3.0)] + np.testing.assert_allclose(world_accel(observations[0]), [0, 1, 9.81], atol=1e-12) + velocity_step = integrate_velocity(np.array([0.0, 0.0, 9.81])) + state = (np.zeros(3), None) + twists = [] + for obs in observations: + state, twist = velocity_step(state, obs) + twists.append(Observation(ts=obs.ts, _data=twist)) + assert twist.header.stamp == time_from_seconds(obs.ts) + assert twist.header.frame_id == "world" + position_state = (np.zeros(3), None) + for obs in twists: + position_state, pose = integrate_position(position_state, obs) + np.testing.assert_allclose( + [pose.pose.position.x, pose.pose.position.y, pose.pose.position.z], [0, 1, 0], atol=1e-12 + ) + assert pose.header.stamp == time_from_seconds(3.0) + + +def test_path_accumulation_keeps_previous_snapshot_and_exact_pose_headers(): + poses = [ + PoseStamped( + header=Header(frame_id="world", stamp=time_from_seconds(ts)), + pose=Pose(position=Point(x=ts), orientation=Quaternion(w=1)), + ) + for ts in (1.0, 2.0) + ] + result = list( + accumulate_path( + iter([Observation(ts=ts, _data=p) for ts, p in zip((1.0, 2.0), poses, strict=True)]) + ) + ) + assert [len(obs.data.poses) for obs in result] == [1, 2] + assert result[1].data.header == poses[1].header + assert result[0].data.poses[0].encode() == poses[0].encode() + + +def test_sportmode_wxyz_is_converted_to_generated_xyzw(): + sm = SimpleNamespace( + position=[1.0, 2.0, 3.0], imu_state=SimpleNamespace(quaternion=[0.5, -0.5, 0.5, -0.5]) + ) + pose = sportmode_pose(Observation(ts=4.25, _data=sm)) + assert pose.header.stamp == time_from_seconds(4.25) + q = pose.pose.orientation + assert (q.x, q.y, q.z, q.w) == (-0.5, 0.5, -0.5, 0.5) + assert PoseStamped.decode(pose.encode()).pose.position.y == 2.0 + + +def test_measured_lidar_mount_retains_coordinates_and_cloud_stamp(): + cloud = pointcloud_from_xyz( + np.array([[1.0, 0.0, 0.0]]), header=Header(frame_id="lidar", stamp=time_from_seconds(12.25)) + ) + original = cloud.encode() + transformed = transform_cloud(cloud, LIDAR_TO_BASE) + matrix = transform_matrix(LIDAR_TO_BASE.transform) + np.testing.assert_allclose(matrix[:3, :3], EXT_R, atol=1e-6) + np.testing.assert_allclose( + pointcloud_xyz(transformed), (matrix[:3, :3] @ [1.0, 0.0, 0.0] + EXT_T)[None, :], atol=1e-6 + ) + assert transformed.header.stamp == cloud.header.stamp + assert transformed.header.frame_id == "base_link" + assert cloud.encode() == original diff --git a/dimos/robot/unitree/go2/dds/codec.py b/dimos/robot/unitree/go2/dds/codec.py index ed645d77e04..0d3b136698a 100644 --- a/dimos/robot/unitree/go2/dds/codec.py +++ b/dimos/robot/unitree/go2/dds/codec.py @@ -31,12 +31,11 @@ import json from typing import Any, Protocol, runtime_checkable -from dimos.msgs.nav_msgs.Odometry import Odometry -from dimos.msgs.sensor_msgs.Image import Image -from dimos.msgs.sensor_msgs.Imu import Imu -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 +from dimos_generated.foxglove_msgs.msg import CompressedVideo +from dimos_generated.nav_msgs.msg import Odometry +from dimos_generated.sensor_msgs.msg import CompressedImage, Imu, PointCloud2 + from dimos.robot.unitree.go2.dds import cdr, ros -from dimos.robot.unitree.go2.dds.msgs.CompressedVideo import CompressedVideo from dimos.robot.unitree.go2.dds.msgs.ControlEvent import ControlEvent from dimos.robot.unitree.go2.dds.msgs.HeightMap import HeightMap from dimos.robot.unitree.go2.dds.msgs.LowCmd import LowCmd @@ -114,7 +113,7 @@ def encode(self, msg: Any) -> bytes: "rt/utlidar/height_map_array": FnCodec(HeightMap, ros.decode_height_map), "rt/utlidar/imu": FnCodec(Imu, ros.decode_imu), "rt/utlidar/robot_odom": FnCodec(Odometry, ros.decode_odometry), - "rt/frontvideo": FnCodec(Image, ros.decode_compressed_image), + "rt/frontvideo": FnCodec(CompressedImage, ros.decode_compressed_image), "rt/frontvideo/h264": FnCodec(CompressedVideo, ros.decode_compressed_video), "rt/lowstate": CdrStructCodec(LowState), "rt/lowcmd": CdrStructCodec(LowCmd), diff --git a/dimos/robot/unitree/go2/dds/extrinsics.py b/dimos/robot/unitree/go2/dds/extrinsics.py index 8db4f797534..10d5002c601 100644 --- a/dimos/robot/unitree/go2/dds/extrinsics.py +++ b/dimos/robot/unitree/go2/dds/extrinsics.py @@ -24,11 +24,12 @@ [yaw -57.9, pitch -8.1, roll -167.3] (agrees to a few degrees). """ +from dimos_generated.geometry_msgs.msg import Quaternion, Transform, TransformStamped, Vector3 +from dimos_generated.std_msgs.msg import Header import numpy as np +from scipy.spatial.transform import Rotation -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Transform import Transform -from dimos.msgs.geometry_msgs.Vector3 import Vector3 +from dimos.msgs.geometry import quaternion_from_matrix # base_link <- lidar rotation (lidar points -> base frame: p_base = EXT_R @ p_lidar + EXT_T) EXT_R = np.array( @@ -46,16 +47,21 @@ CAM_T = np.array([0.30, 0.0, 0.0], dtype=np.float64) CAM_Q = np.array([-0.5, 0.5, -0.5, 0.5], dtype=np.float64) # xyzw -# Same mounts as standard Transform msgs (typed; carry frame ids; have to_rerun). -LIDAR_TO_BASE = Transform( - translation=Vector3(EXT_T), - rotation=Quaternion.from_rotation_matrix(EXT_R), - frame_id="base_link", +# Normalize the measured six-decimal rotation with the same SciPy conversion +# as the previous numeric helper, then publish generated frame-labelled values. +LIDAR_TO_BASE = TransformStamped( + header=Header(frame_id="base_link"), child_frame_id="lidar", + transform=Transform( + translation=Vector3(x=EXT_T[0], y=EXT_T[1], z=EXT_T[2]), + rotation=quaternion_from_matrix(Rotation.from_matrix(EXT_R).as_matrix()), + ), ) -BASE_TO_CAMERA = Transform( - translation=Vector3(CAM_T), - rotation=Quaternion(CAM_Q), - frame_id="base_link", +BASE_TO_CAMERA = TransformStamped( + header=Header(frame_id="base_link"), child_frame_id="camera_optical", + transform=Transform( + translation=Vector3(x=CAM_T[0], y=CAM_T[1], z=CAM_T[2]), + rotation=Quaternion(x=CAM_Q[0], y=CAM_Q[1], z=CAM_Q[2], w=CAM_Q[3]), + ), ) diff --git a/dimos/robot/unitree/go2/dds/ros.py b/dimos/robot/unitree/go2/dds/ros.py index c9174a78c63..4d7c263a2e8 100644 --- a/dimos/robot/unitree/go2/dds/ros.py +++ b/dimos/robot/unitree/go2/dds/ros.py @@ -12,276 +12,46 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""Decode the standard-ROS DDS channels into ``dimos.msgs`` types. +"""Generated ROS2 DDS codecs with explicit Unitree firmware adaptations. -The wire layouts are declared as ``__cdr_fields__`` specs and walked by the -generic :mod:`cdr` decoder (no hand-rolled byte cursors). A thin per-type -adapter maps the decoded wire struct into the dimos message — only the parts -that genuinely differ from a field copy (point-buffer reinterpretation, jpeg -decode, pose nesting) live there. +Standard payloads retain all generated fields, exact stamps and arbitrary +point layouts. The device's Imu quaternion uses wxyz and is reordered only at +this Unitree boundary. HeightMap remains a device-specific wire structure. """ -# TODO this file needs to go away, dimos/msgs are structurally the same as -# these messages here so we will write an automatic translator, temporary so -# we can iterate on go2 dds research, see if it's viable at all -# -# TODO pointcloud has timestamps and intensities, we drop those on LCM round trip -# and our pointcloud2 message doesn't support arbitrary fields per point, we need -# to implement those - -from __future__ import annotations - from dataclasses import dataclass +from dimos_generated.foxglove_msgs.msg import CompressedVideo +from dimos_generated.geometry_msgs.msg import Quaternion +from dimos_generated.nav_msgs.msg import Odometry +from dimos_generated.sensor_msgs.msg import CompressedImage, Imu, PointCloud2 import numpy as np -from dimos.msgs.geometry_msgs.Pose import Pose -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Twist import Twist -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.msgs.nav_msgs.Odometry import Odometry -from dimos.msgs.sensor_msgs.Image import Image, ImageFormat -from dimos.msgs.sensor_msgs.Imu import Imu -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 from dimos.robot.unitree.go2.dds import cdr -from dimos.robot.unitree.go2.dds.msgs.CompressedVideo import CompressedVideo from dimos.robot.unitree.go2.dds.msgs.HeightMap import HeightMap -# Shared wire specs (Header/Time) reused by the per-message layouts below. -@dataclass -class _Time: - sec: int - nanosec: int - - __cdr_fields__ = [("sec", "i32"), ("nanosec", "u32")] - - -@dataclass -class _Header: - stamp: _Time - frame_id: str - - __cdr_fields__ = [("stamp", _Time), ("frame_id", "string")] - - -def _ts(h: _Header) -> float: - return h.stamp.sec + h.stamp.nanosec * 1e-9 - - -# sensor_msgs/Imu -@dataclass -class _ImuWire: - header: _Header - orientation: np.ndarray # f64[4] xyzw - orientation_covariance: np.ndarray # f64[9] - angular_velocity: np.ndarray # f64[3] - angular_velocity_covariance: np.ndarray - linear_acceleration: np.ndarray # f64[3] - linear_acceleration_covariance: np.ndarray - - __cdr_fields__ = [ - ("header", _Header), - ("orientation", ("array", "f64", 4)), - ("orientation_covariance", ("array", "f64", 9)), - ("angular_velocity", ("array", "f64", 3)), - ("angular_velocity_covariance", ("array", "f64", 9)), - ("linear_acceleration", ("array", "f64", 3)), - ("linear_acceleration_covariance", ("array", "f64", 9)), - ] - - def decode_imu(buf: bytes) -> Imu: - w: _ImuWire = cdr.decode(buf, _ImuWire)[0] - # Unitree fills orientation wxyz even in this sensor_msgs/Imu — reorder to xyzw - # (verified: rotating accel by this lands gravity on +z in leg-odom-stationary windows). - qw, qx, qy, qz = (float(v) for v in w.orientation) - return Imu( - orientation=Quaternion(qx, qy, qz, qw), - angular_velocity=Vector3(w.angular_velocity.tolist()), - linear_acceleration=Vector3(w.linear_acceleration.tolist()), - orientation_covariance=w.orientation_covariance.tolist(), - angular_velocity_covariance=w.angular_velocity_covariance.tolist(), - linear_acceleration_covariance=w.linear_acceleration_covariance.tolist(), - frame_id=w.header.frame_id, - ts=_ts(w.header), - ) - - -# nav_msgs/Odometry -@dataclass -class _PoseWire: - position: np.ndarray # f64[3] - orientation: np.ndarray # f64[4] xyzw - - __cdr_fields__ = [("position", ("array", "f64", 3)), ("orientation", ("array", "f64", 4))] - - -@dataclass -class _PoseWithCov: - pose: _PoseWire - covariance: np.ndarray # f64[36] - - __cdr_fields__ = [("pose", _PoseWire), ("covariance", ("array", "f64", 36))] - - -@dataclass -class _TwistWire: - linear: np.ndarray # f64[3] - angular: np.ndarray # f64[3] - - __cdr_fields__ = [("linear", ("array", "f64", 3)), ("angular", ("array", "f64", 3))] - - -@dataclass -class _TwistWithCov: - twist: _TwistWire - covariance: np.ndarray # f64[36] - - __cdr_fields__ = [("twist", _TwistWire), ("covariance", ("array", "f64", 36))] - - -@dataclass -class _OdomWire: - header: _Header - child_frame_id: str - pose: _PoseWithCov - twist: _TwistWithCov - - __cdr_fields__ = [ - ("header", _Header), - ("child_frame_id", "string"), - ("pose", _PoseWithCov), - ("twist", _TwistWithCov), - ] + message = Imu.decode(buf) + firmware = message.orientation + message.orientation = Quaternion(x=firmware.y, y=firmware.z, z=firmware.w, w=firmware.x) + return message def decode_odometry(buf: bytes) -> Odometry: - w: _OdomWire = cdr.decode(buf, _OdomWire)[0] - pose = Pose() - pose.position = Vector3(w.pose.pose.position.tolist()) - pose.orientation = Quaternion(*w.pose.pose.orientation.tolist()) - twist = Twist() - twist.linear = Vector3(w.twist.twist.linear.tolist()) - twist.angular = Vector3(w.twist.twist.angular.tolist()) - return Odometry( - ts=_ts(w.header), - frame_id=w.header.frame_id, - child_frame_id=w.child_frame_id, - pose=pose, - twist=twist, - ) - - -# sensor_msgs/PointCloud2 -@dataclass -class _PointField: - name: str - offset: int - datatype: int - count: int - - __cdr_fields__ = [ - ("name", "string"), - ("offset", "u32"), - ("datatype", "u8"), - ("count", "u32"), - ] - - -@dataclass -class _Pc2Wire: - header: _Header - height: int - width: int - fields: list[_PointField] - is_bigendian: int - point_step: int - row_step: int - data: np.ndarray # u8[] - is_dense: int - - __cdr_fields__ = [ - ("header", _Header), - ("height", "u32"), - ("width", "u32"), - ("fields", ("seq", _PointField)), - ("is_bigendian", "u8"), - ("point_step", "u32"), - ("row_step", "u32"), - ("data", ("seq", "u8")), - ("is_dense", "u8"), - ] - - -# ROS PointField datatype code -> numpy dtype -_PF_DT = {1: " PointCloud2: - w: _Pc2Wire = cdr.decode(buf, _Pc2Wire)[0] - ts, frame = _ts(w.header), w.header.frame_id - if w.point_step == 0 or w.data.size < w.point_step: - return PointCloud2.from_numpy(np.empty((0, 3), np.float32), frame, ts) - dt = np.dtype( - { - "names": [f.name for f in w.fields], - "formats": [_PF_DT[f.datatype] for f in w.fields], - "offsets": [f.offset for f in w.fields], - "itemsize": w.point_step, - } - ) - arr = w.data.view(dt) - xyz = np.stack([arr["x"], arr["y"], arr["z"]], axis=-1).astype(np.float32) - inten = arr["intensity"].astype(np.float32) if dt.names and "intensity" in dt.names else None - return PointCloud2.from_numpy(xyz, frame, ts, inten) - - -# sensor_msgs/CompressedImage -@dataclass -class _CImgWire: - header: _Header - format: str - data: np.ndarray # u8[] - - __cdr_fields__ = [ - ("header", _Header), - ("format", "string"), - ("data", ("seq", "u8")), - ] + return PointCloud2.decode(buf) -def decode_compressed_image(buf: bytes) -> Image | None: - import cv2 - - w: _CImgWire = cdr.decode(buf, _CImgWire)[0] - bgr = cv2.imdecode(w.data, cv2.IMREAD_COLOR) - if bgr is None: - return None - return Image.from_numpy(bgr, ImageFormat.BGR, w.header.frame_id, _ts(w.header)) - - -# foxglove_msgs/CompressedVideo (field order differs from CompressedImage: -# timestamp, frame_id, data, format). -@dataclass -class _CVideoWire: - stamp: _Time - frame_id: str - data: np.ndarray # u8[] - format: str - - __cdr_fields__ = [ - ("stamp", _Time), - ("frame_id", "string"), - ("data", ("seq", "u8")), - ("format", "string"), - ] +def decode_compressed_image(buf: bytes) -> CompressedImage: + return CompressedImage.decode(buf) def decode_compressed_video(buf: bytes) -> CompressedVideo: - """Decode the CDR envelope only — the encoded packet is left for H264Decoder.""" - w: _CVideoWire = cdr.decode(buf, _CVideoWire)[0] - return CompressedVideo(data=w.data, format=w.format, frame_id=w.frame_id) + return CompressedVideo.decode(buf) # unitree_go/HeightMap (rt/utlidar/height_map_array) diff --git a/dimos/robot/unitree/go2/dds/test_generated_codecs.py b/dimos/robot/unitree/go2/dds/test_generated_codecs.py new file mode 100644 index 00000000000..44930920549 --- /dev/null +++ b/dimos/robot/unitree/go2/dds/test_generated_codecs.py @@ -0,0 +1,111 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import struct + +from dimos_generated.builtin_interfaces.msg import Time +from dimos_generated.foxglove_msgs.msg import CompressedVideo +from dimos_generated.geometry_msgs.msg import Quaternion +from dimos_generated.nav_msgs.msg import Odometry +from dimos_generated.sensor_msgs.msg import CompressedImage, Imu, PointCloud2, PointField +from dimos_generated.std_msgs.msg import Header +import numpy as np +import pytest +from rosbags.typesys import Stores, get_typestore + +from dimos.memory.type.observation import Observation +from dimos.msgs.image import image_view +from dimos.msgs.pointcloud import pointcloud_xyz +from dimos.robot.unitree.go2.dds import ros +from dimos.robot.unitree.go2.dds.codec import GO2_CODECS +from dimos.robot.unitree.go2.dds.video import H264Decoder + + +@pytest.mark.parametrize("little_endian", [True, False]) +def test_odometry_preserves_independent_cdr_covariances_and_exact_stamp(little_endian): + source = Odometry( + header=Header(stamp=Time(sec=42, nanosec=123456789), frame_id="odom"), + child_frame_id="base_link", + ) + source.pose.pose.position.x = 1.25 + source.pose.covariance = list(range(36)) + source.twist.covariance = list(range(36, 72)) + reference = get_typestore(Stores.ROS2_JAZZY) + independent = reference.deserialize_cdr(source.encode(), source.msg_name) + payload = bytes( + reference.serialize_cdr(independent, source.msg_name, little_endian=little_endian) + ) + result = ros.decode_odometry(payload) + assert result.header.stamp.nanosec == 123456789 + assert result.header.frame_id == "odom" and result.child_frame_id == "base_link" + assert result.pose.pose.position.x == 1.25 + assert list(result.pose.covariance) == list(range(36)) + assert list(result.twist.covariance) == list(range(36, 72)) + + +def test_pointcloud_preserves_big_endian_fields_and_row_padding(): + source = PointCloud2( + header=Header(stamp=Time(sec=42, nanosec=123456789), frame_id="sensor"), + height=1, + width=1, + point_step=26, + row_step=34, + is_bigendian=True, + fields=[ + PointField(name=n, offset=i * 8, datatype=8, count=1) + for i, n in enumerate(["x", "y", "z"]) + ] + + [PointField(name="intensity", offset=24, datatype=4, count=1)], + data=struct.pack(">dddH", 1.25, -2.5, 3.0, 1234) + b"padding!", + ) + result = ros.decode_pointcloud2(source.encode()) + assert result.encode() == source.encode() + assert result.header.stamp.nanosec == 123456789 + np.testing.assert_array_equal(pointcloud_xyz(result), [[1.25, -2.5, 3.0]]) + + +def test_unitree_imu_reorders_firmware_quaternion_without_losing_metadata(): + source = Imu( + header=Header(stamp=Time(nanosec=123456789), frame_id="imu"), + orientation=Quaternion(x=0.5, y=0.1, z=0.2, w=0.3), + ) + source.orientation_covariance = list(range(9)) + result = ros.decode_imu(source.encode()) + q = result.orientation + assert (q.x, q.y, q.z, q.w) == (0.1, 0.2, 0.3, 0.5) + assert list(result.orientation_covariance) == list(range(9)) + assert result.header.stamp.nanosec == 123456789 + + +def test_compressed_image_stays_a_compressed_generated_message(): + source = CompressedImage( + header=Header(frame_id="camera"), format="jpeg", data=b"encoded-packet" + ) + result = GO2_CODECS["rt/frontvideo"].decode(source.encode()) + assert isinstance(result, CompressedImage) + assert result.encode() == source.encode() + + +def test_h264_emission_preserves_packet_stamp_and_pixel_layout(mocker): + stamp = Time(sec=42, nanosec=123456789) + packet = CompressedVideo(timestamp=stamp, frame_id="camera", format="h264", data=b"packet") + source = Observation(id=7, ts=99.0, data_type=CompressedVideo, _data=packet) + frame = mocker.Mock() + pixels = np.arange(18, dtype=np.uint8).reshape(2, 3, 3) + frame.to_ndarray.return_value = pixels + result = H264Decoder._emit(frame, source) + assert result.id == 7 and result.ts == 99.0 + assert result.data.encoding == "bgr8" and result.data.header.frame_id == "camera" + assert result.data.header.stamp.sec == 42 and result.data.header.stamp.nanosec == 123456789 + np.testing.assert_array_equal(image_view(result.data), pixels) diff --git a/dimos/robot/unitree/go2/dds/test_store.py b/dimos/robot/unitree/go2/dds/test_store.py index 296ca56e0ae..70831c198bc 100644 --- a/dimos/robot/unitree/go2/dds/test_store.py +++ b/dimos/robot/unitree/go2/dds/test_store.py @@ -21,13 +21,13 @@ import importlib.util +from dimos_generated.nav_msgs.msg import Odometry +from dimos_generated.sensor_msgs.msg import CompressedImage, Imu, PointCloud2 import numpy as np import pytest -from dimos.msgs.nav_msgs.Odometry import Odometry -from dimos.msgs.sensor_msgs.Image import Image -from dimos.msgs.sensor_msgs.Imu import Imu -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 +from dimos.msgs.image import image_from_compressed, image_view +from dimos.msgs.pointcloud import pointcloud_xyz from dimos.robot.unitree.go2.dds.msgs.ControlEvent import ControlEvent from dimos.robot.unitree.go2.dds.msgs.LowState import LowState from dimos.robot.unitree.go2.dds.msgs.SportModeState import SportModeState @@ -63,9 +63,9 @@ def test_lists_streams(store: Go2McapStore) -> None: def test_lidar(store: Go2McapStore) -> None: pc = store.streams.lidar.first().data - xyz = pc.points_f32() + xyz = pointcloud_xyz(pc) print( - f"\nPointCloud2: {xyz.shape[0]} pts frame={pc.frame_id!r} " + f"\nPointCloud2: {xyz.shape[0]} pts frame={pc.header.frame_id!r} " f"range=[{np.linalg.norm(xyz[:, :3], axis=1).min():.2f}, " f"{np.linalg.norm(xyz[:, :3], axis=1).max():.2f}] m" ) @@ -79,7 +79,7 @@ def test_imu(store: Go2McapStore) -> None: print( f"\nImu: |q|={np.linalg.norm([q.x, q.y, q.z, q.w]):.4f} " f"acc=({imu.linear_acceleration.x:.2f}, {imu.linear_acceleration.y:.2f}, " - f"{imu.linear_acceleration.z:.2f}) frame={imu.frame_id!r}" + f"{imu.linear_acceleration.z:.2f}) frame={imu.header.frame_id!r}" ) assert isinstance(imu, Imu) assert abs(imu.linear_acceleration.z) == pytest.approx(9.8, abs=0.5) # gravity @@ -90,7 +90,7 @@ def test_odom(store: Go2McapStore) -> None: p = odom.pose.pose.position print( f"\nOdometry: pos=({p.x:.2f}, {p.y:.2f}, {p.z:.2f}) " - f"{odom.frame_id!r} -> {odom.child_frame_id!r}" + f"{odom.header.frame_id!r} -> {odom.child_frame_id!r}" ) assert isinstance(odom, Odometry) assert odom.child_frame_id == "base_link" @@ -98,9 +98,9 @@ def test_odom(store: Go2McapStore) -> None: def test_color_image(store: Go2McapStore) -> None: img = store.streams.color_image.first().data - arr = img.as_numpy() - print(f"\nImage: {arr.shape} frame={img.frame_id!r}") - assert isinstance(img, Image) + arr = image_view(image_from_compressed(img)) + print(f"\nImage: {arr.shape} frame={img.header.frame_id!r}") + assert isinstance(img, CompressedImage) assert arr.ndim == 3 diff --git a/dimos/robot/unitree/go2/dds/video.py b/dimos/robot/unitree/go2/dds/video.py index 6b76617367c..0162a6c8eb7 100644 --- a/dimos/robot/unitree/go2/dds/video.py +++ b/dimos/robot/unitree/go2/dds/video.py @@ -36,9 +36,12 @@ from collections.abc import Iterator from typing import TYPE_CHECKING, Any +from dimos_generated.foxglove_msgs.msg import CompressedVideo +from dimos_generated.sensor_msgs.msg import Image +from dimos_generated.std_msgs.msg import Header + from dimos.memory.transform import Transformer -from dimos.msgs.sensor_msgs.Image import Image, ImageFormat -from dimos.robot.unitree.go2.dds.msgs.CompressedVideo import CompressedVideo +from dimos.msgs.image import image_from_array if TYPE_CHECKING: from dimos.memory.type.observation import Observation @@ -60,7 +63,7 @@ def __call__( continue last = obs try: - frames = decoder.decode(av.packet.Packet(packet.data.tobytes())) + frames = decoder.decode(av.packet.Packet(bytes(packet.data))) except av.error.FFmpegError: continue # P-frame with no reference yet (e.g. seeked past a keyframe) for frame in frames: @@ -75,4 +78,10 @@ def __call__( @staticmethod def _emit(frame: Any, obs: Observation[CompressedVideo]) -> Observation[Image]: bgr = frame.to_ndarray(format="bgr24") - return obs.derive(data=Image.from_numpy(bgr, ImageFormat.BGR, obs.data.frame_id, obs.ts)) + return obs.derive( + data=image_from_array( + bgr, + encoding="bgr8", + header=Header(stamp=obs.data.timestamp, frame_id=obs.data.frame_id), + ) + ) diff --git a/dimos/robot/unitree/go2/fleet_connection.py b/dimos/robot/unitree/go2/fleet_connection.py index 9f580b17c70..3e6649f0295 100644 --- a/dimos/robot/unitree/go2/fleet_connection.py +++ b/dimos/robot/unitree/go2/fleet_connection.py @@ -37,7 +37,7 @@ from typing_extensions import Self if TYPE_CHECKING: - from dimos.msgs.geometry_msgs.Twist import Twist + from dimos_generated.geometry_msgs.msg import Twist logger = setup_logger() diff --git a/dimos/robot/unitree/go2/go2_mid360_recorder.py b/dimos/robot/unitree/go2/go2_mid360_recorder.py index 5c2ab2f243d..bd1a3486749 100644 --- a/dimos/robot/unitree/go2/go2_mid360_recorder.py +++ b/dimos/robot/unitree/go2/go2_mid360_recorder.py @@ -23,12 +23,11 @@ from __future__ import annotations +from dimos_generated.geometry_msgs.msg import PoseStamped +from dimos_generated.sensor_msgs.msg import CameraInfo, Image, PointCloud2 + from dimos.core.stream import In from dimos.hardware.sensors.lidar.pointlio.recorder import PointlioRecorder -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo -from dimos.msgs.sensor_msgs.Image import Image -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 class Go2Mid360Recorder(PointlioRecorder): diff --git a/dimos/robot/unitree/go2/go2_mid360_static_transforms.py b/dimos/robot/unitree/go2/go2_mid360_static_transforms.py index 1158bbbc762..da411efa371 100644 --- a/dimos/robot/unitree/go2/go2_mid360_static_transforms.py +++ b/dimos/robot/unitree/go2/go2_mid360_static_transforms.py @@ -33,7 +33,9 @@ import math -from dimos.msgs.geometry_msgs.Transform import Transform +from dimos_generated.geometry_msgs.msg import TransformStamped + +from dimos.msgs.geometry import inverse_transform from dimos.protocol.tf.static_tf_publisher import ( FrameSpec, StaticTfPublisher, @@ -65,14 +67,18 @@ ] -def mount_transforms() -> list[Transform]: +def mount_transforms() -> list[TransformStamped]: """The mount tree as published: rooted at mid360_link.""" edges = {t.child_frame_id: t for t in frames_to_edge_transforms(FRAMES)} - return [-edges["mid360_link"], -edges["front_camera"], edges["camera_optical"]] + return [ + inverse_transform(edges["mid360_link"]), + inverse_transform(edges["front_camera"]), + edges["camera_optical"], + ] class Go2Mid360StaticTf(StaticTfPublisher): """Publishes the Go2/Mid-360 mount tree onto tf on a fixed interval.""" - def transforms(self) -> list[Transform]: + def transforms(self) -> list[TransformStamped]: return mount_transforms() diff --git a/dimos/robot/unitree/go2/test_connection.py b/dimos/robot/unitree/go2/test_connection.py index 9474af7bb63..7fa9e4ad31a 100644 --- a/dimos/robot/unitree/go2/test_connection.py +++ b/dimos/robot/unitree/go2/test_connection.py @@ -19,16 +19,19 @@ """ from collections.abc import Callable, Iterator -import time from types import SimpleNamespace from unittest.mock import MagicMock +from dimos_generated.builtin_interfaces.msg import Time +from dimos_generated.geometry_msgs.msg import Point, Pose, PoseStamped +from dimos_generated.sensor_msgs.msg import CameraInfo +from dimos_generated.std_msgs.msg import Header +from dimos_generated.tf2_msgs.msg import TFMessage import pytest from pytest_mock import MockerFixture from dimos.core.global_config import GlobalConfig -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo +from dimos.msgs.time import to_nanoseconds from dimos.robot.unitree.go2 import connection as go2_conn from dimos.robot.unitree.go2.connection import ConnectionConfig, GO2Connection @@ -60,11 +63,11 @@ def test_connection_config_aes_key_defaults_from_global_config() -> None: def test_odom_to_tf_unprefixed_by_default() -> None: - odom = PoseStamped(ts=1.0, frame_id="world") + odom = PoseStamped(header=Header(stamp=Time(sec=1, nanosec=123456789), frame_id="world")) base, camera_link, camera_optical = GO2Connection._odom_to_tf(odom) - assert (base.frame_id, base.child_frame_id) == ("world", "base_link") - assert (camera_link.frame_id, camera_link.child_frame_id) == ("base_link", "camera_link") - assert (camera_optical.frame_id, camera_optical.child_frame_id) == ( + assert (base.header.frame_id, base.child_frame_id) == ("world", "base_link") + assert (camera_link.header.frame_id, camera_link.child_frame_id) == ("base_link", "camera_link") + assert (camera_optical.header.frame_id, camera_optical.child_frame_id) == ( "camera_link", "camera_optical", ) @@ -96,14 +99,14 @@ def test_publish_tf_off_keeps_odometry_on_its_port( ) -> None: """Turning tf off hands the base_link edge to another publisher, not the odom port.""" conn = connection(publish_tf=False) - conn._publish_tf(PoseStamped(ts=1.0, frame_id="ignored")) + conn._publish_tf(PoseStamped(header=Header(stamp=Time(sec=1), frame_id="ignored"))) assert conn.tf.publish.call_count == 0 assert conn.odom.publish.call_count == 1 def test_publish_tf_on_by_default(connection: Callable[[bool], GO2Connection]) -> None: conn = connection(publish_tf=True) - conn._publish_tf(PoseStamped(ts=1.0, frame_id="ignored")) + conn._publish_tf(PoseStamped(header=Header(stamp=Time(sec=1), frame_id="ignored"))) assert conn.tf.publish.call_count == 1 assert conn.odom.publish.call_count == 1 @@ -118,34 +121,61 @@ def test_camera_info_is_restamped_on_each_publish( """A frozen stamp collapses a recording's camera_info onto one instant before the run.""" conn = connection(publish_tf=True) conn.camera_info = MagicMock() - conn.camera_info_static = CameraInfo(frame_id="camera_optical", ts=1.0) + conn.camera_info_static = CameraInfo( + header=Header(frame_id="camera_optical", stamp=Time(sec=1)) + ) def sleep(_seconds: float) -> None: if conn.camera_info.publish.call_count >= 2: raise _StopLoopError mocker.patch.object(go2_conn.time, "sleep", side_effect=sleep) - before = time.time() + mocker.patch.object( + go2_conn.time, "time_ns", side_effect=[1700000000123456789, 1700000000123456790] + ) with pytest.raises(_StopLoopError): conn.publish_camera_info() published = [call.args[0] for call in conn.camera_info.publish.call_args_list] - assert [info.ts >= before for info in published] == [True, True] - assert [info.frame_id for info in published] == [conn.camera_info_static.frame_id] * 2 - assert conn.camera_info_static.ts == 1.0 + assert [to_nanoseconds(info.header.stamp) for info in published] == [ + 1700000000123456789, + 1700000000123456790, + ] + assert [info.header.frame_id for info in published] == [ + conn.camera_info_static.header.frame_id + ] * 2 + assert conn.camera_info_static.header.stamp == Time(sec=1) def test_odom_to_tf_prefixed() -> None: """.namespace() sets frame_id_prefix: robot-local frames get prefixed, the odom parent frame stays global so all robots hang off one tree root.""" - odom = PoseStamped(ts=1.0, frame_id="world") + odom = PoseStamped(header=Header(stamp=Time(sec=1, nanosec=123456789), frame_id="world")) base, camera_link, camera_optical = GO2Connection._odom_to_tf(odom, prefix="robot0") - assert (base.frame_id, base.child_frame_id) == ("world", "robot0/base_link") - assert (camera_link.frame_id, camera_link.child_frame_id) == ( + assert (base.header.frame_id, base.child_frame_id) == ("world", "robot0/base_link") + assert (camera_link.header.frame_id, camera_link.child_frame_id) == ( "robot0/base_link", "robot0/camera_link", ) - assert (camera_optical.frame_id, camera_optical.child_frame_id) == ( + assert (camera_optical.header.frame_id, camera_optical.child_frame_id) == ( "robot0/camera_link", "robot0/camera_optical", ) + + +def test_published_tf_copies_source_pose_and_keeps_exact_stamp(connection): + conn = connection(publish_tf=True) + source = PoseStamped( + header=Header(stamp=Time(sec=1700000000, nanosec=123456789), frame_id="device_odom"), + pose=Pose(position=Point(x=1.25, y=-2.5, z=0.3)), + ) + conn._publish_tf(source) + message = conn.odom.publish.call_args.args[0] + tf = TFMessage.decode(conn.tf.publish.call_args.args[0].encode()) + assert source.header.frame_id == "device_odom" + assert message.header.frame_id == "go2_odom" + assert all(edge.header.stamp == source.header.stamp for edge in tf.transforms) + assert tf.transforms[0].header.frame_id == "go2_odom" + assert tf.transforms[0].transform.translation.x == 1.25 + message.pose.position.x = 99 + assert source.pose.position.x == 1.25 diff --git a/dimos/robot/unitree/go2/test_go2_mid360_static_transforms.py b/dimos/robot/unitree/go2/test_go2_mid360_static_transforms.py index 7cf999cd0f0..1330ecdd685 100644 --- a/dimos/robot/unitree/go2/test_go2_mid360_static_transforms.py +++ b/dimos/robot/unitree/go2/test_go2_mid360_static_transforms.py @@ -21,6 +21,9 @@ import math +from scipy.spatial.transform import Rotation + +from dimos.msgs.geometry import inverse_transform from dimos.protocol.tf.tf import MultiTBuffer from dimos.robot.unitree.go2.go2_mid360_static_transforms import ( MID360_PITCH_DOWN, @@ -42,16 +45,17 @@ def test_mount_height_survives_the_inversion() -> None: """The lidar sits MOUNT_Z above base_link, the offset every ground projection uses.""" leg = _buffer().get("mid360_link", "base_link") assert leg is not None - base_to_sensor = -leg - assert abs(base_to_sensor.translation.z - MOUNT_Z) < 1e-6 - assert abs(base_to_sensor.translation.x - MOUNT_X) < 1e-6 + base_to_sensor = inverse_transform(leg) + assert abs(base_to_sensor.transform.translation.z - MOUNT_Z) < 1e-6 + assert abs(base_to_sensor.transform.translation.x - MOUNT_X) < 1e-6 def test_mount_pitch_survives_the_inversion() -> None: """A sign flip here steers the follower off-heading rather than failing loudly.""" leg = _buffer().get("mid360_link", "base_link") assert leg is not None - pitch = (-leg).rotation.euler.y + q = inverse_transform(leg).transform.rotation + pitch = Rotation.from_quat([q.x, q.y, q.z, q.w]).as_euler("xyz")[1] assert abs(pitch - MID360_PITCH_DOWN) < 1e-6 assert abs(math.degrees(pitch) - 60.0) < 1e-6 @@ -60,5 +64,5 @@ def test_camera_optical_hangs_off_base_link() -> None: """The tree is rooted at mid360_link, so the camera edge is reachable by composition.""" optical = _buffer().get("base_link", "camera_optical") assert optical is not None - assert abs(optical.translation.x - 0.32715) < 1e-6 - assert abs(optical.translation.z - 0.04297) < 1e-6 + assert abs(optical.transform.translation.x - 0.32715) < 1e-6 + assert abs(optical.transform.translation.z - 0.04297) < 1e-6 diff --git a/dimos/robot/unitree/go2/zenoh/zenohconnection.py b/dimos/robot/unitree/go2/zenoh/zenohconnection.py index e2310533554..e5555f991b3 100644 --- a/dimos/robot/unitree/go2/zenoh/zenohconnection.py +++ b/dimos/robot/unitree/go2/zenoh/zenohconnection.py @@ -31,23 +31,25 @@ import time from typing import Any +from dimos_generated.foxglove_msgs.msg import CompressedVideo +from dimos_generated.geometry_msgs.msg import TransformStamped, Twist +from dimos_generated.nav_msgs.msg import Odometry +from dimos_generated.sensor_msgs.msg import CameraInfo, PointCloud2 +from dimos_generated.std_msgs.msg import String +from dimos_generated.tf2_msgs.msg import TFMessage from pydantic import Field, field_validator from reactivex.disposable import Disposable from dimos.core.core import rpc from dimos.core.stream import In, Out -from dimos.msgs.foxglove_msgs.CompressedVideo import CompressedVideo -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Transform import Transform -from dimos.msgs.geometry_msgs.Twist import Twist -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.msgs.nav_msgs.Odometry import Odometry -from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 -from dimos.msgs.std_msgs.String import String -from dimos.msgs.tf2_msgs.TFMessage import TFMessage -from dimos.protocol.tf.static_tf_publisher import StaticTfPublisher, StaticTfPublisherConfig -from dimos.robot.unitree.go2.connection import _camera_info_static +from dimos.msgs.geometry import inverse_transform, transform_from_odometry +from dimos.msgs.time import time_from_nanoseconds +from dimos.protocol.tf.static_tf_publisher import ( + StaticTfPublisher, + StaticTfPublisherConfig, + frames_to_edge_transforms, +) +from dimos.robot.unitree.go2.camera_calibration import front_camera_calibration from dimos.robot.unitree.go2.go2_mid360_static_transforms import ( CAMERA_XYZ, MID360_MOUNT_PRESETS, @@ -94,7 +96,7 @@ class GO2Zenoh(StaticTfPublisher): # Ours: nothing on the robot emits intrinsics. camera_info: Out[CameraInfo] - _camera_info: CameraInfo = _camera_info_static() + _camera_info: CameraInfo = front_camera_calibration() @rpc def start(self) -> None: @@ -124,7 +126,7 @@ def stop(self) -> None: @rpc def send_command(self, verb: str) -> None: """Fire an action verb at the bridge (see go2web ``topics::sport_id``).""" - self.command.transport.publish(String(verb)) + self.command.transport.publish(String(data=verb)) @rpc def sport_command(self, api_id: int) -> None: @@ -159,40 +161,34 @@ def jump(self) -> None: def set_lidar(self, enabled: bool) -> None: self.send_command("lidar on" if enabled else "lidar off") - def transforms(self) -> list[Transform]: + def transforms(self) -> list[TransformStamped]: """The mount tree, rooted at mid360_link because Point-LIO owns that frame. Measured outward from the body, but odom -> mid360_link is the only live edge, so the two edges above the lidar are inverted — otherwise mid360_link has two parents and the body snaps between them at 35 Hz. """ - base_to_camera = Transform( - translation=Vector3(*CAMERA_XYZ), - frame_id="base_link", - child_frame_id="front_camera", - ) - camera_to_mid360 = Transform( - translation=Vector3(*MID360_XYZ), - rotation=Quaternion.from_euler( - Vector3(*(math.radians(float(d)) for d in self.config.mid360_mount)) - ), - frame_id="front_camera", - child_frame_id="mid360_link", - ) - camera_to_optical = Transform( - rotation=Quaternion.from_euler(Vector3(*OPTICAL_RPY)), - frame_id="front_camera", - child_frame_id="camera_optical", + roll, pitch, yaw = (math.radians(float(d)) for d in self.config.mid360_mount) + base_to_camera, camera_to_mid360, camera_to_optical = frames_to_edge_transforms( + [ + ("front_camera", "base_link", CAMERA_XYZ, (0.0, 0.0, 0.0)), + ("mid360_link", "front_camera", MID360_XYZ, (roll, pitch, yaw)), + ("camera_optical", "front_camera", (0.0, 0.0, 0.0), OPTICAL_RPY), + ] ) - return [-camera_to_mid360, -base_to_camera, camera_to_optical] + return [ + inverse_transform(camera_to_mid360), + inverse_transform(base_to_camera), + camera_to_optical, + ] def _publish_tf(self, odom: Odometry) -> None: """The one moving edge, odom -> mid360_link; the bridge publishes no tf.""" - self.tf.publish(TFMessage(Transform.from_pose(odom.child_frame_id, odom.to_pose_stamped()))) + self.tf.publish(TFMessage(transforms=[transform_from_odometry(odom)])) async def _publish_camera_info(self) -> None: period = 1.0 / self.config.camera_info_hz while self._running: - self._camera_info.ts = time.time() + self._camera_info.header.stamp = time_from_nanoseconds(time.time_ns()) self.camera_info.publish(self._camera_info) await asyncio.sleep(period) diff --git a/dimos/robot/unitree/keyboard_teleop.py b/dimos/robot/unitree/keyboard_teleop.py index f5249da0380..d1982254bd5 100644 --- a/dimos/robot/unitree/keyboard_teleop.py +++ b/dimos/robot/unitree/keyboard_teleop.py @@ -18,6 +18,8 @@ import threading from typing import Any +from dimos_generated.geometry_msgs.msg import Twist, Vector3 +from dimos_generated.std_msgs.msg import Float32, Int8 import pygame from dimos.constants import DEFAULT_THREAD_JOIN_TIMEOUT @@ -30,10 +32,6 @@ from dimos.core.core import rpc from dimos.core.module import Module from dimos.core.stream import Out -from dimos.msgs.geometry_msgs.Twist import Twist -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.msgs.std_msgs.Float32 import Float32 -from dimos.msgs.std_msgs.Int8 import Int8 from dimos.utils.logging_config import setup_logger logger = setup_logger() @@ -129,8 +127,8 @@ def start(self) -> None: @rpc def stop(self) -> None: stop_twist = Twist() - stop_twist.linear = Vector3(0, 0, 0) - stop_twist.angular = Vector3(0, 0, 0) + stop_twist.linear = Vector3() + stop_twist.angular = Vector3() self.cmd_vel.publish(stop_twist) self._stop_event.set() @@ -162,19 +160,19 @@ def _pygame_loop(self) -> None: # Emergency stop - clear all keys and send zero twist self._keys_held.clear() stop_twist = Twist() - stop_twist.linear = Vector3(0, 0, 0) - stop_twist.angular = Vector3(0, 0, 0) + stop_twist.linear = Vector3() + stop_twist.angular = Vector3() self.cmd_vel.publish(stop_twist) logger.warning("EMERGENCY STOP!") elif event.key == pygame.K_ESCAPE: # ESC quits self._stop_event.set() elif event.key == pygame.K_RETURN: - self.operator_command.publish(Int8(GATE_ADVANCE)) + self.operator_command.publish(Int8(data=GATE_ADVANCE)) elif event.key == pygame.K_k: - self.operator_command.publish(Int8(GATE_SKIP)) + self.operator_command.publish(Int8(data=GATE_SKIP)) elif event.key == pygame.K_BACKSPACE: - self.operator_command.publish(Int8(GATE_QUIT)) + self.operator_command.publish(Int8(data=GATE_QUIT)) elif pygame.K_0 <= event.key <= pygame.K_9: # 0 → 0.0 m, 1 → 0.1 m, …, 9 → 0.9 m corridor half-width. self.e_max.publish(Float32(data=(event.key - pygame.K_0) * 0.1)) @@ -184,8 +182,8 @@ def _pygame_loop(self) -> None: # Generate Twist message from held keys twist = Twist() - twist.linear = Vector3(0, 0, 0) - twist.angular = Vector3(0, 0, 0) + twist.linear = Vector3() + twist.angular = Vector3() # Movement keys (WASD/QE) — guarded by disable_movement so the # window can run as a pure e_max slider (0-9 keys stay live in diff --git a/dimos/robot/unitree/mujoco_connection.py b/dimos/robot/unitree/mujoco_connection.py index 7fab1241bb7..fa28672960a 100644 --- a/dimos/robot/unitree/mujoco_connection.py +++ b/dimos/robot/unitree/mujoco_connection.py @@ -31,6 +31,9 @@ from typing import Any, TypeVar import weakref +from dimos_generated.geometry_msgs.msg import Point, Pose, PoseStamped, Quaternion, Twist +from dimos_generated.sensor_msgs.msg import CameraInfo, Image, PointCloud2 +from dimos_generated.std_msgs.msg import Header import numpy as np from numpy.typing import NDArray from reactivex import Observable, empty @@ -39,13 +42,9 @@ from dimos.constants import DEFAULT_THREAD_JOIN_TIMEOUT from dimos.core.global_config import GlobalConfig -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Twist import Twist -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo -from dimos.msgs.sensor_msgs.Image import Image, ImageFormat -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 -from dimos.robot.unitree.type.odometry import Odometry +from dimos.msgs.camera_info import camera_info_from_fov +from dimos.msgs.image import image_from_array +from dimos.msgs.time import header_now, time_from_seconds from dimos.simulation.mujoco.constants import ( LAUNCHER_PATH, LIDAR_FPS, @@ -98,12 +97,12 @@ def __init__(self, global_config: GlobalConfig) -> None: self._stop_events: list[threading.Event] = [] self._is_cleaned_up = False - camera_info_static: CameraInfo = CameraInfo.from_fov( - fov_deg=VIDEO_CAMERA_FOV, + camera_info_static: CameraInfo = camera_info_from_fov( + fov_degrees=VIDEO_CAMERA_FOV, width=VIDEO_WIDTH, height=VIDEO_HEIGHT, axis="vertical", - frame_id="camera_optical", + header=Header(frame_id="camera_optical"), ) def start(self) -> None: @@ -314,7 +313,7 @@ def get_video_frame(self) -> NDArray[Any] | None: return None - def get_odom_message(self) -> Odometry | None: + def get_odom_message(self) -> PoseStamped | None: if self.shm_data is None: return None @@ -324,13 +323,11 @@ def get_odom_message(self) -> Odometry | None: pos, quat_wxyz, timestamp = odom_data # Convert quaternion from (w,x,y,z) to (x,y,z,w) for ROS/Dimos - orientation = Quaternion(quat_wxyz[1], quat_wxyz[2], quat_wxyz[3], quat_wxyz[0]) + orientation = Quaternion(x=quat_wxyz[1], y=quat_wxyz[2], z=quat_wxyz[3], w=quat_wxyz[0]) - return Odometry( - position=Vector3(pos[0], pos[1], pos[2]), - orientation=orientation, - ts=timestamp, - frame_id="world", + return PoseStamped( + header=Header(stamp=time_from_seconds(timestamp), frame_id="world"), + pose=Pose(position=Point(x=pos[0], y=pos[1], z=pos[2]), orientation=orientation), ) return None @@ -388,15 +385,19 @@ def lidar_stream(self) -> Observable[PointCloud2]: return self._create_stream(self.get_lidar_message, LIDAR_FPS, "Lidar") @functools.cache - def odom_stream(self) -> Observable[Odometry]: + def odom_stream(self) -> Observable[PoseStamped]: return self._create_stream(self.get_odom_message, ODOM_FREQUENCY, "Odom") @functools.cache def video_stream(self) -> Observable[Image]: def get_video_as_image() -> Image | None: frame = self.get_video_frame() - # MuJoCo renderer returns RGB uint8 frames; Image.from_numpy defaults to BGR. - return Image.from_numpy(frame, format=ImageFormat.RGB) if frame is not None else None + # MuJoCo renderer returns RGB uint8 pixels. + return ( + image_from_array(frame, encoding="rgb8", header=header_now("camera_optical")) + if frame is not None + else None + ) return self._create_stream(get_video_as_image, VIDEO_FPS, "Video") diff --git a/dimos/robot/unitree/test_connection.py b/dimos/robot/unitree/test_connection.py index 8180f2d70d9..041ac8b5621 100644 --- a/dimos/robot/unitree/test_connection.py +++ b/dimos/robot/unitree/test_connection.py @@ -19,18 +19,25 @@ """ import json +import threading from typing import Any from unittest.mock import ANY, AsyncMock, MagicMock, call +from dimos_generated.geometry_msgs.msg import PoseStamped, TransformStamped, Twist, Vector3 +from dimos_generated.sensor_msgs.msg import Image, PointCloud2 +import numpy as np import pytest +import reactivex as rx +from reactivex.scheduler import ThreadPoolScheduler from unitree_webrtc_connect.constants import DATA_CHANNEL_TYPE, RTC_TOPIC, SPORT_CMD from dimos.constants import DEFAULT_THREAD_JOIN_TIMEOUT from dimos.core.global_config import GlobalConfig -from dimos.msgs.geometry_msgs.Twist import Twist -from dimos.msgs.geometry_msgs.Vector3 import Vector3 +from dimos.msgs.image import image_view +from dimos.msgs.pointcloud import pointcloud_xyz from dimos.robot.unitree import connection as conn_mod -from dimos.robot.unitree.connection import UnitreeWebRTCConnection +from dimos.robot.unitree.connection import SerializableVideoFrame, UnitreeWebRTCConnection +from dimos.robot.unitree.type.odometry import raw_odometry_msg_sample def _stub_driver(connect_exc: Exception | None = None) -> MagicMock: @@ -113,8 +120,8 @@ def test_move_api_toggle_sends_selected_wire_command( driver = _stub_driver() monkeypatch.setattr(conn_mod, "LegionConnection", MagicMock(return_value=driver)) twist = Twist( - linear=Vector3(1.5, -0.4, 0.0), - angular=Vector3(0.0, 0.0, 0.8), + linear=Vector3(x=1.5, y=-0.4), + angular=Vector3(z=0.8), ) connection = UnitreeWebRTCConnection(ip="10.0.0.99", **connection_options) @@ -165,3 +172,102 @@ def test_global_config_reads_unitree_aes_128_key_env(monkeypatch: pytest.MonkeyP """The key enters via GlobalConfig, read from the UNITREE_AES_128_KEY env var.""" monkeypatch.setenv("UNITREE_AES_128_KEY", "ee" * 16) assert GlobalConfig().unitree_aes_128_key == "ee" * 16 + + +@pytest.fixture +def sensor_scheduler(mocker): + scheduler = ThreadPoolScheduler(max_workers=2) + mocker.patch("dimos.utils.reactive.get_scheduler", return_value=scheduler) + try: + yield scheduler + finally: + scheduler.executor.shutdown(wait=True) + + +@pytest.mark.parametrize("kind", ["lidar", "video"]) +def test_sensor_streams_emit_generated_cdr_with_exact_arrival_time( + built_connection, mocker, sensor_scheduler, kind +): + connection, _driver = built_connection + mocker.patch.object(conn_mod.time, "time_ns", return_value=1700000000123456789) + values = np.array([[1.25, 2.5, 3.75]], dtype=np.float32) + pixels = np.arange(18, dtype=np.uint8).reshape(2, 3, 3) + if kind == "lidar": + raw = {"data": {"stamp": 1.0, "data": {"points": values}}} + mocker.patch.object(connection, "raw_lidar_stream", return_value=rx.just(raw)) + stream = connection.lidar_stream() + else: + frame = SerializableVideoFrame(data=pixels) + mocker.patch.object(connection, "raw_video_stream", return_value=rx.just(frame)) + stream = connection.video_stream() + received = [] + errors = [] + ready = threading.Event() + + def receive(message): + received.append(message) + ready.set() + + def fail(error): + errors.append(error) + ready.set() + + subscription = stream.subscribe(receive, fail) + try: + assert ready.wait(5), "Sensor conversion did not publish" + assert not errors + assert len(received) == 1 + message = received[0] + assert (message.header.stamp.sec, message.header.stamp.nanosec) == (1700000000, 123456789) + if kind == "lidar": + decoded = PointCloud2.decode(message.encode()) + np.testing.assert_array_equal(pointcloud_xyz(decoded), values) + assert decoded.header.frame_id == "world" + else: + decoded = Image.decode(message.encode()) + np.testing.assert_array_equal(image_view(decoded), pixels) + assert decoded.header.frame_id == "camera_optical" + finally: + subscription.dispose() + + +@pytest.mark.parametrize("as_tf", [False, True]) +def test_odometry_stream_preserves_pose_and_uses_exact_arrival_stamp( + built_connection, mocker, sensor_scheduler, as_tf +): + connection, _driver = built_connection + mocker.patch.object(conn_mod.time, "time_ns", return_value=1700000000123456789) + mocker.patch.object( + connection, "raw_odom_stream", return_value=rx.just(raw_odometry_msg_sample) + ) + stream = connection.tf_stream() if as_tf else connection.odom_stream() + received = [] + errors = [] + ready = threading.Event() + + def receive(message): + received.append(message) + ready.set() + + def fail(error): + errors.append(error) + ready.set() + + subscription = stream.subscribe(receive, fail) + try: + assert ready.wait(5), "Odometry conversion did not publish" + assert not errors + assert len(received) == 1 + cls = TransformStamped if as_tf else PoseStamped + message = cls.decode(received[0].encode()) + assert message.header.frame_id == "world" + assert (message.header.stamp.sec, message.header.stamp.nanosec) == (1700000000, 123456789) + if as_tf: + assert message.child_frame_id == "base_link" + position, rotation = message.transform.translation, message.transform.rotation + else: + position, rotation = message.pose.position, message.pose.orientation + assert (position.x, position.y, position.z) == (5.961965, -2.916958, 0.319509) + assert rotation.w == -0.242112 + finally: + subscription.dispose() diff --git a/dimos/robot/unitree/test_generated_commands.py b/dimos/robot/unitree/test_generated_commands.py new file mode 100644 index 00000000000..12ee1901377 --- /dev/null +++ b/dimos/robot/unitree/test_generated_commands.py @@ -0,0 +1,87 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +# Copyright 2026 Dimensional Inc. +# SPDX-License-Identifier: Apache-2.0 + +from types import SimpleNamespace +from unittest.mock import Mock + +from dimos_generated.geometry_msgs.msg import Twist +from dimos_generated.std_msgs.msg import Float32, Int8 +import pytest + +from dimos.control.benchmarking.gate import GATE_ADVANCE, GATE_QUIT, GATE_SKIP +from dimos.robot.unitree import keyboard_teleop as keyboard +from dimos.robot.unitree.g1.skill_container import UnitreeG1SkillContainer + + +def test_keyboard_generated_operator_gates_and_slider_without_window(monkeypatch): + module = keyboard.KeyboardTeleop(disable_movement=True) + module._keys_held = set() + module._thread = Mock() + received = [] + slider = [] + motion = [] + monkeypatch.setattr(module.operator_command, "publish", received.append) + monkeypatch.setattr(module.e_max, "publish", slider.append) + monkeypatch.setattr(module.cmd_vel, "publish", motion.append) + monkeypatch.setattr(module, "_update_display", lambda message: None) + monkeypatch.setattr(keyboard.pygame, "init", Mock()) + monkeypatch.setattr(keyboard.pygame, "quit", Mock()) + monkeypatch.setattr(keyboard.pygame.display, "set_mode", lambda *args: Mock()) + monkeypatch.setattr(keyboard.pygame.display, "set_caption", Mock()) + monkeypatch.setattr(keyboard.pygame.font, "Font", lambda *args: Mock()) + monkeypatch.setattr( + keyboard.pygame.time, + "Clock", + lambda: SimpleNamespace(tick=lambda hz: module._stop_event.set()), + ) + keys = [ + keyboard.pygame.K_RETURN, + keyboard.pygame.K_k, + keyboard.pygame.K_BACKSPACE, + keyboard.pygame.K_7, + ] + events = [SimpleNamespace(type=keyboard.pygame.KEYDOWN, key=key) for key in keys] + monkeypatch.setattr(keyboard.pygame.event, "get", lambda: events) + try: + module._pygame_loop() + assert [Int8.decode(value.encode()).data for value in received] == [ + GATE_ADVANCE, + GATE_SKIP, + GATE_QUIT, + ] + assert Float32.decode(slider[0].encode()).data == pytest.approx(0.7) + assert not motion # disable_movement keeps this an operator-only interface + finally: + module.stop() + assert len(motion) == 1 and Twist.decode(motion[0].encode()) == Twist() + + +def test_g1_skill_constructs_generated_twist_at_captured_rpc_boundary(): + module = UnitreeG1SkillContainer() + gateway = Mock() + module._connection = gateway + try: + module.move(x=0.25, y=-0.125, yaw=0.5, duration=2) + gateway.move.assert_called_once() + args, kwargs = gateway.move.call_args + assert type(args[0]) is Twist + value = Twist.decode(args[0].encode()) + assert value.linear.x == 0.25 and value.linear.y == -0.125 + assert value.angular.z == 0.5 and value.linear.z == 0 + assert kwargs == {"duration": 2} + finally: + module.stop() diff --git a/dimos/robot/unitree/test_mujoco_connection.py b/dimos/robot/unitree/test_mujoco_connection.py index 0c385dc9d24..26f32e58a02 100644 --- a/dimos/robot/unitree/test_mujoco_connection.py +++ b/dimos/robot/unitree/test_mujoco_connection.py @@ -18,7 +18,10 @@ import threading from types import ModuleType from typing import Any, cast +from unittest.mock import MagicMock +from dimos_generated.geometry_msgs.msg import Point, PoseStamped, Quaternion +import numpy as np from pytest import MonkeyPatch from dimos.core.global_config import GlobalConfig @@ -96,6 +99,7 @@ def kill(self) -> None: def _bare_connection(monkeypatch: MonkeyPatch) -> MujocoConnection: + monkeypatch.setitem(sys.modules, "mujoco", MagicMock(name="mujoco")) mjx_env = ModuleType("mujoco_playground._src.mjx_env") mjx_env.ensure_menagerie_exists = lambda: None playground_src = ModuleType("mujoco_playground._src") @@ -184,3 +188,24 @@ def test_stop_terminates_child_before_closing_pumped_output( assert not stopper.is_alive() assert process.terminations == 1 + + +def test_odometry_reads_generated_pose_and_converts_wxyz(monkeypatch): + connection = _bare_connection(monkeypatch) + shared = mujoco_connection.ShmWriter() + connection.shm_data = shared + try: + values = np.ndarray((8,), dtype=np.float64, buffer=shared.shm.odom.buf) + values[:] = [1.25, -2.5, 0.3, 0.8, 0, 0, 0.6, -0.5] + sequence = np.ndarray((8,), dtype=np.int64, buffer=shared.shm.seq.buf) + sequence[2] = 1 + message = connection.get_odom_message() + assert message is not None + decoded = PoseStamped.decode(message.encode()) + assert decoded.header.frame_id == "world" + assert (decoded.header.stamp.sec, decoded.header.stamp.nanosec) == (-1, 500000000) + assert decoded.pose.position == Point(x=1.25, y=-2.5, z=0.3) + assert decoded.pose.orientation == Quaternion(z=0.6, w=0.8) + assert connection.get_odom_message() is None + finally: + connection.stop() diff --git a/dimos/robot/unitree/test_simulation_tf.py b/dimos/robot/unitree/test_simulation_tf.py new file mode 100644 index 00000000000..9ffb8d97eff --- /dev/null +++ b/dimos/robot/unitree/test_simulation_tf.py @@ -0,0 +1,87 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from dimos_generated.builtin_interfaces.msg import Time +from dimos_generated.geometry_msgs.msg import Point, Pose, PoseStamped +from dimos_generated.sensor_msgs.msg import CameraInfo +from dimos_generated.std_msgs.msg import Header +from dimos_generated.tf2_msgs.msg import TFMessage +import pytest + +from dimos.core.global_config import GlobalConfig +from dimos.robot.unitree.dimsim_connection import _odom_to_tf +from dimos.robot.unitree.g1.mujoco_sim import G1SimConnection +from dimos.robot.unitree.mujoco_connection import MujocoConnection + + +@pytest.fixture +def source_pose(): + return PoseStamped( + header=Header(frame_id="world", stamp=Time(sec=1700000000, nanosec=123456789)), + pose=Pose(position=Point(x=1.25, y=2.5, z=0.7)), + ) + + +def test_dimsim_tf_preserves_source_pose_and_stamp(source_pose): + message = TFMessage.decode(TFMessage(transforms=_odom_to_tf(source_pose)).encode()) + assert [(edge.header.frame_id, edge.child_frame_id) for edge in message.transforms] == [ + ("world", "base_link"), + ("base_link", "camera_link"), + ("camera_link", "camera_optical"), + ("base_link", "lidar_link"), + ] + assert all(edge.header.stamp == source_pose.header.stamp for edge in message.transforms) + assert message.transforms[0].transform.translation.x == 1.25 + assert message.transforms[1].transform.translation.x == 0.3 + + +@pytest.fixture +def g1_connection(mocker): + connection = G1SimConnection(g=GlobalConfig(robot_ip="127.0.0.1")) + mocker.patch.object(connection, "connection", mocker.MagicMock(spec=MujocoConnection)) + mocker.patch.object(connection, "tf", mocker.MagicMock()) + mocker.patch.object(connection, "odom", mocker.MagicMock()) + mocker.patch.object(connection, "camera_info", mocker.MagicMock()) + try: + yield connection + finally: + connection.stop() + + +def test_g1_simulator_tf_uses_pose_time_for_all_mounts(g1_connection, source_pose): + g1_connection._publish_tf(source_pose) + message = TFMessage.decode(g1_connection.tf.publish.call_args.args[0].encode()) + assert [(edge.header.frame_id, edge.child_frame_id) for edge in message.transforms] == [ + ("world", "base_link"), + ("base_link", "camera_link"), + ("camera_link", "camera_optical"), + ("map", "world"), + ] + assert all(edge.header.stamp == source_pose.header.stamp for edge in message.transforms) + assert message.transforms[1].transform.translation.z == 0.6 + assert g1_connection.odom.publish.call_args.args[0] == source_pose + + +def test_g1_camera_info_stamps_each_publication_without_mutating_template(g1_connection, mocker): + template = CameraInfo(header=Header(frame_id="camera_optical"), width=640, height=288) + g1_connection.connection.camera_info_static = template + mocker.patch("dimos.robot.unitree.g1.mujoco_sim.time.time_ns", return_value=1700000000123456789) + mocker.patch.object( + g1_connection._stop_event, "wait", side_effect=lambda _: g1_connection._stop_event.set() + ) + g1_connection._publish_camera_info_loop() + message = CameraInfo.decode(g1_connection.camera_info.publish.call_args.args[0].encode()) + assert message.header.stamp == Time(sec=1700000000, nanosec=123456789) + assert message.header.frame_id == "camera_optical" + assert template.header.stamp == Time() diff --git a/dimos/robot/unitree/testing/tool_actors.py b/dimos/robot/unitree/testing/tool_actors.py index cd0cdc05dbf..942ceb28c2b 100644 --- a/dimos/robot/unitree/testing/tool_actors.py +++ b/dimos/robot/unitree/testing/tool_actors.py @@ -14,13 +14,13 @@ import asyncio from collections.abc import Callable +from dimos_generated.sensor_msgs.msg import PointCloud2 import pytest from dimos.core.coordination.module_coordinator import ModuleCoordinator from dimos.core.core import rpc from dimos.core.module import Module from dimos.core.transport import LCMTransport -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 from dimos.robot.unitree.type.map import Map as Mapper diff --git a/dimos/robot/unitree/testing/tool_tooling.py b/dimos/robot/unitree/testing/tool_tooling.py index fe4d76a7428..db2e48e6acb 100644 --- a/dimos/robot/unitree/testing/tool_tooling.py +++ b/dimos/robot/unitree/testing/tool_tooling.py @@ -15,14 +15,14 @@ import time from dimos.robot.unitree.type.lidar import pointcloud2_from_webrtc_lidar -from dimos.robot.unitree.type.odometry import Odometry +from dimos.robot.unitree.type.odometry import pose_from_webrtc_odometry from dimos.utils.reactive import backpressure from dimos.utils.testing.legacy_pickle import LegacyPickleStore def test_replay_all() -> None: lidar_store = LegacyPickleStore("unitree/lidar", autocast=pointcloud2_from_webrtc_lidar) - odom_store = LegacyPickleStore("unitree/odom", autocast=Odometry.from_msg) + odom_store = LegacyPickleStore("unitree/odom", autocast=pose_from_webrtc_odometry) video_store = LegacyPickleStore("unitree/video") backpressure(odom_store.stream()).subscribe(print) diff --git a/dimos/robot/unitree/type/lidar.py b/dimos/robot/unitree/type/lidar.py index c8e01962e2d..90df6536bb9 100644 --- a/dimos/robot/unitree/type/lidar.py +++ b/dimos/robot/unitree/type/lidar.py @@ -16,21 +16,21 @@ from collections.abc import Callable import time -from typing import TypedDict, TypeVar +from typing import Protocol, TypedDict, TypeVar +from dimos_generated.builtin_interfaces.msg import Time +from dimos_generated.sensor_msgs.msg import PointCloud2 +from dimos_generated.std_msgs.msg import Header import numpy as np from reactivex import operators as ops from reactivex.observable import Observable -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 -from dimos.types.timestamped import Timestamped +from dimos.msgs.pointcloud import pointcloud_from_xyz +from dimos.msgs.time import time_from_seconds, to_seconds from dimos.utils.logging_config import setup_logger logger = setup_logger() -# Backwards compatibility alias for pickled data -LidarMessage = PointCloud2 - class RawLidarPoints(TypedDict): points: np.ndarray # Shape (N, 3) array of 3D points [x, y, z] @@ -56,34 +56,28 @@ class RawLidarMsg(TypedDict): data: RawLidarData -def pointcloud2_from_webrtc_lidar(raw_message: RawLidarMsg, ts: float | None = None) -> PointCloud2: - """Convert a raw Unitree WebRTC lidar message to PointCloud2. - - Args: - raw_message: Raw lidar message from Unitree WebRTC API - ts: Optional timestamp override. If None, uses the sensor stamp from - ``raw_message["data"]["stamp"]``. +def pointcloud2_from_webrtc_lidar( + raw_message: RawLidarMsg, *, stamp: Time | None = None +) -> PointCloud2: + """Copy decoded WebRTC XYZ points into a generated cloud in the world frame. - The sensor stamp is authoritative when valid but Unitree's publisher - occasionally re-emits a stale value on fresh scans — see - :func:`repair_stale_ts` for the downstream repair. + The raw sensor's floating-second stamp is used unless the connection supplies + an explicit arrival stamp. No Open3D object or legacy message is constructed. """ - import open3d as o3d # type: ignore[import-untyped] - data = raw_message["data"] - points = data["data"]["points"] + return pointcloud_from_xyz( + data["data"]["points"], + header=Header( + frame_id="world", stamp=stamp if stamp is not None else time_from_seconds(data["stamp"]) + ), + ) - pointcloud = o3d.geometry.PointCloud() - pointcloud.points = o3d.utility.Vector3dVector(points) - return PointCloud2( - pointcloud=pointcloud, - ts=ts if ts is not None else data["stamp"], - frame_id="world", - ) +class StampedMessage(Protocol): + header: Header -T = TypeVar("T", bound=Timestamped) +T = TypeVar("T", bound=StampedMessage) def repair_stale_ts( @@ -109,20 +103,20 @@ def _repair(item: T) -> T: nonlocal prev_good, prev_raw, n_seen, calibrated, use_system_time if use_system_time: - item.ts = now() + item.header.stamp = time_from_seconds(now()) return item if not calibrated: - if prev_raw is not None and item.ts != prev_raw: + if prev_raw is not None and to_seconds(item.header.stamp) != prev_raw: calibrated = True # lidar stamps advancing — using lidar time", - prev_raw = item.ts + prev_raw = to_seconds(item.header.stamp) n_seen += 1 - if prev_good is not None and item.ts <= prev_good: - item.ts = prev_good + default_period + if prev_good is not None and to_seconds(item.header.stamp) <= prev_good: + item.header.stamp = time_from_seconds(prev_good + default_period) - prev_good = item.ts + prev_good = to_seconds(item.header.stamp) if not calibrated and n_seen >= calibration_frames: calibrated = True diff --git a/dimos/robot/unitree/type/map.py b/dimos/robot/unitree/type/map.py index 473a6e1df21..5d8e515a365 100644 --- a/dimos/robot/unitree/type/map.py +++ b/dimos/robot/unitree/type/map.py @@ -14,10 +14,15 @@ from __future__ import annotations +from copy import deepcopy from pathlib import Path import time from typing import TYPE_CHECKING, Any +from dimos_generated.nav_msgs.msg import OccupancyGrid +from dimos_generated.sensor_msgs.msg import PointCloud2 +from dimos_generated.std_msgs.msg import Header +import numpy as np from reactivex import interval from reactivex.disposable import Disposable @@ -27,8 +32,9 @@ from dimos.mapping.pointclouds.accumulators.general import GeneralPointCloudAccumulator from dimos.mapping.pointclouds.accumulators.protocol import PointCloudAccumulator from dimos.mapping.pointclouds.occupancy import general_occupancy -from dimos.msgs.nav_msgs.OccupancyGrid import OccupancyGrid -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 +from dimos.msgs.occupancy import occupancy_from_file +from dimos.msgs.pointcloud import pointcloud_from_xyz, pointcloud_from_xyz_rgb, pointcloud_to_open3d +from dimos.msgs.time import time_from_seconds if TYPE_CHECKING: import open3d as o3d # type: ignore[import-untyped] @@ -53,6 +59,7 @@ class Map(Module): def __init__(self, **kwargs: Any) -> None: super().__init__(**kwargs) + self._latest_header = Header(frame_id="world", stamp=time_from_seconds(time.time())) self.voxel_size = self.config.voxel_size self.cost_resolution = self.config.cost_resolution self.global_publish_interval = self.config.global_publish_interval @@ -78,15 +85,22 @@ def stop(self) -> None: super().stop() def to_PointCloud2(self) -> PointCloud2: - return PointCloud2( - pointcloud=self._point_cloud_accumulator.get_point_cloud(), - ts=time.time(), - ) + cloud = self._point_cloud_accumulator.get_point_cloud() + points = np.asarray(cloud.points) + if cloud.has_colors(): + return pointcloud_from_xyz_rgb( + points, + (np.asarray(cloud.colors) * 255).astype(np.uint8), + header=deepcopy(self._latest_header), + ) + return pointcloud_from_xyz(points, header=deepcopy(self._latest_header)) # TODO: Why is this RPC? @rpc def add_frame(self, frame: PointCloud2) -> None: - self._point_cloud_accumulator.add(frame.pointcloud) + self._point_cloud_accumulator.add(pointcloud_to_open3d(frame)) + self._latest_header = deepcopy(frame.header) + self._latest_header.frame_id = "world" @property def o3d_geometry(self) -> o3d.geometry.PointCloud: @@ -106,7 +120,9 @@ def _publish(self, _: Any) -> None: if self.config.g.mujoco_global_costmap_from_occupancy: if self._preloaded_occupancy is None: path = Path(self.config.g.mujoco_global_costmap_from_occupancy) - self._preloaded_occupancy = OccupancyGrid.from_path(path) + self._preloaded_occupancy = occupancy_from_file( + path, header=deepcopy(self._latest_header) + ) occupancygrid = self._preloaded_occupancy self.global_costmap.publish(occupancygrid) diff --git a/dimos/robot/unitree/type/odometry.py b/dimos/robot/unitree/type/odometry.py index fabf800b6c1..8240d414b89 100644 --- a/dimos/robot/unitree/type/odometry.py +++ b/dimos/robot/unitree/type/odometry.py @@ -13,15 +13,16 @@ # limitations under the License. from typing import Literal, TypedDict -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.robot.unitree.type.timeseries import ( - Timestamped, +from dimos_generated.builtin_interfaces.msg import Time +from dimos_generated.geometry_msgs.msg import ( + Point, + Pose, + PoseStamped as GeneratedPoseStamped, + Quaternion as GeneratedQuaternion, ) -from dimos.types.timestamped import to_timestamp +from dimos_generated.std_msgs.msg import Header as GeneratedHeader -raw_odometry_msg_sample = { +raw_odometry_msg_sample: "RawOdometryMessage" = { "type": "msg", "topic": "rt/utlidar/robot_pose", "data": { @@ -73,32 +74,21 @@ class RawOdometryMessage(TypedDict): data: OdometryData -class Odometry(PoseStamped, Timestamped): # type: ignore[misc] - name = "geometry_msgs.PoseStamped" - - def __init__(self, frame_id: str = "base_link", *args, **kwargs) -> None: # type: ignore[no-untyped-def] - super().__init__(frame_id=frame_id, *args, **kwargs) # type: ignore[misc] - - @classmethod - def from_msg(cls, msg: RawOdometryMessage) -> "Odometry": - pose = msg["data"]["pose"] - - # Extract position - pos = Vector3( - pose["position"].get("x"), - pose["position"].get("y"), - pose["position"].get("z"), - ) - - rot = Quaternion( - pose["orientation"].get("x"), - pose["orientation"].get("y"), - pose["orientation"].get("z"), - pose["orientation"].get("w"), - ) - - ts = to_timestamp(msg["data"]["header"]["stamp"]) - return Odometry(position=pos, orientation=rot, ts=ts, frame_id="world") - - def __repr__(self) -> str: - return f"Odom pos({self.position}), rot({self.orientation})" +def pose_from_webrtc_odometry( + message: RawOdometryMessage, *, header: GeneratedHeader | None = None +) -> GeneratedPoseStamped: + """Copy the device's ROS-shaped pose, preserving its header unless explicitly replaced.""" + data = message["data"] + source_header = data["header"] + pose = data["pose"] + return GeneratedPoseStamped( + header=header + if header is not None + else GeneratedHeader( + stamp=Time(**source_header["stamp"]), frame_id=source_header["frame_id"] + ), + pose=Pose( + position=Point(**pose["position"]), + orientation=GeneratedQuaternion(**pose["orientation"]), + ), + ) diff --git a/dimos/robot/unitree/type/test_lidar.py b/dimos/robot/unitree/type/test_lidar.py index 631e7f680ef..e92bbf19135 100644 --- a/dimos/robot/unitree/type/test_lidar.py +++ b/dimos/robot/unitree/type/test_lidar.py @@ -16,19 +16,27 @@ import itertools from typing import cast +from dimos_generated.builtin_interfaces.msg import Time +from dimos_generated.sensor_msgs.msg import PointCloud2 +from dimos_generated.std_msgs.msg import Header +import numpy as np import pytest import reactivex as rx -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 +from dimos.msgs.pointcloud import pointcloud_xyz +from dimos.msgs.time import time_from_seconds, to_seconds from dimos.robot.unitree.type.lidar import ( RawLidarMsg, pointcloud2_from_webrtc_lidar, repair_stale_ts, ) -from dimos.types.timestamped import Timestamped from dimos.utils.testing.replay import SensorReplay +def _stamped(seconds: float) -> PointCloud2: + return PointCloud2(header=Header(stamp=time_from_seconds(seconds))) + + @pytest.mark.self_hosted def test_init() -> None: lidar = SensorReplay("office_lidar") @@ -41,63 +49,75 @@ def test_init() -> None: def test_repair_stale_ts_extrapolates_non_monotonic_stamp() -> None: # input: two healthy frames, one stale, one healthy - raw = [Timestamped(0.000), Timestamped(0.130), Timestamped(-6.354), Timestamped(0.260)] + raw = [_stamped(0.000), _stamped(0.130), _stamped(-6.354), _stamped(0.260)] out: list[float] = [] rx.from_iterable(raw).pipe(repair_stale_ts(default_period=0.130)).subscribe( - on_next=lambda item: out.append(item.ts) + on_next=lambda item: out.append(to_seconds(item.header.stamp)) ) assert out == [0.000, 0.130, 0.260, 0.390] def test_repair_stale_ts_passes_monotonic_unchanged() -> None: - raw = [Timestamped(0.0), Timestamped(0.130), Timestamped(0.260)] + raw = [_stamped(0.0), _stamped(0.130), _stamped(0.260)] out: list[float] = [] rx.from_iterable(raw).pipe(repair_stale_ts()).subscribe( - on_next=lambda item: out.append(item.ts) + on_next=lambda item: out.append(to_seconds(item.header.stamp)) ) assert out == [0.0, 0.130, 0.260] def test_repair_stale_ts_handles_consecutive_bad_frames() -> None: # two stale-stamp frames in a row → each forward-extrapolated by default_period - raw = [Timestamped(0.0), Timestamped(-6.354), Timestamped(-6.354), Timestamped(0.500)] + raw = [_stamped(0.0), _stamped(-6.354), _stamped(-6.354), _stamped(0.500)] out: list[float] = [] rx.from_iterable(raw).pipe(repair_stale_ts(default_period=0.130)).subscribe( - on_next=lambda item: out.append(item.ts) + on_next=lambda item: out.append(to_seconds(item.header.stamp)) ) assert out == [0.0, 0.130, 0.260, 0.500] def test_repair_stale_ts_old_firmware_uses_system_time_after_calibration() -> None: - raw = [Timestamped(100.0) for _ in range(13)] + raw = [_stamped(100.0) for _ in range(13)] out: list[float] = [] rx.from_iterable(raw).pipe( repair_stale_ts(default_period=0.130, calibration_frames=10, now=lambda: 999.0) - ).subscribe(on_next=lambda item: out.append(item.ts)) + ).subscribe(on_next=lambda item: out.append(to_seconds(item.header.stamp))) assert out[0] == 100.0 assert out[1:10] == pytest.approx([100.0 + 0.130 * i for i in range(1, 10)]) assert out[10:] == [999.0, 999.0, 999.0] def test_repair_stale_ts_new_firmware_repair_persists_after_calibration() -> None: - raw = [Timestamped(i * 0.130) for i in range(11)] + [Timestamped(-6.354), Timestamped(2.0)] + raw = [_stamped(i * 0.130) for i in range(11)] + [_stamped(-6.354), _stamped(2.0)] out: list[float] = [] rx.from_iterable(raw).pipe( repair_stale_ts(default_period=0.130, calibration_frames=10, now=lambda: 999.0) - ).subscribe(on_next=lambda item: out.append(item.ts)) + ).subscribe(on_next=lambda item: out.append(to_seconds(item.header.stamp))) assert out[:11] == pytest.approx([i * 0.130 for i in range(11)]) assert out[11] == pytest.approx(10 * 0.130 + 0.130) assert out[12] == pytest.approx(2.0) def test_repair_stale_ts_calibration_boundary_one_differs() -> None: - raw = [Timestamped(5.0) for _ in range(9)] + [ - Timestamped(5.5), - Timestamped(5.6), - Timestamped(5.7), + raw = [_stamped(5.0) for _ in range(9)] + [ + _stamped(5.5), + _stamped(5.6), + _stamped(5.7), ] out: list[float] = [] rx.from_iterable(raw).pipe( repair_stale_ts(default_period=0.130, calibration_frames=10, now=lambda: 999.0) - ).subscribe(on_next=lambda item: out.append(item.ts)) + ).subscribe(on_next=lambda item: out.append(to_seconds(item.header.stamp))) assert 999.0 not in out + + +@pytest.mark.parametrize("override", [None, Time(sec=1700000000, nanosec=123456789)]) +def test_webrtc_xyz_conversion_preserves_points_and_explicit_time(override): + raw = {"data": {"stamp": -0.5, "data": {"points": np.array([[1.25, 2.5, 3.75]])}}} + cloud = pointcloud2_from_webrtc_lidar(cast("RawLidarMsg", raw), stamp=override) + decoded = PointCloud2.decode(cloud.encode()) + np.testing.assert_array_equal(pointcloud_xyz(decoded), [[1.25, 2.5, 3.75]]) + assert decoded.header.frame_id == "world" + assert decoded.header.stamp == ( + Time(sec=-1, nanosec=500000000) if override is None else override + ) diff --git a/dimos/robot/unitree/type/test_odometry.py b/dimos/robot/unitree/type/test_odometry.py index 8020684fb79..b383a18e6ee 100644 --- a/dimos/robot/unitree/type/test_odometry.py +++ b/dimos/robot/unitree/type/test_odometry.py @@ -14,9 +14,11 @@ from __future__ import annotations +from dimos_generated.geometry_msgs.msg import PoseStamped import pytest -from dimos.robot.unitree.type.odometry import Odometry +from dimos.msgs.geometry import yaw +from dimos.robot.unitree.type.odometry import pose_from_webrtc_odometry from dimos.utils.testing.replay import SensorReplay _EXPECTED_TOTAL_RAD = -4.05212 @@ -28,22 +30,24 @@ def test_dataset_size() -> None: def test_odometry_conversion_and_count() -> None: - """Each replay entry converts to :class:`Odometry` and count is correct.""" + """Each replay entry converts to :class:`PoseStamped` and count is correct.""" for raw in SensorReplay(name="raw_odometry_rotate_walk").iterate(): - odom = Odometry.from_msg(raw) + odom = pose_from_webrtc_odometry(raw) assert isinstance(raw, dict) - assert isinstance(odom, Odometry) + assert isinstance(odom, PoseStamped) def test_total_rotation_travel_iterate() -> None: total_rad = 0.0 prev_yaw: float | None = None - for odom in SensorReplay(name="raw_odometry_rotate_walk", autocast=Odometry.from_msg).iterate(): - yaw = odom.orientation.radians.z + for odom in SensorReplay( + name="raw_odometry_rotate_walk", autocast=pose_from_webrtc_odometry + ).iterate(): + angle = yaw(odom.pose.orientation) if prev_yaw is not None: - diff = yaw - prev_yaw + diff = angle - prev_yaw total_rad += diff - prev_yaw = yaw + prev_yaw = angle assert total_rad == pytest.approx(_EXPECTED_TOTAL_RAD, abs=0.001) diff --git a/dimos/robot/unitree/type/test_pose_conversion.py b/dimos/robot/unitree/type/test_pose_conversion.py new file mode 100644 index 00000000000..a8da638faed --- /dev/null +++ b/dimos/robot/unitree/type/test_pose_conversion.py @@ -0,0 +1,45 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import copy + +from dimos_generated.builtin_interfaces.msg import Time +from dimos_generated.geometry_msgs.msg import PoseStamped +from dimos_generated.std_msgs.msg import Header +import pytest + +from dimos.robot.unitree.type.odometry import pose_from_webrtc_odometry, raw_odometry_msg_sample + + +@pytest.mark.parametrize("seconds,nanoseconds", [(-1, 987654321), (1700000000, 123456789)]) +def test_device_pose_preserves_original_ros_header(seconds, nanoseconds): + raw = copy.deepcopy(raw_odometry_msg_sample) + raw["data"]["header"]["stamp"] = {"sec": seconds, "nanosec": nanoseconds} + message = pose_from_webrtc_odometry(raw) + decoded = PoseStamped.decode(message.encode()) + assert decoded.header == Header(frame_id="odom", stamp=Time(sec=seconds, nanosec=nanoseconds)) + assert decoded.pose.position.x == 5.961965 + assert decoded.pose.position.y == -2.916958 + assert decoded.pose.position.z == 0.319509 + assert decoded.pose.orientation.w == -0.242112 + message.header.frame_id = "changed" + assert raw["data"]["header"]["frame_id"] == "odom" + + +def test_explicit_arrival_header_is_copied(): + header = Header(frame_id="world", stamp=Time(sec=5, nanosec=123)) + message = pose_from_webrtc_odometry(raw_odometry_msg_sample, header=header) + assert message.header == header + header.stamp.nanosec = 999 + assert message.header.stamp.nanosec == 123 diff --git a/dimos/robot/unitree/unitree_skill_container.py b/dimos/robot/unitree/unitree_skill_container.py index 881e8eb3514..8a253809cb3 100644 --- a/dimos/robot/unitree/unitree_skill_container.py +++ b/dimos/robot/unitree/unitree_skill_container.py @@ -19,16 +19,21 @@ import math import time +from dimos_generated.geometry_msgs.msg import Point, Pose, PoseStamped, Vector3 +from dimos_generated.tf2_msgs.msg import TFMessage from unitree_webrtc_connect.constants import RTC_TOPIC from dimos.agents.annotation import skill from dimos.core.core import rpc from dimos.core.module import Module from dimos.core.stream import In -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.msgs.tf2_msgs.TFMessage import TFMessage +from dimos.msgs.geometry import ( + pose_from_transform, + quaternion_euler, + quaternion_from_euler, + translate_pose_local, +) +from dimos.msgs.time import header_now from dimos.navigation.base import NavigationState from dimos.navigation.navigation_spec import NavigationInterfaceSpec from dimos.robot.unitree.go2.connection_spec import GO2ConnectionSpec @@ -197,20 +202,24 @@ def _goal_pose( current: PoseStamped, x: float, y: float, degrees: float | None, relative: bool ) -> PoseStamped: """Where move_to sends the robot, in the world frame.""" - euler = current.orientation.to_euler() + roll, pitch, yaw = quaternion_euler(current.pose.orientation) if relative: - position = current.position + current.orientation.rotate_vector(Vector3(x, y, 0)) - yaw = euler.yaw + math.radians(degrees or 0.0) + translated = translate_pose_local(current.pose, Vector3(x=x, y=y)) + position = translated.position + heading = yaw + math.radians(degrees or 0.0) else: - position = Vector3(x, y, current.position.z) - yaw = euler.yaw if degrees is None else math.radians(degrees) - orientation = Quaternion.from_euler(Vector3(euler.roll, euler.pitch, yaw)) - return PoseStamped(position=position, orientation=orientation, frame_id="world") + position = Point(x=x, y=y, z=current.pose.position.z) + heading = yaw if degrees is None else math.radians(degrees) + orientation = quaternion_from_euler(roll, pitch, heading) + return PoseStamped( + header=header_now("world"), pose=Pose(position=position, orientation=orientation) + ) def _pose_text(pose: PoseStamped) -> str: - yaw = math.degrees(pose.orientation.to_euler().yaw) - return f"x={pose.position.x:.2f} y={pose.position.y:.2f} heading={yaw:.0f}deg" + heading = math.degrees(quaternion_euler(pose.pose.orientation)[2]) + position = pose.pose.position + return f"x={position.x:.2f} y={position.y:.2f} heading={heading:.0f}deg" class UnitreeSkillContainer(Module): @@ -256,12 +265,12 @@ def move_to( if tf is None: return "Failed to get the position of the robot." - goal = _goal_pose(tf.to_pose(), x, y, degrees, relative) + goal = _goal_pose(pose_from_transform(tf), x, y, degrees, relative) self._navigation.set_goal(goal) outcome = self._wait_for_goal() tf = self.tfbuffer.get("world", "base_link") - now = "unknown" if tf is None else _pose_text(tf.to_pose()) + now = "unknown" if tf is None else _pose_text(pose_from_transform(tf)) return f"{outcome}. Robot is at {now}; goal was {_pose_text(goal)}." def _wait_for_goal(self, timeout: float = 100.0, settle: float = 2.0) -> str: diff --git a/dimos/simulation/dimsim/cdr_schemas.py b/dimos/simulation/dimsim/cdr_schemas.py new file mode 100644 index 00000000000..44d4f858f30 --- /dev/null +++ b/dimos/simulation/dimsim/cdr_schemas.py @@ -0,0 +1,41 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Export canonical generated ROS 2 schemas for the standalone DimSim bundle. + +Run ``python -m dimos.simulation.dimsim.cdr_schemas`` after changing messages. +""" + +import json + +from dimos_generated.geometry_msgs.msg import PoseStamped, Twist +from dimos_generated.sensor_msgs.msg import Image, PointCloud2 + +from dimos.constants import DIMOS_PROJECT_ROOT + +SCHEMA_PATH = DIMOS_PROJECT_ROOT / "misc/DimSim/cli/bridge/cdr_schemas.ts" + + +def schema_source() -> str: + definitions = {t.msg_name: t.schema for t in (Image, PointCloud2, PoseStamped, Twist)} + return ( + "// Generated from the canonical .msg definitions; do not edit by hand.\n" + "export const schemas: Record = " + + json.dumps(definitions, indent=2) + + ";\n" + ) + + +if __name__ == "__main__": + SCHEMA_PATH.write_text(schema_source()) diff --git a/dimos/msgs/sensor_msgs/image_impls/AbstractImage.py b/dimos/simulation/dimsim/test_cdr_schemas.py similarity index 74% rename from dimos/msgs/sensor_msgs/image_impls/AbstractImage.py rename to dimos/simulation/dimsim/test_cdr_schemas.py index 3b901b874b8..c77bb9f8bdf 100644 --- a/dimos/msgs/sensor_msgs/image_impls/AbstractImage.py +++ b/dimos/simulation/dimsim/test_cdr_schemas.py @@ -12,6 +12,8 @@ # See the License for the specific language governing permissions and # limitations under the License. -# Backwards compatibility stub for unpickling old data. -# AbstractImage and ImageFormat were moved to Image. -from dimos.msgs.sensor_msgs.Image import Image as AbstractImage, ImageFormat # noqa: F401 +from dimos.simulation.dimsim.cdr_schemas import SCHEMA_PATH, schema_source + + +def test_standalone_dimsim_schemas_match_canonical_messages(): + assert SCHEMA_PATH.read_text() == schema_source() diff --git a/dimos/simulation/engines/base.py b/dimos/simulation/engines/base.py index d4b0735528d..594456e0e9a 100644 --- a/dimos/simulation/engines/base.py +++ b/dimos/simulation/engines/base.py @@ -22,7 +22,7 @@ if TYPE_CHECKING: from pathlib import Path - from dimos.msgs.sensor_msgs.JointState import JointState + from dimos_generated.sensor_msgs.msg import JointState class SimulationEngine(ABC): diff --git a/dimos/simulation/engines/mujoco_engine.py b/dimos/simulation/engines/mujoco_engine.py index feb65fe79e2..3d32204b71a 100644 --- a/dimos/simulation/engines/mujoco_engine.py +++ b/dimos/simulation/engines/mujoco_engine.py @@ -40,7 +40,7 @@ from dimos.utils.logging_config import setup_logger if TYPE_CHECKING: - from dimos.msgs.sensor_msgs.JointState import JointState + from dimos_generated.sensor_msgs.msg import JointState logger = setup_logger() @@ -724,15 +724,15 @@ def read_joint_efforts(self) -> list[float]: def write_joint_command(self, command: JointState) -> None: if command.position: self._command_mode = "position" - self._set_position_targets(command.position) + self._set_position_targets(list(command.position)) return if command.velocity: self._command_mode = "velocity" - self._set_velocity_targets(command.velocity) + self._set_velocity_targets(list(command.velocity)) return if command.effort: self._command_mode = "effort" - self._set_effort_targets(command.effort) + self._set_effort_targets(list(command.effort)) return def _set_position_targets(self, positions: list[float]) -> None: diff --git a/dimos/simulation/engines/mujoco_sim_module.py b/dimos/simulation/engines/mujoco_sim_module.py index 87ff5a0a212..d831b69ac37 100644 --- a/dimos/simulation/engines/mujoco_sim_module.py +++ b/dimos/simulation/engines/mujoco_sim_module.py @@ -32,6 +32,18 @@ import time from typing import Any +from dimos_generated.geometry_msgs.msg import ( + Point, + Pose, + PoseStamped, + Quaternion, + Transform, + TransformStamped, + Vector3, +) +from dimos_generated.sensor_msgs.msg import CameraInfo, Image, Imu, JointState, PointCloud2 +from dimos_generated.std_msgs.msg import Header +from dimos_generated.tf2_msgs.msg import TFMessage import mujoco import numpy as np from numpy.typing import NDArray @@ -43,16 +55,11 @@ from dimos.core.module import Module, ModuleConfig from dimos.core.stream import Out from dimos.hardware.sensors.camera.spec import DepthCameraConfig, DepthCameraHardware -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Transform import Transform -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo -from dimos.msgs.sensor_msgs.Image import Image, ImageFormat -from dimos.msgs.sensor_msgs.Imu import Imu -from dimos.msgs.sensor_msgs.JointState import JointState -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 -from dimos.msgs.tf2_msgs.TFMessage import TFMessage +from dimos.msgs.camera_info import camera_info_from_intrinsics, camera_info_with_stamp +from dimos.msgs.geometry import quaternion_from_matrix +from dimos.msgs.image import image_from_array +from dimos.msgs.pointcloud import pointcloud_from_rgbd, pointcloud_from_xyz, voxel_downsample_cloud +from dimos.msgs.time import time_from_seconds from dimos.simulation.engines.mujoco_engine import ( CameraConfig, CameraFrame, @@ -96,25 +103,21 @@ def _pose_matrix( def _transform_from_matrix( matrix: NDArray[np.float64], *, frame_id: str, child_frame_id: str, ts: float -) -> Transform: - return Transform( - translation=Vector3( - float(matrix[0, 3]), - float(matrix[1, 3]), - float(matrix[2, 3]), - ), - rotation=Quaternion.from_rotation_matrix(matrix[:3, :3]), - frame_id=frame_id, +) -> TransformStamped: + return TransformStamped( + header=Header(stamp=time_from_seconds(ts), frame_id=frame_id), child_frame_id=child_frame_id, - ts=ts, + transform=Transform( + translation=Vector3( + x=float(matrix[0, 3]), y=float(matrix[1, 3]), z=float(matrix[2, 3]) + ), + rotation=quaternion_from_matrix(matrix[:3, :3]), + ), ) -def _default_identity_transform() -> Transform: - return Transform( - translation=Vector3(0.0, 0.0, 0.0), - rotation=Quaternion(0.0, 0.0, 0.0, 1.0), - ) +def _default_identity_transform() -> TransformStamped: + return TransformStamped(transform=Transform(rotation=Quaternion(w=1.0))) def _imu_from_mujoco_wxyz( @@ -127,11 +130,10 @@ def _imu_from_mujoco_wxyz( ) -> Imu: w, x, y, z = quaternion return Imu( - orientation=Quaternion(x, y, z, w), - angular_velocity=Vector3(*gyroscope), - linear_acceleration=Vector3(*accelerometer), - frame_id=frame_id, - ts=ts, + header=Header(stamp=time_from_seconds(ts), frame_id=frame_id), + orientation=Quaternion(x=x, y=y, z=z, w=w), + angular_velocity=Vector3(x=gyroscope[0], y=gyroscope[1], z=gyroscope[2]), + linear_acceleration=Vector3(x=accelerometer[0], y=accelerometer[1], z=accelerometer[2]), ) @@ -260,7 +262,7 @@ class MujocoSimModuleConfig(ModuleConfig, DepthCameraConfig): height: int = 480 fps: int = 15 base_frame_id: str = "link7" - base_transform: Transform | None = Field(default_factory=_default_identity_transform) + base_transform: TransformStamped | None = Field(default_factory=_default_identity_transform) align_depth_to_color: bool = True enable_color: bool = True enable_depth: bool = True @@ -392,7 +394,7 @@ def get_color_camera_info(self) -> CameraInfo | None: base = self._camera_info_base if base is None: return None - return base.with_ts(self._camera_info_ts()) + return camera_info_with_stamp(base, time_from_seconds(self._camera_info_ts())) @rpc def get_depth_camera_info(self) -> CameraInfo | None: @@ -400,7 +402,7 @@ def get_depth_camera_info(self) -> CameraInfo | None: base = self._camera_info_base if base is None: return None - return base.with_ts(self._camera_info_ts()) + return camera_info_with_stamp(base, time_from_seconds(self._camera_info_ts())) @rpc def get_depth_scale(self) -> float: @@ -794,15 +796,18 @@ def _publish_shm_and_lcm(self, engine: MujocoEngine) -> None: ] # (w, x, y, z) per MuJoCo convention self.odom.publish( PoseStamped( - ts=time.time(), - frame_id="world", - position=Vector3(float(base_pos[0]), float(base_pos[1]), float(base_pos[2])), - orientation=Quaternion( - float(base_quat[1]), - float(base_quat[2]), - float(base_quat[3]), - float(base_quat[0]), - ), # PoseStamped uses x,y,z,w + header=Header(stamp=time_from_seconds(time.time()), frame_id="world"), + pose=Pose( + position=Point( + x=float(base_pos[0]), y=float(base_pos[1]), z=float(base_pos[2]) + ), + orientation=Quaternion( + x=float(base_quat[1]), + y=float(base_quat[2]), + z=float(base_quat[3]), + w=float(base_quat[0]), + ), + ), ) ) @@ -859,14 +864,14 @@ def _build_camera_info(self) -> None: fovy_rad = math.radians(fovy_deg) fy = h / (2.0 * math.tan(fovy_rad / 2.0)) fx = fy # square pixels - camera_info = CameraInfo.from_intrinsics( + camera_info = camera_info_from_intrinsics( fx=fx, fy=fy, cx=w / 2.0, cy=h / 2.0, width=w, height=h, - frame_id=self._color_optical_frame, + header=Header(frame_id=self._color_optical_frame), ) with self._state_lock: self._camera_info_base = camera_info @@ -914,20 +919,18 @@ def _publish_loop(self) -> None: self._latest_frame_ts = ts if self.config.enable_color: - color_img = Image( - data=frame.rgb, - format=ImageFormat.RGB, - frame_id=self._color_optical_frame, - ts=ts, + color_img = image_from_array( + frame.rgb, + encoding="rgb8", + header=Header(frame_id=self._color_optical_frame, stamp=time_from_seconds(ts)), ) self.color_image.publish(color_img) if self.config.enable_depth: - depth_img = Image( - data=frame.depth, - format=ImageFormat.DEPTH, - frame_id=self._color_optical_frame, - ts=ts, + depth_img = image_from_array( + frame.depth, + encoding="32FC1", + header=Header(frame_id=self._color_optical_frame, stamp=time_from_seconds(ts)), ) self.depth_image.publish(depth_img) @@ -952,16 +955,7 @@ def _publish_camera_info(self) -> None: if base is None: return ts = self._camera_info_ts() - info = CameraInfo( - height=base.height, - width=base.width, - distortion_model=base.distortion_model, - D=base.D, - K=base.K, - P=base.P, - frame_id=base.frame_id, - ts=ts, - ) + info = camera_info_with_stamp(base, time_from_seconds(ts)) self.camera_info.publish(info) self.depth_camera_info.publish(info) @@ -982,24 +976,26 @@ def _publish_tf(self, ts: float, frame: CameraFrame | None) -> None: self.tf.publish( TFMessage( - _transform_from_matrix( - optical_transform, - frame_id=parent_frame, - child_frame_id=self._color_optical_frame, - ts=ts, - ), - _transform_from_matrix( - optical_transform, - frame_id=parent_frame, - child_frame_id=self._depth_optical_frame, - ts=ts, - ), - _transform_from_matrix( - camera_transform, - frame_id=parent_frame, - child_frame_id=self._camera_link, - ts=ts, - ), + transforms=[ + _transform_from_matrix( + optical_transform, + frame_id=parent_frame, + child_frame_id=self._color_optical_frame, + ts=ts, + ), + _transform_from_matrix( + optical_transform, + frame_id=parent_frame, + child_frame_id=self._depth_optical_frame, + ts=ts, + ), + _transform_from_matrix( + camera_transform, + frame_id=parent_frame, + child_frame_id=self._camera_link, + ts=ts, + ), + ] ) ) @@ -1018,25 +1014,27 @@ def _generate_pointcloud(self) -> None: if frame is None: return try: - color_img = Image( - data=frame.rgb, - format=ImageFormat.RGB, - frame_id=self._color_optical_frame, - ts=frame.timestamp, + color_img = image_from_array( + frame.rgb, + encoding="rgb8", + header=Header( + frame_id=self._color_optical_frame, stamp=time_from_seconds(frame.timestamp) + ), ) - depth_img = Image( - data=frame.depth, - format=ImageFormat.DEPTH, - frame_id=self._color_optical_frame, - ts=frame.timestamp, + depth_img = image_from_array( + frame.depth, + encoding="32FC1", + header=Header( + frame_id=self._color_optical_frame, stamp=time_from_seconds(frame.timestamp) + ), ) - pcd = PointCloud2.from_rgbd( - color_image=color_img, - depth_image=depth_img, - camera_info=camera_info, + pcd = pointcloud_from_rgbd( + color=color_img, + depth=depth_img, + calibration=camera_info, depth_scale=1.0, ) - pcd = pcd.voxel_downsample(0.005) + pcd = voxel_downsample_cloud(pcd, 0.005) self.pointcloud.publish(pcd) except Exception as exc: logger.error("Pointcloud generation error", error=str(exc)) @@ -1064,12 +1062,11 @@ def _generate_mujoco_lidar_pointcloud(self) -> None: return try: - pcd = PointCloud2.from_numpy( + pcd = pointcloud_from_xyz( np.vstack(all_points), - frame_id="world", - timestamp=latest_ts or time.time(), + header=Header(frame_id="world", stamp=time_from_seconds(latest_ts or time.time())), ) - pcd = pcd.voxel_downsample(self.config.mujoco_lidar_voxel_size) + pcd = voxel_downsample_cloud(pcd, self.config.mujoco_lidar_voxel_size) self.pointcloud.publish(pcd) except Exception as exc: logger.error("MuJoCo lidar pointcloud generation error", error=str(exc)) diff --git a/dimos/simulation/engines/test_mujoco_sim_module.py b/dimos/simulation/engines/test_mujoco_sim_module.py index 3fa20048b52..48cdab2d263 100644 --- a/dimos/simulation/engines/test_mujoco_sim_module.py +++ b/dimos/simulation/engines/test_mujoco_sim_module.py @@ -21,10 +21,14 @@ from typing import Any from unittest.mock import MagicMock +from dimos_generated.geometry_msgs.msg import PoseStamped +from dimos_generated.sensor_msgs.msg import Imu +from dimos_generated.std_msgs.msg import Header import numpy as np import pytest -from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo +from dimos.msgs.camera_info import camera_info_from_intrinsics +from dimos.msgs.time import to_seconds from dimos.simulation.engines.mujoco_engine import CameraFrame, MujocoEngine from dimos.simulation.engines.mujoco_sim_module import MujocoSimModule, MujocoSimModuleConfig @@ -126,6 +130,27 @@ def cleanup(self) -> None: module._publish_shm_and_lcm(_FakeEngine) assert events == ["joint_state", "imu", "ready"] + pose = PoseStamped.decode(module.odom.publish.call_args.args[0].encode()) + assert pose.header.frame_id == "world" + assert (pose.pose.position.x, pose.pose.position.y, pose.pose.position.z) == ( + 0.0, + 0.0, + 0.75, + ) + assert pose.pose.orientation.w == 1.0 + imu = Imu.decode(module.imu.publish.call_args.args[0].encode()) + assert imu.orientation.w == 1.0 + assert (imu.angular_velocity.x, imu.angular_velocity.y, imu.angular_velocity.z) == ( + 0.1, + 0.2, + 0.3, + ) + assert ( + imu.linear_acceleration.x, + imu.linear_acceleration.y, + imu.linear_acceleration.z, + ) == (1.0, 2.0, 3.0) + assert imu.header.stamp.sec > 0 finally: module.stop() @@ -159,14 +184,28 @@ def disconnect(self) -> None: module._publish_tf(10.0, frame) color_tf, depth_tf, camera_link_tf = messages[-1].transforms - assert color_tf.frame_id == "link7" + assert color_tf.header.frame_id == "link7" assert color_tf.child_frame_id == "wrist_camera_color_optical_frame" - assert np.allclose(color_tf.translation.to_numpy(), [0.0, 0.0, 1.0]) - assert depth_tf.frame_id == "link7" + assert np.allclose( + [ + color_tf.transform.translation.x, + color_tf.transform.translation.y, + color_tf.transform.translation.z, + ], + [0.0, 0.0, 1.0], + ) + assert depth_tf.header.frame_id == "link7" assert depth_tf.child_frame_id == "wrist_camera_depth_optical_frame" - assert camera_link_tf.frame_id == "link7" + assert camera_link_tf.header.frame_id == "link7" assert camera_link_tf.child_frame_id == "wrist_camera_link" - assert np.allclose(camera_link_tf.translation.to_numpy(), [0.0, 0.0, 1.0]) + assert np.allclose( + [ + camera_link_tf.transform.translation.x, + camera_link_tf.transform.translation.y, + camera_link_tf.transform.translation.z, + ], + [0.0, 0.0, 1.0], + ) finally: module.stop() @@ -510,8 +549,14 @@ def test_publish_loop_stamps_messages_with_frame_timestamp() -> None: module = MujocoSimModule() try: module.config = MujocoSimModuleConfig(fps=1000) - module._camera_info_base = CameraInfo.from_intrinsics( - width=1, height=1, fx=1.0, fy=1.0, cx=0.5, cy=0.5, frame_id="wrist_camera_color_frame" + module._camera_info_base = camera_info_from_intrinsics( + width=1, + height=1, + fx=1.0, + fy=1.0, + cx=0.5, + cy=0.5, + header=Header(frame_id="wrist_camera_color_frame"), ) color: list[Any] = [] depth: list[Any] = [] @@ -525,13 +570,13 @@ def test_publish_loop_stamps_messages_with_frame_timestamp() -> None: _run_publish_loop(module, frame_ts) module._publish_camera_info() - assert [img.ts for img in color] == frame_ts - assert [img.ts for img in depth] == frame_ts - assert [msg.transforms[0].ts for msg in tf] == frame_ts + assert [to_seconds(img.header.stamp) for img in color] == frame_ts + assert [to_seconds(img.header.stamp) for img in depth] == frame_ts + assert [to_seconds(msg.transforms[0].header.stamp) for msg in tf] == frame_ts # camera_info rides the latest frame's sim clock, not wall time. - assert info[-1].ts == frame_ts[-1] - assert module.get_color_camera_info().ts == frame_ts[-1] - assert module.get_depth_camera_info().ts == frame_ts[-1] + assert to_seconds(info[-1].header.stamp) == frame_ts[-1] + assert to_seconds(module.get_color_camera_info().header.stamp) == frame_ts[-1] + assert to_seconds(module.get_depth_camera_info().header.stamp) == frame_ts[-1] finally: module.stop() @@ -540,8 +585,14 @@ def test_camera_info_falls_back_to_wall_clock_before_first_frame() -> None: module = MujocoSimModule() try: module.config = MujocoSimModuleConfig() - module._camera_info_base = CameraInfo.from_intrinsics( - width=1, height=1, fx=1.0, fy=1.0, cx=0.5, cy=0.5, frame_id="wrist_camera_color_frame" + module._camera_info_base = camera_info_from_intrinsics( + width=1, + height=1, + fx=1.0, + fy=1.0, + cx=0.5, + cy=0.5, + header=Header(frame_id="wrist_camera_color_frame"), ) assert module._latest_frame_ts is None assert module._camera_info_ts() == pytest.approx(time.time(), abs=5.0) diff --git a/dimos/simulation/habitat/connection.py b/dimos/simulation/habitat/connection.py index 673f1ea2770..2cf184d56a7 100644 --- a/dimos/simulation/habitat/connection.py +++ b/dimos/simulation/habitat/connection.py @@ -16,17 +16,15 @@ from __future__ import annotations +from dimos_generated.geometry_msgs.msg import Twist +from dimos_generated.nav_msgs.msg import Odometry +from dimos_generated.sensor_msgs.msg import CameraInfo, Image, PointCloud2 +from dimos_generated.tf2_msgs.msg import TFMessage from pydantic import Field from dimos.constants import DIMOS_PROJECT_ROOT from dimos.core.native_module import LogFormat, NativeModule, NativeModuleConfig from dimos.core.stream import In, Out -from dimos.msgs.geometry_msgs.Twist import Twist -from dimos.msgs.nav_msgs.Odometry import Odometry -from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo -from dimos.msgs.sensor_msgs.Image import Image -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 -from dimos.msgs.tf2_msgs.TFMessage import TFMessage class HabitatConnectionConfig(NativeModuleConfig): diff --git a/dimos/simulation/habitat/nix/flake.nix b/dimos/simulation/habitat/nix/flake.nix index 89016fef741..60508eff56d 100644 --- a/dimos/simulation/habitat/nix/flake.nix +++ b/dimos/simulation/habitat/nix/flake.nix @@ -12,8 +12,8 @@ # Provides the installer, not the simulator: habitat-sim is conda-only and # headless rendering needs the host's EGL driver, so this cannot be a derivation. devShells = forAll (pkgs: { - default = pkgs.mkShellNoCC { - packages = [ pkgs.micromamba pkgs.curl pkgs.cacert ]; + default = pkgs.mkShell { + packages = [ pkgs.micromamba pkgs.curl pkgs.cacert pkgs.cmake ]; }; }); }; diff --git a/dimos/simulation/habitat/nix/install.sh b/dimos/simulation/habitat/nix/install.sh index 59e523e773b..e4450a9873a 100755 --- a/dimos/simulation/habitat/nix/install.sh +++ b/dimos/simulation/habitat/nix/install.sh @@ -16,10 +16,24 @@ if [ ! -x ./env/bin/python ]; then python=3.9 habitat-sim headless withbullet fi -# Pinned to uv.lock: the native must speak the same zenoh wire version as the -# dimos peers, or SHM payloads arrive as unreadable handles. --no-deps skips -# lcm-dimos-fork, the LCM runtime; only dimos_lcm's pure-python encoders are needed. -./env/bin/pip install --no-input --no-deps "dimos-lcm==0.1.3" +# Build standalone generated CDR messages for Habitat's Python 3.9 interpreter. +# The DimOS checkout's environment generates the package; the native environment +# builds its own extension and never imports or installs DimOS at runtime. +if [ ! -x "$ROOT/.venv/bin/python" ]; then + echo "Set up the DimOS project virtualenv before building Habitat messages." >&2 + exit 1 +fi +if [ ! -f "$ROOT/build/message-codegen/install/lib/cmake/fastcdr/fastcdr-config.cmake" ]; then + bash "$ROOT/scripts/setup_message_codegen.sh" +fi +(cd "$ROOT" && .venv/bin/python -m dimos.message_codegen.generate --package \ + --output "$OUT/messages" \ + --type geometry_msgs/msg/Twist --type nav_msgs/msg/Odometry \ + --type sensor_msgs/msg/CameraInfo --type sensor_msgs/msg/Image \ + --type sensor_msgs/msg/PointCloud2 --type tf2_msgs/msg/TFMessage) +CMAKE_PREFIX_PATH="$ROOT/build/message-codegen/install${CMAKE_PREFIX_PATH:+;$CMAKE_PREFIX_PATH}" \ + ./env/bin/pip install --no-input "$OUT/messages/python" +# Keep the native transport wire version pinned to uv.lock. ./env/bin/pip install --no-input "eclipse-zenoh==1.10.1" numpy # Annotated HM3D house, no credentials. --no-replace resumes a partial download diff --git a/dimos/simulation/habitat/server.py b/dimos/simulation/habitat/server.py index a9c61cb7345..e23dd93ec2f 100644 --- a/dimos/simulation/habitat/server.py +++ b/dimos/simulation/habitat/server.py @@ -16,7 +16,7 @@ """Habitat-sim native: drives a scene from Twist, publishes dimos messages on zenoh. Runs under python 3.9 in its own env, so: no dimos imports, no 3.10+ syntax -(``test_server.py`` enforces both). ``dimos_lcm`` provides the encoders. +(``test_server.py`` enforces both). ``dimos_generated`` provides the standalone CDR encoders. Reads one JSON line on stdin: ``topics`` (port -> zenoh key), ``config``, ``session``. """ @@ -31,18 +31,17 @@ import time from typing import Any -from dimos_lcm.geometry_msgs.Quaternion import Quaternion -from dimos_lcm.geometry_msgs.Transform import Transform -from dimos_lcm.geometry_msgs.TransformStamped import TransformStamped -from dimos_lcm.geometry_msgs.Twist import Twist as LCMTwist -from dimos_lcm.geometry_msgs.Vector3 import Vector3 -from dimos_lcm.nav_msgs.Odometry import Odometry as LCMOdometry -from dimos_lcm.sensor_msgs.CameraInfo import CameraInfo as LCMCameraInfo -from dimos_lcm.sensor_msgs.Image import Image as LCMImage -from dimos_lcm.sensor_msgs.PointCloud2 import PointCloud2 as LCMPointCloud2 -from dimos_lcm.sensor_msgs.PointField import PointField -from dimos_lcm.std_msgs.Header import Header -from dimos_lcm.tf2_msgs.TFMessage import TFMessage as LCMTFMessage +from dimos_generated.geometry_msgs.msg import ( + Quaternion, + Transform, + TransformStamped, + Twist, + Vector3, +) +from dimos_generated.nav_msgs.msg import Odometry +from dimos_generated.sensor_msgs.msg import CameraInfo, Image, PointCloud2, PointField +from dimos_generated.std_msgs.msg import Header +from dimos_generated.tf2_msgs.msg import TFMessage import numpy as np import zenoh @@ -67,13 +66,12 @@ def warn(msg: str) -> None: def _stamp(header: Header, ts: float) -> None: - header.seq = 0 header.stamp.sec = int(ts) - header.stamp.nsec = int((ts - int(ts)) * 1e9) + header.stamp.nanosec = int((ts - int(ts)) * 1e9) def image_msg(array: np.ndarray, encoding: str, frame_id: str, ts: float) -> bytes: - m = LCMImage() + m = Image() m.header = Header() _stamp(m.header, ts) m.header.frame_id = frame_id @@ -83,26 +81,24 @@ def image_msg(array: np.ndarray, encoding: str, frame_id: str, ts: float) -> byt channels = 1 if array.ndim == 2 else array.shape[2] m.step = m.width * array.dtype.itemsize * channels view = memoryview(np.ascontiguousarray(array)).cast("B") - m.data_length = len(view) m.data = view - return bytes(m.lcm_encode()) + return bytes(m.encode()) def camera_info_msg(k: dict[str, float], frame_id: str, ts: float) -> bytes: - m = LCMCameraInfo() + m = CameraInfo() m.header = Header() _stamp(m.header, ts) m.header.frame_id = frame_id m.width, m.height = int(k["width"]), int(k["height"]) m.distortion_model = "plumb_bob" - m.D = [0.0] * 5 - m.D_length = 5 + m.d = [0.0] * 5 fx, fy, cx, cy = k["fx"], k["fy"], k["cx"], k["cy"] - m.K = [fx, 0.0, cx, 0.0, fy, cy, 0.0, 0.0, 1.0] - m.R = [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0] - m.P = [fx, 0.0, cx, 0.0, 0.0, fy, cy, 0.0, 0.0, 0.0, 1.0, 0.0] + m.k = [fx, 0.0, cx, 0.0, fy, cy, 0.0, 0.0, 1.0] + m.r = [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0] + m.p = [fx, 0.0, cx, 0.0, 0.0, fy, cy, 0.0, 0.0, 0.0, 1.0, 0.0] m.binning_x = m.binning_y = 0 - return bytes(m.lcm_encode()) + return bytes(m.encode()) def _xyzrgb_fields() -> list[Any]: @@ -119,12 +115,11 @@ def _xyzrgb_fields() -> list[Any]: def cloud_msg(points: np.ndarray, colors: np.ndarray, frame_id: str, ts: float) -> bytes: """xyz + packed-rgb PointCloud2, matching dimos PointCloud2.lcm_encode's layout.""" - m = LCMPointCloud2() + m = PointCloud2() m.header = Header() _stamp(m.header, ts) m.header.frame_id = frame_id m.fields = _xyzrgb_fields() - m.fields_length = 4 m.point_step = 16 m.is_bigendian = False m.is_dense = True @@ -133,9 +128,8 @@ def cloud_msg(points: np.ndarray, colors: np.ndarray, frame_id: str, ts: float) if len(points) == 0: m.height = 0 m.row_step = 0 - m.data_length = 0 m.data = b"" - return bytes(m.lcm_encode()) + return bytes(m.encode()) # ROS convention: rgb is a float32 whose bytes are [padding, r, g, b]. rgb_u32 = ( @@ -146,9 +140,8 @@ def cloud_msg(points: np.ndarray, colors: np.ndarray, frame_id: str, ts: float) data = np.column_stack([points.astype(np.float32), rgb_u32.view(np.float32)]).astype(np.float32) view = memoryview(np.ascontiguousarray(data)).cast("B") m.row_step = m.point_step * m.width - m.data_length = len(view) m.data = view - return bytes(m.lcm_encode()) + return bytes(m.encode()) def odometry_msg( @@ -159,7 +152,7 @@ def odometry_msg( child_frame_id: str, ts: float, ) -> bytes: - m = LCMOdometry() + m = Odometry() m.header = Header() _stamp(m.header, ts) m.header.frame_id = frame_id @@ -173,16 +166,16 @@ def odometry_msg( m.twist.twist.linear.y = float(twist[1]) m.twist.twist.angular.z = float(twist[2]) m.twist.covariance = [0.0] * 36 - return bytes(m.lcm_encode()) + return bytes(m.encode()) def tf_msg(links: list[tuple[str, str, Any, Any]], ts: float) -> bytes: """links: (parent, child, translation xyz, rotation xyzw).""" - m = LCMTFMessage() + m = TFMessage() out = [] for parent, child, xyz, quat in links: t = TransformStamped() - # Fresh nested messages: the generated bindings share defaults across instances. + # Each edge owns independent generated nested values. t.header = Header() _stamp(t.header, ts) t.header.frame_id = parent @@ -202,8 +195,7 @@ def tf_msg(links: list[tuple[str, str, Any, Any]], ts: float) -> bytes: t.transform = transform out.append(t) m.transforms = out - m.transforms_length = len(out) - return bytes(m.lcm_encode()) + return bytes(m.encode()) def unproject( @@ -368,7 +360,7 @@ def main() -> None: def on_cmd_vel(sample: Any) -> None: try: - t = LCMTwist.lcm_decode(bytes(sample.payload.to_bytes())) + t = Twist.decode(bytes(sample.payload.to_bytes())) cmd["vx"], cmd["vy"], cmd["wz"] = t.linear.x, t.linear.y, t.angular.z cmd["ts"] = time.time() except Exception as exc: diff --git a/dimos/simulation/habitat/test_server.py b/dimos/simulation/habitat/test_server.py index e58892f0cb3..63f6856862a 100644 --- a/dimos/simulation/habitat/test_server.py +++ b/dimos/simulation/habitat/test_server.py @@ -12,22 +12,23 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""The native encodes with dimos_lcm; dimos decodes. These pin that contract. +"""The native encodes with generated CDR; dimos decodes. These pin that contract. -server.py runs under python 3.9 in nix/env, but its encoders are pure dimos_lcm +server.py runs under python 3.9 in nix/env, but its encoders are standalone generated CDR and import fine here, so the wire format is testable without habitat installed. """ import ast from pathlib import Path +from dimos_generated.nav_msgs.msg import Odometry +from dimos_generated.sensor_msgs.msg import CameraInfo, Image, PointCloud2 +from dimos_generated.tf2_msgs.msg import TFMessage import numpy as np +from rosbags.typesys import Stores, get_typestore -from dimos.msgs.nav_msgs.Odometry import Odometry -from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo -from dimos.msgs.sensor_msgs.Image import Image -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 -from dimos.msgs.tf2_msgs.TFMessage import TFMessage +from dimos.msgs.image import image_view +from dimos.msgs.pointcloud import pointcloud_xyz from dimos.simulation.habitat import server SERVER_PY = Path(server.__file__) @@ -35,21 +36,21 @@ def test_color_image_round_trip(): rgb = np.arange(8 * 12 * 3, dtype=np.uint8).reshape(8, 12, 3) - out = Image.lcm_decode(server.image_msg(rgb, "rgb8", "camera_optical", 1.5)) - np.testing.assert_array_equal(out.data, rgb) - assert out.frame_id == "camera_optical" + out = Image.decode(server.image_msg(rgb, "rgb8", "camera_optical", 1.5)) + np.testing.assert_array_equal(image_view(out), rgb) + assert out.header.frame_id == "camera_optical" def test_depth_image_round_trip(): depth = np.linspace(0.2, 4.5, 8 * 12, dtype=np.float32).reshape(8, 12) - out = Image.lcm_decode(server.image_msg(depth, "32FC1", "camera_optical", 1.5)) - np.testing.assert_allclose(out.data, depth, atol=1e-6) + out = Image.decode(server.image_msg(depth, "32FC1", "camera_optical", 1.5)) + np.testing.assert_allclose(image_view(out), depth, atol=1e-6) def test_camera_info_round_trip(): k = {"fx": 320.0, "fy": 320.0, "cx": 320.0, "cy": 180.0, "width": 640.0, "height": 360.0} - out = CameraInfo.lcm_decode(server.camera_info_msg(k, "camera_optical", 1.5)) - matrix = out.get_K_matrix() + out = CameraInfo.decode(server.camera_info_msg(k, "camera_optical", 1.5)) + matrix = np.asarray(out.k).reshape(3, 3) assert (out.width, out.height) == (640, 360) np.testing.assert_allclose([matrix[0, 0], matrix[1, 1]], [320.0, 320.0]) np.testing.assert_allclose([matrix[0, 2], matrix[1, 2]], [320.0, 180.0]) @@ -58,23 +59,23 @@ def test_camera_info_round_trip(): def test_cloud_round_trip_keeps_points_and_colour(): pts = np.array([[1.0, 2.0, 3.0], [-4.0, 0.5, 0.25]], dtype=np.float32) colors = np.array([[255, 0, 0], [0, 128, 64]], dtype=np.uint8) - out = PointCloud2.lcm_decode(server.cloud_msg(pts, colors, "world", 1.5)) - got = out.points_f32() - assert out.frame_id == "world" + out = PointCloud2.decode(server.cloud_msg(pts, colors, "world", 1.5)) + got = pointcloud_xyz(out) + assert out.header.frame_id == "world" np.testing.assert_allclose(np.sort(got, axis=0), np.sort(pts, axis=0), atol=1e-5) def test_empty_cloud_is_valid(): - out = PointCloud2.lcm_decode( + out = PointCloud2.decode( server.cloud_msg(np.zeros((0, 3), np.float32), np.zeros((0, 3), np.uint8), "world", 1.0) ) - assert len(out) == 0 + assert out.width == 0 and len(out.data) == 0 def test_odometry_round_trip(): pos = np.array([1.0, -2.0, 0.5]) quat = np.array([0.0, 0.0, 0.3826834, 0.9238795]) # 45 deg yaw - out = Odometry.lcm_decode( + out = Odometry.decode( server.odometry_msg(pos, quat, (0.4, 0.0, 0.2), "world", "base_link", 1.5) ) np.testing.assert_allclose( @@ -82,7 +83,7 @@ def test_odometry_round_trip(): pos, atol=1e-6, ) - assert out.frame_id == "world" + assert out.header.frame_id == "world" assert out.child_frame_id == "base_link" @@ -92,14 +93,14 @@ def test_tf_round_trip_carries_the_optical_link(): ("base_link", "camera", (0.0, 0.0, 0.45), (0.0, 0.0, 0.0, 1.0)), ("camera", "camera_optical", (0.0, 0.0, 0.0), server.frames.OPTICAL_QUAT_XYZW), ] - out = TFMessage.lcm_decode(server.tf_msg(links, 1.5)) - pairs = {(t.frame_id, t.child_frame_id) for t in out.transforms} + out = TFMessage.decode(server.tf_msg(links, 1.5)) + pairs = {(t.header.frame_id, t.child_frame_id) for t in out.transforms} assert ("camera", "camera_optical") in pairs assert len(out.transforms) == 3 def test_each_tf_link_keeps_its_own_pose(): - """Regression: the generated bindings share nested defaults between instances. + """Regression: TF edges must retain independent nested transform values. Mutating ``Transform().translation`` in place made all three links serialize the last one's pose, which showed up as the whole map pointing up. Only a @@ -110,29 +111,41 @@ def test_each_tf_link_keeps_its_own_pose(): ("base_link", "camera", (0.0, 0.0, 0.45), (0.0, 0.0, 0.0, 1.0)), ("camera", "camera_optical", (0.0, 0.0, 0.0), server.frames.OPTICAL_QUAT_XYZW), ] - by_child = { - t.child_frame_id: t for t in TFMessage.lcm_decode(server.tf_msg(links, 1.5)).transforms - } + by_child = {t.child_frame_id: t for t in TFMessage.decode(server.tf_msg(links, 1.5)).transforms} base = by_child["base_link"] np.testing.assert_allclose( - [base.translation.x, base.translation.y, base.translation.z], [1.0, 2.0, 3.0] + [base.transform.translation.x, base.transform.translation.y, base.transform.translation.z], + [1.0, 2.0, 3.0], ) np.testing.assert_allclose( - [base.rotation.x, base.rotation.y, base.rotation.z, base.rotation.w], [0, 0, 0, 1] + [ + base.transform.rotation.x, + base.transform.rotation.y, + base.transform.rotation.z, + base.transform.rotation.w, + ], + [0, 0, 0, 1], ) cam = by_child["camera"] np.testing.assert_allclose( - [cam.translation.x, cam.translation.y, cam.translation.z], [0.0, 0.0, 0.45] + [cam.transform.translation.x, cam.transform.translation.y, cam.transform.translation.z], + [0.0, 0.0, 0.45], ) opt = by_child["camera_optical"] np.testing.assert_allclose( - [opt.translation.x, opt.translation.y, opt.translation.z], [0.0, 0.0, 0.0] + [opt.transform.translation.x, opt.transform.translation.y, opt.transform.translation.z], + [0.0, 0.0, 0.0], ) np.testing.assert_allclose( - [opt.rotation.x, opt.rotation.y, opt.rotation.z, opt.rotation.w], + [ + opt.transform.rotation.x, + opt.transform.rotation.y, + opt.transform.rotation.z, + opt.transform.rotation.w, + ], server.frames.OPTICAL_QUAT_XYZW, ) @@ -145,12 +158,15 @@ def test_tf_chain_puts_the_camera_looking_forward(): ("base_link", "camera", (0.0, 0.0, 0.45), (0.0, 0.0, 0.0, 1.0)), ("camera", "camera_optical", (0.0, 0.0, 0.0), server.frames.OPTICAL_QUAT_XYZW), ] - by_child = { - t.child_frame_id: t for t in TFMessage.lcm_decode(server.tf_msg(links, 1.5)).transforms - } + by_child = {t.child_frame_id: t for t in TFMessage.decode(server.tf_msg(links, 1.5)).transforms} def rot(t): - x, y, z, w = t.rotation.x, t.rotation.y, t.rotation.z, t.rotation.w + x, y, z, w = ( + t.transform.rotation.x, + t.transform.rotation.y, + t.transform.rotation.z, + t.transform.rotation.w, + ) return np.array( [ [1 - 2 * (y * y + z * z), 2 * (x * y - z * w), 2 * (x * z + y * w)], @@ -226,3 +242,56 @@ def test_every_frames_attribute_used_by_server_exists(): } missing = sorted(name for name in used if not hasattr(frames_mod, name)) assert not missing, f"server.py calls frames.{missing} which frames.py does not define" + + +def test_habitat_payloads_decode_with_independent_ros_types(): + types = get_typestore(Stores.ROS2_HUMBLE) + packets = [ + ( + "sensor_msgs/msg/Image", + server.image_msg(np.full((2, 3, 3), 7, dtype=np.uint8), "rgb8", "camera", 12.25), + ), + ( + "sensor_msgs/msg/CameraInfo", + server.camera_info_msg( + {"width": 3, "height": 2, "fx": 10, "fy": 11, "cx": 1, "cy": 1}, "camera", 12.25 + ), + ), + ( + "sensor_msgs/msg/PointCloud2", + server.cloud_msg( + np.array([[1, 2, 3]], dtype=np.float32), + np.array([[255, 128, 64]], dtype=np.uint8), + "world", + 12.25, + ), + ), + ( + "nav_msgs/msg/Odometry", + server.odometry_msg( + np.array([1, 2, 3]), + np.array([0, 0, 0, 1]), + (0.4, 0.2, 0.3), + "world", + "base_link", + 12.25, + ), + ), + ( + "tf2_msgs/msg/TFMessage", + server.tf_msg([("world", "base_link", (1, 2, 3), (0, 0, 0, 1))], 12.25), + ), + ] + decoded = {} + for typename, packet in packets: + message = types.deserialize_cdr(packet, typename) + header = ( + message.transforms[0].header if typename == "tf2_msgs/msg/TFMessage" else message.header + ) + assert (header.stamp.sec, header.stamp.nanosec) == (12, 250000000) + decoded[typename] = message + assert len(decoded["sensor_msgs/msg/Image"].data) == 18 + assert decoded["sensor_msgs/msg/CameraInfo"].k[0] == 10.0 + assert decoded["sensor_msgs/msg/PointCloud2"].row_step == 16 + assert decoded["nav_msgs/msg/Odometry"].pose.pose.position.z == 3.0 + assert decoded["tf2_msgs/msg/TFMessage"].transforms[0].transform.translation.x == 1.0 diff --git a/dimos/simulation/mujoco/direct_cmd_vel_explorer.py b/dimos/simulation/mujoco/direct_cmd_vel_explorer.py index 73bb35a9de2..3b012c4092f 100644 --- a/dimos/simulation/mujoco/direct_cmd_vel_explorer.py +++ b/dimos/simulation/mujoco/direct_cmd_vel_explorer.py @@ -16,11 +16,11 @@ import threading from typing import TYPE_CHECKING +from dimos_generated.geometry_msgs.msg import PoseStamped, Twist, Vector3 + from dimos.core.transport import PubSubTransport from dimos.core.transport_factory import make_transport -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.geometry_msgs.Twist import Twist -from dimos.msgs.geometry_msgs.Vector3 import Vector3 +from dimos.msgs.geometry import yaw if TYPE_CHECKING: from collections.abc import Callable @@ -82,13 +82,13 @@ def _drive_to(self, target_x: float, target_y: float) -> None: """Pursuit controller: steer toward the target while driving forward.""" while True: pose = self._wait_for_pose() - dx = target_x - pose.x - dy = target_y - pose.y + dx = target_x - pose.pose.position.x + dy = target_y - pose.pose.position.y distance = math.hypot(dx, dy) if distance < 0.3: break target_heading = math.atan2(dy, dx) - heading_error = self._normalize_angle(target_heading - pose.yaw) + heading_error = self._normalize_angle(target_heading - yaw(pose.pose.orientation)) # Only drive forward when roughly facing the target. if abs(heading_error) > 0.3: linear = 0.0 @@ -98,7 +98,7 @@ def _drive_to(self, target_x: float, target_y: float) -> None: assert self._cmd_vel is not None self._cmd_vel.broadcast( None, - Twist(linear=Vector3(linear, 0, 0), angular=Vector3(0, 0, angular)), + Twist(linear=Vector3(x=linear), angular=Vector3(z=angular)), ) self._stop() diff --git a/dimos/simulation/mujoco/model.py b/dimos/simulation/mujoco/model.py index bc309b73077..d7379724a4a 100644 --- a/dimos/simulation/mujoco/model.py +++ b/dimos/simulation/mujoco/model.py @@ -18,6 +18,7 @@ from pathlib import Path import xml.etree.ElementTree as ET +from dimos_generated.std_msgs.msg import Header from etils import epath import mujoco from mujoco_playground._src import mjx_env @@ -25,7 +26,7 @@ from dimos.core.global_config import GlobalConfig from dimos.mapping.occupancy.extrude_occupancy import generate_mujoco_scene -from dimos.msgs.nav_msgs.OccupancyGrid import OccupancyGrid +from dimos.msgs.occupancy import occupancy_from_file from dimos.simulation.mujoco.input_controller import InputController from dimos.simulation.mujoco.policy import G1OnnxController, Go1OnnxController, OnnxController from dimos.utils.data import get_data @@ -148,7 +149,7 @@ def _add_person_object(root: ET.Element) -> None: def load_scene_xml(config: GlobalConfig) -> str: if config.mujoco_room_from_occupancy: path = Path(config.mujoco_room_from_occupancy) - return generate_mujoco_scene(OccupancyGrid.from_path(path)) + return generate_mujoco_scene(occupancy_from_file(path, header=Header(frame_id="world"))) mujoco_room = config.mujoco_room or "office1" xml_file = (_get_data_dir() / f"scene_{mujoco_room}.xml").as_posix() diff --git a/dimos/simulation/mujoco/mujoco_process.py b/dimos/simulation/mujoco/mujoco_process.py index d8b36fc3dfe..fd4ae018c4a 100755 --- a/dimos/simulation/mujoco/mujoco_process.py +++ b/dimos/simulation/mujoco/mujoco_process.py @@ -28,7 +28,8 @@ from numpy.typing import NDArray from dimos.core.global_config import GlobalConfig -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 +from dimos.msgs.pointcloud import pointcloud_from_xyz +from dimos.msgs.time import header_now from dimos.simulation.mujoco.constants import ( DEPTH_CAMERA_FOV, LIDAR_FPS, @@ -212,10 +213,8 @@ def _run_simulation(config: GlobalConfig, shm: ShmReader) -> None: pcd.points = o3d.utility.Vector3dVector(combined_points) pcd = pcd.voxel_down_sample(voxel_size=LIDAR_RESOLUTION) - lidar_msg = PointCloud2( - pointcloud=pcd, - ts=time.time(), - frame_id="world", + lidar_msg = pointcloud_from_xyz( + np.asarray(pcd.points), header=header_now("world") ) shm.write_lidar(lidar_msg) diff --git a/dimos/simulation/mujoco/person_on_track.py b/dimos/simulation/mujoco/person_on_track.py index 280569ca008..78353c52459 100644 --- a/dimos/simulation/mujoco/person_on_track.py +++ b/dimos/simulation/mujoco/person_on_track.py @@ -14,13 +14,13 @@ from typing import Any +from dimos_generated.geometry_msgs.msg import Point, Pose, Quaternion import mujoco import numpy as np from numpy.typing import NDArray from dimos.core.transport import PubSubTransport from dimos.core.transport_factory import make_transport -from dimos.msgs.geometry_msgs.Pose import Pose class PersonPositionController: @@ -152,8 +152,8 @@ def tick(self) -> None: def _publish_pose(self, pos: NDArray[np.floating[Any]], heading: float) -> None: c, s = np.cos(heading / 2), np.sin(heading / 2) pose = Pose( - position=[pos[0], pos[1], 0.0], - orientation=[0.0, 0.0, s, c], # x, y, z, w + position=Point(x=float(pos[0]), y=float(pos[1]), z=0.0), + orientation=Quaternion(x=0.0, y=0.0, z=float(s), w=float(c)), # x, y, z, w ) self._transport.broadcast(None, pose) diff --git a/dimos/simulation/mujoco/shared_memory.py b/dimos/simulation/mujoco/shared_memory.py index f677863edf0..534efbb0a7c 100644 --- a/dimos/simulation/mujoco/shared_memory.py +++ b/dimos/simulation/mujoco/shared_memory.py @@ -15,13 +15,12 @@ from dataclasses import dataclass from multiprocessing import resource_tracker from multiprocessing.shared_memory import SharedMemory -import pickle from typing import Any +from dimos_generated.sensor_msgs.msg import PointCloud2 import numpy as np from numpy.typing import NDArray -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 from dimos.simulation.mujoco.constants import VIDEO_HEIGHT, VIDEO_WIDTH from dimos.utils.logging_config import setup_logger @@ -148,13 +147,16 @@ def write_odom(self, pos: NDArray[Any], quat: NDArray[Any], timestamp: float) -> self._increment_seq(2) def write_lidar(self, lidar_msg: PointCloud2) -> None: - data = pickle.dumps(lidar_msg) + data = lidar_msg.encode() data_len = len(data) if data_len > self.shm.lidar.size: logger.error(f"Lidar data too large: {data_len} > {self.shm.lidar.size}") return + # Odd sequence means a write is in progress; even commits the payload. + self._increment_seq(4) + # Write length len_array: NDArray[Any] = np.ndarray((1,), dtype=np.uint32, buffer=self.shm.lidar_len.buf) len_array[0] = data_len @@ -243,7 +245,7 @@ def write_command(self, linear: NDArray[Any], angular: NDArray[Any]) -> None: def read_lidar(self) -> tuple[PointCloud2 | None, int]: seq = self._get_seq(4) - if seq > 0: + if seq > 0 and seq % 2 == 0: # Read length len_array: NDArray[Any] = np.ndarray( (1,), dtype=np.uint32, buffer=self.shm.lidar_len.buf @@ -256,9 +258,11 @@ def read_lidar(self) -> tuple[PointCloud2 | None, int]: (data_len,), dtype=np.uint8, buffer=self.shm.lidar.buf ) data = bytes(lidar_array) + if self._get_seq(4) != seq: + return None, 0 try: - lidar_msg = pickle.loads(data) + lidar_msg = PointCloud2.decode(data) return lidar_msg, seq except Exception as e: logger.error(f"Failed to deserialize lidar message: {e}") diff --git a/dimos/simulation/mujoco/test_generated_control_consumers.py b/dimos/simulation/mujoco/test_generated_control_consumers.py new file mode 100644 index 00000000000..bd54c2e62db --- /dev/null +++ b/dimos/simulation/mujoco/test_generated_control_consumers.py @@ -0,0 +1,70 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from unittest.mock import MagicMock + +from dimos_generated.geometry_msgs.msg import Point, Pose, PoseStamped, Quaternion, Twist +from dimos_generated.std_msgs.msg import Header +import numpy as np +import pytest + +from dimos.msgs.geometry import quaternion_from_euler +from dimos.simulation.mujoco.direct_cmd_vel_explorer import DirectCmdVelExplorer +from dimos.simulation.mujoco.person_on_track import PersonTrackPublisher + + +@pytest.mark.parametrize( + "heading,expected_linear,expected_angular", [(0.0, 0.8, 0.0), (1.0, 0.0, -1.5)] +) +def test_pursuit_generated_pose_preserves_steering_and_stop( + heading: float, expected_linear: float, expected_angular: float, monkeypatch: pytest.MonkeyPatch +) -> None: + explorer = DirectCmdVelExplorer() + output = MagicMock() + explorer._cmd_vel = output + poses = iter( + [ + PoseStamped( + header=Header(frame_id="world"), + pose=Pose(position=Point(), orientation=quaternion_from_euler(0.0, 0.0, heading)), + ), + PoseStamped( + header=Header(frame_id="world"), + pose=Pose(position=Point(x=1.0), orientation=Quaternion(w=1.0)), + ), + ] + ) + monkeypatch.setattr(explorer, "_wait_for_pose", lambda: next(poses)) + explorer._drive_to(1.0, 0.0) + commands = [Twist.decode(call.args[1].encode()) for call in output.broadcast.call_args_list] + assert len(commands) == 2 + assert commands[0].linear.x == expected_linear + assert commands[0].angular.z == expected_angular + assert commands[1].linear.x == 0.0 + assert commands[1].angular.z == 0.0 + + +def test_person_track_generated_pose_preserves_position_and_heading( + monkeypatch: pytest.MonkeyPatch, +) -> None: + output = MagicMock() + monkeypatch.setattr( + "dimos.simulation.mujoco.person_on_track.make_transport", lambda channel, message: output + ) + publisher = PersonTrackPublisher([(1.0, 2.0), (1.0, 3.0), (2.0, 3.0)]) + publisher.tick() + pose = Pose.decode(output.broadcast.call_args.args[1].encode()) + assert (pose.position.x, pose.position.y, pose.position.z) == (1.0, 2.0, 0.0) + assert np.isclose(pose.orientation.z, np.sin(np.pi / 4)) + assert np.isclose(pose.orientation.w, np.cos(np.pi / 4)) diff --git a/dimos/simulation/mujoco/test_shared_memory.py b/dimos/simulation/mujoco/test_shared_memory.py new file mode 100644 index 00000000000..7b4301754a3 --- /dev/null +++ b/dimos/simulation/mujoco/test_shared_memory.py @@ -0,0 +1,55 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from pathlib import Path +import subprocess +import sys + +import numpy as np +import pytest + +from dimos.simulation.mujoco.shared_memory import ShmWriter + + +@pytest.fixture +def consumer(): + value = ShmWriter() + try: + yield value + finally: + value.cleanup() + + +def test_lidar_cross_process_generated_cdr(): + demo = Path(__file__).resolve().parents[3] / "examples/message-codegen/demo_mujoco_lidar_shm.py" + result = subprocess.run([sys.executable, str(demo)], capture_output=True, text=True, timeout=45) + assert result.returncode == 0, result.stdout + result.stderr + assert "PASS:" in result.stdout + assert "resource_tracker" not in result.stderr + + +def test_lidar_ignores_uncommitted_write(consumer): + sequence = np.ndarray((8,), dtype=np.int64, buffer=consumer.shm.seq.buf) + sequence[4] = 1 + assert consumer.read_lidar() == (None, 0) + + +@pytest.mark.parametrize("length", [0, 4, 2**32 - 1]) +def test_lidar_invalid_payload_is_not_delivered(consumer, length): + sequence = np.ndarray((8,), dtype=np.int64, buffer=consumer.shm.seq.buf) + sequence[4] = 2 + size = np.ndarray((1,), dtype=np.uint32, buffer=consumer.shm.lidar_len.buf) + size[0] = length + consumer.shm.lidar.buf[:4] = b"bad!" + assert consumer.read_lidar() == (None, 0) diff --git a/dimos/spec/control.py b/dimos/spec/control.py index b597b4faafa..81aa5ab7ff0 100644 --- a/dimos/spec/control.py +++ b/dimos/spec/control.py @@ -14,8 +14,9 @@ from typing import Protocol +from dimos_generated.geometry_msgs.msg import Twist + from dimos.core.stream import Out -from dimos.msgs.geometry_msgs.Twist import Twist class LocalPlanner(Protocol): diff --git a/dimos/spec/mapping.py b/dimos/spec/mapping.py index f35778f40b4..93a762f687a 100644 --- a/dimos/spec/mapping.py +++ b/dimos/spec/mapping.py @@ -14,9 +14,10 @@ from typing import Protocol +from dimos_generated.nav_msgs.msg import OccupancyGrid +from dimos_generated.sensor_msgs.msg import PointCloud2 + from dimos.core.stream import Out -from dimos.msgs.nav_msgs.OccupancyGrid import OccupancyGrid -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 class GlobalPointcloud(Protocol): diff --git a/dimos/spec/perception.py b/dimos/spec/perception.py index 4fac65ad020..72bdae0fbd8 100644 --- a/dimos/spec/perception.py +++ b/dimos/spec/perception.py @@ -14,12 +14,10 @@ from typing import Protocol +from dimos_generated.nav_msgs.msg import Odometry as OdometryMsg +from dimos_generated.sensor_msgs.msg import CameraInfo, Image as ImageMsg, Imu, PointCloud2 + from dimos.core.stream import Out -from dimos.msgs.nav_msgs.Odometry import Odometry as OdometryMsg -from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo -from dimos.msgs.sensor_msgs.Image import Image as ImageMsg -from dimos.msgs.sensor_msgs.Imu import Imu -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 class Image(Protocol): diff --git a/dimos/teleop/hosted/arm_command.py b/dimos/teleop/hosted/arm_command.py index 47b8aec6261..04d1a272016 100644 --- a/dimos/teleop/hosted/arm_command.py +++ b/dimos/teleop/hosted/arm_command.py @@ -24,15 +24,14 @@ import time from typing import Any -from dimos_lcm.geometry_msgs import TwistStamped as LCMTwistStamped +from dimos_generated.geometry_msgs.msg import PoseStamped, Twist, TwistStamped, Vector3 +from dimos_generated.std_msgs.msg import Float32, Header from reactivex.disposable import Disposable from dimos.control.coordinator import ControlCoordinator from dimos.core.core import rpc from dimos.core.stream import In, Out -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.geometry_msgs.TwistStamped import TwistStamped -from dimos.msgs.std_msgs.Float32 import Float32 +from dimos.msgs.time import to_seconds from dimos.teleop.hosted.command_executor import SerializedCommandExecutor from dimos.teleop.utils.teleop_transforms import webxr_to_robot from dimos.teleop.webxr.controller_types import Hand @@ -75,7 +74,8 @@ def __init__(self, **kwargs: Any) -> None: self._last_stale_warn = 0.0 self._last_future_warn = 0.0 - self._decoders[LCMTwistStamped._get_packed_fingerprint()] = self._on_twist_bytes + self._command_types["twist"] = TwistStamped + self._decoders["twist"] = self._on_twist_bytes # No local WebSocket server — the operator connects through the broker. def _start_server(self) -> None: @@ -103,25 +103,20 @@ def stop(self) -> None: # ─── Inbound command plane (operator → robot) ───────────────────── def _on_cmd_raw(self, data: Any) -> None: - """Fingerprint-dispatch LCM bytes (Pose/Joy inherited, Twist added here).""" + """Dispatch explicit CDR command frames (pose, joy and twist).""" if isinstance(data, str): data = data.encode() - decoder = self._decoders.get(data[:8]) - if decoder is None: - return - try: - decoder(data) - except Exception: - logger.warning("cmd_raw decode failed", exc_info=True) + if isinstance(data, bytes): + self._dispatch_binary_message(data) def _on_pose_bytes(self, data: bytes) -> None: """Controller pose → robot frame; stale/future/out-of-order dropped.""" - msg = PoseStamped.lcm_decode(data) + msg = PoseStamped.decode(data) try: - hand = self._resolve_hand(msg.frame_id) + hand = self._resolve_hand(msg.header.frame_id) except ValueError: return - ts = float(msg.ts) + ts = to_seconds(msg.header.stamp) if not math.isfinite(ts): return age = time.time() - ts @@ -148,8 +143,8 @@ def _on_twist_bytes(self, data: bytes) -> None: """Browser keyboard EE-twist → coordinator's eef_twist task.""" if self._estopped: return - msg = TwistStamped.lcm_decode(data) - ts = float(msg.ts) + msg = TwistStamped.decode(data) + ts = to_seconds(msg.header.stamp) if not math.isfinite(ts): return age = time.time() - ts @@ -172,13 +167,15 @@ def _on_twist_bytes(self, data: bytes) -> None: scale = self._translation_scale self.ee_twist_command.publish( TwistStamped( - linear=[ - msg.linear.x * scale, - msg.linear.y * scale, - msg.linear.z * scale, - ], - angular=[msg.angular.x, msg.angular.y, msg.angular.z], - ts=msg.ts, + header=Header(stamp=msg.header.stamp), + twist=Twist( + linear=Vector3( + x=msg.twist.linear.x * scale, + y=msg.twist.linear.y * scale, + z=msg.twist.linear.z * scale, + ), + angular=msg.twist.angular, + ), ) ) diff --git a/dimos/teleop/hosted/blueprints/cloudflare.py b/dimos/teleop/hosted/blueprints/cloudflare.py index dcf2340f41e..fc82ad59142 100644 --- a/dimos/teleop/hosted/blueprints/cloudflare.py +++ b/dimos/teleop/hosted/blueprints/cloudflare.py @@ -26,6 +26,9 @@ from __future__ import annotations +from dimos_generated.geometry_msgs.msg import Twist, TwistStamped +from dimos_generated.sensor_msgs.msg import Image + from dimos.core.coordination.blueprints import autoconnect from dimos.core.transport import ( CloudflareAudioTransport, @@ -36,9 +39,6 @@ from dimos.hardware.sensors.camera.realsense.camera import RealSenseCamera from dimos.mapping.costmapper import CostMapper from dimos.mapping.voxels.module import VoxelGridMapper -from dimos.msgs.geometry_msgs.Twist import Twist -from dimos.msgs.geometry_msgs.TwistStamped import TwistStamped -from dimos.msgs.sensor_msgs.Image import Image from dimos.navigation.movement_manager.movement_manager import MovementManager from dimos.navigation.replanning_a_star.module import ReplanningAStarPlanner from dimos.robot.manipulators.xarm.blueprints.teleop import ( diff --git a/dimos/teleop/hosted/camera_mux.py b/dimos/teleop/hosted/camera_mux.py index d6eeb7ae3c7..7d5dc07dd23 100644 --- a/dimos/teleop/hosted/camera_mux.py +++ b/dimos/teleop/hosted/camera_mux.py @@ -28,13 +28,16 @@ import time from typing import Any +from dimos_generated.sensor_msgs.msg import Image +from dimos_generated.std_msgs.msg import Header import numpy as np from reactivex.disposable import Disposable from dimos.core.core import rpc from dimos.core.module import Module, ModuleConfig from dimos.core.stream import In, Out -from dimos.msgs.sensor_msgs.Image import Image +from dimos.msgs.image import image_from_array, image_view +from dimos.msgs.time import to_nanoseconds from dimos.utils.logging_config import setup_logger logger = setup_logger() @@ -128,6 +131,8 @@ def _on_cam(self, cam: str, img: Image) -> None: def _composite(self) -> Image | None: """Selected frames → one even-sized Image; None on any error (a raise would kill the RxPY camera subscription).""" + import cv2 + with self._cam_lock: order = [c for c in self._cam_order if c in self._cam_selected] imgs = [self._cam_frames[c] for c in order if c in self._cam_frames] @@ -136,21 +141,27 @@ def _composite(self) -> Image | None: try: if len(imgs) == 1: return self._even_dims(self._stamp(self._downscale(imgs[0]))) - import cv2 - - target_h = min(im.data.shape[0] for im in imgs) + if len({im.encoding for im in imgs}) != 1: + raise ValueError("Selected camera encodings must match") + target_h = min(im.height for im in imgs) tiles = [] for im in imgs: - h, w = im.data.shape[:2] + data = image_view(im) + h, w = data.shape[:2] tiles.append( - cv2.resize(im.data, (max(1, int(w * target_h / h)), target_h)) + cv2.resize(data, (max(1, int(w * target_h / h)), target_h)) if h != target_h - else im.data + else data ) + latest = max(imgs, key=lambda im: to_nanoseconds(im.header.stamp)) return self._even_dims( self._stamp( self._downscale( - Image(data=np.hstack(tiles), format=imgs[0].format, frame_id="camera_mux") + image_from_array( + np.hstack(tiles), + encoding=imgs[0].encoding, + header=Header(stamp=latest.header.stamp, frame_id="camera_mux"), + ) ) ) ) @@ -162,28 +173,30 @@ def _composite(self) -> Image | None: def _even_dims(img: Image) -> Image: """Crop to even width/height — an odd composite crashes libx264's avcodec_open2 when a camera switch reopens the encoder.""" - data = img.data + data = image_view(img) if data.ndim < 2: return img h, w = data.shape[:2] if h % 2 == 0 and w % 2 == 0: return img data = data[: h - (h % 2), : w - (w % 2)] - return Image(data=np.ascontiguousarray(data), format=img.format, frame_id=img.frame_id) + return image_from_array( + np.ascontiguousarray(data), encoding=img.encoding, header=img.header + ) def _downscale(self, img: Image) -> Image: """Cap publish width at config.video_max_width (0 = off). Runs before _stamp so the strip's 16px cells stay decodable at the sent size.""" + import cv2 + max_w = self.config.video_max_width - if max_w <= 0 or img.data.ndim < 2: + if max_w <= 0 or image_view(img).ndim < 2: return img - h, w = img.data.shape[:2] + h, w = image_view(img).shape[:2] if w <= max_w: return img - import cv2 - - out = cv2.resize(img.data, (max_w, max(1, int(h * max_w / w)))) - return Image(data=out, format=img.format, frame_id=img.frame_id) + out = cv2.resize(image_view(img), (max_w, max(1, int(h * max_w / w)))) + return image_from_array(out, encoding=img.encoding, header=img.header) def _stamp(self, img: Image) -> Image: """Append (not overwrite) a bottom strip encoding capture time as B/W @@ -197,7 +210,7 @@ def _stamp(self, img: Image) -> Image: ] s = _STAMP_CELL_PX - data = img.data + data = image_view(img) if data.ndim < 2 or data.shape[1] < _STAMP_CELLS * s: if not self._stamp_warned: self._stamp_warned = True @@ -215,7 +228,7 @@ def _stamp(self, img: Image) -> Image: if bit: strip[:, i * s : (i + 1) * s] = 255 out = np.vstack([data, strip]) - return Image(data=out, format=img.format, frame_id=img.frame_id) + return image_from_array(out, encoding=img.encoding, header=img.header) def _set_cam_selection(self, data: bytes) -> None: """camera_select kind → filter to known cams, republish immediately so diff --git a/dimos/teleop/hosted/go2_command.py b/dimos/teleop/hosted/go2_command.py index bdac4661e7d..5ac70951f22 100644 --- a/dimos/teleop/hosted/go2_command.py +++ b/dimos/teleop/hosted/go2_command.py @@ -27,16 +27,22 @@ import time from typing import Any -from dimos_lcm.std_msgs import Bool +from dimos_generated.geometry_msgs.msg import ( + Point, + Pose, + PoseStamped, + Quaternion, + Twist, + TwistStamped, +) +from dimos_generated.std_msgs.msg import Bool from reactivex.disposable import Disposable from unitree_webrtc_connect.constants import SPORT_CMD from dimos.core.core import rpc from dimos.core.module import Module, ModuleConfig from dimos.core.stream import In, Out -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.geometry_msgs.Twist import Twist -from dimos.msgs.geometry_msgs.TwistStamped import TwistStamped +from dimos.msgs.time import header_now, to_nanoseconds from dimos.robot.unitree.go2.connection import GO2Connection from dimos.teleop.hosted.command_executor import SerializedCommandExecutor from dimos.utils.generic import finite_number @@ -100,7 +106,7 @@ def __init__(self, **kwargs: Any) -> None: self._obstacle_avoidance = True self._light = 0.0 self._posture = "StandReady" - self._last_cmd_ts = 0.0 + self._last_cmd_ns = 0 self._last_cmd_nonzero = False @rpc @@ -358,7 +364,8 @@ def _handle_nav_goal(self, msg: dict[str, Any]) -> None: self._send_ack(nonce, False) return pose = PoseStamped( - ts=time.time(), frame_id="world", position=[x, y, 0.0], orientation=[0, 0, 0, 1] + header=header_now("world"), + pose=Pose(position=Point(x=x, y=y), orientation=Quaternion(w=1)), ) try: self.goal_request.publish(pose) @@ -388,36 +395,48 @@ def _on_cmd_vel_in(self, twist: TwistStamped) -> None: """Guard raw operator drive, republish on tele_cmd_vel for MovementManager.""" if self._estopped: return - ts = float(twist.ts) - if not math.isfinite(ts): - # NaN ts passes every comparison below and would poison _last_cmd_ts. + try: + stamp_ns = to_nanoseconds(twist.header.stamp) + except ValueError: + logger.warning("dropping cmd_vel with invalid nanoseconds") return - age = time.time() - ts - if age > self.config.cmd_stale_after_sec: + age_ns = time.time_ns() - stamp_ns + if age_ns > self.config.cmd_stale_after_sec * 1_000_000_000: return - if age < 0: # future-stamped: don't advance _last_cmd_ts (would stall drive) + if age_ns < 0: # future-stamped: do not stall subsequent drive frames return - if ts <= self._last_cmd_ts: # out-of-order + if stamp_ns <= self._last_cmd_ns: return - self._last_cmd_ts = ts - - if not _all_finite(twist): + command = twist.twist + if not _all_finite(command): logger.warning("dropping non-finite cmd_vel") return + self._last_cmd_ns = stamp_ns + lin_max = self.config.max_linear_mps ang_max = self.config.max_angular_rps - twist.linear.x = _clamp(twist.linear.x, -lin_max, lin_max) - twist.linear.y = _clamp(twist.linear.y, -lin_max, lin_max) - twist.angular.z = _clamp(twist.angular.z, -ang_max, ang_max) + command.linear.x = _clamp(command.linear.x, -lin_max, lin_max) + command.linear.y = _clamp(command.linear.y, -lin_max, lin_max) + command.angular.z = _clamp(command.angular.z, -ang_max, ang_max) # Idle zeros would make MovementManager cancel the nav plan, so forward a # zero only as the release edge (prev frame moving), then stay silent. - moving = not twist.is_zero() + moving = any( + abs(value) > 1e-8 + for value in ( + command.linear.x, + command.linear.y, + command.linear.z, + command.angular.x, + command.angular.y, + command.angular.z, + ) + ) if not moving and not self._last_cmd_nonzero: return self._last_cmd_nonzero = moving - self.tele_cmd_vel.publish(Twist(linear=twist.linear, angular=twist.angular)) + self.tele_cmd_vel.publish(command) # ─── robot-authoritative state → stats module ───────────────────── diff --git a/dimos/teleop/hosted/hosted_stats.py b/dimos/teleop/hosted/hosted_stats.py index c572398b09f..3b8bdd66fdb 100644 --- a/dimos/teleop/hosted/hosted_stats.py +++ b/dimos/teleop/hosted/hosted_stats.py @@ -28,15 +28,17 @@ import time from typing import Any +from dimos_generated.dimos_msgs.msg import VideoStats +from dimos_generated.geometry_msgs.msg import TwistStamped from reactivex.disposable import Disposable from dimos.core.core import rpc from dimos.core.module import Module, ModuleConfig from dimos.core.stream import In, Out -from dimos.msgs.geometry_msgs.TwistStamped import TwistStamped +from dimos.msgs.time import to_seconds from dimos.robot.unitree.go2.connection import GO2Connection from dimos.teleop.utils.stream_stats import LiveStreamStats -from dimos.teleop.utils.video_stats import VideoStats +from dimos.teleop.utils.video_stats import video_stats_from_dict from dimos.utils.logging_config import setup_logger logger = setup_logger() @@ -108,7 +110,7 @@ def _on_state_json(self, data: Any) -> None: kind = msg.get("type") if kind == "video_stats": try: - self.video_stats.publish(VideoStats.from_dict(msg)) + self.video_stats.publish(video_stats_from_dict(msg)) except (TypeError, ValueError): logger.warning("state_reliable: malformed video_stats, dropping") elif kind == "clock_report": @@ -124,10 +126,11 @@ def _on_cmd_raw(self, data: Any) -> None: if isinstance(data, str): data = data.encode() try: - cmd = TwistStamped.lcm_decode(data) + cmd = TwistStamped.decode(data) + source_time = to_seconds(cmd.header.stamp) except Exception: return - self._cmd_stats.record(cmd.ts, nbytes=len(data)) + self._cmd_stats.record(source_time, nbytes=len(data)) self.cmd_vel_stamped.publish(cmd) def _on_robot_state(self, data: Any) -> None: diff --git a/dimos/teleop/hosted/map_compress.py b/dimos/teleop/hosted/map_compress.py index 0928080aa5b..b1c64758786 100644 --- a/dimos/teleop/hosted/map_compress.py +++ b/dimos/teleop/hosted/map_compress.py @@ -27,13 +27,17 @@ import time from typing import Any +from dimos_generated.geometry_msgs.msg import PoseStamped +from dimos_generated.nav_msgs.msg import OccupancyGrid +import numpy as np from reactivex.disposable import Disposable from dimos.core.core import rpc from dimos.core.module import Module, ModuleConfig from dimos.core.stream import In, Out -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.nav_msgs.OccupancyGrid import OccupancyGrid, block_max_reduce +from dimos.msgs.geometry import quaternion_euler +from dimos.msgs.occupancy import block_max_reduce, occupancy_view +from dimos.msgs.time import to_seconds from dimos.utils.logging_config import setup_logger logger = setup_logger() @@ -75,19 +79,18 @@ def stop(self) -> None: def _on_costmap(self, grid: OccupancyGrid) -> None: """Throttle → coarsen → colorize → PNG → map_out (under the 32 KB cap).""" + import cv2 + now = time.monotonic() if now - self._last_map_pub < 1.0 / self.config.map_hz: return - cells = grid.grid - if cells is None or cells.size == 0: - return - # A bad frame must drop, not kill the RxPY costmap subscription. try: - import cv2 - - res = grid.resolution + cells = occupancy_view(grid) + if cells.size == 0: + return + res = grid.info.resolution img_cells = cells if 0 < res < self.config.map_min_resolution: factor = max(1, round(self.config.map_min_resolution / res)) @@ -102,7 +105,7 @@ def _on_costmap(self, grid: OccupancyGrid) -> None: # origin lets the browser place map + robot: cell = (world_xy - origin)/res. h, w = img_cells.shape[:2] - origin = grid.origin.position + origin = grid.info.origin.position payload = { "type": "map", "fmt": "png", @@ -110,7 +113,7 @@ def _on_costmap(self, grid: OccupancyGrid) -> None: "h": int(h), "res": float(res), "origin": [float(origin.x), float(origin.y)], - "stamp": float(grid.ts), + "stamp": to_seconds(grid.header.stamp), "png_b64": png_b64, } data = json.dumps(payload, separators=(",", ":")).encode() @@ -132,10 +135,10 @@ def _on_odom(self, pose: PoseStamped) -> None: try: payload = { "type": "odom", - "x": float(pose.position.x), - "y": float(pose.position.y), - "yaw": float(pose.orientation.to_euler().yaw), - "ts": float(pose.ts), + "x": float(pose.pose.position.x), + "y": float(pose.pose.position.y), + "yaw": quaternion_euler(pose.pose.orientation)[2], + "ts": to_seconds(pose.header.stamp), } self.map_out.publish(json.dumps(payload).encode()) except Exception: @@ -146,7 +149,6 @@ def _on_odom(self, pose: PoseStamped) -> None: @staticmethod def _occupancy_to_bgra(cells: Any) -> Any: """Occupancy int8 {-1,0,1..100} → BGRA for PNG; unknown transparent.""" - import numpy as np # (B, G, R, A) — RGB reversed for OpenCV. c_unknown = (0, 0, 0, 0) diff --git a/dimos/teleop/hosted/test_arm_command.py b/dimos/teleop/hosted/test_arm_command.py index b95e953bb48..fa194ceed3c 100644 --- a/dimos/teleop/hosted/test_arm_command.py +++ b/dimos/teleop/hosted/test_arm_command.py @@ -30,14 +30,16 @@ from typing import Any from unittest.mock import MagicMock, patch +from dimos_generated.geometry_msgs.msg import PoseStamped, Twist, TwistStamped, Vector3 +from dimos_generated.std_msgs.msg import Header import pytest from dimos.core.module import Module -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.geometry_msgs.TwistStamped import TwistStamped +from dimos.msgs.time import time_from_seconds from dimos.teleop.hosted.arm_command import ArmCommandModule from dimos.teleop.webxr.controller_types import Hand, WebXRControllerState from dimos.utils.testing.waiting import wait_until +from dimos.web.relay_bridge.protocol import FrameHeader, encode_data_frame @pytest.fixture @@ -72,16 +74,40 @@ def _fake_init(self: Any, **kwargs: Any) -> None: module._cmd.stop() +def _frame(channel, message): + return encode_data_frame( + FrameHeader( + ch=channel, + seq=0, + ts=time.time(), + delivery="latest", + meta={"encoding": "cdr", "type": message.msg_name}, + ), + message.encode(), + ) + + def _pose_bytes(frame_id: str, ts: float | None = None) -> bytes: - return PoseStamped(ts=time.time() if ts is None else ts, frame_id=frame_id).lcm_encode() + return _frame( + "pose", + PoseStamped( + header=Header( + stamp=time_from_seconds(time.time() if ts is None else ts), frame_id=frame_id + ) + ), + ) def _twist_bytes(x: float = 0.1, angular_x: float = 0.0, ts: float | None = None) -> bytes: - # ts=None keeps TwistStamped's default stamp (now) — a fresh command. - kwargs = {} if ts is None else {"ts": ts} - return TwistStamped( - frame_id="eef_twist_arm", linear=[x, 0.0, 0.0], angular=[angular_x, 0.0, 0.0], **kwargs - ).lcm_encode() + return _frame( + "twist", + TwistStamped( + header=Header( + stamp=time_from_seconds(time.time() if ts is None else ts), frame_id="eef_twist_arm" + ), + twist=Twist(linear=Vector3(x=x), angular=Vector3(x=angular_x)), + ), + ) def _tick(module: ArmCommandModule) -> None: @@ -161,8 +187,8 @@ def test_twist_republished_without_task_address(module: ArmCommandModule) -> Non module._on_cmd_raw(_twist_bytes(0.2)) module.ee_twist_command.publish.assert_called_once() out = module.ee_twist_command.publish.call_args.args[0] - assert out.frame_id == "" # addressing is the port wiring, not the payload - assert out.linear.x == pytest.approx(0.2) + assert out.header.frame_id == "" # addressing is the port wiring, not the payload + assert out.twist.linear.x == pytest.approx(0.2) def test_ui_scale_disabled_is_rejected(module: ArmCommandModule) -> None: @@ -180,8 +206,8 @@ def test_ui_scale_updates_pose_and_keyboard_twist(module: ArmCommandModule) -> N assert _sent_acks(module) == [{"type": "cmd_ack", "nonce": 4, "ok": True}] assert module._translation_scale == 0.5 out = module.ee_twist_command.publish.call_args.args[0] - assert out.linear.x == pytest.approx(0.1) - assert out.angular.x == pytest.approx(0.3) + assert out.twist.linear.x == pytest.approx(0.1) + assert out.twist.angular.x == pytest.approx(0.3) def test_twist_dropped_while_estopped(module: ArmCommandModule) -> None: @@ -243,7 +269,7 @@ def test_controller_pose_publishes_on_hand_port(module: ArmCommandModule) -> Non assert not module._is_engaged[Hand.RIGHT] module.right_controller_output.publish.assert_called() out = module.right_controller_output.publish.call_args.args[0] - assert out.frame_id == "right" # handedness preserved; no task-name overwrite + assert out.header.frame_id == "right" # handedness preserved; no task-name overwrite module.left_controller_output.publish.assert_not_called() diff --git a/dimos/teleop/hosted/test_camera_mux.py b/dimos/teleop/hosted/test_camera_mux.py index c7a82fcfd67..35f8e811b63 100644 --- a/dimos/teleop/hosted/test_camera_mux.py +++ b/dimos/teleop/hosted/test_camera_mux.py @@ -24,10 +24,13 @@ import json from types import SimpleNamespace +from dimos_generated.sensor_msgs.msg import Image +from dimos_generated.std_msgs.msg import Header import numpy as np import pytest -from dimos.msgs.sensor_msgs.Image import Image, ImageFormat +from dimos.msgs.image import image_from_array, image_view +from dimos.msgs.time import time_from_nanoseconds, to_nanoseconds from dimos.teleop.hosted.camera_mux import CameraMuxModule @@ -55,7 +58,7 @@ def _make(cameras: list[str], **cfg: object) -> _Mux: def _img(w: int, h: int) -> Image: - return Image(data=np.zeros((h, w, 3), dtype=np.uint8), format=ImageFormat.BGR) + return image_from_array(np.zeros((h, w, 3), dtype=np.uint8), encoding="bgr8") def _feed(mux: _Mux, cam: str, img: Image) -> None: @@ -64,7 +67,7 @@ def _feed(mux: _Mux, cam: str, img: Image) -> None: def _is_even(img: Image) -> bool: - h, w = img.data.shape[:2] + h, w = image_view(img).shape[:2] return h % 2 == 0 and w % 2 == 0 @@ -73,8 +76,8 @@ def _is_even(img: Image) -> bool: def test_even_dims_crops_odd_width_and_height() -> None: out = CameraMuxModule._even_dims(_img(641, 481)) - assert out.data.shape[:2] == (480, 640) - assert out.data.flags["C_CONTIGUOUS"] # from_ndarray needs contiguous + assert image_view(out).shape[:2] == (480, 640) + assert image_view(out).flags["C_CONTIGUOUS"] # from_ndarray needs contiguous def test_even_dims_passes_even_through_unchanged() -> None: @@ -126,8 +129,8 @@ def test_composite_returns_none_on_error_does_not_raise() -> None: mux = _make(["cam1", "cam2"]) with mux._cam_lock: mux._cam_selected = ["cam1", "cam2"] - _feed(mux, "cam1", Image(data=np.zeros((480, 640, 3), np.uint8), format=ImageFormat.BGR)) - _feed(mux, "cam2", Image(data=np.zeros((480, 640, 4), np.uint8), format=ImageFormat.BGRA)) + _feed(mux, "cam1", image_from_array(np.zeros((480, 640, 3), np.uint8), encoding="bgr8")) + _feed(mux, "cam2", image_from_array(np.zeros((480, 640, 4), np.uint8), encoding="bgra8")) assert mux._composite() is None @@ -185,7 +188,7 @@ def test_latency_stamp_fits_640_width() -> None: mux = _make(["cam1"], latency_stamp=True) _feed(mux, "cam1", _img(640, 480)) out = mux._composite() - assert out is not None and out.data.shape[:2] == (480 + 16, 640) + assert out is not None and image_view(out).shape[:2] == (480 + 16, 640) def test_fps_cap_drops_frames_within_window() -> None: @@ -199,9 +202,39 @@ def test_fps_cap_window_released_on_failed_composite() -> None: mux = _make(["cam1", "cam2"], video_max_fps=30.0) with mux._cam_lock: mux._cam_selected = ["cam1", "cam2"] - _feed(mux, "cam2", Image(data=np.zeros((480, 640, 4), np.uint8), format=ImageFormat.BGRA)) + _feed(mux, "cam2", image_from_array(np.zeros((480, 640, 4), np.uint8), encoding="bgra8")) mux._on_cam("cam1", _img(640, 480)) # BGR + BGRA hstack fails → None assert mux.published == [] _feed(mux, "cam2", _img(640, 480)) mux._on_cam("cam1", _img(640, 480)) # still inside the window — must publish assert len(mux.published) == 1 + + +def test_generated_crop_scale_stamp_preserve_zero_source_header() -> None: + mux = _make(["cam1"], video_max_width=641, latency_stamp=True) + source = image_from_array( + np.zeros((721, 1280, 3), np.uint8), + encoding="bgr8", + header=Header(frame_id="camera_optical"), + ) + mux._on_cam("cam1", Image.decode(source.encode())) + output = Image.decode(mux.published[0].encode()) + assert output.width % 2 == 0 and output.height % 2 == 0 + assert output.header.frame_id == "camera_optical" + assert to_nanoseconds(output.header.stamp) == 0 + assert image_view(output).shape[1] == 640 + + +def test_composite_preserves_latest_exact_stamp_without_aliasing() -> None: + mux = _make(["cam1", "cam2"]) + mux._cam_selected = ["cam1", "cam2"] + first, second = _img(320, 240), _img(320, 240) + first.header.stamp = time_from_nanoseconds(1700000000123456788) + second.header.stamp = time_from_nanoseconds(1700000000123456789) + _feed(mux, "cam1", first) + _feed(mux, "cam2", second) + output = mux._composite() + assert output is not None and output.header.frame_id == "camera_mux" + assert to_nanoseconds(output.header.stamp) == 1700000000123456789 + output.header.stamp.nanosec = 0 + assert second.header.stamp.nanosec == 123456789 diff --git a/dimos/teleop/hosted/test_go2_command.py b/dimos/teleop/hosted/test_go2_command.py index 17430fdb165..d604791ac92 100644 --- a/dimos/teleop/hosted/test_go2_command.py +++ b/dimos/teleop/hosted/test_go2_command.py @@ -29,10 +29,12 @@ from typing import Any from unittest.mock import MagicMock +from dimos_generated.geometry_msgs.msg import PoseStamped, Twist, TwistStamped, Vector3 +from dimos_generated.std_msgs.msg import Header import pytest from dimos.core.module import Module -from dimos.msgs.geometry_msgs.TwistStamped import TwistStamped +from dimos.msgs.time import time_from_nanoseconds, time_from_seconds, to_nanoseconds from dimos.teleop.hosted.go2_command import ALLOWED_SPORT_CMDS, Go2CommandModule from dimos.utils.testing.waiting import wait_until @@ -62,8 +64,7 @@ def module(monkeypatch: pytest.MonkeyPatch) -> Iterator[Go2CommandModule]: def _twist(ts: float, *, vx: float = 0.3) -> TwistStamped: """A drive frame at time ``ts`` (vx=0.3 moving, vx=0 idle-joystick).""" - t = TwistStamped(ts=ts, linear=[0.0, 0.0, 0.0], angular=[0.0, 0.0, 0.0]) - t.linear.x = vx + t = TwistStamped(header=Header(stamp=time_from_seconds(ts)), twist=Twist(linear=Vector3(x=vx))) return t @@ -124,25 +125,25 @@ def test_drive_drops_stale(module: Go2CommandModule) -> None: def test_drive_drops_future(module: Go2CommandModule) -> None: module._on_cmd_vel_in(_twist(time.time() + 5.0)) module.tele_cmd_vel.publish.assert_not_called() - assert module._last_cmd_ts == 0.0 # future stamp must not poison the guard + assert module._last_cmd_ns == 0.0 # future stamp must not poison the guard def test_drive_drops_in_window_future_without_poisoning_guard(module: Go2CommandModule) -> None: # A future stamp SMALLER than cmd_stale_after_sec (clock skew) must still be - # rejected and must NOT advance _last_cmd_ts — otherwise every subsequent + # rejected and must NOT advance _last_cmd_ns — otherwise every subsequent # in-order frame would be dropped as out-of-order until wall-clock catches # up, stalling drive. Regression guard for the in-window future case. module._on_cmd_vel_in(_twist(time.time() + 0.2)) # +0.2s < 0.5s stale window module.tele_cmd_vel.publish.assert_not_called() - assert module._last_cmd_ts == 0.0 + assert module._last_cmd_ns == 0.0 # a normal fresh frame right after must still be forwarded module._on_cmd_vel_in(_twist(time.time())) module.tele_cmd_vel.publish.assert_called_once() def test_drive_drops_out_of_order(module: Go2CommandModule) -> None: - module._last_cmd_ts = time.time() - module._on_cmd_vel_in(_twist(module._last_cmd_ts - 0.1)) + module._last_cmd_ns = time.time_ns() + module._on_cmd_vel_in(_twist(module._last_cmd_ns / 1e9 - 0.1)) module.tele_cmd_vel.publish.assert_not_called() @@ -150,7 +151,7 @@ def test_drive_forwards_fresh(module: Go2CommandModule) -> None: ts = time.time() module._on_cmd_vel_in(_twist(ts)) module.tele_cmd_vel.publish.assert_called_once() - assert module._last_cmd_ts == ts + assert module._last_cmd_ns == to_nanoseconds(time_from_seconds(ts)) def test_drive_suppresses_idle_zero_stream(module: Go2CommandModule) -> None: @@ -179,17 +180,28 @@ def test_estopped_drive_is_dropped(module: Go2CommandModule) -> None: module.tele_cmd_vel.publish.assert_not_called() -def test_drive_drops_nan_timestamp(module: Go2CommandModule) -> None: - # A NaN ts passes every comparison and would poison _last_cmd_ts (ts <= NaN - # is False forever → reorder guard permanently disabled). Must be rejected. - module._on_cmd_vel_in(_twist(float("nan"))) +def test_drive_drops_invalid_nanoseconds(module: Go2CommandModule) -> None: + message = _twist(time.time()) + message.header.stamp.nanosec = 1_000_000_000 + module._on_cmd_vel_in(message) module.tele_cmd_vel.publish.assert_not_called() - assert module._last_cmd_ts == 0.0 # guard not poisoned + assert module._last_cmd_ns == 0 + + +def test_drive_orders_frames_one_nanosecond_apart(module: Go2CommandModule) -> None: + first = _twist(time.time() - 0.1) + stamp_ns = to_nanoseconds(first.header.stamp) + second = TwistStamped.decode(first.encode()) + second.header.stamp = time_from_nanoseconds(stamp_ns + 1) + module._on_cmd_vel_in(TwistStamped.decode(first.encode())) + module._on_cmd_vel_in(second) + assert module.tele_cmd_vel.publish.call_count == 2 + assert module._last_cmd_ns == stamp_ns + 1 def test_drive_drops_non_finite_velocity(module: Go2CommandModule) -> None: t = _twist(time.time()) - t.linear.x = float("inf") + t.twist.linear.x = float("inf") module._on_cmd_vel_in(t) module.tele_cmd_vel.publish.assert_not_called() @@ -197,8 +209,8 @@ def test_drive_drops_non_finite_velocity(module: Go2CommandModule) -> None: def test_drive_clamps_excessive_velocity(module: Go2CommandModule) -> None: # An untrusted operator sending huge velocities is clamped to the envelope. t = _twist(time.time()) - t.linear.x = 99.0 # way over max_linear_mps=1.5 - t.angular.z = -50.0 # way under -max_angular_rps=2.0 + t.twist.linear.x = 99.0 # way over max_linear_mps=1.5 + t.twist.angular.z = -50.0 # way under -max_angular_rps=2.0 module._on_cmd_vel_in(t) published = module.tele_cmd_vel.publish.call_args[0][0] assert published.linear.x == 1.5 @@ -377,7 +389,8 @@ def test_nav_goal_publishes_and_acks( module._handle_nav_goal({"x": 2.5, "y": -1.0, "nonce": 11}) (pose,) = module.goal_request.publish.call_args.args - assert pose.position.x == pytest.approx(2.5) + assert pose.pose.position.x == pytest.approx(2.5) + assert PoseStamped.decode(pose.encode()).header.frame_id == "world" assert acks == [(11, True)] diff --git a/dimos/teleop/hosted/test_hosted_stats.py b/dimos/teleop/hosted/test_hosted_stats.py index 1047a0c1493..78ec00b76d2 100644 --- a/dimos/teleop/hosted/test_hosted_stats.py +++ b/dimos/teleop/hosted/test_hosted_stats.py @@ -21,11 +21,12 @@ from types import SimpleNamespace from unittest.mock import MagicMock +from dimos_generated.geometry_msgs.msg import Twist, TwistStamped, Vector3 +from dimos_generated.std_msgs.msg import Header import pytest from dimos.core.module import Module -from dimos.msgs.geometry_msgs.TwistStamped import TwistStamped -from dimos.msgs.geometry_msgs.Vector3 import Vector3 +from dimos.msgs.time import time_from_seconds, to_seconds from dimos.teleop.hosted.hosted_stats import HostedStatsModule @@ -43,7 +44,9 @@ def module(monkeypatch: pytest.MonkeyPatch) -> Iterator[HostedStatsModule]: def _cmd(vx: float = 0.3, ts: float = 123.0) -> bytes: - return TwistStamped(ts=ts, linear=Vector3(vx, 0, 0), angular=Vector3(0, 0, 0)).lcm_encode() + return TwistStamped( + header=Header(stamp=time_from_seconds(ts)), twist=Twist(linear=Vector3(x=vx)) + ).encode() def test_telemetry_without_go2(monkeypatch: pytest.MonkeyPatch) -> None: @@ -64,8 +67,8 @@ def test_cmd_raw_republishes_stamped_for_recorder(module: HostedStatsModule) -> module._on_cmd_raw(_cmd(vx=0.5, ts=42.0)) module.cmd_vel_stamped.publish.assert_called_once() published = module.cmd_vel_stamped.publish.call_args[0][0] - assert published.ts == 42.0 - assert published.linear.x == 0.5 + assert to_seconds(published.header.stamp) == 42.0 + assert published.twist.linear.x == 0.5 def test_cmd_raw_records_stats(module: HostedStatsModule) -> None: @@ -94,3 +97,10 @@ def test_state_json_ignores_foreign_kind(module: HostedStatsModule) -> None: # estop/sport/etc. on the shared state plane are owned by other modules. module._on_state_json(json.dumps({"type": "estop", "nonce": "x"}).encode()) module.video_stats.publish.assert_not_called() + + +def test_cmd_raw_rejects_invalid_ros_nanoseconds(module: HostedStatsModule) -> None: + message = TwistStamped.decode(_cmd()) + message.header.stamp.nanosec = 1_000_000_000 + module._on_cmd_raw(message.encode()) + module.cmd_vel_stamped.publish.assert_not_called() diff --git a/dimos/teleop/hosted/test_map_compress.py b/dimos/teleop/hosted/test_map_compress.py index d39f981ca88..3dde277789b 100644 --- a/dimos/teleop/hosted/test_map_compress.py +++ b/dimos/teleop/hosted/test_map_compress.py @@ -20,31 +20,29 @@ from collections.abc import Iterator import json import math -from types import SimpleNamespace from typing import Any from unittest.mock import MagicMock import cv2 +from dimos_generated.geometry_msgs.msg import Point, Pose, PoseStamped, Quaternion +from dimos_generated.nav_msgs.msg import MapMetaData, OccupancyGrid +from dimos_generated.std_msgs.msg import Header import numpy as np import pytest -from dimos.core.module import Module -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.msgs.nav_msgs.OccupancyGrid import OccupancyGrid +from dimos.msgs.geometry import quaternion_from_euler +from dimos.msgs.time import time_from_seconds from dimos.teleop.hosted.map_compress import MapCompressModule @pytest.fixture -def module(monkeypatch: pytest.MonkeyPatch) -> Iterator[MapCompressModule]: - """A MapCompressModule with its throttle state initialized for real (only the - framework Module.__init__ is skipped) and its config / map_out port mocked.""" - monkeypatch.setattr(Module, "__init__", lambda self, **kwargs: None) +def module(mocker) -> Iterator[MapCompressModule]: module = MapCompressModule() - module.config = SimpleNamespace(map_hz=2.0, map_min_resolution=0.1, odom_hz=15.0) - module.map_out = MagicMock() - yield module + mocker.patch.object(module.map_out, "publish") + try: + yield module + finally: + module.stop() def _published_json(mock: MagicMock, msg_type: str) -> dict[str, Any] | None: @@ -60,8 +58,13 @@ def _published_json(mock: MagicMock, msg_type: str) -> dict[str, Any] | None: return None -def _occupancy(grid: Any) -> Any: - return OccupancyGrid(grid=np.asarray(grid, dtype=np.int8), resolution=0.1) +def _occupancy(grid: Any, resolution: float = 0.1) -> OccupancyGrid: + cells = np.asarray(grid, dtype=np.int8) + message = OccupancyGrid( + info=MapMetaData(width=cells.shape[1], height=cells.shape[0], resolution=resolution), + data=cells.ravel(), + ) + return OccupancyGrid.decode(message.encode()) def test_costmap_encodes_and_publishes_map(module: MapCompressModule) -> None: @@ -102,7 +105,7 @@ def test_block_max_preserves_obstacle_when_coarsening(module: MapCompressModule) # 0.02 m/cell → coarsen by 5× to reach 0.1. A lone obstacle must survive. cells = np.zeros((10, 10), dtype=np.int8) cells[3, 3] = 100 - module._on_costmap(OccupancyGrid(grid=cells, resolution=0.02)) + module._on_costmap(_occupancy(cells, resolution=0.02)) msg = _published_json(module.map_out, "map") assert msg is not None assert msg["res"] == pytest.approx(0.1) # coarsened 5× @@ -114,8 +117,11 @@ def test_block_max_preserves_obstacle_when_coarsening(module: MapCompressModule) def test_odom_publishes_planar_pose(module: MapCompressModule) -> None: - q = Quaternion.from_euler(Vector3(0.0, 0.0, math.pi / 2)) # yaw = 90° - pose = PoseStamped(ts=123.0, position=[1.5, -2.0, 0.3], orientation=[q.x, q.y, q.z, q.w]) + q = quaternion_from_euler(0.0, 0.0, math.pi / 2) # yaw = 90° + pose = PoseStamped( + header=Header(stamp=time_from_seconds(123)), + pose=Pose(position=Point(x=1.5, y=-2, z=0.3), orientation=q), + ) module._on_odom(pose) msg = _published_json(module.map_out, "odom") @@ -133,7 +139,7 @@ def test_empty_costmap_publishes_nothing(module: MapCompressModule) -> None: def test_odom_degenerate_quaternion_does_not_raise(module: MapCompressModule) -> None: # A zero quaternion makes to_euler() (scipy) raise; _on_odom runs inside an # RxPY subscriber, so it must drop the frame, not kill the odom stream. - pose = PoseStamped(ts=1.0, position=[0.0, 0.0, 0.0], orientation=[0.0, 0.0, 0.0, 0.0]) + pose = PoseStamped(pose=Pose(orientation=Quaternion(w=0))) module._on_odom(pose) # must not raise assert _published_json(module.map_out, "odom") is None @@ -144,3 +150,10 @@ def test_oversized_map_dropped(module: MapCompressModule) -> None: module._on_costmap(_occupancy(noise)) module.map_out.publish.assert_not_called() assert module._last_map_pub > 0 # throttle window consumed + + +def test_malformed_grid_does_not_break_next_frame(module): + module._on_costmap(OccupancyGrid(info=MapMetaData(width=2, height=2, resolution=0.1), data=[0])) + module.map_out.publish.assert_not_called() + module._on_costmap(_occupancy([[0, 100]])) + assert _published_json(module.map_out, "map")["w"] == 2 diff --git a/dimos/teleop/keyboard/keyboard_teleop_module.py b/dimos/teleop/keyboard/keyboard_teleop_module.py index 56e68f62ebf..360b7209409 100644 --- a/dimos/teleop/keyboard/keyboard_teleop_module.py +++ b/dimos/teleop/keyboard/keyboard_teleop_module.py @@ -43,12 +43,14 @@ if TYPE_CHECKING: from pygame.key import _ScancodeWrapper +from dimos_generated.geometry_msgs.msg import Twist, TwistStamped, Vector3 +from dimos_generated.std_msgs.msg import Float32 + from dimos.constants import DEFAULT_THREAD_JOIN_TIMEOUT from dimos.core.core import rpc from dimos.core.module import Module, ModuleConfig from dimos.core.stream import Out -from dimos.msgs.geometry_msgs.TwistStamped import TwistStamped -from dimos.msgs.std_msgs.Float32 import Float32 +from dimos.msgs.time import header_now from dimos.utils.logging_config import setup_logger logger = setup_logger() @@ -235,7 +237,15 @@ def _publish_twist( linear: TwistVector = (0.0, 0.0, 0.0), angular: TwistVector = (0.0, 0.0, 0.0), ) -> None: - self.ee_twist_command.publish(TwistStamped(linear=list(linear), angular=list(angular))) + self.ee_twist_command.publish( + TwistStamped( + header=header_now(), + twist=Twist( + linear=Vector3(x=linear[0], y=linear[1], z=linear[2]), + angular=Vector3(x=angular[0], y=angular[1], z=angular[2]), + ), + ) + ) def _publish_gripper_command(self, *, opening: float) -> None: """Publish a changed normalized gripper opening.""" diff --git a/dimos/teleop/keyboard/test_keyboard_teleop_module.py b/dimos/teleop/keyboard/test_keyboard_teleop_module.py index f04845006e6..1a1ba1a680c 100644 --- a/dimos/teleop/keyboard/test_keyboard_teleop_module.py +++ b/dimos/teleop/keyboard/test_keyboard_teleop_module.py @@ -16,9 +16,9 @@ from collections.abc import Iterator +from dimos_generated.geometry_msgs.msg import TwistStamped import pytest -from dimos.msgs.geometry_msgs.TwistStamped import TwistStamped import dimos.teleop.keyboard.keyboard_teleop_module as keyboard_mod from dimos.teleop.keyboard.keyboard_teleop_module import ( KeyboardTeleopModule, @@ -50,11 +50,11 @@ def test_publish_twist_emits_unaddressed_twist_stamped( module._publish_twist(linear=(0.1, 0.2, 0.3), angular=(0.4, 0.5, 0.6)) - msg = publish.call_args.args[0] + msg = TwistStamped.decode(publish.call_args.args[0].encode()) assert isinstance(msg, TwistStamped) - assert msg.frame_id == "" # no task-name address in the payload - assert [msg.linear.x, msg.linear.y, msg.linear.z] == [0.1, 0.2, 0.3] - assert [msg.angular.x, msg.angular.y, msg.angular.z] == [0.4, 0.5, 0.6] + assert msg.header.frame_id == "" # no task-name address in the payload + assert [msg.twist.linear.x, msg.twist.linear.y, msg.twist.linear.z] == [0.1, 0.2, 0.3] + assert [msg.twist.angular.x, msg.twist.angular.y, msg.twist.angular.z] == [0.4, 0.5, 0.6] def test_publish_twist_defaults_to_zero_twist(module: KeyboardTeleopModule, mocker) -> None: @@ -62,9 +62,9 @@ def test_publish_twist_defaults_to_zero_twist(module: KeyboardTeleopModule, mock module._publish_twist() - msg = publish.call_args.args[0] - assert [msg.linear.x, msg.linear.y, msg.linear.z] == [0.0, 0.0, 0.0] - assert [msg.angular.x, msg.angular.y, msg.angular.z] == [0.0, 0.0, 0.0] + msg = TwistStamped.decode(publish.call_args.args[0].encode()) + assert [msg.twist.linear.x, msg.twist.linear.y, msg.twist.linear.z] == [0.0, 0.0, 0.0] + assert [msg.twist.angular.x, msg.twist.angular.y, msg.twist.angular.z] == [0.0, 0.0, 0.0] def test_twist_from_keys_maps_translation_keys_to_eef_linear_twist() -> None: @@ -98,9 +98,9 @@ def test_final_key_release_publishes_zero_velocity(module: KeyboardTeleopModule, assert held == set() assert publish.call_count == 1 - msg = publish.call_args.args[0] - assert [msg.linear.x, msg.linear.y, msg.linear.z] == [0.0, 0.0, 0.0] - assert [msg.angular.x, msg.angular.y, msg.angular.z] == [0.0, 0.0, 0.0] + msg = TwistStamped.decode(publish.call_args.args[0].encode()) + assert [msg.twist.linear.x, msg.twist.linear.y, msg.twist.linear.z] == [0.0, 0.0, 0.0] + assert [msg.twist.angular.x, msg.twist.angular.y, msg.twist.angular.z] == [0.0, 0.0, 0.0] def test_keyup_preserves_remaining_motion_key(module: KeyboardTeleopModule, mocker) -> None: @@ -112,8 +112,8 @@ def test_keyup_preserves_remaining_motion_key(module: KeyboardTeleopModule, mock assert held == {keyboard_mod.pygame.K_a} assert publish.call_count == 1 - msg = publish.call_args.args[0] - assert [msg.linear.x, msg.linear.y, msg.linear.z] == [0.0, 0.05, 0.0] + msg = TwistStamped.decode(publish.call_args.args[0].encode()) + assert [msg.twist.linear.x, msg.twist.linear.y, msg.twist.linear.z] == [0.0, 0.05, 0.0] def test_keyup_publishes_directly_without_timeout_wait( diff --git a/dimos/teleop/phone/phone_extensions.py b/dimos/teleop/phone/phone_extensions.py index e46bab239a2..1839defb728 100644 --- a/dimos/teleop/phone/phone_extensions.py +++ b/dimos/teleop/phone/phone_extensions.py @@ -19,11 +19,10 @@ - SimplePhoneTeleop: Filters to ground robot axes and outputs cmd_vel: Out[Twist] """ +from dimos_generated.geometry_msgs.msg import Twist, TwistStamped, Vector3 + from dimos.core.core import rpc from dimos.core.stream import Out -from dimos.msgs.geometry_msgs.Twist import Twist -from dimos.msgs.geometry_msgs.TwistStamped import TwistStamped -from dimos.msgs.geometry_msgs.Vector3 import Vector3 from dimos.teleop.phone.phone_teleop_module import PhoneTeleopModule @@ -48,7 +47,7 @@ def stop(self) -> None: def _publish_msg(self, output_msg: TwistStamped) -> None: self.cmd_vel.publish( Twist( - linear=Vector3(x=output_msg.linear.x, y=output_msg.linear.y, z=0.0), - angular=Vector3(x=0.0, y=0.0, z=output_msg.linear.z), + linear=Vector3(x=output_msg.twist.linear.x, y=output_msg.twist.linear.y, z=0.0), + angular=Vector3(x=0.0, y=0.0, z=output_msg.twist.linear.z), ) ) diff --git a/dimos/teleop/phone/phone_teleop_module.py b/dimos/teleop/phone/phone_teleop_module.py index 241e0538895..2417bcf529c 100644 --- a/dimos/teleop/phone/phone_teleop_module.py +++ b/dimos/teleop/phone/phone_teleop_module.py @@ -27,8 +27,8 @@ import time from typing import Any -from dimos_lcm.geometry_msgs import TwistStamped as LCMTwistStamped -from dimos_lcm.std_msgs import Bool as LCMBool +from dimos_generated.geometry_msgs.msg import Twist, TwistStamped, Vector3 +from dimos_generated.std_msgs.msg import Bool, Header from fastapi import WebSocket, WebSocketDisconnect from fastapi.responses import HTMLResponse from fastapi.staticfiles import StaticFiles @@ -37,11 +37,12 @@ from dimos.core.core import rpc from dimos.core.module import Module, ModuleConfig from dimos.core.stream import Out -from dimos.msgs.geometry_msgs.Twist import Twist -from dimos.msgs.geometry_msgs.TwistStamped import TwistStamped -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.msgs.std_msgs.Bool import Bool from dimos.utils.logging_config import setup_logger +from dimos.web.relay_bridge.protocol import ( + ProtocolError, + decode_data_frame, + peek_data_frame_lengths, +) from dimos.web.robot_web_interface import RobotWebInterface logger = setup_logger() @@ -88,11 +89,11 @@ def __init__(self, **kwargs: Any) -> None: self._web_server = RobotWebInterface(port=self.config.server_port) self._web_server_thread: threading.Thread | None = None - # Fingerprint-based message dispatch table - self._decoders: dict[bytes, Any] = { - LCMTwistStamped._get_packed_fingerprint(): self._on_sensors_bytes, - LCMBool._get_packed_fingerprint(): self._on_button_bytes, + self._command_types: dict[str, type[TwistStamped] | type[Bool]] = { + "sensors": TwistStamped, + "button": Bool, } + self._decoders = {"sensors": self._on_sensors_bytes, "button": self._on_button_bytes} self._setup_routes() @@ -104,6 +105,13 @@ async def teleop_index() -> HTMLResponse: index_path = STATIC_DIR / "index.html" return HTMLResponse(content=index_path.read_text()) + @self._web_server.app.get("/teleop/schema") + async def command_schemas() -> dict[str, dict[str, str]]: + return { + channel: {"type": message_type.msg_name, "definition": message_type.schema} + for channel, message_type in self._command_types.items() + } + if STATIC_DIR.is_dir(): self._web_server.app.mount( "/static", StaticFiles(directory=str(STATIC_DIR)), name="teleop_static" @@ -116,17 +124,33 @@ async def websocket_endpoint(ws: WebSocket) -> None: try: while True: data = await ws.receive_bytes() - fingerprint = data[:8] - decoder = self._decoders.get(fingerprint) - if decoder: - decoder(data) - else: - logger.warning(f"Unknown message fingerprint: {fingerprint.hex()}") + self._dispatch_binary_message(data) except WebSocketDisconnect: logger.info("Phone client disconnected") except Exception: logger.exception("WebSocket error") + def _dispatch_binary_message(self, data: bytes) -> bool: + """Route a complete CDR frame by explicit channel and schema identity.""" + try: + lengths = peek_data_frame_lengths(data) + if lengths is None or lengths[2] != len(data): + return False + frame = decode_data_frame(data) + message_type = self._command_types.get(frame.header.ch) + metadata = frame.header.meta or {} + if ( + message_type is None + or metadata.get("encoding") != "cdr" + or metadata.get("type") != message_type.msg_name + ): + return False + self._decoders[frame.header.ch](frame.payload) + except (ProtocolError, ValueError): + logger.warning("Dropping malformed phone command frame") + return False + return True + @rpc def start(self) -> None: super().start() @@ -156,14 +180,14 @@ def _disengage(self) -> None: logger.info("Phone teleop disengaged") def _on_sensors_bytes(self, data: bytes) -> None: - """Decode raw LCM bytes into TwistStamped and update sensor state.""" - msg = TwistStamped.lcm_decode(data) + """Decode generated CDR bytes into TwistStamped and update sensor state.""" + msg = TwistStamped.decode(data) with self._lock: self._current_sensors = msg def _on_button_bytes(self, data: bytes) -> None: - """Decode raw LCM bytes into Bool and update button state.""" - msg = Bool.lcm_decode(data) + """Decode generated CDR bytes into Bool and update button state.""" + msg = Bool.decode(data) with self._lock: self._teleop_button = bool(msg.data) @@ -250,7 +274,13 @@ def _get_output_twist(self) -> TwistStamped | None: if current is None or initial is None: return None - delta: Twist = Twist(current) - Twist(initial) + delta = Twist( + linear=Vector3( + x=current.twist.linear.x - initial.twist.linear.x, + y=current.twist.linear.y - initial.twist.linear.y, + z=current.twist.linear.z - initial.twist.linear.z, + ) + ) # Handle yaw wraparound (linear.z = yaw, 0-360 degrees) d_yaw = delta.linear.z @@ -261,17 +291,18 @@ def _get_output_twist(self) -> TwistStamped | None: cfg = self.config return TwistStamped( - ts=current.ts, - frame_id="phone", - linear=Vector3( - x=-delta.linear.y * cfg.linear_gain, # pitch forward -> drive forward - y=-delta.linear.x * cfg.linear_gain, # roll right -> strafe right - z=d_yaw * cfg.linear_gain, # yaw delta - ), - angular=Vector3( - x=current.angular.x * cfg.angular_gain, - y=current.angular.y * cfg.angular_gain, - z=current.angular.z * cfg.angular_gain, + header=Header(stamp=current.header.stamp, frame_id="phone"), + twist=Twist( + linear=Vector3( + x=-delta.linear.y * cfg.linear_gain, # pitch forward -> drive forward + y=-delta.linear.x * cfg.linear_gain, # roll right -> strafe right + z=d_yaw * cfg.linear_gain, # yaw delta + ), + angular=Vector3( + x=current.twist.angular.x * cfg.angular_gain, + y=current.twist.angular.y * cfg.angular_gain, + z=current.twist.angular.z * cfg.angular_gain, + ), ), ) diff --git a/dimos/teleop/phone/test_phone_cdr.py b/dimos/teleop/phone/test_phone_cdr.py new file mode 100644 index 00000000000..e426374ffc1 --- /dev/null +++ b/dimos/teleop/phone/test_phone_cdr.py @@ -0,0 +1,121 @@ +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Exercise schema advertisement and CDR command routing without physical sensors.""" + +from collections.abc import Iterator + +from dimos_generated.builtin_interfaces.msg import Time +from dimos_generated.geometry_msgs.msg import Twist, TwistStamped, Vector3 +from dimos_generated.std_msgs.msg import Bool, Header +from fastapi.testclient import TestClient +import pytest + +from dimos.msgs.protocol import DimosMsg +from dimos.teleop.phone.phone_extensions import SimplePhoneTeleop +from dimos.teleop.phone.phone_teleop_module import PhoneTeleopModule +from dimos.web.relay_bridge.protocol import FrameHeader, encode_data_frame + + +def frame(channel: str, message: DimosMsg, *, type_name: str | None = None) -> bytes: + return encode_data_frame( + FrameHeader( + ch=channel, + seq=1, + ts=0, + delivery="latest", + meta={"type": type_name or message.msg_name, "encoding": "cdr"}, + ), + message.encode(), + ) + + +@pytest.fixture +def module() -> Iterator[PhoneTeleopModule]: + instance = PhoneTeleopModule() + try: + yield instance + finally: + instance.stop() + + +def test_websocket_and_schema_endpoint_use_generated_contract(module: PhoneTeleopModule) -> None: + with TestClient(module._web_server.app) as client: + schemas = client.get("/teleop/schema").json() + assert schemas["sensors"] == { + "type": TwistStamped.msg_name, + "definition": TwistStamped.schema, + } + assert schemas["button"] == {"type": Bool.msg_name, "definition": Bool.schema} + with client.websocket_connect("/ws") as ws: + ws.send_bytes(frame("button", Bool(data=True))) + ws.send_bytes(frame("sensors", TwistStamped(header=Header(frame_id="phone")))) + assert module._teleop_button is True + assert module._current_sensors.header.frame_id == "phone" + + +@pytest.mark.parametrize( + "wire", + [ + b"", + Bool(data=True).encode(), + frame("unknown", Bool(data=True)), + frame("button", Bool(data=True), type_name="std_msgs/msg/Int8"), + frame("sensors", Bool(data=True)), + frame("button", Bool(data=True)) + b"trailing", + ], +) +def test_wrong_identity_or_legacy_unframed_bytes_are_rejected( + module: PhoneTeleopModule, wire: bytes +) -> None: + assert module._dispatch_binary_message(wire) is False + assert module._teleop_button is False + assert module._current_sensors is None + + +def test_generated_control_math_preserves_exact_stamp_and_wraps_yaw( + module: PhoneTeleopModule, +) -> None: + initial = TwistStamped(twist=Twist(linear=Vector3(z=350))) + current = TwistStamped( + header=Header(stamp=Time(sec=1700000000, nanosec=123456789)), + twist=Twist(linear=Vector3(x=15, y=-30, z=10), angular=Vector3(z=60)), + ) + module._on_sensors_bytes(initial.encode()) + assert module._engage() + module._on_sensors_bytes(current.encode()) + output = module._get_output_twist() + decoded = TwistStamped.decode(output.encode()) + assert decoded.header.stamp.nanosec == 123456789 + assert decoded.header.frame_id == "phone" + assert decoded.twist.linear.x == 1.0 + assert decoded.twist.linear.y == -0.5 + assert decoded.twist.linear.z == pytest.approx(20 / 30) + assert decoded.twist.angular.z == 2.0 + + +def test_ground_robot_extension_publishes_generated_twist() -> None: + module = SimplePhoneTeleop() + outputs: list[Twist] = [] + unsubscribe = module.cmd_vel.subscribe(outputs.append) + try: + module._publish_msg(TwistStamped(twist=Twist(linear=Vector3(x=1, y=2, z=3)))) + result = Twist.decode(outputs[0].encode()) + assert result.linear.x == 1 + assert result.linear.y == 2 + assert result.linear.z == 0 + assert result.angular.z == 3 + finally: + unsubscribe() + module.stop() diff --git a/dimos/teleop/phone/web/static/index.html b/dimos/teleop/phone/web/static/index.html index 5bad9c727e9..12bdcd0c45a 100644 --- a/dimos/teleop/phone/web/static/index.html +++ b/dimos/teleop/phone/web/static/index.html @@ -165,7 +165,27 @@

Sensors

- - diff --git a/examples/language-interop/ts/web/server.ts b/examples/language-interop/ts/web/server.ts deleted file mode 100644 index 5b8ec035bef..00000000000 --- a/examples/language-interop/ts/web/server.ts +++ /dev/null @@ -1,69 +0,0 @@ -#!/usr/bin/env -S deno run --allow-net --allow-read --unstable-net - -// LCM to WebSocket Bridge for Robot Control -// Forwards robot pose to browser, receives twist commands from browser - -import { LCM } from "jsr:@dimos/lcm"; -import { decodePacket, geometry_msgs } from "jsr:@dimos/msgs"; - -const PORT = 8080; -const clients = new Set(); - -Deno.serve({ port: PORT }, async (req) => { - const url = new URL(req.url); - - if (req.headers.get("upgrade") === "websocket") { - const { socket, response } = Deno.upgradeWebSocket(req); - socket.onopen = () => { console.log("Client connected"); clients.add(socket); }; - socket.onclose = () => { console.log("Client disconnected"); clients.delete(socket); }; - socket.onerror = () => clients.delete(socket); - - // Forward binary LCM packets from browser directly to UDP - socket.binaryType = "arraybuffer"; - socket.onmessage = async (event) => { - if (event.data instanceof ArrayBuffer) { - const packet = new Uint8Array(event.data); - try { - // we don't need to decode, just showing we can - const { channel, data } = decodePacket(packet); - console.log(`[ws->lcm] ${channel}`, data); - await lcm.publishPacket(packet); - } catch (e) { - console.error("Forward error:", e); - } - } - }; - - return response; - } - - if (url.pathname === "/" || url.pathname === "/index.html") { - const html = await Deno.readTextFile(new URL("./index.html", import.meta.url)); - return new Response(html, { headers: { "content-type": "text/html" } }); - } - - return new Response("Not found", { status: 404 }); -}); - -console.log(`Server: http://localhost:${PORT}`); - -const lcm = new LCM(); -await lcm.start(); - -// Subscribe to pose and just log to show how server can decode messages for itself -lcm.subscribe("/odom", geometry_msgs.PoseStamped, (msg) => { - const pos = msg.data.pose.position; - const ori = msg.data.pose.orientation; - console.log(`[pose] x=${pos.x.toFixed(2)} y=${pos.y.toFixed(2)} z=${pos.z.toFixed(2)}`); -}); - -// Forward all raw packets to browser (we are decoding LCM directly in the browser) -lcm.subscribePacket((packet) => { - for (const client of clients) { - if (client.readyState === WebSocket.OPEN) { - client.send(packet); - } - } -}); - -await lcm.run(); diff --git a/examples/message-codegen/EVIDENCE-navigation.md b/examples/message-codegen/EVIDENCE-navigation.md new file mode 100644 index 00000000000..1fb284b12d7 --- /dev/null +++ b/examples/message-codegen/EVIDENCE-navigation.md @@ -0,0 +1,371 @@ +# Navigation CDR verification + +### Generated holonomic tracking and command limits + +Converted the holonomic tracking law, command limiter, and trajectory samples +to generated Pose/Twist/Vector3. Quaternion yaw uses the shared geometry helper; +samples retain independent copies of caller-owned values. + +Verification: five focused tests passed (frame rotation, feedforward, position +and yaw correction, one-sided damping, speed/acceleration bounds, copy isolation, +and CDR round-trip output). Mypy passed for all three production modules. +`demo_holonomic_tracking.py` ran 200 synthetic ticks and reached `(2, 1)` with +`0.00000002m` position error, printing intermediate decoded velocity commands. +This verifies the tracking core only; path-controller and live module conversion +remain part of the ongoing coordinated runtime cutover. + +### Generated holonomic path follower and module + +Converted PathDistancer, HolonomicPathController, and DanHolonomicTC to generated +nested PoseStamped/Path, Twist, and Bool messages. Removed their legacy message +imports and copy constructors. Velocity estimation subtracts integer nanosecond +timestamps before conversion and owns its previous-pose snapshot. + +The three closed-loop follower cases (straight-line deceleration, right-angle +speed cap, initial rotation), two run-envelope cases, and four module lifecycle +cases passed. Module input test transport now CDR-round-trips each value. A +separate regression passed for a one-nanosecond interval at epoch 1700000000 +and mutation of the caller's previous pose. Mypy passed for all three changed +production modules. The threaded `demo_holonomic_path.py` arrived at `(0.926, 0)` +within its 0.08m goal tolerance and printed a final zero command. Network +transport behavior is outside this focused demo's scope. + +### Generated local-planner gating and smoothing + +DanLocalPlanner and its replan gate now consume generated PointStamped, +PoseStamped, and Path. Seven tests passed with CDR-decoded inputs, covering +progress-based commits, fresh/stale goals, cancellation, empty-path stop, +disabled gating, and smoothing with exact sec/nanosec header preservation. +Mypy passed for the production module. The cumulative holonomic path demo now +runs this gate first: it prints 10 resampled poses and suppression of a duplicate +replan, then the threaded follower arrives at `(0.926, 0)` and publishes zero. + +### Generated MLS Python boundary and TF start relay + +MLSPlan now reads generated PointCloud2 via the shared layout-aware XYZ helper +and emits generated Path/PoseStamped with the cloud's exact Header. The start +relay consumes generated TFMessage and emits generated PoseStamped. + +Built the actual Rust/PyO3 extension with maturin 1.13.3, release mode, offline +from cached dependencies. Installation succeeded after allowing uv's cache lock +outside the sandbox. Seven tests passed against that extension: terrain planning, +missing-pose skipping, robot-height offset, no-route output, TF chain composition, +missing-chain suppression, and retry throttling. Mypy passed for both production +modules. Tests check CDR path headers at `1700000000.123456789` and decoded relay +poses. No tests were skipped for a missing native backend. + +`demo_mls_transformer.py` ran successfully: TF start `(-2, -2, 1)`, 3,600 terrain +points, 900 voxels, and 18 decoded path poses ending at `(2, 2, 0)`. The exact +cloud timestamp survived into the output path. This is a native planning and +serialization demo; it does not establish network or hardware performance. + +### Generated MLS visualization adapters + +Planner visualization now reads generated PointCloud2 fields through shared +layout helpers and renders explicit generated LineSegments3D endpoints/weights. +Three tests passed against actual Rerun archetypes: clearance filtering, +missing-intensity coloring, raised nodes, weighted edge colors, empty geometry, +and unchanged source messages after rendering. Mypy passed for the adapter. + +The native MLS terrain demo successfully wrote `mls-planner.rrd` with terrain, +path nodes, and edges. `rerun rrd verify` accepted the recording. This is +headless recording verification; interactive viewer acceptance remains pending. + +### Navigation evaluator on generated SQLite streams + +Converted recording registration and suite frame counting to generated +PointCloud2/Odometry, nested pose fields, and shared point-cloud/pose helpers. +The evaluator's 14 tests passed after replacing its recording fixtures with +generated CDR messages. These include recorded frame alignment, rotation, +trajectory loading, suite generation, and causal/final evaluation paths. +Mypy passed for recording and case handling. + +`demo_navigation_recording.py` wrote and reopened actual SQLite streams and +verified three sensor points rotated by 90 degrees and translated along the +recorded trajectory: `(0,1,0)`, `(1,1,0)`, `(2,1,0)`. Loaded odometry yielded +2m total distance. The temporary database was removed automatically. This +validates newly written recordings; historical dataset replacement remains a +separate outstanding part of the full migration. + +### Offline planner rendering and ray-tracing boundary + +Converted `plan_rrd` to generated cloud/TF types and shared point-cloud helpers. +Extracted named TF rendering and colormap annotations from legacy message classes +into `visualization/rerun/message_helpers.py`. Converted the upstream RayTraceMap +transformer to generated input/output clouds, preserving the source stamp and +removing the Open3D allocation previously used only to construct output messages. + +Built and installed the actual ray-tracing Python extension offline. Five native +transformer tests and two offline-rendering tests passed; mypy passed for all +three production modules. The terrain demo now prints the generated ray-traced +map and writes it alongside TF edges, terrain, and the planned route. The updated +RRD passed `rerun rrd verify`. Full execution of `plan_rrd` against historical +recordings remains unverified until those datasets are replaced. + +### Ray-tracing CLI and combined regression checks + +Converted the remaining raytrace_rrd CLI to generated PointCloud2 fields and +nested TransformStamped values. The recorded-navigation demo now invokes the +actual CLI in a subprocess on newly written CDR lidar/TF SQLite streams. It +processed three frames, wrote `raytrace-cli.rrd`, and Rerun verified the file. +Mypy passed for the CLI after annotating the existing normal-length array. + +The combined dannav, evaluator, MLS, and ray-tracing test selection passed all +75 tests, including both compiled native Python backends. A search found no +legacy geometry/nav/sensor/TF message imports in navigation or ray_tracing Python +files. This search establishes only that scoped import cleanup, not completion +of the repository-wide migration or historical dataset replacement. + +### Generated TF dispatch in the live Rerun bridge + +The bridge now recognizes generated TFMessage directly. Named transforms are +logged even with axes disabled; enabled axes use a separate generated-message +frame-tree renderer. Six bridge checks passed, including generated CDR dispatch +with axes enabled/disabled, reparenting cleanup, and existing bridge configuration +checks. Mypy passed for the extracted frame-tree renderer. + +The terrain demo now sends its CDR TF message through the actual bridge callback +before recording terrain and the route; the resulting RRD passed verification. +This exercises dispatch without starting the viewer or a transport subscription. +Camera/image and other built-in bridge conversions remain outstanding. + +### Generated camera and image bridge + +Converted bridge image/calibration dispatch to generated CameraInfo, Image, and +CompressedImage with separate pinhole/image adapters. Calibration pairs by +header.frame_id in either arrival order. Depth units are explicit (16UC1 in +millimetres, 32FC1 in metres). JPEG/PNG compressed formats set the required +Rerun media type; unsupported formats fail explicitly. + +Twelve camera/TF/config checks passed. A warning exposed missing compressed-image +media metadata in the initial run; fixed it and reran all six camera checks with +Rerun warnings treated as errors. That rule is now part of the test file. Mypy +passed for the adapters. The camera demo then logged raw/compressed color and +both depth representations without warnings, and its RRD passed verification. +Interactive viewing and bridge transport subscriptions remain unverified here. + +### Generated 3D detections in the Rerun bridge + +Added a separate generated Detection3DArray renderer and wired it into bridge +conversion. It preserves box centers, half sizes, orientations, class labels, +and detection IDs, including empty arrays. Updated the existing bridge fixture +to generated CDR messages. The two detection checks passed after fixing a +missing adapter import and an unintended early return caught by the dispatch +test. Nine camera/TF checks +also passed during this increment, and mypy passed for the adapter module. + +The camera demo now sends a labeled generated detection through the bridge in +addition to images and calibration. Its final successful run produced an RRD +that passed verification. This is callback/serialization verification, with +interactive viewer inspection still outstanding. + +### Generated point clouds through the shared bridge + +Added generated PointCloud2 dispatch with shared XYZ/RGB layout helpers. The +adapter filters non-finite coordinates with the same mask for packed colors, +uses explicit height colors for colorless clouds, and never mutates the source. +The padded big-endian FLOAT32-packed-RGB test verifies color alignment, CDR +round-trip, frame attachment, and source-byte preservation. Empty and height-only +clouds are also covered. Both focused tests and the combined 13 generated +cloud/camera/TF/detection bridge tests passed; adapter mypy passed. + +The terrain demo now sends its 3,600-point cloud through the bridge callback. +The native planning demo completed and the resulting RRD passed verification. +This remains headless callback verification, not interactive viewer acceptance. + +### Generated navigation rendering in the bridge + +Added generated PointStamped, PoseStamped, Odometry, and Path dispatch through a +separate adapter. Poses and odometry carry their declared parent frame in the +Rerun transform. Points and paths use bridge frame attachment; paths preserve the +existing 0.5m display lift without changing message coordinates. Empty paths emit +empty geometry for clearing. + +Five focused tests passed with Rerun warnings as errors, covering each message +kind, CDR round-trips, coordinates, parent frames, source immutability, and empty +paths. Adapter mypy passed. The native terrain demo routed its planned path +through the shared bridge and its RRD passed verification. Network subscriptions +and interactive viewer acceptance remain outside this check. + +### Combined bridge and viewer environment verification + +Installed the exact repository-pinned `dimos-viewer==0.32.0a2` into the local +verification environment. Two initial viewer binary-discovery checks failed +because the package was absent; after installation and venv activation, all +29 generated adapter, bridge configuration, and viewer integration checks passed. +These viewer checks cover installation and mocked launch/lifecycle behavior, +not interactive visual acceptance. No dependency manifest changed. + +Mypy passed for the bridge, message adapters, and frame tree after giving the TF +loop distinct variable names to avoid an incompatible archetype assignment. + +### Generated occupancy grids through the bridge + +Added generated OccupancyGrid dispatch to a separate textured-quad adapter. +The quad uses the complete origin pose, including rotation and elevation; its +texture preserves row orientation and default free/occupied/unknown colors. +Two focused tests passed with Rerun warnings as errors, checking a 90-degree +rotated grid, exact texture bytes, frame attachment, source immutability, empty +maps, and malformed dimensions. Bridge and adapter mypy passed. + +The native terrain demo now sends a small generated occupancy grid through the +bridge alongside its cloud and path. The completed demo's RRD passed verification. +Interactive rendering acceptance is still outstanding. + +### Live LCM bridge verification + +Added a bounded headless demo that calls the real bridge start lifecycle, +creates its gRPC server, subscribes through typed LCM, publishes a generated +160x120 image on a unique topic, and verifies delivery after bridge rendering. +The demo passed, and Rerun verified its recorded output. The subprocess regression +also passed with 30-second bounds on the demo and RRD verification. No transport +or viewer startup behavior is mocked in this demo; no GUI was opened. + +Bridge initialization now imports colormap registration from the separate +visualization helper instead of the legacy PointCloud2 class. All eight viewer +integration checks passed after this change. This establishes the LCM headless +path only; Zenoh and interactive visual acceptance remain outstanding. + +### Live bridge on both transports + +Extended the headless live bridge demo and regression to LCM and Zenoh. The +Zenoh variant uses two independent session pools and an explicit loopback TCP +connection. A first run rejected the demo's leading-slash LCM topic name; +corrected the fixture to a valid `dimos/vb/...` Zenoh key. Both bounded subprocess +tests then passed, including real bridge startup, CDR image delivery, shutdown, +and verification of each transport's separate RRD output. + +Scouting/gossip are disabled in the explicit Zenoh fixture. This is not evidence +that the previously observed default discovery issue is fixed, nor is it +interactive viewer acceptance. + +### Viewer control WebSocket + +The viewer's click, velocity, and stop handlers now emit generated `PointStamped` +and `Twist`. Click timestamps convert integer milliseconds directly to ROS time; +nullable 2D click coordinates remain zero. No legacy message imports remain in +`dimos/visualization` Python sources. + +```sh +PYTHONPATH=.:build/message-codegen/demo/cpp/build .venv/bin/python examples/message-codegen/demo_viewer_controls.py +``` + +Observed: a real loopback WebSocket delivered a map click at `(1.5, 2.5, 0)` with +stamp `1700000000.123000000`, forward/yaw velocity `(0.5, 0.8)`, and a stop with all +six velocity components zero. All received values were encoded and decoded as +CDR. This exercises the control protocol without requiring a graphical viewer. +The six server tests passed; production-file mypy passed. The invalid-JSON test +now proves that a subsequent stop is delivered instead of relying on sleeps. + +### Command-center generated message boundary + +`WebsocketVisModule` now consumes generated pose/path/occupancy messages and +publishes generated click goals, exploration flags, and both velocity forms. +Costmap inflation and gradient use the shared occupancy view; JSON retains +compressed cells and now reports the origin quaternion's yaw. Local subscribers +receive velocity commands through the same publication path as configured +transports. + +```sh +PYTHONPATH=.:build/message-codegen/demo/cpp/build .venv/bin/python examples/message-codegen/demo_command_center.py +``` + +Observed: printed browser state for robot `(2, 3)`, path to `(5, 3)`, and a 2×2 +compressed costmap; the click produced a CDR-round-tripped world goal `(5, 3, 0)`. +This demo invokes registered handlers directly; it does not claim browser or +Socket.IO network acceptance. Two focused tests passed, covering generated +commands, stamped velocity, pose/path state, exact decompressed cells, rotated +map origin, and unchanged input bytes. Production-file mypy passed. + +### Shared interfaces and VLM image requests + +Control and mapping protocols now reference generated Twist and OccupancyGrid. +The VLM agent, its RPC protocol, and stream tester reference generated Image; +model requests use the shared JPEG encoder and explicit base64 content instead +of methods on legacy values. The unused manipulator identity-transform factory +was removed along with its legacy imports. + +```sh +PYTHONPATH=.:build/message-codegen/demo/cpp/build .venv/bin/python examples/message-codegen/demo_vlm_image.py +``` + +Observed: `build/message-codegen/demo/evidence/vlm-request.jpg` is a 160×120 image +with red left and green right halves, also inspected directly. The demo exercises +CDR decode and the actual agent request assembly with a stubbed model; no external +inference is claimed. Twelve VLM-boundary and existing spec tests passed. Coverage +includes stream and RPC image requests, JPEG colors and dimensions, unchanged +input bytes, and the no-image response. Mypy passed for the six changed production +files after annotating the request content list. + +### Navigation skill goals + +The navigation skill's image/odometry streams and goal construction use generated +messages. Tagged locations now convert quaternion orientation to the Euler angles +required by RobotLocation, fixing the previous storage of quaternion xyz as +roll/pitch/yaw. Tagged and semantic goals use explicit generated pose fields. + +```sh +PYTHONPATH=.:build/message-codegen/demo/cpp/build .venv/bin/python examples/message-codegen/demo_navigation_skill.py +``` + +Observed: a generated odometry pose at `(1, 2, 3)` was tagged as `desk`, retrieved, +and emitted as a CDR map goal retaining roll/pitch/yaw `(0.2, -0.3, π/2)`. +Memory and navigation RPCs were stubbed; no inference or robot motion occurred. +Two focused tests passed for the tagged orientation round trip and semantic-map +goal construction/rejection. Production mypy passed. Existing agent integration +test message annotations were converted; those inference tests were not run. + +### Generated camera observation through MCP + +ObserveSkill consumes generated Image. MCP serializes that value explicitly as a +base64 JPEG `image` content block with `mimeType`; the agent client converts it +into an image URL content block when appending model history. Generated values +need no agent-specific methods. + +```sh +PYTHONPATH=.:build/message-codegen/demo/cpp/build .venv/bin/python examples/message-codegen/demo_mcp_observe.py +``` + +Observed: a camera frame delivered over real LCM/CDR through ObserveSkill became +`build/message-codegen/demo/evidence/mcp-observe.jpg`, a 160×120 red-left, +green-right image inspected directly. MCP request handling was called directly; +no HTTP MCP transport or external model inference is claimed. Eleven focused +observe/server/image-boundary tests passed, including timeout and client-history +conversion. Mypy passed for all three changed production files. Missing FastAPI +and langchain-openai dependencies were installed locally at repository lockfile +versions; manifests were unchanged. + +### Keyboard teleoperation outputs + +KeyboardTeleopModule now publishes generated TwistStamped and Float32 messages. +Velocity commands have explicit ROS headers and nested twists. Tests decode the +published CDR values before checking motion and key-release behavior. + +```sh +PYTHONPATH=.:build/message-codegen/demo/cpp/build .venv/bin/python examples/message-codegen/demo_keyboard_cdr.py +``` + +Observed: releasing W while holding A retained left velocity `0.05 m/s`; releasing +A then produced all-zero velocity. Gripper bracket events produced `1.0`, then +`0.0`, after CDR round trips. Synthetic pygame events require no window or robot. +All eight keyboard tests and production-file mypy passed. Pygame 2.6.1 was +installed locally at the lockfile version after the first run exposed its absence. + +### Hosted operator map compression + +MapCompressModule consumes generated OccupancyGrid and PoseStamped, using shared +occupancy views, block reduction, quaternion angles, and ROS-time conversion. +Malformed grid layout is caught inside the callback so a later valid map still +publishes. The existing browser PNG/JSON payload contract is retained. + +```sh +PYTHONPATH=.:build/message-codegen/demo/cpp/build .venv/bin/python examples/message-codegen/demo_hosted_map.py +``` + +Observed: `build/message-codegen/demo/evidence/hosted-map.png` contains the expected +64×64 occupancy palette, inspected directly, and generated odometry produces +operator coordinates `(3.2, 3.2)` with yaw `1.0`. Callbacks run locally; no hosted +network/browser acceptance is claimed. Nine tests and production-file mypy passed. +Tests include palette, coarsening obstacle preservation, throttling, malformed and +empty maps, oversized payload rejection, and invalid quaternion rejection. Test +fixtures now construct and clean up real modules instead of skipping Module init. diff --git a/examples/message-codegen/EVIDENCE-recovery.md b/examples/message-codegen/EVIDENCE-recovery.md new file mode 100644 index 00000000000..fa51eb7da60 --- /dev/null +++ b/examples/message-codegen/EVIDENCE-recovery.md @@ -0,0 +1,285 @@ +# Desktop recovery and continued CDR validation + +Verified on 2026-09-30 in the dedicated `feat/cdr-runtime-cutover` worktree. +This is a progress checkpoint, not final migration acceptance. + +## Preserved history + +The recovered local tip `ae00dd3e8c` was clean. CC Desktop had three linear +successors, `87ad59d853`, `08cb4eb4ba`, and `be3832092d`. They were fetched and +fast-forwarded locally without changing the source machine or rewriting commits. +These migrate MCP observation images, keyboard commands and hosted map compression. + +## Changes and visible demos + +- `scripts/test_message_user_story.sh` generates an application-local + `story_msgs/msg/DeviceReading`, builds Python/C++/Rust, and runs a real Python + module handler followed by native CDR file consumers. Output returns sequence + 42, value 23.5, label `new-local-type/python/cpp/rust`, preserving nanoseconds. +- `demo_video_stats.py` converts browser JSON to the explicit generated + `dimos_msgs/msg/VideoStats`, records and reopens SQLite, and displays the exact + integer counter 4294967297. It removes its temporary database automatically. +- Video telemetry consumers now import the generated type. The old positional + Joy carrier and its legacy codec methods are removed. Invalid unsigned metric + values fail at the JSON boundary with a descriptive ValueError. +- The unused legacy `lcm_msg_type` discovery function is removed. Tests resolve + generated types by `package/msg/Type`; old dotted names do not import codecs. +- `docs/development/messages.md` provides message-authoring, Python module, + C++/Rust codec and native module, packaging and recording/replay user stories. + Its runnable shell commands were executed and local source/doc links checked. + +## Executed validation + +Use the generated demo extension on PYTHONPATH and the activated project venv. +The restored native dependency prefix is `build/message-codegen/install`. + +- Generated/built the complete demo definitions including the new telemetry type. +- 79 Python tests passed across generated video stats/type discovery, memory CDR + codecs, MCAP store, Rust recorder configuration, MCP images, keyboard and map + compression. Tests used `--noconftest --import-mode=importlib -o addopts=''`. +- 3 native recorder E2E tests passed over explicit local Zenoh TCP: Rust SQLite + and MCAP artifacts read through Python, plus TF capture and Python replay. + The two LCM CLI cases were deselected. Tests used the actual local Cargo-built + `target/debug/dimos-memory-recorder`; no result from the separate main-branch + MCAP compatibility thread is used as CDR evidence. +- `cargo test -p dimos-memory-recorder --lib`: 16 passed. +- `cargo test -p dimos-module --lib`: 146 passed. +- `cargo build -p dimos-native-module-examples --offline`: passed after fetching + the lockfile dependencies during explicit setup/build. +- Ruff check/format, shell syntax and diff whitespace checks passed for this + change. Mypy passed the two changed helper modules with generated typing on + MYPYPATH. This is not a full-repository mypy claim. + +Terminal and build logs remain ignored under +`build/message-codegen/demo/evidence/recovery-*`; user-story CDR files and its +terminal capture live under `build/message-codegen/user-story/evidence/`. +Standard setuptools egg-info was generated to expose the checkout's declared +`dimos.messages` entry point; simply putting an uninstalled extension on +PYTHONPATH does not register an installed provider. + +## Continued hosted command cutover + +Hosted Go2 command and telemetry boundaries now use generated TwistStamped, +Twist, PoseStamped and Bool, matching the converted Go2 driver. Drive ordering +uses integer source nanoseconds, including frames one nanosecond apart. Stale +and future rejection, finite-velocity validation, limits, idle suppression, +release-edge stopping and E-STOP remain covered by mocked driver tests. Invalid +ROS nanoseconds are rejected before telemetry processing. A runnable +`demo_go2_command_cdr.py` prints clamped drive and navigation fields and asserts +zero driver calls. Both production modules passed mypy with generated stubs. + +The combined phone/Go2/hosted telemetry suite passed 53 tests. Separately, 135 +generated geometry, time, buffer, point-cloud, trajectory, camera-info and +occupancy helper tests passed. Neither count represents the entire repository. +Unitree import dependencies were installed in the project venv for these mocked +regressions; no device connection, microphone or audio capture was started. + +## CI environment and browser-fixture repairs + +Both workflows were manually dispatched on `dd9a5e385df9b4966953c50e142bb695f38d3beb`. +The generator job passed its codec conformance, raw fragmented LCM exchange, +and transport unit tests before a helper demo failed because PyYAML was absent. +The workflow now explicitly provisions the lockfile's PyYAML 6.0.3. That helper +demo passed in a fresh minimal project venv. + +Main CI's lint, docs and Rust binding jobs failed during editable installation +because their Fast CDR preflight was absent. They now run the same pinned setup +script already used by the test jobs. A real no-dependency editable install +succeeded in an isolated venv in 1m48s. All jobs that perform `uv sync` now have +an explicit CDR setup step. This validates installation, not the full jobs. + +Web CI passed its Deno/SDK/cockpit checks and seven browser E2E cases; retired +PoseStamped inputs and an old replay database caused the remaining failures. +The typed-channel test now uses generated fields and expects the CDR schema +contract. Go2 cockpit/SDK tests use a deterministic 1.3 MB replay fixture with +three generated streams, 720 frames each, exact source nanoseconds and changing +RGB pixels. Images use lossless `lz4+cdr`; the old large LFS fetch is removed +from the web job. The full fixture was written and reopened locally with type, +stamp, frame-count and pixel assertions. Generated fixture mypy and Ruff passed. +61 focused schema/MCAP/teleop checks passed. Browser E2E and full CI acceptance +for these repairs remain to be verified on the next published commit. + +## Exact-commit CI and native bake follow-up + +At `1e7705ab3cf98edc35c72bd4c3bcceaa2d243b49`, manually dispatched +[message-codegen run 36691321793](https://github.com/dimensionalOS/dimos/actions/runs/36691321793) +completed successfully in both standalone and Jazzy-reference jobs. This includes +installed wheel/sdist/CMake/Cargo consumers and offline Foxglove/Rerun decoders. +The retained MCAP contains 150 messages: five generated channels, 30 each. +The viewer evidence is copied locally under ignored +`build/message-codegen/ci-evidence/1e7705ab3/`. This proves offline decoding, +not authenticated Foxglove UI acceptance. + +[Main CI run 36691317959](https://github.com/dimensionalOS/dimos/actions/runs/36691317959) +completed with failures; its web browser job and native C++/Rust job succeeded. +Lint lacked generated typing stubs; the next batch generates them explicitly +and provisions pinned native-build typing tools. The emitter also had a real +loop-variable type collision, now fixed. Full-repository migration errors remain +to be resolved after these environment errors. Five changed production/demo +files passed targeted mypy; generated-interface positive/negative tests passed. + +The native bake E2E exposed old registry type names and LCM test publishers +against a CDR decoder. All five native module registry entries now use the same +canonical names as their generated Python ports. Macro validation accepts the +canonical spelling. The actual Python-to-baked-Rust graph passed all three E2E +cases over loopback-only Zenoh, including real surface-map publication and +suppression, in 17.18 seconds. No hardware or networking settings were used. + +Main Python CI collection lacked MCAP/rosbags test tools and offline planner +bindings; its next batch explicitly provisions those. Updating the whole uv +lockfile is currently blocked by the existing `a750-control==0.1.1` wheel +availability for supported Python 3.11. The runtime manifest and existing +lockfile were preserved; pinned supplemental CI tools are installed explicitly. + +CameraMux uses generated images and separate array helpers. Its 21 tests cover +even dimensions, scaling, selection, FPS caps, latency strips, exact source +stamps and independent composite storage. The executable bounded camera demo +produces a 100x36 RGB composite and an 854-byte JPEG while preserving +`1700000000123456790ns`. No camera or encoder process is started. + +## Remaining gates + +The OpenSpec checklist remains unchanged because these slices do not complete +all stage-4, stage-5 or stage-6 requirements. Old wrappers and consumers remain +in other teleop, robot and perception paths. The public type replacement and +complete old-dependency removal still need coordinated migration. + +The C++ SDK dependency blocker was resolved using an isolated project prefix +`build/native-deps/prefix`: official LCM 1.5.1 sources, pinned Zenoh C/C++ 1.10.0 +release archives, and the already built Fast CDR prefix. Zenoh archives were +checked against the same SHA256 values used in CI. Their pkg-config prefix +was relocated within the ignored project directory; the actual unstable-API +compile probe passed. No system package or networking setting was changed. +The SDK built and all 77 CTest tests passed. C++ examples built, then +`demo_native.py --backend zenoh` exchanged fields across Python/C++/Rust and +verified a 921600-byte image with its exact source nanoseconds. + +The phone browser command path now advertises generated schemas and dispatches +explicit channel/type CDR frames. Nine Python tests passed, including the real +FastAPI WebSocket endpoint, invalid frame rejection and control gain/yaw math. +A real in-app browser connected to `demo_phone_cdr.py`: five schema-driven +Foxglove CDR frames decoded in Python. The preview stopped its server without +starting a control loop or requesting sensor access. Production phone modules +passed mypy with generated stubs; Ruff check/format passed. + +At published commit `dbf5583075154f9a174efe59a794ba5167e6592b`, all CodeQL +language analyses passed. Neither `ci.yml` nor `message-codegen.yml` has a +pull-request run for #4257 among the latest 100 runs. GitHub reports mergeability +unknown and exposes only the PR head ref; the missing merge ref may be relevant, +but a cause has not been established. The desktop browser was unsigned in. Existing authenticated GitHub CLI on +CC Desktop subsequently enabled manual dispatch without credential changes. LCM default +multicast self-test is blocked on this host; no networking settings were changed. +Full blueprint/viewer demo, final installed packaging matrix, authenticated +Foxglove UI evidence, and exact published-commit CI acceptance remain pending. +No hardware commands, model inference or model downloads were used. + + +## Generated-consumer cleanup checkpoint + +The next local batch migrates model/detector/embedding image boundaries, +recording headers and TF payloads, report source-time calculations, G1 mount +composition, and generated video rendering. Public generated messages remain +plain schema values. OpenCV/Open3D/Rerun imports are deferred per the existing +codebase check, without suppressing that check. Old typed-transport test inputs +now use canonical generated types. Empty codegen namespace initializers are +removed; bundled standalone generator precedence still passes its existing test. + +Affected offline helpers, WebSocket protocol, camera mux, reports and G1 math: +195 passed, one desktop viewer binary test excluded because `dimos-viewer` is +not installed locally. Transport/package/structure checks: 57 passed. Strict +checks passed for the migrated recorder/report, image/projection, detector +modules, person tracker, patrol consumer, time adapter, G1 and viewer helpers. +Model weights and hardware behavior are not validated by these checks. + +The original timestamp self-hosted test accidentally acquired/extracted +`unitree_office_walk` during the explicit-file run (ignored data directory about +2 GB); the download was not requested in advance. Its backpressure/alignment +fixture now uses synthetic capture events, retaining its existing assertions. +All 21 timestamp/projection tests pass without replay assets. Existing cached +data is retained; subsequent tests avoid automatic large-asset acquisition. + +Exact published `febed89cec435bc2b853b4d163f033a6fd7dc8f2` message-codegen run +36694469475 succeeded. Main CI run 36694465187 completed with ARM tests +5940 passed / 54 failed / 229 skipped / one teardown error; other Python jobs +were fail-fast cancelled. Native, Rust, Web, Linux and macOS builds succeeded. +Lint reported 188 errors in 61 files and md-babel failed on remaining old +examples. These results predate this cleanup batch and are not final acceptance. + + +## Offline RGB-D and namespace-entrypoint repair + +Generated PointCloud2 helpers now project rectified RGB-D with scaled camera +intrinsics, integer-depth units, invalid-depth filtering and packed RGB fields. +Transforming a cloud retains declared fields, endian, organized dimensions and +all point/row padding; finite values beyond the target field range are rejected. +Support-plane preparation consumes these generated values before its existing +Open3D downsampling/RANSAC. Open3D fitting itself was not executed locally. +Image benchmark fixtures now construct generated RGB8 values. + +Offline image views, calibration, geometry and point-cloud tests: **143 passed**. +Strict checks passed for point-cloud helpers, support-plane and benchmark data. +Standalone codegen tests with PYTHONPATH unset: **24 passed, one existing skip**. +The codegen and MCAP shell gates use python -m pytest so namespace-package +imports retain the checkout root. This fixes the observed collection failure of +published 6547f9c message-codegen run 36704834764 without reducing coverage. + +Local full mypy: 178 errors in 78 files (1158 checked); the intersection with +original failing CI file paths is 90 errors in 28 files, down from 96. This +includes unavailable local dependency errors and is not CI acceptance. +Published 6547f9c main run 36704509445 has successful native, Rust, Web and +Linux/macOS builds; lint and md-babel failed, Python test jobs remain running. +WebXR control migration and model-backed memory-doc execution remain held for +specific approval after automatic review rejected those actions. Their existing +uncommitted work is preserved and excluded from this repair batch. + + +## Static maps, memory consumers and offline Spot viewer conversion + +Static occupancy NPY/PNG loading now returns generated OccupancyGrid values, +with explicit header/map-load time, identity origin and ROS cell validation; +object-array loading is disabled. The MuJoCo scene loader and occupancy demo +use the standalone helper. No simulation/control loop or GUI was started. +OSM marker drawing and PNG saving convert generated pixels explicitly, retain +source metadata, and do not mutate cached tile images; its test mocks tile fetch. + +SpatialMemory reads nested generated TF fields and RGB/BGR conversions. +TemporalMemory uses generated image/pose streams, quality selection and real +header stamps in its fixtures. Scene-staleness ignores row padding. +Inventory/localization use generated XYZ views and TF fields; source-point +selection retains RGB and other fields when oversized supports split. Native +Open3D clustering, plane fitting and oriented boxes are not locally validated. +Inventory's strict check reports unavailable local torch; no model was loaded. +Spot Rerun converters construct TF/calibration/capture-anchored depth archetypes +from generated messages; offline construction passed without a viewer or robot. + +Combined selected offline regression: **195 passed, one external temporal-memory +integration class deselected**. Full local mypy: **161 errors in 73 files**; +original-CI-path intersection: **75 errors in 24 files**. No suppressions added. + +Published c690ef58a439b377cb6d9bb8eeb9bdc60e0128f6 message-codegen run +36707053518 completed successfully for standalone and Jazzy reference. +Main run 36707056720 completed fail-fast cancelled: Native/Rust/Web succeeded, +lint reported 93 errors in 30 files, md-babel passed 126/128 blocks, ARM failed +and other Python matrix jobs were cancelled. This consumer batch follows that +run and still requires exact published-commit CI acceptance. + + +### Generated consumer contract repairs after 201a3053 + +- Exact published 201a3053 CI: standalone/Jazzy message-codegen run 36709681735 succeeded; main run 36709677922 terminated by fail-fast. Native C++/Rust builds and Web succeeded. Lint reported 70 errors in 21 files; Python 3.11 retry reported 40 failures and three errors. The main workflow is not green. +- Migrated VQA, image-file evaluation and replay benchmark fixtures to generated Image/CameraInfo/PointCloud2/CompressedImage. Image observation encoding lives in the application layer; generated messages remain plain values. JPEG benchmark codec preserves exact source headers, and transport cleanup now also runs on test failure. +- Moved the complete gradient contract test to the generated occupancy suite, preserving obstacle, distance and unknown-cell assertions. Replaced the pointcloud occupancy input with a deterministic synthetic cluster rather than legacy LCM asset loading. +- Explicit CPU voxel backend does not import Open3D; CUDA selection remains unchanged. Added a real accumulation/CDR roundtrip test checking exact nanoseconds, output frame and disposal. +- Local combined offline checks: 154 passed, 5 deselected (CPU voxel, generated occupancy, VQA, evaluation and Zenoh/pickle replay contracts). LCM multicast and large benchmark nodes were excluded; no network settings changed. +- Full memory voxel and Go2 marker blueprint collection is blocked locally by absent torch; legacy occupancy suite collection by absent numba; YOLOe collection by absent ultralytics. Those fixture migrations await CI and are not recorded as local passes. +- WebXR control/ESM changes and full memory documentation execution remain paused after automatic approval rejection. Eight related earlier work-in-progress files remain uncommitted. No model download, hardware execution or approval bypass occurred in this batch. + + +### Camera, scene and mapping consumers after aecb4d7e7 + +- Exact aecb4d7e7 message-codegen run 36712561619 succeeded. Main run 36712557915 terminated by fail-fast: Native C++/Rust builds, Rust (including E2E) and Web succeeded; lint reported 69 errors in 20 files. ARM Python reported 6037 passed, 229 skipped, four failures: two PGO generated PoseStamped conversions and two control-coordinator collection blueprint links. The latter remain related to held work. +- ZED/MuJoCo image, calibration and cloud output constructors now use generated messages. Calibration retimestamping copies all fields and nested data. Existing tensor Open3D voxel downsampling moved into a standalone helper with the small-cloud fast path; unsupported fields are rejected instead of lost. No camera, robot, simulation or RTSP connection was started. +- Scene Object storage, aggregation and interfaces use generated clouds/geometry. Native Open3D geometry/filtering remains an algorithm input; generated messages carry the wire values. Exact source cloud nanoseconds survive aggregation. RGB-D alignment uses the existing local TimestampedData adapter. +- World-belief visibility and frame selection use generated values and standalone matrix/distance helpers. PGO correction applies its existing correction matrix to generated Pose values, fixing the old constructor boundary; internal optimizer math still requires later legacy retirement. Map output and mock lidar replay use generated wire messages. +- Local regression set: 90 passed, one optional native Open3D test skipped. This covers calibration copies, generated cloud/object values, visibility inverse transforms, PGO correction order and pose-less passthrough. Production scoped strict mypy: 16 files clean. +- Installed only the repository-locked types-PyYAML 6.0.12.20250915 typing stub (20 KB wheel) into the existing venv; no global software or model assets. Mocked camera/scene suites that import absent mujoco/pyzed/ultralytics await CI; they are not claimed as local passes. diff --git a/examples/message-codegen/EVIDENCE.md b/examples/message-codegen/EVIDENCE.md index 743c025b824..53564bcc168 100644 --- a/examples/message-codegen/EVIDENCE.md +++ b/examples/message-codegen/EVIDENCE.md @@ -129,3 +129,1184 @@ sign-in in the isolated browser; no credentials were entered. Its decoding libraries pass independently, but that does not establish application UI acceptance. Rerun's direct-file screen capture also needs final confirmation of pose visualization and complete semantic playback before task 3.5 is closed. + +## Runtime transports and browser cutover (2026-09-21) + +The runtime work is still in progress. This evidence covers typed transports, +web schema decoding, and the migrated built-in web adapters; it does not establish +that all DimOS consumers or native SDKs have completed the cutover. + +- `demo_pubsub.py`: generated `LineSegments3D` and 921,600-byte `Image` exchanged + over raw LCM, separate loopback TCP Zenoh sessions, and CPU shared memory. + Decoded bytes and integer nanosecond timestamps matched on every path. +- Typed pub/sub pattern, core CDR transport, and CLI checks: 67 passed. A glob + subscription regression was fixed: `/sensor/*` must match the logical topic + before the slash-separated `#package/msg/Type` suffix. +- WebRTC plus core transport checks: 34 passed. Explicit data-frame channel/type + routing distinguishes `Point` and `Vector3` despite their identical CDR layouts. + Truncated, trailing, raw/unframed, foreign-type, foreign-channel, and wrong-codec + frames are dropped; the next valid frame is delivered. +- Python web codecs: 48 passed. Cockpit authoring plus generated array/helper + checks: 94 passed. Costmap and map codec checks: 17 passed. +- Browser SDK: 322 passed. Cockpit: 154 passed. Both TypeScript checks passed. + The CDR decoder uses pinned Foxglove parser/serialization libraries. Fixtures + cover standard and custom messages in both byte orders, raw images, fixed and + dynamic arrays, empty messages, Unicode, and integers beyond JavaScript's safe + number range. Former LCM codecs and their browser fixtures were removed. +- Chromium opened `demo_cdr.html` and displayed the generated custom message's + endpoints, weight 4, and `stamp.nanosec = 500000123`. Switching to the image + and big-endian CDR displayed both pixels. Reading the rendered canvas yielded + RGBA `[0, 1, 2, 255, 253, 254, 255, 255]` as expected. + Screenshots: ignored `build/message-codegen/viewers/cdr-browser-custom.png` + and `cdr-browser-image.png`. Browser recording: + `/tmp/dimos-cdr-browser-harness/agent-workspace/recordings/session-20260921-192504` + (6 frames). The task-owned browser and Vite server were stopped afterward. + +The combined Python runtime/web subset passed 318 tests before adding the explicit +WebRTC sequence regression; that WebRTC file then passed all 29 tests. The final +SDK and cockpit runs remained 322 and 154 tests, with both type checks passing. +Mypy passed on 12 changed runtime/helper/codec modules. The external packaging +relay and installed typing check also passed after making the bundled generator +a regular package so an installed DimOS cannot shadow it. + +The subsequent relay consumer migration passed the entire `dimos/web` suite +plus the generated image/grid helper tests: **677 passed in 35.59 s**. This +includes forkserver blueprint deployment, real relay session/transport tests, +image and map delivery, cached-map replay after unsubscribing, relay respawn, +teleoperation stop/watchdog behavior, and lease handover. The watchdog's old +`Twist.zero()` call was replaced with the generated zero-valued `Twist()`. +The new array-to-image helper also passed strided input, independent copy, +explicit encoding, and big-endian depth tests. + +### Native SDK CDR relay + +The C++ SDK suite passed all **76 tests** using real generated messages in its +default-codec tests. The Rust SDK passed **143 tests**, including TF, both CDR +byte orders, malformed input, and propagation of publish encoding errors without +enqueuing a payload. C++ and Rust ping/pong examples and the custom/image relays +built successfully. The generated Rust crate also passed Cargo source-package +verification: a build from the packaged archive, with only Python's standard +library used for generation and no preexisting generated source. + +The native demo used the real SDK processes, generated message packages, stdin +launch protocol, and Python typed transports: + +```text +lcm: Python weight=4 → C++ weight=5 → Rust weight=6 → Python verified +lcm: 921,600 image bytes and source nanoseconds=1700000000123456789 match +zenoh: Python weight=4 → C++ weight=5 → Rust weight=6 → Python verified +zenoh: 921,600 image bytes and source nanoseconds=1700000000123456789 match +``` + +Full output and process logs are under `build/message-codegen/demo/evidence`. +This does not complete the native consumer, Nix build matrix, coordinator +blueprint, or live Rerun migration; stage 4 remains open. + +### Lidar and mapping message consumers + +After switching their generated fields and codecs, the Livox Rust suite passed +24 tests, voxel mapping passed 60, and the MLS planner passed 94. The planner's +new regression checks that graph endpoints and cost survive CDR in explicit +`LineSegments3D` fields. The SDK suite passed 146 tests after extracting the shared +point-cloud reader, including padded organized rows, mixed float32/float64 XYZ, +both byte orders, malformed layouts, and nonfinite-coordinate filtering. + +The Python structured point-cloud helper passed 21 tests; together with the +Livox configuration tests, the focused Python run passed 26. The helper and +three native Python module declarations passed mypy with the generated stubs. +`demo_pointcloud.py` displayed both byte orders with organized shape `(2, 1)`, +row/point strides `(32, 20)`, and preserved two-value tags. Editing its explicit +copy left the borrowed source unchanged. Output is recorded in +`build/message-codegen/demo/evidence/pointcloud-layouts.txt`. + +The Livox live loopback E2E also passed against the released CDR binaries +(`test_live_loopback_handshake_and_stream`, 3.03 s). It exercised the virtual +Mid-360 handshake, actual UDP sensor traffic, native CDR publication, Python +schema decoding, a 5 m point ring, per-point deskew offsets, and gravity IMU. +The physical-sensor recording test remains deselected; no robot was required. + +### Remaining lidar and camera native adapters + +Point-LIO's Rust suite passed 7 tests after moving to generated CDR. Its point +reader now preserves deskew offsets for padded organized rows in either byte +order. RealSense's Rust suite passed 3 tests, including the generated `ImuInfo` +round trip that replaces its handwritten fingerprint and LCM encoder; its Python +configuration suite also passed 3. The hardware-free RGBD benchmark built and +encoded 407,040, 101,760, and 45,280 points at pixel strides 1, 2, and 3. +These are synthetic conversion checks, not a physical-camera acceptance test. + +The dimSLAM Rust adapter passed 7 tests, covering generated odometry/cloud +conversion, exact and negative timestamps, depth byte-order normalization with +row padding, BGR-to-RGB conversion, and invalid layouts/backend dimensions. +The Python camera/lidar declarations and geometry helper passed mypy. +The new odometry-to-TF helper passed zero, negative, and nanosecond-precise source +timestamp cases and preserves the message's declared parent/child frames. + +The C++ SDK suite passed 77 tests including the production lidar cloud builder. +Its standalone `dimos_lidar_cdr_demo` visibly decoded two XYZI points in each CDR +byte order, preserving frame `lidar`, timestamp `1s + 250000000ns`, and intensity +`0.5`. Output is in `build/message-codegen/demo/evidence/lidar-cpp.txt`. + +Both full C++ lidar Nix builds completed successfully with the generated CMake +package: Point-LIO and FAST-LIO. Their flake inputs/locks and CMake configurations +no longer fetch `dimos-lcm` headers. The generated package itself built from the +pinned source definitions in Nix. The root Rust workspace passed `cargo check +--workspace --locked`; the independently built RealSense and dimSLAM crates +passed the tests reported above. + +Clippy passed with warnings denied for the SDK and migrated root-workspace +modules, and independently for RealSense and dimSLAM. Generated explicit Default +implementations retain a targeted lint annotation: the same emitter supports +nonzero `.msg` defaults and fixed arrays longer than 32 elements. + +### Independently generated messages in the native SDKs + +The external package relay was rebuilt after aligning generated Rust codec errors +with SDK port signatures. Both the installed file relay and the actual native +SDK relay passed. The external application changed only its local `.msg` copy; +its Rust ports used the external type's generated methods directly: + +```text +lcm: external Python → native C++ → native Rust → Python: added-locally/cpp-native/rust-native +zenoh: external Python → native C++ → native Rust → Python: added-locally/cpp-native/rust-native +SDK codecs accepted the external generated type directly; no handwritten codec or upstream schema PR. +``` + +All other fields and the exact timestamp matched after both native processes. +The external Rust consumer test also verified SDK-compatible function signatures, +`InvalidInput` on bounded-field encode failure, and `InvalidData` on truncated +CDR. The built-in custom-message/image SDK relay still passed on both transports, +and the Rust SDK suite still passed all 146 tests. Mypy passed for both runtime +demo entry points using their separately generated type stubs. + +### TF through the real module coordinator + +The generated-message Python/Rust TF example now runs through +`ModuleCoordinator.build`, Python worker deployment, native process startup, +and typed stream wiring. Both LCM and Zenoh checks passed, each collecting four +composed `a → d` transforms with `x=1.5`, `y² + z²=1`, and changing positions. +Both runs returned normally after interruption and stopped their native workers. +The test logs are captured in pytest's temporary directory; the test summary is +`build/message-codegen/rust-tf-blueprint-tests.log`. + +This verifies coordinator wiring beyond the direct native SDK relay. It does not +complete the three-language blueprint with live viewers required by task 4.10. + +### Raw LCM fragment validation + +The retained Rust raw transport passed 18 unit tests and its compile doctest after +adding bounded reassembly. Checks cover reordered/duplicate fragments, conflicting +metadata, invalid offsets/counts, overlapping or missing bytes, invalid channels, +expiry, aggregate payload limits, and continuing reception after malformed UDP +packets. Clippy passed with warnings denied. + +The Python ↔ Rust raw transport demo still exchanged 128, 65,536, and 1,048,576 +bytes exactly. After rebuilding the Rust native examples, the three-language SDK +relay passed on LCM and Zenoh again, preserving 921,600 image bytes and timestamp +`1700000000123456789`. Transport limits are documented in the raw crate README. + +### Three-language coordinator blueprint + +`demo_blueprint.py` now deploys the Python producer/verifier and C++/Rust relays +through the actual module coordinator. Its parametrized E2E test passed on both +LCM and Zenoh, checking three distinct samples each. Two additional standalone +Zenoh launches each verified five samples. Every sample changes its custom +segment fields, 921,600 RGB image bytes, and exact source nanoseconds; Python +compares the complete returned messages against the expected native edits. +Mypy and Ruff passed for the demo. + +The Zenoh demo uses a temporary loopback router and explicit client endpoints. +Initial runs using default multicast peer discovery intermittently returned +neither stream despite successful native startup. Explicit routing passed the +repeated launches, but this does not establish the cause or fix default peer +discovery. Keep that finding open for the final runtime checks. Logs are +`build/message-codegen/three-language-blueprint-{tests,zenoh,zenoh-repeat}.log`. + +This completes the three-language coordinator portion of the human demo; +live Rerun/browser image and pose integration is still required for task 4.10. + +### Generated Python TF and recorded-stream lookup + +The live TF buffer and `StreamTF` now use generated `TransformStamped` and +`TFMessage` values. Composition/inversion/Euler conversion live in geometry +helpers backed by the existing SciPy dependency, outside generated classes. +The time index keys transforms by integer nanoseconds, snapshots incoming data, +and returns independent lookup values. + +The migrated TF suite and geometry helper suite passed 68 checks together: +live/in-memory/SQLite lookup parity, composition/inversion, nearest-time and +retention behavior, exact/negative timestamps, distinct adjacent nanoseconds, +copy independence, and invalid rotations/frames. Mypy passed on the TF buffer, +recorded-stream lookup, geometry helpers, and native TF demo. The coordinator +TF check passed on both LCM and Zenoh, now verifying Python and Rust composition +of the same chain. + +Correction to the earlier quaternion diagnosis: the pinned Jazzy +`geometry_msgs/msg/Quaternion.msg` defines `w=1`; the generated default is an +identity rotation. A zero quaternion must be constructed explicitly to test +rejection. The initial missing TF replies were not evidence of a zero default. +Robot-specific publishers and other legacy convenience-class consumers remain +to be updated; this is not completion of task 4.2 or 4.6. + +### Static publishers and robot mount callers + +`StaticTfPublisher` and its frame-tree helper now use generated stamped transforms +and exact `time.time_ns()` stamps. The Go2/Mid-360, Alfred URDF, RealSense, and +Mid-360/RealSense mount declarations use those values. Go2's Zenoh mount tree and +odometry-to-TF callback were converted as well; its camera/video consumers still +need their separate runtime migration. + +Spot's URDF mount composition and optical-frame rotation now use the geometry +helpers. Its SDK odometry boundary publishes generated Odometry and a matching +TFMessage, and replay's TF/odometry declarations use the generated types. A +synthetic SDK-value test checks negative source time, pose, velocity, frame names, +and CDR round trips without importing or connecting to the robot SDK. + +Eleven mount/helper/Spot boundary checks passed. The cumulative TF demo now +includes the production periodic `StaticTfPublisher` for `b → c`; Python publishes +`a → b`, Rust publishes `c → d`, and both language buffers compose `a → d`. +Both LCM and Zenoh E2E checks passed, including shutdown. This is hardware-free +message/geometry verification, not physical robot or camera acceptance. + +### Camera calibration and webcam cutover + +Calibration construction/YAML loading now returns generated CameraInfo values. +The Go2 YAML helper was moved out of the connection module so the Zenoh-side +module no longer imports the robot SDK just to obtain calibration. Go2's periodic +calibration publisher uses integer nanoseconds and leaves its stored template +unchanged. The broader Go2 connection still has legacy image/cloud/pose paths; +its two generated perception-interface type mismatches remain to be resolved by +that consumer migration. + +The webcam and CameraModule use generated Image, CameraInfo, and TFMessage values. +Stereo crops preserve RGB pixels and exact source headers. Metadata uses a shared +stamp, declared frame names, and independent copies of configured mounts. Image +sharpness is now an explicit helper used by the existing quality-barrier operator. + +Forty-four camera/helper/view tests passed, plus the focused Go2 publication test. +Checks cover the actual 1280x720 equidistant calibration, CDR round trips, invalid +matrix dimensions/intrinsics/FOV, synthetic BGR capture and stereo crops, metadata +frame/stamp consistency, and sharpness ordering across visual encodings. Mypy +passed on eight changed helper/camera/calibration/demo modules. Physical capture +and the complete Go2 stack were not run. + +`demo_message_helpers.py` visibly prints the decoded calibration matrix and four +distortion coefficients with source timestamp `1700000000123456789`; output is +`build/message-codegen/demo/evidence/message-helpers.txt`. + +### Go2 sensor values and simulation lidar IPC + +The WebRTC video/lidar conversion now constructs generated Image/PointCloud2 +values directly. Hardware-free tests exercise the actual reactive conversion +using synthetic driver events, checking RGB pixels, XYZ coordinates, declared +frames, exact host-arrival stamps, and CDR round trips. The new XYZ factory +preserves organized shape, copies source storage, and rejects finite coordinates +that cannot fit float32. Missing points remain NaN/infinity with `is_dense=False`. + +MuJoCo video/calibration construction and its lidar shared-memory boundary use +generated messages. Lidar IPC now carries CDR instead of pickle. Odd/even sequence +checks discard an observed in-progress or changed write before decoding. The +standalone `demo_mujoco_lidar_shm.py` passed with separate producer/consumer +processes and printed two expected XYZ points and timestamp +`1700000000123456789`. No engine or robot was launched. + +The focused cloud, WebRTC conversion, connection lifecycle, and shared-memory +suite passed **51 tests**, with one hardware-recording test deselected. Log: +`build/message-codegen/go2-sensor-cutover-tests.log`. Mypy passed on eight changed +production modules. Pytest ran with `-o addopts=''` because this local environment +lacks the configured xdist/timeout/coverage plugins. Repository conftest emitted +its existing subprocess ResourceWarning; the standalone IPC demo exited cleanly. + +This resolves the Go2 image/cloud interface type mismatches recorded above. +Go2/WebRTC/MuJoCo pose and TF callers still need their generated-value cutover; +DimSim's external browser producer and complete live simulation remain unverified. + +### Go2 and simulation pose/TF values + +WebRTC odometry and TF streams now return generated PoseStamped and +TransformStamped. The raw converter preserves the device header; the live +connection explicitly replaces it with a host-arrival header, preserving the +existing timing policy without a float timestamp round trip. Go2's publication +copies the received pose before overriding its configured parent frame. Its +mount chain and TFMessage retain that pose's exact stamp. + +MuJoCo's pose boundary maps simulator WXYZ quaternions into generated XYZW fields. +DimSim's TF builder and G1 simulation ports/mounts also use generated values. G1's +camera metadata is restamped per publication without mutating the template. +The Unitree legacy Odometry subclass still serves unconverted recording-fixture +callers; it has no new fallback role and remains scheduled for deletion. + +The focused geometry, device conversion, Go2/WebRTC, MuJoCo lifecycle/pose, and +simulation TF suites passed **43 tests**. Simulator startup dependencies are +stubbed in lifecycle/pose tests; shared-memory odometry uses real regions. No +engine or robot was run. Eight changed production modules passed mypy. +Logs: `build/message-codegen/unitree-pose-tests.log` and +`build/message-codegen/unitree-simulation-tf-tests.log`. + +The human-facing `demo_robot_tf.py` passed and printed the decoded namespaced +camera chain with source timestamp `1746565669448350564`. Captured output: +`build/message-codegen/demo/evidence/robot-tf.txt`. + +The broader Go2 blueprint topology test could not collect because PyTorch is +absent from this environment. Its generated-pose fixture has been updated, but +that test is not counted as passing. Full blueprint/runtime acceptance remains +open, as do recorder and legacy-fixture cutovers. + +The cumulative Python/Rust coordinator TF demo was rerun on both LCM and Zenoh: +**2 E2E tests passed** in 8.68 seconds, including changing transform lookups and +clean worker shutdown. Log: `build/message-codegen/unitree-pose-tf-e2e.log`. + +### Python storage codecs and generated-message replay + +Generated messages now select CDR instead of falling through to pickle. SQLite +persists/reconstructs `cdr` or `lz4+cdr`; old `lcm`, `lz4+lcm`, and private `jpeg` +codec IDs are rejected. The obsolete Python LCM and JPEG storage codecs are +removed. Raw/depth Image values remain lossless; compressed images are explicit +CompressedImage values stored through the same CDR codec. + +The generic MCAP reader resolves installed types from `cdr`/`ros2msg` schema names. +Unknown schemas remain raw bytes, and Python payload-module metadata does not +trigger imports. Duplicate compatible channels on a topic aggregate counts; +conflicting schemas/timing policies and stream-name collisions are rejected. +Generic dataset opening now uses McapStore, leaving the robot-specific DDS preset +available for callers that explicitly choose it. + +TransportRecorder now accepts generated-message contracts and reads source time +from Header. A zero source stamp is preserved; unstamped values use receipt time. +Observation pose conversion accepts generated nested PoseStamped/TransformStamped +and returns generated values. Replay ports use the recorded generated class +directly; the old rich-subclass-to-base-type substitution is removed. + +**47 focused tests passed**, covering exact values/bytes after SQLite reopening, +raw/depth/compressed images, codec reconstruction and obsolete-ID rejection, +MCAP schema discovery/time ordering, pose metadata, recorder timestamps, and real +module replay. Log: `build/message-codegen/storage-cdr-tests.log`. +Tests ran with `--noconftest` because the memory conftest eagerly imports optional +CLIP/PyTorch dependencies. Two broader checks were deselected after their imports +failed: Rust-recorder delegation needs PyTorch, and the full Rerun blueprint needs +Numba. They are not counted as passing. Seven changed production modules passed +mypy. Checking the demo's full import graph additionally hits the installed MCAP +writer's `from .__init__ import __version__`, which mypy reports as duplicate +module names; the demo itself was executed successfully. + +`demo_storage_replay.py` passed: SQLite and MCAP both returned weights `[4, 5, 6]`, +and module ports replayed stamps `1700000000123456789` through +`1700000000123456791`. Output is +`build/message-codegen/demo/evidence/storage-replay.txt`. Temporary files and module +resources were cleaned up. The Rust recorder and remaining legacy fixtures still +need conversion; this does not close stage 4 or stage 5 acceptance. + +### Native recorder configuration dependency audit + +The shared OnExisting policy moved to `dimos.memory.recording_policy`; its callers +now import it there. Native recorder configuration no longer imports the Python +embedding module just to obtain this enum. The previously blocked Rust-session +delegation test passes without PyTorch, and the policy/native configuration +modules pass mypy. + +Running both native-recorder Python configuration suites now reaches their real +storage behavior: **23 passed, 9 failed**. Every failure requests the obsolete +`lcm` storage codec. This is an outstanding coordinated Python/Rust recorder +cutover, not an accepted regression result. Log: +`build/message-codegen/rust-recorder-config-audit.log`. + +### Native recorder CDR and external schemas + +The coordinated native recorder cutover resolves the nine obsolete-codec failures +above: **32 Python configuration checks, 16 Rust unit tests, five native process +E2E tests, and one external-package recording E2E test passed**. Production Python +configuration modules pass mypy; the Rust recorder passes Clippy with warnings +denied. Root workspace `cargo check --workspace --locked --offline` passes. + +The recorder now receives complete generated schemas, writes ROS2-profile MCAP +with `cdr` channels and embedded `ros2msg` definitions, and uses chunk compression. +SQLite accepts `cdr` and `lz4+cdr`; obsolete LCM/JPEG wrappers are removed. Raw +image pixels remain exact. The root Cargo lockfile no longer depends on the +external `dimos-lcm` message repository; the in-tree raw LCM transport remains. + +Recognized stamped messages retain integer source nanoseconds, including the E2E +MCAP value `1700000000123456789`. Zero stamps remain zero; invalid nanosecond +fields are rejected. MCAP rejects negative timestamps instead of silently +clamping them. SQLite converts timestamps to seconds only at its existing query +API boundary. Known ordinary message payloads preserve their original CDR bytes, +including byte order; TF arrays split into generated single-transform TFMessages. + +`demo_native_recording.py` records three changing external Telemetry messages on +each transport with the same Rust binary. The locally added `application_note` +field survives along with every payload timestamp. The unknown custom timestamp +layout uses reception time for MCAP indexing. Foxglove's independent decoder +reads all six samples from embedded schemas without the custom package installed +in its environment. This is decoder evidence, not Foxglove/Rerun UI acceptance. + +Review artifacts and transcripts: + +- `build/message-codegen/demo/evidence/external-native-recording-{lcm,zenoh}.mcap` + and matching `.log` files. +- `build/message-codegen/demo/evidence/native-recording.txt` and + `native-recording-foxglove.txt`. +- `build/message-codegen/rust-recorder-cdr-tests.log`, + `rust-recorder-config-cdr-tests.log`, `rust-recorder-cdr-e2e.log`, and + `external-native-recording-e2e.log`. +- `build/message-codegen/rust-recorder-cdr-clippy.log` and + `recorder-workspace-check.log`. + +The Python recorder checks use `--noconftest` to avoid unrelated optional +CLIP/PyTorch imports. This leaves an unregistered `self_hosted` marker warning. +The host filesystem filled during the first workspace/Nix build attempts; those +attempts failed and are not counted as passing. Removing only this worktree's +reproducible compiler caches allowed the workspace check and executable build +to pass. The isolated Nix retry then passed, including all 16 Rust tests and +installation/fixup. An offline Nix build confirmed the completed output. The +installed Nix executable also passed the external custom-message demo on both +transports; its transcript is +`build/message-codegen/demo/evidence/nix-native-recording.txt`, with MCAP/log +artifacts in the adjacent `nix-native-recording/` directory. Full stage 4/5 +acceptance, legacy consumers/fixtures, and live viewer demos remain outstanding. + +### Optional ROS bridge uses CDR directly + +Removed the LCM intermediate types, recursive field-copy conversion, renamed +time fields, complex-type allowlist, and old type-import caches. The bridge +resolves canonical `package/msg/Type` names and uses installed ROS serialization +with generated DimOS encode/decode. Mismatched message identities are rejected +even when their wire layouts match. Importing the conversion module remains +possible without ROS; actually requesting conversion reports the missing +optional dependency. + +**25 checks passed** against the installed Jazzy type support in the isolated +reference container. Coverage includes seven real pub/sub cases, nested poses, +empty/nonempty TF arrays, raw/compressed image bytes, padded big-endian point +cloud data, camera calibration arrays, Unicode frames, exact epoch/zero/negative +stamps, invalid names, and wrong-type rejection. On the ROS-free host, four +contract checks pass and 21 ROS-dependent checks skip. The container's minimal +pytest reports three unknown optional-plugin configuration warnings. + +`demo_ros_bridge.py` passed with three changing image/pose pairs on actual ROS +topics. Transcript: `build/message-codegen/demo/evidence/ros-bridge.txt`. The +local existing reference image needed `typing_extensions.py` mounted from the +host virtualenv; the updated reference Dockerfile explicitly installs that +dependency and pytest. The CI job now downloads the standalone Python binding +and runs the same bridge checks and demo; this is workflow wiring, not a claim +that the remote job has passed. + +Galaxea depth/lidar/IMU and compressed-image callers now use generated sensor +types. Compressed images preserve their source format and header rather than +forcing JPEG and replacing zero timestamps with wall time. Other Galaxea +geometry/control consumers still need conversion; robot hardware was not used. +Mypy passes on the converter, Galaxea connection, and demo. The full runtime +cutover remains incomplete. + +### Galaxea generated control and feedback messages + +The R1Pro connection now uses generated MotorCommandArray, JointState, Twist, +PoseStamped, Odometry, and TFMessage alongside the previously converted sensor +streams. No legacy message imports remain in this connection module. Commands +use generated arrays directly and require 18 entries in every declared array +before publishing. ROS command conversion uses the CDR bridge; unsupported +base axes remain zero. + +Wheel integration computes delta time from integer source nanoseconds and +publishes nested ROS2 pose/twist fields. Pose, odometry, and both TF edges retain +the same exact stamp. Joint-state aggregation retains the oldest segment stamp, +including zero, without converting through floating-point seconds or replacing +it with wall time. The publisher's interval wait now responds to its stop event. + +**13 focused checks passed**, covering integration and clock jumps, exact +zero/epoch joint stamps, all 18 joint values, three command segments, tracking +velocity sentinels, and rejection of each malformed command array. Tests use +real generated messages and mock only the ROS/output boundaries. Mypy passes on +the connection and demo. Log: `build/message-codegen/galaxea-cdr-tests.log`. +The test launcher emitted a subprocess ResourceWarning; the reported process +was no longer live when checked after the run. + +`demo_galaxea_messages.py` passed using the actual module output subscriptions. +It printed base x coordinates `0.100000`, `0.199980`, and `0.299900`, the complete +lidar TF chain, and exact stamps `1700000000223456789` through +`1700000000423456789`. Transcript: +`build/message-codegen/demo/evidence/galaxea-messages.txt`. No robot hardware or +ROS was required. This validates the connection's generated-message boundary; +remaining downstream whole-body/navigation consumers and hardware acceptance +are not claimed complete. + +### Whole-body adapter generated streams + +TransportWholeBodyAdapter now declares generated JointState, Imu, and +MotorCommandArray types. Commands receive an explicit wall-clock header. Five +focused tests pass for disconnected behavior, preservation of complete feedback +when a short frame arrives, and XYZW-to-WXYZ IMU conversion. The adapter and demo +pass mypy. + +The hardware-free demo passed on LCM and independent loopback Zenoh sessions, +exchanging two-joint feedback, IMU, and full hybrid motor commands. Transcript: +`build/message-codegen/demo/evidence/whole-body-transport.txt`. The automatic +subprocess E2E check exercises the same assertions and cleanup. The initial demo +Zenoh factory incorrectly passed a leading slash to the raw transport; it now +uses the same `dimos/` topic mapping as the production transport factory. +No production topic compatibility path was added. + +### G1 and shared coordinator joint-state cutover + +G1WholeBodyConnection now uses generated MotorCommandArray, JointState, and Imu +messages. It constructs one explicit integer-nanosecond arrival header for each +feedback sample, shared by the joint and IMU messages. Unitree WXYZ is explicitly +copied to ROS XYZW. Every command array must contain all 29 joints before any +DDS command is written. Existing damping-first soft start, mode-machine, and +CRC behavior remain covered by the command tests. + +The shared coordinator, tick-loop outputs, velocity/trajectory consumers, and +IK task joint states now use generated JointState. IK command snapshots use +explicit deep copies; generated sequence names are compared as lists. Internal +coordinator snapshot timestamps remain floating seconds, converted explicitly +to Header.stamp at publication. G1/GROOT and Galaxea coordinator transport maps +and declared joint ports now reference the same generated types. Other legacy +pose/trajectory consumers remain outstanding. + +**12 G1 tests and all 369 tests in `dimos/control` passed.** The narrower 138-test +routing/joint-state suite and 60-test IK suite passed before broadening to the +full directory. Pinocchio 4.1.0, pin-pink 4.3.0, and QP dependencies were installed +from the local cache; ONNX Runtime 1.24.1 was installed to collect the GROOT task +tests. Mypy passes on 12 changed production/blueprint/demo modules. + +Both human demos passed: G1 printed 29-joint samples with q0 `0.0`, `0.1`, `0.2` +and exact stamps `1700000000123456789` through `1700000000123456791`; the running +mock coordinator printed left `[0.1, 0.2]`, right `[0.3, 0.4]`, and the merged +four-joint state with matching tick stamps. Transcripts: + +- `build/message-codegen/demo/evidence/g1-messages.txt` +- `build/message-codegen/demo/evidence/coordinator-joints.txt` +- `build/message-codegen/g1-wholebody-cdr-tests.log` +- `build/message-codegen/control-all-cdr-tests.log` + +These checks exercise synthetic hardware boundaries and real coordinator ticks; +no physical G1 control or complete robot blueprint acceptance is claimed. + +### Generated base-motion adapter and coordinator commands + +TransportTwistAdapter now uses generated Twist/PoseStamped and reads nested +position/quaternion fields through the geometry yaw helper. The coordinator and +GROOT command contract, plus Go2/GROOT transport maps, use generated Twist. +Existing enable, stop, one/two/three-axis mapping, and odometry copy-isolation +behavior remain covered. + +**373 checks passed** (four base-adapter cases and all 369 control tests), plus +one subprocess E2E that runs the base demo on both LCM and independent Zenoh +sessions. Mypy passes on six changed production/blueprint/demo modules. +The demo received odometry `[2.0, -1.0, pi/2]`, observed the generated velocity +command `(0.5, -0.2, 0.3)`, and observed a zero Twist after disable. + +Logs: `build/message-codegen/base-control-cdr-tests.log`, +`base-transport-cdr-e2e.log`, and +`build/message-codegen/demo/evidence/base-transport.txt`. No hardware motion was +performed. Legacy benchmark/trajectory/pose consumers outside this adapter path +remain pending in the broader runtime cutover. + +### Manipulation planning consumes generated JointState + +Planning-group projection/target normalization, joint-space validation, planning +protocol/model declarations, and RobotStateMonitor now use generated JointState. +Projection and normalization explicitly copy the source Header instead of +inventing a new timestamp. RobotStateMonitor preserves that header when syncing +its reordered state into the world and invoking callbacks; arrival-time freshness +tracking remains separate. Generated sequence data is converted explicitly at +the NumPy joint-space boundary. + +**47 tests passed** across planning groups, planning specs/model validation, and +the state monitor. Added checks cover negative/epoch source stamps, header copy +isolation, reordered positions/velocities, incomplete feedback, and stopped +monitor behavior. Six production/demo modules pass mypy. The yourdfpy 0.0.60 +dependency was installed to run model-validation tests rather than omit them. + +The hardware-free planning demo passed: generated CDR feedback selected left-arm +positions `[-0.2, 0.3]`, normalized target `[0.1, 0.5]`, validated joint limits, +and retained source nanoseconds `1700000000123456789`. Logs: +`build/message-codegen/planning-joints-cdr-tests.log` and +`build/message-codegen/demo/evidence/planning-joints.txt`. + +Planner backend implementations, manipulation module entry points, and the +interactive manipulation visualizer still have legacy message consumers. These +checks do not claim the full manipulation stack is converted. + + +### Planner backend JointState and PointCloud2 self-filter cutover + +All planning backend JointState imports now use generated values, including +Drake/Jacobian/Pink IK, RRT and RoboPlan, world synchronization, path utilities, +and trajectory parametrizer inputs. Dictionary constructors became explicit +keyword construction; the parametrizer copies generated inputs with deepcopy. +Array/list conversions are explicit at numerical and sequence API boundaries. +Pose and trajectory message types remain part of the pending cutover. + +The robot self-filter now consumes generated PointCloud2 and TransformStamped. +A separate selection helper retains complete point records, including arbitrary +fields, field counts, point padding, and byte order. It removes row padding when +forming an unorganized result and preserves the source Header. Clear masks copy +the source stamp into a world-frame header. Transform matrices come from an +explicit geometry helper. + +**253 tests passed with no skips** across the complete planning subtree and the +geometry/point-cloud helper suites. Installed pinned RoboPlan 0.6.0 and Drake +1.49.0 allowed actual backend tests to run. Added selection tests cover both byte +orders, custom array fields, point padding, header preservation, copy isolation, +empty output, and invalid masks. Nineteen production/demo modules pass mypy. + +Both human-facing terminal demos passed: `demo_planning_backends.py` printed five +CDR-decoded, collision-checked joint waypoints in a real Drake world; +`demo_self_filter.py` printed three changing arm positions, one retained point +per capture, and the original nanosecond stamp on both outputs. Logs: +`build/message-codegen/planner-backends-cdr-tests.log`, +`planner-backends-mypy.log`, and +`build/message-codegen/demo/evidence/{planning-backends,self-filter}.txt`. + +These local results do not claim that manipulation entry points, legacy poses +and trajectories, interactive visualizers, or the full repository are converted. + + +### Generated trajectories from planning through execution + +Trajectory generators, parametrizers, previews, execution splitting, joint and +planar-base tasks, coordinator RPC contracts, and the legacy threaded controller +now use generated ROS2 JointTrajectory and JointTrajectoryPoint values. Waypoint +times are Duration fields; validation compares normalized integer nanoseconds, +including adjacent nanoseconds beyond floating-point resolution. Interpolation +and duration calculation live in `dimos.msgs.trajectory`. Position-only joint +commands are accepted with zero velocity feed-forward. Joint/base splitting +preserves all optional point fields and the original Header. Anchoring a running +trajectory also preserves its metadata and optional fields. + +Execution feedback now uses generated `dimos_msgs/msg/TrajectoryStatus` with +Duration elapsed/remaining fields and a wall-clock Header. The internal enum +references generated wire constants, and wire state comparisons use equality. +The three handwritten trajectory codecs were deleted, with no legacy imports +remaining in Python source. The typed test spy uses the generated decode API and +coordinator tests use canonical schema names. + +The remaining manipulation JointState consumers (module, SDK, operator, Viser, +and controllers) now use generated values, explicit copies, and list conversion +at sequence API boundaries. Model-state reordering preserves the incoming Header. +Snapshot reprs still expose joint values for interactive inspection. + +Validation: + +- **968 tests passed** across all manipulation and control tests, trajectory + helper tests, the transport demo subprocess test, and LeRobot's host module + contract tests. No skips. This includes real Drake/RoboPlan and Viser tests. +- **Three Galaxea planar-preview tests passed**, including moving its mock base + through generated trajectory execution. +- **Five process-level coordinator E2E tests passed on Zenoh and the same five on + LCM**: RPC startup, execution, published feedback, cancellation, and dual-arm + execution. No coordinator or pytest processes remained afterward. +- **26 focused checks passed** after the final header-preservation and demo edits. +- **28 production/demo/test-support modules passed mypy.** +- The isolated LeRobot runtime's joint/trajectory inputs and corresponding tests + were updated and syntax-checked. Its full runtime suite is still pending; the + separate torch/lerobot environment is not installed here. Its legacy image API + also remains part of the broader image-consumer cutover. + +`demo_trajectory.py` passed on LCM and independent loopback Zenoh sessions. It +transmitted 51 generated points, preserved source stamp +`1700000000123456789`, printed changing commands from `[0, 0]` to `[0.3, -0.2]`, +and received generated COMPLETED status with zero remaining duration. The demo's +synthetic clock exercises the actual task without moving hardware. + +Logs: `build/message-codegen/trajectory-cdr-tests.log`, +`trajectory-cdr-mypy.log`, `trajectory-planar-preview-tests.log`, +`trajectory-blueprint-tests.log` (Zenoh), `trajectory-blueprint-lcm-tests.log`, +`trajectory-final-focused-tests.log`, and +`build/message-codegen/demo/evidence/trajectory.txt`. + +Local setup added the pinned optional xacro, Viser, MuJoCo, websocket-client, and +pytest plugins needed for the existing suites. These results do not establish +full-repository CI or completion of the remaining pose/image/viewer cutover. + +### Generated pose, grasp, and path consumers (runtime cutover in progress) + +Manipulation planning, FK/IK, preview, dispatch, the SDK, and Viser now use +nested generated Pose/PoseStamped values. Matrix/Euler/local-offset operations +live outside the generated classes. Robot state snapshots retain the source +Header atomically; FK-derived poses and their TF edges retain exact source +nanoseconds. Static mount publication copies its configured transform before +setting the publication time. + +GraspCandidate and GraspCandidateArray now come from the generated DimOS schema +package. The two handwritten pickle codecs are removed. The heuristic and +isolated GraspGenX adapter accept generated clouds, preserve their source +headers, and return generated ranked proposals. Pick-and-place validates finite +scores and valid pose geometry before issuing a gripper or motion command. +The GraspGenX tests use a mocked inference backend, not a loaded GPU model. + +Base path followers, benchmarking, and coordinator scalar/stamped-twist commands +use generated messages. SVG and Rerun scene rendering reads generated points, +poses, paths, detection boxes, and occupancy grids. The occupancy origin rotation +is applied in both renderers. Existing occupancy borrowing and block reduction +helpers remain in use; renderers validate physical resolution separately. + +Human-facing demonstrations: + +- `demo_planning_backends.py`: real Drake/RRT, five FK tool poses and CDR TF edges; + the first source stamp is `1700000000.123456789`. +- `demo_grasp_proposals.py`: a four-point CDR cloud becomes a CDR ranked grasp at + `[0.5, 0, 0.15]`, with downward approach and unchanged nanoseconds. +- `demo_path_following.py`: 41 generated path waypoints and CDR Twist commands + drive the existing base simulator to `arrived` in 38 ticks. The SVG in + `build/message-codegen/demo/evidence/path-following.svg` overlays reference + and executed paths. + +Validation logs are under `build/message-codegen/`: `pose-combined-tests.log`, +`pose-space-tests.log`, `pose-combined-mypy.log`, `pose-blueprint-lcm-tests.log`, +and `pose-blueprint-zenoh-tests.log`. The combined manipulation, control, geometry, grasp, object, and transform suite +passed 1080 tests. Both coordinator E2E runs passed all five tests. The +renderer/occupancy/view suite passed 61 tests, including actual +headless Rerun SDK logging and rotated-grid geometry. + +This checkpoint is not an accepted stage-4 completion. The broad type check of +127 modules still reports four errors at legacy point-cloud boundaries: the +older grasp provider, the scene-to-pick cloud interface, and cloud-derived +occupancy rendering. Their producers must be converted; no compatibility reader +or fallback was added. The temporal object-registration suite also fails +collection because the optional Hydra/segmentation environment is absent. +Focused generated-object CDR and spatial-deduplication tests pass. Full viewer UI +acceptance and the remaining inventory retain their unchecked OpenSpec tasks. + +### Generated cloud and occupancy renderers (runtime cutover in progress) + +Cloud-to-map algorithms, inflation, gradient/Voronoi maps, obstacle operations, +and navigation-map composition now use generated PointCloud2/OccupancyGrid values. +Map images and path footprints use generated Image/Path values. Space accepts +CDR point clouds directly in both SVG and Rerun. Packed rgb/rgba conversion handles +FLOAT32 color bits, UINT32, both byte orders, and padded rows. + +- `occupancy-renderer-tests.log`: 92 generated mapping, color, view, and renderer + checks passed, including real headless Rerun SDK output and rotated-grid paths. +- `occupancy-fixtures-tests.log`: 10 existing image/XML fixture checks passed. + Three gradient image fixtures were regenerated for the ROS float32 resolution: + 0.05 is stored as 0.05000000074505806. A direct comparison reproduced the old + fixture exactly with float64 0.05 and identified 718 changed pixels with the + declared float32 value. Seven other fixture comparisons passed unchanged. +- `occupancy-cutover-mypy.log`: all 15 checked production/demo modules passed. +- `demo_occupancy.py`: 1,200 CDR points, 44 occupied cells before inflation and + 208 afterward; exact source timestamp 1700000000123456789 ns retained. SVG + output is `build/message-codegen/demo/evidence/occupancy.svg`. + +These checks do not complete the broader navigation cutover. A* and path +resampling/masking callers and their fixtures still require conversion, as do +remaining perception cloud/image producers. The demo SVG was generated and +validated programmatically; human visual review is deferred. + +### Generated A* and path processing (runtime cutover in progress) + +A* consumes generated occupancy buffers in both Python and the compiled C++ +extension and returns generated Path/PoseStamped values with the source header. +Resampling and path masks use nested ROS fields. Coordinate helpers apply the +full grid-origin transform. Sub-nanocell rotation roundoff no longer assigns an +exact cell corner to the preceding cell, and negative out-of-map positions are +rejected. Resampling writes pose edits back into generated sequences and merges +a numerically duplicated final point rather than reversing the last heading. + +- `path-all-tests.log`: 38 checks passed, covering the complete occupancy test + directory, recorded-map A*, native/Python parity, generated CDR paths, rotated + grids, mask exclusion, source-header retention, and input isolation. +- Mypy passed on the four changed production modules and the demo. +- The native A* extension was compiled locally; generated-pipeline tests assert + its availability before exercising the native branch. +- Six path-image fixtures were regenerated. On both recorded input maps, all + 161/107 generated A* waypoints exactly matched the old implementation when + both were given the declared float32 resolution. Image expectations now also + reflect generated path coordinates and the corrected final-point handling. +- `demo_grid_path.py`: both planners produced 41 waypoints, resampled to 122 + CDR-decoded poses, with exact source headers and no occupied mask cells. + `grid-path-python.svg` and `grid-path-cpp.svg` are the human review artifacts. + +Goal validation, higher-level global/local planners, and navigation module +wiring still require conversion; this evidence does not accept stage 4. + +### Generated replanning navigation module (runtime cutover in progress) + +Goal validation, global/local replanning, navigation maps, clearance masks, +position tracking, retry limiting, and the module's stream/spec declarations now +use generated messages. Odometry and clicked points convert explicitly to nested +PoseStamped values. Robot-footprint clearing copies and replaces the read-only +grid buffer. Clearance masks invalidate when map metadata changes. + +- `navigation-tests.log`: 67 focused checks passed, including all replanning A*, + occupancy, and geometry tests plus the two-transport subprocess navigation E2E. +- `navigation-mypy.log`: 16 production/demo modules passed. +- The arrival regression initially failed because an aligned robot at the first + path point skipped directly to final rotation. It now checks the last path + point, and the regression requires physical displacement before arrival. +- Cancellation joins the local control thread and sends generated zero Twist; + its regression verifies the captured thread is stopped. +- Goal-validator fixture coordinates changed by one cell at the float32 map + resolution. Running the old validator with that resolution reproduced all + three new coordinates exactly; obstacle-search behavior is retained. +- `demo_navigation.py` passed on LCM and independent explicit-loopback Zenoh + sessions: each run reached (2.815, 1.000) for a (3, 1) goal within the configured + 0.2 m tolerance, with 38 commands and 21 published path poses. Source timestamp + 1700000000123456789 ns survived map processing and path publication. + `navigation-lcm.svg` and `navigation-zenoh.svg` are the review artifacts. + +This verifies the navigation module with synthetic transport inputs, not a full +robot blueprint. Other navigation implementations, map producers, and perception +consumers still need conversion before stage 4 can be accepted. + +### Generated voxel and cost-map producers (runtime cutover in progress) + +CostMapper and the voxel accumulator now consume generated PointCloud2. Both +voxel implementations read declared fields through the shared point-cloud +helper; obsolete wrapper/Open3D conversion functions are removed. Generated +voxel output preserves the latest source stamp exactly, including zero stamps, +and initial-safe-radius changes replace an independent occupancy array after +releasing the borrowed view. The safety disc respects the map-origin rotation. + +- `map-producer-combined-tests.log`: 88 checks passed across voxel/cost-map + producers, cloud-to-occupancy, replanning navigation, occupancy operations, + geometry, and the expanded two-transport subprocess E2E. +- `map-producer-mypy.log`: seven production/demo modules passed. +- Packed CPU and Open3D-on-CPU backends passed union/column-carving and missing + point checks. CUDA hardware behavior was not exercised. +- The cumulative `demo_navigation.py` now sends 576 source points through CDR, + accumulates 576 voxels, runs the real CostMapper module to produce a 62×62 + grid, and drives the navigation module to arrival on both LCM and independent + explicit-loopback Zenoh sessions. Exact source nanoseconds survive the pipeline. + Terminal output is in `map-navigation-demo.log`; existing navigation SVGs are + regenerated by the expanded demo. + +The VoxelGridMapper memory-stream wrapper's generated declarations are updated, +but runtime import remains unverified here: the existing memory module imports +optional PyTorch, which is absent. The old recorded voxel tests still depend on +LegacyPickleStore/Go2Moment fixtures; replacing those recordings remains part of +the full cutover. These limitations do not affect the verified direct voxel → +CostMapper → navigation transport demo, and do not complete stage 4. + +### Generated basic follower and odometry history (runtime cutover in progress) + +The basic path follower, public navigation protocol, and OdometryHist now use +generated messages. TF lookups retain the generated PoseStamped shape. History +uses integer nanoseconds for publication throttling and copies source headers +and nested poses into generated Path output. Its Rerun override constructs the +archetype directly, retaining the actual Z coordinate. + +- `history-combined-tests.log`: 97 checks passed across follower/history, map + producers, navigation, occupancy, geometry, and the two-transport module E2E. +- `history-mypy.log`: four production/demo modules passed. +- Nine focused follower/history tests cover TF mount composition and lookup + throttling, generated velocity/arrival/empty-path behavior, one-nanosecond + publication intervals at epoch time, copy isolation, bounded history, frame + overrides, replay reset to zero, and Rerun path coordinates. +- `demo_odometry_history.py`: fixed-step simulation reached x=1.700 within the + basic follower's 0.3 m tolerance, emitted zero Twist on arrival, and retained + 35 history poses stamped 1700000000123456789 through 1700000000123456823 ns. + The generated Rerun line retains z=0.25 m. The SVG review artifact is + `build/message-codegen/demo/evidence/odometry-history.svg`. + +Exploration and patrol consumers still require generated-message conversion. +This checkpoint does not complete the runtime or viewer acceptance gates. + +### Generated frontier exploration (runtime cutover in progress) + +Wavefront exploration now consumes generated OccupancyGrid/PoseStamped/Bool and +publishes generated goals. Coordinate conversion uses the full map origin; +frontier centroids retain the map plane's height. Stop goals preserve the +current odometry header. Perimeters smaller than one cell no longer admit +empty frontier groups. + +- `frontier-combined-tests.log`: 101 checks passed across frontier exploration, + follower/history, map producers, navigation, occupancy, geometry, and the + cumulative two-transport navigation E2E. +- `frontier-mypy.log`: the explorer and demo pass mypy. +- Four frontier regressions verify translated/rotated map centroids, original + pose headers on stop, copy isolation, and an actual exploration-loop goal + decoded from generated CDR with the map's exact frame and nanoseconds. +- `demo_frontiers.py`: selected (5.00, 4.75) m at the known/unknown boundary; + stamp 1700000000123456789 ns retained. The review artifact is + `build/message-codegen/demo/evidence/frontiers.svg`. + +Patrol and other remaining consumers still need conversion. This checkpoint +does not complete the full exploration-to-robot blueprint or stage 4 acceptance. + +### Generated patrol routing + +The patrol module and all three routers now consume generated occupancy maps and +nested poses. Coordinate conversion uses the shared map-origin helpers, and +selected goals retain the map frame and integer timestamp. The existing office +coverage fixture now constructs generated clouds and maps directly. + +- `build/message-codegen/patrol-tests.log`: **9 passed**, including six generated + CDR cases covering all routers with ordinary and rotated/translated maps, plus + all three existing office-map coverage tests. +- Mypy passed for 11 checked source files (patrol production code and demo). +- `build/message-codegen/patrol-demo.log`: all routers produced five CDR goals; + three reviewable SVGs were written under `demo/evidence/patrol-*.svg`. +- This verifies goal selection and coverage calculations. It does not claim + patrol lifecycle or hardware motion acceptance, or completion of stage 4. + +### Generated movement manager + +The click-to-goal relay and teleop/navigation velocity arbiter use generated +PointStamped, Twist, and Bool. Clicks retain their source header; cancellation +headers use integer wall-clock nanoseconds. Component scaling uses explicit +keyword fields. Existing cancellation behavior is unchanged. + +- `build/message-codegen/movement-patrol-tests.log`: **14 passed**, comprising + five movement tests with CDR-decoded outputs and nine patrol checks. +- Mypy passed for the movement module and its demo. +- `demo_movement.py` printed the preserved click timestamp, teleop suppression, + and navigation resumption and completed its assertions. + +### Generated 2D visual-servo controller + +VisualServoing2D uses generated CameraInfo's lowercase `k` intrinsic matrix and +returns generated Twist values. Five parameterized checks passed for forward, +stationary, reverse, turning, and invalid-width detections after CDR decoding. +The terminal `demo_visual_servo.py` passed all four visible scenario assertions; +mypy passed for controller and demo. Person-follow and security-demo callers, +and their detection/cloud pipelines, remain part of the unfinished coordinated +runtime cutover; these controller checks do not establish their acceptance. + +### Generated detection-cloud filters + +Height, statistical, radius, and visibility filters now consume generated +PointCloud2, CameraInfo, and TransformStamped. Open3D performs geometric +selection; original point records are selected by index so custom fields are +not discarded when reconstructing the output. Four checks passed, verifying +CDR round trips, exact source headers, field definitions, and byte-equivalent +selected records carrying custom point IDs. Mypy passed for filters and demo. +`demo_cloud_filters.py` passed both visible 101-to-100-point scenarios. +Detection3DPC and its higher-level consumers still require conversion; these +results establish filter behavior, not acceptance of the person-follow pipeline. + +### Generated 3D detection projection and target calculation + +Detection3DPC and its batch projection wrapper now accept generated depth, +clouds, camera calibration, and stamped transforms. Depth projection preserves +the depth stamp and target frame; cloud projection selects original records. +Centroid poses use the cloud header. Open3D bounding calculations are explicit. +DetectionNavigation now consumes these generated values and produces generated +Twist commands using the shared quaternion helper. + +Seven projection/filter checks passed, covering translated depth unprojection, +image/behind-camera rejection, bounding extents, robust target selection, CDR +velocity output, and field-preserving filters. Mypy passed for four production +modules. `demo_detection_projection.py` passed and printed the expected world +centroid (11.5, 1.5, 2.0) with stamp 1700000000123456789 ns. The 2D detection image +API and person-follow/security callers remain unfinished; no full pipeline or +model-inference acceptance is claimed. + +### Generated 2D bounding-box image and wire boundary + +Detection2DBBox now crops generated image views, annotates an independent BGR +copy, and emits generated vision_msgs/Detection2D with the original image header. +Class identifiers use the ROS string field on the wire. Eight combined generated +bbox/projection/filter checks passed. The bbox case checks crop pixels, RGB/BGR +conversion, source immutability, integer timestamp preservation, and decoded +identifiers. Mypy passed for bbox/base and the earlier projection demo. +`demo_detection_bbox.py` generated the annotated review PNG and passed its CDR +assertions. The model-backed existing bbox fixture was not run; its generated +image shape assertion was updated, and model/image fixture conversion remains +part of the remaining perception work. + +### Generated detection collections + +ImageDetections and ImageDetections2D use generated images and Detection2DArray; +array headers preserve the source image stamp and frame. Person/segmentation +constructors now derive their application-level floating timestamp from generated +headers. Shared `image_to_bgr` produces an independent drawing array and serves +both individual and collection annotation. The annotation demo now exercises the +collection's CDR array and rendered output. + +Nine combined generated detection/projection/filter checks passed. Mypy passed +for the shared image helper, collection base, and bbox. Broader checking remains +incomplete: Torch typing is unavailable and the person class has pre-existing +unparameterized NumPy annotations. Model-backed inference and person-follow +lifecycle acceptance are still outstanding. + +### Generated vision-provider and tracker image boundaries + +Person-follow streams, EdgeTAM preprocessing, visual queries, and Qwen/OpenAI +providers now use generated messages. Provider images use explicit JPEG encoding; +legacy ndarray query compatibility was removed. VLM resize and warmup paths use +explicit array helpers. Point detections preserve generated image headers. + +Sixteen combined generated vision/detection/projection/filter tests passed. +Provider tests intercept API calls and decode their JPEGs to verify dimensions, +colors, MIME type, and single/batch request structure. CPU tracker tests verify +normalization, temporary image cleanup, and mask conversion; model construction +is substituted, so no weights or inference are exercised. The offline +`demo_vl_images.py` passed. Mypy passed for person-follow and the three VLM modules. + +Local verification installed OpenAI 2.21.0, Hydra 1.3.2, and CPU Torch 2.7.1. +Person-follow imports successfully. Actual tracking inference, simulation camera +producers, and the full person-follow lifecycle remain unverified and unfinished. + +### Generated person-follow thread lifecycle + +Two standalone tests pass for the real control thread with deterministic tracker +output: successful detection produces motion then a final zero command, and +immediate tracking loss produces only zero commands. Both join the thread within +two seconds and clean up the module. Commands are decoded from CDR before +assertion. `demo_person_follow.py` passed the visible motion/stop scenario. + +The tests use `--noconftest` because the unrelated agent conftest requires missing +LangGraph. Model creation and tracker inference are substituted; neither actual +weights nor robot actuation are verified. Inspection confirms MuJoCo and DimSim +camera producers already use generated CameraInfo and Image; no producer change +was needed for this increment. Simulation execution remains a separate gate. +The standalone control-thread demo does not enter an MCP skill-call context; +its final tool-progress update logs an unknown-tool warning. MCP tool-stream +lifecycle is not part of this check. + +### Generated 3D bbox and marker messages + +3D bbox, marker, and array conversions now emit generated vision messages and +poses. Image headers supply exact timestamps. Marker hypotheses are assigned back +to the generated result sequence after editing, preserving dictionary-qualified +identity. Six converted existing/extended tests passed for bbox geometry, marker +identity after CDR, array behavior, annotation, exact stamps, and copy isolation. +Mypy passed for three production modules. `demo_marker_messages.py` passed its +visible two-marker CDR check. Upstream fiducial producers still need conversion; +these checks do not establish the full marker detection pipeline. + +### Generated fiducial frame detection + +Marker calibration, image conversion, Rodrigues pose conversion, and world-pose +composition now use generated messages and shared helpers. The stamped transform +constructor accepts a Header directly, retaining integer timestamps. Ten combined +marker geometry, actual rendered-marker detection, and wire/array checks passed. +Mypy passed for both production modules. The ArUco demo detected DICT_4X4_50:7 at +world x=2.000 m, z=0.402 m, retained the exact image stamp, and wrote an annotated +PNG; the rendered output was inspected. Fiducial stream modules/transformer and +hardware-camera integration remain to be converted or verified. + +### Generated fiducial observation stream + +DetectMarkers, MarkersPerFrame, and MarkerDetectionStreamModule now use generated +images, camera calibration, transforms, and arrays. The live input path preserves +zero image stamps instead of replacing them with wall-clock time. Shared SpeedLimit +now computes distance and angular separation with generated geometry helpers. + +Nineteen marker/stream checks passed, including intrinsics refresh during +smoothing, grouping/empty frames, quality and speed gating, quaternion sign +invariance, and zero timestamp lookup. Mypy passed for four changed production +modules. The ArUco demo now also runs image observation → marker observation → +CDR array and retains the exact image stamp. Shared fiducial test helpers were +converted; the separate MarkerTfModule tests and producer still need conversion. + +### Generated marker TF publication and desk blueprint + +MarkerTfModule publishes generated TFMessage/TransformStamped values directly +from generated detections, retaining exact array timestamps. The desk blueprint +uses standalone calibration loading and generated static transforms. Converted +fixture verification renders a twelve-marker board and checks its published TF +chain. All **38 fiducial tests passed**, including the existing LCM input +integration, namespace/identity cases, empty arrays, static TF thread, calibration +loading, and generated stream checks (`build/message-codegen/fiducial-tests.log`). +Mypy passed for the TF publisher and desk blueprint. No legacy message imports or +LCM codec calls remain in fiducial Python files. The cumulative ArUco demo now +prints CDR TF edges world → markers → marker_7 and passed. Physical camera +acquisition and actual robot use were not exercised. ReportLab 4.5.0 was installed +locally to run the existing generated-board tests. + +### Generated bbox navigation + +BBoxNavigationModule now consumes generated CameraInfo/Detection2DArray and +publishes generated PoseStamped with the exact detection header. Its existing +forward/left/up projection convention is unchanged. One integration test passed +with real LCM input channels, CDR output decoding, calibration gating, empty +arrays, and exact timestamps. Mypy passed for the production module. +`demo_bbox_navigation.py` passed and printed the expected projected goal. This +is a message/projection check, not physical navigation or TF-frame validation. + +Recent holonomic, MLS, ray-tracing, and evaluation results are in +[EVIDENCE-navigation.md](EVIDENCE-navigation.md). + + +### Offline replay, PGO cloud inputs, recorder and metadata consumers + +The generated-message batch converts B1 odometry forwarding, M20 video/camera/TF +metadata, offline replay and marker rendering, map reconstruction/CDR export, +pose-fill mounts, and PGO point-cloud input/submap/ICP boundaries. Existing +GTSAM and Open3D numerical algorithms remain; their internal numeric transforms +still require retirement review. CollectionRecorder now records generated image +and joint-state values and preserves the timestamp of application EpisodeStatus +messages instead of substituting wall-clock time. ObjectDB cleanup and convex +hull extraction use generated point buffers. Moondream fixtures use real bbox +values so generated Detection2DArray construction exercises the actual conversion. + +The local synthetic regression batch passed **51 tests**, covering geometry, +pose-fill rotation/unmatched frames, PGO body-to-world submap placement and merge, +CLI import cost, B1 forwarding, M20 metadata, and Rerun camera/cloud modes. Scoped +pre-commit hooks passed. Local perception/collection test collection is blocked +by missing hydra, torch and h5py; no extra model or dataset was downloaded. Native +GTSAM/Open3D full-pipeline validation remains dependent on CI. + +Before this batch, exact published commit +42c3f4411a4d19135952dc22df8cdee927dff235 passed message-codegen standalone and +Jazzy-reference jobs, C++/Rust native builds, Rust and Web CI. Main CI still failed: +lint reported 34 errors in 12 files; Python 3.12 reported 6267 passed, 82 skipped, +3 failed and 3 errors. Other matrix jobs were cancelled by fail-fast. The fixes +above address a subset of those failures; these previous results do not prove +this batch or the entire migration complete. + +WebXR control-path edits and full memory-document execution remain paused after +automatic approval rejection. Eight pending files are excluded from this batch. +No hardware, network configuration, or live-control acceptance test was performed. + + +### Generated relocalization and offline memory viewers + +The next batch completes runtime CDR premap loading (`.pc2.cdr`), generated TF +publication/placement, lidar scan-window merging, and Go2 premap/live-map merging. +Matching thresholds and native algorithms remain unchanged. Offline tuning and +map viewing now use generated point-cloud APIs. The existing synthetic map-view +case moved out of the self-hosted asset suite and writes an RRD in ordinary tests. +Go2 DDS offline odometry/IMU integration and path/camera/cloud rendering use +nested generated values; measured extrinsics retain the previous SciPy +normalization of the six-decimal rotation. + +Memory-store Rerun output dispatches through generated-message helpers outside +message classes and keeps explicit application adapters. Camera pairing and +first-TF timing are preserved. Space camera rendering and near queries now use +generated calibration/image/nested pose fields. Query radius boundary behavior +is unchanged, and prior path/image/cloud snapshots remain unmodified. + +**55 focused checks passed** in the batch: 29 relocalization/DDS/map-view/CLI and +camera/cloud bridge tests, 12 memory-render tests, 8 generated near-query cases, +2 actual Rerun scene/camera tests, and 4 PGO/codebase-constructor checks. The pure +memory tests were run with scoped confcutdir to avoid unrelated parent CLIP +fixtures; they do not establish full model-dependent memory behavior. Two SVG +cases remain locally blocked by missing numba. Strict scoped mypy passed for +15 production files. Native mesh/perception fixtures still require CI because +Open3D/hydra are absent locally. + +Previous exact published commit 31800fc679ecd5e904ca860ca49bd1cab511d191 passed +message-codegen, Native C++/Rust, Rust and Web CI. Main CI failed: lint had 5 errors +in 2 files; ARM had 6057 passed, 229 skipped and 3 failed. Other Python lanes were +cancelled by fail-fast. This batch repairs those five consumer type errors and +three identified fixture failures; the new exact-commit CI remains required. +md-babel still fails on old memory documentation codecs; the paused eight files +are excluded again. Historical `.pc2.lcm` map assets must be regenerated or +explicitly migrated before using the CDR loader; none was downloaded or rewritten. +Final legacy API/dependency retirement and viewer UI acceptance remain open. + +For the subsequent compressed-image/runtime adapter batch, see +[evidence/compressed-image-runtime.md](evidence/compressed-image-runtime.md). + +Recorder/graph boundaries and the isolated Python example are covered in +[evidence/recorder-graph-runtime.md](evidence/recorder-graph-runtime.md). + +[Unitree/Spot/camera runtime evidence](evidence/unitree-spot-camera-runtime.md) +covers the next consumer batch. + +[Camera/export, PGO, approved WebXR and memory documentation evidence](evidence/camera-export-runtime.md) +covers the subsequent offline consumer batch. + +[Hosted arm, robot and simulation consumer evidence](evidence/robot-simulation-consumers.md) +covers the next offline migration batch and its remaining gates. + +[Geometry, simulation, eval and experimental consumer evidence](evidence/experimental-consumers.md) +records terminal CI for the previous batch and standalone Python 3.9 encoding. + +## Remaining consumer cutover + +See [final-consumer-cutover.md](evidence/final-consumer-cutover.md) for generated +entity/control/PGO/eval migration, exact previous CI failures, offline evidence +and remaining legacy retirement boundaries. diff --git a/examples/message-codegen/README.md b/examples/message-codegen/README.md index dae3f433fc3..1faa8e95315 100644 --- a/examples/message-codegen/README.md +++ b/examples/message-codegen/README.md @@ -5,6 +5,9 @@ reading, a position, defaults, fixed and variable arrays, and a bounded string. This example generates Python, C++, and Rust from that definition without ROS. The native processes print the fields they receive and each adds its own hop. +See [Add and use a message](/docs/development/messages.md) for the new-type and +Python/C++/Rust user stories, including a bounded runnable local example. + ## Build dependencies Use Python 3.12+, a C++17 compiler, CMake 3.20+, and Rust 1.92+. Install Python @@ -223,3 +226,1098 @@ Stop the Rerun process with Ctrl-C and close its browser tab. The script starts no persistent services. Remove `build/message-codegen/viewers/` to clean up recordings and `examples/message-codegen/viewer-checker/node_modules/` to clean up the Node dependencies. + +## Runtime cutover foundations + +The generated set also includes the in-tree `dimos_msgs` definitions. To inspect +weighted segments and trajectory duration fields without a running robot: + +```bash +PYTHONPATH=.:build/message-codegen/demo/cpp/build \ + .venv/bin/python examples/message-codegen/demo_message_helpers.py +``` + +The source timestamp survives CDR at full nanosecond precision. Segment endpoints +and weights are explicit fields, and controller-facing floating seconds are an +explicit helper conversion from ROS Duration. The generated classes carry data +and codecs; helpers do not add methods or compatibility properties to them. +This is preparation for stage 4, whose runtime consumer cutover is still in +progress. + +The typed transport demo exercises DimOS's actual transport classes. It sends a +custom weighted-segment message and a 921,600-byte RGB image through LCM, two +separate Zenoh sessions connected by loopback TCP, and CPU shared memory: + +```bash +PYTHONPATH=.:build/message-codegen/demo/cpp/build \ + .venv/bin/python examples/message-codegen/demo_pubsub.py +``` + +Use `--backend lcm`, `--backend zenoh`, or `--backend shm` to select one path. +The demo chooses temporary ports and topics and closes its sessions, sockets, +threads, and shared-memory handles on exit. It prints the received custom field, +pixel count, and exact source timestamp. This is a transport demonstration; +the complete three-language blueprint and live viewer cutover remain stage-4 +acceptance work. LCM limits its physical channel name, including the type suffix, +to 63 UTF-8 bytes; overlong names fail explicitly before sending. + +### Browser schema decoding during the runtime cutover + +After building the generated Python module, regenerate the shared vectors: + +```bash +PYTHONPATH=.:build/message-codegen/demo/cpp/build .venv/bin/python -m dimos.web.relay_bridge.gen_cdr_fixtures +cd web +deno install +cd sdk +deno task fixture --host 127.0.0.1 +``` + +Open `http://127.0.0.1:5174/demo_cdr.html`. Select `custom_segments` to inspect the +nested endpoints, weight, and exact timestamp. Select `image` to see the decoded +pixels, and switch byte order to verify both generated encodings. Each selection +uses the SDK's actual decoder registry and the complete schema from its manifest +record. The viewer has no generated message bindings. Stop Vite with Ctrl-C. +This is a fixture-based browser demonstration; live three-language blueprint +integration remains a separate acceptance gate. + +Automated checks for this part of the cutover: + +```bash +PYTHONPATH=.:build/message-codegen/demo/cpp/build .venv/bin/pytest \ + dimos/core/test_cdr_transport.py \ + dimos/protocol/pubsub/impl/webrtc/test_transport.py \ + dimos/web/test_cdr_codec.py dimos/web/test_codecs.py dimos/web/test_cockpit.py \ + dimos/web/relay_bridge/test_map_codecs.py \ + dimos/web/relay_bridge/test_costmap_encoding.py \ + dimos/msgs/test_generated_views.py -o addopts='' -q +cd web/sdk +deno task test +deno task check +cd ../cockpit +deno task test +deno task check +``` + +Generated messages keep their ROS fields; conversion operations live in helpers. +For example, image arrays require an explicit encoding and header: + +```python +from dimos.msgs.image import image_from_array, image_view +from dimos.msgs.time import header_now + +message = image_from_array(pixels, encoding="rgb8", header=header_now("camera")) +borrowed = image_view(message) # read-only, retains its owner; honors row padding and byte order +editable = borrowed.copy() # an explicit independent mutable copy +``` + +`image_from_array` copies the supplied pixels and validates their dtype and shape. +It accepts non-contiguous inputs. `occupancy_view` provides the corresponding +read-only grid view using `info.width` and `info.height`. Pose consumers access +`message.pose.position`; planar heading is `yaw(message.pose.orientation)` from +`dimos.msgs.geometry`. Creating a zero velocity command is simply generated +`Twist()`, including in watchdog and stop paths. + +### Native SDK relay during the runtime cutover + +With the C++ SDK's LCM, Zenoh C/C++, JSON, and PFR build dependencies installed, +build the generated package and native relays: + +```bash +bash scripts/setup_message_codegen.sh +bash scripts/install_native_messages.sh +cmake -S native/cpp -B build/native-cpp -DDIMOS_NATIVE_BUILD_TESTS=ON \ + -DCMAKE_PREFIX_PATH="$PWD/build/message-codegen/install" +cmake --build build/native-cpp -j 2 +ctest --test-dir build/native-cpp --output-on-failure +cmake -S examples/native-modules/cpp -B build/native-cpp-examples \ + -DCMAKE_PREFIX_PATH="$PWD/build/message-codegen/install" +cmake --build build/native-cpp-examples -j 2 +cargo build -p dimos-native-module-examples +cargo test -p dimos-module --lib +PYTHONPATH=.:build/message-codegen/demo/cpp/build \ + .venv/bin/python examples/message-codegen/demo_native.py +``` + +The generated Python extension comes from the initial generation/build demo. +The `native` job in `.github/workflows/ci.yml` lists the pinned Zenoh archives +and system dependencies. Add their installation prefix to `CMAKE_PREFIX_PATH` +and their pkg-config directory to `PKG_CONFIG_PATH` when installed locally. + +The demo starts actual SDK subprocesses, supplies their stdin launch configuration, +and sends a custom `LineSegments3D` and a 640×480 RGB image through +Python → C++ → Rust → Python, first over LCM and then over loopback Zenoh TCP. +The segment weight changes from 4 to 5 to 6. The full image and exact source +nanoseconds must survive both relays. LCM fragments the image along each hop. +Use `--backend lcm` or `--backend zenoh` for one transport. Temporary topics and +ports isolate runs; subprocesses and sessions close automatically, including on +failure. Native logs go to `build/message-codegen/demo/evidence/native-*.log`. +This exercises the SDK launch protocol; the full coordinator blueprint and live +Rerun integration are still separate acceptance work. + +The in-tree Rust message dependency generates only inside Cargo's `OUT_DIR`. +Its source package bundles the parser, generator, schemas, and licenses. Cargo +requires Python 3.10+ during the build (`DIMOS_CODEGEN_PYTHON` can select it); +the resulting Rust binaries need neither Python nor ROS. Verify the actual source +archive with `cargo package -p dimos-generated-messages --allow-dirty --offline`. + +### Point-cloud layouts + +```bash +PYTHONPATH=.:build/message-codegen/demo/cpp/build \ + .venv/bin/python examples/message-codegen/demo_pointcloud.py +``` + +This displays an organized cloud with field padding, row padding, and a two-value +`tags` field, in both byte orders. The helper borrows a read-only structured view; +`pointcloud_xyz` explicitly copies XYZ coordinates. The demo edits a separate +copy and checks that the borrowed source remains unchanged. No processes or +viewer windows remain after it exits. + +### C++ lidar message construction + +The native C++ test build also produces a hardware-free demonstration of the +production lidar cloud builder: + +```bash +cmake --build build/native-cpp -j 2 +build/native-cpp/tests/dimos_lidar_cdr_demo +``` + +It prints the decoded XYZI point count, frame, timestamp, and intensity for both +CDR byte orders. It also verifies negative timestamp normalization and rejects +invalid timestamps and negative cloud sizes. The executable exits on its own. + +### External messages through native SDK ports + +After the external packaging demo and the native SDK dependency setup above: + +```bash +bash scripts/test_message_packages.sh +bash scripts/test_external_native_messages.sh +``` + +The first command adds `application_note` to a local copy of `Telemetry.msg` and +builds separate Python/CMake/Cargo packages. The second builds C++ and Rust SDK +modules against those packages and exchanges the extended message through +Python → C++ → Rust → Python over LCM and Zenoh. The displayed note becomes +`added-locally/cpp-native/rust-native`; every other field and the source timestamp +must remain unchanged. It uses the generated Rust `Telemetry::encode` and +`Telemetry::decode` directly in port declarations. No SDK-specific codec adapter +or change to the built-in definition registry is needed. + +Generated Rust codec entry points return `std::io::Result`: invalid values at +encode time report `InvalidInput`, while malformed CDR reports `InvalidData`. +The external consumer test checks both errors against the SDK's function types. +Subprocesses and sessions close on exit. Logs and visible results are under +`build/message-codegen/external-native/evidence`. + +### Python/Rust TF through the module coordinator + +Build the Rust examples, then run the TF blueprint using either backend: + +```bash +cargo build --release -p dimos-native-module-examples +DIMOS_TRANSPORT=zenoh PYTHONPATH=.:build/message-codegen/demo/cpp/build \ + .venv/bin/python examples/native-modules/rust_tf.py +# Repeat with DIMOS_TRANSPORT=lcm on a host configured for multicast. +``` + +The Python worker publishes `a → b → c` using generated `TransformStamped` +messages, and the Rust broadcaster publishes `c → d`. The Rust listener prints +`x=1.5` and changing `y=cos(t), z=sin(t)` for the composed `a → d` transform. +The pinned ROS quaternion definition defaults `w=1`; generated types preserve +that identity rotation, and this example also sets it explicitly. Stop with Ctrl-C; the coordinator +stops both native processes and its Python workers. + +The automated check runs the same blueprint, verifies four changing composed +transforms per backend, and checks clean shutdown. It requires the normal DimOS +runtime dependencies (including Open3D). Pytest disables interactive host system +configuration, and the LCM run reserves a temporary local UDP port: + +```bash +PYTHONPATH=.:build/message-codegen/demo/cpp/build .venv/bin/pytest \ + examples/native-modules/test_rust_tf.py -m native_e2e -o addopts='' -v +``` + +### Three-language module coordinator blueprint + +After building the native relays above, run the complete worker-based graph: + +```bash +PYTHONPATH=.:build/message-codegen/demo/cpp/build \ + .venv/bin/python examples/message-codegen/demo_blueprint.py --transport zenoh +# On a multicast-configured host: +PYTHONPATH=.:build/message-codegen/demo/cpp/build \ + .venv/bin/python examples/message-codegen/demo_blueprint.py --transport lcm +``` + +```text +Python worker → C++ native worker → Rust native worker → Python worker + weight n n + 1 n + 2 validate fields + RGB image ──────────────────────────────────────── validate all pixels +``` + +Each of five samples changes the custom weight, endpoint, image pixels, and exact +source timestamp. The final Python worker validates the full encoded messages; +the terminal reports each result. `--samples N` changes the sample count. Native +processes and workers stop automatically after the finite run, including failures. + +The Zenoh demo starts a temporary loopback router and connects every worker to +its explicit endpoint, with multicast discovery disabled. The topics and module +names have a unique namespace. The LCM run uses `LCM_DEFAULT_URL`, so set it to an +isolated multicast port when other local blueprints are running. As with the TF +demo, normal DimOS runtime dependencies are required. Automated checks choose a +temporary LCM port and run the same command on each transport: + +```bash +PYTHONPATH=.:build/message-codegen/demo/cpp/build .venv/bin/pytest \ + examples/message-codegen/test_blueprint.py -m native_e2e -o addopts='' -v +``` + +This finite demo exercises coordinator deployment, remapping, native startup, +and generated streams. Live Rerun/browser integration remains a later addition. + +### Generated TF values and geometry helpers + +The live TF buffer and recorded-stream lookup return generated +`geometry_msgs/msg/TransformStamped` values. Read the parent frame and timestamp +from `value.header`, the child from `value.child_frame_id`, and the translation +and quaternion from `value.transform`. The separate helpers in +`dimos.msgs.geometry` compose, invert, or convert those values: + +```python +from dimos.msgs.geometry import compose_transforms, inverse_transform, pose_from_transform + +world_to_tool = compose_transforms(world_to_arm, arm_to_tool) +tool_to_world = inverse_transform(world_to_tool) +world_pose = pose_from_transform(world_to_tool) +``` + +Composition requires matching intermediate frame names and preserves the first +edge's exact source stamp. Inversion swaps frames and preserves the stamp. The +TF time index uses integer nanoseconds and owns snapshots of received transforms; +mutating a received value or a lookup result cannot change buffered history. +Float-second lookup arguments remain for existing timing APIs. + +The Python/Rust TF demo above now logs the composed transform from both Python +and Rust. Its automated check verifies both results. Robot-specific publishers +and downstream consumers are being converted to these fields as part of the +remaining runtime cutover. + +The TF demo's fixed `b → c` edge is published by the production +`StaticTfPublisher` in its own worker. This exercises periodic generated +`TFMessage` publication alongside the dynamic Python and Rust edges. Static mount +implementations return `list[TransformStamped]`; `frames_to_edge_transforms` +constructs them from `(child, parent, xyz, fixed-axis-rpy)` entries. A `None` parent +marks the root and produces no edge. The publisher supplies current integer +nanosecond stamps on every cycle. + +### Camera calibration and webcam messages + +`dimos.msgs.camera_info` constructs generated `CameraInfo` values from pinhole +intrinsics, field of view, or a ROS calibration YAML file. Construction requires +an explicit `Header`; message defaults do not invent a capture time. YAML loading +checks the matrix dimensions and finite values. The generated fields are lowercase +`k`, `d`, `r`, and `p`. `intrinsic_matrix(info)` returns an independent mutable +NumPy copy. + +The existing helper demo now loads the packaged Go2 calibration, encodes and +decodes it with CDR, and prints its dimensions, distortion model, matrix, +coefficients, and exact source timestamp: + +```bash +PYTHONPATH=.:build/message-codegen/demo/cpp/build \ + .venv/bin/python examples/message-codegen/demo_message_helpers.py +``` + +The webcam path emits generated RGB images; stereo slicing happens on pixels +before message construction. Camera metadata and both TF edges share one exact +stamp without mutating the configured calibration or mount. Hardware-free tests +feed known BGR pixels into the capture boundary and verify the decoded RGB crops. + +### Simulation lidar shared memory (no engine required) + +Run the actual simulation lidar producer and consumer in separate processes: + +```bash +PYTHONPATH=.:build/message-codegen/demo/cpp/build \ + .venv/bin/python examples/message-codegen/demo_mujoco_lidar_shm.py +``` + +The demo prints decoded XYZ coordinates, frame `world`, and the exact timestamp +`1700000000123456789`. It writes generated PointCloud2 CDR bytes through the +production shared-memory buffer and cleans up its regions on exit. It does not +launch MuJoCo or require robot hardware. Live simulation and Go2 pose/TF consumers +remain separate acceptance checks. + +### Robot pose and TF conversion (no hardware required) + +```bash +PYTHONPATH=.:build/message-codegen/demo/cpp/build \ + .venv/bin/python examples/message-codegen/demo_robot_tf.py +``` + +This runs the WebRTC device-pose converter and Go2's TF-chain builder on a sample +packet, round-trips generated TFMessage through CDR, and prints the three edges +and their composed camera frame. The robot-local frames receive `robot0/`; the +source parent stays `odom`. Timestamp `1746565669448350564` survives unchanged. +The live WebRTC connection deliberately supplies a host-arrival header instead, +matching its previous timing policy with integer-nanosecond precision. + +### Generated-message storage and module replay + +```bash +PYTHONPATH=.:build/message-codegen/demo/cpp/build \ + .venv/bin/python examples/message-codegen/demo_storage_replay.py +``` + +The demo records custom weighted segments to SQLite and a ROS2-profile MCAP, +reopens both by their stored type/schema, and replays SQLite through a real +module's `Out` port. It prints weights `4, 5, 6` and consecutive source nanoseconds +starting at `1700000000123456789`. Temporary recordings are removed automatically. +This checks Python storage and replay. + +### Record an external custom message with the Rust recorder + +First complete the external-package demo above, including its locally added +`application_note` field. Build the recorder once, then record that external type +over both transports using the same executable: + +```bash +cargo build -p dimos-memory-recorder --locked +PYTHONPATH=.:build/message-codegen/demo/cpp/build:build/message-codegen/external-app/venv/lib/python3.12/site-packages \ + .venv/bin/python examples/message-codegen/demo_native_recording.py +``` + +Adjust the external virtualenv's Python version in the path when necessary. +The demo prints `locally-added-field-0` through `locally-added-field-2` and exact +payload stamps on LCM and Zenoh. It stops each recorder and closes its transport +resources, retaining `external-native-recording-{lcm,zenoh}.mcap` and `.log` files +under `build/message-codegen/demo/evidence/` for inspection. Use `--output` to +choose another artifact directory and `--executable` for a packaged recorder. + +With the viewer-checker dependencies installed as described above, independently +decode both recordings from their embedded schemas: + +```bash +node examples/message-codegen/viewer-checker/check-native-recording.mjs \ + build/message-codegen/demo/evidence/external-native-recording-lcm.mcap +node examples/message-codegen/viewer-checker/check-native-recording.mjs \ + build/message-codegen/demo/evidence/external-native-recording-zenoh.mcap +``` + +The Foxglove decoder requires no custom message package. Arbitrary custom payloads +are preserved byte-for-byte; their MCAP publish time uses reception time because +the recorder has no compiled knowledge of their timestamp layout. Recognized +standard stamped messages use exact source nanoseconds. Delete the four retained +MCAP/log files when finished reviewing them. These terminal checks complement +the separate Foxglove and Rerun UI acceptance demos. + +### Optional ROS2 bridge (isolated verification) + +DimOS generation and runtime remain ROS-free. To check the existing optional +bridge against ROS2 Jazzy after building the standalone Python binding, use the +independent reference container: + +```bash +docker build -f .github/docker/message-codegen-reference.Dockerfile \ + -t dimos-cdr-jazzy-reference .github/docker +docker run --rm -v "$PWD:/source:ro" dimos-cdr-jazzy-reference ' + set -e + source /opt/ros/jazzy/setup.bash + export PYTHONPATH=/source:/source/build/message-codegen/demo/cpp/build:$PYTHONPATH + cd /source + python3 -m pytest -o addopts="" --noconftest --import-mode=importlib -p no:cacheprovider \ + dimos/protocol/pubsub/impl/test_rospubsub_conversion.py \ + dimos/protocol/pubsub/impl/test_rospubsub.py -q + python3 examples/message-codegen/demo_ros_bridge.py +' +``` + +The demo prints three poses (`x=0, 1, 2`) and RGB pixels (`[0,128,255]`, +`[1,128,254]`, `[2,128,253]`) received through real ROS publishers/subscribers. +Source nanoseconds `123456789` through `123456791` survive unchanged. Nodes, +timers, subscriptions, and the container are cleaned up on exit. With ROS +already installed and sourced, run the same Python demo directly. + +The bridge uses ROS's serialization/type support and generated DimOS CDR codecs. +It requires matching ROS message packages only in the bridge environment; it +does not translate through LCM, rename fields, or load legacy message classes. +The independent Jazzy CI job runs these checks using the binding produced by +the preceding ROS-free build. + +### Galaxea feedback without hardware or ROS + +```bash +PYTHONPATH=.:build/message-codegen/demo/cpp/build \ + .venv/bin/python examples/message-codegen/demo_galaxea_messages.py +``` + +This feeds synthetic chassis-speed samples into the actual Galaxea connection +callback and subscribes to its generated pose, odometry, and TF output streams. +It prints three changing positions and the `odom -> base_link -> lidar_chassis_left_link` +chain, with exact source nanoseconds on every edge. It requires neither ROS nor +a robot; it cleans up subscriptions and the module on exit. The neighboring +`test_connection_messages.py` also checks joint-state aggregation, command array +splitting, tracking-speed sentinels, and malformed-command rejection. + +### Whole-body transport feedback and commands + +```bash +PYTHONPATH=.:build/message-codegen/demo/cpp/build \ + .venv/bin/python examples/message-codegen/demo_whole_body_transport.py +``` + +This connects the actual whole-body adapter to synthetic robot-side publishers +and a command subscriber, first over LCM and then through two explicit loopback +Zenoh sessions. It prints feedback positions `[0.25, -0.5]`, commanded positions +`[0.75, -0.25]`, and the hardware-facing WXYZ quaternion. The demo checks command +gains/torques and generated headers, then closes transports, subscriptions, and +sessions. `test_whole_body_transport.py` runs the same exchange automatically. + +### G1 whole-body feedback and coordinator joint streams + +```bash +PYTHONPATH=.:build/message-codegen/demo/cpp/build \ + .venv/bin/python examples/message-codegen/demo_g1_messages.py +PYTHONPATH=.:build/message-codegen/demo/cpp/build \ + .venv/bin/python examples/message-codegen/demo_coordinator_joints.py +``` + +The G1 demo feeds three synthetic 29-motor snapshots through the actual output +ports. It prints changing joint positions, the converted ROS XYZW orientation, +and consecutive exact source nanoseconds shared by joint and IMU messages. It +does not initialize the Unitree SDK or connect to a robot. + +The coordinator demo runs two mock arms and prints aggregate and per-arm joint +streams, verifying names, positions, ROS header frames, CDR round trips, and one +shared tick stamp. Its existing internal snapshot clock remains in floating +seconds and is explicitly converted at the message boundary. Both demos close +subscriptions and stop their modules on exit. + +### Base-motion transport commands, odometry, and stop + +```bash +PYTHONPATH=.:build/message-codegen/demo/cpp/build \ + .venv/bin/python examples/message-codegen/demo_base_transport.py +``` + +The actual base adapter receives generated PoseStamped odometry over LCM and +then independent loopback Zenoh sessions. It prints planar position `(2, -1)` +and yaw `pi/2`, sends velocity `(0.5, -0.2, 0.3)` after enabling, then verifies +a generated zero Twist arrives when disabled. It closes the adapter and all +transport resources. `test_base_transport.py` runs this exchange automatically. + +### Generated joint feedback in manipulation planning + +```bash +PYTHONPATH=.:build/message-codegen/demo/cpp/build \ + .venv/bin/python examples/message-codegen/demo_planning_joints.py +``` + +This decodes a generated coordinator JointState, selects the left-arm joints in +canonical order, normalizes a differently ordered target, and validates both +against declared joint limits. It prints selected positions `[-0.2, 0.3]`, target +`[0.1, 0.5]`, and the preserved source stamp `1700000000123456789`. No robot, +planning backend, ROS installation, or persistent artifact is required. + + +### Planner backends and robot self exclusion + +```bash +PYTHONPATH=.:build/message-codegen/demo/cpp/build \ + .venv/bin/python examples/message-codegen/demo_planning_backends.py +PYTHONPATH=.:build/message-codegen/demo/cpp/build \ + .venv/bin/python examples/message-codegen/demo_self_filter.py +``` + +The backend demo requires the manipulation dependencies (including Drake). It +creates a temporary two-joint URDF, plans between generated JointState values, +interpolates five waypoints, and checks every CDR-decoded waypoint against the +real Drake world. The printed shoulder positions run from -0.4 through 0 to 0.4. +This is an unobstructed joint-path demo; it does not exercise obstacle avoidance +or the still-pending generated trajectory/pose cutover. + +The self-filter demo requires yourdfpy. It moves a modeled cube through x=0, 1, +and 2, showing that the cube's return disappears while a distant point remains. +Both the retained cloud and the world-frame clear mask round-trip through CDR +with their original integer nanosecond timestamps. The demos remove temporary +models and dispose module resources on exit; neither needs robot hardware or ROS. + + +### Generated trajectories and execution status + +```bash +PYTHONPATH=.:build/message-codegen/demo/cpp/build \ + .venv/bin/python examples/message-codegen/demo_trajectory.py +``` + +The trapezoid generator produces 51 ROS2 JointTrajectoryPoint values for two +joints. The demo sends the generated JointTrajectory over LCM and then over +independent loopback Zenoh sessions, preserving source nanoseconds +`1700000000123456789`. A real JointTrajectoryTask executes it against synthetic +feedback; printed commands progress from `[0, 0]` to `[0.3, -0.2]`. Generated +`dimos_msgs/msg/TrajectoryStatus` crosses the return channel and reports +`COMPLETED`, progress 1, and zero remaining duration. Each transport and session +closes on exit. `test_trajectory.py` runs the exchange as a subprocess test. + +Import trajectories from `dimos_generated.trajectory_msgs.msg`; waypoint times +use `builtin_interfaces/Duration`. Use `duration_from_seconds` and `to_seconds` +from `dimos.msgs.time` at floating-time boundaries. `sample_trajectory` and +`trajectory_duration` in `dimos.msgs.trajectory` provide interpolation and total +time outside the generated message classes. Absent velocities mean zero +feed-forward for joint-position execution. Generated status state fields are +integers with constants on TrajectoryStatus; compare them with `==`. + +The old handwritten JointTrajectory, TrajectoryPoint, and TrajectoryStatus +classes are removed. Generated sequences return value copies for message +entries: edit a point and assign it back with `trajectory.points[index] = point` +when changing a waypoint. + +### Generated poses and grasp proposals + +```bash +PYTHONPATH=.:build/message-codegen/demo/cpp/build \ + .venv/bin/python examples/message-codegen/demo_planning_backends.py +PYTHONPATH=.:build/message-codegen/demo/cpp/build \ + .venv/bin/python examples/message-codegen/demo_grasp_proposals.py +``` + +The Drake demo now prints each waypoint's forward-kinematics tool position and +its CDR TF edge. Derived poses retain the joint sample's exact timestamp; the +first printed stamp is `1700000000.123456789`. The synthetic URDF is removed +when the demo exits. + +The grasp demo needs neither hardware nor a GPU. Four generated PointCloud2 +points cross a CDR round trip into the heuristic provider. The generated +`dimos_msgs/msg/GraspCandidateArray` crosses another round trip and prints a +ranked grasp at `[0.5, 0.0, 0.15]`, a downward approach axis, score `1.0`, and the +unchanged source timestamp `1700000000123456789` nanoseconds. The provider stops +on exit. + +Use nested ROS fields (`stamped.pose.position`, `stamped.header.stamp`) and the +functions in `dimos.msgs.geometry` for pose matrices, Euler angles, and local +translation. A pose-matrix conversion rejects non-finite or non-rigid geometry. +Generated configuration poses keep a zero timestamp unless a source supplies +one. Access grasp entries through `.candidates`; the former handwritten pickle +codecs have been removed. Grasp consumers validate scores and geometry before +commanding motion. + +For base motion, run: + +```bash +PYTHONPATH=.:build/message-codegen/demo/cpp/build \ + .venv/bin/python examples/message-codegen/demo_path_following.py +``` + +This demo decodes a generated 41-waypoint Path, runs the path follower against +DimOS's base simulator, and round-trips each Twist command through CDR. It prints +`arrived` and writes `build/message-codegen/demo/evidence/path-following.svg` +with the reference and executed paths. Remove that generated SVG to clean up. +Space's SVG and Rerun renderers now read generated poses, points, paths, detection +boxes, and occupancy grids directly. Both apply the occupancy origin rotation; +headless tests exercise the actual Rerun SDK as well as SVG output. + +### Generated cloud-to-map and inflation demo + +Run after building the generated Python messages above (the normal mapping environment +also requires NumPy, SciPy, Numba, and the visualization dependencies): + +```bash +PYTHONPATH=.:build/message-codegen/demo/cpp/build .venv/bin/python examples/message-codegen/demo_occupancy.py +``` + +The demo sends 1,200 synthetic points through CDR, projects them into an occupancy +map, inflates the obstacle by 0.2 m, and round-trips the generated grid through CDR. +It prints the occupied-cell counts and checks the exact source header. Open +`build/message-codegen/demo/evidence/occupancy.svg` to inspect the resulting map. +The demo needs no robot, ROS installation, viewer login, or running transport. +Remove that SVG to clean up its output. + +### Generated map → native/Python planner → path demo + +The normal native extension build provides A*. To build just that extension for +this demo, with the compiler and pybind11 already installed: + +```bash +c++ -O3 -shared -std=c++17 -fPIC \ + $(.venv/bin/python -m pybind11 --includes) \ + dimos/navigation/replanning_a_star/min_cost_astar_cpp.cpp \ + -o dimos/navigation/replanning_a_star/min_cost_astar_ext$(.venv/bin/python -c 'import sysconfig; print(sysconfig.get_config_var("EXT_SUFFIX"))') +PYTHONPATH=.:build/message-codegen/demo/cpp/build .venv/bin/python examples/message-codegen/demo_grid_path.py +``` + +Both planners route around the same synthetic wall using a CDR-decoded grid. +The demo resamples each result, sends it through generated Path CDR, checks exact +source headers and obstacle exclusion, and exports `grid-path-python.svg` and +`grid-path-cpp.svg` under `build/message-codegen/demo/evidence/`. Open the SVGs to +compare the routes. The native extension is required; the demo fails explicitly +if it is missing. Remove the two SVGs to clean up the demo output. + +### Run the navigation module over both transports + +After building generated messages and the native A* extension above: + +```bash +PYTHONPATH=.:build/message-codegen/demo/cpp/build .venv/bin/python examples/message-codegen/demo_navigation.py +``` + +This starts the real `CostMapper` and `ReplanningAStarPlanner` modules and a +synthetic drive loop. A generated CDR cloud is accumulated by the CPU voxel +mapper, then sent to CostMapper over LCM and separate loopback Zenoh sessions. +The resulting generated OccupancyGrid feeds navigation alongside generated +Odometry and PointStamped inputs. The drive loop consumes generated Twist +commands and sends new odometry until the module publishes a generated arrival +Bool. It checks the published Path's exact source header and the final stop +command. The simulation advances 0.1 seconds per control command at an accelerated +100 Hz wall-clock rate; it needs no robot or ROS installation. + +The terminal prints source/voxel counts, costmap dimensions, final position, +command count, and source timestamp. +Open `build/message-codegen/demo/evidence/navigation-lcm.svg` and +`navigation-zenoh.svg` to inspect the planned route. Module threads, subscriptions, +and transport sessions stop automatically; remove the two SVGs to clean up output. +`test_navigation.py` runs this same demo as a bounded subprocess E2E check. + +### Basic follower and exact odometry history + +```bash +PYTHONPATH=.:build/message-codegen/demo/cpp/build .venv/bin/python examples/message-codegen/demo_odometry_history.py +``` + +This hardware-free, fixed-step simulation passes a generated CDR path through the +basic follower's control step, applies its generated Twist commands, and checks +its generated arrival Bool. Each simulated Odometry sample passes through CDR +and the real OdometryHist handler. The demo checks consecutive nanosecond stamps +and true-height Rerun geometry, then exports the reference and traveled paths to +`build/message-codegen/demo/evidence/odometry-history.svg`. Open that file for +visual review; remove it to clean up the demo output. This exercises control +steps directly; it does not start a robot or the follower's timed control thread. + +### Generated frontier goals + +```bash +PYTHONPATH=.:build/message-codegen/demo/cpp/build .venv/bin/python examples/message-codegen/demo_frontiers.py +``` + +The demo decodes an occupancy map from CDR, detects the frontier between its +known and unknown halves, and round-trips the selected pose through CDR while +checking the exact source header. It prints the selected coordinates and writes +`build/message-codegen/demo/evidence/frontiers.svg`. Open that SVG to inspect +the selected goal; remove it to clean up. No robot, ROS installation, or viewer +login is required. + +### Generated patrol goals + +After the generation/build setup above, run: + +```bash +PYTHONPATH=.:build/message-codegen/demo/cpp/build .venv/bin/python examples/message-codegen/demo_patrol.py +``` + +The demo prints five goals each for random, coverage, and frontier patrol, and +writes `build/message-codegen/demo/evidence/patrol-{random,coverage,frontier}.svg`. +Each goal crosses a CDR encode/decode boundary and retains the map's exact header. +The red markers show selected goals; this demo does not simulate travel between +them. No robot or ROS installation is required. Remove those three SVG files to +clean up the demo outputs. + +### Generated movement arbitration + +```bash +PYTHONPATH=.:build/message-codegen/demo/cpp/build .venv/bin/python examples/message-codegen/demo_movement.py +``` + +This terminal demo forwards a stamped click through CDR, shows teleop overriding +navigation, then shows navigation resuming after cooldown. It uses in-process +module callbacks and CDR output round trips; it does not actuate a robot or +exercise network discovery. No output files require cleanup. + +### Generated visual-servo commands + +```bash +PYTHONPATH=.:build/message-codegen/demo/cpp/build .venv/bin/python examples/message-codegen/demo_visual_servo.py +``` + +This terminal demo decodes generated CameraInfo and prints CDR-decoded velocity +commands for four synthetic detections: far away, at target distance, too close, +and right of center. It needs no camera, robot, or ROS installation and writes no +files. It exercises the 2D controller, not the complete person-follow pipeline. + +### Generated detection-cloud filters + +```bash +PYTHONPATH=.:build/message-codegen/demo/cpp/build .venv/bin/python examples/message-codegen/demo_cloud_filters.py +``` + +This terminal demo filters a synthetic cloud containing a dense object and one +isolated low point. Height and radius filters each retain 100 of 101 points; +results cross CDR boundaries and preserve the source header. It requires Open3D +from the development environment, no sensor or ROS, and writes no files. + +### Generated depth-to-detection projection + +```bash +PYTHONPATH=.:build/message-codegen/demo/cpp/build .venv/bin/python examples/message-codegen/demo_detection_projection.py +``` + +The terminal demo projects sixteen depth pixels into a translated world frame, +then prints the generated CDR centroid and exact source timestamp. It uses +synthetic depth and needs no sensor, robot, or ROS installation. No files are +written. This exercises projection, not model inference or the person-follow +module lifecycle. + +### Generated image detection annotation + +```bash +PYTHONPATH=.:build/message-codegen/demo/cpp/build .venv/bin/python examples/message-codegen/demo_detection_bbox.py +``` + +Open `build/message-codegen/demo/evidence/detection-bbox.png` to inspect the +synthetic target's bounding box and label. The terminal prints CDR-decoded track +and class identifiers and checks the exact image header. Remove the PNG to clean +up. This uses synthetic pixels, with no model download or sensor required. + +### Generated vision-model image boundary + +```bash +PYTHONPATH=.:build/message-codegen/demo/cpp/build .venv/bin/python examples/message-codegen/demo_vl_images.py +``` + +This offline terminal demo resizes a generated image and converts a fixed model +response into a CDR detection array. It prints the dimensions and verifies the +source header. It does not call an API or run model inference, and writes no files. + +### Person-follow control thread with generated messages + +```bash +PYTHONPATH=.:build/message-codegen/demo/cpp/build .venv/bin/python examples/message-codegen/demo_person_follow.py +``` + +This hardware-free demo starts the actual person-follow control thread with +fixed tracker output. It prints a CDR-decoded forward command, requests stopping, +checks the final zero command, and joins the thread. Model creation and tracker +inference are substituted; this is not a model-accuracy or robot demo. It needs +the optional Torch/Hydra imports but downloads no weights and writes no files. + +### Generated 3D marker messages + +```bash +PYTHONPATH=.:build/message-codegen/demo/cpp/build .venv/bin/python examples/message-codegen/demo_marker_messages.py +``` + +This terminal demo serializes two synthetic marker detections and prints their +CDR-decoded dictionary-qualified identities and world positions. It verifies the +exact source timestamp. It performs no marker inference or camera acquisition +and writes no files. + +### Detect an ArUco marker from a generated image + +```bash +PYTHONPATH=.:build/message-codegen/demo/cpp/build .venv/bin/python examples/message-codegen/demo_aruco_detection.py +``` + +This demo renders marker 7, runs OpenCV ArUco detection and pose estimation, +composes a translated world pose, and decodes the generated CDR detection. It +writes `build/message-codegen/demo/evidence/aruco-detection.png` with the detected +bbox and label. Expected world coordinates are x approximately 2 m and z +approximately 0.402 m. No camera or model weights are needed; delete the PNG to +clean up. + +### Generated bounding-box navigation goal + +```bash +PYTHONPATH=.:build/message-codegen/demo/cpp/build .venv/bin/python examples/message-codegen/demo_bbox_navigation.py +``` + +This terminal demo sends generated calibration and detections over LCM, then +prints a CDR-decoded goal (2.00, -0.40, -0.40) with the original detection header. +It retains the module's existing forward/left/up coordinate convention and does +not perform TF conversion. It needs no robot and writes no files. + +### Holonomic tracking through CDR + +After building the generated demo bindings, run: + +```bash +PYTHONPATH=.:build/message-codegen/demo/cpp/build .venv/bin/python examples/message-codegen/demo_holonomic_tracking.py +``` + +The terminal shows a synthetic robot accelerating toward `(2, 1)` and settling +at the target. Reference poses, measured poses, and limited velocity commands +round-trip through generated CDR codecs each tick. This exercises the tracking +law and limiter, without a transport, path planner, hardware, or ROS install. +No processes or output files require cleanup. + +### Threaded holonomic path following + +```bash +PYTHONPATH=.:build/message-codegen/demo/cpp/build .venv/bin/python examples/message-codegen/demo_holonomic_path.py +``` + +The real follower thread drives a synthetic robot along a one-metre path, +prints decoded velocity and position, reports arrival, and publishes zero +velocity. Path, odometry, and commands use generated CDR round-trips. The demo +closes its thread and subscriptions automatically; it needs no robot or ROS. +This is an in-process control demo, not network transport verification. + +The holonomic path demo also exercises `DanLocalPlanner`'s core: it smooths and +resamples the incoming CDR path and visibly suppresses a duplicate replan inside +the configured commit window before starting the follower. + +### Native MLS planning from generated clouds + +With the generated demo bindings already built, install the native Python +planner into the checkout environment and run the terrain demo: + +```bash +uv pip install --python .venv/bin/python 'maturin>=1,<2' +source .venv/bin/activate +maturin develop --release --uv -m dimos/navigation/nav_3d/mls_planner/rust/py/Cargo.toml +PYTHONPATH=.:build/message-codegen/demo/cpp/build .venv/bin/python examples/message-codegen/demo_mls_transformer.py +``` + +The demo prints a TF-derived starting pose, then sends a CDR-decoded synthetic +floor to the native MLS planner and prints the CDR-decoded path waypoints and +exact source timestamp. It requires no robot, model download, or ROS install. +It runs in one process and writes no output artifacts; native build products +remain in Cargo's ignored target directory. + +The MLS terrain demo also writes +`build/message-codegen/demo/evidence/mls-planner.rrd`, containing the floor, +raised path nodes, and connecting segments through the planner's Rerun adapters. +Open it with `rerun build/message-codegen/demo/evidence/mls-planner.rrd`. +For headless verification, use `rerun rrd verify` on that file. The recording is +an ignored build artifact and may be deleted after review. + +### Recorded navigation evaluation + +```bash +PYTHONPATH=.:build/message-codegen/demo/cpp/build .venv/bin/python examples/message-codegen/demo_navigation_recording.py +``` + +The demo writes generated lidar and odometry messages to temporary SQLite +streams, reads them through the navigation evaluator, and prints sensor-to-world +point registration and accumulated trajectory distance. It removes the temporary +database automatically. No recording download or hardware is required. + +The terrain demo also exercises native ray tracing and records its local map, +TF edges, and colormap annotations. Install its additional native extension with: + +```bash +maturin develop --release --uv -m dimos/mapping/ray_tracing/rust/py/Cargo.toml +``` + +The recorded-navigation demo now also invokes the actual native ray-tracing CLI +on its generated SQLite lidar/TF streams. Install the ray-tracing extension as +above. It writes `build/message-codegen/demo/evidence/raytrace-cli.rrd`; inspect +that file with Rerun or check it with `rerun rrd verify`. The temporary SQLite +source is still removed automatically. + +The terrain recording's TF hierarchy is now logged through the live Rerun +bridge's generated-message callback, including its frame axes. This demo invokes +the callback directly and does not start a network subscription or viewer. + +### Generated camera messages through the bridge + +```bash +PYTHONPATH=.:build/message-codegen/demo/cpp/build .venv/bin/python examples/message-codegen/demo_camera_bridge.py +rerun rrd verify build/message-codegen/demo/evidence/camera-bridge.rrd +``` + +Open `camera-bridge.rrd` in Rerun to inspect a red/green image, its compressed +copy, calibration, and two depth images that both represent one metre. The demo +uses generated CDR messages and the bridge callback; it does not start a network +subscription. Its only output is the ignored RRD file. + +The camera demo also logs a generated 3D box labelled `demo-box id=4` in the +camera's optical frame, exercising the same bridge used for image dispatch. + +The terrain demo sends the generated terrain cloud through the shared bridge, +which renders explicit height colors. The same adapter preserves standard packed +RGB colors when supplied by a cloud producer. + +The terrain demo's `world/planned_path` entity now comes from the shared bridge's +generated Path adapter. It is displayed 0.5m above the original route for clarity; +the path message retains its original coordinates. + +The terrain recording also includes `world/occupancy`, a small generated grid +with free, occupied, unknown, and intermediate-cost cells. The shared adapter +places its textured plane using the grid's origin pose. + +### Live LCM/Zenoh-to-Rerun bridge + +```bash +PYTHONPATH=.:build/message-codegen/demo/cpp/build .venv/bin/python examples/message-codegen/demo_live_bridge.py --transport lcm +PYTHONPATH=.:build/message-codegen/demo/cpp/build .venv/bin/python examples/message-codegen/demo_live_bridge.py --transport zenoh +``` + +This starts the real bridge headlessly on an available local gRPC port, publishes +a generated image over the selected transport, waits for decoded/rendered delivery, and writes +`build/message-codegen/demo/evidence/live-bridge-lcm.rrd`. It closes the subscriptions +and transport automatically and never opens a window. A bounded subprocess test +runs the demo and verifies the recording with Rerun. Multicast loopback must be +available in the test environment. + +The Zenoh variant writes `live-bridge-zenoh.rrd`. It uses separate publisher and +subscriber sessions connected explicitly over loopback TCP with scouting and +gossip disabled. This verifies the transport-to-bridge route without claiming +default peer-discovery reliability. Both variants have bounded subprocess tests. + +Run the viewer control protocol without a GUI: + +```sh +PYTHONPATH=.:build/message-codegen/demo/cpp/build .venv/bin/python examples/message-codegen/demo_viewer_controls.py +``` + +This opens a local WebSocket, sends a click, velocity, and stop, and prints the +verified generated CDR values. It requires no robot or ROS installation. + +Inspect command-center JSON and a generated click goal without a GUI: + +```sh +PYTHONPATH=.:build/message-codegen/demo/cpp/build .venv/bin/python examples/message-codegen/demo_command_center.py +``` + +The demo exercises registered command-center handlers directly and prints the +state prepared for the browser, including a compressed costmap. + +Inspect a VLM request image without credentials or model inference: + +```sh +PYTHONPATH=.:build/message-codegen/demo/cpp/build .venv/bin/python examples/message-codegen/demo_vlm_image.py +``` + +The demo writes `build/message-codegen/demo/evidence/vlm-request.jpg` from a +generated CDR image through the agent's request builder, using a stubbed model. + +Exercise named-location navigation without hardware or inference: + +```sh +PYTHONPATH=.:build/message-codegen/demo/cpp/build .venv/bin/python examples/message-codegen/demo_navigation_skill.py +``` + +The demo tags generated odometry and verifies that a retrieved CDR goal preserves +position and orientation, with memory and navigation RPCs stubbed. + +Observe a synthetic camera over LCM/CDR and inspect its MCP image response: + +```sh +PYTHONPATH=.:build/message-codegen/demo/cpp/build .venv/bin/python examples/message-codegen/demo_mcp_observe.py +``` + +The demo saves `build/message-codegen/demo/evidence/mcp-observe.jpg`. It invokes +MCP request handling directly and needs no model credentials or robot. + +Check keyboard commands and immediate key-release stopping without a display: + +```sh +PYTHONPATH=.:build/message-codegen/demo/cpp/build .venv/bin/python examples/message-codegen/demo_keyboard_cdr.py +``` + +Synthetic pygame events drive the real handlers; subscribers verify generated +CDR velocity and gripper values. No hardware commands are sent. + +Inspect the hosted operator's PNG map and compact odometry payload: + +```sh +PYTHONPATH=.:build/message-codegen/demo/cpp/build .venv/bin/python examples/message-codegen/demo_hosted_map.py +``` + +This writes `build/message-codegen/demo/evidence/hosted-map.png` from CDR inputs +through local callbacks, with no hosted connection or robot. + +### Explicit generated video telemetry + +After rebuilding the generated messages with the current in-tree definitions: + +```sh +PYTHONPATH=.:build/message-codegen/demo/cpp/build .venv/bin/python examples/message-codegen/demo_video_stats.py +``` + +The demo converts browser health JSON into `dimos_msgs/msg/VideoStats`, crosses +CDR and SQLite boundaries, and prints an exact dropped-frame counter of +4294967297. Counters use integer schema fields rather than Joy's float32 axes. +The temporary database is removed automatically; no browser, robot or hosted +connection is required. The browser JSON plane retains its named metric fields. + +### Phone browser command CDR + +With the generated extension built, start an isolated loopback preview: + +```sh +PYTHONPATH=.:build/message-codegen/demo/cpp/build .venv/bin/python examples/message-codegen/demo_phone_cdr.py +``` + +Open the printed loopback URL and click **Connect**. No sensor permission is +needed: the page sends its default values. The demo validates five generated +TwistStamped messages and exits, shutting down the web server. It never starts +the module control loop or publishes robot commands. + +The browser fetches `/teleop/schema`, uses pinned generic Foxglove ROS2/CDR +libraries to encode each advertised type, and wraps bytes in the existing relay +data frame with an explicit channel, qualified type and `cdr` encoding. The +backend rejects unknown channels, mismatched types and unframed legacy bytes. +This preview requires access to esm.sh for the pinned browser libraries. + +### Hosted Go2 command boundaries + +Inspect CDR velocity and navigation commands with mocked streams and driver RPC: + +```sh +PYTHONPATH=.:build/message-codegen/demo/cpp/build .venv/bin/python examples/message-codegen/demo_go2_command_cdr.py +``` + +The handler harness accepts two CDR commands one nanosecond apart, displays +the clamped velocity and a generated world-frame navigation goal, and checks +that E-STOP suppresses publication. It asserts that no driver RPC was called. +This is a command-boundary demo; it does not start a blueprint or connect to Go2. + +## Inspect generated camera compositing + +Run the bounded handler demo from the checkout's prepared environment: + +```bash +PYTHONPATH=.:build/message-codegen/demo/cpp/build \ + .venv/bin/python examples/message-codegen/demo_camera_mux_cdr.py +``` + +It decodes two generated RGB images, combines and scales them, then encodes +the result as CDR and JPEG. It asserts RGB ordering and the latest source +timestamp `1700000000123456790ns`. Ports are mocked; it starts no camera, +video encoder, control loop or robot connection. + +## Record and replay the native blueprint + +With the runtime installed and the native relay/recorder binaries built, run: + +```bash +python examples/message-codegen/demo_blueprint_recording.py --transport zenoh \ + --samples 3 --output build/message-codegen/demo/evidence/blueprint-recording.mcap +``` + +The output must be a new filename. The demo launches a loopback Zenoh router, +Python producer, C++ relay, Rust relay, and Rust MCAP recorder. It checks custom +line edits, exact image bytes, and standard poses, stops the producers, then +independently decodes the embedded ROS schemas and replays all three streams +through DimOS. Replay bytes must match the recording in each stream's order. +Samples have 100 ms timestamp spacing and retain a nonzero nanosecond remainder; +this avoids collapsing adjacent nanoseconds in the existing float-second replay +scheduler. Host tuning is checked only, never applied by this demo. + +The same command accepts `--transport lcm`; this path requires working multicast +on the host and has not passed the current host's default-interface gate. +The recording remains for viewer inspection; no producer or router remains. +See [recording evidence](evidence/blueprint-recording.md) for the verified run +and remaining viewer/transport acceptance gates. + +### Build the release wheel matrix without publishing + +The `release-build-check.yml` workflow accepts a manual `full-matrix=true` +dispatch on the proposal branch. It builds and runs the installed-wheel smoke +test for Python 3.10, 3.11 and 3.12 on Linux x86-64, Linux ARM64 and macOS ARM64, +and checks the source archive. Successful wheels are retained as review artifacts. +It has no package-publishing or release-tag jobs. Ordinary pull requests retain +the single-wheel smoke check. The release workflow's `dry-run` publishes to +TestPyPI and is not a substitute for this build-only check. diff --git a/examples/message-codegen/demo_aruco_detection.py b/examples/message-codegen/demo_aruco_detection.py new file mode 100644 index 00000000000..5cdb47d58e3 --- /dev/null +++ b/examples/message-codegen/demo_aruco_detection.py @@ -0,0 +1,100 @@ +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from pathlib import Path + +import cv2 +from dimos_generated.builtin_interfaces.msg import Time +from dimos_generated.geometry_msgs.msg import Quaternion, Transform, TransformStamped, Vector3 +from dimos_generated.sensor_msgs.msg import CameraInfo, Image +from dimos_generated.std_msgs.msg import Header +from dimos_generated.tf2_msgs.msg import TFMessage +from dimos_generated.vision_msgs.msg import Detection3D, Detection3DArray +import numpy as np + +from dimos.memory.type.observation import Observation +from dimos.msgs.image import image_from_array, image_view +from dimos.perception.fiducial.marker_detect import detect_markers_in_image +from dimos.perception.fiducial.marker_tf_module import MarkerTfModule +from dimos.perception.fiducial.marker_transformer import DetectMarkers, MarkersPerFrame + + +def main() -> None: + dictionary = cv2.aruco.getPredefinedDictionary(cv2.aruco.DICT_4X4_50) + pixels = np.full((400, 400), 255, dtype=np.uint8) + pixels[100:300, 100:300] = cv2.aruco.generateImageMarker(dictionary, 7, 200) + header = Header(frame_id="camera", stamp=Time(sec=1700000000, nanosec=123456789)) + image = image_from_array(pixels, encoding="mono8", header=header) + image = Image.decode(image.encode()) + camera = CameraInfo(width=400, height=400, k=[400, 0, 200, 0, 400, 200, 0, 0, 1]) + tf = TransformStamped( + header=Header(frame_id="world"), + child_frame_id="camera", + transform=Transform(translation=Vector3(x=2), rotation=Quaternion(w=1)), + ) + found = detect_markers_in_image( + image, + camera_info=camera, + world_T_optical=tf, + marker_length_m=0.2, + aruco_dictionary="DICT_4X4_50", + ) + assert len(found) == 1 + wire = Detection3D.decode(found[0].to_detection3d_msg().encode()) + assert wire.id == "7" + assert wire.results[0].hypothesis.class_id == "DICT_4X4_50:7" + assert wire.header.stamp == header.stamp + assert wire.header.frame_id == "world" + assert abs(wire.bbox.center.position.x - 2) < 0.01 + assert abs(wire.bbox.center.position.z - 0.4) < 0.01 + observation = Observation( + id=1, ts=1700000000.1234567, data_type=Image, _data=image, pose=(2, 0, 0, 0, 0, 0, 1) + ) + detector = DetectMarkers( + camera_info=camera, + marker_length_m=0.2, + aruco_dictionary="DICT_4X4_50", + emit_empty_frames=True, + ) + arrays = list(MarkersPerFrame()(detector(iter([observation])))) + decoded_array = Detection3DArray.decode(arrays[0].data.encode()) + assert len(decoded_array.detections) == 1 + assert decoded_array.header.stamp == image.header.stamp + print("Memory stream: image → marker observation → generated CDR array") + publisher = MarkerTfModule() + transforms: list[TFMessage] = [] + unsubscribe = publisher.tf.subscribe( + lambda msg: transforms.append(TFMessage.decode(msg.encode())) + ) + try: + publisher._process_detections(decoded_array) + assert len(transforms) == 1 + for transform in transforms[0].transforms: + assert transform.header.stamp == image.header.stamp + print(f"CDR TF: {transform.header.frame_id} → {transform.child_frame_id}") + finally: + unsubscribe() + publisher.stop() + output = Path("build/message-codegen/demo/evidence/aruco-detection.png") + output.parent.mkdir(parents=True, exist_ok=True) + assert cv2.imwrite(str(output), image_view(found[0].annotated_image())) + print( + f"Detected {wire.results[0].hypothesis.class_id}: world x={wire.bbox.center.position.x:.3f}, z={wire.bbox.center.position.z:.3f} m" + ) + print(f"Annotated image: {output}") + print("PASS: generated image → ArUco pose → world transform → CDR marker") + + +if __name__ == "__main__": + main() diff --git a/examples/message-codegen/demo_base_transport.py b/examples/message-codegen/demo_base_transport.py new file mode 100644 index 00000000000..057b10ecd8f --- /dev/null +++ b/examples/message-codegen/demo_base_transport.py @@ -0,0 +1,106 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Exchange generated base commands and odometry without robot hardware.""" + +from contextlib import ExitStack +import math +import socket +import threading +from typing import Any +import uuid + +from demo_pubsub import free_port +from dimos_generated.geometry_msgs.msg import Point, Pose, PoseStamped, Quaternion, Twist, Vector3 + +from dimos.core.transport import LCMTransport, PubSubTransport, ZenohTransport +from dimos.hardware.drive_trains.transport.adapter import TransportTwistAdapter +from dimos.protocol.service.zenohservice import ZenohSessionPool +from dimos.utils.testing.waiting import retry_until, wait_until + + +def demonstrate(backend: str) -> None: + with ExitStack() as stack: + pools = [ZenohSessionPool(), ZenohSessionPool()] + for pool in pools: + stack.callback(pool.close_all) + endpoint = f"tcp/127.0.0.1:{free_port(socket.SOCK_STREAM)}" + url = f"udpm://239.255.76.67:{free_port(socket.SOCK_DGRAM)}?ttl=0" + prefix = f"base_{uuid.uuid4().hex[:8]}" + + def transport(topic: str, message_type: type, peer: int = 0) -> Any: + result: PubSubTransport[Any] + if backend == "lcm": + result = LCMTransport(topic, message_type, url=url) + else: + result = ZenohTransport( + "dimos/" + topic.lstrip("/"), + message_type, + session_pool=pools[peer], + scouting=False, + multicast=False, + gossip=False, + listen=[endpoint] if peer == 0 else [], + connect=[] if peer == 0 else [endpoint], + ) + stack.callback(result.stop) + result.start() + return result + + adapter = TransportTwistAdapter(hardware_id=prefix, transport_cls=transport) + stack.callback(adapter.disconnect) + assert adapter.connect() + odom = transport(f"/{prefix}/odom", PoseStamped, 1) + commands = transport(f"/{prefix}/cmd_vel", Twist, 1) + moving, stopped = threading.Event(), threading.Event() + received = [] + + def collect(message: Twist) -> None: + received.append(message) + (stopped if message == Twist() else moving).set() + + stack.callback(commands.subscribe(collect)) + source = PoseStamped( + pose=Pose( + position=Point(x=2.0, y=-1.0), + orientation=Quaternion(z=math.sqrt(0.5), w=math.sqrt(0.5)), + ) + ) + + def feedback_received() -> bool: + odom.publish(source) + return adapter.read_odometry() is not None + + wait_until(feedback_received, timeout=5) + feedback = adapter.read_odometry() + assert feedback is not None and feedback[:2] == [2.0, -1.0] + assert math.isclose(feedback[2], math.pi / 2) + assert not adapter.write_velocities([0.5, -0.2, 0.3]) + adapter.write_enable(True) + + def send_command() -> None: + assert adapter.write_velocities([0.5, -0.2, 0.3]) + + retry_until(moving, send_command, timeout=5) + assert received[0] == Twist(linear=Vector3(x=0.5, y=-0.2), angular=Vector3(z=0.3)) + adapter.write_enable(False) + assert stopped.wait(5), "Disabling the adapter did not deliver a generated stop command" + assert adapter.read_velocities() == [0.0, 0.0, 0.0] + print(f"{backend}: odometry={feedback}, command=(0.5,-0.2,0.3), disable=(0,0,0)") + print(f"PASS: {backend} generated base commands, odometry, and stop") + + +if __name__ == "__main__": + for backend in ("lcm", "zenoh"): + demonstrate(backend) diff --git a/examples/message-codegen/demo_bbox_navigation.py b/examples/message-codegen/demo_bbox_navigation.py new file mode 100644 index 00000000000..5137c1642d4 --- /dev/null +++ b/examples/message-codegen/demo_bbox_navigation.py @@ -0,0 +1,78 @@ +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from threading import Event +from uuid import uuid4 + +from dimos_generated.builtin_interfaces.msg import Time +from dimos_generated.geometry_msgs.msg import PoseStamped +from dimos_generated.sensor_msgs.msg import CameraInfo +from dimos_generated.std_msgs.msg import Header +from dimos_generated.vision_msgs.msg import ( + BoundingBox2D, + Detection2D, + Detection2DArray, + Point2D, + Pose2D, +) + +from dimos.core.transport import LCMTransport +from dimos.navigation.bbox_navigation import BBoxNavigationModule + + +def main() -> None: + module = BBoxNavigationModule(goal_distance=2) + prefix = uuid4().hex[:8] + module.camera_info.transport = LCMTransport(f"/bc/{prefix}", CameraInfo) + module.detection2d.transport = LCMTransport(f"/bd/{prefix}", Detection2DArray) + goals = [] + ready = Event() + received = Event() + unsubscribe = module.goal_request.subscribe( + lambda value: (goals.append(PoseStamped.decode(value.encode())), received.set()) + ) + message = Detection2DArray( + header=Header(frame_id="base_link", stamp=Time(sec=1700000000, nanosec=123456789)), + detections=[Detection2D(bbox=BoundingBox2D(center=Pose2D(position=Point2D(x=420, y=290))))], + ) + try: + module.start() + module._on_detection(message) + assert not goals + camera = CameraInfo(k=[500, 0, 320, 0, 250, 240, 0, 0, 1]) + camera_unsubscribe = module.camera_info.subscribe(lambda _: ready.set()) + module.camera_info.transport.publish(CameraInfo.decode(camera.encode())) + assert ready.wait(2) + camera_unsubscribe() + module.detection2d.transport.publish(Detection2DArray.decode(message.encode())) + assert received.wait(2) + assert len(goals) == 1 + goal = goals[0] + assert goal.header == message.header + assert (goal.pose.position.x, goal.pose.position.y, goal.pose.position.z) == (2, -0.4, -0.4) + print( + f"CDR goal: x={goal.pose.position.x:.2f}, y={goal.pose.position.y:.2f}, z={goal.pose.position.z:.2f} m" + ) + print("PASS: LCM calibration + bbox → generated goal with exact source header") + module._on_detection(Detection2DArray()) + assert len(goals) == 1 + finally: + unsubscribe() + module.stop() + module.camera_info.transport.stop() + module.detection2d.transport.stop() + + +if __name__ == "__main__": + main() diff --git a/examples/message-codegen/demo_blueprint.py b/examples/message-codegen/demo_blueprint.py new file mode 100644 index 00000000000..42e33a3f5a7 --- /dev/null +++ b/examples/message-codegen/demo_blueprint.py @@ -0,0 +1,216 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Exchange changing custom messages and images through a three-language blueprint.""" + +from __future__ import annotations + +import argparse +from contextlib import ExitStack +from functools import partial +from pathlib import Path +import socket +import threading +from typing import Literal +from unittest.mock import patch +import uuid + +from dimos_generated.dimos_msgs.msg import LineSegment3D, LineSegments3D +from dimos_generated.geometry_msgs.msg import Point, Pose, PoseStamped, Quaternion +from dimos_generated.sensor_msgs.msg import Image +import numpy as np + +from dimos.core.coordination.blueprints import Blueprint, autoconnect +from dimos.core.coordination.module_coordinator import ModuleCoordinator +from dimos.core.core import rpc +from dimos.core.module import Module +from dimos.core.native_module import NativeModule +from dimos.core.stream import In, Out +from dimos.msgs.time import time_from_nanoseconds, to_nanoseconds +from dimos.protocol.service.system_configurator.base import configure_system +from dimos.protocol.service.zenohservice import ZenohConfig, ZenohSessionPool + +_ROOT = Path(__file__).resolve().parents[2] +_STAMP = 1_700_000_000_123_456_789 + + +class PythonRelayDemo(Module): + """Publish a sample and validate the replies from both native modules.""" + + l0: Out[LineSegments3D] + l2: In[LineSegments3D] + i0: Out[Image] + i2: In[Image] + p0: Out[PoseStamped] + + @rpc + def start(self) -> None: + super().start() + self._stamp = -1 + self._lines: LineSegments3D | None = None + self._image: Image | None = None + self._received = threading.Event() + self.l2.subscribe(self._receive_lines) + self.i2.subscribe(self._receive_image) + + def _receive_lines(self, message: LineSegments3D) -> None: + if to_nanoseconds(message.header.stamp) == self._stamp: + self._lines = message + if self._image is not None: + self._received.set() + + def _receive_image(self, message: Image) -> None: + if to_nanoseconds(message.header.stamp) == self._stamp: + self._image = message + if self._lines is not None: + self._received.set() + + @rpc + def exchange(self, sample: int) -> str: + self._stamp = _STAMP + sample * 100_000_000 + self._lines = None + self._image = None + self._received.clear() + lines = LineSegments3D( + segments=[LineSegment3D(start=Point(x=sample), end=Point(y=2), weight=sample)] + ) + lines.header.stamp = time_from_nanoseconds(self._stamp) + lines.header.frame_id = "map" + image = Image( + width=640, + height=480, + step=1920, + encoding="rgb8", + data=np.full(640 * 480 * 3, sample % 256, dtype=np.uint8), + ) + image.header.stamp = time_from_nanoseconds(self._stamp) + image.header.frame_id = "camera" + pose = PoseStamped( + header=lines.header, pose=Pose(position=Point(x=sample), orientation=Quaternion(w=1)) + ) + for _ in range(60): + self.p0.publish(pose) + self.l0.publish(lines) + self.i0.publish(image) + if self._received.wait(0.25): + break + if self._lines is None or self._image is None: + raise TimeoutError( + f"Missing native relay reply for sample {sample}: " + f"lines={self._lines is not None}, image={self._image is not None}" + ) + expected = LineSegments3D.decode(lines.encode()) + expected.segments = [ + LineSegment3D(start=Point(x=sample), end=Point(y=2), weight=sample + 2) + ] + if self._lines.encode() != expected.encode(): + raise ValueError("Native custom message reply differs from the expected edits") + if self._image.encode() != image.encode(): + raise ValueError("Native image reply changed pixels or metadata") + return ( + f"sample {sample}: Python weight={sample} → C++ {sample + 1} → Rust {sample + 2}; " + f"Python verified {len(image.data):,} image bytes and stamp {self._stamp}" + ) + + +class CppRelay(NativeModule): + lines_in: In[LineSegments3D] + lines_out: Out[LineSegments3D] + image_in: In[Image] + image_out: Out[Image] + + +class RustRelay(NativeModule): + lines_in: In[LineSegments3D] + lines_out: Out[LineSegments3D] + image_in: In[Image] + image_out: Out[Image] + + +def blueprint( + backend: Literal["lcm", "zenoh"], + cpp: Path, + rust: Path, + *, + recording: Blueprint | None = None, +) -> Blueprint: + return ( + autoconnect( + PythonRelayDemo.blueprint(), + CppRelay.blueprint(executable=str(cpp.resolve()), stdin_config=True), + RustRelay.blueprint(executable=str(rust.resolve()), stdin_config=True), + *([recording] if recording is not None else []), + ) + .remappings( + [ + (CppRelay, "lines_in", "l0"), + (CppRelay, "lines_out", "l1"), + (RustRelay, "lines_in", "l1"), + (RustRelay, "lines_out", "l2"), + (CppRelay, "image_in", "i0"), + (CppRelay, "image_out", "i1"), + (RustRelay, "image_in", "i1"), + (RustRelay, "image_out", "i2"), + ] + ) + .namespace(f"cdr{uuid.uuid4().hex[:8]}") + .global_config(viewer="none", transport=backend) + ) + + +def main() -> None: + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument("--transport", choices=("lcm", "zenoh"), default="zenoh") + parser.add_argument("--samples", type=int, default=5) + parser.add_argument("--cpp", type=Path, default=_ROOT / "build/native-cpp-examples/cdr_relay") + parser.add_argument("--rust", type=Path, default=_ROOT / "target/debug/cdr_relay") + args = parser.parse_args() + if args.samples < 1: + parser.error("--samples must be positive") + for executable in (args.cpp, args.rust): + if not executable.is_file(): + parser.error(f"Build the native relay first: {executable}") + with ExitStack() as stack: + # A hardware-free demo reports host tuning requirements without applying them. + stack.enter_context( + patch( + "dimos.protocol.service.system_configurator.base.configure_system", + partial(configure_system, check_only=True), + ) + ) + bp = blueprint(args.transport, args.cpp, args.rust) + if args.transport == "zenoh": + with socket.socket() as reservation: + reservation.bind(("127.0.0.1", 0)) + endpoint = f"tcp/127.0.0.1:{reservation.getsockname()[1]}" + pool = ZenohSessionPool() + stack.callback(pool.close_all) + pool.acquire( + ZenohConfig( + mode="router", listen=[endpoint], connect=[], multicast=False, gossip=False + ) + ) + bp = bp.global_config( + zenoh_mode="client", zenoh_connect=endpoint, zenoh_multicast=False + ) + coordinator = ModuleCoordinator.build(bp) + stack.callback(coordinator.stop) + producer = coordinator.get_instance(PythonRelayDemo) + for sample in range(args.samples): + print(producer.exchange(sample), flush=True) + print(f"{args.transport}: three-language coordinator blueprint verified", flush=True) + + +if __name__ == "__main__": + main() diff --git a/examples/message-codegen/demo_blueprint_recording.py b/examples/message-codegen/demo_blueprint_recording.py new file mode 100644 index 00000000000..457fab89131 --- /dev/null +++ b/examples/message-codegen/demo_blueprint_recording.py @@ -0,0 +1,173 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Record the native relay blueprint to MCAP, stop it, then replay every CDR value.""" + +import argparse +from contextlib import ExitStack +from functools import partial +from pathlib import Path +import socket +import threading +from typing import Any +from unittest.mock import patch + +from demo_blueprint import PythonRelayDemo, blueprint +from dimos_generated.dimos_msgs.msg import LineSegments3D +from dimos_generated.geometry_msgs.msg import PoseStamped +from dimos_generated.sensor_msgs.msg import Image +from mcap.reader import make_reader +import numpy as np +from rosbags.typesys import Stores, get_types_from_msg, get_typestore + +from dimos.core.coordination.module_coordinator import ModuleCoordinator +from dimos.core.stream import In +from dimos.experimental.memory.rust_recorder import RustMcapStoreConfig, RustRecorder +from dimos.memory.recording_policy import OnExisting +from dimos.memory.replay_module import replay_module +from dimos.memory.store.mcap import McapStore +from dimos.msgs.time import to_nanoseconds +from dimos.protocol.service.system_configurator.base import configure_system +from dimos.protocol.service.zenohservice import ZenohConfig, ZenohSessionPool + + +class RelayRecorder(RustRecorder): + l2: In[LineSegments3D] + i2: In[Image] + p0: In[PoseStamped] + + +def verify_and_replay(artifact: Path, samples: int) -> None: + expected: dict[str, list[bytes]] = {} + independent = get_typestore(Stores.ROS2_JAZZY) + with artifact.open("rb") as source: + for schema, channel, row in make_reader(source).iter_messages(): + assert schema is not None and schema.encoding == "ros2msg" + assert channel.message_encoding == "cdr" + independent.register(get_types_from_msg(schema.data.decode(), schema.name)) + independent.deserialize_cdr(row.data, schema.name) + with McapStore(path=str(artifact)) as store: + for name in ("l2", "i2", "p0"): + messages = [obs.data for obs in store.stream(name)] + expected[name] = [message.encode() for message in messages] + observed_samples = set() + for message in messages: + delta = to_nanoseconds(message.header.stamp) - 1700000000123456789 + sample, remainder = divmod(delta, 100_000_000) + assert remainder == 0 + assert 0 <= sample < samples + observed_samples.add(sample) + if name == "l2": + assert message.segments[0].weight == sample + 2 + assert message.segments[0].start.x == sample + elif name == "p0": + assert ( + message.pose.position.x, + message.pose.position.y, + message.pose.position.z, + ) == (sample, 0, 0) + assert message.pose.orientation.w == 1 + else: + assert message.encoding == "rgb8" + assert (message.width, message.height, message.step) == (640, 480, 1920) + np.testing.assert_array_equal( + message.data.view(), np.full(921600, sample % 256, dtype=np.uint8) + ) + assert observed_samples == set(range(samples)), (name, observed_samples) + print( + f"MCAP {name}: verified {len(messages)} values, samples={sorted(observed_samples)}" + ) + + module = replay_module(str(artifact))(dataset=str(artifact)) + received: dict[str, list[bytes]] = {name: [] for name in expected} + done = threading.Event() + lock = threading.Lock() + + def receive(name: str, message: Any) -> None: + with lock: + received[name].append(message.encode()) + if all(len(received[key]) >= len(values) for key, values in expected.items()): + done.set() + + with ExitStack() as stack: + for name in expected: + stack.callback( + module.outputs[name].subscribe(lambda message, name=name: receive(name, message)) + ) + stack.callback(module.stop) + module.start() + assert done.wait(15), {name: len(values) for name, values in received.items()} + assert received == expected + print("Replay: every recorded CDR value matched in per-stream order after producers stopped") + + +def main() -> None: + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument("--transport", choices=("lcm", "zenoh"), default="zenoh") + parser.add_argument("--output", type=Path, required=True) + parser.add_argument("--samples", type=int, default=3) + parser.add_argument("--cpp", type=Path, default=Path("build/native-cpp-examples/cdr_relay")) + parser.add_argument("--rust", type=Path, default=Path("target/debug/cdr_relay")) + parser.add_argument("--recorder", type=Path, default=Path("target/debug/dimos-memory-recorder")) + args = parser.parse_args() + if args.samples < 1: + parser.error("--samples must be positive") + if args.output.exists(): + parser.error("--output must be a new file; existing recordings are preserved") + for executable in (args.cpp, args.rust, args.recorder): + if not executable.is_file(): + parser.error(f"Build the native executable first: {executable}") + args.output.parent.mkdir(parents=True, exist_ok=True) + recording = RelayRecorder.blueprint( + executable=str(args.recorder.resolve()), + build_command="", + cwd=str(args.recorder.resolve().parent), + store=RustMcapStoreConfig(path=str(args.output.resolve())), + on_existing=OnExisting.ERROR, + record_tf=False, + encoding_threads=2, + ) + with ExitStack() as stack: + stack.enter_context( + patch( + "dimos.protocol.service.system_configurator.base.configure_system", + partial(configure_system, check_only=True), + ) + ) + bp = blueprint(args.transport, args.cpp, args.rust, recording=recording) + if args.transport == "zenoh": + with socket.socket() as reservation: + reservation.bind(("127.0.0.1", 0)) + endpoint = f"tcp/127.0.0.1:{reservation.getsockname()[1]}" + pool = ZenohSessionPool() + stack.callback(pool.close_all) + pool.acquire( + ZenohConfig( + mode="router", listen=[endpoint], connect=[], multicast=False, gossip=False + ) + ) + bp = bp.global_config( + zenoh_mode="client", zenoh_connect=endpoint, zenoh_multicast=False + ) + coordinator = ModuleCoordinator.build(bp) + stack.callback(coordinator.stop) + producer = coordinator.get_instance(PythonRelayDemo) + for sample in range(args.samples): + print(producer.exchange(sample), flush=True) + verify_and_replay(args.output.resolve(), args.samples) + print(f"Retained self-describing {args.transport} recording: {args.output}") + + +if __name__ == "__main__": + main() diff --git a/examples/message-codegen/demo_camera_bridge.py b/examples/message-codegen/demo_camera_bridge.py new file mode 100644 index 00000000000..5f340f34027 --- /dev/null +++ b/examples/message-codegen/demo_camera_bridge.py @@ -0,0 +1,101 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Record generated image and calibration messages through the Rerun bridge.""" + +from pathlib import Path +from types import SimpleNamespace + +from dimos_generated.geometry_msgs.msg import Point, Pose, Quaternion, Vector3 +from dimos_generated.sensor_msgs.msg import CameraInfo, CompressedImage +from dimos_generated.std_msgs.msg import Header +from dimos_generated.vision_msgs.msg import ( + BoundingBox3D, + Detection3D, + Detection3DArray, + ObjectHypothesis, + ObjectHypothesisWithPose, +) +import numpy as np +import rerun as rr + +from dimos.msgs.image import image_from_array, image_to_jpeg +from dimos.visualization.rerun.bridge import RerunBridgeModule + + +def main() -> None: + output = Path("build/message-codegen/demo/evidence/camera-bridge.rrd") + output.parent.mkdir(parents=True, exist_ok=True) + rr.init("generated-camera-bridge", spawn=False) + rr.save(str(output)) + bridge = RerunBridgeModule() + bridge._min_intervals = {} + bridge._camera_infos = {} + bridge._image_entities = set() + header = Header(frame_id="camera_optical") + pixels = np.zeros((120, 160, 3), dtype=np.uint8) + pixels[:, :80, 0] = 255 + pixels[:, 80:, 1] = 255 + color = image_from_array(pixels, encoding="rgb8", header=header) + info = CameraInfo(header=header, width=160, height=120, k=[100, 0, 80, 0, 100, 60, 0, 0, 1]) + messages = [ + ("color", color), + ("camera_info", info), + ("jpeg", CompressedImage(header=header, format="jpeg", data=image_to_jpeg(color))), + ( + "depth_mm", + image_from_array( + np.full((120, 160), 1000, dtype=np.uint16), encoding="16UC1", header=header + ), + ), + ( + "depth_m", + image_from_array( + np.ones((120, 160), dtype=np.float32), encoding="32FC1", header=header + ), + ), + ] + detections = Detection3DArray( + header=header, + detections=[ + Detection3D( + id="4", + bbox=BoundingBox3D( + center=Pose(position=Point(z=1), orientation=Quaternion(w=1)), + size=Vector3(x=0.5, y=0.5, z=0.2), + ), + results=[ + ObjectHypothesisWithPose( + hypothesis=ObjectHypothesis(class_id="demo-box", score=1) + ) + ], + ) + ], + ) + messages.append(("detections", detections)) + try: + for name, value in messages: + bridge._on_message( + type(value).decode(value.encode()), SimpleNamespace(name="/camera/" + name) + ) + print(f"CDR {type(value).__name__} → world/camera/{name}") + finally: + bridge.stop() + rr.disconnect() + assert output.stat().st_size > 0 + print(f"Recording: {output}; both depth images represent one metre") + + +if __name__ == "__main__": + main() diff --git a/examples/message-codegen/demo_camera_mux_cdr.py b/examples/message-codegen/demo_camera_mux_cdr.py new file mode 100644 index 00000000000..8ba87f9aefb --- /dev/null +++ b/examples/message-codegen/demo_camera_mux_cdr.py @@ -0,0 +1,63 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Inspect generated camera compositing and JPEG with mocked module ports.""" + +from unittest.mock import MagicMock, patch + +from dimos_generated.sensor_msgs.msg import Image +from dimos_generated.std_msgs.msg import Header +import numpy as np + +from dimos.core.module import Module +from dimos.msgs.image import image_from_array, image_to_jpeg, image_view +from dimos.msgs.time import time_from_nanoseconds, to_nanoseconds +from dimos.teleop.hosted.camera_mux import CameraMuxConfig, CameraMuxModule + + +def main() -> None: + with ( + patch.object(Module, "__init__", return_value=None), + patch.object( + CameraMuxModule, "config", CameraMuxConfig(cameras=["cam1", "cam2"]), create=True + ), + ): + mux = CameraMuxModule() + mux.config = CameraMuxConfig(cameras=["cam1", "cam2"], video_max_width=100) + mux.mux_image = MagicMock() + mux._cam_selected = ["cam1", "cam2"] + stamp_ns = 1_700_000_000_123_456_789 + for index, camera in enumerate(("cam1", "cam2")): + pixels = np.zeros((61, 81, 3), dtype=np.uint8) + pixels[:, :, index] = 255 + image = image_from_array( + pixels, + encoding="rgb8", + header=Header(stamp=time_from_nanoseconds(stamp_ns + index), frame_id=camera), + ) + mux._on_cam(camera, Image.decode(image.encode())) + output = Image.decode(mux.mux_image.publish.call_args.args[0].encode()) + assert output.width == 100 and output.height == 36 + assert to_nanoseconds(output.header.stamp) == stamp_ns + 1 + assert output.header.frame_id == "camera_mux" + assert image_view(output)[0, 0].tolist() == [255, 0, 0] + assert image_view(output)[0, -1].tolist() == [0, 255, 0] + jpeg = image_to_jpeg(output) + assert jpeg.startswith(b"\xff\xd8") and jpeg.endswith(b"\xff\xd9") + print(f"CDR camera mux: {output.width}x{output.height}, {len(jpeg)} JPEG bytes") + print(f"PASS: RGB order, exact {stamp_ns + 1}ns source stamp; mocked ports only") + + +if __name__ == "__main__": + main() diff --git a/examples/message-codegen/demo_cloud_filters.py b/examples/message-codegen/demo_cloud_filters.py new file mode 100644 index 00000000000..de8366f8bcd --- /dev/null +++ b/examples/message-codegen/demo_cloud_filters.py @@ -0,0 +1,51 @@ +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Show generated CDR cloud filtering without ROS or a sensor.""" + +from typing import cast + +from dimos_generated.geometry_msgs.msg import Quaternion, Transform, TransformStamped +from dimos_generated.sensor_msgs.msg import CameraInfo, PointCloud2 +from dimos_generated.std_msgs.msg import Header +import numpy as np + +from dimos.msgs.pointcloud import pointcloud_from_xyz +from dimos.perception.detection.type.detection2d.bbox import Detection2DBBox +from dimos.perception.detection.type.detection3d.pointcloud_filters import ( + height_filter, + radius_outlier, +) + + +def main() -> None: + points = np.random.default_rng(42).normal(scale=0.05, size=(100, 3)) + points[:, 2] += 2 + points = np.vstack((points, [10, 10, -1])) + cloud = pointcloud_from_xyz(points, header=Header(frame_id="world")) + cloud = PointCloud2.decode(cloud.encode()) + transform = TransformStamped(transform=Transform(rotation=Quaternion(w=1))) + # These geometric filters do not inspect the detection argument. + detection = cast("Detection2DBBox", None) + for name, operation in (("height", height_filter()), ("radius", radius_outlier())): + result = operation(detection, cloud, CameraInfo(), transform) + assert result is not None + result = PointCloud2.decode(result.encode()) + assert result.width == 100 and result.header == cloud.header + print(f"{name}: 101 input points → {result.width} retained points; source header preserved") + print("PASS: generated clouds survive filtering and CDR round trips") + + +if __name__ == "__main__": + main() diff --git a/examples/message-codegen/demo_command_center.py b/examples/message-codegen/demo_command_center.py new file mode 100644 index 00000000000..1732391893e --- /dev/null +++ b/examples/message-codegen/demo_command_center.py @@ -0,0 +1,59 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Print command-center state and commands from generated messages, without a GUI.""" + +import asyncio +import json + +from dimos_generated.geometry_msgs.msg import Point, Pose, PoseStamped, Quaternion +from dimos_generated.nav_msgs.msg import MapMetaData, OccupancyGrid, Path + +from dimos.web.websocket_vis.websocket_vis_module import WebsocketVisModule + + +def main() -> None: + module = WebsocketVisModule() + goals: list[PoseStamped] = [] + unsubscribe = module.goal_request.subscribe( + lambda value: goals.append(PoseStamped.decode(value.encode())) + ) + try: + module._create_server() + pose = PoseStamped(pose=Pose(position=Point(x=2, y=3), orientation=Quaternion(w=1))) + module._on_robot_pose(PoseStamped.decode(pose.encode())) + path = Path(poses=[pose, PoseStamped(pose=Pose(position=Point(x=5, y=3)))]) + module._on_path(Path.decode(path.encode())) + grid = OccupancyGrid( + info=MapMetaData( + width=2, height=2, resolution=1, origin=Pose(orientation=Quaternion(w=1)) + ), + data=[100, 0, 0, -1], + ) + module._on_global_costmap(OccupancyGrid.decode(grid.encode())) + assert module.sio is not None + asyncio.run(module.sio.handlers["/"]["click"]("demo", [5, 3])) + assert goals[0].pose.position == Point(x=5, y=3) + assert goals[0].header.frame_id == "world" + print("Command-center state from CDR pose, path, and costmap:") + print(json.dumps(module.vis_state, indent=2)) + print("Command-center click → generated CDR goal: world (5, 3, 0)") + print("Handlers exercised directly; no browser or network server was opened.") + finally: + unsubscribe() + module.stop() + + +if __name__ == "__main__": + main() diff --git a/examples/message-codegen/demo_coordinator_joints.py b/examples/message-codegen/demo_coordinator_joints.py new file mode 100644 index 00000000000..03749906c6e --- /dev/null +++ b/examples/message-codegen/demo_coordinator_joints.py @@ -0,0 +1,82 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Inspect generated joint-state streams from a running two-arm mock coordinator.""" + +from contextlib import ExitStack +import threading + +from dimos_generated.sensor_msgs.msg import JointState + +from dimos.control.components import HardwareComponent, HardwareType +from dimos.control.coordinator import ControlCoordinator +from dimos.core.stream import Out +from dimos.msgs.time import to_nanoseconds + + +class DemoCoordinator(ControlCoordinator): + left_arm_joints: Out[JointState] + right_arm_joints: Out[JointState] + + +def main() -> None: + coordinator = DemoCoordinator( + instance_name="ControlCoordinator", + publish_robot_joint_states=True, + hardware=[ + HardwareComponent( + hardware_id=name, + hardware_type=HardwareType.MANIPULATOR, + joints=[f"{name}/joint1", f"{name}/joint2"], + adapter_type="mock", + adapter_kwargs={"initial_positions": positions}, + ) + for name, positions in [("left_arm", [0.1, 0.2]), ("right_arm", [0.3, 0.4])] + ], + ) + ready = threading.Event() + samples: dict[int, dict[str, JointState]] = {} + with ExitStack() as stack: + stack.callback(coordinator.stop) + + def receive(port: str, message: JointState) -> None: + tick = samples.setdefault(to_nanoseconds(message.header.stamp), {}) + tick[port] = message + if len(tick) == 3: + ready.set() + + for name in ["coordinator_joint_state", "left_arm_joints", "right_arm_joints"]: + stack.callback( + getattr(coordinator, name).subscribe( + lambda message, name=name: receive(name, message) + ) + ) + coordinator.start() + assert ready.wait(5), "Coordinator did not publish a full generated feedback tick" + # Stop the tick loop before reading the accumulated snapshot. + coordinator.stop() + complete = next(tick for tick in samples.values() if len(tick) == 3) + assert list(complete["left_arm_joints"].position) == [0.1, 0.2] + assert list(complete["right_arm_joints"].position) == [0.3, 0.4] + assert len(complete["coordinator_joint_state"].position) == 4 + for port, message in complete.items(): + assert JointState.decode(message.encode()) == message + print( + f"{port}: frame={message.header.frame_id}, names={list(message.name)}, q={list(message.position)}, stamp={to_nanoseconds(message.header.stamp)}" + ) + print("PASS: aggregate and per-robot streams use generated JointState and share one tick stamp") + + +if __name__ == "__main__": + main() diff --git a/examples/message-codegen/demo_detection_bbox.py b/examples/message-codegen/demo_detection_bbox.py new file mode 100644 index 00000000000..43a9b92bd98 --- /dev/null +++ b/examples/message-codegen/demo_detection_bbox.py @@ -0,0 +1,62 @@ +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Render a generated-image detection and print its CDR metadata.""" + +from pathlib import Path + +import cv2 +from dimos_generated.builtin_interfaces.msg import Time +from dimos_generated.std_msgs.msg import Header +from dimos_generated.vision_msgs.msg import Detection2DArray +import numpy as np + +from dimos.msgs.image import image_from_array, image_view +from dimos.perception.detection.type.detection2d.bbox import Detection2DBBox +from dimos.perception.detection.type.detection2d.imageDetections2D import ImageDetections2D + + +def main() -> None: + pixels = np.full((240, 320, 3), 40, dtype=np.uint8) + pixels[70:180, 100:220] = [180, 100, 40] + source = image_from_array( + pixels, + encoding="rgb8", + header=Header(frame_id="camera_optical", stamp=Time(sec=1700000000, nanosec=123456789)), + ) + detection = Detection2DBBox( + bbox=(100, 70, 220, 180), + track_id=7, + class_id=2, + confidence=0.9, + name="synthetic target", + ts=1700000000, + image=source, + ) + collection = ImageDetections2D(image=source, detections=[detection]) + array = Detection2DArray.decode(collection.to_ros_detection2d_array().encode()) + wire = array.detections[0] + assert array.header == source.header + assert wire.header == source.header + annotated = collection.annotated_image() + output = Path("build/message-codegen/demo/evidence/detection-bbox.png") + output.parent.mkdir(parents=True, exist_ok=True) + assert cv2.imwrite(str(output), image_view(annotated)) + print(f"CDR detection: track={wire.id}, class={wire.results[0].hypothesis.class_id}") + print(f"Image: {output}; source stamp=1700000000123456789 ns") + print("PASS: generated image → annotated bbox and CDR Detection2DArray") + + +if __name__ == "__main__": + main() diff --git a/examples/message-codegen/demo_detection_projection.py b/examples/message-codegen/demo_detection_projection.py new file mode 100644 index 00000000000..78b58d18bc0 --- /dev/null +++ b/examples/message-codegen/demo_detection_projection.py @@ -0,0 +1,67 @@ +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Project synthetic generated depth into a world-frame detection pose.""" + +from dimos_generated.builtin_interfaces.msg import Time +from dimos_generated.geometry_msgs.msg import ( + PoseStamped, + Quaternion, + Transform, + TransformStamped, + Vector3, +) +from dimos_generated.sensor_msgs.msg import CameraInfo, Image +from dimos_generated.std_msgs.msg import Header +import numpy as np + +from dimos.msgs.image import image_from_array +from dimos.perception.detection.type.detection2d.bbox import Detection2DBBox +from dimos.perception.detection.type.detection3d.pointcloud import Detection3DPC + + +def main() -> None: + header = Header(frame_id="camera", stamp=Time(sec=1700000000, nanosec=123456789)) + depth = image_from_array( + np.full((4, 4), 2000, dtype=np.uint16), encoding="16UC1", header=header + ) + depth = Image.decode(depth.encode()) + detection = Detection2DBBox( + bbox=(0, 0, 3, 3), + track_id=1, + class_id=0, + confidence=1, + name="target", + ts=1700000000.0, + image=depth, + ) + camera = CameraInfo(width=4, height=4, k=[2, 0, 0, 0, 2, 0, 0, 0, 1]) + transform = TransformStamped( + header=Header(frame_id="camera"), + child_frame_id="world", + transform=Transform(translation=Vector3(x=-10), rotation=Quaternion(w=1)), + ) + result = Detection3DPC.from_depth(detection, depth, camera, transform, filters=[]) + assert result is not None + pose = PoseStamped.decode(result.pose.encode()) + assert pose.header.stamp == header.stamp + assert pose.header.frame_id == "world" + assert (pose.pose.position.x, pose.pose.position.y, pose.pose.position.z) == (11.5, 1.5, 2) + print("16 depth pixels at 2000 mm → 16 world-frame points") + print("CDR centroid: (11.5, 1.5, 2.0) m; stamp=1700000000123456789 ns") + print("PASS: generated depth → detection cloud → generated CDR pose") + + +if __name__ == "__main__": + main() diff --git a/examples/message-codegen/demo_export_detection_fixture.py b/examples/message-codegen/demo_export_detection_fixture.py new file mode 100644 index 00000000000..ae6a3ad0980 --- /dev/null +++ b/examples/message-codegen/demo_export_detection_fixture.py @@ -0,0 +1,179 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import argparse +from dataclasses import dataclass +from enum import Enum +import hashlib +import json +from pathlib import Path +import pickle +import pickletools +from typing import Any + +from dimos_generated.geometry_msgs.msg import PoseStamped +from dimos_generated.sensor_msgs.msg import Image, PointCloud2 +from dimos_generated.std_msgs.msg import Header +import numpy as np +from numpy.core.multiarray import _reconstruct + +from dimos.msgs.image import image_from_array, image_view +from dimos.msgs.pointcloud import pointcloud_from_xyz, pointcloud_xyz +from dimos.msgs.time import time_from_seconds +from dimos.robot.unitree.type.odometry import pose_from_webrtc_odometry + + +# One-time export of the hash-verified project archive. These inert state records +# are not installed in DimOS and are never used by runtime or pytest readers. +@dataclass +class ArchivedState: + def __setstate__(self, state: Any) -> None: + if isinstance(state, tuple): + state = state[0] | state[1] + self.__dict__.update(state) + + +class ArchivedFormat(Enum): + BGR = "BGR" + + +def points_array(points: Any) -> Any: + return points + + +class ArchiveReader(pickle.Unpickler): + def find_class(self, module: str, name: str) -> Any: + allowed = { + ("dimos.msgs.sensor_msgs.Image", "Image"): ArchivedState, + ("dimos.msgs.sensor_msgs.Image", "ImageFormat"): ArchivedFormat, + ("dimos.robot.unitree_webrtc.type.lidar", "LidarMessage"): ArchivedState, + ("dimos.msgs.geometry_msgs.Vector3", "Vector3"): ArchivedState, + ("dimos.core.o3dpickle", "reconstruct_pointcloud"): points_array, + ("numpy.core.multiarray", "_reconstruct"): _reconstruct, + ("numpy", "ndarray"): np.ndarray, + ("numpy", "dtype"): np.dtype, + } + if (module, name) not in allowed: + raise ValueError(f"Unexpected archived global: {module}.{name}") + return allowed[module, name] + + +parser = argparse.ArgumentParser( + description="Export only the five pinned Go2 regression moments; not a runtime legacy reader." +) +parser.add_argument( + "--source", + type=Path, + required=True, + help="Safely extracted unitree_go2_lidar_corrected directory", +) +parser.add_argument("--archive", type=Path, required=True, help="Original project LFS archive") +parser.add_argument("--output", type=Path, required=True) +args = parser.parse_args() +with args.archive.open("rb") as archive: + digest = hashlib.sha256() + for chunk in iter(lambda: archive.read(1024 * 1024), b""): + digest.update(chunk) + assert digest.hexdigest() == "51a817f2b5664c9e2f2856293db242e030f0edce276e21da0edc2821d947aad2" +root = args.source +out = args.output +out.mkdir(exist_ok=False) + + +def stamp(path: Path) -> float: + with path.open("rb") as f: + for op, arg, _pos in pickletools.genops(f): + if op.name == "BINFLOAT": + assert isinstance(arg, float) + return arg + raise ValueError(path) + + +indexes = { + k: [(stamp(p), p) for p in sorted((root / k).glob("*.pickle"))] + for k in ["video", "lidar", "odom"] +} + + +def nearest(kind: str, target: float) -> tuple[float, Path, Any]: + ts, p = min(indexes[kind], key=lambda item: abs(item[0] - target)) + with p.open("rb") as f: + actual, value = ArchiveReader(f).load() + assert actual == ts + return ts, p, value + + +manifest: dict[str, Any] = { + "source_lfs_sha256": "51a817f2b5664c9e2f2856293db242e030f0edce276e21da0edc2821d947aad2", + "moments": [], +} +for seek in [10, 12, 14, 16, 18]: + lidar = nearest("lidar", indexes["lidar"][0][0] + seek) + selected = { + "lidar": lidar, + "video": nearest("video", lidar[2].ts), + "odom": nearest("odom", lidar[2].ts), + } + entry: dict[str, Any] = {"seek": seek, "streams": {}} + msg: Image | PointCloud2 | PoseStamped + for kind, (ts, p, value) in selected.items(): + if kind == "lidar": + msg = pointcloud_from_xyz( + value.pointcloud, + header=Header(stamp=time_from_seconds(value.ts), frame_id=value.frame_id), + ) + np.testing.assert_allclose(pointcloud_xyz(msg), value.pointcloud, rtol=1e-6, atol=1e-6) + elif kind == "video": + assert value.format is ArchivedFormat.BGR + msg = image_from_array( + value.data, + encoding="bgr8", + header=Header(stamp=time_from_seconds(value.ts), frame_id=value.frame_id), + ) + np.testing.assert_array_equal(image_view(msg), value.data) + else: + msg = pose_from_webrtc_odometry(value) + data = msg.encode() + assert type(msg).decode(data).encode() == data + name = f"{seek}-{kind}.cdr" + (out / name).write_bytes(data) + entry["streams"][kind] = { + "file": name, + "msg_name": msg.msg_name, + "recorded_ts": ts, + "source": str(p.relative_to(root)), + "source_sha256": hashlib.sha256(p.read_bytes()).hexdigest(), + "cdr_sha256": hashlib.sha256(data).hexdigest(), + "source_sec": msg.header.stamp.sec, + "source_nanosec": msg.header.stamp.nanosec, + } + manifest["moments"].append(entry) +message_types: tuple[type[Image] | type[PointCloud2] | type[PoseStamped], ...] = ( + Image, + PointCloud2, + PoseStamped, +) +(out / "schemas.json").write_text( + json.dumps({t.msg_name: t.schema for t in message_types}, indent=2) + "\n" +) +(out / "manifest.json").write_text(json.dumps(manifest, indent=2) + "\n") +print( + json.dumps( + { + "output": str(out), + "moments": len(manifest["moments"]), + "bytes": sum(p.stat().st_size for p in out.iterdir()), + } + ) +) diff --git a/examples/message-codegen/demo_external_native.py b/examples/message-codegen/demo_external_native.py new file mode 100644 index 00000000000..24cada5296a --- /dev/null +++ b/examples/message-codegen/demo_external_native.py @@ -0,0 +1,60 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +# Copyright 2026 Dimensional Inc. +# SPDX-License-Identifier: Apache-2.0 + +"""Run a locally extended, independently packaged message through both native SDKs.""" + +import argparse +from pathlib import Path + +from demo_native import exchange_native +from external_telemetry.demo_msgs.msg import Telemetry + + +def main() -> None: + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument("--build", type=Path, default=Path("build/message-codegen/external-native")) + args = parser.parse_args() + evidence = args.build / "evidence" + evidence.mkdir(parents=True, exist_ok=True) + for backend in ("lcm", "zenoh"): + message = Telemetry(application_note="added-locally") + message.header.stamp.sec = 1700000000 + message.header.stamp.nanosec = 123456789 + message.header.frame_id = "application" + result = exchange_native( + backend, + args.build / "cpp/external_native_relay", + args.build / "rust/target/debug/external-native-relay", + evidence, + {"telemetry": message}, + )["telemetry"] + assert result.application_note == "added-locally/cpp-native/rust-native" + assert result.header.stamp.sec == 1700000000 + assert result.header.stamp.nanosec == 123456789 + assert result.header.frame_id == "application" + message.application_note = result.application_note + assert result.encode() == message.encode() + print( + f"{backend}: external Python → native C++ → native Rust → Python: {result.application_note}" + ) + print( + "SDK codecs accepted the external generated type directly; no handwritten codec or upstream schema PR." + ) + + +if __name__ == "__main__": + main() diff --git a/examples/message-codegen/demo_frontiers.py b/examples/message-codegen/demo_frontiers.py new file mode 100644 index 00000000000..c19734d33f5 --- /dev/null +++ b/examples/message-codegen/demo_frontiers.py @@ -0,0 +1,67 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Inspect frontier selection on a generated CDR occupancy map.""" + +from pathlib import Path + +from dimos_generated.builtin_interfaces.msg import Time +from dimos_generated.geometry_msgs.msg import Pose, PoseStamped, Quaternion +from dimos_generated.nav_msgs.msg import MapMetaData, OccupancyGrid +from dimos_generated.std_msgs.msg import Header +import numpy as np + +from dimos.memory.vis.space.space import Space +from dimos.msgs.occupancy import grid_to_world +from dimos.navigation.frontier_exploration.wavefront_frontier_goal_selector import ( + WavefrontFrontierExplorer, +) + + +def main() -> None: + cells = np.full((20, 20), -1, dtype=np.int8) + cells[:, :10] = 0 + header = Header(stamp=Time(sec=1700000000, nanosec=123456789), frame_id="map") + grid = OccupancyGrid( + header=header, + info=MapMetaData( + width=20, height=20, resolution=0.5, origin=Pose(orientation=Quaternion(w=1)) + ), + data=cells.ravel(), + ) + explorer = WavefrontFrontierExplorer() + try: + received = OccupancyGrid.decode(grid.encode()) + frontiers = explorer.detect_frontiers(grid_to_world(received, (5, 10)), received) + assert len(frontiers) == 1 + goal = PoseStamped( + header=received.header, pose=Pose(position=frontiers[0], orientation=Quaternion(w=1)) + ) + decoded = PoseStamped.decode(goal.encode()) + assert decoded.header == header + assert decoded.pose.position.x == 5 and decoded.pose.position.y == 4.75 + output = Path("build/message-codegen/demo/evidence/frontiers.svg") + output.parent.mkdir(parents=True, exist_ok=True) + output.write_text(Space().base_map(received).add(decoded, color="#ff0000").to_svg()) + print( + f"Frontier centroid: ({decoded.pose.position.x:.2f}, {decoded.pose.position.y:.2f}) m" + ) + print(f"SVG: {output}; stamp=1700000000123456789 ns") + print("PASS: generated map → frontier centroid → CDR goal with exact source header") + finally: + explorer.stop() + + +if __name__ == "__main__": + main() diff --git a/examples/message-codegen/demo_g1_messages.py b/examples/message-codegen/demo_g1_messages.py new file mode 100644 index 00000000000..61ef5e7ea46 --- /dev/null +++ b/examples/message-codegen/demo_g1_messages.py @@ -0,0 +1,65 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Publish a synthetic G1 low-state sample as generated feedback without hardware.""" + +from contextlib import ExitStack +from typing import Any + +from dimos_generated.builtin_interfaces.msg import Time +from dimos_generated.std_msgs.msg import Header + +from dimos.msgs.time import to_nanoseconds +from dimos.robot.unitree.g1.wholebody_connection import G1LowStateSnapshot, G1WholeBodyConnection + + +def main() -> None: + robot = G1WholeBodyConnection() + joints: list[Any] = [] + imu: list[Any] = [] + with ExitStack() as stack: + stack.callback(robot.stop) + stack.callback(robot.motor_states.subscribe(joints.append)) + stack.callback(robot.imu.subscribe(imu.append)) + for index in range(3): + header = Header( + stamp=Time(sec=1700000000, nanosec=123456789 + index), frame_id="g1_pelvis" + ) + sample = G1LowStateSnapshot( + positions=[index * 0.1] * 29, + velocities=[0.2] * 29, + efforts=[0.3] * 29, + quaternion=(0.8, 0.0, 0.0, 0.6), + gyroscope=(1.0, 2.0, 3.0), + accelerometer=(4.0, 5.0, 6.0), + ) + robot._publish_motor_state_and_imu(header=header, sample=sample) + assert len(joints) == len(imu) == 3 + for index, (state, orientation) in enumerate(zip(joints, imu, strict=True)): + assert state.header == orientation.header + assert to_nanoseconds(state.header.stamp) == 1700000000123456789 + index + assert list(state.position) == [index * 0.1] * 29 + assert orientation.orientation.w == 0.8 and orientation.orientation.z == 0.6 + for message in (state, orientation): + assert type(message).decode(message.encode()) == message + print( + f"G1 sample {index}: joints={len(state.name)}, q0={state.position[0]:.1f}, IMU xyzw=(0,0,0.6,0.8), stamp={to_nanoseconds(state.header.stamp)}" + ) + print( + "PASS: G1 generated joint/IMU feedback, exact shared stamps, unchanged quaternion convention" + ) + + +if __name__ == "__main__": + main() diff --git a/examples/message-codegen/demo_galaxea_messages.py b/examples/message-codegen/demo_galaxea_messages.py new file mode 100644 index 00000000000..e83894638ff --- /dev/null +++ b/examples/message-codegen/demo_galaxea_messages.py @@ -0,0 +1,82 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Run Galaxea wheel feedback through generated output streams without ROS or hardware.""" + +from contextlib import ExitStack +import math +import threading +from typing import Any + +from dimos_generated.builtin_interfaces.msg import Time +from dimos_generated.geometry_msgs.msg import Twist, TwistStamped, Vector3 +from dimos_generated.std_msgs.msg import Header + +from dimos.msgs.time import to_nanoseconds +from dimos.robot.galaxea.r1pro.connection import R1ProConnection + + +def main() -> None: + robot = R1ProConnection() + outputs: dict[str, list[Any]] = {name: [] for name in ("odom", "odometry", "tf")} + ready = threading.Event() + lock = threading.Lock() + with ExitStack() as stack: + stack.callback(robot.stop) + + def receive(name: str, message: Any) -> None: + with lock: + outputs[name].append(message) + if all(len(values) == 3 for values in outputs.values()): + ready.set() + + for name in outputs: + unsubscribe = getattr(robot, name).subscribe( + lambda message, name=name: receive(name, message) + ) + stack.callback(unsubscribe) + for index in range(4): + robot._on_chassis_speed( + TwistStamped( + header=Header( + stamp=Time(sec=1700000000, nanosec=123456789 + index * 100000000) + ), + twist=Twist(linear=Vector3(x=1.0), angular=Vector3(z=0.2)), + ), + None, + ) + assert ready.wait(5), "Generated Galaxea outputs did not reach subscribers" + for index, (pose, odometry, tf) in enumerate(zip(*outputs.values(), strict=True), start=1): + stamp = 1700000000123456789 + index * 100000000 + assert to_nanoseconds(pose.header.stamp) == stamp + assert odometry.pose.pose == pose.pose + assert all(to_nanoseconds(edge.header.stamp) == stamp for edge in tf.transforms) + assert math.isclose(pose.pose.orientation.z, math.sin(index * 0.01)) + for message in (pose, odometry, tf): + assert type(message).decode(message.encode()) == message + print( + f"tick {index}: x={pose.pose.position.x:.6f}, y={pose.pose.position.y:.6f}, stamp={stamp}" + ) + print( + " TF: " + + " -> ".join( + [tf.transforms[0].header.frame_id] + + [edge.child_frame_id for edge in tf.transforms] + ) + ) + print("PASS: Galaxea generated pose, odometry, and TF streams preserve exact source time") + + +if __name__ == "__main__": + main() diff --git a/examples/message-codegen/demo_go2_command_cdr.py b/examples/message-codegen/demo_go2_command_cdr.py new file mode 100644 index 00000000000..fc053c0b9cc --- /dev/null +++ b/examples/message-codegen/demo_go2_command_cdr.py @@ -0,0 +1,60 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Inspect generated Go2 command boundaries with mocked streams and driver RPC.""" + +import time +from unittest.mock import MagicMock, patch + +from dimos_generated.geometry_msgs.msg import PoseStamped, Twist, TwistStamped, Vector3 +from dimos_generated.std_msgs.msg import Header + +from dimos.core.module import Module +from dimos.msgs.time import time_from_nanoseconds +from dimos.teleop.hosted.go2_command import Go2CommandConfig, Go2CommandModule + + +def main() -> None: + # This handler harness never builds a blueprint or connects a robot driver. + with patch.object(Module, "__init__", return_value=None): + module = Go2CommandModule() + module.config = Go2CommandConfig() + module.go2 = MagicMock() + for port in ("tele_cmd_vel", "goal_request", "cmd_ack"): + setattr(module, port, MagicMock()) + stamp_ns = time.time_ns() - 100_000_000 + for offset in (0, 1): + message = TwistStamped( + header=Header(stamp=time_from_nanoseconds(stamp_ns + offset)), + twist=Twist(linear=Vector3(x=99), angular=Vector3(z=-50)), + ) + module._on_cmd_vel_in(TwistStamped.decode(message.encode())) + assert module.tele_cmd_vel.publish.call_count == 2 + command = Twist.decode(module.tele_cmd_vel.publish.call_args.args[0].encode()) + assert command.linear.x == 1.5 and command.angular.z == -2 + module._handle_nav_goal({"x": 2.5, "y": -1, "nonce": 1}) + goal = PoseStamped.decode(module.goal_request.publish.call_args.args[0].encode()) + assert goal.header.frame_id == "world" and goal.pose.position.x == 2.5 + module._estopped = True + module._on_cmd_vel_in(message) + assert module.tele_cmd_vel.publish.call_count == 2 + module.go2.assert_not_called() + assert module.go2.mock_calls == [] + print(f"CDR drive: two frames 1ns apart, x={command.linear.x}, yaw={command.angular.z}") + print(f"CDR goal: {goal.header.frame_id} ({goal.pose.position.x}, {goal.pose.position.y})") + print("PASS: generated command types, limits, exact ordering and E-STOP; no driver calls") + + +if __name__ == "__main__": + main() diff --git a/examples/message-codegen/demo_grasp_proposals.py b/examples/message-codegen/demo_grasp_proposals.py new file mode 100644 index 00000000000..3f2a0c99f94 --- /dev/null +++ b/examples/message-codegen/demo_grasp_proposals.py @@ -0,0 +1,60 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Generate a top-down grasp from CDR cloud data, without a robot or GPU.""" + +from dimos_generated.builtin_interfaces.msg import Time +from dimos_generated.dimos_msgs.msg import GraspCandidateArray +from dimos_generated.sensor_msgs.msg import PointCloud2 +from dimos_generated.std_msgs.msg import Header +import numpy as np + +from dimos.manipulation.grasping.heuristic_grasp import HeuristicGraspModule +from dimos.msgs.geometry import pose_matrix +from dimos.msgs.pointcloud import pointcloud_from_xyz +from dimos.msgs.time import to_nanoseconds + + +def main() -> None: + points = np.array( + [[0.4, -0.02, 0.1], [0.4, 0.02, 0.1], [0.6, -0.02, 0.2], [0.6, 0.02, 0.2]], + dtype=np.float32, + ) + cloud = pointcloud_from_xyz( + points, + header=Header(frame_id="world", stamp=Time(sec=1700000000, nanosec=123456789)), + ) + provider = HeuristicGraspModule() + try: + proposals = provider.propose_grasps(PointCloud2.decode(cloud.encode())) + payload = proposals.encode() + received = GraspCandidateArray.decode(payload) + assert received.header == cloud.header + assert len(received.candidates) == 1 + candidate = received.candidates[0] + matrix = pose_matrix(candidate.pose) + np.testing.assert_allclose(matrix[:3, 3], [0.5, 0.0, 0.15], atol=1e-7) + np.testing.assert_allclose(matrix[:3, 2], [0.0, 0.0, -1.0], atol=1e-12) + print(f"Input: {cloud.width} points in {cloud.header.frame_id}; CDR decoded") + print(f"Output: dimos_msgs/msg/GraspCandidateArray; {len(payload)} CDR bytes") + print(f"Source timestamp: {to_nanoseconds(received.header.stamp)} ns") + print(f"Rank 0: position={matrix[:3, 3].round(6).tolist()}; score={candidate.score}") + print(f"Approach axis: {matrix[:3, 2].round(6).tolist()}") + print("PASS: generated CDR proposals preserve the cloud header and point down") + finally: + provider.stop() + + +if __name__ == "__main__": + main() diff --git a/examples/message-codegen/demo_grid_path.py b/examples/message-codegen/demo_grid_path.py new file mode 100644 index 00000000000..e82637cbe13 --- /dev/null +++ b/examples/message-codegen/demo_grid_path.py @@ -0,0 +1,73 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +# Copyright 2026 Dimensional Inc. +# SPDX-License-Identifier: Apache-2.0 + +"""Plan on a generated CDR map in Python and C++, then render the resampled path.""" + +from pathlib import Path as FilePath + +from dimos_generated.builtin_interfaces.msg import Time +from dimos_generated.geometry_msgs.msg import Pose, Quaternion +from dimos_generated.nav_msgs.msg import MapMetaData, OccupancyGrid, Path +from dimos_generated.std_msgs.msg import Header +import numpy as np + +from dimos.mapping.occupancy.path_mask import make_path_mask +from dimos.mapping.occupancy.path_resampling import simple_resample_path +from dimos.memory.vis.space.elements import Polyline +from dimos.memory.vis.space.space import Space +from dimos.msgs.occupancy import grid_to_world +from dimos.navigation.replanning_a_star.min_cost_astar import _USE_CPP, min_cost_astar + + +def main() -> None: + if not _USE_CPP: + raise RuntimeError("Build the native A* extension before running this demo") + cells = np.zeros((40, 40), dtype=np.int8) + cells[20, :30] = 100 + header = Header(stamp=Time(sec=1700000000, nanosec=123456789), frame_id="map") + grid = OccupancyGrid( + header=header, + info=MapMetaData( + width=40, height=40, resolution=0.25, origin=Pose(orientation=Quaternion(w=1)) + ), + data=cells.ravel(), + ) + received = OccupancyGrid.decode(grid.encode()) + start = grid_to_world(received, (5, 5)) + goal = grid_to_world(received, (35, 35)) + output_dir = FilePath("build/message-codegen/demo/evidence") + output_dir.mkdir(parents=True, exist_ok=True) + for use_cpp in (False, True): + name = "cpp" if use_cpp else "python" + path = min_cost_astar(received, goal, start, use_cpp=use_cpp) + assert path is not None + resampled = simple_resample_path(path, Pose(orientation=Quaternion(w=1)), 0.1) + decoded = Path.decode(resampled.encode()) + assert decoded.header == header + assert all(pose.header == header for pose in decoded.poses) + mask = make_path_mask(received, decoded, 0.1) + assert mask.any() and not np.any(mask & (cells == 100)) + output = output_dir / f"grid-path-{name}.svg" + output.write_text(Space().base_map(received).add(Polyline(decoded)).to_svg()) + print( + f"{name}: {len(path.poses)} planned points → {len(decoded.poses)} CDR poses; SVG={output}" + ) + print("PASS: both planners route around the wall; source nanoseconds survive resampling/CDR") + + +if __name__ == "__main__": + main() diff --git a/examples/message-codegen/demo_habitat_native.py b/examples/message-codegen/demo_habitat_native.py new file mode 100644 index 00000000000..587be189c71 --- /dev/null +++ b/examples/message-codegen/demo_habitat_native.py @@ -0,0 +1,96 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Offline standalone Habitat encoder check; run with its installed message package.""" + +import importlib.util +import json +from pathlib import Path +import sys + +from dimos_generated.geometry_msgs.msg import Twist +from dimos_generated.nav_msgs.msg import Odometry +from dimos_generated.sensor_msgs.msg import CameraInfo, Image, PointCloud2 +from dimos_generated.tf2_msgs.msg import TFMessage +import numpy as np + +root = Path(__file__).resolve().parents[2] +spec = importlib.util.spec_from_file_location( + "habitat_standalone", root / "dimos/simulation/habitat/server.py" +) +module = importlib.util.module_from_spec(spec) +spec.loader.exec_module(module) +output = root / "build/message-codegen/evidence/habitat-py39" +output.mkdir(parents=True, exist_ok=True) +payloads = { + "image": ( + Image, + module.image_msg(np.full((2, 3, 3), 7, dtype=np.uint8), "rgb8", "camera", 12.25), + ), + "camera_info": ( + CameraInfo, + module.camera_info_msg( + {"width": 3, "height": 2, "fx": 10, "fy": 11, "cx": 1, "cy": 1}, "camera", 12.25 + ), + ), + "cloud": ( + PointCloud2, + module.cloud_msg( + np.array([[1, 2, 3]], dtype=np.float32), + np.array([[255, 128, 64]], dtype=np.uint8), + "world", + 12.25, + ), + ), + "odometry": ( + Odometry, + module.odometry_msg( + np.array([1, 2, 3]), + np.array([0, 0, 0, 1]), + (0.4, 0.2, 0.3), + "world", + "base_link", + 12.25, + ), + ), + "tf": ( + TFMessage, + module.tf_msg( + [ + ("world", "base_link", (1, 2, 3), (0, 0, 0, 1)), + ("base_link", "camera", (0, 0, 0.45), (0, 0, 0, 1)), + ], + 12.25, + ), + ), +} +for name, (message_type, payload) in payloads.items(): + value = message_type.decode(payload) + header = value.transforms[0].header if name == "tf" else value.header + assert header.stamp.sec == 12 and header.stamp.nanosec == 250000000 + (output / (name + ".cdr")).write_bytes(payload) +assert Odometry.decode(payloads["odometry"][1]).pose.pose.position.x == 1 +assert PointCloud2.decode(payloads["cloud"][1]).row_step == 16 +assert TFMessage.decode(payloads["tf"][1]).transforms[1].transform.translation.z == 0.45 +assert Twist.decode(Twist().encode()).linear.x == 0 +print( + json.dumps( + { + "python": sys.version.split()[0], + "standalone_import": True, + "cdr_payloads": sorted(payloads), + "source_stamp": [12, 250000000], + } + ) +) diff --git a/examples/message-codegen/demo_holonomic_path.py b/examples/message-codegen/demo_holonomic_path.py new file mode 100644 index 00000000000..ebcec9fd6dc --- /dev/null +++ b/examples/message-codegen/demo_holonomic_path.py @@ -0,0 +1,103 @@ +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Run the threaded holonomic path follower with a synthetic CDR robot.""" + +import math +import time + +from dimos_generated.geometry_msgs.msg import Point, Pose, PoseStamped, Quaternion, Twist +from dimos_generated.nav_msgs.msg import Path +from dimos_generated.std_msgs.msg import Header + +from dimos.msgs.time import time_from_nanoseconds +from dimos.navigation.dannav.holonomic_tc.module import DanHolonomicTCConfig, _HolonomicPathFollower +from dimos.navigation.dannav.local_planner.module import DanLocalPlannerConfig, _ReplanGate + + +def main() -> None: + core = _HolonomicPathFollower( + DanHolonomicTCConfig(speed_m_s=1.2, control_frequency=60.0, goal_tolerance=0.08) + ) + command = Twist() + stopped: list[str] = [] + + def receive(value: Twist) -> None: + nonlocal command + command = Twist.decode(value.encode()) + + subscription = core.cmd_vel.subscribe(receive) + stop_subscription = core.stopped_navigating.subscribe(stopped.append) + x = y = yaw = 0.0 + dt = 1 / 60 + + def odometry(tick: int) -> PoseStamped: + value = PoseStamped( + header=Header( + frame_id="map", + stamp=time_from_nanoseconds(1700000000000000000 + round(tick * dt * 1e9)), + ), + pose=Pose( + position=Point(x=x, y=y), + orientation=Quaternion(z=math.sin(yaw / 2), w=math.cos(yaw / 2)), + ), + ) + return PoseStamped.decode(value.encode()) + + path = Path( + header=Header(frame_id="map"), + poses=[ + PoseStamped(pose=Pose(position=Point(x=px), orientation=Quaternion(w=1.0))) + for px in (0.1, 1.0) + ], + ) + try: + gate = _ReplanGate( + DanLocalPlannerConfig(lock_replan=0.5, resample_spacing_m=0.1, smoothing_window=3) + ) + gate.on_odom(odometry(0)) + committed = gate.on_planner_path(Path.decode(path.encode())) + assert committed is not None + assert gate.on_planner_path(Path.decode(path.encode())) is None + print( + f"Local planner committed {len(committed.poses)} resampled poses; duplicate replan held" + ) + core.handle_odom(odometry(0)) + core.start_planning(Path.decode(committed.encode())) + for tick in range(300): + if stopped: + break + time.sleep(dt * 1.1) + c, s = math.cos(yaw), math.sin(yaw) + x += (c * command.linear.x - s * command.linear.y) * dt + y += (s * command.linear.x + c * command.linear.y) * dt + yaw += command.angular.z * dt + core.handle_odom(odometry(tick + 1)) + if tick % 30 == 0: + print( + f"position=({x:.3f}, {y:.3f}) decoded speed={math.hypot(command.linear.x, command.linear.y):.3f}m/s" + ) + assert "arrived" in stopped, stopped + assert math.hypot(1 - x, y) < 0.15 + print( + f"Path follower arrived at ({x:.3f}, {y:.3f}); final command={command.linear.x:.3f}m/s" + ) + finally: + core.close() + subscription.dispose() + stop_subscription.dispose() + + +if __name__ == "__main__": + main() diff --git a/examples/message-codegen/demo_holonomic_tracking.py b/examples/message-codegen/demo_holonomic_tracking.py new file mode 100644 index 00000000000..097e7c4d166 --- /dev/null +++ b/examples/message-codegen/demo_holonomic_tracking.py @@ -0,0 +1,59 @@ +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Drive a synthetic holonomic robot to a target using generated CDR messages.""" + +import math + +from dimos_generated.geometry_msgs.msg import Point, Pose, Quaternion, Twist + +from dimos.navigation.dannav.holonomic_tc.command_limits import ( + HolonomicCommandLimits, + clamp_holonomic_cmd_vel, +) +from dimos.navigation.dannav.holonomic_tc.holonomic_tracking_controller import ( + HolonomicTrackingController, +) +from dimos.navigation.dannav.holonomic_tc.types import ( + TrajectoryMeasuredSample, + TrajectoryReferenceSample, +) + + +def main() -> None: + limits = HolonomicCommandLimits(1.0, 1.0, 2.0, 2.0) + controller = HolonomicTrackingController(k_position_per_s=2.0, k_yaw_per_s=1.0) + controller.configure(limits) + target = Pose(position=Point(x=2.0, y=1.0), orientation=Quaternion(w=1.0)) + pose = Pose(orientation=Quaternion(w=1.0)) + command = Twist() + dt = 0.05 + for tick in range(200): + reference = TrajectoryReferenceSample(tick * dt, Pose.decode(target.encode()), Twist()) + measured = TrajectoryMeasuredSample(tick * dt, Pose.decode(pose.encode()), command) + raw = controller.control(reference, measured) + command = Twist.decode(clamp_holonomic_cmd_vel(command, raw, limits, dt).encode()) + pose.position.x += command.linear.x * dt + pose.position.y += command.linear.y * dt + if tick % 40 == 0: + print( + f"t={tick * dt:.1f}s position=({pose.position.x:.3f}, {pose.position.y:.3f}) CDR velocity=({command.linear.x:.3f}, {command.linear.y:.3f})" + ) + error = math.hypot(target.position.x - pose.position.x, target.position.y - pose.position.y) + assert error < 0.001, error + print(f"Reached target (2, 1); position error={error:.8f}m") + + +if __name__ == "__main__": + main() diff --git a/examples/message-codegen/demo_hosted_map.py b/examples/message-codegen/demo_hosted_map.py new file mode 100644 index 00000000000..49d3858c807 --- /dev/null +++ b/examples/message-codegen/demo_hosted_map.py @@ -0,0 +1,61 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Save the hosted operator's PNG and odometry payload from generated CDR inputs.""" + +import base64 +import json +from pathlib import Path + +from dimos_generated.geometry_msgs.msg import Point, Pose, PoseStamped +from dimos_generated.nav_msgs.msg import MapMetaData, OccupancyGrid +import numpy as np + +from dimos.msgs.geometry import quaternion_from_euler +from dimos.teleop.hosted.map_compress import MapCompressModule + + +def main() -> None: + module = MapCompressModule() + payloads = [] + unsubscribe = module.map_out.subscribe(lambda data: payloads.append(json.loads(data))) + try: + cells = np.zeros((64, 64), dtype=np.int8) + cells[:8] = -1 + cells[20:44, 20:44] = 50 + cells[28:36, 28:36] = 100 + grid = OccupancyGrid( + info=MapMetaData(width=64, height=64, resolution=0.1), data=cells.ravel() + ) + module._on_costmap(OccupancyGrid.decode(grid.encode())) + pose = PoseStamped( + pose=Pose(position=Point(x=3.2, y=3.2), orientation=quaternion_from_euler(0, 0, 1.0)) + ) + module._on_odom(PoseStamped.decode(pose.encode())) + assert [value["type"] for value in payloads] == ["map", "odom"] + assert payloads[1]["yaw"] == 1.0 + destination = Path("build/message-codegen/demo/evidence/hosted-map.png") + destination.parent.mkdir(parents=True, exist_ok=True) + destination.write_bytes(base64.b64decode(payloads[0]["png_b64"])) + print(f"CDR occupancy → hosted PNG: {destination}") + print("64 x 64: transparent unknown strip, cyan obstacle square, white lethal center") + print(f"CDR pose → operator marker: {payloads[1]}") + print("Local callbacks only; no hosted connection or hardware required.") + finally: + unsubscribe() + module.stop() + + +if __name__ == "__main__": + main() diff --git a/examples/message-codegen/demo_keyboard_cdr.py b/examples/message-codegen/demo_keyboard_cdr.py new file mode 100644 index 00000000000..51e3723eb47 --- /dev/null +++ b/examples/message-codegen/demo_keyboard_cdr.py @@ -0,0 +1,54 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Exercise keyboard command and release events without a display or robot.""" + +from dimos_generated.geometry_msgs.msg import Twist, TwistStamped +from dimos_generated.std_msgs.msg import Float32 +import pygame + +from dimos.teleop.keyboard.keyboard_teleop_module import KeyboardTeleopModule + + +def main() -> None: + module = KeyboardTeleopModule() + commands: list[TwistStamped] = [] + gripper: list[Float32] = [] + subscriptions = [ + module.ee_twist_command.subscribe( + lambda msg: commands.append(TwistStamped.decode(msg.encode())) + ), + module.gripper_command.subscribe(lambda msg: gripper.append(Float32.decode(msg.encode()))), + ] + try: + held = {pygame.K_w, pygame.K_a} + module._handle_pygame_event(pygame.event.Event(pygame.KEYUP, key=pygame.K_w), held) + assert commands[-1].twist.linear.y == 0.05 + print("Release W while A remains held → CDR velocity: left 0.05 m/s") + module._handle_pygame_event(pygame.event.Event(pygame.KEYUP, key=pygame.K_a), held) + assert commands[-1].twist == Twist() + print("Release final motion key → CDR velocity: all six components zero") + for key in [pygame.K_LEFTBRACKET, pygame.K_RIGHTBRACKET]: + module._handle_pygame_event(pygame.event.Event(pygame.KEYDOWN, key=key), held) + assert [msg.data for msg in gripper] == [1, 0] + print("Gripper [ then ] → generated CDR Float32: open 1.0, closed 0.0") + print("Synthetic pygame events; no window, keyboard capture, or hardware motion.") + finally: + for unsubscribe in subscriptions: + unsubscribe() + module.stop() + + +if __name__ == "__main__": + main() diff --git a/examples/message-codegen/demo_lidar.cpp b/examples/message-codegen/demo_lidar.cpp new file mode 100644 index 00000000000..17bab0281b6 --- /dev/null +++ b/examples/message-codegen/demo_lidar.cpp @@ -0,0 +1,43 @@ +// Copyright 2026 Dimensional Inc. +// SPDX-License-Identifier: Apache-2.0 + +#include +#include +#include +#include + +#include "../../dimos/hardware/sensors/lidar/common/point_cloud_utils.hpp" + +int main() { + auto cloud = dimos::make_xyzi_cloud("lidar", 1.25, 2); + for (int i = 0; i < 2; ++i) { + float* point = dimos::xyzi_point(cloud, i); + point[0] = static_cast(i + 1); + point[1] = 2; + point[2] = 3; + point[3] = 0.5; + } + for (bool little : {true, false}) { + auto bytes = dimos::cdr::encode(cloud, little); + auto decoded = dimos::cdr::decode(bytes); + assert(decoded == cloud); + assert(decoded.header.stamp.sec == 1 && decoded.header.stamp.nanosec == 250000000); + assert(decoded.width == 2 && decoded.fields.size() == 4 && decoded.data.size() == 32); + std::cout << (little ? "little" : "big") << "-endian CDR: " + << decoded.width << " XYZI points, frame=" << decoded.header.frame_id + << ", stamp=" << decoded.header.stamp.sec << "s + " + << decoded.header.stamp.nanosec << "ns, intensity=" + << dimos::xyzi_point(decoded, 0)[3] << '\n'; + } + auto negative = dimos::time_from_seconds(-0.25); + assert(negative.sec == -1 && negative.nanosec == 750000000); + bool rejected = false; + try { dimos::time_from_seconds(std::numeric_limits::infinity()); } + catch (const std::invalid_argument&) { rejected = true; } + assert(rejected); + rejected = false; + try { dimos::make_xyzi_cloud("lidar", 0, -1); } + catch (const std::invalid_argument&) { rejected = true; } + assert(rejected); + std::cout << "Negative timestamps normalize; invalid timestamps and negative point counts fail.\n"; +} diff --git a/examples/message-codegen/demo_live_bridge.py b/examples/message-codegen/demo_live_bridge.py new file mode 100644 index 00000000000..7edbdcfa452 --- /dev/null +++ b/examples/message-codegen/demo_live_bridge.py @@ -0,0 +1,122 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Send a generated image over LCM or Zenoh into a running headless Rerun bridge.""" + +import argparse +from pathlib import Path +import socket +from threading import Event +from typing import Any +from uuid import uuid4 + +from dimos_generated.sensor_msgs.msg import Image +from dimos_generated.std_msgs.msg import Header +import numpy as np +import rerun as rr + +from dimos.core.global_config import GlobalConfig +from dimos.msgs.image import image_from_array +from dimos.protocol.pubsub.impl.lcmpubsub import LCM, Topic +from dimos.protocol.pubsub.impl.zenohpubsub import Zenoh +from dimos.protocol.service.zenohservice import ZenohSessionPool +from dimos.visualization.rerun.bridge import RerunBridgeModule + + +class ObservedBridge(RerunBridgeModule): + def __init__(self, expected_topic: str, **kwargs: Any) -> None: + super().__init__(**kwargs) + self.expected_topic = expected_topic + self.received = Event() + + def _on_message(self, msg: Any, topic: Any) -> None: + super()._on_message(msg, topic) + if getattr(topic, "topic", None) == self.expected_topic and isinstance(msg, Image): + assert msg.width == 160 and msg.height == 120 + assert msg.header.frame_id == "camera_optical" + self.received.set() + + +def main() -> None: + parser = argparse.ArgumentParser() + parser.add_argument("--transport", choices=("lcm", "zenoh"), default="lcm") + backend = parser.parse_args().transport + with socket.socket() as sock: + sock.bind(("127.0.0.1", 0)) + port = sock.getsockname()[1] + channel = ("dimos/vb/" if backend == "zenoh" else "/vb/") + uuid4().hex[:8] + pools = [ZenohSessionPool(), ZenohSessionPool()] + if backend == "zenoh": + with socket.socket() as sock: + sock.bind(("127.0.0.1", 0)) + endpoint = f"tcp/127.0.0.1:{sock.getsockname()[1]}" + receiver = Zenoh( + session_pool=pools[0], + listen=[endpoint], + connect=[], + scouting=False, + multicast=False, + gossip=False, + ) + sender = Zenoh( + session_pool=pools[1], + listen=[], + connect=[endpoint], + scouting=False, + multicast=False, + gossip=False, + ) + else: + receiver = LCM() + sender = LCM() + bridge = ObservedBridge( + channel, + g=GlobalConfig(transport=backend), + pubsubs=[receiver], + rerun_open="none", + rerun_web=False, + connect_url=f"rerun+http://127.0.0.1:{port}/proxy", + ) + output = Path(f"build/message-codegen/demo/evidence/live-bridge-{backend}.rrd") + output.parent.mkdir(parents=True, exist_ok=True) + try: + bridge.start() + rr.save(str(output)) + sender.start() + pixels = np.zeros((120, 160, 3), dtype=np.uint8) + pixels[:, :80, 0] = 255 + pixels[:, 80:, 2] = 255 + message = image_from_array( + pixels, encoding="rgb8", header=Header(frame_id="camera_optical") + ) + for _ in range(10): + sender.publish(Topic(channel, Image), message) + if bridge.received.wait(0.2): + break + assert bridge.received.is_set(), ( + f"bridge did not receive the generated image over {backend}" + ) + print(f"{backend.upper()} CDR image received and rendered on {channel}: 160x120 rgb8") + finally: + bridge.stop() + sender.stop() + rr.disconnect() + for pool in pools: + pool.close_all() + assert output.stat().st_size > 0 + print(f"Headless live bridge recording: {output}") + + +if __name__ == "__main__": + main() diff --git a/examples/message-codegen/demo_marker_messages.py b/examples/message-codegen/demo_marker_messages.py new file mode 100644 index 00000000000..90e97b3c1d0 --- /dev/null +++ b/examples/message-codegen/demo_marker_messages.py @@ -0,0 +1,63 @@ +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Show generated CDR marker identity and exact image-derived timestamps.""" + +from dimos_generated.builtin_interfaces.msg import Time +from dimos_generated.geometry_msgs.msg import Vector3 +from dimos_generated.std_msgs.msg import Header +from dimos_generated.vision_msgs.msg import Detection3DArray +import numpy as np + +from dimos.msgs.image import image_from_array +from dimos.perception.detection.type.detection3d.imageDetections3D import ImageDetections3D +from dimos.perception.detection.type.detection3d.marker import Detection3DMarker + + +def main() -> None: + image = image_from_array( + np.zeros((80, 100, 3), dtype=np.uint8), + encoding="bgr8", + header=Header(frame_id="camera", stamp=Time(sec=1700000000, nanosec=123456789)), + ) + markers = [ + Detection3DMarker( + bbox=(10, 10, 30, 30), + track_id=-1, + class_id=identifier, + confidence=1, + name="", + ts=0, + image=image, + center=Vector3(x=float(identifier), y=2, z=3), + size=Vector3(x=0.16, y=0.16), + frame_id="world", + marker_id=identifier, + dictionary="DICT_APRILTAG_36h11", + ) + for identifier in (7, 42) + ] + message = ImageDetections3D(image, markers).to_ros_detection3d_array() + decoded = Detection3DArray.decode(message.encode()) + assert decoded.header.stamp == image.header.stamp + for marker in decoded.detections: + assert marker.results[0].hypothesis.class_id == f"DICT_APRILTAG_36h11:{marker.id}" + print( + f"Marker {marker.id}: {marker.results[0].hypothesis.class_id}, world x={marker.bbox.center.position.x:.1f} m" + ) + print("PASS: generated marker array retains identity and 1700000000123456789 ns stamp") + + +if __name__ == "__main__": + main() diff --git a/examples/message-codegen/demo_mcp_observe.py b/examples/message-codegen/demo_mcp_observe.py new file mode 100644 index 00000000000..1bc428b2311 --- /dev/null +++ b/examples/message-codegen/demo_mcp_observe.py @@ -0,0 +1,72 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Deliver a generated camera frame over LCM and save the MCP observe image.""" + +import asyncio +import base64 +from pathlib import Path +from threading import Event, Thread +from uuid import uuid4 + +from dimos_generated.sensor_msgs.msg import Image +import numpy as np + +from dimos.agents.mcp.mcp_server import handle_request +from dimos.agents.skills.observe_skill import ObserveSkill +from dimos.core.transport import LCMTransport +from dimos.msgs.image import image_from_array + + +def main() -> None: + module = ObserveSkill() + module.color_image.transport = LCMTransport(f"/observe/{uuid4().hex[:8]}", Image) + pixels = np.zeros((120, 160, 3), dtype=np.uint8) + pixels[:, :80, 0] = 255 + pixels[:, 80:, 1] = 255 + frame = image_from_array(pixels, encoding="rgb8") + stop = Event() + + def publish() -> None: + while not stop.is_set(): + module.color_image.transport.publish(frame) + stop.wait(0.05) + + thread = Thread(target=publish, daemon=True) + try: + thread.start() + response = asyncio.run( + handle_request( + {"method": "tools/call", "id": 1, "params": {"name": "observe"}}, + [], + {"observe": module.observe}, + ) + ) + assert response is not None + image = response["result"]["content"][0] + assert image["type"] == "image" and image["mimeType"] == "image/jpeg" + destination = Path("build/message-codegen/demo/evidence/mcp-observe.jpg") + destination.parent.mkdir(parents=True, exist_ok=True) + destination.write_bytes(base64.b64decode(image["data"])) + print(f"LCM CDR camera → observe skill → MCP JPEG image: {destination}") + print("Expected image: red left half, green right half, 160 x 120 pixels") + print("Real LCM delivery; MCP handler invoked directly, no model inference.") + finally: + stop.set() + thread.join(timeout=3) + module.stop() + + +if __name__ == "__main__": + main() diff --git a/examples/message-codegen/demo_message_helpers.py b/examples/message-codegen/demo_message_helpers.py new file mode 100644 index 00000000000..c84834403d7 --- /dev/null +++ b/examples/message-codegen/demo_message_helpers.py @@ -0,0 +1,60 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Inspect generated DimOS messages with explicit timestamp and duration helpers.""" + +from pathlib import Path + +from dimos_generated.dimos_msgs.msg import LineSegment3D, LineSegments3D, TrajectoryStatus +from dimos_generated.geometry_msgs.msg import Point +from dimos_generated.sensor_msgs.msg import CameraInfo +from dimos_generated.std_msgs.msg import Header + +from dimos.msgs.camera_info import camera_info_from_yaml, intrinsic_matrix +from dimos.msgs.time import ( + duration_from_seconds, + time_from_nanoseconds, + to_nanoseconds, + to_seconds, +) + + +def main() -> None: + message = LineSegments3D(segments=[LineSegment3D(start=Point(x=1), end=Point(y=2), weight=4)]) + message.header.stamp = time_from_nanoseconds(1_700_000_000_123_456_789) + message.header.frame_id = "map" + decoded = LineSegments3D.decode(message.encode()) + print(f"{decoded.msg_name}: frame={decoded.header.frame_id}") + print(f"Exact source nanoseconds: {to_nanoseconds(decoded.header.stamp)}") + segment = decoded.segments[0] + print(f"Segment: start.x={segment.start.x}, end.y={segment.end.y}, weight={segment.weight}") + status = TrajectoryStatus( + state=TrajectoryStatus.EXECUTING, time_remaining=duration_from_seconds(1.25) + ) + remaining = TrajectoryStatus.decode(status.encode()).time_remaining + print(f"Trajectory remaining: sec={remaining.sec}, nanosec={remaining.nanosec}") + print(f"Seconds for a controller API: {to_seconds(remaining)}") + calibration = camera_info_from_yaml( + Path(__file__).resolve().parents[2] / "dimos/robot/unitree/go2/front_camera_720.yaml", + header=Header(frame_id="camera_optical", stamp=message.header.stamp), + ) + restored = CameraInfo.decode(calibration.encode()) + print(f"Camera calibration: {restored.width}x{restored.height}, {restored.distortion_model}") + print(f"Camera source nanoseconds: {to_nanoseconds(restored.header.stamp)}") + print(f"Intrinsic matrix (independent NumPy copy):\n{intrinsic_matrix(restored)}") + print(f"Distortion coefficients: {list(restored.d)}") + + +if __name__ == "__main__": + main() diff --git a/examples/message-codegen/demo_mls_transformer.py b/examples/message-codegen/demo_mls_transformer.py new file mode 100644 index 00000000000..548517a90f8 --- /dev/null +++ b/examples/message-codegen/demo_mls_transformer.py @@ -0,0 +1,156 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Plan across synthetic terrain using the native MLS backend and CDR messages.""" + +from pathlib import Path as FilePath +from types import SimpleNamespace + +from dimos_generated.builtin_interfaces.msg import Time +from dimos_generated.dimos_msgs.msg import LineSegment3D, LineSegments3D +from dimos_generated.geometry_msgs.msg import ( + Point, + PoseStamped, + Quaternion, + Transform, + TransformStamped, + Vector3, +) +from dimos_generated.nav_msgs.msg import MapMetaData, OccupancyGrid, Path +from dimos_generated.sensor_msgs.msg import PointCloud2 +from dimos_generated.std_msgs.msg import Header +from dimos_generated.tf2_msgs.msg import TFMessage +import numpy as np +import rerun as rr + +from dimos.mapping.ray_tracing.transformer import RayTraceMap +from dimos.memory.type.observation import Observation +from dimos.msgs.pointcloud import pointcloud_from_xyz +from dimos.navigation.nav_3d.mls_planner.start_relay import StartRelay +from dimos.navigation.nav_3d.mls_planner.transformer import MLSPlan +from dimos.navigation.nav_3d.mls_planner.viz import ( + render_node_edges, + render_nodes, + render_surface_map, +) +from dimos.protocol.tf.tf import MultiTBuffer +from dimos.visualization.rerun.bridge import RerunBridgeModule +from dimos.visualization.rerun.message_helpers import register_colormap_annotation + + +def main() -> None: + coords = np.arange(-3, 3, 0.1, dtype=np.float32) + x, y = np.meshgrid(coords, coords) + points = np.stack([x.ravel(), y.ravel(), np.zeros(x.size, dtype=np.float32)], axis=1) + cloud = pointcloud_from_xyz( + points, header=Header(frame_id="world", stamp=Time(sec=1700000000, nanosec=123456789)) + ) + tf = MultiTBuffer() + edge = TransformStamped( + header=cloud.header, + child_frame_id="base_link", + transform=Transform(translation=Vector3(x=-2.0, y=-2.0, z=1.0), rotation=Quaternion(w=1.0)), + ) + tf.receive_transform(TransformStamped.decode(edge.encode())) + relay = StartRelay(world_frame="world") + relay._tf = tf + poses: list[PoseStamped] = [] + unsubscribe = relay.start_pose.subscribe( + lambda value: poses.append(PoseStamped.decode(value.encode())) + ) + try: + relay._on_tf(TFMessage(transforms=[edge])) + assert len(poses) == 1 + print( + f"TF relay start: ({poses[0].pose.position.x}, {poses[0].pose.position.y}, {poses[0].pose.position.z})" + ) + finally: + unsubscribe() + relay._tf = None + relay.stop() + obs = Observation( + id=0, + ts=0.0, + pose=poses[0], + tags={"region_bounds": (0.0, 0.0, 5.0, -1.0, 2.0)}, + _data=PointCloud2.decode(cloud.encode()), + ) + ray_input = obs.derive(data=cloud, pose_tuple=(0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0)) + [mapped] = list(RayTraceMap(voxel_size=0.2)(iter([ray_input]))) + mapped_cloud = PointCloud2.decode(mapped.data.encode()) + assert mapped_cloud.header.stamp.nanosec == 123456789 + print( + f"Ray-traced CDR local map: {mapped_cloud.width * mapped_cloud.height} points, frame={mapped_cloud.header.frame_id}" + ) + [result] = list(MLSPlan(goal=(2.0, 2.0, 0.0), voxel_size=0.2, robot_height=1.0)(iter([obs]))) + path = Path.decode(result.data.encode()) + assert result.tags["planned"] and len(path.poses) >= 2 + assert path.header.stamp.nanosec == 123456789 + print( + f"CDR terrain: {len(points)} points → {result.tags['voxels']} voxels → {len(path.poses)} path poses" + ) + print(f"Header: {path.header.frame_id} {path.header.stamp.sec}.{path.header.stamp.nanosec:09d}") + output = FilePath("build/message-codegen/demo/evidence/mls-planner.rrd") + output.parent.mkdir(parents=True, exist_ok=True) + rr.init("generated-mls-planner", spawn=False) + rr.save(str(output)) + register_colormap_annotation() + bridge = RerunBridgeModule() + bridge._min_intervals = {} + try: + bridge._on_message( + TFMessage.decode(TFMessage(transforms=[edge]).encode()), SimpleNamespace(name="/tf") + ) + finally: + bridge.stop() + bridge = RerunBridgeModule() + bridge._min_intervals = {} + try: + bridge._on_message(PointCloud2.decode(cloud.encode()), SimpleNamespace(name="/terrain")) + bridge._on_message(Path.decode(path.encode()), SimpleNamespace(name="/planned_path")) + grid = OccupancyGrid( + header=path.header, + info=MapMetaData(width=2, height=2, resolution=1.0, origin=poses[0].pose), + data=[0, 100, -1, 50], + ) + bridge._on_message(OccupancyGrid.decode(grid.encode()), SimpleNamespace(name="/occupancy")) + finally: + bridge.stop() + rr.log("world/ray_map", render_surface_map(mapped_cloud)) + nodes = pointcloud_from_xyz( + np.array([[p.pose.position.x, p.pose.position.y, p.pose.position.z] for p in path.poses]), + header=path.header, + ) + rr.log("world/path_nodes", render_nodes(nodes)) + segments = LineSegments3D( + header=path.header, + segments=[ + LineSegment3D( + start=Point(x=a.pose.position.x, y=a.pose.position.y, z=a.pose.position.z), + end=Point(x=b.pose.position.x, y=b.pose.position.y, z=b.pose.position.z), + ) + for a, b in zip(path.poses, list(path.poses)[1:], strict=False) + ], + ) + rr.log("world/path_edges", render_node_edges(LineSegments3D.decode(segments.encode()))) + rr.disconnect() + assert output.stat().st_size > 0 + print(f"Rerun recording: {output}") + for index, pose in enumerate(path.poses): + p = pose.pose.position + print(f" waypoint {index}: ({p.x:.2f}, {p.y:.2f}, {p.z:.2f})") + + +if __name__ == "__main__": + main() diff --git a/examples/message-codegen/demo_movement.py b/examples/message-codegen/demo_movement.py new file mode 100644 index 00000000000..78dc166b23f --- /dev/null +++ b/examples/message-codegen/demo_movement.py @@ -0,0 +1,57 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Show generated CDR click forwarding and teleop/navigation arbitration.""" + +from dimos_generated.builtin_interfaces.msg import Time +from dimos_generated.geometry_msgs.msg import Point, PointStamped, Twist, Vector3 +from dimos_generated.std_msgs.msg import Header + +from dimos.navigation.movement_manager.movement_manager import MovementManager + + +def main() -> None: + manager = MovementManager(tele_cooldown_sec=10) + goals: list[PointStamped] = [] + commands: list[Twist] = [] + unsubs = [ + manager.goal.subscribe(lambda msg: goals.append(PointStamped.decode(msg.encode()))), + manager.cmd_vel.subscribe(lambda msg: commands.append(Twist.decode(msg.encode()))), + ] + try: + clicked = PointStamped( + header=Header(frame_id="map", stamp=Time(sec=1700000000, nanosec=123456789)), + point=Point(x=3, y=4), + ) + manager._on_click(PointStamped.decode(clicked.encode())) + assert goals == [clicked] + print("Click → CDR goal: (3, 4), map, 1700000000123456789 ns") + manager._on_nav(Twist(linear=Vector3(x=0.2))) + manager._on_teleop(Twist(linear=Vector3(x=0.5))) + manager._on_nav(Twist(linear=Vector3(x=0.9))) + assert [msg.linear.x for msg in commands] == [0.2, 0.5] + print("Navigation: 0.2 m/s; teleop: 0.5 m/s; competing navigation suppressed") + manager.config.tele_cooldown_sec = 0 + manager._on_nav(Twist(linear=Vector3(x=0.1))) + assert commands[-1].linear.x == 0.1 + print("Cooldown elapsed → navigation resumes at 0.1 m/s") + print("PASS: generated click and velocity messages cross CDR output boundaries") + finally: + for unsub in unsubs: + unsub() + manager.stop() + + +if __name__ == "__main__": + main() diff --git a/examples/message-codegen/demo_mujoco_lidar_shm.py b/examples/message-codegen/demo_mujoco_lidar_shm.py new file mode 100644 index 00000000000..a1ab75646af --- /dev/null +++ b/examples/message-codegen/demo_mujoco_lidar_shm.py @@ -0,0 +1,80 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Exercise the simulation lidar IPC boundary without running MuJoCo or a robot.""" + +import argparse +import json +from pathlib import Path +import subprocess +import sys + +from dimos_generated.std_msgs.msg import Header +import numpy as np + +from dimos.msgs.pointcloud import pointcloud_from_xyz, pointcloud_xyz +from dimos.msgs.time import time_from_nanoseconds, to_nanoseconds +from dimos.simulation.mujoco.shared_memory import ShmReader, ShmWriter + +STAMP = 1_700_000_000_123_456_789 + + +def produce(names: dict[str, str]) -> None: + producer = ShmReader(names) + try: + points = np.array([[1.25, -2.5, 3.75], [4, 5, 6]], dtype=np.float32) + message = pointcloud_from_xyz( + points, header=Header(stamp=time_from_nanoseconds(STAMP), frame_id="world") + ) + producer.write_lidar(message) + finally: + producer.cleanup() + + +def main() -> None: + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument("--producer-names", help=argparse.SUPPRESS) + args = parser.parse_args() + if args.producer_names: + produce(json.loads(args.producer_names)) + return + + consumer = ShmWriter() + try: + subprocess.run( + [ + sys.executable, + str(Path(__file__).resolve()), + "--producer-names", + json.dumps(consumer.shm.to_names()), + ], + check=True, + timeout=30, + ) + message, sequence = consumer.read_lidar() + assert message is not None + assert sequence > 0 + assert message.header.frame_id == "world" + assert to_nanoseconds(message.header.stamp) == STAMP + coordinates = pointcloud_xyz(message).tolist() + assert coordinates == [[1.25, -2.5, 3.75], [4, 5, 6]] + print(f"Separate process → shared memory → generated CDR decoder: {coordinates}") + print(f"frame={message.header.frame_id}, stamp={STAMP}, sequence={sequence}") + print("PASS: no ROS installation, MuJoCo engine, or robot required") + finally: + consumer.cleanup() + + +if __name__ == "__main__": + main() diff --git a/examples/message-codegen/demo_native.py b/examples/message-codegen/demo_native.py new file mode 100644 index 00000000000..5692d767480 --- /dev/null +++ b/examples/message-codegen/demo_native.py @@ -0,0 +1,193 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +# Copyright 2026 Dimensional Inc. +# SPDX-License-Identifier: Apache-2.0 + +"""Run Python → C++ → Rust → Python through the native SDKs on LCM and Zenoh.""" + +import argparse +from contextlib import ExitStack +import json +import os +from pathlib import Path +import socket +import subprocess +import threading +from typing import Any +from urllib.parse import urlsplit +import uuid + +from dimos_generated.dimos_msgs.msg import LineSegment3D, LineSegments3D +from dimos_generated.geometry_msgs.msg import Point +from dimos_generated.sensor_msgs.msg import Image +import numpy as np + +from dimos.core.transport import LCMTransport, PubSubTransport, ZenohTransport +from dimos.msgs.time import time_from_nanoseconds, to_nanoseconds +from dimos.protocol.service.lcmservice import LCMConfig +from dimos.protocol.service.zenohservice import ZenohConfig, ZenohSessionPool + + +def free_port(kind: socket.SocketKind) -> int: + with socket.socket(socket.AF_INET, kind) as listener: + listener.bind(("127.0.0.1", 0)) + return int(listener.getsockname()[1]) + + +def stop_process(process: subprocess.Popen[bytes]) -> None: + if process.poll() is None: + process.terminate() + try: + process.wait(timeout=5) + except subprocess.TimeoutExpired: + process.kill() + process.wait(timeout=5) + + +def exchange_native( + backend: str, cpp: Path, rust: Path, evidence: Path, messages: dict[str, Any] +) -> dict[str, Any]: + """Launch two native SDK relays, exchange named ports, and close all resources.""" + base = f"demo/{uuid.uuid4().hex[:8]}" + endpoint = f"tcp/127.0.0.1:{free_port(socket.SOCK_STREAM)}" + group = urlsplit(LCMConfig().url).hostname + lcm_url = f"udpm://{group}:{free_port(socket.SOCK_DGRAM)}?ttl=0&recv_buf_size=4194304" + with ExitStack() as stack: + pool = ZenohSessionPool() + stack.callback(pool.close_all) + channels: dict[str, list[PubSubTransport[Any]]] = {} + for channel_index, (name, message) in enumerate(messages.items()): + channels[name] = [] + for index in range(3): + topic = f"{base}/{channel_index}{index}" + peer: PubSubTransport[Any] + if backend == "lcm": + peer = LCMTransport(topic, type(message), url=lcm_url) + else: + peer = ZenohTransport( + topic, + type(message), + session_pool=pool, + scouting=False, + multicast=False, + gossip=False, + listen=[endpoint], + connect=[], + ) + stack.callback(peer.stop) + peer.start() + channels[name].append(peer) + received: dict[str, list[Any]] = {name: [] for name in messages} + ready = {name: threading.Event() for name in received} + for name, peers in channels.items(): + + def receive(value: Any, name: str = name) -> None: + received[name].append(value) + ready[name].set() + + stack.callback(peers[-1].subscribe(receive)) + native_session = ZenohConfig( + mode="client", + connect=[endpoint], + listen=[], + multicast=False, + gossip=False, + scouting=False, + connect_timeout=10, + ).to_wire() + processes = [] + for index, executable in enumerate((cpp, rust)): + log = stack.enter_context((evidence / f"native-{backend}-{index}.log").open("wb")) + process = subprocess.Popen( + [str(executable.resolve())], + stdin=subprocess.PIPE, + stdout=log, + stderr=subprocess.STDOUT, + env={ + **os.environ, + "DIMOS_TRANSPORT": backend, + "LCM_DEFAULT_URL": lcm_url, + "RUST_LOG": "info", + }, + ) + stack.callback(stop_process, process) + processes.append(process) + topics = { + f"{name}_{direction}": peers[index + offset].channel + for name, peers in channels.items() + for direction, offset in (("in", 0), ("out", 1)) + } + launch = { + "topics": topics, + "config": None, + "session": native_session if backend == "zenoh" else None, + } + assert process.stdin is not None + process.stdin.write(json.dumps(launch).encode() + b"\n") + process.stdin.close() + for name, message in messages.items(): + for _ in range(60): + assert all(p.poll() is None for p in processes), ( + f"Native process exited; see {evidence}" + ) + channels[name][0].broadcast(None, message) + if ready[name].wait(0.25): + break + assert received[name], f"No {backend} {name} reply; see {evidence}" + return {name: values[0] for name, values in received.items()} + + +def demonstrate(backend: str, cpp: Path, rust: Path, evidence: Path) -> None: + stamp = 1_700_000_000_123_456_789 + lines = LineSegments3D(segments=[LineSegment3D(start=Point(x=1), end=Point(y=2), weight=4)]) + lines.header.frame_id = "map" + lines.header.stamp = time_from_nanoseconds(stamp) + image = Image( + width=640, + height=480, + encoding="rgb8", + step=1920, + data=np.arange(640 * 480 * 3, dtype=np.uint8), + ) + image.header.frame_id = "camera" + image.header.stamp = time_from_nanoseconds(stamp) + received = exchange_native(backend, cpp, rust, evidence, {"lines": lines, "image": image}) + decoded_lines = received["lines"] + assert decoded_lines.segments[0].weight == 6 + assert to_nanoseconds(decoded_lines.header.stamp) == stamp + assert decoded_lines.header.frame_id == "map" + assert decoded_lines.segments[0].start.x == 1 + assert decoded_lines.segments[0].end.y == 2 + assert received["image"].encode() == image.encode() + print(f"{backend}: Python weight=4 → C++ weight=5 → Rust weight=6 → Python verified") + print(f"{backend}: {len(image.data):,} image bytes and source nanoseconds={stamp} match") + + +def main() -> None: + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument("--backend", choices=["lcm", "zenoh", "all"], default="all") + parser.add_argument("--cpp", type=Path, default=Path("build/native-cpp-examples/cdr_relay")) + parser.add_argument("--rust", type=Path, default=Path("target/debug/cdr_relay")) + parser.add_argument( + "--evidence", type=Path, default=Path("build/message-codegen/demo/evidence") + ) + args = parser.parse_args() + args.evidence.mkdir(parents=True, exist_ok=True) + for backend in ("lcm", "zenoh") if args.backend == "all" else (args.backend,): + demonstrate(backend, args.cpp, args.rust, args.evidence) + + +if __name__ == "__main__": + main() diff --git a/examples/message-codegen/demo_native_recording.py b/examples/message-codegen/demo_native_recording.py new file mode 100644 index 00000000000..9e433be7bb4 --- /dev/null +++ b/examples/message-codegen/demo_native_recording.py @@ -0,0 +1,158 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Record an independently generated custom type with an unchanged Rust binary.""" + +import argparse +from contextlib import ExitStack +import json +import os +from pathlib import Path +import socket +import subprocess +import threading +from typing import Any +import uuid + +from demo_native import free_port, stop_process +from external_telemetry.demo_msgs.msg import Telemetry +from mcap.reader import make_reader + +from dimos.core.transport import LCMTransport, ZenohTransport +from dimos.experimental.memory.rust_recorder import RustStreamSpec +from dimos.protocol.service.zenohservice import ZenohConfig, ZenohSessionPool + + +def record(backend: str, executable: Path, output: Path) -> None: + endpoint = f"tcp/127.0.0.1:{free_port(socket.SOCK_STREAM)}" + url = f"udpm://239.255.76.67:{free_port(socket.SOCK_DGRAM)}?ttl=0" + ready, written = threading.Event(), threading.Event() + transcript: list[str] = [] + with ExitStack() as stack: + pool = ZenohSessionPool() + stack.callback(pool.close_all) + topic = f"demo/{uuid.uuid4().hex[:8]}" + publisher: Any + if backend == "lcm": + publisher = LCMTransport(topic, Telemetry, url=url) + else: + publisher = ZenohTransport( + topic, + Telemetry, + session_pool=pool, + scouting=False, + multicast=False, + gossip=False, + listen=[endpoint], + connect=[], + ) + stack.callback(publisher.stop) + publisher.start() + spec = RustStreamSpec.from_type( + port="telemetry", name="telemetry", payload_type=Telemetry, codec="cdr" + ) + session = ZenohConfig( + mode="client", + connect=[endpoint], + listen=[], + multicast=False, + gossip=False, + scouting=False, + connect_timeout=10, + ).to_wire() + process = stack.enter_context( + subprocess.Popen( + [str(executable.resolve())], + stdin=subprocess.PIPE, + stdout=subprocess.PIPE, + stderr=subprocess.STDOUT, + env={ + **os.environ, + "DIMOS_TRANSPORT": backend, + "LCM_DEFAULT_URL": url, + "RUST_LOG": "info,dimos_memory_recorder=debug", + }, + ) + ) + + def read_logs() -> None: + assert process.stdout is not None + for raw in process.stdout: + line = raw.decode(errors="replace") + transcript.append(line) + if "memory recorder ready" in line: + ready.set() + if "memory recorder batch written" in line: + written.set() + + reader = threading.Thread(target=read_logs, name="recorder-demo-logs") + reader.start() + stack.callback(reader.join, 5) + stack.callback(stop_process, process) + assert process.stdin is not None + launch = { + "topics": {"telemetry": publisher.channel}, + "session": session if backend == "zenoh" else {}, + "config": { + "store": {"kind": "mcap", "path": str(output.resolve())}, + "encoding_threads": 2, + "streams": [spec.model_dump()], + }, + } + process.stdin.write(json.dumps(launch).encode() + b"\n") + process.stdin.close() + assert ready.wait(15), "".join(transcript) + expected = [] + for index in range(3): + message = Telemetry(application_note=f"locally-added-field-{index}") + message.header.stamp.sec = 1700000000 + message.header.stamp.nanosec = 123456789 + index + expected.append(message) + written.clear() + publisher.publish(message) + assert written.wait(5), "".join(transcript) + stop_process(process) + assert process.returncode == 0, "".join(transcript) + with output.open("rb") as stream: + records = list(make_reader(stream).iter_messages()) + assert len(records) == len(expected) + for (schema, channel, row), original in zip(records, expected, strict=True): + assert schema is not None and schema.encoding == "ros2msg" + assert b"application_note" in schema.data + assert channel.message_encoding == "cdr" + assert row.publish_time == row.log_time # Unknown stamp layout uses reception time. + decoded = Telemetry.decode(row.data) + assert decoded == original + print( + f"{backend}: {decoded.application_note}, payload stamp={decoded.header.stamp.sec}.{decoded.header.stamp.nanosec:09d}" + ) + output.with_suffix(".log").write_text("".join(transcript)) + print(f"Recorded {len(records)} independently generated messages with full schema: {output}") + + +def main() -> None: + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument( + "--executable", type=Path, default=Path("target/debug/dimos-memory-recorder") + ) + parser.add_argument("--output", type=Path, default=Path("build/message-codegen/demo/evidence")) + args = parser.parse_args() + args.output.mkdir(parents=True, exist_ok=True) + for backend in ("lcm", "zenoh"): + record(backend, args.executable, args.output / f"external-native-recording-{backend}.mcap") + print("PASS: custom field recorded on both transports without a recorder decoder or rebuild") + + +if __name__ == "__main__": + main() diff --git a/examples/message-codegen/demo_navigation.py b/examples/message-codegen/demo_navigation.py new file mode 100644 index 00000000000..12e747ebe05 --- /dev/null +++ b/examples/message-codegen/demo_navigation.py @@ -0,0 +1,178 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Drive the real navigation module to arrival through generated LCM/Zenoh streams.""" + +from contextlib import ExitStack +import math +from pathlib import Path as FilePath +import socket +from threading import Event, Lock +from typing import Any +import uuid + +from demo_pubsub import free_port +from dimos_generated.builtin_interfaces.msg import Time +from dimos_generated.geometry_msgs.msg import Point, PointStamped, Pose, Quaternion, Twist +from dimos_generated.nav_msgs.msg import OccupancyGrid, Odometry, Path +from dimos_generated.sensor_msgs.msg import PointCloud2 +from dimos_generated.std_msgs.msg import Bool, Header +import numpy as np + +from dimos.core.transport import LCMTransport, ZenohTransport +from dimos.mapping.costmapper import CostMapper +from dimos.mapping.pointclouds.occupancy import GeneralOccupancyConfig +from dimos.mapping.voxels.grid import VoxelGrid +from dimos.memory.vis.space.elements import Polyline +from dimos.memory.vis.space.space import Space +from dimos.msgs.pointcloud import pointcloud_from_xyz +from dimos.navigation.replanning_a_star.module import ReplanningAStarPlanner +from dimos.protocol.service.zenohservice import ZenohSessionPool +from dimos.utils.testing.waiting import wait_until + + +def demonstrate(backend: str) -> None: + with ExitStack() as stack: + pools = [ZenohSessionPool(), ZenohSessionPool()] + for pool in pools: + stack.callback(pool.close_all) + endpoint = f"tcp/127.0.0.1:{free_port(socket.SOCK_STREAM)}" + url = f"udpm://239.255.76.67:{free_port(socket.SOCK_DGRAM)}?ttl=0" + prefix = f"n_{uuid.uuid4().hex[:6]}" + + def transport(name: str, message_type: type, peer: int) -> Any: + topic = f"{prefix}/{name}" + if backend == "lcm": + result: Any = LCMTransport(topic, message_type, url=url) + else: + result = ZenohTransport( + topic, + message_type, + session_pool=pools[peer], + scouting=False, + multicast=False, + gossip=False, + listen=[endpoint] if peer == 0 else [], + connect=[] if peer == 0 else [endpoint], + ) + result.start() + stack.callback(result.stop) + return result + + module = ReplanningAStarPlanner() + module._planner._local_planner._control_frequency = 100 + for name in ( + "odom", + "odometry", + "global_costmap", + "goal_request", + "clicked_point", + "target", + "stop_movement", + "goal_reached", + "navigation_state", + "nav_cmd_vel", + "path", + "navigation_costmap", + ): + port = getattr(module, name) + port.transport = transport(name, port.type, 0) + odometry = transport("odometry", Odometry, 1) + clouds = transport("global_map", PointCloud2, 1) + goals = transport("clicked_point", PointStamped, 1) + velocities = transport("nav_cmd_vel", Twist, 1) + arrivals = transport("goal_reached", Bool, 1) + paths = transport("path", Path, 1) + header = Header(stamp=Time(sec=1700000000, nanosec=123456789), frame_id="map") + mapper = CostMapper( + algo="general", config=GeneralOccupancyConfig(resolution=0.125, min_height=0.5) + ) + mapper.global_map.transport = transport("global_map", PointCloud2, 0) + mapper.merged_map.transport = transport("merged_map", PointCloud2, 0) + mapper.global_costmap.transport = transport("global_costmap", OccupancyGrid, 0) + mapper.start() + stack.callback(mapper.stop) + voxels = VoxelGrid(voxel_size=0.25, device="CPU:0", frame_id="map", show_startup_log=False) + stack.callback(voxels.dispose) + x, y = np.meshgrid(np.arange(0, 6, 0.25), np.arange(0, 6, 0.25)) + points = np.column_stack((x.ravel(), y.ravel(), np.zeros(x.size))) + source = pointcloud_from_xyz(points, header=header) + voxels.add_frame(PointCloud2.decode(source.encode())) + accumulated = voxels.get_global_pointcloud2() + assert accumulated.header == header + position = [1.0, 1.0, 0.0] + lock = Lock() + arrived = Event() + commands: list[Twist] = [] + received_paths: list[Path] = [] + + def feedback() -> None: + message = Odometry(header=header, child_frame_id="base_link") + message.pose.pose = Pose( + position=Point(x=position[0], y=position[1]), + orientation=Quaternion(z=math.sin(position[2] / 2), w=math.cos(position[2] / 2)), + ) + odometry.publish(message) + + def command(message: Twist) -> None: + with lock: + commands.append(message) + position[0] += message.linear.x * math.cos(position[2]) * 0.1 + position[1] += message.linear.x * math.sin(position[2]) * 0.1 + position[2] += message.angular.z * 0.1 + feedback() + + stack.callback(velocities.subscribe(command)) + stack.callback(arrivals.subscribe(lambda message: arrived.set() if message.data else None)) + stack.callback(paths.subscribe(received_paths.append)) + module.start() + stack.callback(module.stop) + + def initialized() -> bool: + clouds.publish(accumulated) + with lock: + feedback() + return ( + module._planner._current_odom is not None + and module._planner._navigation_map._binary is not None + ) + + wait_until(initialized, timeout=5) + goals.publish(PointStamped(header=header, point=Point(x=3, y=1))) + assert arrived.wait(10), f"{backend}: navigation module did not report arrival" + with lock: + final = tuple(position) + assert abs(final[0] - 3) < 0.25 and abs(final[1] - 1) < 0.1 + assert any(command.linear.x > 0 for command in commands) + path = next(path for path in received_paths if len(path.poses)) + assert path.header == header and all(pose.header == header for pose in path.poses) + wait_until(lambda: bool(commands) and commands[-1] == Twist(), timeout=5) + grid = module._planner._navigation_map.binary_costmap + assert grid.header == header + print( + f"{backend}: {len(points)} source points → {accumulated.width} voxels → {grid.info.width}x{grid.info.height} costmap" + ) + output = FilePath(f"build/message-codegen/demo/evidence/navigation-{backend}.svg") + output.parent.mkdir(parents=True, exist_ok=True) + output.write_text(Space().base_map(grid).add(Polyline(path)).to_svg()) + print( + f"{backend}: arrived=({final[0]:.3f},{final[1]:.3f}), commands={len(commands)}, path={len(path.poses)} poses" + ) + print(f"SVG: {output}; exact source timestamp=1700000000123456789 ns") + print(f"PASS: {backend} generated odometry/map/clicked goal → navigation → Twist/Path/Bool") + + +if __name__ == "__main__": + for backend in ("lcm", "zenoh"): + demonstrate(backend) diff --git a/examples/message-codegen/demo_navigation_recording.py b/examples/message-codegen/demo_navigation_recording.py new file mode 100644 index 00000000000..e786d334aa5 --- /dev/null +++ b/examples/message-codegen/demo_navigation_recording.py @@ -0,0 +1,118 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Write generated CDR SQLite streams and load world-frame navigation data.""" + +import math +from pathlib import Path +import subprocess +import sys +from tempfile import TemporaryDirectory + +from dimos_generated.geometry_msgs.msg import ( + Point, + Pose, + PoseWithCovariance, + Quaternion, + Transform, + TransformStamped, + Vector3, +) +from dimos_generated.nav_msgs.msg import Odometry +from dimos_generated.sensor_msgs.msg import PointCloud2 +from dimos_generated.std_msgs.msg import Header +from dimos_generated.tf2_msgs.msg import TFMessage +import numpy as np + +from dimos.memory.store.sqlite import SqliteStore +from dimos.msgs.pointcloud import pointcloud_from_xyz +from dimos.msgs.time import time_from_seconds +from dimos.navigation.nav_3d.evaluator.recording import iter_world_frames, load_trajectory + + +def main() -> None: + with TemporaryDirectory(prefix="dimos-cdr-navigation-") as temporary: + database = Path(temporary) / "navigation.db" + with SqliteStore(path=str(database)) as store: + lidar = store.stream("lidar", PointCloud2) + odometry = store.stream("odom", Odometry) + transforms = store.stream("tf", TFMessage) + for tick in range(3): + timestamp = float(tick + 1) + stamp = time_from_seconds(timestamp) + cloud = pointcloud_from_xyz( + np.array([[1, 0, 0]], dtype=np.float32), + header=Header(frame_id="lidar", stamp=stamp), + ) + pose = Pose( + position=Point(x=float(tick)), + orientation=Quaternion(z=math.sqrt(0.5), w=math.sqrt(0.5)), + ) + lidar.append(cloud, ts=timestamp) + transforms.append( + TFMessage( + transforms=[ + TransformStamped( + header=Header(frame_id="world", stamp=stamp), + child_frame_id="lidar", + transform=Transform( + translation=Vector3(x=float(tick)), rotation=pose.orientation + ), + ) + ] + ), + ts=timestamp, + ) + odometry.append( + Odometry( + header=Header(frame_id="world", stamp=stamp), + pose=PoseWithCovariance(pose=pose), + ), + ts=timestamp, + ) + frames = list(iter_world_frames(database, "lidar", "odom")) + trajectory = load_trajectory(database, "odom") + assert len(frames) == 3 + for tick, frame in enumerate(frames): + np.testing.assert_allclose(frame.points, [[tick, 1, 0]], atol=1e-6) + print(f"t={frame.ts:.1f}: sensor [1, 0, 0] → world {frame.points[0].tolist()}") + np.testing.assert_allclose(trajectory.arc_lengths(), [0, 1, 2]) + print(f"Decoded odometry trajectory: {trajectory.positions.tolist()}; distance=2m") + output = Path("build/message-codegen/demo/evidence/raytrace-cli.rrd").resolve() + output.parent.mkdir(parents=True, exist_ok=True) + subprocess.run( + [ + sys.executable, + "-m", + "dimos.mapping.ray_tracing.utils.raytrace_rrd", + str(database), + "--out", + str(output), + "--lidar-stream", + "lidar", + "--world-frame", + "world", + "--fine-divisor", + "0", + ], + check=True, + timeout=30, + ) + assert output.stat().st_size > 0 + print(f"Native ray-tracing CLI recording: {output}") + print("Temporary SQLite recording removed") + + +if __name__ == "__main__": + main() diff --git a/examples/message-codegen/demo_navigation_skill.py b/examples/message-codegen/demo_navigation_skill.py new file mode 100644 index 00000000000..5a21c0d0892 --- /dev/null +++ b/examples/message-codegen/demo_navigation_skill.py @@ -0,0 +1,60 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Show a named location becoming a generated navigation goal without moving hardware.""" + +import math +from unittest.mock import Mock, patch + +from dimos_generated.geometry_msgs.msg import Point, Pose, PoseStamped +import numpy as np + +from dimos.agents.skills.navigation import NavigationSkillContainer +from dimos.msgs.geometry import quaternion_euler, quaternion_from_euler + + +def main() -> None: + skill = NavigationSkillContainer() + memory, navigator = Mock(), Mock() + try: + with ( + patch.object(skill, "_spatial_memory", memory, create=True), + patch.object(skill, "_navigation", navigator, create=True), + ): + skill._skill_started = True + original = PoseStamped( + pose=Pose( + position=Point(x=1, y=2, z=3), + orientation=quaternion_from_euler(0.2, -0.3, math.pi / 2), + ) + ) + skill._on_odom(PoseStamped.decode(original.encode())) + print(skill.tag_location("desk")) + location = memory.tag_location.call_args.args[0] + memory.query_tagged_location.return_value = location + print(skill._navigate_by_tagged_location("desk")) + goal = PoseStamped.decode(navigator.set_goal.call_args.args[0].encode()) + np.testing.assert_allclose( + quaternion_euler(goal.pose.orientation), (0.2, -0.3, math.pi / 2) + ) + assert goal.pose.position == original.pose.position + print(f"Generated CDR goal: frame={goal.header.frame_id}, position=(1, 2, 3)") + print(f"Preserved roll, pitch, yaw: {quaternion_euler(goal.pose.orientation)}") + print("Memory and navigation RPCs stubbed; no inference or hardware motion.") + finally: + skill.stop() + + +if __name__ == "__main__": + main() diff --git a/examples/message-codegen/demo_occupancy.py b/examples/message-codegen/demo_occupancy.py new file mode 100644 index 00000000000..cfb17f57dff --- /dev/null +++ b/examples/message-codegen/demo_occupancy.py @@ -0,0 +1,57 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +# Copyright 2026 Dimensional Inc. +# SPDX-License-Identifier: Apache-2.0 + +"""Render a synthetic CDR point cloud as an inflated CDR occupancy map.""" + +from pathlib import Path + +from dimos_generated.builtin_interfaces.msg import Time +from dimos_generated.nav_msgs.msg import OccupancyGrid +from dimos_generated.sensor_msgs.msg import PointCloud2 +from dimos_generated.std_msgs.msg import Header +import numpy as np + +from dimos.mapping.occupancy.inflation import simple_inflate +from dimos.mapping.pointclouds.occupancy import general_occupancy +from dimos.memory.vis.space.space import Space +from dimos.msgs.occupancy import occupancy_view +from dimos.msgs.pointcloud import pointcloud_from_xyz + + +def main() -> None: + x, y = np.meshgrid(np.arange(0, 4, 0.1), np.arange(0, 3, 0.1)) + points = np.column_stack((x.ravel(), y.ravel(), np.zeros(x.size))) + points[(points[:, 0] > 1.8) & (points[:, 0] < 2.2), 2] = 1 + header = Header(stamp=Time(sec=1700000000, nanosec=123456789), frame_id="map") + cloud = PointCloud2.decode(pointcloud_from_xyz(points, header=header).encode()) + grid = general_occupancy(cloud, resolution=0.1) + inflated = OccupancyGrid.decode(simple_inflate(grid, 0.2).encode()) + assert inflated.header == header + before = np.count_nonzero(occupancy_view(grid) == 100) + after = np.count_nonzero(occupancy_view(inflated) == 100) + assert after > before + output = Path("build/message-codegen/demo/evidence/occupancy.svg") + output.parent.mkdir(parents=True, exist_ok=True) + output.write_text(Space().add(inflated).to_svg()) + print(f"Input: {len(points)} CDR points; stamp=1700000000123456789 ns") + print(f"Occupied cells: {before} → {after} after 0.2 m inflation") + print(f"SVG: {output}") + print("PASS: cloud → occupancy → inflation → CDR → SVG; exact source header retained") + + +if __name__ == "__main__": + main() diff --git a/examples/message-codegen/demo_odometry_history.py b/examples/message-codegen/demo_odometry_history.py new file mode 100644 index 00000000000..28e2f769a9f --- /dev/null +++ b/examples/message-codegen/demo_odometry_history.py @@ -0,0 +1,91 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Follow a CDR path and inspect generated odometry history without hardware.""" + +import asyncio +from pathlib import Path as FilePath + +from dimos_generated.builtin_interfaces.msg import Time +from dimos_generated.geometry_msgs.msg import Point, Pose, PoseStamped, Quaternion, Twist +from dimos_generated.nav_msgs.msg import Odometry, Path +from dimos_generated.std_msgs.msg import Bool, Header + +from dimos.mapping.odometry_hist import OdometryHist, path_at_true_height +from dimos.memory.vis.space.elements import Polyline +from dimos.memory.vis.space.space import Space +from dimos.navigation.basic_path_follower.module import BasicPathFollower + + +async def main() -> None: + follower = BasicPathFollower() + history = OdometryHist(min_publish_interval_seconds=0) + commands: list[Twist] = [] + arrivals: list[Bool] = [] + paths: list[Path] = [] + follower.nav_cmd_vel.subscribe(lambda msg: commands.append(Twist.decode(msg.encode()))) + follower.goal_reached.subscribe(lambda msg: arrivals.append(Bool.decode(msg.encode()))) + history.odom_hist.subscribe(lambda msg: paths.append(Path.decode(msg.encode()))) + header = Header(stamp=Time(sec=1700000000, nanosec=123456789), frame_id="map") + reference = Path( + header=header, + poses=[ + PoseStamped(header=header, pose=Pose(position=Point(x=x), orientation=Quaternion(w=1))) + for x in [0, 1, 2] + ], + ) + try: + follower._on_path(Path.decode(reference.encode())) + waypoints = follower._waypoints + assert waypoints is not None + x = 0.0 + for tick in range(100): + header.stamp.nanosec = 123456789 + tick + pose = PoseStamped( + header=header, pose=Pose(position=Point(x=x, z=0.25), orientation=Quaternion(w=1)) + ) + sample = Odometry(header=header) + sample.pose.pose = pose.pose + await history.handle_odometry(Odometry.decode(sample.encode())) + follower._step(PoseStamped.decode(pose.encode()), waypoints) + if arrivals: + break + x += commands[-1].linear.x * 0.1 + assert arrivals and arrivals[-1].data and commands[-1] == Twist() + assert x >= 1.7 and x <= 2 + recorded = paths[-1] + assert recorded.poses[0].header.stamp.nanosec == 123456789 + assert recorded.header.stamp == header.stamp + strips = path_at_true_height(recorded).strips.as_arrow_array().to_pylist() + assert all(point[2] == 0.25 for point in strips[0]) + output = FilePath("build/message-codegen/demo/evidence/odometry-history.svg") + output.parent.mkdir(parents=True, exist_ok=True) + output.write_text( + Space().add(Polyline(reference)).add(Polyline(recorded, color="#ff0000")).to_svg() + ) + print( + f"Arrived at x={x:.3f}; {len(recorded.poses)} generated history poses; final Twist is zero" + ) + print( + f"Source stamps: 1700000000123456789..{1700000000000000000 + recorded.header.stamp.nanosec} ns" + ) + print(f"SVG reference/history: {output}; Rerun geometry retains z=0.25 m") + finally: + follower.stop() + history.stop() + print("PASS: generated Path → basic follower → Twist/Bool; Odometry → exact stamped history") + + +if __name__ == "__main__": + asyncio.run(main()) diff --git a/examples/message-codegen/demo_path_following.py b/examples/message-codegen/demo_path_following.py new file mode 100644 index 00000000000..37addcf1692 --- /dev/null +++ b/examples/message-codegen/demo_path_following.py @@ -0,0 +1,85 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Follow a generated CDR path in the existing base simulator and export an SVG.""" + +from pathlib import Path as FilePath + +from dimos_generated.geometry_msgs.msg import PoseStamped, Twist, Vector3 +from dimos_generated.nav_msgs.msg import Path +from dimos_generated.std_msgs.msg import Header + +from dimos.control.benchmarking.paths import straight_line, trajectory_to_svg +from dimos.control.benchmarking.plant import ( + FopdtChannelParams, + TwistBasePlantParams, + TwistBasePlantSim, +) +from dimos.control.task import CoordinatorState, JointStateSnapshot +from dimos.control.tasks.path_follower_task.path_follower_task import ( + PathFollowerTask, + PathFollowerTaskConfig, +) +from dimos.core.global_config import GlobalConfig + + +def main() -> None: + reference = straight_line(length=2.0) + reference.header = Header(frame_id="world") + reference.poses = [ + PoseStamped(header=reference.header, pose=pose.pose) for pose in reference.poses + ] + received = Path.decode(reference.encode()) + names = ["base/vx", "base/vy", "base/wz"] + task = PathFollowerTask( + "demo", PathFollowerTaskConfig(joint_names=names, speed=0.5), GlobalConfig() + ) + channel = FopdtChannelParams(K=1.0, tau=0.1, L=0.0) + plant = TwistBasePlantSim(TwistBasePlantParams(vx=channel, vy=channel, wz=channel)) + plant.reset(0.0, 0.0, 0.0, 0.1) + assert task.start_path(received, PoseStamped(header=received.header)) + trace: list[tuple[float, float]] = [] + for tick in range(200): + state = CoordinatorState( + joints=JointStateSnapshot( + joint_positions=dict(zip(names, [plant.x, plant.y, plant.yaw], strict=True)), + joint_velocities=dict.fromkeys(names, 0.0), + ), + t_now=tick * 0.1, + dt=0.1, + ) + command = task.compute(state) + trace.append((plant.x, plant.y)) + if not task.is_active(): + break + assert command is not None + vx, vy, wz = command.velocities + wire = Twist(linear=Vector3(x=vx, y=vy), angular=Vector3(z=wz)).encode() + decoded = Twist.decode(wire) + plant.step(decoded.linear.x, decoded.linear.y, decoded.angular.z, 0.1) + assert task.get_state() == "arrived" + assert abs(plant.x - 2.0) <= 0.2 and abs(plant.y) < 1e-9 + output = FilePath("build/message-codegen/demo/evidence/path-following.svg") + output.parent.mkdir(parents=True, exist_ok=True) + output.write_text(trajectory_to_svg(received, trace)) + print(f"Input: nav_msgs/msg/Path; {len(received.poses)} CDR-decoded waypoints") + print( + f"Result: {task.get_state()}; position=({plant.x:.3f}, {plant.y:.3f}); ticks={len(trace)}" + ) + print(f"SVG reference and executed path: {output}") + print("PASS: generated paths and Twist commands drive the base simulator to arrival") + + +if __name__ == "__main__": + main() diff --git a/examples/message-codegen/demo_patrol.py b/examples/message-codegen/demo_patrol.py new file mode 100644 index 00000000000..6bb358146d4 --- /dev/null +++ b/examples/message-codegen/demo_patrol.py @@ -0,0 +1,65 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Render generated CDR patrol goals selected by all three routers.""" + +from pathlib import Path + +from dimos_generated.builtin_interfaces.msg import Time +from dimos_generated.geometry_msgs.msg import Pose, PoseStamped, Quaternion +from dimos_generated.nav_msgs.msg import MapMetaData, OccupancyGrid +from dimos_generated.std_msgs.msg import Header +import numpy as np + +from dimos.memory.vis.space.space import Space +from dimos.msgs.occupancy import grid_to_world +from dimos.navigation.patrolling.create_patrol_router import create_patrol_router +from dimos.navigation.patrolling.utilities import point_to_pose_stamped + + +def main() -> None: + np.random.seed(42) + cells = np.zeros((24, 24), dtype=np.int8) + cells[[0, -1], :] = 100 + cells[:, [0, -1]] = 100 + grid = OccupancyGrid( + header=Header(frame_id="map", stamp=Time(sec=1700000000, nanosec=123456789)), + info=MapMetaData( + width=24, height=24, resolution=0.5, origin=Pose(orientation=Quaternion(w=1)) + ), + data=cells.ravel(), + ) + grid = OccupancyGrid.decode(grid.encode()) + for name in ("random", "coverage", "frontier"): + router = create_patrol_router(name, 0.5) + router.handle_occupancy_grid(grid) + router.handle_odom(point_to_pose_stamped(grid_to_world(grid, (12, 12)), grid.header)) + space = Space().base_map(grid) + for _ in range(5): + goal = router.next_goal() + assert goal is not None + goal = PoseStamped.decode(goal.encode()) + assert goal.header == grid.header + space.add(goal, color="#ff0000") + router.handle_odom(goal) + print(f"{name}: ({goal.pose.position.x:.2f}, {goal.pose.position.y:.2f}) m") + output = Path(f"build/message-codegen/demo/evidence/patrol-{name}.svg") + output.parent.mkdir(parents=True, exist_ok=True) + output.write_text(space.to_svg()) + print(f"SVG: {output}") + print("PASS: three patrol routers select CDR goals with exact map headers") + + +if __name__ == "__main__": + main() diff --git a/examples/message-codegen/demo_person_follow.py b/examples/message-codegen/demo_person_follow.py new file mode 100644 index 00000000000..583322b065f --- /dev/null +++ b/examples/message-codegen/demo_person_follow.py @@ -0,0 +1,78 @@ +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Run the person-follow control thread with deterministic tracker output.""" + +from types import SimpleNamespace +from unittest.mock import MagicMock, patch + +from dimos_generated.geometry_msgs.msg import Twist +from dimos_generated.sensor_msgs.msg import CameraInfo +import numpy as np + +from dimos.agents.skills.person_follow import PersonFollowSkillContainer +from dimos.core.global_config import GlobalConfig +from dimos.msgs.image import image_from_array +from dimos.perception.detection.type.detection2d.bbox import Detection2DBBox +from dimos.perception.detection.type.detection2d.imageDetections2D import ImageDetections2D + + +def main() -> None: + camera = CameraInfo(width=640, height=480, k=[500, 0, 320, 0, 500, 240, 0, 0, 1]) + with patch("dimos.agents.skills.person_follow.create", return_value=MagicMock()): + module = PersonFollowSkillContainer(camera_info=camera, g=GlobalConfig(simulation="")) + image = image_from_array(np.zeros((480, 640, 3), dtype=np.uint8), encoding="rgb8") + detection = Detection2DBBox( + bbox=(270, 100, 370, 300), + track_id=1, + class_id=0, + confidence=1, + name="person", + ts=0, + image=image, + ) + tracker = SimpleNamespace( + init_track=lambda **kwargs: ImageDetections2D(image, [detection]), + process_image=lambda frame: ImageDetections2D(frame, [detection]), + stop=lambda: None, + ) + commands: list[Twist] = [] + + def receive(value: Twist) -> None: + command = Twist.decode(value.encode()) + commands.append(command) + print( + f"CDR command: forward={command.linear.x:.2f} m/s, turn={command.angular.z:.2f} rad/s" + ) + module._should_stop.set() + + unsubscribe = module.cmd_vel.subscribe(receive) + try: + module._tracker = tracker + module._on_color_image(image) + print(module._follow_person("synthetic person", (270, 100, 370, 300))) + thread = module._thread + assert thread is not None + thread.join(timeout=2) + assert not thread.is_alive() + assert commands[0].linear.x == 0.5 and commands[-1] == Twist() + print("PASS: real control thread publishes motion, stops, and publishes zero") + print("Tracker/model output is substituted; no inference or robot actuation.") + finally: + unsubscribe() + module.stop() + + +if __name__ == "__main__": + main() diff --git a/examples/message-codegen/demo_phone_cdr.py b/examples/message-codegen/demo_phone_cdr.py new file mode 100644 index 00000000000..b135a8c254a --- /dev/null +++ b/examples/message-codegen/demo_phone_cdr.py @@ -0,0 +1,74 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Open the phone page on loopback and inspect five decoded browser CDR frames.""" + +import argparse +from queue import Queue +import socket +import threading + +from dimos_generated.geometry_msgs.msg import TwistStamped +import uvicorn + +from dimos.teleop.phone.phone_teleop_module import PhoneTeleopModule + + +class PhonePreview(PhoneTeleopModule): + def __init__(self) -> None: + self.received: Queue[TwistStamped] = Queue() + super().__init__() + + def _on_sensors_bytes(self, data: bytes) -> None: + super()._on_sensors_bytes(data) + self.received.put(TwistStamped.decode(data)) + + +def main() -> None: + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument("--timeout", type=float, default=120) + args = parser.parse_args() + module = PhonePreview() + with socket.socket() as listener: + listener.bind(("127.0.0.1", 0)) + port = listener.getsockname()[1] + server = uvicorn.Server(uvicorn.Config(module._web_server.app, log_level="warning")) + thread = threading.Thread(target=server.run, kwargs={"sockets": [listener]}) + thread.start() + try: + print(f"Open http://127.0.0.1:{port}/teleop and click Connect", flush=True) + message = None + for _ in range(5): + message = module.received.get(timeout=args.timeout) + assert message is not None + assert message.header.frame_id == "phone" + assert message.header.stamp.sec > 0 + print( + f"Browser → {message.msg_name}: stamp={message.header.stamp.sec}s + " + f"{message.header.stamp.nanosec}ns", + flush=True, + ) + print( + "PASS: browser uses advertised schemas, generic CDR writer and explicit channel/type frames", + flush=True, + ) + finally: + server.should_exit = True + thread.join(timeout=10) + assert not thread.is_alive(), "demo web server did not stop" + module.stop() + + +if __name__ == "__main__": + main() diff --git a/examples/message-codegen/demo_planning_backends.py b/examples/message-codegen/demo_planning_backends.py new file mode 100644 index 00000000000..4635ee62abb --- /dev/null +++ b/examples/message-codegen/demo_planning_backends.py @@ -0,0 +1,108 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Plan and inspect generated joint waypoints against a real Drake world.""" + +from pathlib import Path +from tempfile import TemporaryDirectory + +from dimos_generated.builtin_interfaces.msg import Time +from dimos_generated.sensor_msgs.msg import JointState +from dimos_generated.std_msgs.msg import Header +from dimos_generated.tf2_msgs.msg import TFMessage +import numpy as np + +from dimos.manipulation.planning.groups.models import PlanningGroupDefinition +from dimos.manipulation.planning.monitor.world_monitor import WorldMonitor +from dimos.manipulation.planning.planners.rrt_planner import RRTConnectPlanner +from dimos.manipulation.planning.spec.config import RobotModelConfig +from dimos.manipulation.planning.utils.path_utils import interpolate_path +from dimos.manipulation.planning.world.drake_world import DrakeWorld +from dimos.msgs.geometry import pose_matrix, transform_from_pose, transform_matrix +from dimos.robot.assets.model import RobotModel + +_URDF = """ + + + + + + + + + +""" + + +def main() -> None: + with TemporaryDirectory(prefix="dimos-cdr-planning-") as directory: + path = Path(directory) / "arm.urdf" + path.write_text(_URDF) + config = RobotModelConfig( + model=RobotModel.from_file(path).with_default_joint_acceleration_limit(2.0), + joint_names=["shoulder", "wrist"], + base_link="base", + planning_groups=[PlanningGroupDefinition("arm", ("shoulder", "wrist"), "base", "tool")], + ) + world = DrakeWorld() + monitor = WorldMonitor(world=world) + monitor.load_model(config) + world.finalize() + start = JointState.decode( + JointState(name=config.joint_names, position=[-0.4, 0.3]).encode() + ) + goal = JointState.decode(JointState(name=config.joint_names, position=[0.4, -0.3]).encode()) + result = RRTConnectPlanner().plan_joint_path(world, start, goal) + assert result.is_success(), result + path_points = interpolate_path(result.path, resolution=0.2) + print(f"Drake model joints: {world.get_prepared_model().joint_space.names}") + print(f"RRT result: {result.status.name}; interpolated waypoints={len(path_points)}") + for index, point in enumerate(path_points): + decoded = JointState.decode(point.encode()) + assert decoded == point + assert world.check_config_collision_free(decoded) + with world.scratch_context() as context: + world.set_joint_state(context, decoded) + np.testing.assert_allclose( + list(world.get_joint_state(context).position), list(decoded.position) + ) + print( + f" waypoint {index}: {list(decoded.position)}; CDR round-trip and collision check passed" + ) + decoded.header = Header( + frame_id="encoder", stamp=Time(sec=1700000000, nanosec=123456789 + index) + ) + pose = monitor.get_group_ee_pose("arm", decoded) + transform = transform_from_pose(pose, child_frame_id="tool") + tf = TFMessage.decode(TFMessage(transforms=[transform]).encode()) + assert tf.transforms[0].header.stamp == decoded.header.stamp + expected = np.array([np.cos(decoded.position[0]), np.sin(decoded.position[0]), 0.0]) + np.testing.assert_allclose(pose_matrix(pose.pose)[:3, 3], expected, atol=1e-12) + np.testing.assert_allclose( + transform_matrix(tf.transforms[0].transform), pose_matrix(pose.pose), atol=1e-12 + ) + print( + f" tool position={expected.round(6).tolist()}; " + f"TF {transform.header.frame_id} -> {transform.child_frame_id}; " + f"source stamp={pose.header.stamp.sec}.{pose.header.stamp.nanosec:09d}" + ) + np.testing.assert_allclose(list(path_points[0].position), list(start.position)) + np.testing.assert_allclose(list(path_points[-1].position), list(goal.position)) + print( + "PASS: generated JointState feeds real Drake and RRT; FK pose and CDR TF preserve source stamps" + ) + + +if __name__ == "__main__": + main() diff --git a/examples/message-codegen/demo_planning_joints.py b/examples/message-codegen/demo_planning_joints.py new file mode 100644 index 00000000000..e5a9f3f8b96 --- /dev/null +++ b/examples/message-codegen/demo_planning_joints.py @@ -0,0 +1,75 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Project generated coordinator feedback into a selected planning group.""" + +from dimos_generated.builtin_interfaces.msg import Time +from dimos_generated.sensor_msgs.msg import JointState +from dimos_generated.std_msgs.msg import Header + +from dimos.manipulation.planning.groups.models import PlanningGroup +from dimos.manipulation.planning.groups.utils import ( + filter_joint_state_to_selected_joints, + normalize_joint_target, +) +from dimos.manipulation.planning.spec.joint_space import ( + CoordinateTopology, + JointCoordinate, + JointSpace, +) +from dimos.msgs.time import to_nanoseconds + + +def main() -> None: + group = PlanningGroup("left_arm", ("left/j1", "left/j2"), "base", "tool") + source = JointState( + header=Header(stamp=Time(sec=1700000000, nanosec=123456789), frame_id="robot"), + name=["right/j1", "left/j2", "left/j1"], + position=[0.9, 0.3, -0.2], + ) + decoded = JointState.decode(source.encode()) + selected = filter_joint_state_to_selected_joints(decoded, group.joint_names) + target = normalize_joint_target( + group, JointState(header=source.header, name=["left/j2", "left/j1"], position=[0.5, 0.1]) + ) + space = JointSpace( + tuple( + JointCoordinate( + name=name, + mechanism_type="revolute", + topology=CoordinateTopology.INTERVAL, + lower=-1.0, + upper=1.0, + max_velocity=1.0, + max_acceleration=2.0, + ) + for name in group.joint_names + ) + ) + positions = space.from_joint_state(selected) + assert positions.tolist() == [-0.2, 0.3] + assert space.from_joint_state(target).tolist() == [0.1, 0.5] + assert selected.header == target.header == source.header + assert JointState.decode(target.encode()) == target + print(f"Coordinator input: names={list(source.name)}, q={list(source.position)}") + print(f"Selected {group.id}: names={list(selected.name)}, q={positions.tolist()}") + print(f"Normalized target: q={list(target.position)}; within declared joint limits") + print( + f"Source stamp preserved: {to_nanoseconds(selected.header.stamp)}, frame={selected.header.frame_id}" + ) + print("PASS: generated CDR feedback feeds planning selection and joint-space validation") + + +if __name__ == "__main__": + main() diff --git a/examples/message-codegen/demo_pointcloud.py b/examples/message-codegen/demo_pointcloud.py new file mode 100644 index 00000000000..ffddace3d46 --- /dev/null +++ b/examples/message-codegen/demo_pointcloud.py @@ -0,0 +1,63 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +# Copyright 2026 Dimensional Inc. +# SPDX-License-Identifier: Apache-2.0 + +"""Inspect padded, organized CDR point clouds without dropping non-XYZ fields.""" + +import struct + +from dimos_generated.sensor_msgs.msg import PointCloud2, PointField + +from dimos.msgs.pointcloud import pointcloud_view, pointcloud_xyz + + +def main() -> None: + for big_endian in (False, True): + order = ">" if big_endian else "<" + payload = bytearray(64) + for row in range(2): + struct.pack_into(order + "3f2H", payload, row * 32, row + 1, 2, 3, 7, 9) + message = PointCloud2( + height=2, + width=1, + point_step=20, + row_step=32, + is_bigendian=big_endian, + data=bytes(payload), + fields=[ + PointField(name="x", offset=0, datatype=7, count=1), + PointField(name="y", offset=4, datatype=7, count=1), + PointField(name="z", offset=8, datatype=7, count=1), + PointField(name="tags", offset=12, datatype=4, count=2), + ], + ) + decoded = PointCloud2.decode(message.encode(little_endian=not big_endian)) + view = pointcloud_view(decoded) + assert pointcloud_xyz(decoded).tolist() == [[1, 2, 3], [2, 2, 3]] + assert view["tags"].tolist() == [[[7, 9]], [[7, 9]]] + assert not view.flags.writeable + copied = view.copy() + copied["x"][0, 0] = 99 + assert view["x"][0, 0] == 1 + print(f"{'big' if big_endian else 'little'}-endian: XYZ={pointcloud_xyz(decoded).tolist()}") + print( + f" organized shape={view.shape}, point/row strides={view.strides}, tags={view['tags'].tolist()}" + ) + print(" borrowed view is read-only; changing the explicit copy leaves source x=1") + + +if __name__ == "__main__": + main() diff --git a/examples/message-codegen/demo_pubsub.py b/examples/message-codegen/demo_pubsub.py new file mode 100644 index 00000000000..fb1e51f0995 --- /dev/null +++ b/examples/message-codegen/demo_pubsub.py @@ -0,0 +1,128 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Exchange generated messages through DimOS LCM, Zenoh TCP, and shared memory.""" + +import argparse +from contextlib import ExitStack +import socket +import threading +from typing import Any +from urllib.parse import urlsplit +import uuid + +from dimos_generated.dimos_msgs.msg import LineSegment3D, LineSegments3D +from dimos_generated.geometry_msgs.msg import Point +from dimos_generated.sensor_msgs.msg import Image +import numpy as np + +from dimos.core.transport import LCMTransport, PubSubTransport, SHMTransport, ZenohTransport +from dimos.msgs.protocol import DimosMsg +from dimos.msgs.time import time_from_nanoseconds, to_nanoseconds +from dimos.protocol.service.lcmservice import LCMConfig +from dimos.protocol.service.zenohservice import ZenohSessionPool +from dimos.utils.testing.waiting import retry_until + + +def free_port(kind: socket.SocketKind) -> int: + with socket.socket(socket.AF_INET, kind) as listener: + listener.bind(("127.0.0.1", 0)) + return int(listener.getsockname()[1]) + + +def exchange( + publisher: PubSubTransport[Any], subscriber: PubSubTransport[Any], message: DimosMsg +) -> DimosMsg: + received: list[DimosMsg] = [] + ready = threading.Event() + + def receive(value: DimosMsg) -> None: + received.append(value) + ready.set() + + unsubscribe = subscriber.subscribe(receive) + try: + retry_until(ready, lambda: publisher.broadcast(None, message), timeout=10) + return received[0] + finally: + unsubscribe() + + +def demonstrate(backend: str) -> None: + image = Image( + width=640, + height=480, + encoding="rgb8", + step=1920, + data=np.arange(640 * 480 * 3, dtype=np.uint8), + ) + image.header.frame_id = "camera" + image.header.stamp = time_from_nanoseconds(1_700_000_000_123_456_789) + lines = LineSegments3D(segments=[LineSegment3D(start=Point(x=1), end=Point(y=2), weight=4)]) + lines.header.frame_id = "map" + with ExitStack() as stack: + pools = [ZenohSessionPool(), ZenohSessionPool()] + for pool in pools: + stack.callback(pool.close_all) + tcp_endpoint = f"tcp/127.0.0.1:{free_port(socket.SOCK_STREAM)}" + group = urlsplit(LCMConfig().url).hostname + lcm_url = f"udpm://{group}:{free_port(socket.SOCK_DGRAM)}?ttl=0&recv_buf_size=4194304" + for message in (lines, image): + topic = f"dimos/demo/{uuid.uuid4().hex[:8]}" + peers = [] + for index in range(2): + peer: PubSubTransport[Any] + if backend == "lcm": + peer = LCMTransport(topic, type(message), url=lcm_url) + elif backend == "zenoh": + peer = ZenohTransport( + topic, + type(message), + session_pool=pools[index], + scouting=False, + multicast=False, + gossip=False, + listen=[tcp_endpoint] if index == 0 else [], + connect=[] if index == 0 else [tcp_endpoint], + ) + else: + peer = SHMTransport(topic, type(message), prefer="cpu") + stack.callback(peer.stop) + peer.start() + peers.append(peer) + decoded = exchange(peers[0], peers[1], message) + assert decoded.encode() == message.encode() + if isinstance(decoded, LineSegments3D): + print( + f"{backend}: {decoded.msg_name}, frame={decoded.header.frame_id}, " + f"segment weight={decoded.segments[0].weight}" + ) + else: + assert isinstance(decoded, Image) + print( + f"{backend}: {decoded.msg_name}, {len(decoded.data):,} pixel bytes match, " + f"source nanoseconds={to_nanoseconds(decoded.header.stamp)}" + ) + + +def main() -> None: + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument("--backend", choices=["lcm", "zenoh", "shm", "all"], default="all") + args = parser.parse_args() + for backend in ("lcm", "zenoh", "shm") if args.backend == "all" else (args.backend,): + demonstrate(backend) + + +if __name__ == "__main__": + main() diff --git a/examples/message-codegen/demo_robot_tf.py b/examples/message-codegen/demo_robot_tf.py new file mode 100644 index 00000000000..c15045c4d21 --- /dev/null +++ b/examples/message-codegen/demo_robot_tf.py @@ -0,0 +1,48 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Inspect a Go2 device pose and generated CDR TF chain without connecting to a robot.""" + +from dimos_generated.tf2_msgs.msg import TFMessage + +from dimos.msgs.geometry import compose_transforms +from dimos.msgs.time import to_nanoseconds +from dimos.robot.unitree.go2.connection import GO2Connection +from dimos.robot.unitree.type.odometry import pose_from_webrtc_odometry, raw_odometry_msg_sample + + +def main() -> None: + pose = pose_from_webrtc_odometry(raw_odometry_msg_sample) + edges = GO2Connection._odom_to_tf(pose, prefix="robot0") + decoded = TFMessage.decode(TFMessage(transforms=edges).encode()) + assert len(decoded.transforms) == 3 + print(f"Device pose: frame={pose.header.frame_id}, stamp={to_nanoseconds(pose.header.stamp)}") + for edge in decoded.transforms: + assert edge.header.stamp == pose.header.stamp + translation = edge.transform.translation + print( + f" {edge.header.frame_id} → {edge.child_frame_id}: " + f"xyz=({translation.x:.6f}, {translation.y:.6f}, {translation.z:.6f})" + ) + base, camera, optical = decoded.transforms + result = compose_transforms(compose_transforms(base, camera), optical) + assert result.header == pose.header + assert result.child_frame_id == "robot0/camera_optical" + print(f"Composed camera frame: {result.header.frame_id} → {result.child_frame_id}") + print(f"Exact camera stamp: {to_nanoseconds(result.header.stamp)}") + print("PASS: original ROS header, namespaced local frames, generated CDR round trip") + + +if __name__ == "__main__": + main() diff --git a/examples/message-codegen/demo_ros_bridge.py b/examples/message-codegen/demo_ros_bridge.py new file mode 100644 index 00000000000..8f6dc033049 --- /dev/null +++ b/examples/message-codegen/demo_ros_bridge.py @@ -0,0 +1,94 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Show changing generated images and poses crossing the optional ROS2 bridge.""" + +import threading +import uuid + +from dimos_generated.builtin_interfaces.msg import Time +from dimos_generated.geometry_msgs.msg import Point, Pose, PoseStamped +from dimos_generated.sensor_msgs.msg import Image +from dimos_generated.std_msgs.msg import Header + +from dimos.msgs.protocol import DimosMsg +from dimos.protocol.pubsub.impl.rospubsub import DimosROS, ROSTopic + + +def main() -> None: + prefix = f"/dimos_cdr_demo_{uuid.uuid4().hex[:8]}" + topics = [ROSTopic(prefix + "/pose", PoseStamped), ROSTopic(prefix + "/image", Image)] + publisher, subscriber = DimosROS(), DimosROS() + done = threading.Event() + received: dict[tuple[str, int], DimosMsg] = {} + expected: dict[tuple[str, int], DimosMsg] = {} + count = 0 + try: + publisher.start() + subscriber.start() + + def collect(message: DimosMsg, topic: ROSTopic) -> None: + assert isinstance(message, (PoseStamped, Image)) + stamp = message.header.stamp + key = (topic.topic, stamp.nanosec) + received[key] = message + print(f"ROS received {message.msg_name}: source={stamp.sec}.{stamp.nanosec:09d}") + if len(received) == 6: + done.set() + + for topic in topics: + subscriber.subscribe(topic, collect) + node = publisher._raw._node + assert node is not None + + def publish_next() -> None: + nonlocal count + if count == 3 or any(node.count_subscribers(topic.topic) == 0 for topic in topics): + return + header = Header(stamp=Time(sec=1700000000, nanosec=123456789 + count), frame_id="map") + messages: list[DimosMsg] = [ + PoseStamped(header=header, pose=Pose(position=Point(x=float(count)))), + Image( + header=header, + height=1, + width=1, + encoding="rgb8", + step=3, + data=bytes([count, 128, 255 - count]), + ), + ] + for topic, message in zip(topics, messages, strict=True): + expected[(topic.topic, header.stamp.nanosec)] = message + publisher.publish(topic, message) + count += 1 + + timer = node.create_timer(0.1, publish_next) + try: + assert done.wait(15), "ROS discovery or message delivery timed out" + assert received == expected + for index in range(3): + pose = received[(topics[0].topic, 123456789 + index)] + image = received[(topics[1].topic, 123456789 + index)] + assert isinstance(pose, PoseStamped) and isinstance(image, Image) + print(f"frame {index}: x={pose.pose.position.x}, RGB={list(image.data)}") + finally: + node.destroy_timer(timer) + finally: + subscriber.stop() + publisher.stop() + print("PASS: three changing image/pose pairs crossed ROS2 with exact fields and nanoseconds") + + +if __name__ == "__main__": + main() diff --git a/examples/message-codegen/demo_self_filter.py b/examples/message-codegen/demo_self_filter.py new file mode 100644 index 00000000000..2dda6e584b3 --- /dev/null +++ b/examples/message-codegen/demo_self_filter.py @@ -0,0 +1,76 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Inspect CDR point-cloud self exclusion as a modeled arm changes position.""" + +from pathlib import Path +from tempfile import TemporaryDirectory + +from dimos_generated.geometry_msgs.msg import Transform, TransformStamped, Vector3 +from dimos_generated.sensor_msgs.msg import PointCloud2 +from dimos_generated.std_msgs.msg import Header +import numpy as np + +from dimos.manipulation.planning.utils.point_cloud_self_filter import PointCloudSelfFilter +from dimos.msgs.pointcloud import pointcloud_from_xyz, pointcloud_xyz +from dimos.msgs.time import time_from_nanoseconds, to_nanoseconds +from dimos.protocol.tf.tf import MultiTBuffer +from dimos.robot.assets.model import RobotModel + +_URDF = """ + + + +""" + + +def main() -> None: + with TemporaryDirectory(prefix="dimos-cdr-self-filter-") as directory: + urdf = Path(directory) / "arm.urdf" + urdf.write_text(_URDF) + module = PointCloudSelfFilter(model=RobotModel.from_file(urdf), tf_forward_tolerance_s=0.0) + buffer = MultiTBuffer() + module.__dict__["_tf"] = buffer + try: + for tick, arm_x in enumerate([0.0, 1.0, 2.0]): + stamp = time_from_nanoseconds(1700000000123456789 + tick * 1000000000) + for frame in ("camera", "world"): + buffer.receive_transform( + TransformStamped( + header=Header(frame_id=frame, stamp=stamp), + child_frame_id="arm", + transform=Transform(translation=Vector3(x=arm_x)), + ) + ) + source = pointcloud_from_xyz( + np.array([[arm_x, 0, 0], [5.0, 0, 0]]), + header=Header(frame_id="camera", stamp=stamp), + ) + result = module.filter_cloud(PointCloud2.decode(source.encode())) + assert result is not None + filtered, mask = (PointCloud2.decode(value.encode()) for value in result) + np.testing.assert_allclose(pointcloud_xyz(filtered), [[5.0, 0, 0]]) + assert filtered.header == source.header + assert mask.header.stamp == stamp and mask.header.frame_id == "world" + print( + f"arm x={arm_x}: input=2 points, retained={filtered.width}, clear-mask cells={mask.width}, source ns={to_nanoseconds(stamp)}" + ) + finally: + module.__dict__["_tf"] = None + module.dispose() + print("PASS: robot returns excluded; generated mask and cloud preserve exact stamps") + + +if __name__ == "__main__": + main() diff --git a/examples/message-codegen/demo_storage_replay.py b/examples/message-codegen/demo_storage_replay.py new file mode 100644 index 00000000000..cd90f21a6d8 --- /dev/null +++ b/examples/message-codegen/demo_storage_replay.py @@ -0,0 +1,96 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Record generated values to SQLite and MCAP, then replay SQLite through module ports.""" + +from pathlib import Path +from tempfile import TemporaryDirectory +from threading import Event, Lock +from typing import Any + +from dimos_generated.dimos_msgs.msg import LineSegment3D, LineSegments3D +from dimos_generated.geometry_msgs.msg import Point +from dimos_generated.std_msgs.msg import Header + +from dimos.memory.codecs.base import codec_for, codec_id +from dimos.memory.replay_module import replay_module +from dimos.memory.store.mcap import McapStore +from dimos.memory.store.sqlite import SqliteStore +from dimos.msgs.time import time_from_nanoseconds, to_nanoseconds +from dimos.protocol.cdr_mcap import CdrMcapWriter + + +def main() -> None: + messages = [ + LineSegments3D( + header=Header(frame_id="map", stamp=time_from_nanoseconds(1700000000123456789 + index)), + segments=[LineSegment3D(start=Point(x=index), end=Point(y=2), weight=index + 4)], + ) + for index in range(3) + ] + with TemporaryDirectory(prefix="dimos-cdr-storage-") as directory: + database = Path(directory) / "memory.db" + artifact = Path(directory) / "recording.mcap" + with SqliteStore(path=str(database)) as store, CdrMcapWriter(artifact) as writer: + stream = store.stream("segments", LineSegments3D) + for index, message in enumerate(messages): + stream.append(message, ts=1 + index * 0.05) + writer.write( + "segments", + message.encode(), + schema_name=message.msg_name, + schema=message.schema, + log_time_ns=1000000000 + index * 50000000, + publish_time_ns=to_nanoseconds(message.header.stamp), + ) + print(f"SQLite default codec: {codec_id(codec_for(LineSegments3D))}") + for source in [ + SqliteStore(path=str(database), must_exist=True), + McapStore(path=str(artifact)), + ]: + with source: + decoded = [observation.data for observation in source.stream("segments")] + assert decoded == messages + print( + f"{type(source).__name__}: weights={[value.segments[0].weight for value in decoded]}" + ) + replay = replay_module(str(database))(dataset=str(database)) + received: list[Any] = [] + ready = Event() + lock = Lock() + + def receive(message: LineSegments3D) -> None: + with lock: + received.append(message) + print( + f"Module replay: weight={message.segments[0].weight}, stamp={to_nanoseconds(message.header.stamp)}" + ) + if len(received) == len(messages): + ready.set() + + unsubscribe = replay.outputs["segments"].subscribe(receive) + try: + replay.start() + assert ready.wait(5), "Replay did not deliver all messages" + assert received == messages + finally: + unsubscribe() + replay.stop() + print( + "PASS: CDR persisted, reopened by type/schema, replayed through Out ports; temporary files removed" + ) + + +if __name__ == "__main__": + main() diff --git a/examples/message-codegen/demo_trajectory.py b/examples/message-codegen/demo_trajectory.py new file mode 100644 index 00000000000..a600ada4eab --- /dev/null +++ b/examples/message-codegen/demo_trajectory.py @@ -0,0 +1,137 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Generate, transport, execute, and inspect a CDR joint trajectory without hardware.""" + +from contextlib import ExitStack +import math +import socket +import threading +from typing import Any +import uuid + +from demo_pubsub import free_port +from dimos_generated.dimos_msgs.msg import TrajectoryStatus +from dimos_generated.trajectory_msgs.msg import JointTrajectory + +from dimos.control.task import CoordinatorState, JointStateSnapshot +from dimos.control.tasks.trajectory_task.trajectory_task import ( + JointTrajectoryTask, + JointTrajectoryTaskConfig, + TrajectoryExecutionStatus, +) +from dimos.core.transport import LCMTransport, PubSubTransport, ZenohTransport +from dimos.manipulation.planning.trajectory_generator.joint_trajectory_generator import ( + JointTrajectoryGenerator, +) +from dimos.msgs.time import time_from_nanoseconds, to_nanoseconds, to_seconds +from dimos.msgs.trajectory import TrajectoryState, trajectory_duration +from dimos.protocol.service.zenohservice import ZenohSessionPool +from dimos.utils.testing.waiting import retry_until + + +def demonstrate(backend: str) -> None: + with ExitStack() as stack: + pools = [ZenohSessionPool(), ZenohSessionPool()] + for pool in pools: + stack.callback(pool.close_all) + endpoint = f"tcp/127.0.0.1:{free_port(socket.SOCK_STREAM)}" + url = f"udpm://239.255.76.67:{free_port(socket.SOCK_DGRAM)}?ttl=0" + prefix = f"trajectory_{uuid.uuid4().hex[:8]}" + + def transport(channel: str, message_type: type, peer: int) -> PubSubTransport[Any]: + result: PubSubTransport[Any] + if backend == "lcm": + result = LCMTransport(channel, message_type, url=url) + else: + result = ZenohTransport( + channel, + message_type, + session_pool=pools[peer], + scouting=False, + multicast=False, + gossip=False, + listen=[endpoint] if peer == 0 else [], + connect=[] if peer == 0 else [endpoint], + ) + stack.callback(result.stop) + result.start() + return result + + sender = transport(prefix + "/plan", JointTrajectory, 0) + receiver = transport(prefix + "/plan", JointTrajectory, 1) + feedback_sender = transport(prefix + "/status", TrajectoryStatus, 1) + feedback_receiver = transport(prefix + "/status", TrajectoryStatus, 0) + received: list[JointTrajectory] = [] + feedback: list[TrajectoryStatus] = [] + ready, completed = threading.Event(), threading.Event() + + def receive(plan: JointTrajectory) -> None: + received.append(plan) + ready.set() + + def receive_status(status: TrajectoryStatus) -> None: + feedback.append(status) + if status.state == TrajectoryStatus.COMPLETED: + completed.set() + + stack.callback(receiver.subscribe(receive)) + stack.callback(feedback_receiver.subscribe(receive_status)) + source = JointTrajectoryGenerator( + num_joints=2, max_velocity=1.0, max_acceleration=2.0 + ).generate([[0.0, 0.0], [0.3, -0.2]]) + source.joint_names = ["shoulder", "wrist"] + source.header.frame_id = "robot" + source.header.stamp = time_from_nanoseconds(1700000000123456789) + retry_until(ready, lambda: sender.publish(source), timeout=5) + plan = received[0] + assert plan == source + task = JointTrajectoryTask(JointTrajectoryTaskConfig(joint_names=tuple(plan.joint_names))) + positions = dict.fromkeys(plan.joint_names, 0.0) + assert task.execute(plan, positions).status == TrajectoryExecutionStatus.ACCEPTED + duration = trajectory_duration(plan) + print( + f"{backend}: received {len(plan.points)} generated points, duration={duration:.9f}s, source ns={to_nanoseconds(plan.header.stamp)}" + ) + steps = math.ceil(duration / 0.05) + 3 + for tick in range(steps): + now = tick * 0.05 + output = task.compute( + CoordinatorState( + joints=JointStateSnapshot(joint_positions=positions), t_now=now, dt=0.05 + ) + ) + if output is not None: + assert output.positions is not None + positions = dict(zip(output.joint_names, output.positions, strict=True)) + status = task.get_status(now) + feedback_sender.publish(status) + if tick % max(1, steps // 4) == 0: + print( + f" t={now:.2f}s q={list(positions.values())}, status={TrajectoryState(status.state).name}" + ) + terminal = task.get_status(steps * 0.05) + assert terminal.state == TrajectoryStatus.COMPLETED + retry_until(completed, lambda: feedback_sender.publish(terminal), timeout=5) + final = next(value for value in feedback if value.state == TrajectoryStatus.COMPLETED) + assert final.progress == 1.0 and to_seconds(final.time_remaining) == 0.0 + assert math.isclose(to_seconds(final.time_elapsed), duration, abs_tol=1e-9) + assert math.isclose(positions["shoulder"], 0.3, abs_tol=1e-9) + assert math.isclose(positions["wrist"], -0.2, abs_tol=1e-9) + print(f"PASS: {backend} generated trajectory and status reach the declared goal") + + +if __name__ == "__main__": + for backend in ("lcm", "zenoh"): + demonstrate(backend) diff --git a/examples/message-codegen/demo_video_stats.py b/examples/message-codegen/demo_video_stats.py new file mode 100644 index 00000000000..63ee0f59c5a --- /dev/null +++ b/examples/message-codegen/demo_video_stats.py @@ -0,0 +1,55 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Record browser video metrics as generated CDR and inspect exact integer fields.""" + +from pathlib import Path +import tempfile + +from dimos_generated.dimos_msgs.msg import VideoStats + +from dimos.memory.store.sqlite import SqliteStore +from dimos.teleop.utils.video_stats import video_stats_from_dict + + +def main() -> None: + message = video_stats_from_dict( + { + "ts": 1700000000.5, + "fps": 28.0, + "kbps": 2100.5, + "width": 1280, + "height": 720, + "frames_dropped": 2**32 + 1, + } + ) + decoded = VideoStats.decode(message.encode()) + assert decoded == message + with tempfile.TemporaryDirectory() as directory: + path = Path(directory) / "video.db" + with SqliteStore(path=str(path)) as store: + store.stream("video_stats", VideoStats).append(decoded, ts=1700000000.5) + with SqliteStore(path=str(path)) as store: + restored = store.stream("video_stats").first().data + assert restored == message + print( + f"Recorded {restored.msg_name}: {restored.width}x{restored.height}, {restored.fps} fps" + ) + print(f"Exact dropped-frame counter: {restored.frames_dropped}") + print(f"Source stamp: {restored.header.stamp.sec}s + {restored.header.stamp.nanosec}ns") + print("PASS: browser JSON → generated CDR → SQLite → generated message") + + +if __name__ == "__main__": + main() diff --git a/examples/message-codegen/demo_viewer_controls.py b/examples/message-codegen/demo_viewer_controls.py new file mode 100644 index 00000000000..c6a1ebfc3b4 --- /dev/null +++ b/examples/message-codegen/demo_viewer_controls.py @@ -0,0 +1,85 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Exercise viewer click, velocity, and stop events without opening a GUI.""" + +import asyncio +import json +from threading import Event + +from dimos_generated.geometry_msgs.msg import PointStamped, Twist +import websockets.asyncio.client as ws_client + +from dimos.core.global_config import GlobalConfig +from dimos.visualization.rerun.websocket_server import RerunWebSocketServer + + +def main() -> None: + server = RerunWebSocketServer( + g=GlobalConfig(rerun_host="127.0.0.1", rerun_websocket_server_port=0) + ) + points: list[PointStamped] = [] + velocities: list[Twist] = [] + stopped = Event() + + def receive_velocity(message: Twist) -> None: + velocities.append(Twist.decode(message.encode())) + if len(velocities) == 2: + stopped.set() + + subscriptions = [ + server.clicked_point.subscribe( + lambda message: points.append(PointStamped.decode(message.encode())) + ), + server.tele_cmd_vel.subscribe(receive_velocity), + ] + + async def send_events() -> None: + async with ws_client.connect(f"ws://127.0.0.1:{server.bound_port}/ws") as ws: + for event in [ + { + "type": "click", + "x": 1.5, + "y": 2.5, + "z": None, + "entity_path": "map", + "timestamp_ms": 1700000000123, + }, + {"type": "twist", "linear_x": 0.5, "angular_z": 0.8}, + {"type": "stop"}, + ]: + await ws.send(json.dumps(event)) + + try: + server.start() + asyncio.run(send_events()) + assert stopped.wait(3), "Viewer stop command was not delivered" + assert len(points) == 1 + point = points[0] + assert (point.point.x, point.point.y, point.point.z) == (1.5, 2.5, 0) + assert point.header.frame_id == "map" + assert (point.header.stamp.sec, point.header.stamp.nanosec) == (1700000000, 123000000) + assert velocities[0].linear.x == 0.5 and velocities[0].angular.z == 0.8 + assert velocities[1] == Twist() + print("Viewer click → CDR PointStamped: map (1.5, 2.5, 0), stamp 1700000000.123000000") + print("Viewer velocity → CDR Twist: forward 0.5 m/s, yaw 0.8 rad/s") + print("Viewer stop → CDR Twist: all six components zero") + finally: + for unsubscribe in subscriptions: + unsubscribe() + server.stop() + + +if __name__ == "__main__": + main() diff --git a/examples/message-codegen/demo_visual_servo.py b/examples/message-codegen/demo_visual_servo.py new file mode 100644 index 00000000000..d34f864db09 --- /dev/null +++ b/examples/message-codegen/demo_visual_servo.py @@ -0,0 +1,41 @@ +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Print CDR velocity commands for synthetic camera detections.""" + +from dimos_generated.geometry_msgs.msg import Twist +from dimos_generated.sensor_msgs.msg import CameraInfo + +from dimos.navigation.visual_servoing.visual_servoing_2d import VisualServoing2D + + +def main() -> None: + camera = CameraInfo(width=640, height=480, k=[500, 0, 320, 0, 500, 240, 0, 0, 1]) + controller = VisualServoing2D(CameraInfo.decode(camera.encode())) + for label, bbox, expected in ( + ("far", (270, 0, 370, 100), (0.5, 0)), + ("target distance", (245, 0, 395, 100), (0, 0)), + ("too close", (120, 0, 520, 100), (-0.3, 0)), + ("right of center", (370, 0, 470, 100), (0.36, -0.2)), + ): + command = controller.compute_twist(bbox, camera.width) + decoded = Twist.decode(command.encode()) + assert abs(decoded.linear.x - expected[0]) < 1e-9 + assert abs(decoded.angular.z - expected[1]) < 1e-9 + print(f"{label}: linear={decoded.linear.x:.2f} m/s, angular={decoded.angular.z:.2f} rad/s") + print("PASS: generated CameraInfo → visual servo → CDR Twist") + + +if __name__ == "__main__": + main() diff --git a/examples/message-codegen/demo_vl_images.py b/examples/message-codegen/demo_vl_images.py new file mode 100644 index 00000000000..dc09d6c7b58 --- /dev/null +++ b/examples/message-codegen/demo_vl_images.py @@ -0,0 +1,50 @@ +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Exercise generated image preparation with a fixed model response, offline.""" + +from dimos_generated.sensor_msgs.msg import Image +from dimos_generated.std_msgs.msg import Header +from dimos_generated.vision_msgs.msg import Detection2DArray +import numpy as np + +from dimos.models.vl.base import VlModel +from dimos.msgs.image import image_from_array + + +class FixedResponseModel(VlModel): + def query(self, image: Image, query: str, **kwargs) -> str: + return '[["synthetic target", 2, 4, 10, 12]]' + + def stop(self) -> None: + pass + + +def main() -> None: + image = image_from_array( + np.zeros((40, 60, 3), dtype=np.uint8), encoding="rgb8", header=Header(frame_id="camera") + ) + model = FixedResponseModel(auto_resize=(30, 20)) + resized, scale = model._prepare_image(image) + assert (resized.width, resized.height, scale) == (30, 20, 0.5) + detections = model.query_detections(image, "target") + wire = Detection2DArray.decode(detections.to_ros_detection2d_array().encode()) + assert wire.header == image.header and len(wire.detections) == 1 + print("Generated image: 60x40 → 30x20, scale=0.5") + print("Fixed model response → one generated CDR detection; source header preserved") + print("PASS: offline image preparation and response conversion (no model inference)") + + +if __name__ == "__main__": + main() diff --git a/examples/message-codegen/demo_vlm_image.py b/examples/message-codegen/demo_vlm_image.py new file mode 100644 index 00000000000..1e682394973 --- /dev/null +++ b/examples/message-codegen/demo_vlm_image.py @@ -0,0 +1,54 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Inspect the JPEG request produced from a generated image, without model inference.""" + +import base64 +from pathlib import Path +from unittest.mock import Mock, patch + +from dimos_generated.sensor_msgs.msg import Image +from langchain_core.messages import AIMessage +import numpy as np + +from dimos.agents.vlm_agent import VLMAgent +from dimos.msgs.image import image_from_array + + +def main() -> None: + pixels = np.zeros((120, 160, 3), dtype=np.uint8) + pixels[:, :80, 0] = 255 + pixels[:, 80:, 1] = 255 + image = image_from_array(pixels, encoding="rgb8") + model = Mock() + model.invoke.return_value = AIMessage(content="Demo response (no inference)") + with patch("langchain.chat_models.init_chat_model", return_value=model): + agent = VLMAgent() + try: + result = agent.query_image(Image.decode(image.encode()), "Describe the colors") + request = model.invoke.call_args.args[0][-1] + encoded = request.content[1]["image_url"]["url"].split(",", 1)[1] + destination = Path("build/message-codegen/demo/evidence/vlm-request.jpg") + destination.parent.mkdir(parents=True, exist_ok=True) + destination.write_bytes(base64.b64decode(encoded)) + assert result == "Demo response (no inference)" + print(f"Generated Image → CDR → VLM JPEG request: {destination}") + print("Expected image: red left half, green right half, 160 x 120 pixels") + print("Model invocation stubbed; no credentials or network inference used.") + finally: + agent.stop() + + +if __name__ == "__main__": + main() diff --git a/examples/message-codegen/demo_whole_body_transport.py b/examples/message-codegen/demo_whole_body_transport.py new file mode 100644 index 00000000000..66c2a234cde --- /dev/null +++ b/examples/message-codegen/demo_whole_body_transport.py @@ -0,0 +1,112 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Exchange generated motor commands and feedback through the whole-body adapter.""" + +from contextlib import ExitStack +import socket +import threading +from typing import Any +import uuid + +from demo_pubsub import free_port +from dimos_generated.dimos_msgs.msg import MotorCommandArray +from dimos_generated.geometry_msgs.msg import Quaternion, Vector3 +from dimos_generated.sensor_msgs.msg import Imu, JointState + +from dimos.core.transport import LCMTransport, PubSubTransport, ZenohTransport +from dimos.hardware.whole_body.spec import MotorCommand +from dimos.hardware.whole_body.transport.adapter import TransportWholeBodyAdapter +from dimos.protocol.service.zenohservice import ZenohSessionPool +from dimos.utils.testing.waiting import retry_until, wait_until + + +def demonstrate(backend: str) -> None: + with ExitStack() as stack: + pools = [ZenohSessionPool(), ZenohSessionPool()] + for pool in pools: + stack.callback(pool.close_all) + endpoint = f"tcp/127.0.0.1:{free_port(socket.SOCK_STREAM)}" + url = f"udpm://239.255.76.67:{free_port(socket.SOCK_DGRAM)}?ttl=0" + prefix = f"wb_{uuid.uuid4().hex[:8]}" + + def transport(topic: str, message_type: type, peer: int = 0) -> Any: + result: PubSubTransport[Any] + if backend == "lcm": + result = LCMTransport(topic, message_type, url=url) + else: + result = ZenohTransport( + "dimos/" + topic.lstrip("/"), + message_type, + session_pool=pools[peer], + scouting=False, + multicast=False, + gossip=False, + listen=[endpoint] if peer == 0 else [], + connect=[] if peer == 0 else [endpoint], + ) + stack.callback(result.stop) + result.start() + return result + + adapter = TransportWholeBodyAdapter(dof=2, hardware_id=prefix, transport_cls=transport) + stack.callback(adapter.disconnect) + assert adapter.connect() + states = transport(f"/{prefix}/motor_states", JointState, 1) + imu = transport(f"/{prefix}/imu", Imu, 1) + commands = transport(f"/{prefix}/motor_command", MotorCommandArray, 1) + received = [] + ready = threading.Event() + + def collect(message: MotorCommandArray) -> None: + received.append(message) + ready.set() + + stack.callback(commands.subscribe(collect)) + + def feedback_received() -> bool: + states.publish( + JointState(position=[0.25, -0.5], velocity=[0.1, 0.2], effort=[1.0, 2.0]) + ) + imu.publish(Imu(orientation=Quaternion(w=0.8, z=0.6), angular_velocity=Vector3(z=0.2))) + return adapter.has_motor_states() and adapter.read_imu().gyroscope[2] == 0.2 + + wait_until(feedback_received, timeout=5) + feedback = adapter.read_motor_states() + assert [value.q for value in feedback] == [0.25, -0.5] + assert adapter.read_imu().quaternion == (0.8, 0.0, 0.0, 0.6) + + def send_command() -> None: + assert adapter.write_motor_commands( + [ + MotorCommand(q=0.75, dq=0.1, kp=5.0, kd=0.5, tau=1.0), + MotorCommand(q=-0.25, dq=0.2, kp=6.0, kd=0.6, tau=2.0), + ] + ) + + retry_until(ready, send_command, timeout=5) + message = received[0] + assert list(message.q) == [0.75, -0.25] + assert list(message.kp) == [5.0, 6.0] + assert list(message.tau) == [1.0, 2.0] + assert message.header.stamp.sec > 0 + print( + f"{backend}: feedback q={[value.q for value in feedback]}, command q={list(message.q)}, IMU wxyz={adapter.read_imu().quaternion}" + ) + print(f"PASS: {backend} whole-body generated CDR commands and feedback") + + +if __name__ == "__main__": + for backend in ("lcm", "zenoh"): + demonstrate(backend) diff --git a/examples/message-codegen/evidence/blueprint-recording.md b/examples/message-codegen/evidence/blueprint-recording.md new file mode 100644 index 00000000000..fe1d61c68d4 --- /dev/null +++ b/examples/message-codegen/evidence/blueprint-recording.md @@ -0,0 +1,186 @@ +# Native blueprint recording and replay + +Verified locally on 2026-09-30 using the current runtime batch on top of +`f4a269ce4bc766a1ff876ed2b6f5ac432e6d3900`. + +- 33 recorder, SQLite path, stamped-message and TF regression tests passed. +- Strict scoped mypy passed for RustRecorder and the SQLite replay helper. +- The actual Python -> C++ -> Rust -> Python Zenoh relay produced three + custom LineSegments3D samples, three 640x480 RGB images, and three PoseStamped + samples. Native edits incremented line weight twice; image bytes were exact. +- Rust recorder reported received=9, written=9, encode_errors=0 and exited cleanly. +- MCAP had CDR channels and embedded ros2msg schemas. Independent rosbags Jazzy + decoding succeeded for every row; all three streams contained samples 0,1,2. +- After all producers stopped, DimOS ReplayModule emitted every recorded value + with byte equality and per-stream order preserved. + +The retained local artifact is +`build/message-codegen/demo/evidence/blueprint-current-zenoh-v3.mcap`; terminal +log is `/tmp/cdr-blueprint-recording-v3.log`. Earlier failed artifacts were +preserved. This demo uses installed built-in custom messages; arbitrary installed +external-message capture has separate acceptance requirements. + +The first recorder run exposed inherited TF wiring being passed despite +`record_tf=False`. RustRecorder now filters native launch topics to declared +recording streams; regression tests cover TF enabled and disabled. Another run +exposed float-second replay treating 1 ns spaced epoch timestamps as static. +The demo now uses realistic 100 ms intervals while checking exact nanoseconds; +sub-float-resolution scheduling is not claimed. + +No hardware ran and host tuning was check-only. Default-interface LCM, human +Foxglove/Rerun inspection, full dependency retirement and final installed-package +acceptance remain open. This evidence does not mark stages 5 or 6 complete. + +## Follow-up regression batch + +The published parent `f4a269ce4` codegen workflow 36763634296 succeeded. Main +workflow 36763630343 ended cancelled after ARM reported 6170 passed, 228 skipped +and one documentation-branding failure. Both offending spellings in +`docs/usage/lcm.md` are repaired; other Python jobs were cancelled, not passed. + +Follow-up local checks: 15 Joy/Header/LineSegments3D/branding tests and 53 +stamped-covariance/time tests passed. Migrated suites retain numeric payloads, +empty and large sequences, covariance patterns, copy independence, explicit +clock and datetime conversions, and add independent ROS CDR checks. Legacy +convenience constructors, custom string formatting, inheritance, Path-based +line packing and implicit timestamps are intentionally retired. Generated ROS +time overflow now reports an explicit ValueError; both int32 boundaries and +values immediately outside them are covered. Scoped mypy passed time.py. + +## Future cross-stack compatibility note + +Parent review on 2026-09-30 identified EpisodeStatus in the independent open +chain #3854 -> #3855 -> #3921 -> #3931 -> #3942. It is used by typed live Out, +collection recorder input/source timestamp extraction, offline episode boundaries +and TUI RPC. That type is absent from this CDR checkout. Its eventual integration +would need a coordinated generated CDR design if that proposal is adopted; a JSON substitution would not satisfy +the typed CDR recorder contract. The user has not selected the integration design. +No code or commits from that chain were imported or changed. This is an explicit +future compatibility note, not a regression introduced by this branch. +CDR is not a prerequisite for merging that separate stack: it targets current +main and remains independent. No merge is authorized for this CDR draft. + +## Image and value-type retirement follow-up + +Local verification after `0bac7f454`: 52 Path/basic-covariance checks, five +CameraInfo calibration checks, and 58 generated-image/compression/detection/heavy-import +checks passed. Strict mypy passed image.py, Rerun message_helpers.py and both +manual message tools. The original JPEG/JXL mean-error and compressed-size +thresholds, lossless PNG/JXL depth, codec effort comparison, camera calibration +values, Rerun box geometry/labels and path sequence behavior are retained. + +PNG/JXL encoding and decoding live in external functions over generated values. +Source headers and 16UC1 depth units survive, including big-endian inputs. Raw +64FC1, 16SC1 and 32FC3 views remain supported; unsupported compression types are +rejected explicitly. The local venv was missing the existing lockfile dependency +imagecodecs 2026.6.26; its 27.5 MiB wheel was installed without changing project +dependencies or system packages. + +Image file/selection tests use deterministic temporary pixels and real sharpness +calculation in virtual-time windows. The manual pickle tool retains only its +raw-array input path, converting to generated images after loading; no old typed +message decoding was added. Manual publishers and hardware tools were not run. + +## Geometry and map follow-up + +Local checks: 42 wrench/geometry, 12 odometry, 78 point-cloud/viewer and 32 occupancy +tests passed. Strict scoped mypy passed geometry.py, pointcloud.py, occupancy.py +and the Rerun helper. Force/torque arrays and axis-aligned finite cloud bounds +are external functions; generated types stay value containers. Point-field tests +retain intensity, zero offset_time, tag, line and all original overlap scenarios. +Occupancy tests retain unknown/free/occupied counts, threshold filtering, origin +yaw, coordinate conversion and obstacle-preserving reduction. Borrowed views +remain read-only; tests mutate declared message data explicitly. ROS float32 +resolution is compared with numerical tolerances instead of assuming Python +float64 storage. + +## Generated geometry regression continuation + +The Vector3, Quaternion, Pose, Transform, and Twist suites now use generated +values and explicit external math. Non-unit Hamilton products/inverses, zero +errors, rotated local offsets, frame composition, independent ROS decoding, +and original numerical tolerances remain covered. Constructor coverage now +checks explicit nested fields, deep-copy isolation, covariance lengths and +layout, zero/default timestamps, exact nanoseconds, and rejection of retired +polymorphic fields. Stamped values are distinct from their payloads. + +Offline message-directory validation: **614 passed** with only +`tf2_msgs/test_TFMessage_lcmpub.py` excluded. The attempted whole-directory run +also produced 614 passes, but that real multicast test failed host LCM self-test +and thread teardown; this remains an environment-dependent gate, not a pass. +No host routing/firewall configuration was changed. Strict scoped mypy for +`dimos/msgs/geometry.py` and repository hooks passed. + +Published commit `0bac7f454bcd2f61766c0246e38a40016c62746c` has terminal success +for main CI 36765815202 and codegen CI 36765824915. Self-hosted lanes were +skipped. These results do not cover subsequent local commits until their own +exact-HEAD workflows complete. + +## Python dependency retirement and remaining consumers + +The 47 obsolete Python message implementation files have been removed after +migrating their remaining Python imports. The root project now declares raw +`lcm-dimos-fork` directly and no longer depends on `dimos-lcm`. The task venv's +installed `dimos-lcm` was also uninstalled: **624 offline message/voxel tests +passed without it**. The lock removes only `dimos-lcm` and its unused +`foxglove-websocket` dependency; no remaining package versions changed. +Re-resolution required restricting the existing optional `a750-control` wheel +to Python 3.12, its only published/locked ABI, instead of requesting it on 3.10/3.11. + +The retired typed-pickle voxel regression fixture is now deterministic CDR +geometry, with exact voxel counts at three resolutions, repeated-ingestion +invariance, numerical coordinates, and range preservation. Raw PLY/image +occupancy fixtures remain supported. Full local test collection reached 6555 +tests with 16 collection errors for absent optional dependencies (coacd, h5py, +open_clip, plotext, reportlab, requests_mock, trimesh, ultralytics, yourdfpy), +not obsolete message imports. This is not a full-suite pass. + +Remaining examples and the optional LeRobot runtime use generated image, +pose, point-cloud and UInt32 button fields. The runtime copies image pixels +before retaining an observation. Its own scoped mypy configuration passes, +but its behavioral suite still requires the absent LeRobot environment. +The virtual robot's pure unicycle integration passed a numerical check; +the C++ controller compiled against the installed CMake message package and +raw LCM, without being run. The obsolete Lua and TypeScript LCM-codec demos +are retired explicitly; Lua generation is deferred and the browser CDR SDK +is the supported web path. Transform documentation passed 5 executable Python +blocks; eval documentation passed all 6 blocks (4 no-result, 2 with results). + +Outstanding dependency gate: the M20 onboard DrDDS/Zenoh C++ bridge still +fetches/uses old message headers. The local environment lacks the vendor DrDDS +SDK, and its `sensor_msgs::msg`/`nav_msgs::msg` types collide with generated C++ +type names. A verified vendor boundary/codec migration is still required; +no board build, deployment or robot operation has been performed. Historical +manual replay fixtures and final viewer/transport acceptance also remain open. + +The shared Go2 moment fixture now takes an explicit CDR SQLite path; its tests +create generated recordings instead of resolving the old pickle dataset name. +The manual memory import/query tool now copies 1024 deterministic CDR samples +into an isolated destination. Its original >1000 counts, >10-second duration, +pagination, ordering, overlap, lazy decoding and pose assertions remain. +All **18 tool checks passed**, including cached CPU CLIP embedding/search with +model network access disabled; no new weight download was needed. These +synthetic image results validate the pipeline, not real-scene recognition. + +The separate real detection fixture `unitree_go2_lidar_corrected` is absent +locally and its LFS archive is 1,212,727,745 bytes. Regenerating it while retaining +its semantic assertions is still an explicit outstanding fixture gate. + +### DimSim standalone codec audit + +The remaining browser/Deno `@dimos/msgs` consumers now use pinned Foxglove CDR +codecs and a standalone full-schema bundle exported from canonical generated +messages. RGB publishes raw RGBA Image, depth publishes 16UC1 Image, and lidar, +odometry and cmd_vel use PointCloud2/PoseStamped/Twist with ROS 2 channel names. +The existing LCM transport and LC02 WebSocket envelope remain. JPEG stays only +in eval/sidebar previews. No legacy decoding fallback was introduced. + +Verified locally: Vite production build; complete Deno CLI type check with a +frozen lock preserving unrelated original dependency versions; three socket-free +codec tests (large image envelope, all velocity components, malformed input); +canonical-schema drift pytest; strict Python exporter typing; pre-commit checks. +Four Deno-encoded payloads (Twist, PoseStamped, Image and PointCloud2) decoded in +both generated Python and independent rosbags with exact values/source stamps; +a generated Python Twist decoded in Deno with exact command components. +Full browser/physics simulation acceptance remains unverified. These checks +neither operate hardware nor prove host multicast or end-to-end rendering. diff --git a/examples/message-codegen/evidence/camera-export-runtime.md b/examples/message-codegen/evidence/camera-export-runtime.md new file mode 100644 index 00000000000..9eaf06abc0d --- /dev/null +++ b/examples/message-codegen/evidence/camera-export-runtime.md @@ -0,0 +1,65 @@ +# Remaining camera and plain export consumers + +ZED TF publishing/tracking uses generated TransformStamped and TFMessage, +with exact stamped headers, extracted inverse/composition helpers and the +existing camera/optical frame conventions. Three offline callback tests +passed with only SDK responses mocked; no SDK Camera or start() was called. +GStreamer emits generated BGR Image values owning pixels after buffer unmap. +One mocked callback test passed, retaining invalid/pre-2000 timestamp rejection. +The original float-second/offset calculation is retained; this is not a claim +of arbitrary nanosecond preservation for that existing source path. + +The camera calibration CLI subscribes to generated Image and uses the explicit +BGR helper. All 22 existing synthetic/mocked tests passed, including YAML +round-trips, raw topic delivery, fisheye calibration and timeout cleanup. +The fixed FlowBase mount is a generated Pose with identical numeric values. +Strict scoped mypy passed for all five changed production camera/CLI files. + +Plain agent recording export accepts generated Image/PointCloud2, preserving +source frames and explicit lossless PNG/XYZ/RGB CSV/JSON output. Six existing +agent-boundary/export tests passed without starting an agent or models. +Strict scoped mypy passed for the export implementation. + +## Approved control and documentation continuation + +The combined camera, export, PGO, WebXR, episode-monitor, coordinator and mock +arm regression run passed 192 tests on Python 3.12.14. This includes GTSAM +optimization and the OpenArm bounded 100-step IK test; no physical robot was +connected. Scoped strict mypy passed all eight control production files. + +WebXR serves generated PoseStamped/Joy schemas and accepts explicit CDR data +frames with channel/type metadata. Buttons remain a local bit-field helper; +wire ports publish generated UInt32. Six added checks cover schema advertisement +and rejecting mismatched types, wrong encodings, unknown channels, truncation +and trailing bytes before control handlers. Existing deadman, disconnect and +single-client assertions remain enabled. The actual browser loadCommandSchemas +and sendCommand functions ran in Node with the already installed, pinned +@foxglove/rosmsg 5.0.5 and @foxglove/rosmsg2-serialization 3.1.2 dependencies on +the trusted recovery machine. Python decoded the returned framed CDR values, +including exact nanoseconds, nested poses, analog axes, digital buttons and +sequence numbers. This is offline encoding evidence, not a headset/robot demo. +The browser imports these versions from esm.sh; network access is needed to load +the page dependencies on first use. + +Memory index and plotting documentation passed md-babel-py 1.4.0: 9/9 and 6/6 +non-skipped Python blocks respectively, using CPU execution and cached official +openai/clip-vit-base-patch32 weights. Both brightness-comparison blocks also +executed successfully. Run from docs/capabilities/memory so relative assets +land alongside the documents. Refreshed results and images describe a new +synthetic CDR recording, not the historical office recording. The old 187 MiB +recording is unnecessary for this walkthrough and was not downloaded. The CLIP +cache occupies about 1.2 GiB because the upstream loader fetched both .bin and +.safetensors weights (roughly 605 MB each). Skipped detector sections were not +executed; their historical screenshots are not new-format acceptance evidence. + +The unchanged 100 ms Zenoh sync/async RPC assertion passed in six independent +cold pytest processes locally. Python 3.10's earlier CI failure remains +unexplained; no pre-warming, retry inside the test, or timeout relaxation was +applied. Main CI at e386163c56 terminated cancelled: Python 3.10 had 6316 passed, +82 skipped and this one RPC failure; Python 3.12 was cancelled without a final +test result. Codegen, lint, Web, Rust, native, ARM and Python 3.11 passed. +The next published commit requires its own terminal CI results. + +Legacy consumers/dependency retirement, full clean build matrix, human viewer +and cumulative live demo gates remain open. Default-interface LCM multicast +and real hardware gates are not accepted by these offline checks. diff --git a/examples/message-codegen/evidence/compressed-image-runtime.md b/examples/message-codegen/evidence/compressed-image-runtime.md new file mode 100644 index 00000000000..9dfca986527 --- /dev/null +++ b/examples/message-codegen/evidence/compressed-image-runtime.md @@ -0,0 +1,35 @@ +# Standard compressed-image transport and raw adapters + +JPEG LCM and JPEG SHM now serialize generated `sensor_msgs/CompressedImage` +CDR values, retaining the original image header and standard ROS compressed +format. The application-facing transport remains generated `Image`; its wire +topic declares `CompressedImage`, including after pickling. WebRTC video uses +explicit generated ROS encodings and array views without changing frame timing. +Raw robot bridge JSON/JPEG/XYZ contracts and deadman behavior remain unchanged; +M20 ports/scalar commands now use generated messages without changing guards. +Tests use synthetic values, capture-only M20 construction, and local loopback. + +**23 focused tests passed** in the final combined image/codec/WebRTC/raw-bridge/ +M20 run. The earlier codec/video/registry run passed 33 tests, and separate raw +bridge/M20 runs passed 7 and 1 respectively (overlapping coverage, not summed). +Strict scoped mypy passed for all seven changed production files; repository +hooks passed after normal license-header insertion. The image fixture expanded +the existing 133 KiB cafe archive; this check does not prove legacy Image API +retirement. The exact Python 3.11 CI failure in the collection E2E fixture was +repaired by importing generated Image and viewing its pixel buffer with +`image_view`; local E2E collection remains blocked by missing h5py. + +A fresh synthetic MCAP contains 150 messages across five channels (30 each), +3,123,201 bytes. Native Rerun 0.32.0 imported it with Python provider imports +absent and raw fallback disabled. Its RRD contains typed image/encoded-image, +pose/TF, and nested telemetry fields; `check_rerun_mcap.py` passed. Evidence is +under ignored `build/message-codegen/viewers-current/evidence`. This is offline +conversion evidence, not GUI/Foxglove acceptance. The viewer script prioritizes +the demo binding over the root binding and now disables Rerun raw fallback. + +Exact prior published HEAD 2334dd05d3e62b1fbc05f3e773b1ff1696f345c1 passed +message-codegen, strict lint, Native C++/Rust, Rust, Web, and ARM CI. Python 3.11 +had 6292 passed, 82 skipped, and two fixture errors fixed above; 3.10/3.12 were +cancelled by fail-fast. md-babel still fails on old memory codecs. The eight +paused files remain excluded. WebXR control edits and model-dependent memory +document execution have not been retried. Full migration/retirement remains open. diff --git a/examples/message-codegen/evidence/experimental-consumers.md b/examples/message-codegen/evidence/experimental-consumers.md new file mode 100644 index 00000000000..1a42d385ec5 --- /dev/null +++ b/examples/message-codegen/evidence/experimental-consumers.md @@ -0,0 +1,44 @@ +# Geometry, simulation, eval and experimental consumers + +The previous published HEAD `3a233c549807d0b7172551702aa9f356d38ed817` passed +main CI run 36750845515 and message-codegen run 36750850610. Python 3.10, 3.11, +3.12 and ARM all reached terminal success, as did documentation, Web, lint, +Rust and native C++/Rust builds. Self-hosted and large-data tests were skipped +by the dispatch workflow. The original 100 ms RPC assertion was unchanged; +its previous isolated timeout root cause is not established by this green run. + +The subsequent geometry/simulation/eval/recall/detection regression passed +120 tests on Python 3.12.14; strict scoped mypy passed 19 production files. +This includes 67 original geometry numerical tests, explicit stamped frame +mismatch rejection, pursuit forward gating/angular clamp/arrival stop, track +pose publication, Habitat wire geometry, mock environment freshness checks, +image sharpness/brightness selection, thumbnails and source stamps, invalid +depth rejection and real reactive Reid alignment with an owned thread pool. +Only external tracker/inference/transport boundaries are mocked. + +The Habitat server and connection use standalone generated CDR messages. +The installer now builds the same package for the separate native interpreter +instead of installing dimos-lcm; its Nix shell provides compiler and CMake. +The installer passed shell syntax checks but was not executed: no Habitat, +scene assets or native simulator was installed/started by this validation. + +Python 3.9.25 was downloaded into ignored `build/python` (27.0 MiB), and an +isolated environment at `build/message-codegen/habitat-py39/venv` installed +numpy 1.26.4, Zenoh 1.10.1 and pinned pybind11 3.0.1 build dependencies. +The Habitat-specific generated package contains an 18-type closure, builds +against the existing Fast-CDR 2.4.0 local prefix, and installs without DimOS. +Its actual server functions encoded image, CameraInfo, PointCloud2, Odometry +and TFMessage under Python 3.9. Independent rosbags decoding under Python 3.12 +confirmed all five payloads and exact source stamp (12 sec, 250000000 nanosec). +The offline check is `examples/message-codegen/demo_habitat_native.py`: + +```bash +build/message-codegen/habitat-py39/venv/bin/python -I \ + examples/message-codegen/demo_habitat_native.py +``` + +The output is JSON plus five small CDR samples in the ignored evidence folder. +These checks do not prove the full Nix/conda installer, EGL rendering, a real +camera, robot control, large historical recordings, or human viewer acceptance. +Remaining consumers, fixture/dependency retirement and final cumulative demos +remain open; no whole-plan completion is claimed. diff --git a/examples/message-codegen/evidence/final-consumer-cutover.md b/examples/message-codegen/evidence/final-consumer-cutover.md new file mode 100644 index 00000000000..0868c5dd29c --- /dev/null +++ b/examples/message-codegen/evidence/final-consumer-cutover.md @@ -0,0 +1,45 @@ +# Remaining consumer cutover + +The previous published batch `a36cc9565e92b4acda71d5e883b3480362b6b9e7` +passed message-codegen run 36755062352. Main run 36755058007 failed three +Python 3.10 assertions: two simulated settling fixtures still constructed the +old PoseStamped, and image resizing had a module-level OpenCV import. Other +Python lanes were cancelled by fail-fast. Web, documentation, lint, Rust and +native builds passed. This is not a full green CI result. + +The follow-up migrates eval dataset/prompt/coverage fixtures and the actual +coverage scorer to generated poses. Pose text remains numeric and includes +frame, position and quaternion through an external agent formatter; the +original selection, downsampling, timing, scoring and stopping assertions +remain. OpenCV resizing now imports its native extension on demand. + +Remaining mock-only consumers use generated messages: Alfred wheel odometry +preserves coordinate inversion, source time and local velocity; existing +Cartesian/trajectory controllers use generated JointCommand/RobotState and +retain PID limits, joint count and interpolation. No driver/controller start +or physical robot operation was performed. Passive lidar conversion and +recorder scripts were type checked, without executing device/network setup. + +TemporalMemory publishes the existing generated dimos_msgs/EntityMarkers +schema, with nested geometry_msgs/Point positions. Its obsolete private JSON +message wrapper is removed. The external Rerun helper preserves colors, +labels, coordinates and marker radius. An independent rosbags decoder verifies +Unicode metadata, dependency closure, positions and exact sec/nanosec source +time. No rendering method is attached to a generated message. + +The alternate pgo_auto implementation uses generated TransformStamped/clouds +and external geometry/cloud algorithms. Solver parameters and the existing +nearest-neighbor correction lookup remain unchanged. Tests exercise time zero, +clamped lookup endpoints, frame composition, unchanged payloads, empty input, +and a synthetic cloud through real GTSAM; no recording download is required. + +Scoped strict mypy passed all 15 changed production files on Python 3.12.14. +The generation/build succeeded with the existing authoritative nested Point +schema. The full targeted offline suite passed 147 tests, excluding one +explicitly networked TemporalMemory integration test. No acceptance gate was +weakened or marked complete based only on these tests. + +Legacy convenience modules, their tests and fixtures, manual tools and the +external dimos-lcm package are still present and require final retirement. +Default-interface multicast and human viewer/physical demonstrations remain +separate unresolved gates; these results do not establish whole-plan completion. diff --git a/examples/message-codegen/evidence/fixture-routing-cutover.md b/examples/message-codegen/evidence/fixture-routing-cutover.md new file mode 100644 index 00000000000..f9afdd9266b --- /dev/null +++ b/examples/message-codegen/evidence/fixture-routing-cutover.md @@ -0,0 +1,33 @@ +# Fixture routing and generated-message regression coverage + +Core/worker, TF, browser, detector, MCP and offline manipulation fixtures now +use generated ROS-shaped messages. The test spy encodes CDR and its topic names +use `package/msg/Type`. Browser paths preserve the same numeric points; map +click assertions inspect nested PointStamped fields. The offline IK fixtures +retain the original rotation composition and numeric acceptance thresholds. +G1 debug command tools use generated CDR without running a driver. + +Verified locally: + +- 12 core baked-host tests and 13 worker tests passed. +- The existing bidirectional TF IO integration test passed (48 unrelated tests + deselected), using generated TransformStamped inside TFMessage. +- 35 isolated Space rendering tests passed. +- 109 offline CDR spy/browser-codec/framing tests passed. +- 15 offline Pi adapter tests passed after fixing the exact CI assertion below. +- All four executable blocks in `docs/usage/lcm.md` passed. The guide now + describes generated CDR and raw LCM transport instead of rich LCM overlays. + +The previous published HEAD `11bae55835e56c33f3940a9563cf15cfc81993b2` has a +successful message-codegen run 36760272088. Its main run 36760267791 completed +with an ARM failure: 6158 passed, 228 skipped and one failed. The failed Pi +recording-export test read `obs.data.position.x`; the fixture already stores +generated PoseStamped and requires `obs.data.pose.position.x`. This batch +preserves its selected-stream, timestamp and numeric-content assertions. +Other Python lanes and lint were cancelled, not passed. Rust, native, Web and +docs jobs passed. A new exact-HEAD full CI result is still required. + +These checks did not execute browser UI, external inference, full simulator, +hardware, or old recording fixtures. Remaining rich-message implementations, +legacy tests/assets and the dimos-lcm dependency still require retirement; +stages 5–6 and human viewer acceptance remain open. diff --git a/examples/message-codegen/evidence/manual-tools-replay.md b/examples/message-codegen/evidence/manual-tools-replay.md new file mode 100644 index 00000000000..cf0545bfe86 --- /dev/null +++ b/examples/message-codegen/evidence/manual-tools-replay.md @@ -0,0 +1,26 @@ +# Manual tools and SQLite replay cutover + +The pending tool batch now uses generated messages and external image, cloud, +pose and viewer adapters. The VLM batch tool requires `DIMOS_VLM_RECORDING` +pointing to `.db/` containing generated CDR images; it no +longer assumes a historical typed-pickle recording is compatible. + +Offline verification: 40 tests passed across `test_sqlite_moment.py`, +`test_generated_views.py` and `test_generated_cloud_bridge.py`. Temporary SQLite +recordings verify absolute database paths, CDR values, seek/window looping, +missing frames and SensorMoment publication. File image tests preserve RGB +values and the supplied nanosecond header. Explicit cloud RGB/RGBA colors stay +aligned after nonfinite/height filtering and reject invalid channels. + +Scoped strict mypy passed seven files: image.py, moment.py, replay.py, +message_helpers.py, tool_visualizer.py, tool_localize.py and tool_vlm.py. +Required hooks passed after formatting. Manual broker/model/robot tools were +not executed; this evidence establishes their shared offline helpers, not +end-to-end broker or hardware behavior. Historical Go2 recordings still need +replacement before the self-hosted replay tools can run on new-format data. + +At published HEAD `11bae55835e56c33f3940a9563cf15cfc81993b2`, codegen CI +36760272088 completed successfully (standalone and independent Jazzy reference). +Main CI 36760267791 remained in progress at this checkpoint: Rust, native, +Web and docs passed; lint was cancelled and Python lanes were still running. +This is not an all-green or final migration acceptance claim. diff --git a/examples/message-codegen/evidence/real-detection-fixture.md b/examples/message-codegen/evidence/real-detection-fixture.md new file mode 100644 index 00000000000..a7708d7472d --- /dev/null +++ b/examples/message-codegen/evidence/real-detection-fixture.md @@ -0,0 +1,62 @@ +# Real-scene detection fixture after CDR cutover + +The replacement `unitree_go2_detection_cdr` fixture contains the five moments +used by the existing detection and object-history tests (seeks 10, 12, 14, 16, +and 18 seconds). It contains generated Image, PointCloud2 and PoseStamped CDR, +complete `ros2msg` schemas, and a manifest of source paths, source SHA-256 hashes, +recording times, source sec/nanosec stamps and encoded payload hashes. + +Source: project LFS `data/.lfs/unitree_go2_lidar_corrected.tar.gz`, endpoint +`https://lfs.dimensionalos.com/dimensionalOS/dimos`, SHA-256 +`51a817f2b5664c9e2f2856293db242e030f0edce276e21da0edc2821d947aad2`. +The original archive is 1,212,727,745 bytes; safe extraction produced +3,776,299,125 regular-file bytes across 3,490 archive entries. The source is +preserved. The replacement archive is 5,823,304 bytes, SHA-256 +`966a7b4d542a58a7891b7c0fc4951e3ea3975c56f52cd99b0a5f0541198d2d40`. + +Selection exactly follows the historical fixture: choose lidar by nearest +recording-envelope timestamp to first lidar plus seek; choose image and odometry +by nearest recording-envelope time to the selected lidar's source timestamp. +The archived odometry firmware timestamp is stale relative to its envelope; +retain it rather than silently repairing the recording. The test consumer maps +its odometry parent to `world`, matching the historical fixture and Go2's current +configured parent. It labels the image `camera_optical` as before. + +The explicitly pinned archival exporter is +[`demo_export_detection_fixture.py`](/examples/message-codegen/demo_export_detection_fixture.py). +It is an offline fixture maintenance tool, not a runtime compatibility reader. +It accepts only the named archive hash and a small allowlist of archived state +records. It never installs old message classes. Runtime and pytest decode only +CDR. After safely extracting the verified archive, reproduce the payloads: + +```bash +python examples/message-codegen/demo_export_detection_fixture.py \ + --archive data/.lfs/unitree_go2_lidar_corrected.tar.gz \ + --source /path/to/extracted/unitree_go2_lidar_corrected \ + --output /path/to/new/unitree_go2_detection_cdr +``` + +The exporter refuses an existing output directory. All 15 reproduced payloads +matched the replacement fixture byte for byte. Export checks preserve exact +image pixels and point coordinates within float32 wire precision. Source poses +and timestamps are copied explicitly to generated ROS-shaped fields. + +Validation: **26 detection-type tests passed; one manual live-publish demo was +deselected**. This includes the original real-scene OBB/AABB extents, centers, +point-count thresholds, 2D round-trip identity/confidence, and five-frame object +history assertions, with their original numerical tolerances. CPU YOLO uses +repository-locked Ultralytics 8.4.14 and project `models_yolo/yolo11n.pt`. +The model archive SHA-256 is +`01796d5884cf29258820cf0e617bf834e9ffb63d8a4c7a54eea802e96fe6a818`. +No robot, external inference API, or hardware controller was run. + +```bash +HF_HUB_OFFLINE=1 TRANSFORMERS_OFFLINE=1 YOLO_AUTOINSTALL=false \ +CUDA_VISIBLE_DEVICES='' python -m pytest -o addopts='' --import-mode=importlib \ + dimos/perception/detection/type -k 'not test_guess_projection' -q +``` + +The same audit found G1 viewer callbacks still calling rich cloud and stamped +point methods. They now use the external XYZ helper and `PointStamped.point`; +three offline Rerun geometry tests verify coordinates, height colors, lifted +waypoints/goals and non-finite rejection. No live G1 stack was started. diff --git a/examples/message-codegen/evidence/recorder-graph-runtime.md b/examples/message-codegen/evidence/recorder-graph-runtime.md new file mode 100644 index 00000000000..ae1df98caca --- /dev/null +++ b/examples/message-codegen/evidence/recorder-graph-runtime.md @@ -0,0 +1,37 @@ +# Recorder graph and isolated Python contracts + +Point-LIO and FAST-LIO recorder ports now match their generated Odometry and +PointCloud2 producers. Their existing last-odometry pose anchoring is unchanged. +The RealSense/Mid360 assembly recorder and relocalization replay ports also +use generated message identities. Offline camera/relocalization viewer overrides +call external cloud helpers, and the standalone RealSense mount has an explicit +identity quaternion, with an offline identity-matrix check. No devices, +SLAM native processes or dataset replays were started. + +Strict scoped mypy passed for these five production files. Three existing +RealSense configuration/registry tests passed; the actual generated mount matrix +and Rerun cloud helper were checked separately with a synthetic cloud. The four +new recorder/graph/render regression cases could not collect locally because +Recorder imports torch through EmbeddingModel; they remain pending CI, without +mocking torch or downloading models. + +The isolated Python example's host contract and runtime now both use generated +Int32 and keyword scalar construction. Its existing process-level E2E producer +was migrated as well. **26 local tests passed** across module/bootstrap and a new +actual runtime callback test: input CDR is decoded, a negative value is multiplied, +and the generated output is independently re-decoded. No external worker was +started or environment installed locally. Process-level E2E remains a CI gate. + +Exact prior published 23b8e1dd3fe3880c952acee39d860b7104c723b6 passed +codegen, strict lint, native, Rust, Web, and ARM. Python 3.10 had 6286 passed, +83 skipped and two dataset-export failures; 3.11/3.12 were cancelled. +The fixture now reaches export, where generic coercion had treated generated +Image as an object scalar. `resolve_field` explicitly views raw generated images +and decodes standard generated CompressedImage pixels. **36 dataprep tests +passed**, including CDR roundtrips and mono16 endian/row-padding behavior. +Strict scoped mypy passed for core.py. Full HDF5/LeRobot E2E still needs CI. + +Historical lcm/jpeg SQLite codecs now give deliberate-cutover guidance, while +unknown codec IDs retain their distinct configuration error. No legacy decoding +or in-place rewriting was added. Model-dependent memory docs and the eight held +files remain paused, so this error improvement is not a passing md-babel claim. diff --git a/examples/message-codegen/evidence/robot-simulation-consumers.md b/examples/message-codegen/evidence/robot-simulation-consumers.md new file mode 100644 index 00000000000..1bd0ce3515a --- /dev/null +++ b/examples/message-codegen/evidence/robot-simulation-consumers.md @@ -0,0 +1,55 @@ +# Hosted arm, robot and simulation consumer continuation + +The hosted arm uses generated PoseStamped, TwistStamped and Float32 with the +base WebXR explicit channel/type/CDR framing. Its existing 24 tests pass, +including E-stop, stale/future/out-of-order rejection, per-hand watermarks, +disconnect and scaling. Blueprint transport type keys match generated ports. +No physical device was connected or controlled. + +Drone odometry and camera callbacks use generated headers and nested poses, +explicit image views and stamped TF edges. Mock numerical processing, PID and +camera tracking checks pass; this does not validate historical drone pickle +recordings, a camera pipeline or a live MAVLink connection. + +The old Unitree Odometry subclass is removed. Device dictionaries convert to +standard generated PoseStamped. The mock robot's CDR port preserves the source +header; a separate TimestampedData Python-object port carries publication time +for the existing latency benchmark. The original 179-entry raw odometry fixture +and rotation checks pass. Its 15,685-byte LFS archive was fetched; large legacy +recordings were not fetched for this validation. + +Go2 standard DDS payloads decode through generated codecs. Six independent +checks cover little/big-endian Odometry against rosbags, exact timestamp and +covariance fields, arbitrary padded PointCloud2 layouts, the device's IMU +quaternion ordering, compressed JPEG payloads and mocked H264 frame emission. +The device-specific HeightMap layout remains separate. The optional ROS fallback +uses generated standard values; an installed ROS runtime is not required. + +Blueprints, manipulator configurations and simulation interfaces use generated +message type keys. Rerun optical-frame and path-height overrides are explicit +helper parameters without modifying source messages. Existing mapping and +manipulation safety assertions remain enabled. + +MuJoCo publishes generated PoseStamped, Imu and TFMessage. All 12 existing +module tests pass with temporary synthetic MJCFs and mocks, including camera +relative transforms, source frame timestamps and SHM ready ordering. Added +assertions decode the actual published pose and IMU CDR bytes. These are bounded +offline tests, not a robot or viewer demonstration. MuJoCo 3.10.0 from the lockfile +was installed only in the project virtualenv (19.9 MiB wheel). + +The combined hosted arm, DDS, Rerun, manipulation, MuJoCo, odometry and timestamp +regression passed 119 tests on Python 3.12.14. Strict scoped mypy passed all 40 changed production files. Required repository +hooks passed. +Large historical perception/DDS/replay fixture tests, default-interface LCM +multicast, remaining consumer and dependency retirement, the clean build matrix +and human viewer/cumulative demo gates remain open. + +## Previous exact-commit CI + +At 36a0fd2c35848d26b1daf19b4b744e6d49afd9fd, message-codegen run 36744961105 +succeeded. Main run 36744956207 terminated cancelled: native C++/Rust, Rust, +lint, web and complete md-babel succeeded. ARM failed seven hosted-arm tests +that still dispatched through legacy fingerprints; this batch fixes that path. +Other Python lanes were cancelled by fail-fast, so they do not supply a terminal +RPC result. The unchanged 100 ms RPC assertion has no pre-warming or relaxed +limit. A subsequent published HEAD requires its own terminal CI results. diff --git a/examples/message-codegen/evidence/unitree-spot-camera-runtime.md b/examples/message-codegen/evidence/unitree-spot-camera-runtime.md new file mode 100644 index 00000000000..5f9639c6634 --- /dev/null +++ b/examples/message-codegen/evidence/unitree-spot-camera-runtime.md @@ -0,0 +1,45 @@ +# Unitree, Spot and camera metadata boundaries + +B1 command/joystick/transport types are generated TwistStamped and Int32. +The handler reads nested twist fields; mode changes, thresholds, saturation and +200 ms watchdog remain unchanged. Existing MockB1 watchdog tests passed (10 +tests together with the prior odometry case). Three additional CDR cases passed +for WALK/STAND mapping and captured zero publication; neither a robot socket +nor a pygame control window was opened. + +G1 high-level RPC/skill contracts and keyboard operator gates/slider values use +generated Twist, Int8 and Float32. G1's lossless depth storage codec is lz4+cdr. +Two tests captured real constructor/callback outputs with mocked pygame and RPC +collaborators; operator-only mode emitted no movement. The G1 recorder metadata +case awaits CI because local Recorder imports torch. + +Lazy named calibration YAML loading is an external helper returning ordinary +generated CameraInfo. ZED calibration access is tested with its SDK absent. +Seven calibration tests passed. The camera module and G1 camera/navigation +blueprint use generated stamped transforms/calibration/rendering helpers; 19 +offline/mocked camera tests passed. Explicit identity rotations and matrix +assertions preserve existing behavior: generated Quaternion already defaults +to w=1. The earlier evidence's all-zero-default claim has been corrected. + +Spot image/calibration publishers, recorder and replay ports use generated +messages, retaining robot-to-local clock conversion and camera rotation rules. +Lossless raw/depth storage uses lz4+cdr, with no legacy payload decoder. Seventeen +offline tests passed, including five raw pixel layouts, JPEG decoding, five +quarter-turn cases with exact headers/nonmutation, calibration and the existing +odometry/TF cases. SDK image constants/responses were mocked; these tests do not +establish native SDK interoperability or hardware behavior. No Spot SDK startup +or device commands ran. Scoped strict mypy passed for the changed production +files. Full repository/optional-device integration remains a CI/manual gate. + +Paused WebXR/control/imitation/memory-index files remain excluded, and neither +denied script was retried. Legacy convenience consumers/dependency retirement, +model-dependent memory documentation and final human viewer evidence remain +open; these local results do not mark the OpenSpec change complete. + +CI run 36729267784 at 2c0ce030f passed Python 3.10, 3.11 and ARM +3.12, lint, Web, Rust and native builds; codegen run 36729275058 also +passed. The x86 Python 3.12 job exhausted its 20-minute whole-job budget +after approximately nine minutes of cold setup and reached 99% of tests +without a final result. The Python job budget is raised to 30 minutes; +no tests, fail-fast behavior or assertions are disabled. md-babel still fails +on deliberately unsupported legacy JPEG storage and is not claimed passing. diff --git a/examples/message-codegen/external-native/CMakeLists.txt b/examples/message-codegen/external-native/CMakeLists.txt new file mode 100644 index 00000000000..e91fd998295 --- /dev/null +++ b/examples/message-codegen/external-native/CMakeLists.txt @@ -0,0 +1,9 @@ +cmake_minimum_required(VERSION 3.20) +project(external_native_relay LANGUAGES CXX) +find_package(external_telemetry CONFIG REQUIRED) +if(NOT DEFINED DIMOS_NATIVE_CPP_DIR) + set(DIMOS_NATIVE_CPP_DIR "${CMAKE_CURRENT_SOURCE_DIR}/../../../native/cpp") +endif() +add_subdirectory(${DIMOS_NATIVE_CPP_DIR} ${CMAKE_BINARY_DIR}/dimos_native) +add_executable(external_native_relay relay.cpp) +target_link_libraries(external_native_relay PRIVATE dimos_native external_telemetry::messages) diff --git a/examples/message-codegen/external-native/relay.cpp b/examples/message-codegen/external-native/relay.cpp new file mode 100644 index 00000000000..30e5ff884fe --- /dev/null +++ b/examples/message-codegen/external-native/relay.cpp @@ -0,0 +1,25 @@ +// Copyright 2026 Dimensional Inc. +// SPDX-License-Identifier: Apache-2.0 + +#include "dimos/native.hpp" +#include + +using demo_msgs::msg::Telemetry; +using namespace dimos::native; + +class Relay : public Module { +public: + void build(Builder& builder, Config&) override { + telemetry_out_ = builder.output("telemetry_out"); + builder.input("telemetry_in", &Relay::receive, this); + } +private: + void receive(const Telemetry& message) { + auto changed = message; + changed.application_note += "/cpp-native"; + telemetry_out_.publish(changed); + } + Output telemetry_out_; +}; + +int main() { dimos::native::run_with_transport(); } diff --git a/examples/message-codegen/external-native/relay.rs b/examples/message-codegen/external-native/relay.rs new file mode 100644 index 00000000000..1bb923d53fc --- /dev/null +++ b/examples/message-codegen/external-native/relay.rs @@ -0,0 +1,65 @@ +// Copyright 2026 Dimensional Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// Copyright 2026 Dimensional Inc. +// SPDX-License-Identifier: Apache-2.0 + +use dimos_module::{Input, Module, Output, run_with_transport}; +use external_telemetry_messages::{codec::Message, demo_msgs::msg::Telemetry}; + +#[derive(Module)] +struct Relay { + #[input(decode = Telemetry::decode)] + telemetry_in: Input, + #[output(encode = Telemetry::encode)] + telemetry_out: Output, +} + +impl Relay { + async fn handle_telemetry_in(&mut self, mut message: Telemetry) { + message.application_note.push_str("/rust-native"); + if let Err(error) = self.telemetry_out.publish(&message).await { + tracing::error!(%error, "external message publish failed"); + } + } +} + +#[tokio::main] +async fn main() { + run_with_transport::().await; +} + +#[cfg(test)] +mod tests { + use super::*; + use std::io; + + #[test] + fn generated_external_codecs_match_sdk_ports_and_report_invalid_data() { + let encode: fn(&Telemetry) -> io::Result> = Telemetry::encode; + let decode: fn(&[u8]) -> io::Result = Telemetry::decode; + let message = Telemetry { + label: "x".repeat(33), + ..Default::default() + }; + assert_eq!( + encode(&message).unwrap_err().kind(), + io::ErrorKind::InvalidInput + ); + assert_eq!( + decode(&[0, 1, 0, 0]).unwrap_err().kind(), + io::ErrorKind::InvalidData + ); + } +} diff --git a/examples/message-codegen/test_base_transport.py b/examples/message-codegen/test_base_transport.py new file mode 100644 index 00000000000..3421de2e27a --- /dev/null +++ b/examples/message-codegen/test_base_transport.py @@ -0,0 +1,32 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from pathlib import Path +import subprocess +import sys + +import pytest + + +@pytest.mark.native_e2e +def test_generated_base_commands_odometry_and_stop_on_both_transports(): + result = subprocess.run( + [sys.executable, str(Path(__file__).with_name("demo_base_transport.py"))], + capture_output=True, + text=True, + timeout=30, + ) + assert result.returncode == 0, result.stdout + result.stderr + assert "PASS: lcm generated base commands, odometry, and stop" in result.stdout + assert "PASS: zenoh generated base commands, odometry, and stop" in result.stdout diff --git a/examples/message-codegen/test_blueprint.py b/examples/message-codegen/test_blueprint.py new file mode 100644 index 00000000000..2b1201df9ba --- /dev/null +++ b/examples/message-codegen/test_blueprint.py @@ -0,0 +1,75 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Run the same three-language blueprint users launch from the demo command.""" + +import os +from pathlib import Path +import signal +import socket +import subprocess +import sys + +import pytest + +_ROOT = Path(__file__).resolve().parents[2] + + +@pytest.mark.native_e2e +@pytest.mark.parametrize("backend", ["lcm", "zenoh"]) +def test_three_language_blueprint_preserves_changing_messages(backend, tmp_path): + for executable in ("build/native-cpp-examples/cdr_relay", "target/debug/cdr_relay"): + if not (_ROOT / executable).is_file(): + pytest.skip("Build both native CDR relay examples first") + with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as reservation: + reservation.bind(("127.0.0.1", 0)) + port = reservation.getsockname()[1] + env = { + **os.environ, + "LCM_DEFAULT_URL": f"udpm://239.255.76.67:{port}?ttl=0&recv_buf_size=4194304", + } + with subprocess.Popen( + [ + sys.executable, + str(Path(__file__).with_name("demo_blueprint.py")), + "--transport", + backend, + "--samples", + "3", + ], + cwd=_ROOT, + env=env, + stdout=subprocess.PIPE, + stderr=subprocess.STDOUT, + start_new_session=True, + ) as process: + try: + output, _ = process.communicate(timeout=75) + except subprocess.TimeoutExpired: + process.send_signal(signal.SIGINT) + try: + output, _ = process.communicate(timeout=15) + except subprocess.TimeoutExpired: + os.killpg(process.pid, signal.SIGKILL) + output, _ = process.communicate(timeout=5) + pytest.fail(output.decode(errors="replace")) + transcript = output.decode(errors="replace") + (tmp_path / f"blueprint-{backend}.log").write_text(transcript) + assert process.returncode == 0, transcript + for sample in range(3): + assert ( + f"sample {sample}: Python weight={sample} → C++ {sample + 1} → Rust {sample + 2}; " + f"Python verified 921,600 image bytes and stamp {1_700_000_000_123_456_789 + sample}" + ) in transcript + assert f"{backend}: three-language coordinator blueprint verified" in transcript diff --git a/examples/message-codegen/test_live_bridge.py b/examples/message-codegen/test_live_bridge.py new file mode 100644 index 00000000000..a627f686872 --- /dev/null +++ b/examples/message-codegen/test_live_bridge.py @@ -0,0 +1,42 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Bound the headless CDR transport-to-viewer demonstration in CI.""" + +from pathlib import Path +import subprocess +import sys + +import pytest + + +@pytest.mark.parametrize("backend", ["lcm", "zenoh"]) +def test_live_bridge_records_generated_image(backend: str) -> None: + demo = Path(__file__).with_name("demo_live_bridge.py") + result = subprocess.run( + [sys.executable, str(demo), "--transport", backend], + check=True, + capture_output=True, + text=True, + timeout=30, + ) + assert f"{backend.upper()} CDR image received and rendered" in result.stdout + recording = Path(f"build/message-codegen/demo/evidence/live-bridge-{backend}.rrd") + subprocess.run( + [str(Path(sys.executable).with_name("rerun")), "rrd", "verify", str(recording)], + check=True, + capture_output=True, + text=True, + timeout=30, + ) diff --git a/examples/message-codegen/test_native_recording.py b/examples/message-codegen/test_native_recording.py new file mode 100644 index 00000000000..616bf2b3a74 --- /dev/null +++ b/examples/message-codegen/test_native_recording.py @@ -0,0 +1,44 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import importlib.util +from pathlib import Path +import subprocess +import sys + +import pytest + + +@pytest.mark.native_e2e +def test_external_schema_records_on_both_transports_with_unchanged_binary(tmp_path): + executable = Path("target/debug/dimos-memory-recorder").resolve() + if not executable.is_file() or importlib.util.find_spec("external_telemetry") is None: + pytest.skip("Build the recorder and install the external message demo package first") + result = subprocess.run( + [ + sys.executable, + str(Path(__file__).with_name("demo_native_recording.py")), + "--executable", + str(executable), + "--output", + str(tmp_path), + ], + capture_output=True, + text=True, + timeout=60, + ) + assert result.returncode == 0, result.stdout + result.stderr + assert "lcm: locally-added-field-2" in result.stdout + assert "zenoh: locally-added-field-2" in result.stdout + assert "PASS:" in result.stdout diff --git a/examples/message-codegen/test_navigation.py b/examples/message-codegen/test_navigation.py new file mode 100644 index 00000000000..1684f9a2374 --- /dev/null +++ b/examples/message-codegen/test_navigation.py @@ -0,0 +1,32 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from pathlib import Path +import subprocess +import sys + +import pytest + + +@pytest.mark.native_e2e +def test_generated_navigation_arrival_on_both_transports(): + result = subprocess.run( + [sys.executable, str(Path(__file__).with_name("demo_navigation.py"))], + capture_output=True, + text=True, + timeout=30, + ) + assert result.returncode == 0, result.stdout + result.stderr + assert "PASS: lcm generated odometry/map/clicked goal" in result.stdout + assert "PASS: zenoh generated odometry/map/clicked goal" in result.stdout diff --git a/examples/message-codegen/test_trajectory.py b/examples/message-codegen/test_trajectory.py new file mode 100644 index 00000000000..ad6b6a39366 --- /dev/null +++ b/examples/message-codegen/test_trajectory.py @@ -0,0 +1,32 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from pathlib import Path +import subprocess +import sys + +import pytest + + +@pytest.mark.native_e2e +def test_generated_trajectory_and_status_on_both_transports(): + result = subprocess.run( + [sys.executable, str(Path(__file__).with_name("demo_trajectory.py"))], + capture_output=True, + text=True, + timeout=30, + ) + assert result.returncode == 0, result.stdout + result.stderr + assert "PASS: lcm generated trajectory and status reach the declared goal" in result.stdout + assert "PASS: zenoh generated trajectory and status reach the declared goal" in result.stdout diff --git a/examples/message-codegen/test_whole_body_transport.py b/examples/message-codegen/test_whole_body_transport.py new file mode 100644 index 00000000000..52a0e32c457 --- /dev/null +++ b/examples/message-codegen/test_whole_body_transport.py @@ -0,0 +1,32 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from pathlib import Path +import subprocess +import sys + +import pytest + + +@pytest.mark.native_e2e +def test_generated_whole_body_feedback_and_commands_on_both_transports(): + result = subprocess.run( + [sys.executable, str(Path(__file__).with_name("demo_whole_body_transport.py"))], + capture_output=True, + text=True, + timeout=30, + ) + assert result.returncode == 0, result.stdout + result.stderr + assert "PASS: lcm whole-body generated CDR commands and feedback" in result.stdout + assert "PASS: zenoh whole-body generated CDR commands and feedback" in result.stdout diff --git a/examples/message-codegen/user-story/consumer.cpp b/examples/message-codegen/user-story/consumer.cpp new file mode 100644 index 00000000000..2043e28d5d6 --- /dev/null +++ b/examples/message-codegen/user-story/consumer.cpp @@ -0,0 +1,28 @@ +// Copyright 2026 Dimensional Inc. +// SPDX-License-Identifier: Apache-2.0 +#include "messages.hpp" +#include +#include +#include +#include + +int main(int argc, char** argv) { + if (argc != 3) return 2; + try { + std::ifstream input(argv[1], std::ios::binary); + if (!input) throw std::runtime_error("Cannot open input"); + std::vector bytes((std::istreambuf_iterator(input)), {}); + auto message = dimos::cdr::decode(bytes); + ++message.sequence; + message.value += 1; + message.label += "/cpp"; + std::cout << "C++ consumer: value=" << message.value << " label=" << message.label << '\n'; + auto encoded = dimos::cdr::encode(message); + std::ofstream output(argv[2], std::ios::binary); + output.write(reinterpret_cast(encoded.data()), encoded.size()); + if (!output) throw std::runtime_error("Cannot write output"); + } catch (const std::exception& error) { + std::cerr << error.what() << '\n'; + return 1; + } +} diff --git a/examples/message-codegen/user-story/consumer.rs b/examples/message-codegen/user-story/consumer.rs new file mode 100644 index 00000000000..cdd9550a98b --- /dev/null +++ b/examples/message-codegen/user-story/consumer.rs @@ -0,0 +1,29 @@ +// Copyright 2026 Dimensional Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +use story_messages_messages::{codec::Message, story_msgs::msg::DeviceReading}; + +fn main() -> Result<(), Box> { + let args: Vec = std::env::args().collect(); + if args.len() != 3 { + return Err("usage: story-consumer INPUT OUTPUT".into()); + } + let mut message = DeviceReading::decode(&std::fs::read(&args[1])?)?; + message.sequence += 1; + message.value += 1.0; + message.label.push_str("/rust"); + println!("Rust consumer: value={} label={}", message.value, message.label); + std::fs::write(&args[2], message.encode()?)?; + Ok(()) +} diff --git a/examples/message-codegen/user-story/demo_module.py b/examples/message-codegen/user-story/demo_module.py new file mode 100644 index 00000000000..cb2df8adce6 --- /dev/null +++ b/examples/message-codegen/user-story/demo_module.py @@ -0,0 +1,91 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Use an application-local generated message in a Python module and file relay.""" + +import argparse +from pathlib import Path +import subprocess + +from story_messages.builtin_interfaces.msg import Time +from story_messages.std_msgs.msg import Header +from story_messages.story_msgs.msg import DeviceReading + +from dimos.core.module import Module +from dimos.core.stream import In, Out + + +class ReadingProcessor(Module): + reading: In[DeviceReading] + processed: Out[DeviceReading] + + def handle_reading(self, message: DeviceReading) -> None: + result = DeviceReading.decode(message.encode()) + result.value += 1 + result.label += "/python" + self.processed.publish(result) + + +def main() -> None: + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument("--build", type=Path, required=True) + args = parser.parse_args() + processor = ReadingProcessor() + outputs: list[DeviceReading] = [] + unsubscribe = processor.processed.subscribe(outputs.append) + try: + reading = DeviceReading( + header=Header(stamp=Time(sec=1700000000, nanosec=123456789), frame_id="sensor"), + sequence=40, + value=20.5, + ) + processor.handle_reading(DeviceReading.decode(reading.encode())) + assert len(outputs) == 1 + output = outputs[0] + print(f"Python module: {output.msg_name}, value={output.value}, label={output.label}") + evidence = args.build / "evidence" + evidence.mkdir(parents=True, exist_ok=True) + (evidence / "python.cdr").write_bytes(output.encode()) + subprocess.run( + [ + str(args.build / "cpp-consumer"), + str(evidence / "python.cdr"), + str(evidence / "cpp.cdr"), + ], + check=True, + ) + subprocess.run( + [ + str(args.build / "rust/target/debug/story-consumer"), + str(evidence / "cpp.cdr"), + str(evidence / "rust.cdr"), + ], + check=True, + ) + final = DeviceReading.decode((evidence / "rust.cdr").read_bytes()) + assert final.sequence == 42 + assert final.value == 23.5 + assert final.label == "new-local-type/python/cpp/rust" + assert final.header.stamp.nanosec == 123456789 + print( + f"Python receives: sequence={final.sequence}, value={final.value}, label={final.label}" + ) + print("PASS: application-local message crosses Python module, C++, Rust and Python") + finally: + unsubscribe() + processor.stop() + + +if __name__ == "__main__": + main() diff --git a/examples/message-codegen/user-story/story_msgs/msg/DeviceReading.msg b/examples/message-codegen/user-story/story_msgs/msg/DeviceReading.msg new file mode 100644 index 00000000000..cbb5e8ebcd5 --- /dev/null +++ b/examples/message-codegen/user-story/story_msgs/msg/DeviceReading.msg @@ -0,0 +1,4 @@ +std_msgs/Header header +uint32 sequence +float64 value +string label "new-local-type" diff --git a/examples/message-codegen/viewer-checker/check-native-recording.mjs b/examples/message-codegen/viewer-checker/check-native-recording.mjs new file mode 100644 index 00000000000..efb5cb7f03e --- /dev/null +++ b/examples/message-codegen/viewer-checker/check-native-recording.mjs @@ -0,0 +1,40 @@ +import assert from 'node:assert/strict'; +import { readFile } from 'node:fs/promises'; +import { McapIndexedReader } from '@mcap/core'; +import { parse, fixupTypes } from '@foxglove/rosmsg'; +import { MessageReader } from '@foxglove/rosmsg2-serialization'; +import zstd from '@foxglove/wasm-zstd'; + +await zstd.isLoaded; +const bytes = await readFile(process.argv[2]); +const reader = await McapIndexedReader.Initialize({ + readable: { + size: async () => BigInt(bytes.length), + read: async (offset, size) => bytes.subarray(Number(offset), Number(offset + size)), + }, + decompressHandlers: { zstd: (data, size) => zstd.decompress(data, Number(size)) }, +}); +assert.equal(reader.header.profile, 'ros2'); +const decoders = new Map(); +for (const schema of reader.schemasById.values()) { + assert.equal(schema.encoding, 'ros2msg'); + const definitions = parse(new TextDecoder().decode(schema.data), { ros2: true, skipTypeFixup: true }); + definitions[0].name = schema.name.replace('/msg/', '/'); + fixupTypes(definitions); + decoders.set(schema.id, new MessageReader(definitions)); +} +let count = 0; +for await (const message of reader.readMessages({ validateCrcs: true })) { + const channel = reader.channelsById.get(message.channelId); + assert.equal(channel.messageEncoding, 'cdr'); + assert.equal(channel.topic, 'telemetry'); + const value = decoders.get(channel.schemaId).readMessage(message.data); + assert.equal(value.application_note, `locally-added-field-${count}`); + assert.equal(value.header.stamp.sec, 1700000000); + assert.equal(value.header.stamp.nanosec, 123456789 + count); + assert.equal(message.logTime, message.publishTime); + console.log(`Foxglove decoded ${value.application_note} from embedded schema.`); + count += 1; +} +assert.equal(count, 3); +console.log('PASS: native recorder custom messages need no viewer-side message package'); diff --git a/examples/message-project/.gitignore b/examples/message-project/.gitignore new file mode 100644 index 00000000000..36b54060ba9 --- /dev/null +++ b/examples/message-project/.gitignore @@ -0,0 +1,5 @@ +/build/ +/dist/ +/sdk/ +/.venv/ +/rust/target/ diff --git a/examples/message-project/cpp/CMakeLists.txt b/examples/message-project/cpp/CMakeLists.txt new file mode 100644 index 00000000000..62b118da718 --- /dev/null +++ b/examples/message-project/cpp/CMakeLists.txt @@ -0,0 +1,6 @@ +cmake_minimum_required(VERSION 3.20) +project(reading_processor LANGUAGES CXX) +find_package(dimos_native CONFIG REQUIRED) +find_package(story_messages CONFIG REQUIRED) +add_executable(processor processor.cpp) +target_link_libraries(processor PRIVATE dimos_native::dimos_native story_messages::messages) diff --git a/examples/message-project/cpp/CMakePresets.json b/examples/message-project/cpp/CMakePresets.json new file mode 100644 index 00000000000..a256fdff4e2 --- /dev/null +++ b/examples/message-project/cpp/CMakePresets.json @@ -0,0 +1,18 @@ +{ + "version": 3, + "configurePresets": [ + { + "name": "dimos", + "generator": "Unix Makefiles", + "binaryDir": "${sourceDir}/../build/cpp", + "toolchainFile": "${sourceDir}/../build/dimos/toolchain.cmake" + } + ], + "buildPresets": [ + { + "name": "dimos", + "configurePreset": "dimos", + "jobs": 2 + } + ] +} diff --git a/examples/message-project/cpp/processor.cpp b/examples/message-project/cpp/processor.cpp new file mode 100644 index 00000000000..a6d3c876446 --- /dev/null +++ b/examples/message-project/cpp/processor.cpp @@ -0,0 +1,23 @@ +// Copyright 2026 Dimensional Inc. +// SPDX-License-Identifier: Apache-2.0 +#include +#include + +using namespace dimos::native; +using Reading = story_msgs::msg::DeviceReading; + +class Processor : public Module { + Output processed_; +public: + void build(Builder& builder, Config&) override { + processed_ = builder.output("processed"); + builder.input("reading", &Processor::process, this); + } + void process(const Reading& input) { + auto output = input; + output.value += 1; + processed_.publish(output); + } +}; + +int main() { run_with_transport(); } diff --git a/examples/message-project/demo_blueprint.py b/examples/message-project/demo_blueprint.py new file mode 100644 index 00000000000..a42b0d3ee9c --- /dev/null +++ b/examples/message-project/demo_blueprint.py @@ -0,0 +1,130 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Run the documented custom message through actual Python/C++/Rust workers.""" + +import argparse +from contextlib import ExitStack +from functools import partial +from pathlib import Path +import socket +import threading +from unittest.mock import patch +import uuid + +from demo_modules import ReadingProcessor +from dimos_generated.builtin_interfaces.msg import Time +from dimos_generated.std_msgs.msg import Header +from story_messages.story_msgs.msg import DeviceReading + +from dimos.core.coordination.blueprints import Blueprint, autoconnect +from dimos.core.coordination.module_coordinator import ModuleCoordinator +from dimos.core.core import rpc +from dimos.core.module import Module +from dimos.core.native_module import NativeModule +from dimos.core.stream import In, Out +from dimos.protocol.service.system_configurator.base import configure_system +from dimos.protocol.service.zenohservice import ZenohConfig, ZenohSessionPool + + +class Exchange(Module): + raw: Out[DeviceReading] + checked: In[DeviceReading] + + @rpc + def start(self) -> None: + self.received: DeviceReading | None = None + self.done = threading.Event() + super().start() + + async def handle_checked(self, message: DeviceReading) -> None: + self.received = message + self.done.set() + + @rpc + def exchange(self) -> str: + message = DeviceReading( + header=Header(stamp=Time(sec=17, nanosec=123456789), frame_id="sensor"), + sequence=42, + value=20.5, + ) + for _ in range(60): + self.raw.publish(message) + if self.done.wait(0.25): + break + if self.received is None: + raise TimeoutError("No reply from the three-language module chain") + expected = DeviceReading(header=message.header, sequence=42, value=23.5, label="sensor") + if self.received.encode() != expected.encode(): + raise ValueError("Received fields differ from the expected three module edits") + return "PASS: 20.5 -> Python 21.5 -> C++ 22.5 -> Rust 23.5; Header and sequence preserved" + + +class CppProcessor(NativeModule): + reading: In[DeviceReading] + processed: Out[DeviceReading] + + +class RustProcessor(NativeModule): + processed: In[DeviceReading] + checked: Out[DeviceReading] + + +def blueprint(cpp: Path, rust: Path) -> Blueprint: + return autoconnect( + Exchange.blueprint(), + ReadingProcessor.blueprint(), + CppProcessor.blueprint(executable=str(cpp.resolve()), stdin_config=True), + RustProcessor.blueprint(executable=str(rust.resolve()), stdin_config=True), + ).namespace("reading" + uuid.uuid4().hex[:8]) + + +def main() -> None: + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument("--transport", choices=["zenoh", "lcm"], default="zenoh") + args = parser.parse_args() + root = Path(__file__).resolve().parent + cpp, rust = root / "build/cpp/processor", root / "rust/target/debug/reading-processor" + if not cpp.is_file() or not rust.is_file(): + parser.error("Build both native processors before running this example") + with ExitStack() as stack: + # This finite offline example can report tuning needs but must never apply them. + stack.enter_context( + patch( + "dimos.protocol.service.system_configurator.base.configure_system", + partial(configure_system, check_only=True), + ) + ) + configured = blueprint(cpp, rust).global_config(viewer="none", transport=args.transport) + if args.transport == "zenoh": + with socket.socket() as reservation: + reservation.bind(("127.0.0.1", 0)) + endpoint = f"tcp/127.0.0.1:{reservation.getsockname()[1]}" + pool = ZenohSessionPool() + stack.callback(pool.close_all) + pool.acquire( + ZenohConfig( + mode="router", listen=[endpoint], connect=[], multicast=False, gossip=False + ) + ) + configured = configured.global_config( + zenoh_mode="client", zenoh_connect=endpoint, zenoh_multicast=False + ) + coordinator = ModuleCoordinator.build(configured) + stack.callback(coordinator.stop) + print(coordinator.get_instance(Exchange).exchange(), flush=True) + + +if __name__ == "__main__": + main() diff --git a/examples/message-project/demo_modules.py b/examples/message-project/demo_modules.py new file mode 100644 index 00000000000..c2cc571223c --- /dev/null +++ b/examples/message-project/demo_modules.py @@ -0,0 +1,35 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""The exact message and module snippets used in the message-authoring walkthrough.""" + +from story_messages.story_msgs.msg import DeviceReading + +from dimos.core.module import Module +from dimos.core.stream import In, Out + + +class ReadingProcessor(Module): + raw: In[DeviceReading] + reading: Out[DeviceReading] + + async def handle_raw(self, message: DeviceReading) -> None: + self.reading.publish( + DeviceReading( + header=message.header, + sequence=message.sequence, + value=message.value + 1, + label=message.label, + ) + ) diff --git a/examples/message-project/interfaces/story_msgs/msg/DeviceReading.msg b/examples/message-project/interfaces/story_msgs/msg/DeviceReading.msg new file mode 100644 index 00000000000..1bfea6b684f --- /dev/null +++ b/examples/message-project/interfaces/story_msgs/msg/DeviceReading.msg @@ -0,0 +1,4 @@ +std_msgs/Header header +uint32 sequence +float64 value +string label "sensor" diff --git a/examples/message-project/pyproject.toml b/examples/message-project/pyproject.toml new file mode 100644 index 00000000000..6b8d643b882 --- /dev/null +++ b/examples/message-project/pyproject.toml @@ -0,0 +1,10 @@ +[build-system] +requires = ["dimos-message-build==0.1.0"] +build-backend = "dimos_message_build.backend" + +[project] +name = "story-messages" +version = "0.1.0" + +[tool.dimos.messages] +languages = ["python", "cpp", "rust"] diff --git a/examples/message-project/rust/Cargo.lock b/examples/message-project/rust/Cargo.lock new file mode 100644 index 00000000000..9abe5364e4e --- /dev/null +++ b/examples/message-project/rust/Cargo.lock @@ -0,0 +1,4349 @@ +# This file is automatically @generated by Cargo. +# It is not intended for manual editing. +version = 4 + +[[package]] +name = "adler2" +version = "2.0.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "320119579fcad9c21884f5c4861d16174d0e06250625266f50fe6898340abefa" + +[[package]] +name = "advisory-lock" +version = "0.3.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a6caee7d48f930f9ad3fc9546f8cbf843365da0c5b0ca4eee1d1ac3dd12d8f93" +dependencies = [ + "libc", + "winapi", +] + +[[package]] +name = "aes" +version = "0.8.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b169f7a6d4742236a0a00c541b845991d0ac43e546831af1249753ab4c3aa3a0" +dependencies = [ + "cfg-if", + "cipher", + "cpufeatures 0.2.17", +] + +[[package]] +name = "ahash" +version = "0.8.12" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "5a15f179cd60c4584b8a8c596927aadc462e27f2ca70c04e0071964a73ba7a75" +dependencies = [ + "cfg-if", + "getrandom 0.3.4", + "once_cell", + "version_check", + "zerocopy", +] + +[[package]] +name = "aho-corasick" +version = "1.1.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "c982642fa9e8606056828ee9a8505737230110bb1099153c79efe865c59d12ba" +dependencies = [ + "memchr", +] + +[[package]] +name = "allocator-api2" +version = "0.2.21" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "683d7910e743518b0e34f1186f92494becacb047c7b6bf616c96772180fef923" + +[[package]] +name = "android_system_properties" +version = "0.1.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ae221649c9976a6f6c56ae1facf410f3ddb33cc661c4b7b61020a912d4237fbc" +dependencies = [ + "libc", +] + +[[package]] +name = "anyhow" +version = "1.0.104" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "330a5ed07fa54e4702c9d6c4174f74427fc0ef6e214bbd677ae50a5099946470" + +[[package]] +name = "approx" +version = "0.5.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "cab112f0a86d568ea0e627cc1d6be74a1e9cd55214684db5561995f6dad897c6" +dependencies = [ + "num-traits", +] + +[[package]] +name = "arc-swap" +version = "1.9.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "c049c0be4daef0b145cb3555416b3b8ef5b7888a38aea1a3a155801fe7b0810b" +dependencies = [ + "rustversion", +] + +[[package]] +name = "array-init" +version = "2.1.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "3d62b7694a562cdf5a74227903507c56ab2cc8bdd1f781ed5cb4cf9c9f810bfc" + +[[package]] +name = "asn1-rs" +version = "0.7.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b7f43a50ac4fdca5df8e885c21b835997f0a1cdee65494a6847694a98652d9d8" +dependencies = [ + "asn1-rs-derive", + "asn1-rs-impl", + "displaydoc", + "nom", + "num-traits", + "rusticata-macros", + "thiserror", + "time", +] + +[[package]] +name = "asn1-rs-derive" +version = "0.6.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "3109e49b1e4909e9db6515a30c633684d68cdeaa252f215214cb4fa1a5bfee2c" +dependencies = [ + "proc-macro2", + "quote", + "syn 2.0.119", + "synstructure", +] + +[[package]] +name = "asn1-rs-impl" +version = "0.2.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "7b18050c2cd6fe86c3a76584ef5e0baf286d038cda203eb6223df2cc413565f7" +dependencies = [ + "proc-macro2", + "quote", + "syn 2.0.119", +] + +[[package]] +name = "async-trait" +version = "0.1.92" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "82f6aeea286b8eb4dd3431a1be1b59d290ace00f5bfd8e2a159bc2a05e2c1667" +dependencies = [ + "proc-macro2", + "quote", + "syn 3.0.6", +] + +[[package]] +name = "autocfg" +version = "1.5.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f2032f911046de80f0a198e0901378627c33f59ea0ac00e363d481118bd70a53" + +[[package]] +name = "base64" +version = "0.22.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "72b3254f16251a8381aa12e40e3c4d2f0199f8c6508fbecb9d91f575e0fbb8c6" + +[[package]] +name = "base64" +version = "0.23.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ac07cdecf99051d9a5238b80f35af32cdeba5b336e55d957b318b50137e18da5" + +[[package]] +name = "bit-vec" +version = "0.9.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b71798fca2c1fe1086445a7258a4bc81e6e49dcd24c8d0dd9a1e57395b603f51" +dependencies = [ + "serde", +] + +[[package]] +name = "bitflags" +version = "1.3.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "bef38d45163c2f1dde094a7dfd33ccf595c92905c8f8f4fdc18d06fb1037718a" + +[[package]] +name = "bitflags" +version = "2.13.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b588b76d00fde79687d7646a9b5bdf3cc0f655e0bbd080335a95d7e96f3587da" +dependencies = [ + "serde_core", +] + +[[package]] +name = "block-buffer" +version = "0.10.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "3078c7629b62d3f0439517fa394996acacc5cbc91c5a20d8c658e77abd503a71" +dependencies = [ + "generic-array", +] + +[[package]] +name = "bs58" +version = "0.5.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "bf88ba1141d185c399bee5288d850d63b8369520c1eafc32a0430b5b6c287bf4" +dependencies = [ + "tinyvec", +] + +[[package]] +name = "buddy_system_allocator" +version = "0.10.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a7913f22349ffcfc6ca0ca9a656ec26cfbba538ed49c31a273dff2c5d1ea83d9" +dependencies = [ + "spin 0.9.9", +] + +[[package]] +name = "bumpalo" +version = "3.20.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "72f5acc6cb2ba439de613abc23857ec3d78374d8ed5ac84e9d11336e87da8649" + +[[package]] +name = "bytemuck" +version = "1.25.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "95832e849adfb21180ccb6826a99da14e5d266ae5c2e668e1602cf234f153797" +dependencies = [ + "bytemuck_derive", +] + +[[package]] +name = "bytemuck_derive" +version = "1.12.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "6a1f896587b6f2c069c73d2f0913e2d590c3990285cd2f0b6aa02b786b4c679c" +dependencies = [ + "proc-macro2", + "quote", + "syn 3.0.6", +] + +[[package]] +name = "byteorder" +version = "1.5.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1fd0f2584146f6f2ef48085050886acf353beff7305ebd1ae69500e27c67f64b" + +[[package]] +name = "bytes" +version = "1.12.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "fc652a48c352aef3ea3aed32080501cf3ef6ed5da78602a020c991775b0aff04" + +[[package]] +name = "cc" +version = "1.4.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0ad534f4357a5264cce5019c989cf66a4f0dc4e0d1b1d15f8aacec0ff7360273" +dependencies = [ + "find-msvc-tools", + "shlex", +] + +[[package]] +name = "cfg-if" +version = "1.0.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9330f8b2ff13f34540b44e946ef35111825727b38d33286ef986142615121801" + +[[package]] +name = "cfg_aliases" +version = "0.1.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "fd16c4719339c4530435d38e511904438d07cce7950afa3718a84ac36c10e89e" + +[[package]] +name = "cfg_aliases" +version = "0.2.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f079e83a288787bcd14a6aea84cee5c87a67c5a3e660c30f557a3d24761b3527" + +[[package]] +name = "chacha20" +version = "0.10.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "65c35e4b699c7e15ccbe7ee35c005e4fc0a278d22238a2857e6ce2dadeda1b06" +dependencies = [ + "cfg-if", + "cpufeatures 0.3.1", + "rand_core 0.10.1", +] + +[[package]] +name = "chrono" +version = "0.4.45" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1aa79e62e7697b8e29b513a68abacf485adcd1fe8284a4316c5ae868e6633327" +dependencies = [ + "iana-time-zone", + "num-traits", + "serde", + "windows-link", +] + +[[package]] +name = "cipher" +version = "0.4.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "773f3b9af64447d2ce9850330c473515014aa235e6a783b02db81ff39e4a3dad" +dependencies = [ + "crypto-common", + "inout", +] + +[[package]] +name = "combine" +version = "4.6.8" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "cfc320937d09e6de266b31b9afb480f197d7a861be86be7cb2ea7e5d1bfffc5e" +dependencies = [ + "bytes", + "memchr", +] + +[[package]] +name = "const_format" +version = "0.2.36" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "4481a617ad9a412be3b97c5d403fef8ed023103368908b9c50af598ff467cc1e" +dependencies = [ + "const_format_proc_macros", + "konst", +] + +[[package]] +name = "const_format_proc_macros" +version = "0.2.34" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1d57c2eccfb16dbac1f4e61e206105db5820c9d26c3c472bc17c774259ef7744" +dependencies = [ + "proc-macro2", + "quote", + "unicode-xid", +] + +[[package]] +name = "core-foundation" +version = "0.10.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b2a6cd9ae233e7f62ba4e9353e81a88df7fc8a5987b8d445b4d90c879bd156f6" +dependencies = [ + "core-foundation-sys", + "libc", +] + +[[package]] +name = "core-foundation-sys" +version = "0.8.7" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "773648b94d0e5d620f64f280777445740e61fe701025087ec8b57f45c791888b" + +[[package]] +name = "cpufeatures" +version = "0.2.17" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "59ed5838eebb26a2bb2e58f6d5b5316989ae9d08bab10e0e6d103e656d1b0280" +dependencies = [ + "libc", +] + +[[package]] +name = "cpufeatures" +version = "0.3.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "5ca28b0ae3115b884660db4118d803791fd6756b6e88f39c0f3f7859060d7566" +dependencies = [ + "libc", +] + +[[package]] +name = "crc32fast" +version = "1.5.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8498c871161e1742aaa9d52551b2d6ebdd4c3d45a3be423e3728f33b955be550" +dependencies = [ + "cfg-if", +] + +[[package]] +name = "crossbeam" +version = "0.8.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1137cd7e7fc0fb5d3c5a8678be38ec56e819125d8d7907411fe24ccb943faca8" +dependencies = [ + "crossbeam-channel", + "crossbeam-deque", + "crossbeam-epoch", + "crossbeam-queue", + "crossbeam-utils", +] + +[[package]] +name = "crossbeam-channel" +version = "0.5.16" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "d85363c37faeca707aef026efa9f3b34d077bce547e48f770770625c6013679e" +dependencies = [ + "crossbeam-utils", +] + +[[package]] +name = "crossbeam-deque" +version = "0.8.7" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "5181e0de7b61eb03a81e347d6dd8797bae9da5146707b51077e2d71a54ec0ceb" +dependencies = [ + "crossbeam-epoch", + "crossbeam-utils", +] + +[[package]] +name = "crossbeam-epoch" +version = "0.9.20" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "2d6914041f254d6e9176c01941b21115dcfb7089e55135a35411081bd106ef3f" +dependencies = [ + "crossbeam-utils", +] + +[[package]] +name = "crossbeam-queue" +version = "0.3.13" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "803d13fb3b09d88be9f4dbc29062c66b19bf7170867ceb746d2a8689bf6c7a26" +dependencies = [ + "crossbeam-utils", +] + +[[package]] +name = "crossbeam-utils" +version = "0.8.22" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "61803da095bee82a81bb1a452ecc25d3b2f1416d1897eb86430c6159ef717c17" + +[[package]] +name = "crypto-common" +version = "0.1.7" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "78c8292055d1c1df0cce5d180393dc8cce0abec0a7102adb6c7b1eef6016d60a" +dependencies = [ + "generic-array", + "typenum", +] + +[[package]] +name = "darling" +version = "0.23.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "25ae13da2f202d56bd7f91c25fba009e7717a1e4a1cc98a76d844b65ae912e9d" +dependencies = [ + "darling_core", + "darling_macro", +] + +[[package]] +name = "darling_core" +version = "0.23.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9865a50f7c335f53564bb694ef660825eb8610e0a53d3e11bf1b0d3df31e03b0" +dependencies = [ + "ident_case", + "proc-macro2", + "quote", + "strsim", + "syn 2.0.119", +] + +[[package]] +name = "darling_macro" +version = "0.23.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ac3984ec7bd6cfa798e62b4a642426a5be0e68f9401cfc2a01e3fa9ea2fcdb8d" +dependencies = [ + "darling_core", + "quote", + "syn 2.0.119", +] + +[[package]] +name = "data-encoding" +version = "2.11.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "4583a4551df46e2792f82ceeac45e850d2e2d5debba0b91f102385cda5b11f06" + +[[package]] +name = "defmt" +version = "1.1.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e2953bfe4f93bbd20cc71198842756f77d161884c99ebbabc41d80231ded88d1" +dependencies = [ + "bitflags 1.3.2", + "defmt-macros", +] + +[[package]] +name = "defmt-macros" +version = "1.1.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "bad9c72e7ca2137e0dc3813245a0d282fd6daad32fd800af018306a9169b5fe8" +dependencies = [ + "defmt-parser", + "proc-macro2", + "quote", + "syn 2.0.119", +] + +[[package]] +name = "defmt-parser" +version = "1.0.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "10d60334b3b2e7c9d91ef8150abfb6fa4c1c39ebbcf4a81c2e346aad939fee3e" +dependencies = [ + "thiserror", +] + +[[package]] +name = "der-parser" +version = "10.0.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "07da5016415d5a3c4dd39b11ed26f915f52fc4e0dc197d87908bc916e51bc1a6" +dependencies = [ + "asn1-rs", + "displaydoc", + "nom", + "num-bigint", + "num-traits", + "rusticata-macros", +] + +[[package]] +name = "deranged" +version = "0.5.8" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "7cd812cc2bc1d69d4764bd80df88b4317eaef9e773c75226407d9bc0876b211c" +dependencies = [ + "serde_core", +] + +[[package]] +name = "digest" +version = "0.10.7" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9ed9a281f7bc9b7576e61468ba615a66a5c8cfdff42420a70aa82701a3b1e292" +dependencies = [ + "block-buffer", + "crypto-common", + "subtle", +] + +[[package]] +name = "dimos-generated-messages" +version = "0.1.0" +dependencies = [ + "re_cdr", + "serde", + "serde-big-array", +] + +[[package]] +name = "dimos-lcm-transport" +version = "0.1.0" +dependencies = [ + "byteorder", + "socket2 0.5.10", + "tokio", +] + +[[package]] +name = "dimos-module" +version = "0.1.0" +dependencies = [ + "dimos-generated-messages", + "dimos-lcm-transport", + "dimos-module-macros", + "nalgebra", + "rayon", + "serde", + "serde_json", + "tokio", + "tracing", + "tracing-subscriber", + "url", + "validator", + "zenoh", +] + +[[package]] +name = "dimos-module-macros" +version = "0.1.0" +dependencies = [ + "proc-macro2", + "quote", + "syn 3.0.6", + "toml 1.1.4+spec-1.1.0", +] + +[[package]] +name = "dirs" +version = "6.0.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "c3e8aa94d75141228480295a7d0e7feb620b1a5ad9f12bc40be62411e38cce4e" +dependencies = [ + "dirs-sys", +] + +[[package]] +name = "dirs-sys" +version = "0.5.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e01a3366d27ee9890022452ee61b2b63a67e6f13f58900b651ff5665f0bb1fab" +dependencies = [ + "libc", + "option-ext", + "redox_users", + "windows-sys 0.61.2", +] + +[[package]] +name = "displaydoc" +version = "0.2.7" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "c6232dd377dcc64799954cbd3a9bb882e9cdc1308ccd87b1c098f1fb2eaf82a8" +dependencies = [ + "proc-macro2", + "quote", + "syn 3.0.6", +] + +[[package]] +name = "dtoa" +version = "1.0.11" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "4c3cf4824e2d5f025c7b531afcb2325364084a16806f6d47fbc1f5fbd9960590" + +[[package]] +name = "dyn-clone" +version = "1.0.20" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "d0881ea181b1df73ff77ffaaf9c7544ecc11e82fba9b5f27b262a3c73a332555" + +[[package]] +name = "either" +version = "1.18.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "252afb9ae5eaa683babdc6a068b3f5726eb19e05070c731f9b2a23a7c3e8ed34" + +[[package]] +name = "equivalent" +version = "1.0.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "877a4ace8713b0bcf2a4e7eec82529c029f1d0619886d18145fea96c3ffe5c0f" + +[[package]] +name = "errno" +version = "0.3.14" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "39cab71617ae0d63f51a36d69f866391735b51691dbda63cf6f96d042b63efeb" +dependencies = [ + "libc", + "windows-sys 0.61.2", +] + +[[package]] +name = "event-listener" +version = "5.4.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "5a23add41df1562121a9393cb065eab5146a1242410f23a644851e90cfd669d2" +dependencies = [ + "parking", + "pin-project-lite", +] + +[[package]] +name = "fastbloom" +version = "0.17.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ef975e30683b2d965054bb0a836f8973857c4ebf6acf274fe46617cd285060d8" +dependencies = [ + "foldhash 0.2.0", + "libm", + "portable-atomic", + "siphasher", +] + +[[package]] +name = "fastrand" +version = "2.5.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "da7c62ceae207dd37ea5b845da6a0696c799f85e97da1ab5b7910be3c1c80223" + +[[package]] +name = "find-msvc-tools" +version = "0.1.11" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "d45db016d36b838f563236e9193d0ee6ce38f3f68b6c94e914b4929c96bbb890" + +[[package]] +name = "fixedbitset" +version = "0.5.7" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1d674e81391d1e1ab681a28d99df07927c6d4aa5b027d7da16ba32d1d21ecd99" + +[[package]] +name = "flate2" +version = "1.1.10" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "6e634e2e0ebac1ee034020da1ca582e17ffe4e0f5e985823721e168928136dcb" +dependencies = [ + "crc32fast", + "miniz_oxide", + "zlib-rs", +] + +[[package]] +name = "flume" +version = "0.11.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "da0e4dd2a88388a1f4ccc7c9ce104604dab68d9f408dc34cd45823d5a9069095" +dependencies = [ + "futures-core", + "futures-sink", + "nanorand", + "spin 0.9.9", +] + +[[package]] +name = "foldhash" +version = "0.1.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "d9c4f5dac5e15c24eb999c26181a6ca40b39fe946cbe4c263c7209467bc83af2" + +[[package]] +name = "foldhash" +version = "0.2.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "77ce24cb58228fbb8aa041425bb1050850ac19177686ea6e0f41a70416f56fdb" + +[[package]] +name = "form_urlencoded" +version = "1.2.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "cb4cb245038516f5f85277875cdaa4f7d2c9a0fa0468de06ed190163b1581fcf" +dependencies = [ + "percent-encoding", +] + +[[package]] +name = "futures" +version = "0.3.34" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9a31d2a3fbaaeb2af2368bbdd904aa8e812d3c04a1ee10d3171f52d556e5d0a3" +dependencies = [ + "futures-channel", + "futures-core", + "futures-executor", + "futures-io", + "futures-sink", + "futures-task", + "futures-util", +] + +[[package]] +name = "futures-channel" +version = "0.3.34" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b1f9e3d69d39e4862ffed03ed071a76f9a13ba1d9109d355b0f0aa6b15e393c4" +dependencies = [ + "futures-core", + "futures-sink", +] + +[[package]] +name = "futures-core" +version = "0.3.34" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "92d699e522242e69e3003b94ecc1f960f3a5e015aa7c5d7486e65ad01dd94f5e" + +[[package]] +name = "futures-executor" +version = "0.3.34" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "031b47cf1a3c6cc8bc2fc76cd437f521619387907d469316e7c0bc278f1f5432" +dependencies = [ + "futures-core", + "futures-task", + "futures-util", +] + +[[package]] +name = "futures-io" +version = "0.3.34" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "53c0fa8157de1303bfffdaa1cc2a673bfffb60102f76b0ef4441659124373fed" + +[[package]] +name = "futures-macro" +version = "0.3.34" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9fb9654ba8355388abeb8dcb4fc62f511300867002afc858860463bdd9fe0c44" +dependencies = [ + "proc-macro2", + "quote", + "syn 3.0.6", +] + +[[package]] +name = "futures-sink" +version = "0.3.34" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1944426bf7d03f1d14f708785e4b33efd750b36d48a157b836b3efc15ede8e1d" + +[[package]] +name = "futures-task" +version = "0.3.34" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "cd417de3d1d015fc3bfd2b1ea46dfc7bab72ef86f1cc7cc9c78e728b34a6d1fd" + +[[package]] +name = "futures-util" +version = "0.3.34" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0d50a92467f8ba5dd6e3ee5d4bd04d73ab2e4e1c44474a0674821dfce14b79bc" +dependencies = [ + "futures-channel", + "futures-core", + "futures-io", + "futures-macro", + "futures-sink", + "futures-task", + "memchr", + "pin-project-lite", + "slab", +] + +[[package]] +name = "generic-array" +version = "0.14.7" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "85649ca51fd72272d7821adaf274ad91c288277713d9c18820d8499a7ff69e9a" +dependencies = [ + "typenum", + "version_check", +] + +[[package]] +name = "getrandom" +version = "0.2.17" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ff2abc00be7fca6ebc474524697ae276ad847ad0a6b3faa4bcb027e9a4614ad0" +dependencies = [ + "cfg-if", + "js-sys", + "libc", + "wasi", + "wasm-bindgen", +] + +[[package]] +name = "getrandom" +version = "0.3.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "899def5c37c4fd7b2664648c28120ecec138e4d395b459e5ca34f9cce2dd77fd" +dependencies = [ + "cfg-if", + "libc", + "r-efi 5.3.0", + "wasip2", +] + +[[package]] +name = "getrandom" +version = "0.4.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "300e883d756b2e4ec94e02791f39b04b522276138852cfc41d9fb7e904106099" +dependencies = [ + "cfg-if", + "js-sys", + "libc", + "r-efi 6.0.0", + "rand_core 0.10.1", + "wasm-bindgen", +] + +[[package]] +name = "git-version" +version = "0.3.9" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1ad568aa3db0fcbc81f2f116137f263d7304f512a1209b35b85150d3ef88ad19" +dependencies = [ + "git-version-macro", +] + +[[package]] +name = "git-version-macro" +version = "0.3.9" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "53010ccb100b96a67bc32c0175f0ed1426b31b655d562898e57325f81c023ac0" +dependencies = [ + "proc-macro2", + "quote", + "syn 2.0.119", +] + +[[package]] +name = "glam" +version = "0.30.10" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "19fc433e8437a212d1b6f1e68c7824af3aed907da60afa994e7f542d18d12aa9" + +[[package]] +name = "glam" +version = "0.31.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "556f6b2ea90b8d15a74e0e7bb41671c9bdf38cd9f78c284d750b9ce58a2b5be7" + +[[package]] +name = "glam" +version = "0.32.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f70749695b063ecbf6b62949ccccde2e733ec3ecbbd71d467dca4e5c6c97cca0" + +[[package]] +name = "glam" +version = "0.33.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "21fef0953c54fd3de2f44b743fbf77e044c81a25faee03636dfccc0d35135e23" + +[[package]] +name = "hashbrown" +version = "0.12.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8a9ee70c43aaf417c914396645a0fa852624801b24ebb7ae78fe8272889ac888" + +[[package]] +name = "hashbrown" +version = "0.14.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e5274423e17b7c9fc20b6e7e208532f9b19825d82dfd615708b70edd83df41f1" +dependencies = [ + "ahash", + "allocator-api2", +] + +[[package]] +name = "hashbrown" +version = "0.15.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9229cfe53dfd69f0609a49f65461bd93001ea1ef889cd5529dd176593f5338a1" +dependencies = [ + "foldhash 0.1.5", +] + +[[package]] +name = "hashbrown" +version = "0.16.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "841d1cc9bed7f9236f321df977030373f4a4163ae1a7dbfe1a51a2c1a51d9100" +dependencies = [ + "allocator-api2", + "equivalent", + "foldhash 0.2.0", +] + +[[package]] +name = "hashbrown" +version = "0.17.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ed5909b6e89a2db4456e54cd5f673791d7eca6732202bbf2a9cc504fe2f9b84a" + +[[package]] +name = "hermit-abi" +version = "0.5.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e17592d60ebacc7d5e169f4663c5f84f9161cc90328abcfe8456f41e4dfcb284" + +[[package]] +name = "hex" +version = "0.4.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "7f24254aa9a54b5c858eaee2f5bccdb46aaf0e486a595ed5fd8f86ba55232a70" + +[[package]] +name = "hmac" +version = "0.12.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "6c49c37c09c17a53d937dfbb742eb3a961d65a994e6bcdcf37e7399d0cc8ab5e" +dependencies = [ + "digest", +] + +[[package]] +name = "home" +version = "0.5.12" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "cc627f471c528ff0c4a49e1d5e60450c8f6461dd6d10ba9dcd3a61d3dff7728d" +dependencies = [ + "windows-sys 0.61.2", +] + +[[package]] +name = "humantime" +version = "2.4.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "15cdd26707701c53297e2fa6afb323d55fbc1d0810c3aec078ae3ef0424c3c15" + +[[package]] +name = "iana-time-zone" +version = "0.1.65" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e31bc9ad994ba00e440a8aa5c9ef0ec67d5cb5e5cb0cc7f8b744a35b389cc470" +dependencies = [ + "android_system_properties", + "core-foundation-sys", + "iana-time-zone-haiku", + "js-sys", + "log", + "wasm-bindgen", + "windows-core", +] + +[[package]] +name = "iana-time-zone-haiku" +version = "0.1.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f31827a206f56af32e590ba56d5d2d085f558508192593743f16b2306495269f" +dependencies = [ + "cc", +] + +[[package]] +name = "icu_collections" +version = "2.3.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "fa68d21081c4a05d5a901a1c62add574c77048b6a1c67be3b50ce0b60d4ca513" +dependencies = [ + "displaydoc", + "potential_utf", + "utf8_iter", + "yoke", + "zerofrom", + "zerovec", +] + +[[package]] +name = "icu_locale_core" +version = "2.3.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "d56e28588da92eee5c3201a6eff33fabdd49b62269c8938d4ff050ce4d900deb" +dependencies = [ + "displaydoc", + "litemap", + "tinystr", + "writeable", + "zerovec", +] + +[[package]] +name = "icu_normalizer" +version = "2.3.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "12f9cf5f235641ed274641dd81c3f28d870e276763d0797aeeab72317b1c646f" +dependencies = [ + "icu_collections", + "icu_normalizer_data", + "icu_properties", + "icu_provider", + "smallvec", + "zerovec", +] + +[[package]] +name = "icu_normalizer_data" +version = "2.3.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1563da1ed3e0b3bf3d74c9b85917ac9c56464d2f57242270c09c9e752f8021a0" + +[[package]] +name = "icu_properties" +version = "2.3.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "7e7ca276ad3145661a65914e6daf131ca5120cd3dcee8f8f3214b8875184a148" +dependencies = [ + "displaydoc", + "icu_collections", + "icu_locale_core", + "icu_properties_data", + "icu_provider", + "zerotrie", + "zerovec", +] + +[[package]] +name = "icu_properties_data" +version = "2.3.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e590f038c1464a96894fd6d10127e90a8be4509f56ff7ecef851b15cee0b7caa" + +[[package]] +name = "icu_provider" +version = "2.3.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "d27bbb9d3abbefac45d55f647c9de1d44aafcd1186eb91879afef17c396c3e73" +dependencies = [ + "displaydoc", + "icu_locale_core", + "writeable", + "yoke", + "zerofrom", + "zerotrie", + "zerovec", +] + +[[package]] +name = "ident_case" +version = "1.0.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b9e0384b61958566e926dc50660321d12159025e767c18e043daf26b70104c39" + +[[package]] +name = "idna" +version = "1.1.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "3b0875f23caa03898994f6ddc501886a45c7d3d62d04d2d90788d47be1b1e4de" +dependencies = [ + "idna_adapter", + "smallvec", + "utf8_iter", +] + +[[package]] +name = "idna_adapter" +version = "1.2.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "cb68373c0d6620ef8105e855e7745e18b0d00d3bdb07fb532e434244cdb9a714" +dependencies = [ + "icu_normalizer", + "icu_properties", +] + +[[package]] +name = "indexmap" +version = "1.9.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "bd070e393353796e801d209ad339e89596eb4c8d430d18ede6a1cced8fafbd99" +dependencies = [ + "autocfg", + "hashbrown 0.12.3", + "serde", +] + +[[package]] +name = "indexmap" +version = "2.14.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "07aa2048142242915a31d35844fb311e0e53fcca590c3a0a40dcf1b841fa09eb" +dependencies = [ + "equivalent", + "hashbrown 0.17.1", + "serde", + "serde_core", +] + +[[package]] +name = "inout" +version = "0.1.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "879f10e63c20629ecabbb64a8010319738c66a5cd0c29b02d63d272b03751d01" +dependencies = [ + "generic-array", +] + +[[package]] +name = "ipnetwork" +version = "0.20.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "bf466541e9d546596ee94f9f69590f89473455f88372423e0008fc1a7daf100e" +dependencies = [ + "serde", +] + +[[package]] +name = "itertools" +version = "0.14.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "2b192c782037fadd9cfa75548310488aabdbf3d2da73885b31bd0abd03351285" +dependencies = [ + "either", +] + +[[package]] +name = "itoa" +version = "1.0.18" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8f42a60cbdf9a97f5d2305f08a87dc4e09308d1276d28c869c684d7777685682" + +[[package]] +name = "jiff" +version = "0.2.35" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "668b7183bd07af9a4885f5c35b0cc5c83c4607a913c16b7e17291832910d2dcc" +dependencies = [ + "defmt", + "jiff-core", + "jiff-static", + "jiff-tzdb-platform", + "log", + "portable-atomic", + "portable-atomic-util", + "serde_core", + "windows-link", +] + +[[package]] +name = "jiff-core" +version = "0.1.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "7feca88439efe53da3754500c1851dedf3cb36c524dd5cf8225cc0794de95d09" +dependencies = [ + "defmt", +] + +[[package]] +name = "jiff-static" +version = "0.2.35" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "3a69dcb3a21cfb32ce1cd056169337ca284af0766dd766e7878819b251a49204" +dependencies = [ + "jiff-core", + "proc-macro2", + "quote", + "syn 2.0.119", +] + +[[package]] +name = "jiff-tzdb" +version = "0.1.8" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "142bd39932ad231f10513df9ab62661fead8719872150b7ad02a2df79f4e141e" + +[[package]] +name = "jiff-tzdb-platform" +version = "0.1.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "875a5a69ac2bab1a891711cf5eccbec1ce0341ea805560dcd90b7a2e925132e8" +dependencies = [ + "jiff-tzdb", +] + +[[package]] +name = "jni" +version = "0.22.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "5efd9a482cf3a427f00d6b35f14332adc7902ce91efb778580e180ff90fa3498" +dependencies = [ + "cfg-if", + "combine", + "jni-macros", + "jni-sys", + "log", + "simd_cesu8", + "thiserror", + "walkdir", + "windows-link", +] + +[[package]] +name = "jni-macros" +version = "0.22.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a00109accc170f0bdb141fed3e393c565b6f5e072365c3bd58f5b062591560a3" +dependencies = [ + "proc-macro2", + "quote", + "rustc_version", + "simd_cesu8", + "syn 2.0.119", +] + +[[package]] +name = "jni-sys" +version = "0.4.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "c6377a88cb3910bee9b0fa88d4f42e1d2da8e79915598f65fb0c7ee14c878af2" +dependencies = [ + "jni-sys-macros", +] + +[[package]] +name = "jni-sys-macros" +version = "0.4.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "38c0b942f458fe50cdac086d2f946512305e5631e720728f2a61aabcd47a6264" +dependencies = [ + "quote", + "syn 2.0.119", +] + +[[package]] +name = "js-sys" +version = "0.3.104" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0e0c1080212aad755ea003d18543e8768dd432c48819efd73a7bf1e39b7a5a3a" +dependencies = [ + "cfg-if", + "futures-util", + "wasm-bindgen", +] + +[[package]] +name = "json5" +version = "0.4.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "96b0db21af676c1ce64250b5f40f3ce2cf27e4e47cb91ed91eb6fe9350b430c1" +dependencies = [ + "pest", + "pest_derive", + "serde", +] + +[[package]] +name = "keccak" +version = "0.1.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "cb26cec98cce3a3d96cbb7bced3c4b16e3d13f27ec56dbd62cbc8f39cfb9d653" +dependencies = [ + "cpufeatures 0.2.17", +] + +[[package]] +name = "keyed-set" +version = "1.1.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "89d255a6b6ecd77bb93ce91de984d7039bff7503f500eb4851a1269732f22baf" +dependencies = [ + "hashbrown 0.14.5", +] + +[[package]] +name = "konst" +version = "0.2.20" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "128133ed7824fcd73d6e7b17957c5eb7bacb885649bd8c69708b2331a10bcefb" +dependencies = [ + "konst_macro_rules", +] + +[[package]] +name = "konst_macro_rules" +version = "0.2.19" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a4933f3f57a8e9d9da04db23fb153356ecaf00cbd14aee46279c33dc80925c37" + +[[package]] +name = "lazy_static" +version = "1.5.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "bbd2bcb4c963f2ddae06a2efc7e9f3591312473c50c6685e1f298068316e66fe" + +[[package]] +name = "libc" +version = "0.2.189" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "3eaf3ede3fee6db1a4c2ee091bf8a8b4dccdc6d17f656fb07896ee72867612f2" + +[[package]] +name = "libloading" +version = "0.8.9" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "d7c4b02199fee7c5d21a5ae7d8cfa79a6ef5bb2fc834d6e9058e89c825efdc55" +dependencies = [ + "cfg-if", + "windows-link", +] + +[[package]] +name = "libm" +version = "0.2.16" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b6d2cec3eae94f9f509c767b45932f1ada8350c4bdb85af2fcab4a3c14807981" + +[[package]] +name = "libredox" +version = "0.1.21" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "d7955dfc218a8afb29dfeffd540e3a6e96baeb94fe7138228dd7cc6937fbbf96" +dependencies = [ + "libc", +] + +[[package]] +name = "litemap" +version = "0.8.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "47d9d19d1d6efa0109d2f65ff4c85cddd50bd572e5a00127ab10987290bcefae" + +[[package]] +name = "lock_api" +version = "0.4.14" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "224399e74b87b5f3557511d98dff8b14089b3dadafcab6bb93eab67d3aace965" +dependencies = [ + "scopeguard", +] + +[[package]] +name = "lockfree" +version = "0.5.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "74ee94b5ad113c7cb98c5a040f783d0952ee4fe100993881d1673c2cb002dd23" +dependencies = [ + "owned-alloc", +] + +[[package]] +name = "log" +version = "0.4.34" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f9f8bd3e56ce4dfc153cf470fffbfa98c7620958b312ca5c3a4b8d5181fd13c6" + +[[package]] +name = "lru-slab" +version = "0.1.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "112b39cec0b298b6c1999fee3e31427f74f676e4cb9879ed1a121b43661a4154" + +[[package]] +name = "lz4_flex" +version = "0.10.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8b8c72594ac26bfd34f2d99dfced2edfaddfe8a476e3ff2ca0eb293d925c4f83" +dependencies = [ + "twox-hash", +] + +[[package]] +name = "matchers" +version = "0.2.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "d1525a2a28c7f4fa0fc98bb91ae755d1e2d1505079e05539e35bc876b5d65ae9" +dependencies = [ + "regex-automata", +] + +[[package]] +name = "matrixmultiply" +version = "0.3.11" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "3f607c237553f086e7043417a51df26b2eb899d3caff94e6a67592ff992fedc7" +dependencies = [ + "autocfg", + "rawpointer", +] + +[[package]] +name = "memchr" +version = "2.8.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "cf8baf1c55e62ffcace7a9f06f4bd9cd3f0c4beb022d3b367256b91b87513d98" + +[[package]] +name = "minimal-lexical" +version = "0.2.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "68354c5c6bd36d73ff3feceb05efa59b6acb7626617f4962be322a825e61f79a" + +[[package]] +name = "miniz_oxide" +version = "0.9.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b63fbc4a50860e98e7b2aa7804ded1db5cbc3aff9193adaff57a6931bf7c4b4c" +dependencies = [ + "adler2", + "simd-adler32", +] + +[[package]] +name = "mio" +version = "1.2.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "30d65c71f1ce40ab09135ce117d742b9f8a19ff91a41a8b57ed50bc2de59c427" +dependencies = [ + "libc", + "wasi", + "windows-sys 0.61.2", +] + +[[package]] +name = "nalgebra" +version = "0.35.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "adc43a60c217b0c6ff46e47f26911015ad8d2e5a8be1af668c67e370d99a4346" +dependencies = [ + "approx", + "glam 0.30.10", + "glam 0.31.1", + "glam 0.32.1", + "glam 0.33.6", + "matrixmultiply", + "nalgebra-macros", + "num-complex", + "num-rational", + "num-traits", + "simba", + "typenum", +] + +[[package]] +name = "nalgebra-macros" +version = "0.3.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "973e7178a678cfd059ccec50887658d482ce16b0aa9da3888ddeab5cd5eb4889" +dependencies = [ + "proc-macro2", + "quote", + "syn 2.0.119", +] + +[[package]] +name = "nanorand" +version = "0.7.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "6a51313c5820b0b02bd422f4b44776fbf47961755c74ce64afc73bfad10226c3" +dependencies = [ + "getrandom 0.2.17", +] + +[[package]] +name = "nix" +version = "0.29.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "71e2746dc3a24dd78b3cfcb7be93368c6de9963d30f43a6a73998a9cf4b17b46" +dependencies = [ + "bitflags 2.13.1", + "cfg-if", + "cfg_aliases 0.2.2", + "libc", +] + +[[package]] +name = "no-std-net" +version = "0.6.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "43794a0ace135be66a25d3ae77d41b91615fb68ae937f904090203e81f755b65" + +[[package]] +name = "nom" +version = "7.1.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "d273983c5a657a70a3e8f2a01329822f3b8c8172b73826411a55751e404a0a4a" +dependencies = [ + "memchr", + "minimal-lexical", +] + +[[package]] +name = "nonempty-collections" +version = "0.3.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e216d0e8cf9d54fa66e5780f6e1d5dc96d1c1b3c25aeba3b6758548bcbbd8b9d" +dependencies = [ + "serde", +] + +[[package]] +name = "nu-ansi-term" +version = "0.50.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "7957b9740744892f114936ab4a57b3f487491bbeafaf8083688b16841a4240e5" +dependencies = [ + "windows-sys 0.61.2", +] + +[[package]] +name = "num-bigint" +version = "0.4.8" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "c89e69e7e0f03bea5ef08013795c25018e101932225a656383bd384495ecc367" +dependencies = [ + "num-integer", + "num-traits", +] + +[[package]] +name = "num-complex" +version = "0.4.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "73f88a1307638156682bada9d7604135552957b7818057dcef22705b4d509495" +dependencies = [ + "num-traits", +] + +[[package]] +name = "num-conv" +version = "0.2.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "521739c6d2bac4aa25192232afe6841231376b2b26d4d9fae5ecf8ca5772e441" + +[[package]] +name = "num-integer" +version = "0.1.47" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "7ce2d95d4b3734dc35aa2f45e1aa22cd416814592a4f9d9205e11affd5b8e10b" +dependencies = [ + "num-traits", +] + +[[package]] +name = "num-rational" +version = "0.4.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f83d14da390562dca69fc84082e73e548e1ad308d24accdedd2720017cb37824" +dependencies = [ + "num-bigint", + "num-integer", + "num-traits", +] + +[[package]] +name = "num-traits" +version = "0.2.19" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "071dfc062690e90b734c0b2273ce72ad0ffa95f0c74596bc250dcfd960262841" +dependencies = [ + "autocfg", +] + +[[package]] +name = "num_cpus" +version = "1.17.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "91df4bbde75afed763b708b7eee1e8e7651e02d97f6d5dd763e89367e957b23b" +dependencies = [ + "hermit-abi", + "libc", +] + +[[package]] +name = "oid-registry" +version = "0.8.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "12f40cff3dde1b6087cc5d5f5d4d65712f34016a03ed60e9c08dcc392736b5b7" +dependencies = [ + "asn1-rs", +] + +[[package]] +name = "once_cell" +version = "1.21.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9f7c3e4beb33f85d45ae3e3a1792185706c8e16d043238c593331cc7cd313b50" + +[[package]] +name = "openssl-probe" +version = "0.2.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "7c87def4c32ab89d880effc9e097653c8da5d6ef28e6b539d313baaacfbafcbe" + +[[package]] +name = "option-ext" +version = "0.2.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "04744f49eae99ab78e0d5c0b603ab218f515ea8cfe5a456d7629ad883a3b6e7d" + +[[package]] +name = "owned-alloc" +version = "0.2.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "30fceb411f9a12ff9222c5f824026be368ff15dc2f13468d850c7d3f502205d6" + +[[package]] +name = "parking" +version = "2.2.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f38d5652c16fde515bb1ecef450ab0f6a219d619a7274976324d5e377f7dceba" + +[[package]] +name = "parking_lot" +version = "0.12.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "93857453250e3077bd71ff98b6a65ea6621a19bb0f559a85248955ac12c45a1a" +dependencies = [ + "lock_api", + "parking_lot_core", +] + +[[package]] +name = "parking_lot_core" +version = "0.9.12" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "2621685985a2ebf1c516881c026032ac7deafcda1a2c9b7850dc81e3dfcb64c1" +dependencies = [ + "cfg-if", + "libc", + "redox_syscall", + "smallvec", + "windows-link", +] + +[[package]] +name = "paste" +version = "1.0.15" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "57c0d7b74b563b49d38dae00a0c37d4d6de9b432382b2892f0574ddcae73fd0a" + +[[package]] +name = "pem" +version = "4.0.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "d354a98a3d1251555de99e8fdd8afda05573c31b82f59063a7b0a29b5527f120" +dependencies = [ + "base64 0.23.1", + "serde_core", +] + +[[package]] +name = "percent-encoding" +version = "2.3.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9b4f627cb1b25917193a259e49bdad08f671f8d9708acfd5fe0a8c1455d87220" + +[[package]] +name = "pest" +version = "2.9.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "5a07a60cc7a4d00c91f95c685609d1d2f79050e6804b70ebedd7650f0b839bcf" +dependencies = [ + "memchr", + "ucd-trie", +] + +[[package]] +name = "pest_derive" +version = "2.9.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b3a83744a5c8455b8b3e0dc5031362780a347c878bdd11584d1a8984228cc88d" +dependencies = [ + "pest", + "pest_generator", +] + +[[package]] +name = "pest_generator" +version = "2.9.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e0cd3451aa3de60d4b9a1e736885e4dea6b31617598026f12256ad566d63304a" +dependencies = [ + "pest", + "pest_meta", + "proc-macro2", + "quote", + "syn 2.0.119", +] + +[[package]] +name = "pest_meta" +version = "2.9.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e04d3a0849e241d7dfce834c83b1c5edc8622009e8dd51a12ba1927c32f05496" +dependencies = [ + "pest", +] + +[[package]] +name = "petgraph" +version = "0.8.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8701b58ea97060d5e5b155d383a69952a60943f0e6dfe30b04c287beb0b27455" +dependencies = [ + "fixedbitset", + "hashbrown 0.15.5", + "indexmap 2.14.1", + "serde", +] + +[[package]] +name = "phf" +version = "0.13.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "c1562dc717473dbaa4c1f85a36410e03c047b2e7df7f45ee938fbef64ae7fadf" +dependencies = [ + "phf_macros", + "phf_shared", + "serde", +] + +[[package]] +name = "phf_generator" +version = "0.13.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "135ace3a761e564ec88c03a77317a7c6b80bb7f7135ef2544dbe054243b89737" +dependencies = [ + "fastrand", + "phf_shared", +] + +[[package]] +name = "phf_macros" +version = "0.13.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "812f032b54b1e759ccd5f8b6677695d5268c588701effba24601f6932f8269ef" +dependencies = [ + "phf_generator", + "phf_shared", + "proc-macro2", + "quote", + "syn 2.0.119", +] + +[[package]] +name = "phf_shared" +version = "0.13.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e57fef6bc5981e38c2ce2d63bfa546861309f875b8a75f092d1d54ae2d64f266" +dependencies = [ + "siphasher", +] + +[[package]] +name = "pin-project-lite" +version = "0.2.17" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a89322df9ebe1c1578d689c92318e070967d1042b512afbe49518723f4e6d5cd" + +[[package]] +name = "pnet_base" +version = "0.35.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ffc190d4067df16af3aba49b3b74c469e611cad6314676eaf1157f31aa0fb2f7" +dependencies = [ + "no-std-net", +] + +[[package]] +name = "pnet_datalink" +version = "0.35.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e79e70ec0be163102a332e1d2d5586d362ad76b01cec86f830241f2b6452a7b7" +dependencies = [ + "ipnetwork", + "libc", + "pnet_base", + "pnet_sys", + "winapi", +] + +[[package]] +name = "pnet_sys" +version = "0.35.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "7d4643d3d4db6b08741050c2f3afa9a892c4244c085a72fcda93c9c2c9a00f4b" +dependencies = [ + "libc", + "winapi", +] + +[[package]] +name = "portable-atomic" +version = "1.15.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "05c8b63e8d9609db387f0324918f81d68fe27748f084ef092fb35954d0539a85" + +[[package]] +name = "portable-atomic-util" +version = "0.2.7" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "c2a106d1259c23fac8e543272398ae0e3c0b8d33c88ed73d0cc71b0f1d902618" +dependencies = [ + "portable-atomic", +] + +[[package]] +name = "potential_utf" +version = "0.1.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "d83eb9bc6d8e5cf568e7a1101d60ee05e81ed50ea106026f3d18deeb046d7661" +dependencies = [ + "zerovec", +] + +[[package]] +name = "powerfmt" +version = "0.2.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "439ee305def115ba05938db6eb1644ff94165c5ab5e9420d1c1bcedbba909391" + +[[package]] +name = "ppv-lite86" +version = "0.2.21" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "85eae3c4ed2f50dcfe72643da4befc30deadb458a9b590d720cde2f2b1e97da9" +dependencies = [ + "zerocopy", +] + +[[package]] +name = "proc-macro-crate" +version = "3.5.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e67ba7e9b2b56446f1d419b1d807906278ffa1a658a8a5d8a39dcb1f5a78614f" +dependencies = [ + "toml_edit", +] + +[[package]] +name = "proc-macro-error-attr3" +version = "3.1.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9e564d14133360e1ae169ffde5da25881b5fa47261665b8e5713c212c27799da" +dependencies = [ + "proc-macro2", + "quote", +] + +[[package]] +name = "proc-macro-error3" +version = "3.1.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8f0d4471b3436c22106b21913b1dda531558918ae9b7ec55d58aa84b43552233" +dependencies = [ + "proc-macro-error-attr3", + "proc-macro2", + "quote", + "syn 3.0.6", +] + +[[package]] +name = "proc-macro2" +version = "1.0.107" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "985e7ec9bb745e6ce6535b544d84d6cd6f7ad8bd711c398938ae983b91a766d9" +dependencies = [ + "unicode-ident", +] + +[[package]] +name = "prometheus-client" +version = "0.24.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "cca3d75b4566b9a29fe1ed623587fb058e826eb329a0be4b7c4da1ebb2d7a6ca" +dependencies = [ + "dtoa", + "itoa", + "parking_lot", + "prometheus-client-derive-encode", +] + +[[package]] +name = "prometheus-client-derive-encode" +version = "0.5.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9adf1691c04c0a5ff46ff8f262b58beb07b0dbb61f96f9f54f6cbd82106ed87f" +dependencies = [ + "proc-macro2", + "quote", + "syn 2.0.119", +] + +[[package]] +name = "quinn" +version = "0.11.11" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0c1a41e437b6bbd489372cd4971de128e85c855f56c57f283d20ff016cf7c0a8" +dependencies = [ + "bytes", + "cfg_aliases 0.2.2", + "pin-project-lite", + "quinn-proto", + "quinn-udp", + "rustc-hash", + "rustls", + "socket2 0.6.5", + "thiserror", + "tokio", + "tracing", + "web-time", +] + +[[package]] +name = "quinn-proto" +version = "0.11.17" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "04759210543be93709136e28212294a659ef5001836ff4eab4d663e4529bba83" +dependencies = [ + "bytes", + "fastbloom", + "getrandom 0.4.3", + "lru-slab", + "rand 0.10.2", + "rand_pcg", + "ring", + "rustc-hash", + "rustls", + "rustls-pki-types", + "rustls-platform-verifier", + "slab", + "thiserror", + "tinyvec", + "tracing", + "web-time", +] + +[[package]] +name = "quinn-udp" +version = "0.5.15" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "35a133f956daabe89a61a685c2649f13d82d5aa4bd5d12d1277e1072a21c0694" +dependencies = [ + "cfg_aliases 0.2.2", + "libc", + "once_cell", + "socket2 0.6.5", + "tracing", + "windows-sys 0.61.2", +] + +[[package]] +name = "quote" +version = "1.0.47" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1fbf4db142a473a8d80c26bbf18454ed458bf8d26c8219c331daecfdbd079001" +dependencies = [ + "proc-macro2", +] + +[[package]] +name = "r-efi" +version = "5.3.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "69cdb34c158ceb288df11e18b4bd39de994f6657d83847bdffdbd7f346754b0f" + +[[package]] +name = "r-efi" +version = "6.0.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f8dcc9c7d52a811697d2151c701e0d08956f92b0e24136cf4cf27b57a6a0d9bf" + +[[package]] +name = "rand" +version = "0.8.8" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e058c7de0b26af77780c769414d6257830bb240f3c38477dbc2c16e5f54d6d4c" +dependencies = [ + "libc", + "rand_chacha", + "rand_core 0.6.4", +] + +[[package]] +name = "rand" +version = "0.10.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "c7f5fa3a058cd35567ef9bfa5e75732bee0f9e4c55fa90477bef2dfcdbc4be80" +dependencies = [ + "chacha20", + "getrandom 0.4.3", + "rand_core 0.10.1", +] + +[[package]] +name = "rand_chacha" +version = "0.3.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e6c10a63a0fa32252be49d21e7709d4d4baf8d231c2dbce1eaa8141b9b127d88" +dependencies = [ + "ppv-lite86", + "rand_core 0.6.4", +] + +[[package]] +name = "rand_core" +version = "0.6.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ec0be4795e2f6a28069bec0b5ff3e2ac9bafc99e6a9a7dc3547996c5c816922c" +dependencies = [ + "getrandom 0.2.17", +] + +[[package]] +name = "rand_core" +version = "0.10.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "63b8176103e19a2643978565ca18b50549f6101881c443590420e4dc998a3c69" + +[[package]] +name = "rand_pcg" +version = "0.10.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "caa0f4137e1c0a72f4c651489402276c8e8e1cf081f3b0ba156d2cbeef09e86a" +dependencies = [ + "rand_core 0.10.1", +] + +[[package]] +name = "rawpointer" +version = "0.2.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "60a357793950651c4ed0f3f52338f53b2f809f32d83a07f72909fa13e4c6c1e3" + +[[package]] +name = "rayon" +version = "1.12.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "fb39b166781f92d482534ef4b4b1b2568f42613b53e5b6c160e24cfbfa30926d" +dependencies = [ + "either", + "rayon-core", +] + +[[package]] +name = "rayon-core" +version = "1.13.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "22e18b0f0062d30d4230b2e85ff77fdfe4326feb054b9783a3460d8435c8ab91" +dependencies = [ + "crossbeam-deque", + "crossbeam-utils", +] + +[[package]] +name = "rcgen" +version = "0.14.10" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8774e05a7d0de114588e6a28fe7e71694b82614ed569d86d8b389dfbc98b8ad8" +dependencies = [ + "pem", + "ring", + "rustls-pki-types", + "time", + "x509-parser", + "yasna", +] + +[[package]] +name = "re_cdr" +version = "0.1.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "bba10c21433b597481f366b978cdf2617e56785b38cb0dcc3833c6b8ad7e061a" +dependencies = [ + "bytemuck", + "byteorder", + "serde", + "thiserror", +] + +[[package]] +name = "reading-processor" +version = "0.1.0" +dependencies = [ + "dimos-module", + "story-messages-messages", + "tokio", + "tracing", +] + +[[package]] +name = "redox_syscall" +version = "0.5.18" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ed2bf2547551a7053d6fdfafda3f938979645c44812fbfcda098faae3f1a362d" +dependencies = [ + "bitflags 2.13.1", +] + +[[package]] +name = "redox_users" +version = "0.5.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a4e608c6638b9c18977b00b475ac1f28d14e84b27d8d42f70e0bf1e3dec127ac" +dependencies = [ + "getrandom 0.2.17", + "libredox", + "thiserror", +] + +[[package]] +name = "ref-cast" +version = "1.0.27" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "7e440fb4e4b4147295338efb76001ab9e4efc0e5839df2c47fc5ac2381d365c3" +dependencies = [ + "ref-cast-impl", +] + +[[package]] +name = "ref-cast-impl" +version = "1.0.27" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "92ecd8964f8453721699a1ed72037b0db49ce2f5a5138486ee89bed6f67cdf3a" +dependencies = [ + "proc-macro2", + "quote", + "syn 3.0.6", +] + +[[package]] +name = "regex" +version = "1.13.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f020237b6c8eed93db2e2cb53c00c60a8e1bc73da7d073199a1180401450218d" +dependencies = [ + "aho-corasick", + "memchr", + "regex-automata", + "regex-syntax", +] + +[[package]] +name = "regex-automata" +version = "0.4.18" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ad8553b9b26413251cbf30e620595c7a41b3887f03da04579c0e6b0d6a06b4b2" +dependencies = [ + "aho-corasick", + "memchr", + "regex-syntax", +] + +[[package]] +name = "regex-syntax" +version = "0.8.11" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "d6f6ff9a378485b298a5286656da665ba74413d36db0979633275d2e708145d4" + +[[package]] +name = "ring" +version = "0.17.14" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a4689e6c2294d81e88dc6261c768b63bc4fcdb852be6d1352498b114f61383b7" +dependencies = [ + "cc", + "cfg-if", + "getrandom 0.2.17", + "libc", + "untrusted", + "windows-sys 0.52.0", +] + +[[package]] +name = "ringbuffer-spsc" +version = "0.1.15" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8d3e7aa0a681b232e7cd7f856a53b10603df88ca74b79a8d8088845185492e35" +dependencies = [ + "array-init", + "crossbeam", +] + +[[package]] +name = "rlimit" +version = "0.10.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "7043b63bd0cd1aaa628e476b80e6d4023a3b50eb32789f2728908107bd0c793a" +dependencies = [ + "libc", +] + +[[package]] +name = "ron" +version = "0.12.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "81116b9531d61eabc41aeb228e4b6b2435bcca3233b98cf3b3077d4e6e9debb3" +dependencies = [ + "bitflags 2.13.1", + "once_cell", + "serde", + "serde_derive", + "typeid", + "unicode-ident", +] + +[[package]] +name = "rustc-hash" +version = "2.1.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "6b1e7f9a428571be2dc5bc0505c13fb6bf936822b894ec87abf8a08a4e51742d" + +[[package]] +name = "rustc_version" +version = "0.4.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "cfcb3a22ef46e85b45de6ee7e79d063319ebb6594faafcf1c225ea92ab6e9b92" +dependencies = [ + "semver", +] + +[[package]] +name = "rusticata-macros" +version = "4.1.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "faf0c4a6ece9950b9abdb62b1cfcf2a68b3b67a10ba445b3bb85be2a293d0632" +dependencies = [ + "nom", +] + +[[package]] +name = "rustls" +version = "0.23.43" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0283386ce02abc0151e1761d08802dfe86c173b0b494af5cbc086574e453da06" +dependencies = [ + "log", + "once_cell", + "ring", + "rustls-pki-types", + "rustls-webpki", + "subtle", + "zeroize", +] + +[[package]] +name = "rustls-native-certs" +version = "0.8.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "dab5152771c58876a2146916e53e35057e1a4dfa2b9df0f0305b07f611fdea4d" +dependencies = [ + "openssl-probe", + "rustls-pki-types", + "schannel", + "security-framework", +] + +[[package]] +name = "rustls-pemfile" +version = "2.2.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "dce314e5fee3f39953d46bb63bb8a46d40c2f8fb7cc5a3b6cab2bde9721d6e50" +dependencies = [ + "rustls-pki-types", +] + +[[package]] +name = "rustls-pki-types" +version = "1.15.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "2f4925028c7eb5d1fcdaf196971378ed9d2c1c4efc7dc5d011256f76c99c0a96" +dependencies = [ + "web-time", + "zeroize", +] + +[[package]] +name = "rustls-platform-verifier" +version = "0.7.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "26d1e2536ce4f35f4846aa13bff16bd0ff40157cdb14cc056c7b14ba41233ba0" +dependencies = [ + "core-foundation", + "core-foundation-sys", + "jni", + "log", + "once_cell", + "rustls", + "rustls-native-certs", + "rustls-platform-verifier-android", + "rustls-webpki", + "security-framework", + "security-framework-sys", + "webpki-root-certs", + "windows-sys 0.61.2", +] + +[[package]] +name = "rustls-platform-verifier-android" +version = "0.1.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f87165f0995f63a9fbeea62b64d10b4d9d8e78ec6d7d51fb2125fda7bb36788f" + +[[package]] +name = "rustls-webpki" +version = "0.103.15" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f3c3cf1d8b1e7d4927e2d154c3fcb02979afb9939629c62cd9048d4f07b60ac2" +dependencies = [ + "ring", + "rustls-pki-types", + "untrusted", +] + +[[package]] +name = "rustversion" +version = "1.0.23" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "cf54715a573b99ac80df0bc206da022bcd442c974952c7b9720069370852e21f" + +[[package]] +name = "ryu" +version = "1.0.23" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9774ba4a74de5f7b1c1451ed6cd5285a32eddb5cccb8cc655a4e50009e06477f" + +[[package]] +name = "safe_arch" +version = "1.2.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "42c6efa15875e6ecb39ca61fb0b0c1a40b84fac5a5ffe71eef7d1000c8eb3f5f" +dependencies = [ + "bytemuck", +] + +[[package]] +name = "same-file" +version = "1.0.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "93fc1dc3aaa9bfed95e02e6eadabb4baf7e3078b0bd1b4d7b6b0b68378900502" +dependencies = [ + "winapi-util", +] + +[[package]] +name = "schannel" +version = "0.1.29" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "91c1b7e4904c873ef0710c1f407dde2e6287de2bebc1bbbf7d430bb7cbffd939" +dependencies = [ + "windows-sys 0.61.2", +] + +[[package]] +name = "schemars" +version = "0.9.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "4cd191f9397d57d581cddd31014772520aa448f65ef991055d7f61582c65165f" +dependencies = [ + "dyn-clone", + "ref-cast", + "serde", + "serde_json", +] + +[[package]] +name = "schemars" +version = "1.2.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "687274d293b6cdc6e73e0fee520bf2049650090d7164f87672d212a3c530cf4a" +dependencies = [ + "dyn-clone", + "either", + "ref-cast", + "schemars_derive", + "serde", + "serde_json", +] + +[[package]] +name = "schemars_derive" +version = "1.2.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "d98c67716b46af2f0b8cf752abc930f6f9aecfbf671ecfb531db8a31dbe4e2ba" +dependencies = [ + "proc-macro2", + "quote", + "serde_derive_internals", + "syn 3.0.6", +] + +[[package]] +name = "scopeguard" +version = "1.2.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "94143f37725109f92c262ed2cf5e59bce7498c01bcc1502d7b9afe439a4e9f49" + +[[package]] +name = "secrecy" +version = "0.8.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9bd1c54ea06cfd2f6b63219704de0b9b4f72dcc2b8fdef820be6cd799780e91e" +dependencies = [ + "serde", + "zeroize", +] + +[[package]] +name = "security-framework" +version = "3.7.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b7f4bc775c73d9a02cde8bf7b2ec4c9d12743edf609006c7facc23998404cd1d" +dependencies = [ + "bitflags 2.13.1", + "core-foundation", + "core-foundation-sys", + "libc", + "security-framework-sys", +] + +[[package]] +name = "security-framework-sys" +version = "2.17.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "6ce2691df843ecc5d231c0b14ece2acc3efb62c0a398c7e1d875f3983ce020e3" +dependencies = [ + "core-foundation-sys", + "libc", +] + +[[package]] +name = "semver" +version = "1.0.28" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8a7852d02fc848982e0c167ef163aaff9cd91dc640ba85e263cb1ce46fae51cd" + +[[package]] +name = "serde" +version = "1.0.229" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "4148590afebada386688f18773da617792bf2ef03ffc1e4cbd2b1d45b023e0ba" +dependencies = [ + "serde_core", + "serde_derive", +] + +[[package]] +name = "serde-big-array" +version = "0.5.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "11fc7cc2c76d73e0f27ee52abbd64eec84d46f370c88371120433196934e4b7f" +dependencies = [ + "serde", +] + +[[package]] +name = "serde_core" +version = "1.0.229" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "67dca2c9c51e58a4791a4b1ed58308b39c64224d349a935ab5039aa360942a48" +dependencies = [ + "serde_derive", +] + +[[package]] +name = "serde_derive" +version = "1.0.229" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e7a5d71263a5a7d47b41f6b3f06ba276f10cc18b0931f1799f710578e2309348" +dependencies = [ + "proc-macro2", + "quote", + "syn 3.0.6", +] + +[[package]] +name = "serde_derive_internals" +version = "0.30.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f852137cce035d6a4df67ccce505ff6b3e9fd3a10e3e52b24dc71e650bb1a9bd" +dependencies = [ + "proc-macro2", + "quote", + "syn 3.0.6", +] + +[[package]] +name = "serde_json" +version = "1.0.151" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "c841b55ecdae098c80dcae9cf767f6f8a0c2cdb3416bbef72181df4d0fe73f14" +dependencies = [ + "indexmap 2.14.1", + "itoa", + "memchr", + "serde", + "serde_core", + "zmij", +] + +[[package]] +name = "serde_spanned" +version = "1.1.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "6662b5879511e06e8999a8a235d848113e942c9124f211511b16466ee2995f26" +dependencies = [ + "serde_core", +] + +[[package]] +name = "serde_with" +version = "3.22.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ee78f1fbe43ac4a0e47aadb3dbd357b69eb0d3793e948624cd03dd2750ab1c0a" +dependencies = [ + "base64 0.22.1", + "bs58", + "chrono", + "hex", + "indexmap 1.9.3", + "indexmap 2.14.1", + "jiff", + "schemars 0.9.0", + "schemars 1.2.2", + "serde_core", + "serde_json", + "serde_with_macros", + "time", +] + +[[package]] +name = "serde_with_macros" +version = "3.22.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8705578779c2b6bd90d84d66eb2e206b708b1a4d7b9f17641b293545bf1c7e46" +dependencies = [ + "darling", + "proc-macro2", + "quote", + "syn 2.0.119", +] + +[[package]] +name = "serde_yaml" +version = "0.9.34+deprecated" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "6a8b1a1a2ebf674015cc02edccce75287f1a0130d394307b36743c2f5d504b47" +dependencies = [ + "indexmap 2.14.1", + "itoa", + "ryu", + "serde", + "unsafe-libyaml", +] + +[[package]] +name = "sha2-const-stable" +version = "0.1.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "5f179d4e11094a893b82fff208f74d448a7512f99f5a0acbd5c679b705f83ed9" + +[[package]] +name = "sha3" +version = "0.10.9" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "77fd7028345d415a4034cf8777cd4f8ab1851274233b45f84e3d955502d93874" +dependencies = [ + "digest", + "keccak", +] + +[[package]] +name = "sharded-slab" +version = "0.1.7" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f40ca3c46823713e0d4209592e8d6e826aa57e928f09752619fc696c499637f6" +dependencies = [ + "lazy_static", +] + +[[package]] +name = "shellexpand" +version = "3.1.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "32824fab5e16e6c4d86dc1ba84489390419a39f97699852b66480bb87d297ed8" +dependencies = [ + "dirs", +] + +[[package]] +name = "shlex" +version = "2.0.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f8fadd59c855ef2080decdef8ff161eb6661b86933c9d82e5ba29dc602a55aba" + +[[package]] +name = "signal-hook-registry" +version = "1.4.8" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "c4db69cba1110affc0e9f7bcd48bbf87b3f4fc7c61fc9155afd4c469eb3d6c1b" +dependencies = [ + "errno", + "libc", +] + +[[package]] +name = "simba" +version = "0.10.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f1a7200d82ff1c7b4235efd12f11e2537a402c91cf83a5cb97ce80eef787fde7" +dependencies = [ + "approx", + "num-complex", + "num-traits", + "wide", +] + +[[package]] +name = "simd-adler32" +version = "0.3.10" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "3a219298ac11a56ea9a6d2120044824d6f01aeb034955e7af7bc16858527deea" + +[[package]] +name = "simd_cesu8" +version = "1.2.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "11031e251abf8611c80f460e19dbdeb54a66db918e49c65a7065b46ac7aec520" +dependencies = [ + "rustc_version", + "simdutf8", +] + +[[package]] +name = "simdutf8" +version = "0.1.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e3a9fe34e3e7a50316060351f37187a3f546bce95496156754b601a5fa71b76e" + +[[package]] +name = "siphasher" +version = "1.0.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8ee5873ec9cce0195efcb7a4e9507a04cd49aec9c83d0389df45b1ef7ba2e649" + +[[package]] +name = "slab" +version = "0.4.12" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0c790de23124f9ab44544d7ac05d60440adc586479ce501c1d6d7da3cd8c9cf5" + +[[package]] +name = "smallvec" +version = "1.15.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8ed6a63f02c8539c91a8685a86f4099661ba3da017932f6ebbea6de3f0fa7c90" + +[[package]] +name = "socket2" +version = "0.5.10" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e22376abed350d73dd1cd119b57ffccad95b4e585a7cda43e286245ce23c0678" +dependencies = [ + "libc", + "windows-sys 0.52.0", +] + +[[package]] +name = "socket2" +version = "0.6.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "c3d1e2c7f27f8d4cb10542a02c49005dbd6e93095799d6f3be745fae9f8fedd4" +dependencies = [ + "libc", + "windows-sys 0.61.2", +] + +[[package]] +name = "spin" +version = "0.9.9" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "3763264f6b73151db08c50ff20d7d8a0b8796e021cdea7ceedad07b80155fa0e" +dependencies = [ + "lock_api", +] + +[[package]] +name = "spin" +version = "0.10.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "023a211cb3138dbc438680b32560ad89f699977624c9f8dbb95a47d5b4c07dd3" + +[[package]] +name = "stabby" +version = "72.1.16" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "3d53d2428934c46277fafd2d41e39357595aa1e47954c75db2b14ed90632f3cc" +dependencies = [ + "rustversion", + "stabby-abi", +] + +[[package]] +name = "stabby-abi" +version = "72.1.16" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f375eae680bb54203ee5e47d4cd2ae7b79c0a79ed90919279f38f500ad53f190" +dependencies = [ + "rustc_version", + "rustversion", + "sha2-const-stable", + "stabby-macros", +] + +[[package]] +name = "stabby-macros" +version = "72.1.16" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ea664671a576c5f7e32fee291ac123d82af5e92b0689beb3555347c00c76eef1" +dependencies = [ + "proc-macro-crate", + "proc-macro2", + "quote", + "syn 2.0.119", +] + +[[package]] +name = "stable_deref_trait" +version = "1.2.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "6ce2be8dc25455e1f91df71bfa12ad37d7af1092ae736f3a6cd0e37bc7810596" + +[[package]] +name = "static_assertions" +version = "1.1.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a2eb9349b6444b326872e140eb1cf5e7c522154d69e7a0ffb0fb81c06b37543f" + +[[package]] +name = "static_init" +version = "1.0.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8bae1df58c5fea7502e8e352ec26b5579f6178e1fdb311e088580c980dee25ed" +dependencies = [ + "bitflags 1.3.2", + "cfg_aliases 0.2.2", + "libc", + "parking_lot", + "parking_lot_core", + "static_init_macro", + "winapi", +] + +[[package]] +name = "static_init_macro" +version = "1.0.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1389c88ddd739ec6d3f8f83343764a0e944cd23cfbf126a9796a714b0b6edd6f" +dependencies = [ + "cfg_aliases 0.1.1", + "memchr", + "proc-macro2", + "quote", + "syn 1.0.109", +] + +[[package]] +name = "story-messages-messages" +version = "0.1.0" +dependencies = [ + "dimos-generated-messages", + "re_cdr", + "serde", + "serde-big-array", +] + +[[package]] +name = "strsim" +version = "0.11.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "7da8b5736845d9f2fcb837ea5d9e2628564b3b043a70948a3f0b778838c5fb4f" + +[[package]] +name = "subtle" +version = "2.6.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "13c2bddecc57b384dee18652358fb23172facb8a2c51ccc10d74c157bdea3292" + +[[package]] +name = "syn" +version = "1.0.109" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "72b64191b275b66ffe2469e8af2c1cfe3bafa67b529ead792a6d0160888b4237" +dependencies = [ + "proc-macro2", + "quote", + "unicode-ident", +] + +[[package]] +name = "syn" +version = "2.0.119" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "872831b642d1a07999a962a351ed35b955ea2cfc8f3862091e2a240a84f17297" +dependencies = [ + "proc-macro2", + "quote", + "unicode-ident", +] + +[[package]] +name = "syn" +version = "3.0.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8593e8e72159ed2257d083c7a454a85cbf854f37a0966d8d483aff8c8a3ebcee" +dependencies = [ + "proc-macro2", + "quote", + "unicode-ident", +] + +[[package]] +name = "synstructure" +version = "0.13.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "728a70f3dbaf5bab7f0c4b1ac8d7ae5ea60a4b5549c8a5914361c99147a709d2" +dependencies = [ + "proc-macro2", + "quote", + "syn 2.0.119", +] + +[[package]] +name = "talc" +version = "4.4.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a3ae828aa394de34c7de08f522d1b86bd1c182c668d27da69caadda00590f26d" + +[[package]] +name = "thiserror" +version = "2.0.21" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "09e52cb86a36cede5cb101bf8908837b3e4c6e5e59fe7fd85c23fb56200d189e" +dependencies = [ + "thiserror-impl", +] + +[[package]] +name = "thiserror-impl" +version = "2.0.21" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "fe5197923287db20a58125f0bc85c062f7f2c892de97b18c356f9efb14b28524" +dependencies = [ + "proc-macro2", + "quote", + "syn 3.0.6", +] + +[[package]] +name = "thread-priority" +version = "1.2.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "cfe075d7053dae61ac5413a34ea7d4913b6e6207844fd726bdd858b37ff72bf5" +dependencies = [ + "bitflags 2.13.1", + "cfg-if", + "libc", + "log", + "rustversion", + "winapi", +] + +[[package]] +name = "thread_local" +version = "1.1.10" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1ad99c4c6d32803332c548b1af0540b357b3f5fc0be8f6c6bfe8b2e6ae784070" +dependencies = [ + "cfg-if", +] + +[[package]] +name = "time" +version = "0.3.55" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "cdb87b95ec50ddfa440816d227a17b2ccbdda963a316a727fda0fc4334f7d134" +dependencies = [ + "deranged", + "num-conv", + "powerfmt", + "serde_core", + "time-core", + "time-macros", +] + +[[package]] +name = "time-core" +version = "0.1.9" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9e1c906769ad99c88eaa54e728060edef082f8e358ff32030cb7c7d315e81109" + +[[package]] +name = "time-macros" +version = "0.2.32" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "7e689342a48d2ea927c87ea50cabf8594854bf940e9310208848d680d668ed85" +dependencies = [ + "num-conv", + "time-core", +] + +[[package]] +name = "tinystr" +version = "0.8.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b1e27c91459209c2986af3dcf603a5a74a4368754ce37414f59acc971167f643" +dependencies = [ + "displaydoc", + "zerovec", +] + +[[package]] +name = "tinyvec" +version = "1.12.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "bb4ebadaa0af04fab11ae01eb5f9fdb5f9c5b875506e210e71c07873528baa7f" +dependencies = [ + "tinyvec_macros", +] + +[[package]] +name = "tinyvec_macros" +version = "0.1.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1f3ccbac311fea05f86f61904b462b55fb3df8837a366dfc601a0161d0532f20" + +[[package]] +name = "token-cell" +version = "2.1.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "fb48920ae769b58126c8c93269805011c793201f95fde28b479b81a9a531bbde" +dependencies = [ + "paste", + "portable-atomic", + "rustversion", +] + +[[package]] +name = "tokio" +version = "1.53.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "202caea871b69668250d242070849eb495be178ed697a3e98aebce5bc81a0bed" +dependencies = [ + "bytes", + "libc", + "mio", + "pin-project-lite", + "signal-hook-registry", + "socket2 0.6.5", + "tokio-macros", + "windows-sys 0.61.2", +] + +[[package]] +name = "tokio-macros" +version = "2.7.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "78773a2a397f451582ce068015985c33193cf6dea8b74d2a639fe457b2f07b0e" +dependencies = [ + "proc-macro2", + "quote", + "syn 3.0.6", +] + +[[package]] +name = "tokio-util" +version = "0.7.19" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "494815d09bf52b5548659851081238f0ca39ff638363907596da739561c62c52" +dependencies = [ + "bytes", + "futures-core", + "futures-sink", + "futures-util", + "pin-project-lite", + "tokio", +] + +[[package]] +name = "toml" +version = "0.9.12+spec-1.1.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "cf92845e79fc2e2def6a5d828f0801e29a2f8acc037becc5ab08595c7d5e9863" +dependencies = [ + "indexmap 2.14.1", + "serde_core", + "serde_spanned", + "toml_datetime 0.7.5+spec-1.1.0", + "toml_parser", + "toml_writer", + "winnow 0.7.15", +] + +[[package]] +name = "toml" +version = "1.1.4+spec-1.1.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "3aace63f4bbcdfc2c965b059de67119c89c4017a70d633be6c104910f67056f5" +dependencies = [ + "indexmap 2.14.1", + "serde_core", + "serde_spanned", + "toml_datetime 1.1.1+spec-1.1.0", + "toml_parser", + "toml_writer", + "winnow 1.0.4", +] + +[[package]] +name = "toml_datetime" +version = "0.7.5+spec-1.1.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "92e1cfed4a3038bc5a127e35a2d360f145e1f4b971b551a2ba5fd7aedf7e1347" +dependencies = [ + "serde_core", +] + +[[package]] +name = "toml_datetime" +version = "1.1.1+spec-1.1.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "3165f65f62e28e0115a00b2ebdd37eb6f3b641855f9d636d3cd4103767159ad7" +dependencies = [ + "serde_core", +] + +[[package]] +name = "toml_edit" +version = "0.25.13+spec-1.1.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "6975367e4d2ef766d86af01ffad14b622fecc8d4357a998fbc4deb6e9bacaf9b" +dependencies = [ + "indexmap 2.14.1", + "toml_datetime 1.1.1+spec-1.1.0", + "toml_parser", + "winnow 1.0.4", +] + +[[package]] +name = "toml_parser" +version = "1.1.3+spec-1.1.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1d38ac1cf9b95face32296c0a3ede1fdc270627c9d9c02a7274dd6d960dc4d56" +dependencies = [ + "winnow 1.0.4", +] + +[[package]] +name = "toml_writer" +version = "1.1.2+spec-1.1.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "7d56353a2a665ad0f41a421187180aab746c8c325620617ad883a99a1cbe66d2" + +[[package]] +name = "tracing" +version = "0.1.44" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "63e71662fa4b2a2c3a26f570f037eb95bb1f85397f3cd8076caed2f026a6d100" +dependencies = [ + "log", + "pin-project-lite", + "tracing-attributes", + "tracing-core", +] + +[[package]] +name = "tracing-attributes" +version = "0.1.31" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "7490cfa5ec963746568740651ac6781f701c9c5ea257c58e057f3ba8cf69e8da" +dependencies = [ + "proc-macro2", + "quote", + "syn 2.0.119", +] + +[[package]] +name = "tracing-core" +version = "0.1.36" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "db97caf9d906fbde555dd62fa95ddba9eecfd14cb388e4f491a66d74cd5fb79a" +dependencies = [ + "once_cell", + "valuable", +] + +[[package]] +name = "tracing-log" +version = "0.2.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ee855f1f400bd0e5c02d150ae5de3840039a3f54b025156404e34c23c03f47c3" +dependencies = [ + "log", + "once_cell", + "tracing-core", +] + +[[package]] +name = "tracing-serde" +version = "0.2.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "704b1aeb7be0d0a84fc9828cae51dab5970fee5088f83d1dd7ee6f6246fc6ff1" +dependencies = [ + "serde", + "tracing-core", +] + +[[package]] +name = "tracing-subscriber" +version = "0.3.23" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "cb7f578e5945fb242538965c2d0b04418d38ec25c79d160cd279bf0731c8d319" +dependencies = [ + "matchers", + "nu-ansi-term", + "once_cell", + "regex-automata", + "serde", + "serde_json", + "sharded-slab", + "smallvec", + "thread_local", + "tracing", + "tracing-core", + "tracing-log", + "tracing-serde", +] + +[[package]] +name = "twox-hash" +version = "1.6.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "97fee6b57c6a41524a810daee9286c02d7752c4253064d0b05472833a438f675" +dependencies = [ + "cfg-if", + "static_assertions", +] + +[[package]] +name = "typeid" +version = "1.0.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "bc7d623258602320d5c55d1bc22793b57daff0ec7efc270ea7d55ce1d5f5471c" + +[[package]] +name = "typenum" +version = "1.20.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b6f5e870be6c3b371b77fe0ee0bafb859fa4964b4404c27de1d380043c4dda20" + +[[package]] +name = "ucd-trie" +version = "0.1.7" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "2896d95c02a80c6d6a5d6e953d479f5ddf2dfdb6a244441010e373ac0fb88971" + +[[package]] +name = "uhlc" +version = "0.8.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b62a645e3e4e6c85b7abe49b086aa3204119431f42b6123b0070419fb6e9d24e" +dependencies = [ + "humantime", + "lazy_static", + "log", + "rand 0.8.8", + "serde", + "spin 0.10.1", +] + +[[package]] +name = "unicode-ident" +version = "1.0.26" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "d245f478577f809a851594d02313b640fb437e0bb33866753cff937863096954" + +[[package]] +name = "unicode-xid" +version = "0.2.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ebc1c04c71510c7f702b52b7c350734c9ff1295c464a03335b00bb84fc54f853" + +[[package]] +name = "unsafe-libyaml" +version = "0.2.11" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "673aac59facbab8a9007c7f6108d11f63b603f7cabff99fabf650fea5c32b861" + +[[package]] +name = "untrusted" +version = "0.9.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8ecb6da28b8a351d773b68d5825ac39017e680750f980f3a1a85cd8dd28a47c1" + +[[package]] +name = "unzip-n" +version = "0.1.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "3b5bb2756c16fb66f80cfbf5fb0e0c09a7001e739f453c9ec241b9c8b1556fda" +dependencies = [ + "proc-macro2", + "quote", + "syn 2.0.119", +] + +[[package]] +name = "url" +version = "2.5.8" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ff67a8a4397373c3ef660812acab3268222035010ab8680ec4215f38ba3d0eed" +dependencies = [ + "form_urlencoded", + "idna", + "percent-encoding", + "serde", +] + +[[package]] +name = "utf8_iter" +version = "1.0.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b6c140620e7ffbb22c2dee59cafe6084a59b5ffc27a8859a5f0d494b5d52b6be" + +[[package]] +name = "validated_struct" +version = "2.2.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "869a93e8a7286e339e1128630051d82babbcd75d585975af07b9f3327220e60e" +dependencies = [ + "json5", + "serde", + "serde_json", + "validated_struct_macros", +] + +[[package]] +name = "validated_struct_macros" +version = "2.2.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8c44ce98e7227a04eeb4cf9c784109a5c9710e54849ceb4f09f8597247897f1e" +dependencies = [ + "proc-macro2", + "quote", + "syn 2.0.119", + "unzip-n", +] + +[[package]] +name = "validator" +version = "0.21.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "c3d68c6633c483df6780cc5277a417c7c2d1bceee2649d06c8ab6b0fd2dd3c81" +dependencies = [ + "idna", + "regex", + "serde", + "serde_derive", + "serde_json", + "url", + "validator_derive", +] + +[[package]] +name = "validator_derive" +version = "0.20.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "240e4b81c20a1d6d50d1d7265c658dfbd204e8b9ac4d80f3c931f39462196335" +dependencies = [ + "darling", + "proc-macro-error3", + "proc-macro2", + "quote", + "syn 2.0.119", +] + +[[package]] +name = "valuable" +version = "0.1.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ba73ea9cf16a25df0c8caa16c51acb937d5712a8429db78a3ee29d5dcacd3a65" + +[[package]] +name = "vec_map" +version = "0.8.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f1bddf1187be692e79c5ffeab891132dfb0f236ed36a43c7ed39f1165ee20191" + +[[package]] +name = "version_check" +version = "0.9.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0b928f33d975fc6ad9f86c8f283853ad26bdd5b10b7f1542aa2fa15e2289105a" + +[[package]] +name = "walkdir" +version = "2.5.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "29790946404f91d9c5d06f9874efddea1dc06c5efe94541a7d6863108e3a5e4b" +dependencies = [ + "same-file", + "winapi-util", +] + +[[package]] +name = "wasi" +version = "0.11.1+wasi-snapshot-preview1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ccf3ec651a847eb01de73ccad15eb7d99f80485de043efb2f370cd654f4ea44b" + +[[package]] +name = "wasip2" +version = "1.0.4+wasi-0.2.12" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b67efb37e106e55ce722a510d6b5f9c17f083e5fc79afc2badeb12cc313d9487" +dependencies = [ + "wit-bindgen", +] + +[[package]] +name = "wasm-bindgen" +version = "0.2.127" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1b70935747edd64d89de3efa29d73789b806c15798f8e7dca4d8ac356b50ce70" +dependencies = [ + "cfg-if", + "once_cell", + "rustversion", + "wasm-bindgen-macro", + "wasm-bindgen-shared", +] + +[[package]] +name = "wasm-bindgen-macro" +version = "0.2.127" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "77775f8f3f7217702089053b94958f8f54061a3f663417df76e19cbdcca29bc1" +dependencies = [ + "quote", + "wasm-bindgen-macro-support", +] + +[[package]] +name = "wasm-bindgen-macro-support" +version = "0.2.127" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e11d33f857dc2fb11b8bc75aee111aa9cbeb12cd9f25efd3d4c2a3dd4e235284" +dependencies = [ + "bumpalo", + "proc-macro2", + "quote", + "syn 2.0.119", + "wasm-bindgen-shared", +] + +[[package]] +name = "wasm-bindgen-shared" +version = "0.2.127" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "7ef64dbcc55df09c7e5a46182d181c2cfa3e925f3da937ea764728b4bbb9dcbf" +dependencies = [ + "unicode-ident", +] + +[[package]] +name = "web-time" +version = "1.1.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "5a6580f308b1fad9207618087a65c04e7a10bc77e02c8e84e9b00dd4b12fa0bb" +dependencies = [ + "js-sys", + "wasm-bindgen", +] + +[[package]] +name = "webpki-root-certs" +version = "1.0.9" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b96554aa2acc8ccdb7e1c9a58a7a68dd5d13bccc69cd124cb09406db612a1c9b" +dependencies = [ + "rustls-pki-types", +] + +[[package]] +name = "webpki-roots" +version = "1.0.9" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "7dcd9d09a39985f5344844e66b0c530a33843579125f23e21e9f0f220850f22a" +dependencies = [ + "rustls-pki-types", +] + +[[package]] +name = "wide" +version = "1.7.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8cf05ca94c9fba0c51316899caa1d1e74a064fc310a5efa7375e614343d415be" +dependencies = [ + "bytemuck", + "safe_arch", +] + +[[package]] +name = "win-sys" +version = "0.3.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "5b7b128a98c1cfa201b09eb49ba285887deb3cbe7466a98850eb1adabb452be5" +dependencies = [ + "windows", +] + +[[package]] +name = "winapi" +version = "0.3.9" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "5c839a674fcd7a98952e593242ea400abe93992746761e38641405d28b00f419" +dependencies = [ + "winapi-i686-pc-windows-gnu", + "winapi-x86_64-pc-windows-gnu", +] + +[[package]] +name = "winapi-i686-pc-windows-gnu" +version = "0.4.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ac3b87c63620426dd9b991e5ce0329eff545bccbbb34f3be09ff6fb6ab51b7b6" + +[[package]] +name = "winapi-util" +version = "0.1.11" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "c2a7b1c03c876122aa43f3020e6c3c3ee5c05081c9a00739faf7503aeba10d22" +dependencies = [ + "windows-sys 0.61.2", +] + +[[package]] +name = "winapi-x86_64-pc-windows-gnu" +version = "0.4.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "712e227841d057c1ee1cd2fb22fa7e5a5461ae8e48fa2ca79ec42cfc1931183f" + +[[package]] +name = "windows" +version = "0.34.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "45296b64204227616fdbf2614cefa4c236b98ee64dfaaaa435207ed99fe7829f" +dependencies = [ + "windows_aarch64_msvc 0.34.0", + "windows_i686_gnu 0.34.0", + "windows_i686_msvc 0.34.0", + "windows_x86_64_gnu 0.34.0", + "windows_x86_64_msvc 0.34.0", +] + +[[package]] +name = "windows-core" +version = "0.62.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b8e83a14d34d0623b51dce9581199302a221863196a1dde71a7663a4c2be9deb" +dependencies = [ + "windows-implement", + "windows-interface", + "windows-link", + "windows-result", + "windows-strings", +] + +[[package]] +name = "windows-implement" +version = "0.60.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "053e2e040ab57b9dc951b72c264860db7eb3b0200ba345b4e4c3b14f67855ddf" +dependencies = [ + "proc-macro2", + "quote", + "syn 2.0.119", +] + +[[package]] +name = "windows-interface" +version = "0.59.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "3f316c4a2570ba26bbec722032c4099d8c8bc095efccdc15688708623367e358" +dependencies = [ + "proc-macro2", + "quote", + "syn 2.0.119", +] + +[[package]] +name = "windows-link" +version = "0.2.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f0805222e57f7521d6a62e36fa9163bc891acd422f971defe97d64e70d0a4fe5" + +[[package]] +name = "windows-result" +version = "0.4.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "7781fa89eaf60850ac3d2da7af8e5242a5ea78d1a11c49bf2910bb5a73853eb5" +dependencies = [ + "windows-link", +] + +[[package]] +name = "windows-strings" +version = "0.5.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "7837d08f69c77cf6b07689544538e017c1bfcf57e34b4c0ff58e6c2cd3b37091" +dependencies = [ + "windows-link", +] + +[[package]] +name = "windows-sys" +version = "0.52.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "282be5f36a8ce781fad8c8ae18fa3f9beff57ec1b52cb3de0789201425d9a33d" +dependencies = [ + "windows-targets", +] + +[[package]] +name = "windows-sys" +version = "0.61.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ae137229bcbd6cdf0f7b80a31df61766145077ddf49416a728b02cb3921ff3fc" +dependencies = [ + "windows-link", +] + +[[package]] +name = "windows-targets" +version = "0.52.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9b724f72796e036ab90c1021d4780d4d3d648aca59e491e6b98e725b84e99973" +dependencies = [ + "windows_aarch64_gnullvm", + "windows_aarch64_msvc 0.52.6", + "windows_i686_gnu 0.52.6", + "windows_i686_gnullvm", + "windows_i686_msvc 0.52.6", + "windows_x86_64_gnu 0.52.6", + "windows_x86_64_gnullvm", + "windows_x86_64_msvc 0.52.6", +] + +[[package]] +name = "windows_aarch64_gnullvm" +version = "0.52.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "32a4622180e7a0ec044bb555404c800bc9fd9ec262ec147edd5989ccd0c02cd3" + +[[package]] +name = "windows_aarch64_msvc" +version = "0.34.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "17cffbe740121affb56fad0fc0e421804adf0ae00891205213b5cecd30db881d" + +[[package]] +name = "windows_aarch64_msvc" +version = "0.52.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "09ec2a7bb152e2252b53fa7803150007879548bc709c039df7627cabbd05d469" + +[[package]] +name = "windows_i686_gnu" +version = "0.34.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "2564fde759adb79129d9b4f54be42b32c89970c18ebf93124ca8870a498688ed" + +[[package]] +name = "windows_i686_gnu" +version = "0.52.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8e9b5ad5ab802e97eb8e295ac6720e509ee4c243f69d781394014ebfe8bbfa0b" + +[[package]] +name = "windows_i686_gnullvm" +version = "0.52.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0eee52d38c090b3caa76c563b86c3a4bd71ef1a819287c19d586d7334ae8ed66" + +[[package]] +name = "windows_i686_msvc" +version = "0.34.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9cd9d32ba70453522332c14d38814bceeb747d80b3958676007acadd7e166956" + +[[package]] +name = "windows_i686_msvc" +version = "0.52.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "240948bc05c5e7c6dabba28bf89d89ffce3e303022809e73deaefe4f6ec56c66" + +[[package]] +name = "windows_x86_64_gnu" +version = "0.34.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "cfce6deae227ee8d356d19effc141a509cc503dfd1f850622ec4b0f84428e1f4" + +[[package]] +name = "windows_x86_64_gnu" +version = "0.52.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "147a5c80aabfbf0c7d901cb5895d1de30ef2907eb21fbbab29ca94c5b08b1a78" + +[[package]] +name = "windows_x86_64_gnullvm" +version = "0.52.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "24d5b23dc417412679681396f2b49f3de8c1473deb516bd34410872eff51ed0d" + +[[package]] +name = "windows_x86_64_msvc" +version = "0.34.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "d19538ccc21819d01deaf88d6a17eae6596a12e9aafdbb97916fb49896d89de9" + +[[package]] +name = "windows_x86_64_msvc" +version = "0.52.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "589f6da84c646204747d1270a2a5661ea66ed1cced2631d546fdfb155959f9ec" + +[[package]] +name = "winnow" +version = "0.7.15" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "df79d97927682d2fd8adb29682d1140b343be4ac0f08fd68b7765d9c059d3945" + +[[package]] +name = "winnow" +version = "1.0.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "23b97319f7b8343df12cc98938e5c3eb436064524c8d2b4e30a1d3a36eecdf81" +dependencies = [ + "memchr", +] + +[[package]] +name = "wit-bindgen" +version = "0.57.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1ebf944e87a7c253233ad6766e082e3cd714b5d03812acc24c318f549614536e" + +[[package]] +name = "writeable" +version = "0.6.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "3ad82d2a33cdc9674dc7465672f271e096168fcdbe0f799d9e6db8c5892679dc" + +[[package]] +name = "x509-parser" +version = "0.18.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "d43b0f71ce057da06bc0851b23ee24f3f86190b07203dd8f567d0b706a185202" +dependencies = [ + "asn1-rs", + "data-encoding", + "der-parser", + "lazy_static", + "nom", + "oid-registry", + "ring", + "rusticata-macros", + "thiserror", + "time", +] + +[[package]] +name = "yasna" +version = "0.6.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b5f6765e852b9b4dc8e2a76843e4d64d1cea8e79bcde0b6901aea8e7c7f08282" +dependencies = [ + "bit-vec", + "time", +] + +[[package]] +name = "yoke" +version = "0.8.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "709fe23a0424b6a435d82152b1bd3fdfb0833487d5fa90d05d42762a9891fef5" +dependencies = [ + "stable_deref_trait", + "yoke-derive", + "zerofrom", +] + +[[package]] +name = "yoke-derive" +version = "0.8.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "de844c262c8848816172cef550288e7dc6c7b7814b4ee56b3e1553f275f1858e" +dependencies = [ + "proc-macro2", + "quote", + "syn 2.0.119", + "synstructure", +] + +[[package]] +name = "zenoh" +version = "1.10.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "6f7ad636296bfdb8a86bbbc9bae1b6a57f0e77b715769f99885ca2ca2368c73a" +dependencies = [ + "ahash", + "arc-swap", + "async-trait", + "bytes", + "const_format", + "flate2", + "flume", + "futures", + "git-version", + "itertools", + "json5", + "lazy_static", + "nonempty-collections", + "once_cell", + "petgraph", + "phf", + "rand 0.8.8", + "rustc_version", + "serde", + "serde_json", + "socket2 0.5.10", + "tokio", + "tokio-util", + "tracing", + "uhlc", + "vec_map", + "zenoh-buffers", + "zenoh-codec", + "zenoh-collections", + "zenoh-config", + "zenoh-core", + "zenoh-keyexpr", + "zenoh-link", + "zenoh-link-commons", + "zenoh-macros", + "zenoh-plugin-trait", + "zenoh-protocol", + "zenoh-result", + "zenoh-runtime", + "zenoh-shm", + "zenoh-sync", + "zenoh-task", + "zenoh-transport", + "zenoh-util", +] + +[[package]] +name = "zenoh-buffers" +version = "1.10.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a81e52197e18bc75587c566df732726aee6e0a1c8779d7a2ff609f461f6b1a07" +dependencies = [ + "zenoh-collections", +] + +[[package]] +name = "zenoh-codec" +version = "1.10.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "31b792ce46854f252af31869030224c2da6bd9d72a1cd2812e29b457444a07b1" +dependencies = [ + "rand 0.8.8", + "tracing", + "uhlc", + "zenoh-buffers", + "zenoh-protocol", + "zenoh-shm", +] + +[[package]] +name = "zenoh-collections" +version = "1.10.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "42b897fae0ddce178e3e58d3bb9ca131dd2a301dd869ebded37d9e1f3784aac0" +dependencies = [ + "ahash", +] + +[[package]] +name = "zenoh-config" +version = "1.10.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1b39224e3029571e0572f88590eb9078bcd60dedc6381ad1a5c8ad5fb7a474bf" +dependencies = [ + "json5", + "nonempty-collections", + "num_cpus", + "secrecy", + "serde", + "serde_json", + "serde_with", + "serde_yaml", + "toml 0.9.12+spec-1.1.0", + "tracing", + "uhlc", + "validated_struct", + "zenoh-core", + "zenoh-keyexpr", + "zenoh-macros", + "zenoh-protocol", + "zenoh-result", + "zenoh-util", +] + +[[package]] +name = "zenoh-core" +version = "1.10.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9335e8f6509fcbec68a073372c13a69ba90c3a284a86427257607d75586b6344" +dependencies = [ + "lazy_static", + "tokio", + "zenoh-result", + "zenoh-runtime", +] + +[[package]] +name = "zenoh-crypto" +version = "1.10.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1b496e8c984260c4d80ce58b42956081dc8656f0b875766e5ae0f8e39510fdee" +dependencies = [ + "aes", + "hmac", + "rand 0.8.8", + "rand_chacha", + "sha3", + "zenoh-result", +] + +[[package]] +name = "zenoh-keyexpr" +version = "1.10.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "af891f8d9f52c3e8617712b65b08fa9ae02f1f1b57e2739ff194bb65df1255df" +dependencies = [ + "getrandom 0.2.17", + "hashbrown 0.16.1", + "keyed-set", + "rand 0.8.8", + "schemars 1.2.2", + "serde", + "token-cell", + "zenoh-result", +] + +[[package]] +name = "zenoh-link" +version = "1.10.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "04f796c9df0ef7736d140363507498d8c5d0acf621c158113f122aa992979d3d" +dependencies = [ + "zenoh-config", + "zenoh-link-commons", + "zenoh-link-tcp", + "zenoh-link-udp", + "zenoh-protocol", + "zenoh-result", +] + +[[package]] +name = "zenoh-link-commons" +version = "1.10.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "4dd110109f4f89450491bd8942152020f632eb86d8fac10349a186185f654873" +dependencies = [ + "async-trait", + "base64 0.22.1", + "bytes", + "flume", + "futures", + "quinn", + "quinn-proto", + "rcgen", + "rustls", + "rustls-pemfile", + "rustls-pki-types", + "rustls-webpki", + "secrecy", + "serde", + "socket2 0.5.10", + "time", + "tokio", + "tokio-util", + "tracing", + "webpki-roots", + "x509-parser", + "zenoh-buffers", + "zenoh-codec", + "zenoh-config", + "zenoh-core", + "zenoh-protocol", + "zenoh-result", + "zenoh-runtime", + "zenoh-util", +] + +[[package]] +name = "zenoh-link-quic_datagram" +version = "1.10.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "97b2cddd28955877b7eceedfaf09c801aac31ab8f0cac32c936f31358f5af133" +dependencies = [ + "async-trait", + "rustls-webpki", + "time", + "tokio-util", + "tracing", + "zenoh-core", + "zenoh-link-commons", + "zenoh-protocol", + "zenoh-result", +] + +[[package]] +name = "zenoh-link-tcp" +version = "1.10.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "4801cc6fb608b5069f3ea6b82484c5f727019764e90e14838fe6930a9dffd9a1" +dependencies = [ + "async-trait", + "socket2 0.5.10", + "tokio", + "tokio-util", + "tracing", + "zenoh-config", + "zenoh-core", + "zenoh-link-commons", + "zenoh-protocol", + "zenoh-result", +] + +[[package]] +name = "zenoh-link-udp" +version = "1.10.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "6785a0b6f0b94131c002a8358aaef3eb99f5ab0e0cd444f28d1f1fac38bd4091" +dependencies = [ + "async-trait", + "libc", + "socket2 0.5.10", + "tokio", + "tokio-util", + "tracing", + "windows-sys 0.61.2", + "zenoh-buffers", + "zenoh-core", + "zenoh-link-commons", + "zenoh-link-quic_datagram", + "zenoh-protocol", + "zenoh-result", + "zenoh-sync", + "zenoh-util", +] + +[[package]] +name = "zenoh-macros" +version = "1.10.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0c813ccde166cf0527402d46615257e0d152140e66f8ecd685721f9399251407" +dependencies = [ + "proc-macro2", + "quote", + "syn 2.0.119", + "zenoh-keyexpr", +] + +[[package]] +name = "zenoh-plugin-trait" +version = "1.10.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "95cd9d698675a5b2f3e958a35df56f6d60f0388669158055b436c0cbe5357ad8" +dependencies = [ + "git-version", + "libloading", + "serde", + "stabby", + "tracing", + "zenoh-config", + "zenoh-keyexpr", + "zenoh-macros", + "zenoh-result", + "zenoh-util", +] + +[[package]] +name = "zenoh-protocol" +version = "1.10.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ce5505e2569421d3cb6792d4b3d9a6589d04add3bf34c0a7c37c03c2573b79ad" +dependencies = [ + "const_format", + "rand 0.8.8", + "serde", + "uhlc", + "zenoh-buffers", + "zenoh-keyexpr", + "zenoh-macros", + "zenoh-result", +] + +[[package]] +name = "zenoh-result" +version = "1.10.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "cf23203517d6dfe04393c42053093c7cffd9e385015e21c85dafaff9a342cccf" +dependencies = [ + "anyhow", +] + +[[package]] +name = "zenoh-runtime" +version = "1.10.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ddf72c7310a090b0b22496649274661de8a17ca0d0674af8f72f184a3ed78664" +dependencies = [ + "lazy_static", + "ron", + "serde", + "tokio", + "tracing", + "zenoh-macros", + "zenoh-result", +] + +[[package]] +name = "zenoh-shm" +version = "1.10.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "d8f49ff9d36be3fdeada23103c5ba10d6e7159269bd8b07fb744448005fb5734" +dependencies = [ + "advisory-lock", + "async-trait", + "buddy_system_allocator", + "crossbeam-channel", + "crossbeam-queue", + "nix", + "num-traits", + "rand 0.8.8", + "rlimit", + "stabby", + "static_assertions", + "static_init", + "talc", + "thread-priority", + "tokio", + "tracing", + "win-sys", + "winapi", + "zenoh-buffers", + "zenoh-core", + "zenoh-macros", + "zenoh-result", +] + +[[package]] +name = "zenoh-stats" +version = "1.10.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f610d551359e0d8864138d4c4c11ed8e3c9f6991717f4eef198ab323b4ff01f4" +dependencies = [ + "ahash", + "prometheus-client", + "serde_json", + "smallvec", + "zenoh-keyexpr", + "zenoh-protocol", +] + +[[package]] +name = "zenoh-sync" +version = "1.10.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9792a8fa9b0f30ebabd3e6b278441178b6832462e959388aa18f92a93527935b" +dependencies = [ + "arc-swap", + "event-listener", + "futures", + "tokio", + "zenoh-buffers", + "zenoh-collections", + "zenoh-core", +] + +[[package]] +name = "zenoh-task" +version = "1.10.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "fbb419757b0c6e824bebf34959302e6edf5a7caea4e725de88007a56f3ba4d4d" +dependencies = [ + "futures", + "tokio", + "tokio-util", + "tracing", + "zenoh-core", + "zenoh-runtime", +] + +[[package]] +name = "zenoh-transport" +version = "1.10.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "fc431ec1c066d7047c601ae4b4d4a359882c5464cd9c4ff3fe905ff6e10017af" +dependencies = [ + "async-trait", + "crossbeam-utils", + "flume", + "futures", + "lazy_static", + "lockfree", + "lz4_flex", + "rand 0.8.8", + "ringbuffer-spsc", + "serde", + "sha3", + "static_init", + "tokio", + "tokio-util", + "tracing", + "zenoh-buffers", + "zenoh-codec", + "zenoh-config", + "zenoh-core", + "zenoh-crypto", + "zenoh-link", + "zenoh-link-commons", + "zenoh-protocol", + "zenoh-result", + "zenoh-runtime", + "zenoh-shm", + "zenoh-stats", + "zenoh-sync", + "zenoh-task", + "zenoh-util", +] + +[[package]] +name = "zenoh-util" +version = "1.10.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "3f99b13044636f453bcb49ca520db4b1400c8e444fdf578e63a1aad0ed9004e0" +dependencies = [ + "async-trait", + "const_format", + "flume", + "home", + "humantime", + "lazy_static", + "libc", + "libloading", + "pnet_datalink", + "schemars 1.2.2", + "serde", + "serde_json", + "shellexpand", + "tokio", + "tracing", + "tracing-subscriber", + "winapi", + "zenoh-core", + "zenoh-result", +] + +[[package]] +name = "zerocopy" +version = "0.8.56" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "556764e583adb45a9f8d413c2a147fa7e8d821e48e12b14fd560b607998b75eb" +dependencies = [ + "zerocopy-derive", +] + +[[package]] +name = "zerocopy-derive" +version = "0.8.56" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f2ab42fc20575779bd240faa45f94a74256f755c0fa9e89f0ede20d91d0cdfc1" +dependencies = [ + "proc-macro2", + "quote", + "syn 2.0.119", +] + +[[package]] +name = "zerofrom" +version = "0.1.8" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0ec05a11813ea801ff6d75110ad09cd0824ddba17dfe17128ea0d5f68e6c5272" +dependencies = [ + "zerofrom-derive", +] + +[[package]] +name = "zerofrom-derive" +version = "0.1.7" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "11532158c46691caf0f2593ea8358fed6bbf68a0315e80aae9bd41fbade684a1" +dependencies = [ + "proc-macro2", + "quote", + "syn 2.0.119", + "synstructure", +] + +[[package]] +name = "zeroize" +version = "1.9.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e13c156562582aa81c60cb29407084cdb54c4164760106ab78e6c5b0858cf64e" + +[[package]] +name = "zerotrie" +version = "0.2.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "4ea269c3bd32f0a32c321907a2ae912ba6f4649bb0fc764a15627e99a7095a3f" +dependencies = [ + "displaydoc", + "yoke", + "zerofrom", +] + +[[package]] +name = "zerovec" +version = "0.11.8" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "bb0464e17806c1d976d5cba29399c7f08e516e279e2ba493f63123b5fca67dd8" +dependencies = [ + "yoke", + "zerofrom", + "zerovec-derive", +] + +[[package]] +name = "zerovec-derive" +version = "0.11.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "34df6fc39dbd26ddc9c10e6a2984476e13acce22e64e4487636ef494369225da" +dependencies = [ + "proc-macro2", + "quote", + "syn 3.0.6", +] + +[[package]] +name = "zlib-rs" +version = "0.6.7" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "34b31d188d9d685a4f9c7b46d6e36631b07058d2cfe190267adce54dc230bf12" + +[[package]] +name = "zmij" +version = "1.0.23" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "29666d0abbfad1e3dc4dcf6144730dd3a3ab225bbbdac83319345b1b44ccfc1b" diff --git a/examples/message-project/rust/Cargo.toml b/examples/message-project/rust/Cargo.toml new file mode 100644 index 00000000000..75868243530 --- /dev/null +++ b/examples/message-project/rust/Cargo.toml @@ -0,0 +1,15 @@ +[package] +name = "reading-processor" +version = "0.1.0" +edition = "2024" + +[workspace] + +[dependencies] +dimos-module = { path = "../sdk/rust/dimos-module" } +story-messages-messages = { path = "../build/dimos/cargo-packages/story_messages" } +tokio = { version = "1", features = ["rt-multi-thread", "macros"] } +tracing = "0.1" + +[patch.crates-io] +dimos-generated-messages = { path = "../build/dimos/cargo-packages/dimos_generated" } diff --git a/examples/message-project/rust/src/main.rs b/examples/message-project/rust/src/main.rs new file mode 100644 index 00000000000..23200f0615a --- /dev/null +++ b/examples/message-project/rust/src/main.rs @@ -0,0 +1,38 @@ +// Copyright 2026 Dimensional Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +use dimos_module::{Input, Module, Output, cdr, run_with_transport}; +use story_messages_messages::story_msgs::msg::DeviceReading; + +#[derive(Module)] +struct Processor { + #[input(decode = cdr::decode)] + processed: Input, + #[output(encode = cdr::encode)] + checked: Output, +} + +impl Processor { + async fn handle_processed(&mut self, mut message: DeviceReading) { + message.value += 1.0; + if let Err(error) = self.checked.publish(&message).await { + tracing::error!(%error, "publish failed"); + } + } +} + +#[tokio::main] +async fn main() { + run_with_transport::().await; +} diff --git a/examples/native-modules/cpp/CMakeLists.txt b/examples/native-modules/cpp/CMakeLists.txt index 6cc6c507c39..80ca62aa212 100644 --- a/examples/native-modules/cpp/CMakeLists.txt +++ b/examples/native-modules/cpp/CMakeLists.txt @@ -7,21 +7,6 @@ project(dimos_native_ping_pong CXX) set(CMAKE_CXX_STANDARD 20) set(CMAKE_CXX_STANDARD_REQUIRED ON) -include(FetchContent) - -# Generated LCM message headers (geometry_msgs::Twist, ...). Under nix, -# -DFETCHCONTENT_SOURCE_DIR_DIMOS_LCM points this at the flake input, so the -# sandboxed build needs no network. -FetchContent_Declare(dimos_lcm - GIT_REPOSITORY https://github.com/dimensionalOS/dimos-lcm.git - GIT_TAG main - GIT_SHALLOW TRUE -) -FetchContent_MakeAvailable(dimos_lcm) - -find_package(PkgConfig REQUIRED) -pkg_check_modules(LCM REQUIRED lcm) - # The header-only dimos native C++ SDK. Overridable so nix can point at the # flake input instead of the in-repo relative path. if(NOT DEFINED DIMOS_NATIVE_CPP_DIR) @@ -29,16 +14,8 @@ if(NOT DEFINED DIMOS_NATIVE_CPP_DIR) endif() add_subdirectory(${DIMOS_NATIVE_CPP_DIR} ${CMAKE_BINARY_DIR}/dimos_native) -foreach(mod ping pong) +foreach(mod ping pong cdr_relay) add_executable(${mod} src/${mod}.cpp) - target_include_directories(${mod} PRIVATE - ${dimos_lcm_SOURCE_DIR}/generated/cpp_lcm_msgs - ${LCM_INCLUDE_DIRS} - ) - target_link_libraries(${mod} PRIVATE - dimos_native - ${LCM_LIBRARIES} - ) - target_link_directories(${mod} PRIVATE ${LCM_LIBRARY_DIRS}) + target_link_libraries(${mod} PRIVATE dimos_native) install(TARGETS ${mod} RUNTIME DESTINATION bin) endforeach() diff --git a/examples/native-modules/cpp/flake.lock b/examples/native-modules/cpp/flake.lock index 0e91e638453..f2730087a0c 100644 --- a/examples/native-modules/cpp/flake.lock +++ b/examples/native-modules/cpp/flake.lock @@ -1,22 +1,5 @@ { "nodes": { - "dimos-lcm": { - "flake": false, - "locked": { - "lastModified": 1782118008, - "narHash": "sha256-z88X38ZM3vr1eneG1CkJ4GAGWBW3JYWPEX/2x7aRVJI=", - "owner": "dimensionalOS", - "repo": "dimos-lcm", - "rev": "0a1c24710ce2f7a569e1673617102cc5254a75e2", - "type": "github" - }, - "original": { - "owner": "dimensionalOS", - "ref": "main", - "repo": "dimos-lcm", - "type": "github" - } - }, "flake-utils": { "inputs": { "systems": "systems" @@ -127,7 +110,6 @@ }, "root": { "inputs": { - "dimos-lcm": "dimos-lcm", "flake-utils": "flake-utils", "lcm-extended": "lcm-extended", "nixpkgs": "nixpkgs", diff --git a/examples/native-modules/cpp/flake.nix b/examples/native-modules/cpp/flake.nix index 9fd97687e86..210bc2fda7d 100644 --- a/examples/native-modules/cpp/flake.nix +++ b/examples/native-modules/cpp/flake.nix @@ -10,11 +10,6 @@ inputs.nixpkgs.follows = "nixpkgs"; inputs.flake-utils.follows = "flake-utils"; }; - # Generated LCM message headers, consumed via a FetchContent source override. - dimos-lcm = { - url = "github:dimensionalOS/dimos-lcm/main"; - flake = false; - }; # Standalone Boost.PFR, consumed by the SDK via a FetchContent source override. pfr = { url = "github:apolukhin/pfr_non_boost/2.3.2"; @@ -22,13 +17,14 @@ }; }; - outputs = { self, nixpkgs, zenoh, flake-utils, lcm-extended, dimos-lcm, pfr, ... }: + outputs = { self, nixpkgs, zenoh, flake-utils, lcm-extended, pfr, ... }: flake-utils.lib.eachDefaultSystem (system: let pkgs = import nixpkgs { inherit system; }; lcm = lcm-extended.packages.${system}.lcm; zenohc = zenoh.packages.${system}.zenoh-c; zenohcpp = zenoh.packages.${system}.zenoh-cpp; + messages = import ../../../native/cpp/messages.nix { inherit pkgs; }; in { packages.default = pkgs.stdenv.mkDerivation { pname = "dimos-native-ping-pong"; @@ -36,11 +32,10 @@ src = ./.; nativeBuildInputs = [ pkgs.cmake pkgs.pkg-config ]; - buildInputs = [ lcm pkgs.glib pkgs.nlohmann_json zenohc zenohcpp ]; + buildInputs = [ lcm pkgs.glib pkgs.nlohmann_json zenohc zenohcpp messages ]; cmakeFlags = [ "-DCMAKE_POLICY_VERSION_MINIMUM=3.5" - "-DFETCHCONTENT_SOURCE_DIR_DIMOS_LCM=${dimos-lcm}" "-DFETCHCONTENT_SOURCE_DIR_PFR=${pfr}" # The header-only SDK lives outside this dir. A git-tree flake can # reach it as a path literal within the repo tree. diff --git a/examples/native-modules/cpp/src/cdr_relay.cpp b/examples/native-modules/cpp/src/cdr_relay.cpp new file mode 100644 index 00000000000..f5236b8fb49 --- /dev/null +++ b/examples/native-modules/cpp/src/cdr_relay.cpp @@ -0,0 +1,31 @@ +// Copyright 2026 Dimensional Inc. +// SPDX-License-Identifier: Apache-2.0 + +#include "dimos/native.hpp" +#include + +using namespace dimos::native; +using dimos_msgs::msg::LineSegments3D; +using sensor_msgs::msg::Image; + +class CdrRelay : public Module { +public: + void build(Builder& builder, Config&) override { + lines_ = builder.output("lines_out"); + image_ = builder.output("image_out"); + builder.input("lines_in", &CdrRelay::on_lines, this); + builder.input("image_in", &CdrRelay::on_image, this); + } + +private: + void on_lines(const LineSegments3D& message) { + auto reply = message; + for (auto& segment : reply.segments) segment.weight += 1; + lines_.publish(reply); + } + void on_image(const Image& message) { image_.publish(message); } + Output lines_; + Output image_; +}; + +int main() { dimos::native::run_with_transport(); } diff --git a/examples/native-modules/cpp/src/ping.cpp b/examples/native-modules/cpp/src/ping.cpp index 80bc05d9319..7647b29b056 100644 --- a/examples/native-modules/cpp/src/ping.cpp +++ b/examples/native-modules/cpp/src/ping.cpp @@ -6,14 +6,14 @@ #include #include "dimos/native.hpp" -#include "geometry_msgs/Twist.hpp" +#include using dimos::native::Builder; using dimos::native::Config; using dimos::native::Module; using dimos::native::Output; namespace logging = dimos::native::log; -using geometry_msgs::Twist; +using geometry_msgs::msg::Twist; constexpr std::chrono::milliseconds PUBLISH_PERIOD{200}; diff --git a/examples/native-modules/cpp/src/pong.cpp b/examples/native-modules/cpp/src/pong.cpp index 9b9e218f6ae..212caaf088d 100644 --- a/examples/native-modules/cpp/src/pong.cpp +++ b/examples/native-modules/cpp/src/pong.cpp @@ -5,13 +5,13 @@ #include #include "dimos/native.hpp" -#include "geometry_msgs/Twist.hpp" +#include using dimos::native::Builder; using dimos::native::Config; using dimos::native::Module; using dimos::native::Output; -using geometry_msgs::Twist; +using geometry_msgs::msg::Twist; constexpr std::int64_t SAMPLE_CONFIG_MIN = 0; constexpr std::int64_t SAMPLE_CONFIG_MAX = 1000; diff --git a/examples/native-modules/cpp_ping_pong.py b/examples/native-modules/cpp_ping_pong.py index 1564f83b0d8..73dda9908f4 100644 --- a/examples/native-modules/cpp_ping_pong.py +++ b/examples/native-modules/cpp_ping_pong.py @@ -26,11 +26,12 @@ import argparse from pathlib import Path +from dimos_generated.geometry_msgs.msg import Twist + from dimos.core.coordination.blueprints import Blueprint, autoconnect from dimos.core.coordination.module_coordinator import ModuleCoordinator from dimos.core.native_module import NativeModule, NativeModuleConfig from dimos.core.stream import In, Out -from dimos.msgs.geometry_msgs.Twist import Twist _CPP_DIR = Path(__file__).parent / "cpp" _BUILD = "nix build .#default" diff --git a/examples/native-modules/rust/Cargo.toml b/examples/native-modules/rust/Cargo.toml index d2c1d48d48e..f70865ec9c1 100644 --- a/examples/native-modules/rust/Cargo.toml +++ b/examples/native-modules/rust/Cargo.toml @@ -19,9 +19,13 @@ path = "src/tf_listener.rs" name = "tf_broadcaster" path = "src/tf_broadcaster.rs" +[[bin]] +name = "cdr_relay" +path = "src/cdr_relay.rs" + [dependencies] dimos-module = { path = "../../../native/rust/dimos-module" } -lcm-msgs = { git = "https://github.com/dimensionalOS/dimos-lcm.git", branch = "rust-codegen" } +dimos-generated-messages = { path = "../../../dimos/message_codegen" } tokio = { version = "1", features = ["rt-multi-thread", "macros", "time"] } serde = { version = "1", features = ["derive"] } tracing = "0.1" diff --git a/examples/native-modules/rust/src/cdr_relay.rs b/examples/native-modules/rust/src/cdr_relay.rs new file mode 100644 index 00000000000..37164709bfc --- /dev/null +++ b/examples/native-modules/rust/src/cdr_relay.rs @@ -0,0 +1,52 @@ +// Copyright 2026 Dimensional Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// Copyright 2026 Dimensional Inc. +// SPDX-License-Identifier: Apache-2.0 + +use dimos_generated_messages::{dimos_msgs::msg::LineSegments3D, sensor_msgs::msg::Image}; +use dimos_module::{cdr, run_with_transport, Input, Module, Output}; + +#[derive(Module)] +struct CdrRelay { + #[input(decode = cdr::decode)] + lines_in: Input, + #[output(encode = cdr::encode)] + lines_out: Output, + #[input(decode = cdr::decode)] + image_in: Input, + #[output(encode = cdr::encode)] + image_out: Output, +} + +impl CdrRelay { + async fn handle_lines_in(&mut self, mut message: LineSegments3D) { + for segment in &mut message.segments { + segment.weight += 1.0; + } + if let Err(error) = self.lines_out.publish(&message).await { + tracing::error!(%error, "failed to publish custom message"); + } + } + async fn handle_image_in(&mut self, message: Image) { + if let Err(error) = self.image_out.publish(&message).await { + tracing::error!(%error, "failed to publish image"); + } + } +} + +#[tokio::main] +async fn main() { + run_with_transport::().await; +} diff --git a/examples/native-modules/rust/src/ping.rs b/examples/native-modules/rust/src/ping.rs index 51b6aef8469..25bd35e7d57 100644 --- a/examples/native-modules/rust/src/ping.rs +++ b/examples/native-modules/rust/src/ping.rs @@ -12,17 +12,18 @@ // See the License for the specific language governing permissions and // limitations under the License. +use dimos_generated_messages::geometry_msgs::msg::{Twist, Vector3}; +use dimos_module::cdr; use dimos_module::{run_with_transport, Input, Module, Output}; -use lcm_msgs::geometry_msgs::{Twist, Vector3}; use tokio::time::{interval, Duration}; #[derive(Module)] #[module(setup = start_publisher)] struct Ping { - #[input(decode = Twist::decode)] + #[input(decode = cdr::decode)] confirm: Input, - #[output(encode = Twist::encode)] + #[output(encode = cdr::encode)] data: Output, } diff --git a/examples/native-modules/rust/src/pong.rs b/examples/native-modules/rust/src/pong.rs index 56ca448b3c8..ac92bb4cb77 100644 --- a/examples/native-modules/rust/src/pong.rs +++ b/examples/native-modules/rust/src/pong.rs @@ -12,8 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. +use dimos_generated_messages::geometry_msgs::msg::{Twist, Vector3}; +use dimos_module::cdr; use dimos_module::{native_config, run_with_transport, Input, Module, Output}; -use lcm_msgs::geometry_msgs::{Twist, Vector3}; #[native_config] struct PongConfig { @@ -23,10 +24,10 @@ struct PongConfig { #[derive(Module)] struct Pong { - #[input(decode = Twist::decode)] + #[input(decode = cdr::decode)] data: Input, - #[output(encode = Twist::encode)] + #[output(encode = cdr::encode)] confirm: Output, #[config] diff --git a/examples/native-modules/rust_ping_pong.py b/examples/native-modules/rust_ping_pong.py index 190fad40a7d..c22619bc6b6 100644 --- a/examples/native-modules/rust_ping_pong.py +++ b/examples/native-modules/rust_ping_pong.py @@ -27,12 +27,13 @@ import argparse from pathlib import Path +from dimos_generated.geometry_msgs.msg import Twist + from dimos.constants import DIMOS_PROJECT_ROOT from dimos.core.coordination.blueprints import Blueprint, autoconnect from dimos.core.coordination.module_coordinator import ModuleCoordinator from dimos.core.native_module import NativeModule, NativeModuleConfig from dimos.core.stream import In, Out -from dimos.msgs.geometry_msgs.Twist import Twist from dimos.protocol.service.zenohservice import ZenohConfig _RUST_DIR = Path(__file__).parent / "rust" diff --git a/examples/native-modules/rust_tf.py b/examples/native-modules/rust_tf.py index 473f39ab1ce..9cc578d8e4b 100644 --- a/examples/native-modules/rust_tf.py +++ b/examples/native-modules/rust_tf.py @@ -28,6 +28,10 @@ from pathlib import Path import time +from dimos_generated.geometry_msgs.msg import Quaternion, Transform, TransformStamped, Vector3 +from dimos_generated.std_msgs.msg import Header +from dimos_generated.tf2_msgs.msg import TFMessage + from dimos.constants import DIMOS_PROJECT_ROOT from dimos.core.coordination.blueprints import autoconnect from dimos.core.coordination.module_coordinator import ModuleCoordinator @@ -35,9 +39,11 @@ from dimos.core.module import Module from dimos.core.native_module import NativeModule, NativeModuleConfig from dimos.core.stream import IO -from dimos.msgs.geometry_msgs.Transform import Transform -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.msgs.tf2_msgs.TFMessage import TFMessage +from dimos.msgs.time import time_from_nanoseconds +from dimos.protocol.tf.static_tf_publisher import StaticTfPublisher, frames_to_edge_transforms +from dimos.utils.logging_config import setup_logger + +logger = setup_logger() _RUST_DIR = Path(__file__).parent / "rust" # The crate is a workspace member, so cargo builds into the repo-root target dir. @@ -46,7 +52,7 @@ class TfProducer(Module): - """Publishes a time-varying a -> b -> c transform chain onto /tf.""" + """Publishes the time-varying a -> b edge onto /tf.""" tf: IO[TFMessage] @@ -62,21 +68,30 @@ async def _publish_loop(self) -> None: start = time.time() while self._running: t = time.time() - start - now = time.time() - self.tfbuffer.publish( - Transform( - translation=Vector3(0.0, math.cos(t), math.sin(t)), - frame_id="a", - child_frame_id="b", - ts=now, - ), - Transform( - translation=Vector3(1.0, 0.0, 0.0), - frame_id="b", - child_frame_id="c", - ts=now, - ), + stamp = time_from_nanoseconds(time.time_ns()) + self.tf.publish( + TFMessage( + transforms=[ + TransformStamped( + header=Header(frame_id="a", stamp=stamp), + child_frame_id="b", + transform=Transform( + translation=Vector3(y=math.cos(t), z=math.sin(t)), + rotation=Quaternion(w=1), + ), + ), + ] + ) ) + composed = self.tfbuffer.get("a", "d", warn=False) + if composed is not None: + position = composed.transform.translation + logger.info( + "Python TF lookup", + x=position.x, + y=position.y, + z=position.z, + ) await asyncio.sleep(0.1) @rpc @@ -85,6 +100,13 @@ def stop(self) -> None: super().stop() +class StaticMount(StaticTfPublisher): + """Exercise the production periodic publisher for the fixed b -> c mount.""" + + def transforms(self) -> list[TransformStamped]: + return frames_to_edge_transforms([("c", "b", (1.0, 0.0, 0.0), (0.0, 0.0, 0.0))]) + + class TfListenerConfig(NativeModuleConfig): executable: str = str(_EXAMPLES / "tf_listener") build_command: str = _BUILD @@ -118,6 +140,9 @@ class TfBroadcasterModule(NativeModule): if __name__ == "__main__": bp = autoconnect( - TfProducer.blueprint(), TfBroadcasterModule.blueprint(), TfListenerModule.blueprint() + TfProducer.blueprint(), + StaticMount.blueprint(), + TfBroadcasterModule.blueprint(), + TfListenerModule.blueprint(), ).global_config(viewer="none") ModuleCoordinator.build(bp).loop() diff --git a/examples/native-modules/test_rust_tf.py b/examples/native-modules/test_rust_tf.py new file mode 100644 index 00000000000..9287fcc85fd --- /dev/null +++ b/examples/native-modules/test_rust_tf.py @@ -0,0 +1,99 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Exercise generated TF messages through real Python and Rust module workers.""" + +import os +from pathlib import Path +import re +import selectors +import signal +import socket +import subprocess +import sys +import time + +import pytest + +_ROOT = Path(__file__).resolve().parents[2] +_LOOKUP = re.compile(r"tf lookup .* x=(\S+) y=(\S+) z=(\S+)\n") +_PYTHON_LOOKUP = re.compile(r"Python TF lookup x=(\S+) y=(\S+) z=(\S+)\n") + + +@pytest.mark.native_e2e +@pytest.mark.parametrize("backend", ["lcm", "zenoh"]) +def test_python_and_rust_compose_generated_tf_through_coordinator(backend, tmp_path): + for executable in ("tf_broadcaster", "tf_listener"): + if not (_ROOT / "target" / "release" / executable).is_file(): + pytest.skip("Build dimos-native-module-examples with cargo build --release first") + + with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as reservation: + reservation.bind(("127.0.0.1", 0)) + port = reservation.getsockname()[1] + env = { + **os.environ, + "DIMOS_TRANSPORT": backend, + "LCM_DEFAULT_URL": f"udpm://239.255.76.67:{port}?ttl=0", + } + output = bytearray() + samples = [] + with subprocess.Popen( + [sys.executable, str(Path(__file__).with_name("rust_tf.py"))], + cwd=_ROOT, + env=env, + stdout=subprocess.PIPE, + stderr=subprocess.STDOUT, + start_new_session=True, + ) as process: + try: + assert process.stdout is not None + with selectors.DefaultSelector() as selector: + selector.register(process.stdout, selectors.EVENT_READ) + deadline = time.monotonic() + 45 + while len(samples) < 4 and time.monotonic() < deadline: + if not selector.select(timeout=max(0, deadline - time.monotonic())): + break + chunk = os.read(process.stdout.fileno(), 65536) + if not chunk: + break + output.extend(chunk) + samples = [ + tuple(map(float, match.groups())) + for match in _LOOKUP.finditer(output.decode(errors="replace")) + ] + finally: + if process.poll() is None: + process.send_signal(signal.SIGINT) + try: + remaining, _ = process.communicate(timeout=15) + except subprocess.TimeoutExpired: + os.killpg(process.pid, signal.SIGKILL) + remaining, _ = process.communicate(timeout=5) + output.extend(remaining) + (tmp_path / f"tf-{backend}.log").write_bytes(output) + + transcript = output.decode(errors="replace") + assert len(samples) >= 4, transcript + for x, y, z in samples: + assert x == pytest.approx(1.5) + assert y * y + z * z == pytest.approx(1.0) + assert max(sample[1] for sample in samples) - min(sample[1] for sample in samples) > 0.1 + python_samples = [ + tuple(map(float, match.groups())) for match in _PYTHON_LOOKUP.finditer(transcript) + ] + assert python_samples, transcript + for x, y, z in python_samples: + assert x == pytest.approx(1.5) + assert y * y + z * z == pytest.approx(1.0) + assert process.returncode == 0, transcript diff --git a/examples/simplerobot/README.md b/examples/simplerobot/README.md index 8d2f9432b35..baa9a6fda4c 100644 --- a/examples/simplerobot/README.md +++ b/examples/simplerobot/README.md @@ -34,13 +34,13 @@ From any language with LCM bindings, publish `Twist` messages to `/cmd_vel`: ```python from dimos.core.transport import LCMTransport -from dimos.msgs.geometry_msgs import Twist +from dimos_generated.geometry_msgs.msg import Twist, Vector3 transport = LCMTransport("/cmd_vel", Twist) -transport.publish(Twist(linear=(0.5, 0, 0), angular=(0, 0, 0.3))) +transport.publish(Twist(linear=Vector3(x=0.5), angular=Vector3(z=0.3))) ``` -See `examples/language-interop/` for C++, TypeScript, and Lua examples. +See `examples/language-interop/` for generated CDR examples and browser support. ## Physics diff --git a/examples/simplerobot/simplerobot.py b/examples/simplerobot/simplerobot.py index 48a41bc6376..74098974c2d 100644 --- a/examples/simplerobot/simplerobot.py +++ b/examples/simplerobot/simplerobot.py @@ -25,28 +25,27 @@ import math import time +from dimos_generated.geometry_msgs.msg import Point, Pose, PoseStamped, Twist, Vector3 +from dimos_generated.std_msgs.msg import Header import reactivex as rx from dimos.core.core import rpc from dimos.core.module import Module, ModuleConfig from dimos.core.stream import In, Out -from dimos.msgs.geometry_msgs.Pose import Pose -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Twist import Twist -from dimos.msgs.geometry_msgs.Vector3 import Vector3 +from dimos.msgs.geometry import quaternion_from_euler, yaw +from dimos.msgs.time import time_from_seconds def apply_twist(pose: Pose, twist: Twist, dt: float) -> Pose: """Apply a velocity command to a pose (unicycle model).""" - yaw = pose.yaw + twist.angular.z * dt + heading = yaw(pose.orientation) + twist.angular.z * dt return Pose( - position=( - pose.x + twist.linear.x * math.cos(yaw) * dt, - pose.y + twist.linear.x * math.sin(yaw) * dt, - pose.z, + position=Point( + x=pose.position.x + twist.linear.x * math.cos(heading) * dt, + y=pose.position.y + twist.linear.x * math.sin(heading) * dt, + z=pose.position.z, ), - orientation=Quaternion.from_euler(Vector3(0, 0, yaw)), + orientation=quaternion_from_euler(0, 0, heading), ) @@ -91,10 +90,8 @@ def _update(self) -> None: self.pose.publish( PoseStamped( - ts=now, - frame_id=self.config.frame_id, - position=self._pose.position, - orientation=self._pose.orientation, + header=Header(stamp=time_from_seconds(now), frame_id=self.config.frame_id), + pose=self._pose, ) ) @@ -123,7 +120,7 @@ def _update(self) -> None: print(" Publishing: /odom (PoseStamped)") print(" Subscribing: /cmd_vel (Twist)") print(" Run 'lcmspy' in another terminal to see LCM messages") - print(" Check /examples/language-interop for sending commands from LUA, C++, TS etc.") + print(" Check /examples/language-interop for generated CDR interoperability examples.") print(" Ctrl+C to exit") try: @@ -131,11 +128,11 @@ def _update(self) -> None: time.sleep(1) print("Forward...") for _ in range(8): - robot._on_twist(Twist(linear=(1.0, 0, 0))) + robot._on_twist(Twist(linear=Vector3(x=1.0))) time.sleep(0.25) print("Turn...") for _ in range(12): - robot._on_twist(Twist(linear=(0.5, 0, 0), angular=(0, 0, 0.5))) + robot._on_twist(Twist(linear=Vector3(x=0.5), angular=Vector3(z=0.5))) time.sleep(0.25) print("Stop") robot._on_twist(Twist()) diff --git a/examples/simplerobot/vis.py b/examples/simplerobot/vis.py index 951a6be1b9e..17631d8a2c0 100644 --- a/examples/simplerobot/vis.py +++ b/examples/simplerobot/vis.py @@ -16,6 +16,9 @@ import math import threading +from typing import Any + +from dimos.msgs.geometry import yaw def run_visualization(robot, window_size=(800, 800), meters_per_pixel=0.02): @@ -57,25 +60,25 @@ def run_visualization(robot, window_size=(800, 800), meters_per_pixel=0.02): pygame.draw.line(screen, GRID, (0, y), (w, y)) # Robot position in screen coords - rx = cx + int(pose.x / meters_per_pixel) - ry = cy - int(pose.y / meters_per_pixel) + rx = cx + int(pose.position.x / meters_per_pixel) + ry = cy - int(pose.position.y / meters_per_pixel) # Robot body pygame.draw.circle(screen, ROBOT, (rx, ry), 20) # Direction arrow - ax = rx + int(45 * math.cos(pose.yaw)) - ay = ry - int(45 * math.sin(pose.yaw)) + ax = rx + int(45 * math.cos(yaw(pose.orientation))) + ay = ry - int(45 * math.sin(yaw(pose.orientation))) pygame.draw.line(screen, ARROW, (rx, ry), (ax, ay), 3) for sign in [-1, 1]: - hx = ax - int(10 * math.cos(pose.yaw + sign * 0.5)) - hy = ay + int(10 * math.sin(pose.yaw + sign * 0.5)) + hx = ax - int(10 * math.cos(yaw(pose.orientation) + sign * 0.5)) + hy = ay + int(10 * math.sin(yaw(pose.orientation) + sign * 0.5)) pygame.draw.line(screen, ARROW, (ax, ay), (hx, hy), 3) # Info text info = [ - f"Position: ({pose.x:.2f}, {pose.y:.2f}) m", - f"Heading: {math.degrees(pose.yaw):.1f}°", + f"Position: ({pose.position.x:.2f}, {pose.position.y:.2f}) m", + f"Heading: {math.degrees(yaw(pose.orientation)):.1f}°", f"Velocity: {vel.linear.x:.2f} m/s", f"Angular: {math.degrees(vel.angular.z):.1f}°/s", ] @@ -88,7 +91,7 @@ def run_visualization(robot, window_size=(800, 800), meters_per_pixel=0.02): pygame.quit() -def start_visualization(robot, **kwargs): +def start_visualization(robot: Any, **kwargs: Any) -> threading.Thread: """Start visualization in a background thread.""" thread = threading.Thread(target=run_visualization, args=(robot,), kwargs=kwargs, daemon=True) thread.start() diff --git a/misc/DimSim/README.md b/misc/DimSim/README.md index affe85a50c6..395aacc75f4 100644 --- a/misc/DimSim/README.md +++ b/misc/DimSim/README.md @@ -52,3 +52,25 @@ dimsim eval --headless --scene apartment # full headless run (CI) npm install # browser deps (three, rapier, vite) npm run build # → dist/ ``` + +## Message encoding + +The browser and Deno bridge use ROS 2 CDR payloads with full `package/msg/Type` +channel suffixes. LCM/UDP and the WebSocket channel envelope remain the transport. +Color frames contain raw RGBA pixels in `sensor_msgs/msg/Image`; depth uses +`16UC1`. JPEG is used only for the eval/sidebar preview. Point clouds, poses and +velocity commands use the same generated schema contract as Python, C++ and Rust. +Old LCM-encoded clients and recordings are incompatible with this proposal. + +The standalone schema bundle is exported from the canonical generated messages: + +```bash +python -m dimos.simulation.dimsim.cdr_schemas +pytest dimos/simulation/dimsim/test_cdr_schemas.py +cd misc/DimSim/cli +deno test bridge/cdr_test.ts +deno check cli.ts +``` + +Foxglove `rosmsg` 5.0.5 and `rosmsg2-serialization` 3.1.2 are pinned in both +package manifests. Codec tests do not open network sockets or start simulation. diff --git a/misc/DimSim/cli/bridge/cdr.ts b/misc/DimSim/cli/bridge/cdr.ts new file mode 100644 index 00000000000..332052ec1a5 --- /dev/null +++ b/misc/DimSim/cli/bridge/cdr.ts @@ -0,0 +1,40 @@ +/** Schema-driven ROS 2 CDR codecs shared by the browser and Deno bridge. */ +import { parse } from "@foxglove/rosmsg"; +import { MessageReader, MessageWriter } from "@foxglove/rosmsg2-serialization"; +import { schemas } from "./cdr_schemas.ts"; + +const writers = new Map(); +const readers = new Map(); +export function encodeCdr(type: string, value: Record): Uint8Array { + let writer = writers.get(type); + if (!writer) { + if (!schemas[type]) throw new Error(`Unknown CDR type: ${type}`); + writer = new MessageWriter(parse(schemas[type], { ros2: true })); + writers.set(type, writer); + } + return writer.writeMessage(value); +} +export function decodeCdr(type: string, data: Uint8Array): any { + let reader = readers.get(type); + if (!reader) { + if (!schemas[type]) throw new Error(`Unknown CDR type: ${type}`); + reader = new MessageReader(parse(schemas[type], { ros2: true })); + readers.set(type, reader); + } + return reader.readMessage(data); +} +export function headerNow(frameId: string) { + const now = Date.now(); + return { stamp: { sec: Math.floor(now / 1000), nanosec: (now % 1000) * 1_000_000 }, frame_id: frameId }; +} +/** WebSocket envelope uses the existing LC02 channel framing, without UDP's size limit. */ +export function sensorPacket(channel: string, value: Record): Uint8Array { + const type = channel.split("#")[1]; + const data = encodeCdr(type, value); + const name = new TextEncoder().encode(channel); + const packet = new Uint8Array(8 + name.length + 1 + data.length); + new DataView(packet.buffer).setUint32(0, 0x4c433032, false); + packet.set(name, 8); + packet.set(data, 9 + name.length); + return packet; +} diff --git a/misc/DimSim/cli/bridge/cdr_schemas.ts b/misc/DimSim/cli/bridge/cdr_schemas.ts new file mode 100644 index 00000000000..2fc4ee65378 --- /dev/null +++ b/misc/DimSim/cli/bridge/cdr_schemas.ts @@ -0,0 +1,7 @@ +// Generated from the canonical .msg definitions; do not edit by hand. +export const schemas: Record = { + "sensor_msgs/msg/Image": "# This message contains an uncompressed image\n# (0, 0) is at top-left corner of image\n\nstd_msgs/Header header # Header timestamp should be acquisition time of image\n # Header frame_id should be optical frame of camera\n # origin of frame should be optical center of cameara\n # +x should point to the right in the image\n # +y should point down in the image\n # +z should point into to plane of the image\n # If the frame_id here and the frame_id of the CameraInfo\n # message associated with the image conflict\n # the behavior is undefined\n\nuint32 height # image height, that is, number of rows\nuint32 width # image width, that is, number of columns\n\n# The legal values for encoding are in file include/sensor_msgs/image_encodings.hpp\n# If you want to standardize a new string format, join\n# ros-users@lists.ros.org and send an email proposing a new encoding.\n\nstring encoding # Encoding of pixels -- channel meaning, ordering, size\n # taken from the list of strings in include/sensor_msgs/image_encodings.hpp\n\nuint8 is_bigendian # is this data bigendian?\nuint32 step # Full row length in bytes\nuint8[] data # actual matrix data, size is (step * rows)\n================================================================================\nMSG: builtin_interfaces/Time\n# This message communicates ROS Time defined here:\n# https://design.ros2.org/articles/clock_and_time.html\n\n# The seconds component, valid over all int32 values.\nint32 sec\n\n# The nanoseconds component, valid in the range [0, 1e9), to be added to the seconds component. \n# e.g.\n# The time -1.7 seconds is represented as {sec: -2, nanosec: 3e8}\n# The time 1.7 seconds is represented as {sec: 1, nanosec: 7e8}\nuint32 nanosec\n================================================================================\nMSG: std_msgs/Header\n# Standard metadata for higher-level stamped data types.\n# This is generally used to communicate timestamped data\n# in a particular coordinate frame.\n\n# Two-integer timestamp that is expressed as seconds and nanoseconds.\nbuiltin_interfaces/Time stamp\n\n# Transform frame with which this data is associated.\nstring frame_id\n", + "sensor_msgs/msg/PointCloud2": "# This message holds a collection of N-dimensional points, which may\n# contain additional information such as normals, intensity, etc. The\n# point data is stored as a binary blob, its layout described by the\n# contents of the \"fields\" array.\n#\n# The point cloud data may be organized 2d (image-like) or 1d (unordered).\n# Point clouds organized as 2d images may be produced by camera depth sensors\n# such as stereo or time-of-flight.\n\n# Time of sensor data acquisition, and the coordinate frame ID (for 3d points).\nstd_msgs/Header header\n\n# 2D structure of the point cloud. If the cloud is unordered, height is\n# 1 and width is the length of the point cloud.\nuint32 height\nuint32 width\n\n# Describes the channels and their layout in the binary data blob.\nPointField[] fields\n\nbool is_bigendian # Is this data bigendian?\nuint32 point_step # Length of a point in bytes\nuint32 row_step # Length of a row in bytes\nuint8[] data # Actual point data, size is (row_step*height)\n\nbool is_dense # True if there are no invalid points\n================================================================================\nMSG: builtin_interfaces/Time\n# This message communicates ROS Time defined here:\n# https://design.ros2.org/articles/clock_and_time.html\n\n# The seconds component, valid over all int32 values.\nint32 sec\n\n# The nanoseconds component, valid in the range [0, 1e9), to be added to the seconds component. \n# e.g.\n# The time -1.7 seconds is represented as {sec: -2, nanosec: 3e8}\n# The time 1.7 seconds is represented as {sec: 1, nanosec: 7e8}\nuint32 nanosec\n================================================================================\nMSG: sensor_msgs/PointField\n# This message holds the description of one point entry in the\n# PointCloud2 message format.\nuint8 INT8 = 1\nuint8 UINT8 = 2\nuint8 INT16 = 3\nuint8 UINT16 = 4\nuint8 INT32 = 5\nuint8 UINT32 = 6\nuint8 FLOAT32 = 7\nuint8 FLOAT64 = 8\n\n# Common PointField names are x, y, z, intensity, rgb, rgba\nstring name # Name of field\nuint32 offset # Offset from start of point struct\nuint8 datatype # Datatype enumeration, see above\nuint32 count # How many elements in the field\n================================================================================\nMSG: std_msgs/Header\n# Standard metadata for higher-level stamped data types.\n# This is generally used to communicate timestamped data\n# in a particular coordinate frame.\n\n# Two-integer timestamp that is expressed as seconds and nanoseconds.\nbuiltin_interfaces/Time stamp\n\n# Transform frame with which this data is associated.\nstring frame_id\n", + "geometry_msgs/msg/PoseStamped": "# A Pose with reference coordinate frame and timestamp\n\nstd_msgs/Header header\nPose pose\n================================================================================\nMSG: builtin_interfaces/Time\n# This message communicates ROS Time defined here:\n# https://design.ros2.org/articles/clock_and_time.html\n\n# The seconds component, valid over all int32 values.\nint32 sec\n\n# The nanoseconds component, valid in the range [0, 1e9), to be added to the seconds component. \n# e.g.\n# The time -1.7 seconds is represented as {sec: -2, nanosec: 3e8}\n# The time 1.7 seconds is represented as {sec: 1, nanosec: 7e8}\nuint32 nanosec\n================================================================================\nMSG: geometry_msgs/Point\n# This contains the position of a point in free space\nfloat64 x\nfloat64 y\nfloat64 z\n================================================================================\nMSG: geometry_msgs/Pose\n# A representation of pose in free space, composed of position and orientation.\n\nPoint position\nQuaternion orientation\n================================================================================\nMSG: geometry_msgs/Quaternion\n# This represents an orientation in free space in quaternion form.\n\nfloat64 x 0\nfloat64 y 0\nfloat64 z 0\nfloat64 w 1\n================================================================================\nMSG: std_msgs/Header\n# Standard metadata for higher-level stamped data types.\n# This is generally used to communicate timestamped data\n# in a particular coordinate frame.\n\n# Two-integer timestamp that is expressed as seconds and nanoseconds.\nbuiltin_interfaces/Time stamp\n\n# Transform frame with which this data is associated.\nstring frame_id\n", + "geometry_msgs/msg/Twist": "# This expresses velocity in free space broken into its linear and angular parts.\n\nVector3 linear\nVector3 angular\n================================================================================\nMSG: geometry_msgs/Vector3\n# This represents a vector in free space.\n\n# This is semantically different than a point.\n# A vector is always anchored at the origin.\n# When a transform is applied to a vector, only the rotational component is applied.\n\nfloat64 x\nfloat64 y\nfloat64 z\n" +}; diff --git a/misc/DimSim/cli/bridge/cdr_test.ts b/misc/DimSim/cli/bridge/cdr_test.ts new file mode 100644 index 00000000000..dbc3e6c4195 --- /dev/null +++ b/misc/DimSim/cli/bridge/cdr_test.ts @@ -0,0 +1,30 @@ +import { strict as assert } from "node:assert"; +import { encodeCdr, decodeCdr, sensorPacket } from "./cdr.ts"; +import { decodePacket } from "../vendor/lcm/transport.ts"; + +const header = { stamp: { sec: 42, nanosec: 123456789 }, frame_id: "camera_optical" }; +Deno.test("CDR image preserves raw pixels, source stamp and large WS envelope", () => { + const pixels = new Uint8Array(640 * 288 * 4).fill(127); + pixels[0] = 255; + const packet = sensorPacket("/color_image#sensor_msgs/msg/Image", { + header, height: 288, width: 640, encoding: "rgba8", is_bigendian: 0, + step: 640 * 4, data: pixels, + }); + const framed = decodePacket(packet); + assert(framed?.type === "small"); + assert.equal(framed.channel, "/color_image#sensor_msgs/msg/Image"); + assert.deepEqual(Array.from(framed.data.slice(0, 4)), [0, 1, 0, 0]); + const image = decodeCdr("sensor_msgs/msg/Image", framed.data); + assert.deepEqual(image.header, header); + assert.equal(image.encoding, "rgba8"); + assert.equal(image.step, 2560); + assert.deepEqual(image.data, pixels); +}); +Deno.test("CDR command decoder preserves all six velocity components", () => { + const command = { linear: { x: 1.5, y: -2, z: 3 }, angular: { x: 4, y: 5, z: -0.25 } }; + assert.deepEqual(decodeCdr("geometry_msgs/msg/Twist", encodeCdr("geometry_msgs/msg/Twist", command)), command); +}); +Deno.test("CDR codec rejects missing schemas and truncated payloads", () => { + assert.throws(() => encodeCdr("unknown/msg/Type", {}), /Unknown CDR type/); + assert.throws(() => decodeCdr("geometry_msgs/msg/Twist", new Uint8Array([0, 1, 0, 0]))); +}); diff --git a/misc/DimSim/cli/bridge/lidar.ts b/misc/DimSim/cli/bridge/lidar.ts index 334940d2027..b1e4a8ba1be 100644 --- a/misc/DimSim/cli/bridge/lidar.ts +++ b/misc/DimSim/cli/bridge/lidar.ts @@ -2,14 +2,11 @@ * Server-side LiDAR raycasting using Rapier physics world snapshot. * * Runs 20K Fibonacci-sphere raycasts at 5 Hz on the Deno bridge server, - * encodes PointCloud2 via @dimos/msgs, and publishes directly to LCM — + * encodes PointCloud2 as ROS 2 CDR, and publishes directly to LCM — * no WebSocket hop needed. */ -import { - sensor_msgs, - std_msgs, -} from "@dimos/msgs"; +import { encodeCdr, headerNow } from "./cdr.ts"; import type { LCM } from "../vendor/lcm/lcm.ts"; // -- Lidar constants (must match engine.js) ----------------------------------- @@ -25,7 +22,7 @@ const V_MAX_RAD = (15 * Math.PI) / 180; // drops frames and effective rate sits below the target on slow/CPU-render boxes. const RATE_MS = 200; // 5 Hz default -const CH_LIDAR = "/lidar#sensor_msgs.PointCloud2"; +const CH_LIDAR = "/lidar#sensor_msgs/msg/PointCloud2"; // Agent capsule geometry → lidar mount offset (must match engine.js) const DEFAULT_HALF_HEIGHT = 0.25; @@ -277,35 +274,29 @@ export class ServerLidar { view.setFloat32(off + 12, intensity[i], true); } - const now = Date.now(); - const header = new std_msgs.Header({ - stamp: new std_msgs.Time({ sec: Math.floor(now / 1000), nsec: (now % 1000) * 1_000_000 }), - frame_id: "world", - }); + const header = headerNow("world"); - const msg = new sensor_msgs.PointCloud2({ + const msg = { header, height: 1, width: n, - fields_length: 4, fields: [ - new sensor_msgs.PointField({ name: "x", offset: 0, datatype: 7, count: 1 }), - new sensor_msgs.PointField({ name: "y", offset: 4, datatype: 7, count: 1 }), - new sensor_msgs.PointField({ name: "z", offset: 8, datatype: 7, count: 1 }), - new sensor_msgs.PointField({ name: "intensity", offset: 12, datatype: 7, count: 1 }), + { name: "x", offset: 0, datatype: 7, count: 1 }, + { name: "y", offset: 4, datatype: 7, count: 1 }, + { name: "z", offset: 8, datatype: 7, count: 1 }, + { name: "intensity", offset: 12, datatype: 7, count: 1 }, ], is_bigendian: false, point_step: pointStep, row_step: n * pointStep, - data_length: n * pointStep, data: new Uint8Array(buf), is_dense: true, - }); + }; // Mark seq for echo filtering (prevent server re-forwarding to browser WS) this.sentSeqs.add(this.lcm.getNextSeq()); // Publish directly to LCM — no WS hop (await so buffer pressure is felt) - await this.lcm.publish(CH_LIDAR, msg); + await this.lcm.publishRaw(CH_LIDAR, encodeCdr("sensor_msgs/msg/PointCloud2", msg)); if (profile) { const total = performance.now() - scanStart; diff --git a/misc/DimSim/cli/bridge/physics.ts b/misc/DimSim/cli/bridge/physics.ts index ff875522406..b29ed77bdaa 100644 --- a/misc/DimSim/cli/bridge/physics.ts +++ b/misc/DimSim/cli/bridge/physics.ts @@ -12,7 +12,7 @@ * position updates and moves the visual avatar. */ -import { geometry_msgs, std_msgs } from "@dimos/msgs"; +import { decodeCdr, encodeCdr, headerNow } from "./cdr.ts"; import type { LCM } from "../vendor/lcm/lcm.ts"; @@ -117,8 +117,8 @@ function resolveMotionModel(embodiment?: EmbodimentConfig): string { return "holonomic"; } -const CH_ODOM = "/odom#geometry_msgs.PoseStamped"; -const CH_CMD_VEL = "/cmd_vel#geometry_msgs.Twist"; +const CH_ODOM = "/odom#geometry_msgs/msg/PoseStamped"; +const CH_CMD_VEL = "/cmd_vel#geometry_msgs/msg/Twist"; const CMD_VEL_TIMEOUT_MS = 500; // -- ServerPhysics ------------------------------------------------------------ @@ -333,7 +333,7 @@ export class ServerPhysics { /** Subscribe to cmd_vel on LCM. */ subscribeCmdVel(): void { - this.lcm.subscribe(CH_CMD_VEL, geometry_msgs.Twist, (msg: any) => { + this.lcm.subscribe(CH_CMD_VEL, { _NAME: "geometry_msgs/msg/Twist", decode: (data: Uint8Array) => decodeCdr("geometry_msgs/msg/Twist", data) }, (msg: any) => { this.handleCmdVel(msg.data); }); // quiet @@ -476,32 +476,18 @@ export class ServerPhysics { const qw = Math.cos(this.yaw / 2); const qRosZ = Math.sin(this.yaw / 2); // rotation about ROS Z - const now = Date.now(); - - const header = new std_msgs.Header({ - seq: this.seq++, - stamp: new std_msgs.Time({ sec: Math.floor(now / 1000), nsec: (now % 1000) * 1_000_000 }), - frame_id: "world", - }); - - const pose = new geometry_msgs.Pose(); - pose.position = new geometry_msgs.Point(); - pose.position.x = rosX; - pose.position.y = rosY; - pose.position.z = rosZ; - pose.orientation = new geometry_msgs.Quaternion(); - pose.orientation.x = 0; - pose.orientation.y = 0; - pose.orientation.z = qRosZ; - pose.orientation.w = qw; - - const odom = new geometry_msgs.PoseStamped(); - odom.header = header; - odom.pose = pose; + this.seq++; + const odom = { + header: headerNow("world"), + pose: { + position: { x: rosX, y: rosY, z: rosZ }, + orientation: { x: 0, y: 0, z: qRosZ, w: qw }, + }, + }; try { this.sentSeqs.add(this.lcm.getNextSeq()); - this.lcm.publishRaw(CH_ODOM, odom.encode()).catch(() => {}); + this.lcm.publishRaw(CH_ODOM, encodeCdr("geometry_msgs/msg/PoseStamped", odom)).catch(() => {}); } catch (e: unknown) { if (this.seq <= 3) console.warn("[physics] odom publish error:", e); } diff --git a/misc/DimSim/cli/bridge/server.ts b/misc/DimSim/cli/bridge/server.ts index 851c415e632..a15fc8de7a5 100644 --- a/misc/DimSim/cli/bridge/server.ts +++ b/misc/DimSim/cli/bridge/server.ts @@ -429,7 +429,7 @@ export async function startBridgeServer(options: BridgeServerOptions) { try { const decoded = decodePacket(packet); if (decoded && decoded.type === "small") { - if (chState.serverPhysics && decoded.channel === "/odom#geometry_msgs.PoseStamped") { + if (chState.serverPhysics && decoded.channel === "/odom#geometry_msgs/msg/PoseStamped") { return; } diff --git a/misc/DimSim/cli/deno.json b/misc/DimSim/cli/deno.json index d3e9506cea6..2f4190d471b 100644 --- a/misc/DimSim/cli/deno.json +++ b/misc/DimSim/cli/deno.json @@ -11,10 +11,11 @@ "eval": "deno run --allow-all cli.ts eval" }, "imports": { - "@dimos/msgs": "jsr:@dimos/msgs@^0.1.4", "@std/path": "jsr:@std/path@^1", "@std/http": "jsr:@std/http@^1", "playwright": "npm:playwright@^1.58", - "@dimforge/rapier3d-compat": "npm:@dimforge/rapier3d-compat@^0.14.0" + "@dimforge/rapier3d-compat": "npm:@dimforge/rapier3d-compat@^0.14.0", + "@foxglove/rosmsg": "npm:@foxglove/rosmsg@5.0.5", + "@foxglove/rosmsg2-serialization": "npm:@foxglove/rosmsg2-serialization@3.1.2" } } diff --git a/misc/DimSim/cli/deno.lock b/misc/DimSim/cli/deno.lock index 7cbc0c61ac8..7edf23bb875 100644 --- a/misc/DimSim/cli/deno.lock +++ b/misc/DimSim/cli/deno.lock @@ -1,8 +1,6 @@ { "version": "5", "specifiers": { - "jsr:@antim/dimsim@*": "0.1.27", - "jsr:@dimos/msgs@~0.1.4": "0.1.4", "jsr:@std/cli@^1.0.27": "1.0.27", "jsr:@std/encoding@^1.0.10": "1.0.10", "jsr:@std/fmt@^1.0.9": "1.0.9", @@ -17,22 +15,11 @@ "jsr:@std/streams@^1.0.17": "1.0.17", "npm:@dimforge/rapier3d-compat@0.14": "0.14.0", "npm:playwright@*": "1.58.2", - "npm:playwright@^1.58.0": "1.58.2" + "npm:playwright@^1.58.0": "1.58.2", + "npm:@foxglove/rosmsg2-serialization@3.1.2": "3.1.2", + "npm:@foxglove/rosmsg@5.0.5": "5.0.5" }, "jsr": { - "@antim/dimsim@0.1.27": { - "integrity": "fc7f8f3aaeb0e06b06aaf35e157308f656be344db434a10bb573343c43a4bbbb", - "dependencies": [ - "jsr:@dimos/msgs", - "jsr:@std/http", - "jsr:@std/path@1", - "npm:@dimforge/rapier3d-compat", - "npm:playwright@^1.58.0" - ] - }, - "@dimos/msgs@0.1.4": { - "integrity": "564bc30b4bc41a562c296c257a15055283ca0cbd66d0627991ede5295832d0c4" - }, "@std/cli@1.0.27": { "integrity": "eba97edd0891871a7410e835dd94b3c260c709cca5983df2689c25a71fbe04de" }, @@ -87,7 +74,9 @@ }, "fsevents@2.3.2": { "integrity": "sha512-xiqMQR4xAeHTuB9uWm+fFRcIOgKBMiOBP+eXiyT7jsgVCq1bkVygt00oASowB7EdtpOHaaPgKt812P9ab+DDKA==", - "os": ["darwin"], + "os": [ + "darwin" + ], "scripts": true }, "playwright-core@1.58.2": { @@ -103,14 +92,42 @@ "fsevents" ], "bin": true + }, + "@foxglove/cdr@3.5.1": { + "integrity": "sha512-Fubm04/xC1SkeNaviwUhbFk8nAiiNvIqlmP5MW7r6IVGFa35rsA0E3/Rg/pGdE77ZXf4XNwShZcQoEmje62ddQ==" + }, + "@foxglove/message-definition@0.5.0": { + "integrity": "sha512-B/2GvWn4wRSsPDywcmy6uNXgAfZRhiOAnmy3qAaS6MHVqiHQqs/RoDsoW6Gw9sXSweJpeH5qAqAihvm1pR9Thg==" + }, + "@foxglove/rosmsg2-serialization@3.1.2": { + "integrity": "sha512-KKtmHuuIQy6I0JLUi01LFhV7hrZb5xd+LegtuGz2+9L9pftBxtVVKhoxO6B/kQREGocIUOLP8494kxLF58LMWw==", + "dependencies": [ + "@foxglove/cdr", + "@foxglove/message-definition", + "@foxglove/rostime" + ] + }, + "@foxglove/rosmsg@5.0.5": { + "integrity": "sha512-+qY6xW+4HdbPI3ItAlFFMjXgAiplXyRsv+kqdBk8OcWGH38BmEd7s6v/VnCkbYovAoB5rfR5giuKfmPRNzwbrA==", + "dependencies": [ + "@foxglove/message-definition", + "md5-typescript" + ] + }, + "@foxglove/rostime@1.1.3": { + "integrity": "sha512-Gr3Xy4sNbo2NcXg0tBHb/jVUqIcDyZ1kgHj9dB7K+Y1A7AQ0oSCmSXIuf8oqeteSIs0pl8BJOWEwAtLNC99Izg==" + }, + "md5-typescript@1.0.5": { + "integrity": "sha512-ovAc4EtiNt2dY8JPhPr/wkC9h4U5k/nuClNVcG0Ga3V1rMlYpAY24ZaaymFXJlz+ccJ6UMPo3FSaVKe7czBsXw==" } }, "workspace": { "dependencies": [ - "jsr:@dimos/msgs@~0.1.4", "jsr:@std/http@1", "jsr:@std/path@1", "npm:@dimforge/rapier3d-compat@0.14", + "npm:@foxglove/rosmsg2-serialization@3.1.2", + "npm:@foxglove/rosmsg@5.0.5", "npm:playwright@^1.58.0" ] } diff --git a/misc/DimSim/cli/vendor/lcm/types.ts b/misc/DimSim/cli/vendor/lcm/types.ts index 2f4e1a14b32..5fd4702c2dd 100644 --- a/misc/DimSim/cli/vendor/lcm/types.ts +++ b/misc/DimSim/cli/vendor/lcm/types.ts @@ -7,12 +7,10 @@ export interface LCMMessage { timestamp: number; } -/** Interface for LCM message classes (generated types) */ +/** Decoder contract for typed payloads carried over raw LCM */ export interface MessageClass { - readonly _HASH: bigint; readonly _NAME: string; decode(data: Uint8Array): T; - new (init?: Partial): T & { encode(): Uint8Array }; } /** Subscription handler function */ diff --git a/misc/DimSim/package-lock.json b/misc/DimSim/package-lock.json index f9164eca1dd..e26eae673d3 100644 --- a/misc/DimSim/package-lock.json +++ b/misc/DimSim/package-lock.json @@ -9,6 +9,8 @@ "version": "1.0.0", "dependencies": { "@dimforge/rapier3d-compat": "^0.14.0", + "@foxglove/rosmsg": "5.0.5", + "@foxglove/rosmsg2-serialization": "3.1.2", "three": "^0.168.0" }, "devDependencies": { @@ -412,6 +414,57 @@ "node": ">=12" } }, + "node_modules/@foxglove/cdr": { + "version": "3.5.1", + "resolved": "https://registry.npmjs.org/@foxglove/cdr/-/cdr-3.5.1.tgz", + "integrity": "sha512-Fubm04/xC1SkeNaviwUhbFk8nAiiNvIqlmP5MW7r6IVGFa35rsA0E3/Rg/pGdE77ZXf4XNwShZcQoEmje62ddQ==", + "license": "MIT", + "engines": { + "node": ">= 14" + } + }, + "node_modules/@foxglove/message-definition": { + "version": "0.5.0", + "resolved": "https://registry.npmjs.org/@foxglove/message-definition/-/message-definition-0.5.0.tgz", + "integrity": "sha512-B/2GvWn4wRSsPDywcmy6uNXgAfZRhiOAnmy3qAaS6MHVqiHQqs/RoDsoW6Gw9sXSweJpeH5qAqAihvm1pR9Thg==", + "license": "MIT" + }, + "node_modules/@foxglove/rosmsg": { + "version": "5.0.5", + "resolved": "https://registry.npmjs.org/@foxglove/rosmsg/-/rosmsg-5.0.5.tgz", + "integrity": "sha512-+qY6xW+4HdbPI3ItAlFFMjXgAiplXyRsv+kqdBk8OcWGH38BmEd7s6v/VnCkbYovAoB5rfR5giuKfmPRNzwbrA==", + "license": "MIT", + "dependencies": { + "@foxglove/message-definition": "0.5.0", + "md5-typescript": "^1.0.5" + }, + "engines": { + "node": ">= 14" + } + }, + "node_modules/@foxglove/rosmsg2-serialization": { + "version": "3.1.2", + "resolved": "https://registry.npmjs.org/@foxglove/rosmsg2-serialization/-/rosmsg2-serialization-3.1.2.tgz", + "integrity": "sha512-KKtmHuuIQy6I0JLUi01LFhV7hrZb5xd+LegtuGz2+9L9pftBxtVVKhoxO6B/kQREGocIUOLP8494kxLF58LMWw==", + "license": "MIT", + "dependencies": { + "@foxglove/cdr": "^3.5.1", + "@foxglove/message-definition": "0.5.0", + "@foxglove/rostime": "^1.1.3" + }, + "engines": { + "node": ">= 20" + } + }, + "node_modules/@foxglove/rostime": { + "version": "1.1.3", + "resolved": "https://registry.npmjs.org/@foxglove/rostime/-/rostime-1.1.3.tgz", + "integrity": "sha512-Gr3Xy4sNbo2NcXg0tBHb/jVUqIcDyZ1kgHj9dB7K+Y1A7AQ0oSCmSXIuf8oqeteSIs0pl8BJOWEwAtLNC99Izg==", + "license": "MIT", + "engines": { + "node": ">= 14" + } + }, "node_modules/@rollup/rollup-android-arm-eabi": { "version": "4.60.4", "resolved": "https://registry.npmjs.org/@rollup/rollup-android-arm-eabi/-/rollup-android-arm-eabi-4.60.4.tgz", @@ -823,6 +876,12 @@ "node": "^8.16.0 || ^10.6.0 || >=11.0.0" } }, + "node_modules/md5-typescript": { + "version": "1.0.5", + "resolved": "https://registry.npmjs.org/md5-typescript/-/md5-typescript-1.0.5.tgz", + "integrity": "sha512-ovAc4EtiNt2dY8JPhPr/wkC9h4U5k/nuClNVcG0Ga3V1rMlYpAY24ZaaymFXJlz+ccJ6UMPo3FSaVKe7czBsXw==", + "license": "ISC" + }, "node_modules/nanoid": { "version": "3.3.12", "resolved": "https://registry.npmjs.org/nanoid/-/nanoid-3.3.12.tgz", diff --git a/misc/DimSim/package.json b/misc/DimSim/package.json index 49c1d7aa615..187450152bc 100644 --- a/misc/DimSim/package.json +++ b/misc/DimSim/package.json @@ -11,7 +11,9 @@ }, "dependencies": { "@dimforge/rapier3d-compat": "^0.14.0", - "three": "^0.168.0" + "three": "^0.168.0", + "@foxglove/rosmsg": "5.0.5", + "@foxglove/rosmsg2-serialization": "3.1.2" }, "devDependencies": { "vite": "^5.4.10" diff --git a/misc/DimSim/src/bridge.ts b/misc/DimSim/src/bridge.ts index eaa2bdf36e4..a610f0cec8c 100644 --- a/misc/DimSim/src/bridge.ts +++ b/misc/DimSim/src/bridge.ts @@ -8,20 +8,15 @@ * wsRgb → /color_image * wsDepth → /depth_image * - * Sensor messages are LCM-encoded binary packets using @dimos/msgs, sent over + * Sensor messages are CDR payloads in LCM channel envelopes, sent over * WebSocket to the bridge server which relays them to dimos via LCM/UDP. */ -// @ts-ignore — CDN import (runs in browser, no Deno/Node type resolution) -import { - encodePacket, - sensor_msgs, - std_msgs, -} from "https://esm.sh/jsr/@dimos/msgs@0.1.4"; +import { headerNow, sensorPacket } from "../cli/bridge/cdr.ts"; // -- Channels ---------------------------------------------------------------- -const CH_IMAGE = "/color_image#sensor_msgs.Image"; -const CH_DEPTH = "/depth_image#sensor_msgs.Image"; +const CH_IMAGE = "/color_image#sensor_msgs/msg/Image"; +const CH_DEPTH = "/depth_image#sensor_msgs/msg/Image"; // -- Default publish rates (ms) ---------------------------------------------- const DEFAULT_RATES: PublishRates = { images: 200 }; // 5 Hz images @@ -214,11 +209,7 @@ export class DimosBridge { } _makeHeader(frameId: string): any { - const now = Date.now(); - return new std_msgs.Header({ - stamp: new std_msgs.Time({ sec: Math.floor(now / 1000), nsec: (now % 1000) * 1_000_000 }), - frame_id: frameId, - }); + return headerNow(frameId); } _publishImages(): void { @@ -236,7 +227,7 @@ export class DimosBridge { /** Send on a sensor WebSocket (images — large data). */ _sendSensor(ws: WebSocket | null, channel: string, msg: any): void { if (!ws || ws.readyState !== WebSocket.OPEN) return; - ws.send(encodePacket(channel, msg)); + ws.send(sensorPacket(channel, msg)); } // -- RGB -------------------------------------------------------------------- @@ -247,16 +238,15 @@ export class DimosBridge { const frame = this.sensors.captureRgb(); if (!frame) return; - this._sendSensor(this.wsRgb, CH_IMAGE, new sensor_msgs.Image({ + this._sendSensor(this.wsRgb, CH_IMAGE, { header, height: frame.height, width: frame.width, - encoding: "jpeg", + encoding: "rgba8", is_bigendian: 0, - step: 0, // not applicable for compressed format - data_length: frame.data.length, + step: frame.width * 4, data: frame.data, - })); + }); } catch (e) { console.warn("[DimosBridge] RGB publish error:", e); } @@ -285,16 +275,15 @@ export class DimosBridge { } const depthBytes = new Uint8Array(u16.buffer, u16.byteOffset, u16.byteLength); - this._sendSensor(this.wsDepth, CH_DEPTH, new sensor_msgs.Image({ + this._sendSensor(this.wsDepth, CH_DEPTH, { header, height: frame.height, width: frame.width, encoding: "16UC1", is_bigendian: 0, step: frame.width * 2, - data_length: depthBytes.length, data: depthBytes, - })); + }); } catch (e) { console.warn("[DimosBridge] depth publish error:", e); } diff --git a/misc/DimSim/src/engine.js b/misc/DimSim/src/engine.js index 6e02f8f78cc..014a1174d1d 100644 --- a/misc/DimSim/src/engine.js +++ b/misc/DimSim/src/engine.js @@ -5774,16 +5774,12 @@ if (dimosMode) { captureRgb: () => { const frame = _dimosCaptureRgb(); if (!frame) return null; - // Render to canvas → JPEG (used for both LCM publish and eval/sidebar) + // Keep JPEG for eval/sidebar; publish the original RGBA pixels as CDR Image. _dimosCapCtx.putImageData(new ImageData(new Uint8ClampedArray(frame.data.buffer, frame.data.byteOffset, frame.data.byteLength), frame.width, frame.height), 0, 0); const dataUrl = _dimosCapCvs.toDataURL("image/jpeg", 0.75); _lastRgbBase64 = dataUrl.split("base64,")[1] || null; if (!_lastRgbBase64) return null; - // Decode base64 → Uint8Array for JPEG LCM transport - const bin = atob(_lastRgbBase64); - const jpegBytes = new Uint8Array(bin.length); - for (let i = 0; i < bin.length; i++) jpegBytes[i] = bin.charCodeAt(i); - return { data: jpegBytes, width: frame.width, height: frame.height }; + return { data: frame.data, width: frame.width, height: frame.height }; }, captureDepth: () => _dimosCaptureDepth(), }, diff --git a/misc/optimize_patrol/optimize_candidates_child.py b/misc/optimize_patrol/optimize_candidates_child.py index f62e5d2d22c..bed3899e22e 100644 --- a/misc/optimize_patrol/optimize_candidates_child.py +++ b/misc/optimize_patrol/optimize_candidates_child.py @@ -23,13 +23,17 @@ import math import time +from dimos_generated.geometry_msgs.msg import Point +from dimos_generated.nav_msgs.msg import OccupancyGrid +from dimos_generated.std_msgs.msg import Header import numpy as np from dimos.mapping.occupancy.gradient import gradient from dimos.mapping.occupancy.path_resampling import smooth_resample_path from dimos.mapping.pointclouds.occupancy import height_cost_occupancy from dimos.mapping.pointclouds.util import read_pointcloud -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 +from dimos.msgs.occupancy import occupancy_view, world_to_grid +from dimos.msgs.pointcloud import pointcloud_from_xyz from dimos.navigation.patrolling.create_patrol_router import create_patrol_router from dimos.navigation.patrolling.routers.coverage_patrol_router import CoveragePatrolRouter from dimos.navigation.patrolling.utilities import point_to_pose_stamped @@ -43,14 +47,19 @@ def _circular_disk(radius_cells: int) -> np.ndarray: y, x = np.ogrid[-radius_cells : radius_cells + 1, -radius_cells : radius_cells + 1] - return (x * x + y * y) <= radius_cells * radius_cells + return np.asarray((x * x + y * y) <= radius_cells * radius_cells) def _stamp_scoring_map( - visited: np.ndarray, x: float, y: float, occupancy_grid, radius_cells: int, disk: np.ndarray + visited: np.ndarray, + x: float, + y: float, + occupancy_grid: OccupancyGrid, + radius_cells: int, + disk: np.ndarray, ) -> None: - grid_pos = occupancy_grid.world_to_grid((x, y)) - col, row = int(grid_pos.x), int(grid_pos.y) + grid_pos = world_to_grid(occupancy_grid, Point(x=x, y=y)) + col, row = int(grid_pos[0]), int(grid_pos[1]) h, w = visited.shape r = radius_cells if row + r < 0 or row - r >= h or col + r < 0 or col - r >= w: @@ -69,8 +78,8 @@ def _stamp_scoring_map( def run_iteration( candidates_to_consider: int, target_coverage: float, - occupancy_grid, - costmap, + occupancy_grid: OccupancyGrid, + costmap: OccupancyGrid, scoring_radius_cells: int, scoring_disk: np.ndarray, ) -> tuple[float, float]: @@ -81,11 +90,11 @@ def run_iteration( assert isinstance(router, CoveragePatrolRouter) router._candidates_to_consider = candidates_to_consider router.handle_occupancy_grid(occupancy_grid) - router.handle_odom(point_to_pose_stamped(start)) + router.handle_odom(point_to_pose_stamped(Point(x=start[0], y=start[1]), occupancy_grid.header)) - h, w = occupancy_grid.height, occupancy_grid.width + h, w = occupancy_grid.info.height, occupancy_grid.info.width scoring_visited = np.zeros((h, w), dtype=bool) - free_mask = occupancy_grid.grid == 0 + free_mask = occupancy_view(occupancy_grid) == 0 total_free = int(np.count_nonzero(free_mask)) if total_free == 0: return 0.0, 0.0 @@ -104,22 +113,28 @@ def run_iteration( if goal is None: break - path = min_cost_astar(costmap, goal.position, start, unknown_penalty=1.0, use_cpp=True) + path = min_cost_astar( + costmap, + goal.pose.position, + Point(x=start[0], y=start[1]), + unknown_penalty=1.0, + use_cpp=True, + ) if path is None: continue - path = smooth_resample_path(path, goal, 0.1) + path = smooth_resample_path(path, goal.pose, 0.1) for pose in path.poses: - dx = pose.position.x - start[0] - dy = pose.position.y - start[1] + dx = pose.pose.position.x - start[0] + dy = pose.pose.position.y - start[1] distance_walked += math.sqrt(dx * dx + dy * dy) - start = (pose.position.x, pose.position.y) + start = (pose.pose.position.x, pose.pose.position.y) router.handle_odom(pose) _stamp_scoring_map( scoring_visited, - pose.position.x, - pose.position.y, + pose.pose.position.x, + pose.pose.position.y, occupancy_grid, scoring_radius_cells, scoring_disk, @@ -141,11 +156,11 @@ def main() -> None: args = parser.parse_args() data = read_pointcloud(get_data("big_office.ply")) - cloud = PointCloud2.from_numpy(np.asarray(data.points), frame_id="") + cloud = pointcloud_from_xyz(np.asarray(data.points), header=Header()) occupancy_grid = height_cost_occupancy(cloud) costmap = gradient(occupancy_grid, max_distance=1.5) - scoring_radius_cells = int(np.ceil(SCORING_STAMP_RADIUS_M / occupancy_grid.resolution)) + scoring_radius_cells = int(np.ceil(SCORING_STAMP_RADIUS_M / occupancy_grid.info.resolution)) scoring_disk = _circular_disk(scoring_radius_cells) avg_times = [] diff --git a/misc/optimize_patrol/optimize_patrol_router_child.py b/misc/optimize_patrol/optimize_patrol_router_child.py index 1f037818ddd..0ce66e43df4 100644 --- a/misc/optimize_patrol/optimize_patrol_router_child.py +++ b/misc/optimize_patrol/optimize_patrol_router_child.py @@ -17,13 +17,17 @@ import argparse import math +from dimos_generated.geometry_msgs.msg import Point +from dimos_generated.nav_msgs.msg import OccupancyGrid +from dimos_generated.std_msgs.msg import Header import numpy as np from dimos.mapping.occupancy.gradient import gradient from dimos.mapping.occupancy.path_resampling import smooth_resample_path from dimos.mapping.pointclouds.occupancy import height_cost_occupancy from dimos.mapping.pointclouds.util import read_pointcloud -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 +from dimos.msgs.occupancy import occupancy_view, world_to_grid +from dimos.msgs.pointcloud import pointcloud_from_xyz from dimos.navigation.patrolling.create_patrol_router import create_patrol_router from dimos.navigation.patrolling.routers.visitation_history import VisitationHistory from dimos.navigation.patrolling.utilities import point_to_pose_stamped @@ -35,14 +39,19 @@ def _circular_disk(radius_cells: int) -> np.ndarray: y, x = np.ogrid[-radius_cells : radius_cells + 1, -radius_cells : radius_cells + 1] - return (x * x + y * y) <= radius_cells * radius_cells + return np.asarray((x * x + y * y) <= radius_cells * radius_cells) def _stamp_scoring_map( - visited: np.ndarray, x: float, y: float, occupancy_grid, radius_cells: int, disk: np.ndarray + visited: np.ndarray, + x: float, + y: float, + occupancy_grid: OccupancyGrid, + radius_cells: int, + disk: np.ndarray, ) -> None: - grid_pos = occupancy_grid.world_to_grid((x, y)) - col, row = int(grid_pos.x), int(grid_pos.y) + grid_pos = world_to_grid(occupancy_grid, Point(x=x, y=y)) + col, row = int(grid_pos[0]), int(grid_pos[1]) h, w = visited.shape r = radius_cells if row + r < 0 or row - r >= h or col + r < 0 or col - r >= w: @@ -62,8 +71,8 @@ def run_iteration( saturation_threshold: float, clearance_radius_m: float, total_distance: float, - occupancy_grid, - costmap, + occupancy_grid: OccupancyGrid, + costmap: OccupancyGrid, scoring_radius_cells: int, scoring_disk: np.ndarray, ) -> float: @@ -72,11 +81,11 @@ def run_iteration( VisitationHistory._saturation_threshold = saturation_threshold router = create_patrol_router("coverage", clearance_radius_m) router.handle_occupancy_grid(occupancy_grid) - router.handle_odom(point_to_pose_stamped(start)) + router.handle_odom(point_to_pose_stamped(Point(x=start[0], y=start[1]), occupancy_grid.header)) - h, w = occupancy_grid.height, occupancy_grid.width + h, w = occupancy_grid.info.height, occupancy_grid.info.width scoring_visited = np.zeros((h, w), dtype=bool) - free_mask = occupancy_grid.grid == 0 + free_mask = occupancy_view(occupancy_grid) == 0 _stamp_scoring_map( scoring_visited, start[0], start[1], occupancy_grid, scoring_radius_cells, scoring_disk @@ -88,22 +97,28 @@ def run_iteration( goal = router.next_goal() if goal is None: break - path = min_cost_astar(costmap, goal.position, start, unknown_penalty=1.0, use_cpp=True) + path = min_cost_astar( + costmap, + goal.pose.position, + Point(x=start[0], y=start[1]), + unknown_penalty=1.0, + use_cpp=True, + ) if path is None: continue - path = smooth_resample_path(path, goal, 0.1) + path = smooth_resample_path(path, goal.pose, 0.1) for pose in path.poses: - dx = pose.position.x - start[0] - dy = pose.position.y - start[1] + dx = pose.pose.position.x - start[0] + dy = pose.pose.position.y - start[1] distance_walked += math.sqrt(dx * dx + dy * dy) - start = (pose.position.x, pose.position.y) + start = (pose.pose.position.x, pose.pose.position.y) router.handle_odom(pose) _stamp_scoring_map( scoring_visited, - pose.position.x, - pose.position.y, + pose.pose.position.x, + pose.pose.position.y, occupancy_grid, scoring_radius_cells, scoring_disk, @@ -127,11 +142,11 @@ def main() -> None: args = parser.parse_args() data = read_pointcloud(get_data("big_office.ply")) - cloud = PointCloud2.from_numpy(np.asarray(data.points), frame_id="") + cloud = pointcloud_from_xyz(np.asarray(data.points), header=Header()) occupancy_grid = height_cost_occupancy(cloud) costmap = gradient(occupancy_grid, max_distance=1.5) - scoring_radius_cells = int(np.ceil(SCORING_STAMP_RADIUS_M / occupancy_grid.resolution)) + scoring_radius_cells = int(np.ceil(SCORING_STAMP_RADIUS_M / occupancy_grid.info.resolution)) scoring_disk = _circular_disk(scoring_radius_cells) scores = [] diff --git a/misc/optimize_patrol/plot_path.py b/misc/optimize_patrol/plot_path.py index 2c473de747b..1962fe19bb6 100644 --- a/misc/optimize_patrol/plot_path.py +++ b/misc/optimize_patrol/plot_path.py @@ -22,11 +22,11 @@ import signal import time +from dimos_generated.geometry_msgs.msg import PoseStamped import matplotlib.pyplot as plt import numpy as np from dimos.core.transport import LCMTransport -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped MIN_DIST = 0.05 # minimum distance (m) between recorded points STUCK_RADIUS = 0.6 # if robot stays within this radius (m) ... @@ -49,7 +49,7 @@ def main() -> None: def on_msg(msg: PoseStamped) -> None: nonlocal stop - x, y = msg.position.x, msg.position.y + x, y = msg.pose.position.x, msg.pose.position.y # Record start time on first message. if not t_start: diff --git a/native/cpp/CMakeLists.txt b/native/cpp/CMakeLists.txt index ac4b2e69312..9286eabea20 100644 --- a/native/cpp/CMakeLists.txt +++ b/native/cpp/CMakeLists.txt @@ -31,6 +31,8 @@ endif() # module.hpp runs the dispatch loop and publish workers on std::thread. find_package(Threads REQUIRED) +find_package(dimos_generated CONFIG REQUIRED) + # Header-only SDK. Consumers link this INTERFACE target and inherit the include # path, the C++ standard, and the SDK's own dependencies. A consumer still sets # CMAKE_CXX_STANDARD for the targets it builds that do not link this one. @@ -41,6 +43,7 @@ target_link_libraries(dimos_native INTERFACE pfr nlohmann_json::nlohmann_json Threads::Threads + dimos_generated::messages ) # Zenoh transport. zenoh-cpp is header-only over zenoh-c, which is a compiled @@ -98,3 +101,19 @@ if(DIMOS_NATIVE_BUILD_TESTS) enable_testing() add_subdirectory(tests) endif() + +# Installed consumers need no checkout or FetchContent invocation. The two pinned +# header-only dependencies travel with the SDK, together with upstream licenses. +include(GNUInstallDirs) +include(CMakePackageConfigHelpers) +install(DIRECTORY include/ DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}) +install(DIRECTORY ${pfr_SOURCE_DIR}/include/ DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}) +install(FILES ${pfr_SOURCE_DIR}/LICENSE_1_0.txt DESTINATION ${CMAKE_INSTALL_DATADIR}/dimos_native/licenses/pfr) +if(nlohmann_json_SOURCE_DIR) + install(DIRECTORY ${nlohmann_json_SOURCE_DIR}/include/nlohmann DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}) + install(FILES ${nlohmann_json_SOURCE_DIR}/LICENSE.MIT DESTINATION ${CMAKE_INSTALL_DATADIR}/dimos_native/licenses/nlohmann_json) +endif() +configure_package_config_file(cmake/dimos_nativeConfig.cmake.in + ${CMAKE_CURRENT_BINARY_DIR}/dimos_nativeConfig.cmake + INSTALL_DESTINATION lib/cmake/dimos_native) +install(FILES ${CMAKE_CURRENT_BINARY_DIR}/dimos_nativeConfig.cmake DESTINATION lib/cmake/dimos_native) diff --git a/native/cpp/cmake/dimos_nativeConfig.cmake.in b/native/cpp/cmake/dimos_nativeConfig.cmake.in new file mode 100644 index 00000000000..5eb0800a286 --- /dev/null +++ b/native/cpp/cmake/dimos_nativeConfig.cmake.in @@ -0,0 +1,28 @@ +@PACKAGE_INIT@ +set(_dimos_native_prefix "${PACKAGE_PREFIX_DIR}") +include(CMakeFindDependencyMacro) +find_dependency(Threads) +find_dependency(dimos_generated CONFIG) +find_dependency(PkgConfig) +pkg_check_modules(LCM REQUIRED IMPORTED_TARGET lcm) +pkg_check_modules(ZENOHC QUIET IMPORTED_TARGET zenohc>=1.9.0) +if(ZENOHC_FOUND AND NOT TARGET zenohc::lib) + add_library(zenohc::lib ALIAS PkgConfig::ZENOHC) +elseif(NOT ZENOHC_FOUND) + find_dependency(zenohc 1.9.0) +endif() +find_dependency(zenohcxx 1.9.0) +# System JSON installations remain normal dependencies; fetched pinned headers +# are included in the SDK archive instead. +if(NOT EXISTS "${_dimos_native_prefix}/include/nlohmann/json.hpp") + find_dependency(nlohmann_json) + set(_dimos_json_target nlohmann_json::nlohmann_json) +endif() +if(NOT TARGET dimos_native::dimos_native) + add_library(dimos_native::dimos_native INTERFACE IMPORTED) + set_target_properties(dimos_native::dimos_native PROPERTIES + INTERFACE_COMPILE_FEATURES cxx_std_20 + INTERFACE_INCLUDE_DIRECTORIES "${_dimos_native_prefix}/include" + INTERFACE_LINK_LIBRARIES "Threads::Threads;dimos_generated::messages;zenohcxx::zenohc;PkgConfig::LCM;${_dimos_json_target}") +endif() +check_required_components(dimos_native) diff --git a/native/cpp/include/dimos/native.hpp b/native/cpp/include/dimos/native.hpp index 8a6a9b90df6..21a02872bbd 100644 --- a/native/cpp/include/dimos/native.hpp +++ b/native/cpp/include/dimos/native.hpp @@ -8,7 +8,7 @@ #include #include "dimos/native/config.hpp" -#include "dimos/native/lcm_codec.hpp" +#include "dimos/native/cdr_codec.hpp" #include "dimos/native/lcm_transport.hpp" #include "dimos/native/log.hpp" #include "dimos/native/module.hpp" diff --git a/native/cpp/include/dimos/native/cdr_codec.hpp b/native/cpp/include/dimos/native/cdr_codec.hpp new file mode 100644 index 00000000000..237e2b86221 --- /dev/null +++ b/native/cpp/include/dimos/native/cdr_codec.hpp @@ -0,0 +1,22 @@ +// Copyright 2026 Dimensional Inc. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include + +namespace dimos::native { + +/// Default output codec for generated ROS2-shaped message values. +template +std::vector cdr_encode(const T& message) { + return dimos::cdr::encode(message); +} + +/// Default input codec; malformed and trailing data raise std::invalid_argument. +template +T cdr_decode(const uint8_t* data, std::size_t size) { + return dimos::cdr::decode(data, size); +} + +} // namespace dimos::native diff --git a/native/cpp/include/dimos/native/lcm_codec.hpp b/native/cpp/include/dimos/native/lcm_codec.hpp deleted file mode 100644 index cf8a5b3be35..00000000000 --- a/native/cpp/include/dimos/native/lcm_codec.hpp +++ /dev/null @@ -1,33 +0,0 @@ -// Copyright 2026 Dimensional Inc. -// SPDX-License-Identifier: Apache-2.0 -// -// Adapters from dimos-lcm message types to the SDK's encode/decode signatures. - -#pragma once - -#include -#include -#include - -namespace dimos::native { - -/// Encode any lcm-gen message `T` to a byte buffer. The default output encoder. -template -std::vector lcm_encode(const T& msg) { - std::vector buf(static_cast(msg.getEncodedSize())); - msg.encode(buf.data(), 0, static_cast(buf.size())); - return buf; -} - -/// Decode a byte buffer into an lcm-gen message `T`, throwing on malformed input. -/// The default input decoder. -template -T lcm_decode(const uint8_t* data, std::size_t len) { - T msg; - if (msg.decode(data, 0, static_cast(len)) < 0) { - throw std::runtime_error("lcm_decode: message decode failed"); - } - return msg; -} - -} // namespace dimos::native diff --git a/native/cpp/include/dimos/native/module.hpp b/native/cpp/include/dimos/native/module.hpp index ab410c8ee52..a39cfaef521 100644 --- a/native/cpp/include/dimos/native/module.hpp +++ b/native/cpp/include/dimos/native/module.hpp @@ -28,7 +28,7 @@ #include #include "dimos/native/config.hpp" -#include "dimos/native/lcm_codec.hpp" +#include "dimos/native/cdr_codec.hpp" #include "dimos/native/log.hpp" #include "dimos/native/transport.hpp" @@ -277,14 +277,14 @@ class Builder { /// Handlers run serialized, so they touch module state without locks. template void input(const std::string& port, void (Self::*handler)(const T&), Self* self, - DecodeFn decode = lcm_decode) { + DecodeFn decode = cdr_decode) { input(port, std::move(decode), [self, handler](T msg) { (self->*handler)(msg); }); } /// publish() hands off to a per-channel worker, so it never blocks. template - Output output(const std::string& port, EncodeFn encode = lcm_encode) { + Output output(const std::string& port, EncodeFn encode = cdr_encode) { std::string topic = topic_for(port); auto queue = std::make_shared(topic); publish_queues_.push_back(queue); diff --git a/native/cpp/messages.nix b/native/cpp/messages.nix new file mode 100644 index 00000000000..5c5c1a9ee04 --- /dev/null +++ b/native/cpp/messages.nix @@ -0,0 +1,26 @@ +# Built-in message package generated from the same pinned inputs as Cargo/Python. +{ pkgs }: +let + fastcdr = pkgs.stdenv.mkDerivation { + pname = "fastcdr"; + version = "2.4.0"; + src = pkgs.fetchurl { + url = "https://github.com/eProsima/Fast-CDR/archive/refs/tags/v2.4.0.tar.gz"; + sha256 = "79d8466107dd6b7d1defe961c4aa31735038937cf9dd1175cf6b0da0df2209ab"; + }; + nativeBuildInputs = [ pkgs.cmake ]; + cmakeFlags = [ "-DBUILD_TESTING=OFF" "-DBUILD_SHARED_LIBS=OFF" "-DCMAKE_POSITION_INDEPENDENT_CODE=ON" ]; + }; +in pkgs.stdenv.mkDerivation { + pname = "dimos-generated-messages"; + version = "0.1.0"; + src = pkgs.lib.fileset.toSource { + root = ../..; + fileset = pkgs.lib.fileset.unions [ ../../dimos/__init__.py ../../dimos/message_codegen ]; + }; + nativeBuildInputs = [ pkgs.cmake pkgs.python3 ]; + propagatedBuildInputs = [ fastcdr ]; + postPatch = "python3 -m dimos.message_codegen.generate --output generated"; + cmakeDir = "../generated/cpp"; + cmakeFlags = [ "-DDIMOS_BUILD_PYTHON=OFF" ]; +} diff --git a/native/cpp/tests/CMakeLists.txt b/native/cpp/tests/CMakeLists.txt index 0489887ee78..1a4c203eae5 100644 --- a/native/cpp/tests/CMakeLists.txt +++ b/native/cpp/tests/CMakeLists.txt @@ -16,7 +16,7 @@ add_executable(dimos_native_tests test_transport_selection.cpp test_config.cpp test_module.cpp - test_lcm_codec.cpp + test_cdr_codec.cpp test_lcm_transport.cpp test_zenoh_transport.cpp ) @@ -33,3 +33,8 @@ target_compile_definitions(dimos_native_tests PRIVATE include(${doctest_SOURCE_DIR}/scripts/cmake/doctest.cmake) doctest_discover_tests(dimos_native_tests) + +# Exercise the production lidar message builder without sensor/PCL dependencies. +add_executable(dimos_lidar_cdr_demo ../../../examples/message-codegen/demo_lidar.cpp) +target_link_libraries(dimos_lidar_cdr_demo PRIVATE dimos_generated::messages) +add_test(NAME lidar_generated_cdr COMMAND dimos_lidar_cdr_demo) diff --git a/native/cpp/tests/test_cdr_codec.cpp b/native/cpp/tests/test_cdr_codec.cpp new file mode 100644 index 00000000000..f3d487369a6 --- /dev/null +++ b/native/cpp/tests/test_cdr_codec.cpp @@ -0,0 +1,34 @@ +// Copyright 2026 Dimensional Inc. +// SPDX-License-Identifier: Apache-2.0 + +#include +#include "dimos/native/cdr_codec.hpp" + +using namespace dimos::native; + +TEST_CASE("default CDR codecs preserve nested generated fields") { + geometry_msgs::msg::PoseStamped message{}; + message.header.frame_id = "map"; + message.header.stamp.sec = 1700000000; + message.header.stamp.nanosec = 123456789; + message.pose.position.x = 1.5; + message.pose.orientation.w = 1; + const auto bytes = cdr_encode(message); + REQUIRE(bytes.size() > 4); + CHECK(bytes[0] == 0); + CHECK(bytes[1] == 1); + CHECK(cdr_decode(bytes.data(), bytes.size()) == message); + const auto big = dimos::cdr::encode(message, false); + CHECK(cdr_decode(big.data(), big.size()) == message); +} + +TEST_CASE("default CDR decoder rejects malformed input") { + std_msgs::msg::Int32 message{}; + message.data = 1234; + auto bytes = cdr_encode(message); + CHECK_THROWS_AS(cdr_decode(bytes.data(), bytes.size() - 1), std::invalid_argument); + bytes.push_back(0); + CHECK_THROWS_AS(cdr_decode(bytes.data(), bytes.size()), std::invalid_argument); + bytes[0] = 255; + CHECK_THROWS_AS(cdr_decode(bytes.data(), bytes.size()), std::invalid_argument); +} diff --git a/native/cpp/tests/test_lcm_codec.cpp b/native/cpp/tests/test_lcm_codec.cpp deleted file mode 100644 index 15b11135139..00000000000 --- a/native/cpp/tests/test_lcm_codec.cpp +++ /dev/null @@ -1,57 +0,0 @@ -// Copyright 2026 Dimensional Inc. -// SPDX-License-Identifier: Apache-2.0 - -#include - -#include -#include -#include -#include - -#include "dimos/native/lcm_codec.hpp" - -using namespace dimos::native; - -namespace { - -// Stand-in with the same method surface every lcm-gen C++ type exposes, so the -// generic codec can be exercised without pulling in real generated messages. -struct FakeMsg { - std::int32_t value = 0; - - int getEncodedSize() const { return static_cast(sizeof(std::int32_t)); } - - int encode(void* buf, int offset, int maxlen) const { - if (maxlen - offset < static_cast(sizeof(std::int32_t))) { - return -1; - } - std::memcpy(static_cast(buf) + offset, &value, sizeof(std::int32_t)); - return static_cast(sizeof(std::int32_t)); - } - - int decode(const void* buf, int offset, int maxlen) { - if (maxlen - offset < static_cast(sizeof(std::int32_t))) { - return -1; - } - std::memcpy(&value, static_cast(buf) + offset, sizeof(std::int32_t)); - return static_cast(sizeof(std::int32_t)); - } -}; - -} // namespace - -TEST_CASE("lcm_encode / lcm_decode round-trip a message") { - FakeMsg msg; - msg.value = 1234; - - std::vector bytes = lcm_encode(msg); - CHECK(bytes.size() == sizeof(std::int32_t)); - - FakeMsg decoded = lcm_decode(bytes.data(), bytes.size()); - CHECK(decoded.value == 1234); -} - -TEST_CASE("lcm_decode throws on a short buffer") { - std::vector truncated{1, 2}; - CHECK_THROWS_AS(lcm_decode(truncated.data(), truncated.size()), std::runtime_error); -} diff --git a/native/cpp/tests/test_module.cpp b/native/cpp/tests/test_module.cpp index a0f6c728d15..a7df43bee41 100644 --- a/native/cpp/tests/test_module.cpp +++ b/native/cpp/tests/test_module.cpp @@ -70,25 +70,11 @@ struct MockTransport : Transport { Bytes identity_decode(const uint8_t* d, std::size_t n) { return Bytes(d, d + n); } Bytes identity_encode(const Bytes& v) { return v; } -// Minimal lcm-gen-shaped message, to exercise the default codecs. -struct Pod { - std::int32_t v = 0; - int getEncodedSize() const { return static_cast(sizeof(std::int32_t)); } - int encode(void* buf, int offset, int maxlen) const { - if (maxlen - offset < static_cast(sizeof(std::int32_t))) return -1; - std::memcpy(static_cast(buf) + offset, &v, sizeof(std::int32_t)); - return static_cast(sizeof(std::int32_t)); - } - int decode(const void* buf, int offset, int maxlen) { - if (maxlen - offset < static_cast(sizeof(std::int32_t))) return -1; - std::memcpy(&v, static_cast(buf) + offset, sizeof(std::int32_t)); - return static_cast(sizeof(std::int32_t)); - } -}; +using Pod = std_msgs::msg::Int32; struct Sink { std::vector got; - void on(const Pod& p) { got.push_back(p.v); } + void on(const Pod& p) { got.push_back(p.data); } }; template @@ -163,7 +149,7 @@ TEST_CASE("member-function handler and default codecs route a message") { Builder builder({{"in", "/in"}, {"out", "/out"}}, ¬ifier); Sink sink; - Output out = builder.output("out"); // encoder defaults to lcm_encode + Output out = builder.output("out"); // encoder defaults to cdr_encode builder.input("in", &Sink::on, &sink); // member fn + default decoder for (const auto& route : builder.routes()) { @@ -172,8 +158,8 @@ TEST_CASE("member-function handler and default codecs route a message") { WorkerGuard workers(builder, transport); Pod m; - m.v = 9; - transport.deliver("/in", dimos::native::lcm_encode(m)); + m.data = 9; + transport.deliver("/in", dimos::native::cdr_encode(m)); for (InputPort* port : builder.input_ports()) { port->drain_one(); } diff --git a/native/python/example/example_external/runtime.py b/native/python/example/example_external/runtime.py index 26bdc85f9aa..d396bf4df2b 100644 --- a/native/python/example/example_external/runtime.py +++ b/native/python/example/example_external/runtime.py @@ -14,10 +14,11 @@ from typing import Any +from dimos_generated.std_msgs.msg import Int32 + from dimos.agents.annotation import skill from dimos.core.core import rpc from dimos.experimental.isolated_python.example.contract import ExampleExternal -from dimos.msgs.std_msgs.Int32 import Int32 class ExampleExternalRuntime(ExampleExternal): @@ -33,7 +34,7 @@ def start(self) -> None: self.value.subscribe(self._publish_doubled) def _publish_doubled(self, message: Int32) -> None: - self.doubled.publish(Int32(message.data * self._multiplier)) + self.doubled.publish(Int32(data=message.data * self._multiplier)) @rpc def get_multiplier(self) -> int: diff --git a/native/python/graspgenx/graspgenx_runtime/runtime.py b/native/python/graspgenx/graspgenx_runtime/runtime.py index e5d672b44ac..ee830520fbe 100644 --- a/native/python/graspgenx/graspgenx_runtime/runtime.py +++ b/native/python/graspgenx/graspgenx_runtime/runtime.py @@ -18,6 +18,9 @@ from typing import TYPE_CHECKING, Any +from dimos_generated.dimos_msgs.msg import GraspCandidate, GraspCandidateArray +from dimos_generated.geometry_msgs.msg import Pose +from dimos_generated.sensor_msgs.msg import PointCloud2 import numpy as np from dimos.core.core import rpc @@ -26,13 +29,9 @@ GraspGenXError, GraspGenXModule, ) -from dimos.msgs.geometry_msgs.Pose import Pose -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.msgs.manipulation_msgs.GraspCandidate import GraspCandidate -from dimos.msgs.manipulation_msgs.GraspCandidateArray import GraspCandidateArray -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 -from dimos.msgs.std_msgs.Header import Header +from dimos.msgs.geometry import pose_from_matrix +from dimos.msgs.pointcloud import pointcloud_xyz +from dimos.msgs.time import to_nanoseconds if TYPE_CHECKING: from .backend import GraspGenXRuntime @@ -71,12 +70,11 @@ def stop(self) -> None: def propose_grasps(self, object_pointcloud: PointCloud2) -> GraspCandidateArray: if self._runtime is None: raise GraspGenXError("GraspGenX module has not been started") - if object_pointcloud.ts is None: - raise ValueError("object pointcloud must have a timestamp") - if not object_pointcloud.frame_id: + to_nanoseconds(object_pointcloud.header.stamp) + if not object_pointcloud.header.frame_id: raise ValueError("object pointcloud frame_id must not be empty") - points = object_pointcloud.points_f32() + points = pointcloud_xyz(object_pointcloud).astype(np.float32) if points.ndim != 2 or points.shape[1] != 3 or len(points) == 0: raise ValueError("object pointcloud must contain at least one XYZ point") if not np.all(np.isfinite(points)): @@ -90,8 +88,8 @@ def propose_grasps(self, object_pointcloud: PointCloud2) -> GraspCandidateArray: if poses.size == 0 and scores.size == 0: return GraspCandidateArray( - Header(float(object_pointcloud.ts), object_pointcloud.frame_id), - [], + header=object_pointcloud.header, + candidates=[], ) if poses.shape != (len(scores), 4, 4): raise ValueError("backend poses must have shape (N, 4, 4)") @@ -108,19 +106,16 @@ def propose_grasps(self, object_pointcloud: PointCloud2) -> GraspCandidateArray: tcp_poses = poses @ np.asarray(self.config.grasp_frame_to_tcp) order = np.argsort(-scores, kind="stable")[: self.config.max_candidates] candidates = [ - GraspCandidate(self._pose_from_matrix(tcp_poses[index]), float(scores[index])) + GraspCandidate( + pose=self._pose_from_matrix(tcp_poses[index]), score=float(scores[index]) + ) for index in order ] return GraspCandidateArray( - Header(float(object_pointcloud.ts), object_pointcloud.frame_id), - candidates, + header=object_pointcloud.header, + candidates=candidates, ) @staticmethod def _pose_from_matrix(matrix: np.ndarray) -> Pose: - return Pose( - { - "position": Vector3(matrix[:3, 3]), - "orientation": Quaternion.from_rotation_matrix(matrix[:3, :3]), - } - ) + return pose_from_matrix(matrix) diff --git a/native/python/graspgenx/graspgenx_runtime/test_runtime.py b/native/python/graspgenx/graspgenx_runtime/test_runtime.py index faa7ea68249..525db7d1eec 100644 --- a/native/python/graspgenx/graspgenx_runtime/test_runtime.py +++ b/native/python/graspgenx/graspgenx_runtime/test_runtime.py @@ -19,6 +19,9 @@ import inspect from typing import Any +from dimos_generated.builtin_interfaces.msg import Time +from dimos_generated.sensor_msgs.msg import PointCloud2 +from dimos_generated.std_msgs.msg import Header import graspgenx_runtime.runtime as runtime_module from graspgenx_runtime.runtime import GraspGenXRuntimeModule import numpy as np @@ -31,7 +34,7 @@ GraspGenXError, GraspGenXModule, ) -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 +from dimos.msgs.pointcloud import pointcloud_from_xyz def config(**overrides: Any) -> GraspGenXConfig: @@ -54,7 +57,9 @@ def module_args(value: GraspGenXConfig | None = None) -> dict[str, Any]: def cloud(points: np.ndarray | None = None) -> PointCloud2: xyz = np.zeros((1, 3), dtype=np.float32) if points is None else points - return PointCloud2.from_numpy(xyz, frame_id="camera", timestamp=12.5) + return pointcloud_from_xyz( + xyz, header=Header(frame_id="camera", stamp=Time(sec=12, nanosec=500000000)) + ) @pytest.fixture @@ -75,7 +80,7 @@ def test_start_is_synchronous_and_idempotent(runtime: Any) -> None: module.start() runtime.assert_called_once_with(module.config) - assert len(module.propose_grasps(cloud())) == 1 + assert len(module.propose_grasps(cloud()).candidates) == 1 finally: module.stop() @@ -111,10 +116,12 @@ def test_adapter_sorts_stably_truncates_and_applies_tcp_transform(runtime: Any) module.start() result = module.propose_grasps(cloud()) - assert [candidate.score for candidate in result] == pytest.approx([0.9, 0.5]) - assert [candidate.pose.position.x for candidate in result] == pytest.approx([13.0, 11.0]) + assert [candidate.score for candidate in result.candidates] == pytest.approx([0.9, 0.5]) + assert [candidate.pose.position.x for candidate in result.candidates] == pytest.approx( + [13.0, 11.0] + ) assert result.header.frame_id == "camera" - assert result.header.timestamp == pytest.approx(12.5) + assert result.header.stamp == Time(sec=12, nanosec=500000000) finally: module.stop() @@ -130,7 +137,7 @@ def test_empty_backend_result_preserves_input_header(runtime: Any) -> None: result = module.propose_grasps(cloud()) assert result.header.frame_id == "camera" - assert result.header.timestamp == pytest.approx(12.5) + assert result.header.stamp == Time(sec=12, nanosec=500000000) assert result.candidates == [] finally: module.stop() @@ -190,16 +197,16 @@ def test_inference_failure_is_wrapped(runtime: Any) -> None: def test_not_started_and_missing_metadata_are_rejected(runtime: Any) -> None: module = GraspGenXRuntimeModule(_isolated_python_runtime=True, **module_args()) missing_frame = cloud() - missing_frame.frame_id = "" + missing_frame.header.frame_id = "" missing_timestamp = cloud() - missing_timestamp.ts = None + missing_timestamp.header.stamp.nanosec = 1000000000 try: with pytest.raises(GraspGenXError, match="not been started"): module.propose_grasps(cloud()) module.start() with pytest.raises(ValueError, match="frame_id"): module.propose_grasps(missing_frame) - with pytest.raises(ValueError, match="timestamp"): + with pytest.raises(ValueError, match="nanosec"): module.propose_grasps(missing_timestamp) finally: module.stop() diff --git a/native/python/lerobot/dimos_lerobot/runtime.py b/native/python/lerobot/dimos_lerobot/runtime.py index 1751cfe45e1..6584b6f6193 100644 --- a/native/python/lerobot/dimos_lerobot/runtime.py +++ b/native/python/lerobot/dimos_lerobot/runtime.py @@ -17,11 +17,15 @@ from __future__ import annotations from contextlib import nullcontext +import copy from dataclasses import dataclass from threading import Condition, Event, RLock, Thread, current_thread import time from typing import Any +from dimos_generated.sensor_msgs.msg import Image, JointState +from dimos_generated.std_msgs.msg import UInt32 +from dimos_generated.trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint from lerobot.configs.policies import PreTrainedConfig from lerobot.policies.factory import get_policy_class, make_pre_post_processors from lerobot.policies.pretrained import PreTrainedPolicy @@ -44,10 +48,8 @@ LeRobotPolicyModule, RolloutStatus, ) -from dimos.msgs.sensor_msgs.Image import Image, ImageFormat -from dimos.msgs.sensor_msgs.JointState import JointState -from dimos.msgs.trajectory_msgs.JointTrajectory import JointTrajectory -from dimos.msgs.trajectory_msgs.TrajectoryPoint import TrajectoryPoint +from dimos.msgs.image import image_view +from dimos.msgs.time import duration_from_seconds, header_now, to_seconds from dimos.teleop.webxr.controller_types import BUTTON_ALIASES, Buttons from dimos.utils.logging_config import setup_logger @@ -217,33 +219,39 @@ def _status_locked(self) -> RolloutStatus: } def _on_color_image(self, image: Image) -> None: - if image.format != ImageFormat.RGB or image.data.dtype != np.uint8: + if image.encoding != "rgb8": logger.warning("Ignoring non-uint8 RGB policy image", image=str(image)) return + pixels = image_view(image) expected_shape = (self.config.image_height, self.config.image_width, 3) - if image.data.shape != expected_shape: + if pixels.shape != expected_shape: logger.warning( "Ignoring policy image with unexpected shape", - shape=image.data.shape, + shape=pixels.shape, expected=expected_shape, ) return with self._lock: - self._latest_image = (np.ascontiguousarray(image.data), image.ts) + self._latest_image = ( + np.array(pixels, dtype=np.uint8, order="C", copy=True), + to_seconds(image.header.stamp), + ) def _on_joint_state(self, state: JointState) -> None: with self._observation_changed: - self._latest_joint_state = JointState(state) + self._latest_joint_state = copy.deepcopy(state) self._observation_changed.notify_all() - def _on_teleop_buttons(self, buttons: Buttons) -> None: + def _on_teleop_buttons(self, message: UInt32) -> None: + buttons = Buttons(data=message.data) with self._observation_changed: self._manual_control = buttons.left_grip or buttons.right_grip if self._manual_control and self._active: self._stop_event.set() self._observation_changed.notify_all() - def _on_button_pressed(self, buttons: Buttons) -> None: + def _on_button_pressed(self, message: UInt32) -> None: + buttons = Buttons(data=message.data) button = BUTTON_ALIASES.get(self.config.rollout_button, self.config.rollout_button) if not bool(getattr(buttons, button)): return @@ -267,8 +275,10 @@ def _snapshot_observation( max_age = self.config.max_observation_age_s if now - image_ts > max_age: raise RuntimeError(f"camera image is stale by {now - image_ts:.2f}s") - if now - state.ts > max_age: - raise RuntimeError(f"joint state is stale by {now - state.ts:.2f}s") + if now - to_seconds(state.header.stamp) > max_age: + raise RuntimeError( + f"joint state is stale by {now - to_seconds(state.header.stamp):.2f}s" + ) positions = dict(zip(state.name, state.position, strict=False)) missing = [name for name in self.config.joint_names if name not in positions] @@ -280,7 +290,7 @@ def _snapshot_observation( ) if not np.all(np.isfinite(vector)): raise RuntimeError("joint state contains non-finite positions") - return image.copy(), vector, state.ts + return image.copy(), vector, to_seconds(state.header.stamp) def _load_policy(self) -> _LoadedPolicy: register_third_party_plugins() @@ -497,21 +507,23 @@ def _trajectory( ) -> JointTrajectory: zeros = [0.0] * len(self.config.joint_names) points = [ - TrajectoryPoint( + JointTrajectoryPoint( positions=[float(value) for value in state], velocities=zeros, - time_from_start=0.0, + time_from_start=duration_from_seconds(0.0), ) ] points.extend( - TrajectoryPoint( + JointTrajectoryPoint( positions=[float(value) for value in action], velocities=zeros, - time_from_start=(index + 1) / self.config.fps, + time_from_start=duration_from_seconds((index + 1) / self.config.fps), ) for index, action in enumerate(actions) ) - return JointTrajectory(joint_names=list(self.config.joint_names), points=points) + return JointTrajectory( + header=header_now(), joint_names=list(self.config.joint_names), points=points + ) def _wait_for_newer_joint_state(self, previous_ts: float) -> None: with self._observation_changed: @@ -519,7 +531,7 @@ def _wait_for_newer_joint_state(self, previous_ts: float) -> None: lambda: self._stop_event.is_set() or ( self._latest_joint_state is not None - and self._latest_joint_state.ts > previous_ts + and to_seconds(self._latest_joint_state.header.stamp) > previous_ts ) ) diff --git a/native/python/lerobot/dimos_lerobot/runtime_tests.py b/native/python/lerobot/dimos_lerobot/runtime_tests.py index f18e66c1adc..0ab424e469a 100644 --- a/native/python/lerobot/dimos_lerobot/runtime_tests.py +++ b/native/python/lerobot/dimos_lerobot/runtime_tests.py @@ -19,6 +19,8 @@ import time from typing import Any, Protocol +from dimos_generated.sensor_msgs.msg import JointState +from dimos_generated.std_msgs.msg import Header from dimos_lerobot import runtime as policy_runtime from dimos_lerobot.runtime import LeRobotPolicyRuntime from lerobot.configs.policies import PreTrainedConfig @@ -35,8 +37,8 @@ TrajectoryExecutionResult, TrajectoryExecutionStatus, ) -from dimos.msgs.sensor_msgs.Image import Image, ImageFormat -from dimos.msgs.sensor_msgs.JointState import JointState +from dimos.msgs.image import image_from_array +from dimos.msgs.time import time_from_seconds, to_seconds from dimos.protocol.rpc.pubsubrpc import LCMRPC from dimos.teleop.webxr.controller_types import Buttons from dimos.utils.testing.waiting import wait_until @@ -229,8 +231,12 @@ def _provide_observation( rgb[..., 2] = 30 values = positions or [float(i) / 10 for i in range(len(JOINTS))] timestamp = time.time() if ts is None else ts - module._on_color_image(Image(data=rgb, format=ImageFormat.RGB, ts=timestamp)) - module._on_joint_state(JointState(ts=timestamp, name=JOINTS, position=values)) + module._on_color_image( + image_from_array(rgb, encoding="rgb8", header=Header(stamp=time_from_seconds(timestamp))) + ) + module._on_joint_state( + JointState(header=Header(stamp=time_from_seconds(timestamp)), name=JOINTS, position=values) + ) return rgb, values, timestamp @@ -256,7 +262,7 @@ def test_policy_predicts_and_executes_one_native_joint_chunk(make_runtime: Runti trajectory = call.args[0] assert call.kwargs == {} assert trajectory.joint_names == JOINTS - assert [point.time_from_start for point in trajectory.points] == [0.0, 0.02, 0.04] + assert [to_seconds(point.time_from_start) for point in trajectory.points] == [0.0, 0.02, 0.04] np.testing.assert_allclose(trajectory.points[0].positions, positions) np.testing.assert_allclose(trajectory.points[1].positions, actions[0]) np.testing.assert_allclose(trajectory.points[2].positions, actions[1]) @@ -366,10 +372,10 @@ def execute_trajectory(*_args: object, **_kwargs: object) -> TrajectoryExecution pressed.right_primary = True def stop_from_button() -> None: - module._on_button_pressed(pressed) + module._on_button_pressed(pressed.to_message()) stop_finished.set() - module._on_button_pressed(pressed) + module._on_button_pressed(pressed.to_message()) assert execute_started.wait(timeout=1.0) stop_thread = Thread(target=stop_from_button) stop_thread.start() @@ -582,9 +588,17 @@ def test_preflight_rejects_invalid_live_joints( module, control = make_runtime(policy) timestamp = time.time() module._on_color_image( - Image(data=np.zeros((4, 5, 3), dtype=np.uint8), format=ImageFormat.RGB, ts=timestamp) + image_from_array( + np.zeros((4, 5, 3), dtype=np.uint8), + encoding="rgb8", + header=Header(stamp=time_from_seconds(timestamp)), + ) + ) + module._on_joint_state( + JointState( + header=Header(stamp=time_from_seconds(timestamp)), name=names, position=positions + ) ) - module._on_joint_state(JointState(ts=timestamp, name=names, position=positions)) status = module.preflight_rollout() @@ -596,20 +610,30 @@ def test_preflight_rejects_invalid_live_joints( @pytest.mark.parametrize( ("image", "image_format"), [ - (np.zeros((8, 8, 3), dtype=np.uint8), ImageFormat.RGB), - (np.zeros((4, 5, 3), dtype=np.uint8), ImageFormat.BGR), + (np.zeros((8, 8, 3), dtype=np.uint8), "rgb8"), + (np.zeros((4, 5, 3), dtype=np.uint8), "bgr8"), ], ) def test_preflight_requires_exact_live_rgb_contract( make_runtime: RuntimeFactory, image: NDArray[np.uint8], - image_format: ImageFormat, + image_format: str, ) -> None: policy = FakePolicy(_action_chunk()) module, control = make_runtime(policy) timestamp = time.time() - module._on_color_image(Image(data=image, format=image_format, ts=timestamp)) - module._on_joint_state(JointState(ts=timestamp, name=JOINTS, position=[0.0] * len(JOINTS))) + module._on_color_image( + image_from_array( + image, encoding=image_format, header=Header(stamp=time_from_seconds(timestamp)) + ) + ) + module._on_joint_state( + JointState( + header=Header(stamp=time_from_seconds(timestamp)), + name=JOINTS, + position=[0.0] * len(JOINTS), + ) + ) status = module.preflight_rollout() @@ -671,12 +695,12 @@ def test_grip_takeover_stops_rollout_until_explicit_restart( held = Buttons() setattr(held, grip, True) - module._on_teleop_buttons(held) + module._on_teleop_buttons(held.to_message()) wait_until(lambda: not module.rollout_status()["active"], timeout=1) assert module.start_rollout()["active"] is False assert "release" in (module.rollout_status()["last_error"] or "") submissions = control.execute_trajectory.call_count - module._on_teleop_buttons(Buttons()) + module._on_teleop_buttons(Buttons().to_message()) assert module.rollout_status()["active"] is False assert control.execute_trajectory.call_count == submissions _provide_observation(module) @@ -704,8 +728,8 @@ def predict(*args: Any, **kwargs: Any) -> NDArray[np.float32]: assert predicting.wait(timeout=1) held = Buttons() held.right_grip = True - module._on_teleop_buttons(held) - module._on_teleop_buttons(Buttons()) + module._on_teleop_buttons(held.to_message()) + module._on_teleop_buttons(Buttons().to_message()) assert module._stop_event.is_set() control.execute_trajectory.assert_not_called() finally: @@ -722,5 +746,5 @@ def test_stopping_inactive_policy_leaves_planner_trajectory_alone( module.stop_rollout() held = Buttons() held.right_grip = True - module._on_teleop_buttons(held) + module._on_teleop_buttons(held.to_message()) control.cancel_trajectory.assert_not_called() diff --git a/native/rust/README.md b/native/rust/README.md index e845b0f577c..cd584d017ee 100644 --- a/native/rust/README.md +++ b/native/rust/README.md @@ -8,8 +8,8 @@ Two crates: ## Writing a module ```rust -use dimos_module::{native_config, run_with_transport, Input, Module, Output}; -use lcm_msgs::geometry_msgs::Twist; +use dimos_module::{cdr, native_config, run_with_transport, Input, Io, Module, Output}; +use dimos_generated_messages::geometry_msgs::msg::Twist; #[native_config] struct MyConfig { @@ -20,13 +20,13 @@ struct MyConfig { #[derive(Module)] #[module(setup = on_start, teardown = on_stop)] struct MyModule { - #[input(decode = Twist::decode)] + #[input(decode = cdr::decode)] cmd: Input, - #[output(encode = Twist::encode)] + #[output(encode = cdr::encode)] out: Output, - #[io(decode = Twist::decode, encode = Twist::encode)] + #[io(decode = cdr::decode, encode = cdr::encode)] shared: Io, #[config] @@ -175,8 +175,8 @@ impl ::dimos_module::Module for MyModule { fn build(builder: &mut ::dimos_module::Builder, config: Self::Config) -> Self { Self { - cmd: builder.input("cmd", Twist::decode), - out: builder.output("out", Twist::encode), + cmd: builder.input("cmd", cdr::decode), + out: builder.output("out", cdr::encode), config, } } diff --git a/native/rust/dimos-lcm-transport/README.md b/native/rust/dimos-lcm-transport/README.md index 57907bcbf8e..c4860be61bb 100644 --- a/native/rust/dimos-lcm-transport/README.md +++ b/native/rust/dimos-lcm-transport/README.md @@ -25,3 +25,13 @@ cargo build -p dimos-lcm-transport --example interop The demo chooses a local UDP port, starts the native receiver, waits for its ready signal, and exchanges both short and fragmented binary payloads with Python's LCM binding. It terminates and reaps the native process even on failure. + +The receiver rejects malformed offsets, lengths, fragment numbers, conflicting +headers, overlaps, holes, and invalid channel UTF-8. Repeated fragments do not +advance completion; out-of-order fragments remain supported. Limits are 64 MiB +per message, 128 MiB of incomplete payload storage, and 64 incomplete messages +per transport instance. Fragment bookkeeping is also bounded by these limits and +the protocol's 16-bit fragment count. Incomplete messages expire after five +seconds and are purged when another fragment arrives. Publishing above 64 MiB +returns `InvalidInput` before any datagrams are sent. These are raw transport +limits and do not depend on the payload's encoding. diff --git a/native/rust/dimos-lcm-transport/src/transport.rs b/native/rust/dimos-lcm-transport/src/transport.rs index 5be58aa18b3..54482f69524 100644 --- a/native/rust/dimos-lcm-transport/src/transport.rs +++ b/native/rust/dimos-lcm-transport/src/transport.rs @@ -19,6 +19,7 @@ use std::io; use std::net::{Ipv4Addr, SocketAddr, SocketAddrV4}; use std::sync::atomic::{AtomicU32, Ordering}; use std::sync::Mutex; +use std::time::{Duration, Instant}; use tokio::net::UdpSocket; const MAGIC_SHORT: u32 = 0x4c433032; // "LC02" @@ -26,6 +27,11 @@ const MAGIC_LONG: u32 = 0x4c433033; // "LC03" const SHORT_HEADER_SIZE: usize = 8; const FRAGMENT_HEADER_SIZE: usize = 20; const MAX_DATAGRAM_SIZE: usize = 65507; +// Bound incomplete UDP messages independently of sender-supplied lengths. +const MAX_MESSAGE_SIZE: usize = 64 * 1024 * 1024; +const MAX_REASSEMBLY_BYTES: usize = 128 * 1024 * 1024; +const MAX_INCOMPLETE_MESSAGES: usize = 64; +const REASSEMBLY_TIMEOUT: Duration = Duration::from_secs(5); /// Default LCM multicast group address. pub const DEFAULT_MULTICAST_GROUP: Ipv4Addr = Ipv4Addr::new(239, 255, 76, 67); @@ -35,9 +41,11 @@ pub const DEFAULT_PORT: u16 = 7667; static SEQ: AtomicU32 = AtomicU32::new(0); struct FragmentBuffer { - channel: String, - num_fragments: u16, - received: u16, + channel: Option, + fragments: Vec>, + received: usize, + received_bytes: usize, + created: Instant, data: Vec, } @@ -131,6 +139,18 @@ impl Lcm { /// Publish encoded message data on the given channel. pub async fn publish(&self, channel: &str, data: &[u8]) -> io::Result<()> { let channel_bytes = channel.as_bytes(); + if channel_bytes.len() > 63 || channel_bytes.contains(&0) { + return Err(io::Error::new( + io::ErrorKind::InvalidInput, + "LCM channel must fit in 63 bytes and contain no NUL", + )); + } + if data.len() > MAX_MESSAGE_SIZE { + return Err(io::Error::new( + io::ErrorKind::InvalidInput, + "LCM message exceeds the 64 MiB transport limit", + )); + } let total = SHORT_HEADER_SIZE + channel_bytes.len() + 1 + data.len(); let seqno = SEQ.fetch_add(1, Ordering::Relaxed); @@ -252,48 +272,111 @@ impl Lcm { let mut offset = FRAGMENT_HEADER_SIZE; - // First fragment carries the channel name + if total_size == 0 + || total_size > MAX_MESSAGE_SIZE + || num_fragments == 0 + || fragment_no >= num_fragments + || usize::from(num_fragments) > total_size + || (fragment_no == 0 && fragment_offset != 0) + { + return Ok(None); + } + + // Only fragment zero carries the channel. Reject invalid UTF-8 rather + // than routing a replacement-character channel to the wrong subscriber. let channel = if fragment_no == 0 { - let channel_end = match buf[offset..].iter().position(|&b| b == 0) { - Some(pos) => offset + pos, - None => return Ok(None), + let Some(pos) = buf[offset..].iter().position(|&b| b == 0) else { + return Ok(None); }; - let ch = String::from_utf8_lossy(&buf[offset..channel_end]).into_owned(); - offset = channel_end + 1; - Some(ch) + if pos > 63 { + return Ok(None); + } + let Ok(ch) = std::str::from_utf8(&buf[offset..offset + pos]) else { + return Ok(None); + }; + offset += pos + 1; + Some(ch.to_owned()) } else { None }; - let payload = &buf[offset..]; + let Some(end) = fragment_offset.checked_add(payload.len()) else { + return Ok(None); + }; + if payload.is_empty() || end > total_size { + return Ok(None); + } let key = (sender, seqno); - + let now = Instant::now(); let mut reassembly = self.reassembly.lock().unwrap(); + reassembly.retain(|_, entry| now.duration_since(entry.created) < REASSEMBLY_TIMEOUT); + if !reassembly.contains_key(&key) { + let used: usize = reassembly.values().map(|entry| entry.data.len()).sum(); + if reassembly.len() >= MAX_INCOMPLETE_MESSAGES + || total_size > MAX_REASSEMBLY_BYTES - used + { + return Ok(None); + } + } let entry = reassembly.entry(key).or_insert_with(|| FragmentBuffer { - channel: channel.clone().unwrap_or_default(), - num_fragments, + channel: None, + fragments: vec![None; usize::from(num_fragments)], received: 0, + received_bytes: 0, + created: now, data: vec![0u8; total_size], }); - - // First fragment also sets the channel name on an existing entry - if let Some(ch) = channel { - entry.channel = ch; + if entry.data.len() != total_size || entry.fragments.len() != usize::from(num_fragments) { + reassembly.remove(&key); + return Ok(None); } - - let end = (fragment_offset + payload.len()).min(total_size); - entry.data[fragment_offset..end].copy_from_slice(&payload[..end - fragment_offset]); + if let Some(range) = entry.fragments[usize::from(fragment_no)] { + // Repeated datagrams do not advance completion. Conflicting repeats + // invalidate this message instead of silently replacing its bytes. + if range != (fragment_offset, end) + || entry.data[fragment_offset..end] != *payload + || (fragment_no == 0 && entry.channel != channel) + { + reassembly.remove(&key); + } + return Ok(None); + } + if entry.received_bytes + payload.len() > total_size { + reassembly.remove(&key); + return Ok(None); + } + if channel.is_some() { + entry.channel = channel; + } + entry.data[fragment_offset..end].copy_from_slice(payload); + entry.fragments[usize::from(fragment_no)] = Some((fragment_offset, end)); entry.received += 1; + entry.received_bytes += payload.len(); - if entry.received == entry.num_fragments { + if entry.received == entry.fragments.len() { let complete = reassembly.remove(&key).unwrap(); - return Ok(Some(ReceivedMessage { - channel: complete.channel, - data: complete.data, - })); + if complete.received_bytes != total_size { + return Ok(None); + } + let mut ranges: Vec<_> = complete.fragments.into_iter().flatten().collect(); + ranges.sort_unstable(); + let mut expected = 0; + for (start, end) in ranges { + if start != expected { + return Ok(None); // overlap or hole + } + expected = end; + } + if expected == total_size { + if let Some(channel) = complete.channel { + return Ok(Some(ReceivedMessage { + channel, + data: complete.data, + })); + } + } } - Ok(None) } @@ -306,7 +389,13 @@ impl Lcm { Some(pos) => channel_start + pos, None => return Ok(None), }; - let channel = String::from_utf8_lossy(&buf[channel_start..channel_end]).into_owned(); + if channel_end - channel_start > 63 { + return Ok(None); + } + let Ok(channel) = std::str::from_utf8(&buf[channel_start..channel_end]) else { + return Ok(None); + }; + let channel = channel.to_owned(); let data = buf[channel_end + 1..].to_vec(); Ok(Some(ReceivedMessage { channel, data })) } @@ -316,6 +405,183 @@ impl Lcm { mod tests { use super::*; + #[tokio::test] + async fn invalid_channels_are_rejected_before_sending() { + let lcm = Lcm::with_options(LcmOptions { + port: 0, + ..Default::default() + }) + .await + .unwrap(); + for channel in ["x".repeat(64), "é".repeat(32), "invalid\0channel".into()] { + let error = lcm.publish(&channel, b"payload").await.unwrap_err(); + assert_eq!(error.kind(), io::ErrorKind::InvalidInput); + } + } + + async fn receiver() -> Lcm { + Lcm::with_options(LcmOptions { + port: 0, + ..Default::default() + }) + .await + .unwrap() + } + + fn fragment(seq: u32, size: u32, offset: u32, index: u16, count: u16, data: &[u8]) -> Vec { + let mut packet = vec![0; FRAGMENT_HEADER_SIZE]; + BigEndian::write_u32(&mut packet[0..4], MAGIC_LONG); + BigEndian::write_u32(&mut packet[4..8], seq); + BigEndian::write_u32(&mut packet[8..12], size); + BigEndian::write_u32(&mut packet[12..16], offset); + BigEndian::write_u16(&mut packet[16..18], index); + BigEndian::write_u16(&mut packet[18..20], count); + if index == 0 { + packet.extend_from_slice(b"CHAN\0"); + } + packet.extend_from_slice(data); + packet + } + + fn accept(lcm: &Lcm, packet: &[u8]) -> Option { + lcm.process_fragment("127.0.0.1:1234".parse().unwrap(), packet) + .unwrap() + } + + #[tokio::test] + async fn reordered_and_repeated_fragments_complete_once_with_exact_bytes() { + let lcm = receiver().await; + let last = fragment(1, 6, 4, 2, 3, b"ef"); + assert!(accept(&lcm, &last).is_none()); + assert!(accept(&lcm, &last).is_none()); + assert!(accept(&lcm, &fragment(1, 6, 0, 0, 3, b"ab")).is_none()); + let message = accept(&lcm, &fragment(1, 6, 2, 1, 3, b"cd")).unwrap(); + assert_eq!(message.channel, "CHAN"); + assert_eq!(message.data, b"abcdef"); + assert!(lcm.reassembly.lock().unwrap().is_empty()); + } + + #[tokio::test] + async fn invalid_headers_and_ranges_do_not_allocate_reassembly() { + let lcm = receiver().await; + for packet in [ + fragment(1, u32::MAX, 0, 0, 2, b"ab"), + fragment(1, 4, u32::MAX, 1, 2, b"cd"), + fragment(1, 4, 3, 1, 2, b"cd"), + fragment(1, 4, 0, 0, 0, b"ab"), + fragment(1, 4, 0, 2, 2, b"ab"), + fragment(1, 1, 0, 0, 2, b"a"), + fragment(1, 4, 1, 0, 2, b"ab"), + fragment(1, 4, 0, 0, 2, b""), + ] { + assert!(accept(&lcm, &packet).is_none()); + assert!(lcm.reassembly.lock().unwrap().is_empty()); + } + } + + #[tokio::test] + async fn conflicting_metadata_and_repeated_payloads_invalidate_message() { + for conflicting in [ + fragment(1, 5, 2, 1, 2, b"cd"), + fragment(1, 4, 2, 1, 3, b"cd"), + fragment(1, 4, 0, 0, 2, b"zz"), + ] { + let lcm = receiver().await; + assert!(accept(&lcm, &fragment(1, 4, 0, 0, 2, b"ab")).is_none()); + assert!(accept(&lcm, &conflicting).is_none()); + assert!(lcm.reassembly.lock().unwrap().is_empty()); + } + } + + #[tokio::test] + async fn overlaps_and_holes_never_produce_a_message() { + for second in [ + fragment(1, 4, 1, 1, 2, b"cd"), // overlap plus a hole + fragment(1, 4, 3, 1, 2, b"d"), // uncovered byte + fragment(1, 4, 1, 1, 2, b"cde"), // too many bytes + ] { + let lcm = receiver().await; + assert!(accept(&lcm, &fragment(1, 4, 0, 0, 2, b"ab")).is_none()); + assert!(accept(&lcm, &second).is_none()); + assert!(lcm.reassembly.lock().unwrap().is_empty()); + } + } + + #[tokio::test] + async fn incomplete_messages_are_bounded_and_expired_slots_are_reused() { + let lcm = receiver().await; + for seq in 0..MAX_INCOMPLETE_MESSAGES as u32 { + assert!(accept(&lcm, &fragment(seq, 4, 0, 0, 2, b"ab")).is_none()); + } + assert!(accept(&lcm, &fragment(100, 4, 0, 0, 2, b"ab")).is_none()); + assert_eq!( + lcm.reassembly.lock().unwrap().len(), + MAX_INCOMPLETE_MESSAGES + ); + { + let mut buffers = lcm.reassembly.lock().unwrap(); + for buffer in buffers.values_mut() { + buffer.created = Instant::now() - REASSEMBLY_TIMEOUT; + } + } + assert!(accept(&lcm, &fragment(100, 4, 0, 0, 2, b"ab")).is_none()); + assert_eq!(lcm.reassembly.lock().unwrap().len(), 1); + assert_eq!( + accept(&lcm, &fragment(100, 4, 2, 1, 2, b"cd")) + .unwrap() + .data, + b"abcd" + ); + } + + #[tokio::test] + async fn incomplete_payload_storage_is_bounded_across_senders() { + let lcm = receiver().await; + for seq in 0..2 { + assert!(accept(&lcm, &fragment(seq, MAX_MESSAGE_SIZE as u32, 0, 0, 2, b"a")).is_none()); + } + assert!(accept(&lcm, &fragment(3, 4, 0, 0, 2, b"ab")).is_none()); + let buffers = lcm.reassembly.lock().unwrap(); + assert_eq!(buffers.len(), 2); + assert_eq!( + buffers + .values() + .map(|entry| entry.data.len()) + .sum::(), + MAX_REASSEMBLY_BYTES + ); + } + + #[tokio::test] + async fn recv_continues_after_malformed_datagrams() { + let lcm = receiver().await; + let sender = UdpSocket::bind("127.0.0.1:0").await.unwrap(); + let destination = + SocketAddrV4::new(Ipv4Addr::LOCALHOST, lcm.socket.local_addr().unwrap().port()); + for packet in [ + fragment(1, 4, u32::MAX, 1, 2, b"cd"), + make_small_packet(&[0xff], b"invalid channel"), + make_small_packet(b"GOOD", b"intact"), + ] { + sender.send_to(&packet, destination).await.unwrap(); + } + let message = tokio::time::timeout(Duration::from_secs(2), lcm.recv()) + .await + .unwrap() + .unwrap(); + assert_eq!(message.channel, "GOOD"); + assert_eq!(message.data, b"intact"); + } + + #[test] + fn invalid_short_channels_are_dropped() { + for channel in [vec![b'x'; 64], vec![0xff]] { + assert!(Lcm::decode_small(&make_small_packet(&channel, b"data")) + .unwrap() + .is_none()); + } + } + const TEST_CHANNEL_LEN: usize = 13; // "/test_channel" const FIRST_PAYLOAD: usize = MAX_DATAGRAM_SIZE - FRAGMENT_HEADER_SIZE - TEST_CHANNEL_LEN - 1; const SUBSEQUENT_PAYLOAD: usize = MAX_DATAGRAM_SIZE - FRAGMENT_HEADER_SIZE; diff --git a/native/rust/dimos-module-macros/src/lib.rs b/native/rust/dimos-module-macros/src/lib.rs index af89a0680b4..46d822af9d3 100644 --- a/native/rust/dimos-module-macros/src/lib.rs +++ b/native/rust/dimos-module-macros/src/lib.rs @@ -428,7 +428,7 @@ struct PortDecl { } /// The last `::`-separated segment of the single generic argument of -/// `Input`/`Output`, e.g. `Input` -> `PointCloud2`. +/// `Input`/`Output`, e.g. `Input` -> `PointCloud2`. fn port_payload_type(ty: &Type) -> Option { let Type::Path(p) = ty else { return None }; let segment = p.path.segments.last()?; @@ -537,7 +537,7 @@ fn check_port_table(entry: &toml::Value, kind: &str, declared: &[PortDecl]) -> R let msg = msg .as_str() .ok_or_else(|| format!("`{kind}.{}` must be a string", port.name))?; - let want = msg.rsplit('.').next().unwrap_or(msg); + let want = msg.rsplit(['/', '.']).next().unwrap_or(msg); if want != port.ty { return Err(format!( "port `{}` is declared `{msg}` in `{kind}` but the struct field carries `{}`", @@ -790,6 +790,18 @@ mod tests { .expect("matching ports should pass"); } + #[test] + fn accepts_canonical_ros_message_names() { + let manifest = MANIFEST.replace("sensor_msgs.PointCloud2", "sensor_msgs/msg/PointCloud2"); + check_manifest_ports( + &manifest, + "demo", + &[port("lidar", "PointCloud2")], + &[port("global_map", "PointCloud2")], + ) + .expect("canonical generated message contracts should pass"); + } + #[test] fn rejects_missing_metadata_section() { let err = check_manifest_ports(MANIFEST, "other", &[], &[]) @@ -868,7 +880,8 @@ mod tests { #[test] fn payload_type_is_the_last_generic_segment() { - let ty: syn::Type = parse_str("Input").unwrap(); + let ty: syn::Type = + parse_str("Input").unwrap(); assert_eq!(port_payload_type(&ty).as_deref(), Some("PointCloud2")); let ty: syn::Type = parse_str("Output").unwrap(); assert_eq!(port_payload_type(&ty).as_deref(), Some("Path")); diff --git a/native/rust/dimos-module/Cargo.toml b/native/rust/dimos-module/Cargo.toml index c09298e6178..83e2647aa60 100644 --- a/native/rust/dimos-module/Cargo.toml +++ b/native/rust/dimos-module/Cargo.toml @@ -6,9 +6,9 @@ description = "Rust native module SDK for dimos NativeModule framework" license = "Apache-2.0" [dependencies] -dimos-lcm = { git = "https://github.com/dimensionalOS/dimos-lcm.git", branch = "rust-codegen" } +dimos-lcm-transport = { path = "../dimos-lcm-transport", version = "0.1.0" } dimos-module-macros = { version = "=0.1.0", path = "../dimos-module-macros" } -lcm-msgs = { git = "https://github.com/dimensionalOS/dimos-lcm.git", branch = "rust-codegen" } +dimos-generated-messages = { version = "0.1.0", path = "../../../dimos/message_codegen" } nalgebra = "0.35.0" rayon = "1" tokio = { version = "1", features = ["rt-multi-thread", "macros", "sync", "time", "signal", "io-std", "io-util"] } diff --git a/native/rust/dimos-module/src/cdr.rs b/native/rust/dimos-module/src/cdr.rs new file mode 100644 index 00000000000..9749b6e0c17 --- /dev/null +++ b/native/rust/dimos-module/src/cdr.rs @@ -0,0 +1,73 @@ +// Copyright 2026 Dimensional Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// Copyright 2026 Dimensional Inc. +// SPDX-License-Identifier: Apache-2.0 + +//! Generated CDR codecs for native module ports. +use dimos_generated_messages::codec::Message; +use std::io; + +pub fn encode(message: &T) -> io::Result> { + message.encode() +} + +pub fn decode(bytes: &[u8]) -> io::Result { + T::decode(bytes) +} + +#[cfg(test)] +mod tests { + use super::*; + use dimos_generated_messages::{geometry_msgs::msg::PoseStamped, std_msgs::msg::Int32}; + + #[test] + fn nested_fields_and_both_byte_orders() { + let mut message = PoseStamped::default(); + message.header.frame_id = "map".into(); + message.header.stamp.sec = 1700000000; + message.header.stamp.nanosec = 123456789; + message.pose.position.x = 1.5; + message.pose.orientation.w = 1.0; + assert_eq!( + decode::(&encode(&message).unwrap()).unwrap(), + message + ); + assert_eq!( + decode::(&message.encode_endian(false).unwrap()).unwrap(), + message + ); + } + + #[test] + fn malformed_payload_is_a_decode_error() { + let mut bytes = encode(&Int32 { data: 1234 }).unwrap(); + assert_eq!( + decode::(&bytes[..bytes.len() - 1]) + .unwrap_err() + .kind(), + io::ErrorKind::InvalidData + ); + bytes.push(0); + assert_eq!( + decode::(&bytes).unwrap_err().kind(), + io::ErrorKind::InvalidData + ); + bytes[0] = 255; + assert_eq!( + decode::(&bytes).unwrap_err().kind(), + io::ErrorKind::InvalidData + ); + } +} diff --git a/native/rust/dimos-module/src/host.rs b/native/rust/dimos-module/src/host.rs index cc7982b7e55..fa01c48f320 100644 --- a/native/rust/dimos-module/src/host.rs +++ b/native/rust/dimos-module/src/host.rs @@ -891,7 +891,7 @@ mod tests { fn build(builder: &mut Builder, config: ProbeConfig) -> Self { Self { key: config.key, - ping: builder.output("ping", |b: &Vec| b.clone()), + ping: builder.output("ping", |b: &Vec| Ok(b.clone())), } } diff --git a/native/rust/dimos-module/src/lcm.rs b/native/rust/dimos-module/src/lcm.rs index 6fe38382538..b8f0704acc2 100644 --- a/native/rust/dimos-module/src/lcm.rs +++ b/native/rust/dimos-module/src/lcm.rs @@ -18,12 +18,12 @@ use std::sync::atomic::{AtomicBool, Ordering}; use std::sync::{Arc, Mutex}; use std::time::Duration; -use dimos_lcm::{Lcm, LcmOptions}; +use dimos_lcm_transport::{Lcm, LcmOptions}; use url::Url; use crate::transport::{Dispatch, Transport}; -/// LCM UDP multicast transport. Wraps `dimos_lcm::Lcm`. +/// LCM UDP multicast transport. Wraps `dimos_lcm_transport::Lcm`. /// /// The multicast socket receives every channel, so `subscribe` registers a /// callback locally and one recv loop routes each message by channel. @@ -184,12 +184,12 @@ mod tests { fn ttl_is_optional() { let options = options_from_url("udpm://239.255.76.67:7712"); assert_eq!(options.port, 7712); - assert_eq!(options.ttl, dimos_lcm::LcmOptions::default().ttl); + assert_eq!(options.ttl, dimos_lcm_transport::LcmOptions::default().ttl); } #[test] fn an_unusable_url_leaves_the_defaults() { - let defaults = dimos_lcm::LcmOptions::default(); + let defaults = dimos_lcm_transport::LcmOptions::default(); for url in [ "tcp://127.0.0.1:7667", "udpm://not-an-ip:7667?ttl=42", diff --git a/native/rust/dimos-module/src/lib.rs b/native/rust/dimos-module/src/lib.rs index a1a6f6efd64..b2ca2a30f39 100644 --- a/native/rust/dimos-module/src/lib.rs +++ b/native/rust/dimos-module/src/lib.rs @@ -17,10 +17,12 @@ #[cfg(test)] extern crate self as dimos_module; +pub mod cdr; pub mod host; pub mod lcm; pub mod log; pub mod module; +pub mod pointcloud; pub mod tf; pub mod transport; pub mod workers; @@ -37,8 +39,8 @@ pub use zenoh::ZenohTransport; pub use nalgebra; -// Re-export LcmOptions so callers don't need to depend on dimos-lcm directly. -pub use dimos_lcm::LcmOptions; +// Re-export LcmOptions so callers don't need to depend on the raw transport crate directly. +pub use dimos_lcm_transport::LcmOptions; /// Run module `M` over the transport named by the `DIMOS_TRANSPORT` env var. /// diff --git a/native/rust/dimos-module/src/module.rs b/native/rust/dimos-module/src/module.rs index 23fe965ba36..1f4cae7b8e9 100644 --- a/native/rust/dimos-module/src/module.rs +++ b/native/rust/dimos-module/src/module.rs @@ -123,13 +123,13 @@ impl Input { #[derive(Clone)] pub struct Output { pub topic: String, - encode: fn(&T) -> Vec, + encode: fn(&T) -> io::Result>, sender: mpsc::Sender>, } impl Output { pub async fn publish(&self, msg: &T) -> io::Result<()> { - publish_encoded(&self.sender, (self.encode)(msg)).await + publish_encoded(&self.sender, (self.encode)(msg)?).await } } @@ -140,7 +140,7 @@ impl Output { pub struct Io { pub topic: String, receiver: mpsc::Receiver, - encode: fn(&T) -> Vec, + encode: fn(&T) -> io::Result>, sender: mpsc::Sender>, } @@ -150,7 +150,7 @@ impl Io { } pub async fn publish(&self, msg: &T) -> io::Result<()> { - publish_encoded(&self.sender, (self.encode)(msg)).await + publish_encoded(&self.sender, (self.encode)(msg)?).await } } @@ -386,7 +386,7 @@ impl Builder { Input { topic, receiver } } - pub fn output(&mut self, port: &str, encode: fn(&T) -> Vec) -> Output { + pub fn output(&mut self, port: &str, encode: fn(&T) -> io::Result>) -> Output { let topic = self.topic_for(port); let sender = self.add_publisher(&topic); Output { @@ -401,7 +401,7 @@ impl Builder { &mut self, port: &str, decode: fn(&[u8]) -> io::Result, - encode: fn(&T) -> Vec, + encode: fn(&T) -> io::Result>, ) -> Io { let topic = self.topic_for(port); let receiver = self.add_route(&topic, decode); @@ -942,15 +942,52 @@ mod tests { #[test] fn output_uses_mapped_topic() { let mut builder = builder_with_topics(&[("cmd_vel", "/robot/cmd_vel")]); - let output = builder.output("cmd_vel", |b: &Vec| b.clone()); + let output = builder.output("cmd_vel", |b: &Vec| Ok(b.clone())); assert_eq!(output.topic, "/robot/cmd_vel"); } + #[tokio::test] + async fn encoding_failure_does_not_enqueue_a_payload() { + let mut builder = builder_with_topics(&[("cmd", "/robot/cmd")]); + let output = builder.output("cmd", |_: &Vec| { + Err(io::Error::new( + io::ErrorKind::InvalidInput, + "field exceeds bound", + )) + }); + assert_eq!( + output.publish(&vec![1]).await.unwrap_err().kind(), + io::ErrorKind::InvalidInput + ); + assert!(matches!( + builder.outputs[0].1.try_recv(), + Err(mpsc::error::TryRecvError::Empty) + )); + let port = builder.io( + "cmd", + |b| Ok(b.to_vec()), + |_: &Vec| { + Err(io::Error::new( + io::ErrorKind::InvalidInput, + "field exceeds bound", + )) + }, + ); + assert_eq!( + port.publish(&vec![1]).await.unwrap_err().kind(), + io::ErrorKind::InvalidInput + ); + assert!(matches!( + builder.outputs[1].1.try_recv(), + Err(mpsc::error::TryRecvError::Empty) + )); + } + #[test] fn topics_matching_ports_exactly_pass() { let mut builder = builder_with_topics(&[("cmd", "/robot/cmd"), ("odom", "/robot/odom")]); builder.input("cmd", |b| Ok(b.to_vec())); - builder.output("odom", |b: &Vec| b.clone()); + builder.output("odom", |b: &Vec| Ok(b.clone())); builder.enforce_topics_match_ports().expect("exact match"); } @@ -958,7 +995,7 @@ mod tests { fn a_port_the_coordinator_never_sent_is_rejected() { let mut builder = builder_with_topics(&[("cmd", "/robot/cmd")]); builder.input("cmd", |b| Ok(b.to_vec())); - builder.output("odom", |b: &Vec| b.clone()); + builder.output("odom", |b: &Vec| Ok(b.clone())); let err = builder .enforce_topics_match_ports() .expect_err("odom has no topic"); @@ -993,14 +1030,14 @@ mod tests { #[test] fn io_uses_mapped_topic() { let mut builder = builder_with_topics(&[("cmd", "/robot/cmd")]); - let io = builder.io("cmd", |b| Ok(b.to_vec()), |b: &Vec| b.clone()); + let io = builder.io("cmd", |b| Ok(b.to_vec()), |b: &Vec| Ok(b.clone())); assert_eq!(io.topic, "/robot/cmd"); } #[test] fn io_registers_one_route_and_one_publisher_on_the_same_topic() { let mut builder = builder_with_topics(&[("cmd", "/robot/cmd")]); - let _io = builder.io("cmd", |b| Ok(b.to_vec()), |b: &Vec| b.clone()); + let _io = builder.io("cmd", |b| Ok(b.to_vec()), |b: &Vec| Ok(b.clone())); assert_eq!(builder.routes.get("/robot/cmd").map(Vec::len), Some(1)); assert_eq!(builder.outputs.len(), 1); assert_eq!(builder.outputs[0].0, "/robot/cmd"); @@ -1009,7 +1046,7 @@ mod tests { #[tokio::test] async fn io_receives_on_its_route_and_publishes_to_its_queue() { let mut builder = builder_with_topics(&[("cmd", "/robot/cmd")]); - let mut io = builder.io("cmd", |b| Ok(b.to_vec()), |b: &Vec| b.clone()); + let mut io = builder.io("cmd", |b| Ok(b.to_vec()), |b: &Vec| Ok(b.clone())); builder.routes["/robot/cmd"][0].try_dispatch(b"inbound"); assert_eq!(io.recv().await.expect("inbound message"), b"inbound"); @@ -1022,7 +1059,7 @@ mod tests { #[tokio::test] async fn io_publish_errors_when_the_publish_worker_is_gone() { let mut builder = builder_with_topics(&[]); - let io = builder.io("cmd", |b| Ok(b.to_vec()), |b: &Vec| b.clone()); + let io = builder.io("cmd", |b| Ok(b.to_vec()), |b: &Vec| Ok(b.clone())); builder.outputs.clear(); let err = io .publish(&b"x".to_vec()) @@ -1065,7 +1102,7 @@ mod tests { let mut builder = Builder::new(topics(&[("data", "/data"), ("out", "/out")])); let _input = builder.input("data", |b| Ok(b.to_vec())); - let output = builder.output("out", |b: &Vec| b.clone()); + let output = builder.output("out", |b: &Vec| Ok(b.clone())); subscribe_routes(&transport, builder.routes).await.unwrap(); let transport = Arc::new(transport); @@ -1115,7 +1152,7 @@ mod tests { } Ok(b.to_vec()) }); - let output = builder.output("out", |b: &Vec| b.clone()); + let output = builder.output("out", |b: &Vec| Ok(b.clone())); subscribe_routes(&transport, builder.routes).await.unwrap(); let transport = Arc::new(transport); @@ -1166,8 +1203,8 @@ mod tests { }); let mut builder = Builder::new(topics(&[("block_out", "/block"), ("fast_out", "/fast")])); - let block_out = builder.output("block_out", |b: &Vec| b.clone()); - let fast_out = builder.output("fast_out", |b: &Vec| b.clone()); + let block_out = builder.output("block_out", |b: &Vec| Ok(b.clone())); + let fast_out = builder.output("fast_out", |b: &Vec| Ok(b.clone())); let _pub_tasks = spawn_publish_tasks(Arc::clone(&transport), builder.outputs); // Wedge the block channel, then publish on the fast channel. @@ -1278,8 +1315,8 @@ mod tests { Ok(Msg(bytes.to_vec())) } - fn encode(msg: &Msg) -> Vec { - msg.0.clone() + fn encode(msg: &Msg) -> io::Result> { + Ok(msg.0.clone()) } #[derive(crate::Module)] diff --git a/native/rust/dimos-module/src/pointcloud.rs b/native/rust/dimos-module/src/pointcloud.rs new file mode 100644 index 00000000000..d8a09772987 --- /dev/null +++ b/native/rust/dimos-module/src/pointcloud.rs @@ -0,0 +1,197 @@ +// Copyright 2026 Dimensional Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// Copyright 2026 Dimensional Inc. +// SPDX-License-Identifier: Apache-2.0 + +//! Geometry extraction kept separate from generated message values and codecs. +use std::io; + +use dimos_generated_messages::sensor_msgs::msg::{PointCloud2, PointField}; + +fn invalid(reason: &str) -> io::Error { + io::Error::new(io::ErrorKind::InvalidData, reason) +} + +/// Read finite XYZ points in row order, respecting field offsets, row padding, +/// float32/float64 coordinates and byte order. Other fields are left untouched. +pub fn xyz_points(message: &PointCloud2) -> io::Result> { + let mut coordinates = Vec::with_capacity(3); + let step = message.point_step as usize; + for name in ["x", "y", "z"] { + let mut matching = message.fields.iter().filter(|field| field.name == name); + let field = matching + .next() + .ok_or_else(|| invalid("missing XYZ field"))?; + if matching.next().is_some() || field.count != 1 { + return Err(invalid("XYZ fields must be unique and scalar")); + } + let size = match field.datatype { + PointField::FLOAT32 => 4, + PointField::FLOAT64 => 8, + _ => return Err(invalid("XYZ fields must be float32 or float64")), + }; + let offset = field.offset as usize; + if offset.checked_add(size).is_none_or(|end| end > step) { + return Err(invalid("XYZ field exceeds point_step")); + } + coordinates.push((offset, size)); + } + let width = message.width as usize; + let height = message.height as usize; + let row_step = message.row_step as usize; + let row_bytes = width + .checked_mul(step) + .ok_or_else(|| invalid("row size overflow"))?; + let total = height + .checked_mul(row_step) + .ok_or_else(|| invalid("data size overflow"))?; + if step == 0 || row_step < row_bytes || total != message.data.len() { + return Err(invalid("point cloud dimensions/strides do not match data")); + } + let count = width + .checked_mul(height) + .ok_or_else(|| invalid("point count overflow"))?; + let read = |base: usize, (offset, size): (usize, usize)| -> f32 { + let bytes = &message.data[base + offset..base + offset + size]; + match (size, message.is_bigendian) { + (4, false) => f32::from_le_bytes(bytes.try_into().unwrap()), + (4, true) => f32::from_be_bytes(bytes.try_into().unwrap()), + (8, false) => f64::from_le_bytes(bytes.try_into().unwrap()) as f32, + (8, true) => f64::from_be_bytes(bytes.try_into().unwrap()) as f32, + _ => unreachable!("field size checked above"), + } + }; + let mut points = Vec::with_capacity(count); + for row in 0..height { + for column in 0..width { + let base = row * row_step + column * step; + let (x, y, z) = ( + read(base, coordinates[0]), + read(base, coordinates[1]), + read(base, coordinates[2]), + ); + if x.is_finite() && y.is_finite() && z.is_finite() { + points.push((x, y, z)); + } + } + } + Ok(points) +} + +#[cfg(test)] +mod tests { + use super::*; + + fn cloud(big: bool) -> PointCloud2 { + let mut data = vec![0; 64]; + for row in 0..2 { + let x = row as f32 + 1.0; + let z = row as f64 + 3.0; + data[row * 32..row * 32 + 4].copy_from_slice(&if big { + x.to_be_bytes() + } else { + x.to_le_bytes() + }); + data[row * 32 + 8..row * 32 + 16].copy_from_slice(&if big { + z.to_be_bytes() + } else { + z.to_le_bytes() + }); + data[row * 32 + 16..row * 32 + 20].copy_from_slice(&if big { + 2f32.to_be_bytes() + } else { + 2f32.to_le_bytes() + }); + } + PointCloud2 { + width: 1, + height: 2, + point_step: 24, + row_step: 32, + data, + is_bigendian: big, + fields: vec![ + PointField { + name: "z".into(), + offset: 8, + datatype: PointField::FLOAT64, + count: 1, + }, + PointField { + name: "x".into(), + offset: 0, + datatype: PointField::FLOAT32, + count: 1, + }, + PointField { + name: "y".into(), + offset: 16, + datatype: PointField::FLOAT32, + count: 1, + }, + ], + ..Default::default() + } + } + + #[test] + fn organized_padded_mixed_precision_both_byte_orders() { + for big in [false, true] { + assert_eq!( + xyz_points(&cloud(big)).unwrap(), + vec![(1., 2., 3.), (2., 2., 4.)] + ); + } + } + + #[test] + fn invalid_layouts_fail_before_reading() { + let mut bad = Vec::new(); + let mut message = cloud(false); + message.row_step = 23; + bad.push(message); + let mut message = cloud(false); + message.data.pop(); + bad.push(message); + let mut message = cloud(false); + message.fields[0].offset = 20; + bad.push(message); + let mut message = cloud(false); + message.fields[0].count = 2; + bad.push(message); + let mut message = cloud(false); + message.fields[0].datatype = PointField::UINT32; + bad.push(message); + let mut message = cloud(false); + message.fields.push(message.fields[0].clone()); + bad.push(message); + let mut message = cloud(false); + message.fields.remove(0); + bad.push(message); + for message in bad { + assert_eq!( + xyz_points(&message).unwrap_err().kind(), + io::ErrorKind::InvalidData + ); + } + } + + #[test] + fn nonfinite_coordinates_are_skipped() { + let mut message = cloud(false); + message.data[..4].copy_from_slice(&f32::NAN.to_le_bytes()); + assert_eq!(xyz_points(&message).unwrap(), vec![(2., 2., 4.)]); + } +} diff --git a/native/rust/dimos-module/src/tf.rs b/native/rust/dimos-module/src/tf.rs index 15ffcd5a26b..1d32f6ad63b 100644 --- a/native/rust/dimos-module/src/tf.rs +++ b/native/rust/dimos-module/src/tf.rs @@ -25,6 +25,7 @@ use std::sync::atomic::AtomicU64; use std::sync::{Arc, Mutex, RwLock}; use std::time::{Duration, SystemTime, UNIX_EPOCH}; +use dimos_generated_messages::codec::Message; use nalgebra::{Isometry3, Quaternion, Translation3, UnitQuaternion, Vector3}; use tokio::sync::{mpsc, Notify}; use tracing::warn; @@ -390,10 +391,11 @@ impl Tf { buffer.receive(&t.parent, &t.child, t.ts, t.iso); } }); - let msg = lcm_msgs::tf2_msgs::TFMessage { + let msg = dimos_generated_messages::tf2_msgs::msg::TFMessage { transforms: transforms.iter().map(to_stamped).collect(), }; - crate::module::publish_encoded(&self.sender, msg.encode()).await + let bytes = msg.encode()?; + crate::module::publish_encoded(&self.sender, bytes).await } } @@ -486,7 +488,7 @@ impl Lookup<'_> { } } -fn to_stamped(t: &Transform) -> lcm_msgs::geometry_msgs::TransformStamped { +fn to_stamped(t: &Transform) -> dimos_generated_messages::geometry_msgs::msg::TransformStamped { let mut sec = t.ts.floor(); let mut nsec = ((t.ts - sec) * 1e9).round(); if nsec >= 1e9 { @@ -495,23 +497,22 @@ fn to_stamped(t: &Transform) -> lcm_msgs::geometry_msgs::TransformStamped { } let p = t.iso.translation.vector; let q = t.iso.rotation; - lcm_msgs::geometry_msgs::TransformStamped { - header: lcm_msgs::std_msgs::Header { - seq: 0, - stamp: lcm_msgs::std_msgs::Time { + dimos_generated_messages::geometry_msgs::msg::TransformStamped { + header: dimos_generated_messages::std_msgs::msg::Header { + stamp: dimos_generated_messages::builtin_interfaces::msg::Time { sec: sec as i32, - nsec: nsec as i32, + nanosec: nsec as u32, }, frame_id: t.parent.clone(), }, child_frame_id: t.child.clone(), - transform: lcm_msgs::geometry_msgs::Transform { - translation: lcm_msgs::geometry_msgs::Vector3 { + transform: dimos_generated_messages::geometry_msgs::msg::Transform { + translation: dimos_generated_messages::geometry_msgs::msg::Vector3 { x: p.x, y: p.y, z: p.z, }, - rotation: lcm_msgs::geometry_msgs::Quaternion { + rotation: dimos_generated_messages::geometry_msgs::msg::Quaternion { x: q.i, y: q.j, z: q.k, @@ -530,7 +531,7 @@ struct TfRoute { impl Route for TfRoute { fn try_dispatch(&self, data: &[u8]) { - let msg = match lcm_msgs::tf2_msgs::TFMessage::decode(data) { + let msg = match dimos_generated_messages::tf2_msgs::msg::TFMessage::decode(data) { Ok(msg) => msg, Err(e) => { crate::error_throttled!( @@ -560,7 +561,7 @@ impl Route for TfRoute { continue; }; let iso = Isometry3::from_parts(Translation3::new(t.x, t.y, t.z), rotation); - let ts = st.header.stamp.sec as f64 + st.header.stamp.nsec as f64 * 1e-9; + let ts = st.header.stamp.sec as f64 + st.header.stamp.nanosec as f64 * 1e-9; buffer.receive(&st.header.frame_id, &st.child_frame_id, ts, iso); } }); @@ -875,18 +876,18 @@ mod tests { x: f64, quat: (f64, f64, f64, f64), ) -> Vec { - use lcm_msgs::geometry_msgs::{ + use dimos_generated_messages::builtin_interfaces::msg::Time; + use dimos_generated_messages::geometry_msgs::msg::{ Quaternion as LQuat, Transform as LTransform, TransformStamped, Vector3 as LVec3, }; - use lcm_msgs::std_msgs::{Header, Time}; + use dimos_generated_messages::std_msgs::msg::Header; let (x_q, y_q, z_q, w_q) = quat; - lcm_msgs::tf2_msgs::TFMessage { + dimos_generated_messages::tf2_msgs::msg::TFMessage { transforms: vec![TransformStamped { header: Header { - seq: 0, stamp: Time { sec: ts as i32, - nsec: 0, + nanosec: 0, }, frame_id: parent.to_string(), }, @@ -903,6 +904,7 @@ mod tests { }], } .encode() + .unwrap() } #[test] @@ -1026,41 +1028,43 @@ mod tests { #[test] fn tf_route_decodes_into_graph() { - use lcm_msgs::geometry_msgs::{ + use dimos_generated_messages::builtin_interfaces::msg::Time; + use dimos_generated_messages::geometry_msgs::msg::{ Quaternion as LQuat, Transform as LTransform, Vector3 as LVec3, }; - use lcm_msgs::std_msgs::{Header, Time}; - use lcm_msgs::tf2_msgs::TFMessage; + use dimos_generated_messages::std_msgs::msg::Header; + use dimos_generated_messages::tf2_msgs::msg::TFMessage; let (tx, _rx) = mpsc::channel(8); let (tf, route) = tf_subscription("/tf".to_string(), DEFAULT_TF_WINDOW_SECS, tx); let msg = TFMessage { - transforms: vec![lcm_msgs::geometry_msgs::TransformStamped { - header: Header { - seq: 0, - stamp: Time { - sec: 5, - nsec: 500_000_000, + transforms: vec![ + dimos_generated_messages::geometry_msgs::msg::TransformStamped { + header: Header { + stamp: Time { + sec: 5, + nanosec: 500_000_000, + }, + frame_id: "base_link".to_string(), }, - frame_id: "base_link".to_string(), - }, - child_frame_id: "mid360_link".to_string(), - transform: LTransform { - translation: LVec3 { - x: 0.1, - y: 0.2, - z: 0.3, - }, - rotation: LQuat { - x: 0.0, - y: 0.0, - z: 0.0, - w: 1.0, + child_frame_id: "mid360_link".to_string(), + transform: LTransform { + translation: LVec3 { + x: 0.1, + y: 0.2, + z: 0.3, + }, + rotation: LQuat { + x: 0.0, + y: 0.0, + z: 0.0, + w: 1.0, + }, }, }, - }], + ], }; - route.try_dispatch(&msg.encode()); + route.try_dispatch(&msg.encode().unwrap()); let t = tf.get_latest("base_link", "mid360_link").unwrap(); assert!((t.translation().x - 0.1).abs() < 1e-9); @@ -1122,7 +1126,7 @@ mod tests { Isometry3::identity(), )); assert_eq!(st.header.stamp.sec, 2); - assert_eq!(st.header.stamp.nsec, 0); + assert_eq!(st.header.stamp.nanosec, 0); } #[test] diff --git a/openspec/changes/replace-lcm-message-encoding/tasks.md b/openspec/changes/replace-lcm-message-encoding/tasks.md index 6da65858296..35d029dfbdf 100644 --- a/openspec/changes/replace-lcm-message-encoding/tasks.md +++ b/openspec/changes/replace-lcm-message-encoding/tasks.md @@ -36,7 +36,7 @@ - [ ] 4.1 Generate the complete inventory of standard and DimOS custom messages; replace the message protocol and generated-type discovery with the transport-neutral codec and schema contract. - [ ] 4.2 Extract geometry, timestamp, NumPy/Open3D, and Rerun helpers; update callers to nested ROS2 fields, sec/nanosec timestamps, and distinct TransformStamped/TFMessage representations without compatibility wrappers. -- [ ] 4.3 Update Python typed LCM/Zenoh pub/sub and typed SHM paths to CDR, preserving raw transport and separate Python-object serialization behavior. +- [x] 4.3 Update Python typed LCM/Zenoh pub/sub and typed SHM paths to CDR, preserving raw transport and separate Python-object serialization behavior. - [ ] 4.4 Update C++ and Rust SDK codecs, native examples, and typed worker/module configuration to the generated types; integrate the proven Rust raw transport replacement with preserved provenance. - [ ] 4.5 Update image and point-cloud producers/consumers to generated fields, proper CompressedImage use, arbitrary PointCloud2 layouts, and explicit conversion helpers. - [ ] 4.6 Update remaining robot/perception/navigation/tool consumers and existing optional ROS bridge conversions; replace affected repository fixtures rather than retaining LCM fallback decoding. diff --git a/packages/dimos-generated/MANIFEST.in b/packages/dimos-generated/MANIFEST.in new file mode 100644 index 00000000000..8c53d338a4d --- /dev/null +++ b/packages/dimos-generated/MANIFEST.in @@ -0,0 +1,4 @@ +include pyproject.toml setup.py +graft _codegen +prune build +global-exclude __pycache__ *.pyc diff --git a/packages/dimos-generated/pyproject.toml b/packages/dimos-generated/pyproject.toml new file mode 100644 index 00000000000..79bcc9a8c3e --- /dev/null +++ b/packages/dimos-generated/pyproject.toml @@ -0,0 +1,12 @@ +[build-system] +requires = ["setuptools>=70", "wheel", "pybind11==3.0.1", "cmake>=3.20"] +build-backend = "setuptools.build_meta" + +[project] +name = "dimos-generated" +version = "0.1.0" +description = "Prebuilt DimOS message values and complete ROS2 schemas" +requires-python = ">=3.10" + +[project.entry-points."dimos.messages"] +dimos_generated = "dimos_generated_schemas.provider" diff --git a/packages/dimos-generated/setup.py b/packages/dimos-generated/setup.py new file mode 100644 index 00000000000..438088beffc --- /dev/null +++ b/packages/dimos-generated/setup.py @@ -0,0 +1,68 @@ +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Maintainer source build for the independently distributed built-in messages.""" + +from pathlib import Path +import shutil +import sys + +from setuptools import setup +from setuptools.command.sdist import sdist as _sdist + +ROOT = Path(__file__).resolve().parent +BUNDLED = ROOT / "_codegen" +REPOSITORY = ROOT.parents[1] +sys.path.insert(0, str(BUNDLED if BUNDLED.is_dir() else REPOSITORY)) +from dimos.message_codegen.distribution import write_distribution +from dimos.message_codegen.generate import generate +from dimos.message_codegen.native_build import MessageBuildExt, MessageExtension + +OUTPUT = ROOT / "build" / "messages" +generate([], OUTPUT, shared=True) +write_distribution(OUTPUT, "dimos_generated", (), shared=True) + + +class SourceDistribution(_sdist): + def make_release_tree(self, base_dir, files): + super().make_release_tree(base_dir, files) + target = Path(base_dir) / "_codegen" / "dimos" / "message_codegen" + source = ( + BUNDLED / "dimos" / "message_codegen" + if BUNDLED.is_dir() + else REPOSITORY / "dimos" / "message_codegen" + ) + shutil.copytree( + source, + target, + ignore=shutil.ignore_patterns( + "build", "dist", "*.egg-info", "__pycache__", "test_*.py", "target" + ), + ) + (target.parent / "__init__.py").write_text("") + + +setup( + packages=["dimos_generated_schemas"], + package_dir={"": "build/messages/python"}, + package_data={"dimos_generated_schemas": ["schemas.json", "schemas/**/*", "package/**/*"]}, + ext_modules=[ + MessageExtension( + "dimos_generated", + OUTPUT / "cpp", + REPOSITORY / "build/message-codegen/install" if not BUNDLED.is_dir() else None, + ) + ], + cmdclass={"build_ext": MessageBuildExt, "sdist": SourceDistribution}, +) diff --git a/pyproject.toml b/pyproject.toml index a74192b58fd..144e8d5b982 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -102,8 +102,10 @@ classifiers = [ ] dependencies = [ + "dimos-generated==0.1.0", + "dimos-message-build==0.1.0", # Transport Protocols - "dimos-lcm>=0.1.3", + "lcm-dimos-fork>=1.5.2.post1", # Raw LCM transport; generated messages use CDR. "eclipse-zenoh>=1.10.1,<2.0", "PyTurboJPEG==1.8.2", "imagecodecs>=2024.6.1", # JPEG-XL for CompressedImage @@ -166,9 +168,6 @@ dependencies = [ ] -[project.entry-points."dimos.messages"] -builtin = "dimos.message_codegen.builtin_provider" - [project.scripts] lcmspy = "dimos.cli.spy.run_spy:lcm_main" agentspy = "dimos.cli.agentspy.agentspy:main" @@ -279,7 +278,7 @@ unitree-dds = [ control = [ "piper-sdk", "xarm-python-sdk>=1.17.0", - "a750-control; sys_platform == 'linux' and platform_machine == 'x86_64'", + "a750-control; python_version >= '3.12' and sys_platform == 'linux' and platform_machine == 'x86_64'", "can-motor-control>=0.0.8", "python-can>=4", "pygame>=2.6.1", @@ -519,7 +518,7 @@ dev = [ required-version = ">=0.9.25" default-groups = ["tests"] exclude-newer = "7 days" -exclude-newer-package = { dimos-viewer = false, dimos-lcm = false, lcm-dimos-fork = false, roboplan = false, md-babel-py = false, can-motor-control = false, edgetam-dimos = false, dim-odom = false, eclipse-zenoh = false } +exclude-newer-package = { dimos-viewer = false, lcm-dimos-fork = false, roboplan = false, md-babel-py = false, can-motor-control = false, edgetam-dimos = false, dim-odom = false, eclipse-zenoh = false } constraint-dependencies = [ # TODO(PY311): drop this fork. onnxruntime(-gpu) 1.24 dropped cp310 # wheels, so 3.10 environments resolve 1.23.x (matching what a 3.10 @@ -551,6 +550,8 @@ override-dependencies = [ ] [tool.uv.sources] +dimos-generated = { path = "packages/dimos-generated" } +dimos-message-build = { path = "dimos/message_codegen" } # CUDA builds are not published for macOS or non-x86_64 Linux. Those platforms # fall back to PyPI. torch = [ @@ -606,7 +607,7 @@ ignore = [ # (camelCase, pinned to the TS side by the golden fixtures). "dimos/web/relay_bridge/protocol.py" = ["N815"] "dimos/web/relay_bridge/manifest.py" = ["N815"] -# Polyfill module: re-exports ROS message types (with a dimos_lcm fallback). +# Polyfill module: re-exports optional ROS message types. "dimos/types/ros_polyfill.py" = ["F401"] [tool.ruff.lint.isort] @@ -637,7 +638,6 @@ module = [ "builtin_interfaces.*", "cyclonedds", "cyclonedds.*", - "dimos_lcm.*", "lz4", "lz4.*", "dimos_mls_planner", @@ -719,6 +719,7 @@ follow_imports = "skip" [tool.pytest.ini_options] testpaths = ["dimos"] +markers = ["native_e2e: native module end-to-end checks requiring built executables"] env = [ "GOOGLE_MAPS_API_KEY=AIzafake_google_key", "PYTHONWARNINGS=ignore:cupyx.jit.rawkernel is experimental:FutureWarning", diff --git a/scripts/g1_replay.py b/scripts/g1_replay.py index e562e0bca30..0169f0465d3 100644 --- a/scripts/g1_replay.py +++ b/scripts/g1_replay.py @@ -27,9 +27,11 @@ import threading import time +from dimos_generated.sensor_msgs.msg import JointState + from dimos.control.components import make_humanoid_joints from dimos.core.transport import LCMTransport -from dimos.msgs.sensor_msgs.JointState import JointState +from dimos.msgs.time import header_now from dimos.utils.data import get_data from dimos.utils.logging_config import setup_logger @@ -64,6 +66,7 @@ def load_trajectory(path: Path) -> tuple[list[float], list[list[float]]]: def make_joint_state(positions: list[float]) -> JointState: return JointState( + header=header_now(), name=list(CANONICAL_JOINTS), position=list(positions), velocity=[0.0] * NUM_DOF, diff --git a/scripts/install_native_messages.sh b/scripts/install_native_messages.sh new file mode 100644 index 00000000000..ae04d66c271 --- /dev/null +++ b/scripts/install_native_messages.sh @@ -0,0 +1,12 @@ +#!/usr/bin/env bash +set -euo pipefail + +# Generate/install the built-in C++ message package after setup_message_codegen.sh. +# Uses Python's standard library only; no ROS installation or Python extension build. +cd "$(dirname "$0")/.." +python3 -m dimos.message_codegen.generate --output build/message-codegen/native +cmake -S build/message-codegen/native/cpp -B build/message-codegen/native/cmake \ + -DDIMOS_BUILD_PYTHON=OFF \ + -DCMAKE_PREFIX_PATH="$PWD/build/message-codegen/install${CMAKE_PREFIX_PATH:+;$CMAKE_PREFIX_PATH}" \ + -DCMAKE_INSTALL_PREFIX="$PWD/build/message-codegen/install" +cmake --install build/message-codegen/native/cmake diff --git a/scripts/package_messages.sh b/scripts/package_messages.sh index 54286636a0a..c99d74a24ec 100644 --- a/scripts/package_messages.sh +++ b/scripts/package_messages.sh @@ -1,10 +1,21 @@ #!/usr/bin/env bash set -euo pipefail cd "$(dirname "$0")/.." -message_version="${1:?Pass the release version}" +if [[ $# -ne 0 ]]; then + echo "Message version comes from packages/dimos-generated/pyproject.toml; no version argument is accepted" >&2 + exit 2 +fi message_output="$PWD/build/message-codegen/release" -.venv/bin/python -m dimos.message_codegen.generate \ - --version "$message_version" --output "$message_output" +message_version="$(.venv/bin/python - <<'PYTHON' +from pathlib import Path +import tomllib +from dimos.message_codegen.generate import generate +version = tomllib.loads(Path("packages/dimos-generated/pyproject.toml").read_text())["project"]["version"] +# Built-ins must never absorb custom providers installed in the maintainer environment. +generate([], Path("build/message-codegen/release"), version=version, shared=True) +print(version) +PYTHON +)" cmake -S "$message_output/cpp" -B "$message_output/cmake" \ -DDIMOS_BUILD_PYTHON=OFF -DCMAKE_PREFIX_PATH="$PWD/build/message-codegen/install" \ -DCMAKE_INSTALL_PREFIX="$message_output/install" diff --git a/scripts/package_native_sdk.py b/scripts/package_native_sdk.py new file mode 100644 index 00000000000..dd270af9a89 --- /dev/null +++ b/scripts/package_native_sdk.py @@ -0,0 +1,44 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Create a relocatable native SDK source bundle without fetching dependencies.""" + +import argparse +from pathlib import Path +import shutil + + +def main() -> None: + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument("--output", type=Path, required=True) + args = parser.parse_args() + root = Path(__file__).resolve().parents[1] + for name in ("dimos-module", "dimos-module-macros", "dimos-lcm-transport"): + source = root / "native" / "rust" / name + target = args.output / "rust" / name + if target.exists(): + shutil.rmtree(target) + shutil.copytree(source, target, ignore=shutil.ignore_patterns("target")) + manifest = target / "Cargo.toml" + manifest.write_text( + manifest.read_text().replace(', path = "../../../dimos/message_codegen"', "") + ) + (args.output / "rust" / "Cargo.toml").write_text( + '[workspace]\nmembers = ["dimos-module", "dimos-module-macros", "dimos-lcm-transport"]\nresolver = "2"\n' + ) + print(args.output.resolve()) + + +if __name__ == "__main__": + main() diff --git a/scripts/test_external_native_messages.sh b/scripts/test_external_native_messages.sh new file mode 100644 index 00000000000..9648cbbfd1e --- /dev/null +++ b/scripts/test_external_native_messages.sh @@ -0,0 +1,30 @@ +#!/usr/bin/env bash +set -euo pipefail +cd "$(dirname "$0")/.." +# Run test_message_packages.sh first: it adds application_note locally and installs packages. +application="$PWD/build/message-codegen/external-app" +native_demo="$PWD/build/message-codegen/external-native" +mkdir -p "$native_demo/rust/src" "$native_demo/evidence" +cp examples/message-codegen/external-native/relay.rs "$native_demo/rust/src/main.rs" +cat > "$native_demo/rust/Cargo.toml" < "$output/evidence/rerun-summary.txt" .venv/bin/rerun rrd print "$output/demo.rrd" --entity /telemetry -vvv \ > "$output/evidence/rerun-telemetry.txt" diff --git a/scripts/test_message_packages.sh b/scripts/test_message_packages.sh index 89295775395..afa0bda7e5e 100644 --- a/scripts/test_message_packages.sh +++ b/scripts/test_message_packages.sh @@ -49,3 +49,12 @@ cargo build --offline --manifest-path "$package_demo/rust-consumer/Cargo.toml" env -u PYTHONPATH "$package_demo/venv/bin/python" -I \ examples/message-codegen/external-app/demo_installed.py --build "$package_demo" \ | tee "$package_demo/evidence/installed-relay.txt" +cat > "$package_demo/typed_consumer.py" <<'PY' +from external_telemetry.demo_msgs.msg import Telemetry + +value = Telemetry(application_note="typed") +result: str = value.application_note +PY +.venv/bin/python -m mypy --strict --config-file=/dev/null \ + --python-executable "$package_demo/venv/bin/python" "$package_demo/typed_consumer.py" \ + | tee "$package_demo/evidence/installed-typing.txt" diff --git a/scripts/test_message_user_story.sh b/scripts/test_message_user_story.sh new file mode 100644 index 00000000000..9e7e9f8e685 --- /dev/null +++ b/scripts/test_message_user_story.sh @@ -0,0 +1,23 @@ +#!/usr/bin/env bash +set -euo pipefail +cd "$(dirname "$0")/.." +story_build="$PWD/build/message-codegen/user-story" +story_python="${DIMOS_CODEGEN_PYTHON:-$PWD/.venv/bin/python}" +story_fastcdr="${DIMOS_FASTCDR_PREFIX:-$PWD/build/message-codegen/install}" +"$story_python" -m dimos.message_codegen.generate \ + --package-root examples/message-codegen/user-story \ + --type story_msgs/msg/DeviceReading --python-module story_messages \ + --output "$story_build" +cmake -S "$story_build/cpp" -B "$story_build/cpp/build" \ + -DCMAKE_BUILD_TYPE=Release -DCMAKE_PREFIX_PATH="$story_fastcdr" \ + -DPython_EXECUTABLE="$story_python" +cmake --build "$story_build/cpp/build" -j 2 +c++ -std=c++17 -I"$story_fastcdr/include" -I"$story_build/cpp" \ + examples/message-codegen/user-story/consumer.cpp "$story_fastcdr/lib/libfastcdr.a" \ + -o "$story_build/cpp-consumer" +mkdir -p "$story_build/rust/src/bin" "$story_build/evidence" +cp examples/message-codegen/user-story/consumer.rs "$story_build/rust/src/bin/story-consumer.rs" +cargo build --offline --manifest-path "$story_build/rust/Cargo.toml" --bin story-consumer +PYTHONPATH="$story_build/cpp/build:$PWD${PYTHONPATH:+:$PYTHONPATH}" \ + "$story_python" examples/message-codegen/user-story/demo_module.py --build "$story_build" \ + | tee "$story_build/evidence/terminal.txt" diff --git a/setup.py b/setup.py index 8eb84067f58..d59b27331de 100644 --- a/setup.py +++ b/setup.py @@ -24,8 +24,7 @@ # PEP 517 does not put the source tree on sys.path when executing setup.py. sys.path.insert(0, str(Path(__file__).resolve().parent)) -from dimos.message_codegen.generate import generate -from dimos.message_codegen.native_build import MessageBuildExt, MessageExtension +from dimos.message_codegen.native_build import MessageBuildExt def python_is_macos_universal_binary(executable: str | None = None) -> bool: @@ -156,14 +155,7 @@ def _copy_relay_dist(self): extra_compile_args.append("-march=native") # C++ extensions -message_output = Path(__file__).parent / "build" / "messages" -generate([], message_output) ext_modules = [ - MessageExtension( - "dimos_generated", - message_output / "cpp", - Path(__file__).parent / "build/message-codegen/install", - ), Pybind11Extension( "dimos.navigation.replanning_a_star.min_cost_astar_ext", [os.path.join("dimos", "navigation", "replanning_a_star", "min_cost_astar_cpp.cpp")], diff --git a/uv.lock b/uv.lock index 6f29fe09f28..019c9d0e319 100644 --- a/uv.lock +++ b/uv.lock @@ -36,14 +36,13 @@ exclude-newer = "0001-01-01T00:00:00Z" # This has no effect and is included for exclude-newer-span = "P7D" [options.exclude-newer-package] +can-motor-control = false +dim-odom = false dimos-viewer = false -dimos-lcm = false eclipse-zenoh = false edgetam-dimos = false -can-motor-control = false lcm-dimos-fork = false md-babel-py = false -dim-odom = false roboplan = false [manifest] @@ -889,12 +888,12 @@ resolution-markers = [ "python_full_version < '3.11' and platform_machine == 'aarch64' and sys_platform != 'darwin' and sys_platform != 'linux' and sys_platform != 'win32'", ] dependencies = [ - { name = "absl-py", marker = "python_full_version < '3.11'" }, - { name = "jax", version = "0.6.2", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version < '3.11'" }, - { name = "jaxlib", version = "0.6.2", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version < '3.11'" }, - { name = "numpy", version = "2.2.6", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version < '3.11'" }, - { name = "toolz", marker = "python_full_version < '3.11'" }, - { name = "typing-extensions", marker = "python_full_version < '3.11'" }, + { name = "absl-py" }, + { name = "jax", version = "0.6.2", source = { registry = "https://pypi.org/simple" } }, + { name = "jaxlib", version = "0.6.2", source = { registry = "https://pypi.org/simple" } }, + { name = "numpy", version = "2.2.6", source = { registry = "https://pypi.org/simple" } }, + { name = "toolz" }, + { name = "typing-extensions" }, ] sdist = { url = "https://files.pythonhosted.org/packages/77/70/53c7d404ce9e2a94009aea7f77ef6e392f6740e071c62683a506647c520f/chex-0.1.90.tar.gz", hash = "sha256:d3c375aeb6154b08f1cccd2bee4ed83659ee2198a6acf1160d2fe2e4a6c87b5c", size = 92363, upload-time = "2025-07-23T19:50:47.945Z" } wheels = [ @@ -924,12 +923,12 @@ resolution-markers = [ "python_full_version == '3.11.*' and platform_machine == 'aarch64' and sys_platform != 'darwin' and sys_platform != 'linux' and sys_platform != 'win32'", ] dependencies = [ - { name = "absl-py", marker = "python_full_version >= '3.11'" }, - { name = "jax", version = "0.9.0.1", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version >= '3.11'" }, - { name = "jaxlib", version = "0.9.0.1", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version >= '3.11'" }, - { name = "numpy", version = "2.3.5", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version >= '3.11'" }, - { name = "toolz", marker = "python_full_version >= '3.11'" }, - { name = "typing-extensions", marker = "python_full_version >= '3.11'" }, + { name = "absl-py" }, + { name = "jax", version = "0.9.0.1", source = { registry = "https://pypi.org/simple" } }, + { name = "jaxlib", version = "0.9.0.1", source = { registry = "https://pypi.org/simple" } }, + { name = "numpy", version = "2.3.5", source = { registry = "https://pypi.org/simple" } }, + { name = "toolz" }, + { name = "typing-extensions" }, ] sdist = { url = "https://files.pythonhosted.org/packages/5b/7d/812f01e7b2ddf28a0caa8dde56bd951a2c8f691c9bbfce38d469458d1502/chex-0.1.91.tar.gz", hash = "sha256:65367a521415ada905b8c0222b0a41a68337fcadf79a1fb6fc992dbd95dd9f76", size = 90302, upload-time = "2025-09-01T21:49:32.834Z" } wheels = [ @@ -1228,7 +1227,7 @@ name = "coloredlogs" version = "15.0.1" source = { registry = "https://pypi.org/simple" } dependencies = [ - { name = "humanfriendly", marker = "python_full_version < '3.11'" }, + { name = "humanfriendly" }, ] sdist = { url = "https://files.pythonhosted.org/packages/cc/c7/eed8f27100517e8c0e6b923d5f0845d0cb99763da6fdee00478f91db7325/coloredlogs-15.0.1.tar.gz", hash = "sha256:7c991aa71a4577af2f82600d8f8f3a89f936baeaf9b50a9c197da014e5bf16b0", size = 278520, upload-time = "2021-06-11T10:22:45.202Z" } wheels = [ @@ -1283,7 +1282,7 @@ resolution-markers = [ "python_full_version < '3.11' and platform_machine == 'aarch64' and sys_platform != 'darwin' and sys_platform != 'linux' and sys_platform != 'win32'", ] dependencies = [ - { name = "numpy", version = "2.2.6", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version < '3.11'" }, + { name = "numpy", version = "2.2.6", source = { registry = "https://pypi.org/simple" } }, ] sdist = { url = "https://files.pythonhosted.org/packages/66/54/eb9bfc647b19f2009dd5c7f5ec51c4e6ca831725f1aea7a993034f483147/contourpy-1.3.2.tar.gz", hash = "sha256:b6945942715a034c671b7fc54f9588126b0b8bf23db2696e3ca8328f3ff0ab54", size = 13466130, upload-time = "2025-04-15T17:47:53.79Z" } wheels = [ @@ -1348,7 +1347,7 @@ resolution-markers = [ "python_full_version == '3.11.*' and platform_machine == 'aarch64' and sys_platform != 'darwin' and sys_platform != 'linux' and sys_platform != 'win32'", ] dependencies = [ - { name = "numpy", version = "2.3.5", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version >= '3.11'" }, + { name = "numpy", version = "2.3.5", source = { registry = "https://pypi.org/simple" } }, ] sdist = { url = "https://files.pythonhosted.org/packages/58/01/1253e6698a07380cd31a736d248a3f2a50a7c88779a1813da27503cadc2a/contourpy-1.3.3.tar.gz", hash = "sha256:083e12155b210502d0bca491432bb04d56dc3432f95a979b429f2848c3dbe880", size = 13466174, upload-time = "2025-07-26T12:03:12.549Z" } wheels = [ @@ -1524,9 +1523,9 @@ name = "cupy-cuda12x" version = "13.6.0" source = { registry = "https://pypi.org/simple" } dependencies = [ - { name = "fastrlock", marker = "(python_full_version >= '3.11' and platform_machine != 'aarch64') or platform_machine == 'x86_64'" }, - { name = "numpy", version = "2.2.6", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version < '3.11' and platform_machine == 'x86_64'" }, - { name = "numpy", version = "2.3.5", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version >= '3.11' and platform_machine != 'aarch64'" }, + { name = "fastrlock" }, + { name = "numpy", version = "2.2.6", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version < '3.11'" }, + { name = "numpy", version = "2.3.5", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version >= '3.11'" }, ] wheels = [ { url = "https://files.pythonhosted.org/packages/53/2b/8064d94a6ab6b5c4e643d8535ab6af6cabe5455765540931f0ef60a0bc3b/cupy_cuda12x-13.6.0-cp310-cp310-manylinux2014_x86_64.whl", hash = "sha256:e78409ea72f5ac7d6b6f3d33d99426a94005254fa57e10617f430f9fd7c3a0a1", size = 112238589, upload-time = "2025-08-18T08:24:15.541Z" }, @@ -1612,8 +1611,8 @@ name = "dataclasses-json" version = "0.6.7" source = { registry = "https://pypi.org/simple" } dependencies = [ - { name = "marshmallow", marker = "python_full_version >= '3.11'" }, - { name = "typing-inspect", marker = "python_full_version >= '3.11'" }, + { name = "marshmallow" }, + { name = "typing-inspect" }, ] sdist = { url = "https://files.pythonhosted.org/packages/64/a4/f71d9cf3a5ac257c993b5ca3f93df5f7fb395c725e7f1e6479d2514173c3/dataclasses_json-0.6.7.tar.gz", hash = "sha256:b6b3e528266ea45b9535223bc53ca645f5208833c29229e847b3f26a1cc55fc0", size = 32227, upload-time = "2024-06-09T16:20:19.103Z" } wheels = [ @@ -1717,7 +1716,8 @@ dependencies = [ { name = "bleak" }, { name = "cmeel-tinyxml2" }, { name = "cryptography" }, - { name = "dimos-lcm" }, + { name = "dimos-generated" }, + { name = "dimos-message-build" }, { name = "dimos-viewer" }, { name = "eclipse-zenoh" }, { name = "filelock" }, @@ -1730,6 +1730,7 @@ dependencies = [ { name = "jsonlines" }, { name = "keyring" }, { name = "lazy-loader" }, + { name = "lcm-dimos-fork" }, { name = "llvmlite" }, { name = "lz4" }, { name = "numba" }, @@ -1780,7 +1781,7 @@ agents = [ { name = "sounddevice" }, ] all = [ - { name = "a750-control", marker = "platform_machine == 'x86_64' and sys_platform == 'linux'" }, + { name = "a750-control", marker = "python_full_version >= '3.12' and platform_machine == 'x86_64' and sys_platform == 'linux'" }, { name = "aiohttp" }, { name = "aioquic" }, { name = "aiortc" }, @@ -1895,7 +1896,7 @@ base = [ { name = "yourdfpy", marker = "platform_machine != 'aarch64' or sys_platform != 'linux'" }, ] control = [ - { name = "a750-control", marker = "platform_machine == 'x86_64' and sys_platform == 'linux'" }, + { name = "a750-control", marker = "python_full_version >= '3.12' and platform_machine == 'x86_64' and sys_platform == 'linux'" }, { name = "can-motor-control" }, { name = "piper-sdk" }, { name = "pygame" }, @@ -1925,7 +1926,7 @@ learning = [ { name = "pyarrow" }, ] manipulation = [ - { name = "a750-control", marker = "platform_machine == 'x86_64' and sys_platform == 'linux'" }, + { name = "a750-control", marker = "python_full_version >= '3.12' and platform_machine == 'x86_64' and sys_platform == 'linux'" }, { name = "aioquic" }, { name = "can-motor-control" }, { name = "chromadb" }, @@ -2006,7 +2007,7 @@ perception = [ { name = "ultralytics" }, ] planning = [ - { name = "a750-control", marker = "platform_machine == 'x86_64' and sys_platform == 'linux'" }, + { name = "a750-control", marker = "python_full_version >= '3.12' and platform_machine == 'x86_64' and sys_platform == 'linux'" }, { name = "can-motor-control" }, { name = "drake", version = "1.45.0", source = { registry = "https://pypi.org/simple" }, marker = "platform_machine != 'aarch64' and sys_platform == 'darwin'" }, { name = "drake", version = "1.49.0", source = { registry = "https://pypi.org/simple" }, marker = "platform_machine != 'aarch64' and sys_platform != 'darwin'" }, @@ -2337,7 +2338,7 @@ tests-self-hosted = [ [package.metadata] requires-dist = [ - { name = "a750-control", marker = "platform_machine == 'x86_64' and sys_platform == 'linux' and extra == 'control'" }, + { name = "a750-control", marker = "python_full_version >= '3.12' and platform_machine == 'x86_64' and sys_platform == 'linux' and extra == 'control'" }, { name = "aiohttp", marker = "extra == 'webrtc'", specifier = ">=3.9.0" }, { name = "aioquic", marker = "extra == 'web'", specifier = ">=1.2.0" }, { name = "aiortc", marker = "extra == 'webrtc'", specifier = ">=1.14.0" }, @@ -2360,7 +2361,8 @@ requires-dist = [ { name = "dimos", extras = ["control"], marker = "extra == 'planning'" }, { name = "dimos", extras = ["planning", "base", "sim", "cpu"], marker = "extra == 'manipulation'" }, { name = "dimos", extras = ["unitree"], marker = "extra == 'unitree-dds'" }, - { name = "dimos-lcm", specifier = ">=0.1.3" }, + { name = "dimos-generated", directory = "packages/dimos-generated" }, + { name = "dimos-message-build", directory = "dimos/message_codegen" }, { name = "dimos-viewer", specifier = "==0.32.0a2" }, { name = "dimos-viewer", marker = "extra == 'visualization'", specifier = "==0.32.0a2" }, { name = "drake", marker = "platform_machine != 'aarch64' and sys_platform == 'darwin' and extra == 'planning'", specifier = "==1.45.0" }, @@ -2391,6 +2393,7 @@ requires-dist = [ { name = "langchain-openai", marker = "extra == 'agents'", specifier = ">=1,<2" }, { name = "lap", marker = "extra == 'perception'", specifier = ">=0.5.12" }, { name = "lazy-loader" }, + { name = "lcm-dimos-fork", specifier = ">=1.5.2.post1" }, { name = "llvmlite", specifier = ">=0.42.0" }, { name = "lz4", specifier = ">=4.4.5" }, { name = "manifold3d", marker = "extra == 'apriltag'", specifier = ">=2.5.0" }, @@ -2670,18 +2673,27 @@ tests-self-hosted = [ ] [[package]] -name = "dimos-lcm" -version = "0.1.3" -source = { registry = "https://pypi.org/simple" } +name = "dimos-generated" +version = "0.1.0" +source = { directory = "packages/dimos-generated" } + +[[package]] +name = "dimos-message-build" +version = "0.1.0" +source = { directory = "dimos/message_codegen" } dependencies = [ - { name = "foxglove-websocket" }, - { name = "lcm-dimos-fork" }, - { name = "numpy", version = "2.2.6", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version < '3.11'" }, - { name = "numpy", version = "2.3.5", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version >= '3.11'" }, + { name = "pybind11" }, + { name = "setuptools" }, + { name = "tomli", marker = "python_full_version < '3.11'" }, + { name = "wheel" }, ] -sdist = { url = "https://files.pythonhosted.org/packages/09/b2/4cdd2bce665ab633313b5d371e0a63fcae2cf77a934a2a9bf820db88a540/dimos_lcm-0.1.3.tar.gz", hash = "sha256:5f7dbd3055f299823bc0e450c59583ad5a2d093c182deec8a86073853881bb09", size = 405688, upload-time = "2026-06-03T07:20:20.552Z" } -wheels = [ - { url = "https://files.pythonhosted.org/packages/95/19/2d8babf544993508359922ea59db2280d42efa0d5a80d4d0cae22ce40d67/dimos_lcm-0.1.3-py3-none-any.whl", hash = "sha256:63317225a0b4ab0f05e4b656f3f78ac9ba4e914998da2c689ca839cbbd83d57d", size = 1714087, upload-time = "2026-06-03T07:20:22.689Z" }, + +[package.metadata] +requires-dist = [ + { name = "pybind11", specifier = "==3.0.1" }, + { name = "setuptools", specifier = ">=70" }, + { name = "tomli", marker = "python_full_version < '3.11'", specifier = ">=2" }, + { name = "wheel" }, ] [[package]] @@ -2751,12 +2763,12 @@ resolution-markers = [ "python_full_version < '3.11' and platform_machine != 'aarch64' and platform_machine != 'x86_64' and sys_platform == 'darwin'", ] dependencies = [ - { name = "matplotlib", marker = "platform_machine != 'aarch64' and sys_platform == 'darwin'" }, - { name = "mosek", version = "11.0.24", source = { registry = "https://pypi.org/simple" }, marker = "platform_machine != 'aarch64' and sys_platform == 'darwin'" }, - { name = "numpy", version = "2.2.6", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version < '3.11' and platform_machine != 'aarch64' and sys_platform == 'darwin'" }, - { name = "numpy", version = "2.3.5", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version >= '3.11' and platform_machine != 'aarch64' and sys_platform == 'darwin'" }, - { name = "pydot", marker = "platform_machine != 'aarch64' and sys_platform == 'darwin'" }, - { name = "pyyaml", marker = "platform_machine != 'aarch64' and sys_platform == 'darwin'" }, + { name = "matplotlib" }, + { name = "mosek", version = "11.0.24", source = { registry = "https://pypi.org/simple" } }, + { name = "numpy", version = "2.2.6", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version < '3.11'" }, + { name = "numpy", version = "2.3.5", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version >= '3.11'" }, + { name = "pydot" }, + { name = "pyyaml" }, ] wheels = [ { url = "https://files.pythonhosted.org/packages/a0/31/aa4f1f5523381539e1028354cc535d5a3307d28fd33872f2b403454d8391/drake-1.45.0-cp312-cp312-macosx_14_0_arm64.whl", hash = "sha256:b0d9bd6196dc6d3b0e660fc6351fcf236727a45ef6a7123f8dc96f85b8662ac3", size = 57314509, upload-time = "2025-09-16T19:02:10.195Z" }, @@ -2780,12 +2792,12 @@ resolution-markers = [ "python_full_version < '3.11' and platform_machine != 'aarch64' and platform_machine != 'x86_64' and sys_platform != 'darwin' and sys_platform != 'win32'", ] dependencies = [ - { name = "matplotlib", marker = "platform_machine != 'aarch64' and sys_platform != 'darwin'" }, - { name = "mosek", version = "11.1.2", source = { registry = "https://pypi.org/simple" }, marker = "platform_machine != 'aarch64' and sys_platform != 'darwin'" }, - { name = "numpy", version = "2.2.6", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version < '3.11' and platform_machine != 'aarch64' and sys_platform != 'darwin'" }, - { name = "numpy", version = "2.3.5", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version >= '3.11' and platform_machine != 'aarch64' and sys_platform != 'darwin'" }, - { name = "pydot", marker = "platform_machine != 'aarch64' and sys_platform != 'darwin'" }, - { name = "pyyaml", marker = "platform_machine != 'aarch64' and sys_platform != 'darwin'" }, + { name = "matplotlib" }, + { name = "mosek", version = "11.1.2", source = { registry = "https://pypi.org/simple" } }, + { name = "numpy", version = "2.2.6", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version < '3.11'" }, + { name = "numpy", version = "2.3.5", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version >= '3.11'" }, + { name = "pydot" }, + { name = "pyyaml" }, ] wheels = [ { url = "https://files.pythonhosted.org/packages/fb/26/2ce3a9caf431f24e39f8b1fc7b3ebba4faafef1d61c849db3194e8d2e21d/drake-1.49.0-cp310-cp310-manylinux_2_34_x86_64.whl", hash = "sha256:6c73dbd061fcb442e82b7b5a94dadcfbf4c44949035d03394df29412114647b2", size = 41482505, upload-time = "2026-01-15T19:44:08.313Z" }, @@ -2877,8 +2889,8 @@ name = "embreex" version = "4.4.0" source = { registry = "https://pypi.org/simple" } dependencies = [ - { name = "numpy", version = "2.2.6", source = { registry = "https://pypi.org/simple" }, marker = "(python_full_version < '3.11' and platform_machine != 'aarch64') or (python_full_version < '3.11' and sys_platform != 'linux')" }, - { name = "numpy", version = "2.3.5", source = { registry = "https://pypi.org/simple" }, marker = "(python_full_version >= '3.11' and platform_machine != 'aarch64') or (python_full_version >= '3.11' and sys_platform != 'linux')" }, + { name = "numpy", version = "2.2.6", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version < '3.11'" }, + { name = "numpy", version = "2.3.5", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version >= '3.11'" }, ] wheels = [ { url = "https://files.pythonhosted.org/packages/29/89/692237d6a60f58f87f7bffdfe8c30008efd8811411e692f96d2cb1df0818/embreex-4.4.0-cp310-cp310-macosx_13_0_arm64.whl", hash = "sha256:b685ed766ef86dbd302ae9e92ab20156007c31cbd2ea5465542b50eea99c6da5", size = 5098460, upload-time = "2026-04-22T19:46:30.276Z" }, @@ -2920,7 +2932,7 @@ name = "exceptiongroup" version = "1.3.1" source = { registry = "https://pypi.org/simple" } dependencies = [ - { name = "typing-extensions", marker = "python_full_version < '3.11'" }, + { name = "typing-extensions" }, ] sdist = { url = "https://files.pythonhosted.org/packages/50/79/66800aadf48771f6b62f7eb014e352e5d06856655206165d775e675a02c9/exceptiongroup-1.3.1.tar.gz", hash = "sha256:8b412432c6055b0b7d14c310000ae93352ed6754f70fa8f7c34141f91c4e3219", size = 30371, upload-time = "2025-11-21T23:01:54.787Z" } wheels = [ @@ -3182,15 +3194,15 @@ resolution-markers = [ "python_full_version < '3.11' and platform_machine == 'aarch64' and sys_platform != 'darwin' and sys_platform != 'linux' and sys_platform != 'win32'", ] dependencies = [ - { name = "jax", version = "0.6.2", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version < '3.11'" }, - { name = "msgpack", marker = "python_full_version < '3.11'" }, - { name = "optax", marker = "python_full_version < '3.11'" }, - { name = "orbax-checkpoint", marker = "python_full_version < '3.11'" }, - { name = "pyyaml", marker = "python_full_version < '3.11'" }, - { name = "rich", marker = "python_full_version < '3.11'" }, - { name = "tensorstore", version = "0.1.78", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version < '3.11'" }, - { name = "treescope", marker = "python_full_version < '3.11'" }, - { name = "typing-extensions", marker = "python_full_version < '3.11'" }, + { name = "jax", version = "0.6.2", source = { registry = "https://pypi.org/simple" } }, + { name = "msgpack" }, + { name = "optax" }, + { name = "orbax-checkpoint" }, + { name = "pyyaml" }, + { name = "rich" }, + { name = "tensorstore", version = "0.1.78", source = { registry = "https://pypi.org/simple" } }, + { name = "treescope" }, + { name = "typing-extensions" }, ] sdist = { url = "https://files.pythonhosted.org/packages/e6/76/4ea55a60a47e98fcff591238ee26ed4624cb4fdc4893aa3ebf78d0d021f4/flax-0.10.7.tar.gz", hash = "sha256:2930d6671e23076f6db3b96afacf45c5060898f5c189ecab6dda7e05d26c2085", size = 5136099, upload-time = "2025-07-02T06:10:07.819Z" } wheels = [ @@ -3220,17 +3232,17 @@ resolution-markers = [ "python_full_version == '3.11.*' and platform_machine == 'aarch64' and sys_platform != 'darwin' and sys_platform != 'linux' and sys_platform != 'win32'", ] dependencies = [ - { name = "jax", version = "0.9.0.1", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version >= '3.11'" }, - { name = "msgpack", marker = "python_full_version >= '3.11'" }, - { name = "numpy", version = "2.3.5", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version >= '3.11'" }, - { name = "optax", marker = "python_full_version >= '3.11'" }, - { name = "orbax-checkpoint", marker = "python_full_version >= '3.11'" }, - { name = "orbax-export", marker = "python_full_version >= '3.11'" }, - { name = "pyyaml", marker = "python_full_version >= '3.11'" }, - { name = "rich", marker = "python_full_version >= '3.11'" }, - { name = "tensorstore", version = "0.1.81", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version >= '3.11'" }, - { name = "treescope", marker = "python_full_version >= '3.11'" }, - { name = "typing-extensions", marker = "python_full_version >= '3.11'" }, + { name = "jax", version = "0.9.0.1", source = { registry = "https://pypi.org/simple" } }, + { name = "msgpack" }, + { name = "numpy", version = "2.3.5", source = { registry = "https://pypi.org/simple" } }, + { name = "optax" }, + { name = "orbax-checkpoint" }, + { name = "orbax-export" }, + { name = "pyyaml" }, + { name = "rich" }, + { name = "tensorstore", version = "0.1.81", source = { registry = "https://pypi.org/simple" } }, + { name = "treescope" }, + { name = "typing-extensions" }, ] sdist = { url = "https://files.pythonhosted.org/packages/48/81/802fd686d3f47d7560a83f73b23efff03de7e3a0342e4f0fc41680136709/flax-0.12.4.tar.gz", hash = "sha256:5e924734a0595ddfa06a824568617e5440c7948e744772cbe6101b7ae06d66a9", size = 5070824, upload-time = "2026-02-12T19:10:17.048Z" } wheels = [ @@ -3270,18 +3282,6 @@ wheels = [ { url = "https://files.pythonhosted.org/packages/c7/4e/ce75a57ff3aebf6fc1f4e9d508b8e5810618a33d900ad6c19eb30b290b97/fonttools-4.61.1-py3-none-any.whl", hash = "sha256:17d2bf5d541add43822bcf0c43d7d847b160c9bb01d15d5007d84e2217aaa371", size = 1148996, upload-time = "2025-12-12T17:31:21.03Z" }, ] -[[package]] -name = "foxglove-websocket" -version = "0.1.4" -source = { registry = "https://pypi.org/simple" } -dependencies = [ - { name = "websockets" }, -] -sdist = { url = "https://files.pythonhosted.org/packages/db/b5/df32ac550eb0df9000ed78d872eb19738edecfd88f47fe08588d5066f317/foxglove_websocket-0.1.4.tar.gz", hash = "sha256:2ec8936982e478d103dd90268a572599fc0cce45a4ab95490d5bc31f7c8a8af8", size = 16616, upload-time = "2025-07-14T20:26:28.278Z" } -wheels = [ - { url = "https://files.pythonhosted.org/packages/c4/73/3a3e6cb864ddf98800a9236ad497d32e5b50eb1682ac659f7d669d92faec/foxglove_websocket-0.1.4-py3-none-any.whl", hash = "sha256:772e24e2c98bdfc704df53f7177c8ff5bab0abc4dac59a91463aca16debdd83a", size = 14392, upload-time = "2025-07-14T20:26:26.899Z" }, -] - [[package]] name = "frozenlist" version = "1.8.0" @@ -3582,9 +3582,9 @@ name = "gtsam-extended" version = "4.3a1.post1" source = { registry = "https://pypi.org/simple" } dependencies = [ - { name = "numpy", version = "2.2.6", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version < '3.11' and platform_machine != 'aarch64' and platform_machine != 'x86_64' and sys_platform == 'darwin'" }, + { name = "numpy", version = "2.2.6", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version < '3.11' and platform_machine == 'arm64' and sys_platform == 'darwin'" }, { name = "numpy", version = "2.3.5", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version >= '3.11'" }, - { name = "pytest", marker = "(python_full_version >= '3.11' and platform_machine == 'aarch64') or (python_full_version >= '3.11' and platform_machine == 'x86_64') or (python_full_version >= '3.11' and sys_platform != 'darwin') or (platform_machine != 'aarch64' and platform_machine != 'x86_64' and sys_platform == 'darwin')" }, + { name = "pytest" }, ] wheels = [ { url = "https://files.pythonhosted.org/packages/7f/a3/f815f6768994d1cd11f76cfa094be1d50c84edf1d85908fa93b461bb2eaf/gtsam_extended-4.3a1.post1-cp310-cp310-macosx_14_0_arm64.whl", hash = "sha256:20291cdf65ae8b97abd0dc3f9d62cbeec6a76144f9f796044cccc3ffacbaae13", size = 26677717, upload-time = "2026-04-02T22:29:33.557Z" }, @@ -3745,7 +3745,7 @@ name = "humanfriendly" version = "10.0" source = { registry = "https://pypi.org/simple" } dependencies = [ - { name = "pyreadline3", marker = "python_full_version < '3.11' and sys_platform == 'win32'" }, + { name = "pyreadline3", marker = "sys_platform == 'win32'" }, ] sdist = { url = "https://files.pythonhosted.org/packages/cc/3f/2c29224acb2e2df4d2046e4c73ee2662023c58ff5b113c4c1adac0886c43/humanfriendly-10.0.tar.gz", hash = "sha256:6b0b831ce8f15f7300721aa49829fc4e83921a9a301cc7f606be6686a2288ddc", size = 360702, upload-time = "2021-09-17T21:40:43.31Z" } wheels = [ @@ -3820,7 +3820,7 @@ resolution-markers = [ "python_full_version < '3.11' and platform_machine == 'aarch64' and sys_platform != 'darwin' and sys_platform != 'linux' and sys_platform != 'win32'", ] dependencies = [ - { name = "numpy", version = "2.2.6", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version < '3.11'" }, + { name = "numpy", version = "2.2.6", source = { registry = "https://pypi.org/simple" } }, ] sdist = { url = "https://files.pythonhosted.org/packages/de/bf/81c848ffe2b42fc141b6db3e4e8e650183b7aab8c4535498ebff25740a3b/imagecodecs-2025.3.30.tar.gz", hash = "sha256:29256f44a7fcfb8f235a3e9b3bae72b06ea2112e63bcc892267a8c01b7097f90", size = 9506573, upload-time = "2025-03-30T04:44:50.368Z" } wheels = [ @@ -3859,7 +3859,7 @@ resolution-markers = [ "python_full_version == '3.11.*' and platform_machine == 'aarch64' and sys_platform != 'darwin' and sys_platform != 'linux' and sys_platform != 'win32'", ] dependencies = [ - { name = "numpy", version = "2.3.5", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version == '3.11.*'" }, + { name = "numpy", version = "2.3.5", source = { registry = "https://pypi.org/simple" } }, ] sdist = { url = "https://files.pythonhosted.org/packages/3b/8d/dc18623e5e926ad53c626e128c8baaf4ec42e41029cf0a07381cfef79289/imagecodecs-2026.3.6.tar.gz", hash = "sha256:471b8a4d1b3843cbf7179b45f7d7261f0c0b28809efc1ca6c47822477b143b85", size = 9565259, upload-time = "2026-03-07T01:26:41.183Z" } wheels = [ @@ -3887,7 +3887,7 @@ resolution-markers = [ "python_full_version >= '3.12' and platform_machine == 'aarch64' and sys_platform != 'darwin' and sys_platform != 'linux' and sys_platform != 'win32'", ] dependencies = [ - { name = "numpy", version = "2.3.5", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version >= '3.12'" }, + { name = "numpy", version = "2.3.5", source = { registry = "https://pypi.org/simple" } }, ] sdist = { url = "https://files.pythonhosted.org/packages/98/9f/b436418349779e1c18cc27575360cad03dd492bc574ae9e297832bc48cab/imagecodecs-2026.6.26.tar.gz", hash = "sha256:da95b145f6b4f746acc9e0b8707b164eefc6a36ade3b6e70f74d102e9affad8c", size = 9669990, upload-time = "2026-06-28T18:27:01.957Z" } wheels = [ @@ -3998,17 +3998,17 @@ resolution-markers = [ "python_full_version < '3.11' and platform_machine == 'aarch64' and sys_platform != 'darwin' and sys_platform != 'linux' and sys_platform != 'win32'", ] dependencies = [ - { name = "colorama", marker = "python_full_version < '3.11' and sys_platform == 'win32'" }, - { name = "decorator", marker = "python_full_version < '3.11'" }, - { name = "exceptiongroup", marker = "python_full_version < '3.11'" }, - { name = "jedi", marker = "python_full_version < '3.11'" }, - { name = "matplotlib-inline", marker = "python_full_version < '3.11'" }, - { name = "pexpect", marker = "python_full_version < '3.11' and sys_platform != 'emscripten' and sys_platform != 'win32'" }, - { name = "prompt-toolkit", marker = "python_full_version < '3.11'" }, - { name = "pygments", marker = "python_full_version < '3.11'" }, - { name = "stack-data", marker = "python_full_version < '3.11'" }, - { name = "traitlets", marker = "python_full_version < '3.11'" }, - { name = "typing-extensions", marker = "python_full_version < '3.11'" }, + { name = "colorama", marker = "sys_platform == 'win32'" }, + { name = "decorator" }, + { name = "exceptiongroup" }, + { name = "jedi" }, + { name = "matplotlib-inline" }, + { name = "pexpect", marker = "sys_platform != 'emscripten' and sys_platform != 'win32'" }, + { name = "prompt-toolkit" }, + { name = "pygments" }, + { name = "stack-data" }, + { name = "traitlets" }, + { name = "typing-extensions" }, ] sdist = { url = "https://files.pythonhosted.org/packages/e5/61/1810830e8b93c72dcd3c0f150c80a00c3deb229562d9423807ec92c3a539/ipython-8.38.0.tar.gz", hash = "sha256:9cfea8c903ce0867cc2f23199ed8545eb741f3a69420bfcf3743ad1cec856d39", size = 5513996, upload-time = "2026-01-05T10:59:06.901Z" } wheels = [ @@ -4038,17 +4038,17 @@ resolution-markers = [ "python_full_version == '3.11.*' and platform_machine == 'aarch64' and sys_platform != 'darwin' and sys_platform != 'linux' and sys_platform != 'win32'", ] dependencies = [ - { name = "colorama", marker = "python_full_version >= '3.11' and sys_platform == 'win32'" }, - { name = "decorator", marker = "python_full_version >= '3.11'" }, - { name = "ipython-pygments-lexers", marker = "python_full_version >= '3.11'" }, - { name = "jedi", marker = "python_full_version >= '3.11'" }, - { name = "matplotlib-inline", marker = "python_full_version >= '3.11'" }, - { name = "pexpect", marker = "python_full_version >= '3.11' and sys_platform != 'emscripten' and sys_platform != 'win32'" }, - { name = "prompt-toolkit", marker = "python_full_version >= '3.11'" }, - { name = "pygments", marker = "python_full_version >= '3.11'" }, - { name = "stack-data", marker = "python_full_version >= '3.11'" }, - { name = "traitlets", marker = "python_full_version >= '3.11'" }, - { name = "typing-extensions", marker = "python_full_version == '3.11.*'" }, + { name = "colorama", marker = "sys_platform == 'win32'" }, + { name = "decorator" }, + { name = "ipython-pygments-lexers" }, + { name = "jedi" }, + { name = "matplotlib-inline" }, + { name = "pexpect", marker = "sys_platform != 'emscripten' and sys_platform != 'win32'" }, + { name = "prompt-toolkit" }, + { name = "pygments" }, + { name = "stack-data" }, + { name = "traitlets" }, + { name = "typing-extensions", marker = "python_full_version < '3.12'" }, ] sdist = { url = "https://files.pythonhosted.org/packages/a6/60/2111715ea11f39b1535bed6024b7dec7918b71e5e5d30855a5b503056b50/ipython-9.10.0.tar.gz", hash = "sha256:cd9e656be97618a0676d058134cd44e6dc7012c0e5cb36a9ce96a8c904adaf77", size = 4426526, upload-time = "2026-02-02T10:00:33.594Z" } wheels = [ @@ -4060,7 +4060,7 @@ name = "ipython-pygments-lexers" version = "1.1.1" source = { registry = "https://pypi.org/simple" } dependencies = [ - { name = "pygments", marker = "python_full_version >= '3.11'" }, + { name = "pygments" }, ] sdist = { url = "https://files.pythonhosted.org/packages/ef/4c/5dd1d8af08107f88c7f741ead7a40854b8ac24ddf9ae850afbcf698aa552/ipython_pygments_lexers-1.1.1.tar.gz", hash = "sha256:09c0138009e56b6854f9535736f4171d855c8c08a563a0dcd8022f78355c7e81", size = 8393, upload-time = "2025-01-17T11:24:34.505Z" } wheels = [ @@ -4139,11 +4139,11 @@ resolution-markers = [ "python_full_version < '3.11' and platform_machine == 'aarch64' and sys_platform != 'darwin' and sys_platform != 'linux' and sys_platform != 'win32'", ] dependencies = [ - { name = "jaxlib", version = "0.6.2", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version < '3.11'" }, - { name = "ml-dtypes", marker = "python_full_version < '3.11'" }, - { name = "numpy", version = "2.2.6", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version < '3.11'" }, - { name = "opt-einsum", marker = "python_full_version < '3.11'" }, - { name = "scipy", version = "1.15.3", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version < '3.11'" }, + { name = "jaxlib", version = "0.6.2", source = { registry = "https://pypi.org/simple" } }, + { name = "ml-dtypes" }, + { name = "numpy", version = "2.2.6", source = { registry = "https://pypi.org/simple" } }, + { name = "opt-einsum" }, + { name = "scipy", version = "1.15.3", source = { registry = "https://pypi.org/simple" } }, ] sdist = { url = "https://files.pythonhosted.org/packages/cf/1e/267f59c8fb7f143c3f778c76cb7ef1389db3fd7e4540f04b9f42ca90764d/jax-0.6.2.tar.gz", hash = "sha256:a437d29038cbc8300334119692744704ca7941490867b9665406b7f90665cd96", size = 2334091, upload-time = "2025-06-17T23:10:27.186Z" } wheels = [ @@ -4173,11 +4173,11 @@ resolution-markers = [ "python_full_version == '3.11.*' and platform_machine == 'aarch64' and sys_platform != 'darwin' and sys_platform != 'linux' and sys_platform != 'win32'", ] dependencies = [ - { name = "jaxlib", version = "0.9.0.1", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version >= '3.11'" }, - { name = "ml-dtypes", marker = "python_full_version >= '3.11'" }, - { name = "numpy", version = "2.3.5", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version >= '3.11'" }, - { name = "opt-einsum", marker = "python_full_version >= '3.11'" }, - { name = "scipy", version = "1.17.0", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version >= '3.11'" }, + { name = "jaxlib", version = "0.9.0.1", source = { registry = "https://pypi.org/simple" } }, + { name = "ml-dtypes" }, + { name = "numpy", version = "2.3.5", source = { registry = "https://pypi.org/simple" } }, + { name = "opt-einsum" }, + { name = "scipy", version = "1.17.0", source = { registry = "https://pypi.org/simple" } }, ] sdist = { url = "https://files.pythonhosted.org/packages/52/40/f85d1feadd8f793fc1bfab726272523ef34b27302b55861ea872ec774019/jax-0.9.0.1.tar.gz", hash = "sha256:e395253449d74354fa813ff9e245acb6e42287431d8a01ff33d92e9ee57d36bd", size = 2534795, upload-time = "2026-02-05T18:47:33.088Z" } wheels = [ @@ -4202,9 +4202,9 @@ resolution-markers = [ "python_full_version < '3.11' and platform_machine == 'aarch64' and sys_platform != 'darwin' and sys_platform != 'linux' and sys_platform != 'win32'", ] dependencies = [ - { name = "ml-dtypes", marker = "python_full_version < '3.11'" }, - { name = "numpy", version = "2.2.6", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version < '3.11'" }, - { name = "scipy", version = "1.15.3", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version < '3.11'" }, + { name = "ml-dtypes" }, + { name = "numpy", version = "2.2.6", source = { registry = "https://pypi.org/simple" } }, + { name = "scipy", version = "1.15.3", source = { registry = "https://pypi.org/simple" } }, ] wheels = [ { url = "https://files.pythonhosted.org/packages/15/c5/41598634c99cbebba46e6777286fb76abc449d33d50aeae5d36128ca8803/jaxlib-0.6.2-cp310-cp310-macosx_11_0_arm64.whl", hash = "sha256:da4601b2b5dc8c23d6afb293eacfb9aec4e1d1871cb2f29c5a151d103e73b0f8", size = 54298019, upload-time = "2025-06-17T23:10:36.916Z" }, @@ -4244,9 +4244,9 @@ resolution-markers = [ "python_full_version == '3.11.*' and platform_machine == 'aarch64' and sys_platform != 'darwin' and sys_platform != 'linux' and sys_platform != 'win32'", ] dependencies = [ - { name = "ml-dtypes", marker = "python_full_version >= '3.11'" }, - { name = "numpy", version = "2.3.5", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version >= '3.11'" }, - { name = "scipy", version = "1.17.0", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version >= '3.11'" }, + { name = "ml-dtypes" }, + { name = "numpy", version = "2.3.5", source = { registry = "https://pypi.org/simple" } }, + { name = "scipy", version = "1.17.0", source = { registry = "https://pypi.org/simple" } }, ] wheels = [ { url = "https://files.pythonhosted.org/packages/b0/fd/040321b0f4303ec7b558d69488c6130b1697c33d88dab0a0d2ccd2e0817c/jaxlib-0.9.0.1-cp311-cp311-macosx_11_0_arm64.whl", hash = "sha256:5ff2c550dab210278ed3a3b96454b19108a02e0795625be56dca5a181c9833c9", size = 56092920, upload-time = "2026-02-05T18:46:20.873Z" }, @@ -4283,7 +4283,7 @@ name = "jaxtyping" version = "0.3.7" source = { registry = "https://pypi.org/simple" } dependencies = [ - { name = "wadler-lindig", marker = "python_full_version >= '3.11'" }, + { name = "wadler-lindig" }, ] sdist = { url = "https://files.pythonhosted.org/packages/38/40/a2ea3ce0e3e5f540eb970de7792c90fa58fef1b27d34c83f9fa94fea4729/jaxtyping-0.3.7.tar.gz", hash = "sha256:3bd7d9beb7d3cb01a89f93f90581c6f4fff3e5c5dc3c9307e8f8687a040d10c4", size = 45721, upload-time = "2026-01-30T14:18:47.409Z" } wheels = [ @@ -5199,7 +5199,7 @@ name = "marshmallow" version = "3.26.2" source = { registry = "https://pypi.org/simple" } dependencies = [ - { name = "packaging", marker = "python_full_version >= '3.11'" }, + { name = "packaging" }, ] sdist = { url = "https://files.pythonhosted.org/packages/55/79/de6c16cc902f4fc372236926b0ce2ab7845268dcc30fb2fbb7f71b418631/marshmallow-3.26.2.tar.gz", hash = "sha256:bbe2adb5a03e6e3571b573f42527c6fe926e17467833660bebd11593ab8dfd57", size = 222095, upload-time = "2025-12-22T06:53:53.309Z" } wheels = [ @@ -5620,8 +5620,8 @@ resolution-markers = [ "python_full_version < '3.11' and platform_machine != 'aarch64' and platform_machine != 'x86_64' and sys_platform == 'darwin'", ] dependencies = [ - { name = "numpy", version = "2.2.6", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version < '3.11' and platform_machine != 'aarch64' and sys_platform == 'darwin'" }, - { name = "numpy", version = "2.3.5", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version >= '3.11' and platform_machine != 'aarch64' and sys_platform == 'darwin'" }, + { name = "numpy", version = "2.2.6", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version < '3.11'" }, + { name = "numpy", version = "2.3.5", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version >= '3.11'" }, ] wheels = [ { url = "https://files.pythonhosted.org/packages/37/e7/d04ea5c587fd8b491fbe9377fafa5feb063bb28a3a6949fb393a62230d9d/mosek-11.0.24-cp39-abi3-macosx_11_0_arm64.whl", hash = "sha256:7f2ab70ad3357f9187c96237d0c49187f82f5885250a5e211b6aa20cb0a7207f", size = 8345311, upload-time = "2025-06-25T10:51:51.777Z" }, @@ -5645,8 +5645,8 @@ resolution-markers = [ "python_full_version < '3.11' and platform_machine != 'aarch64' and platform_machine != 'x86_64' and sys_platform != 'darwin' and sys_platform != 'win32'", ] dependencies = [ - { name = "numpy", version = "2.2.6", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version < '3.11' and platform_machine != 'aarch64' and sys_platform != 'darwin'" }, - { name = "numpy", version = "2.3.5", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version >= '3.11' and platform_machine != 'aarch64' and sys_platform != 'darwin'" }, + { name = "numpy", version = "2.2.6", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version < '3.11'" }, + { name = "numpy", version = "2.3.5", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version >= '3.11'" }, ] wheels = [ { url = "https://files.pythonhosted.org/packages/c3/e9/253e759e6e00b9cfbb4e95e7fe079b0e971b3c81c75f059bf2c2be3216e9/mosek-11.1.2-cp39-abi3-manylinux2014_x86_64.whl", hash = "sha256:5c3566d2a603d94a1773bcd27097c8390dba1d9a1543534f3527deb56f1d0a55", size = 15359313, upload-time = "2026-01-07T08:22:00.805Z" }, @@ -6130,7 +6130,7 @@ name = "nvidia-cublas" version = "13.6.1.10" source = { registry = "https://pypi.org/simple" } dependencies = [ - { name = "nvidia-cuda-nvrtc", marker = "(python_full_version >= '3.11' and platform_machine == 'x86_64' and sys_platform != 'darwin' and sys_platform != 'win32') or (platform_machine != 'aarch64' and platform_machine != 'x86_64' and sys_platform != 'darwin' and sys_platform != 'win32') or sys_platform == 'linux'" }, + { name = "nvidia-cuda-nvrtc" }, ] wheels = [ { url = "https://files.pythonhosted.org/packages/42/51/a174a0e79793e528ef4645a9d554e3aa48fd8da3f9c9cf523c0176bb121b/nvidia_cublas-13.6.1.10-py3-none-manylinux_2_27_aarch64.whl", hash = "sha256:e05a431062a17cb9b02e2f37e67817b637051ce8fad57b388482c594396ddbb4", size = 518610411, upload-time = "2026-07-30T18:18:32.969Z" }, @@ -6192,7 +6192,7 @@ name = "nvidia-cudnn-cu12" version = "9.7.1.26" source = { registry = "https://pypi.org/simple" } dependencies = [ - { name = "nvidia-cublas-cu12", marker = "platform_machine == 'x86_64' and sys_platform == 'linux'" }, + { name = "nvidia-cublas-cu12" }, ] wheels = [ { url = "https://files.pythonhosted.org/packages/25/dc/dc825c4b1c83b538e207e34f48f86063c88deaa35d46c651c7c181364ba2/nvidia_cudnn_cu12-9.7.1.26-py3-none-manylinux_2_27_x86_64.whl", hash = "sha256:6d011159a158f3cfc47bf851aea79e31bcff60d530b70ef70474c84cac484d07", size = 726851421, upload-time = "2025-02-06T22:18:29.812Z" }, @@ -6203,7 +6203,7 @@ name = "nvidia-cufft-cu12" version = "11.3.3.41" source = { registry = "https://pypi.org/simple" } dependencies = [ - { name = "nvidia-nvjitlink-cu12", marker = "platform_machine == 'x86_64' and sys_platform == 'linux'" }, + { name = "nvidia-nvjitlink-cu12" }, ] wheels = [ { url = "https://files.pythonhosted.org/packages/ac/26/b53c493c38dccb1f1a42e1a21dc12cba2a77fbe36c652f7726d9ec4aba28/nvidia_cufft_cu12-11.3.3.41-py3-none-manylinux2014_x86_64.manylinux_2_17_x86_64.whl", hash = "sha256:da650080ab79fcdf7a4b06aa1b460e99860646b176a43f6208099bdc17836b6a", size = 193118795, upload-time = "2025-01-23T17:56:30.536Z" }, @@ -6230,9 +6230,9 @@ name = "nvidia-cusolver" version = "12.2.6.9" source = { registry = "https://pypi.org/simple" } dependencies = [ - { name = "nvidia-cublas", marker = "(python_full_version >= '3.11' and platform_machine == 'x86_64' and sys_platform != 'darwin' and sys_platform != 'win32') or (platform_machine != 'aarch64' and platform_machine != 'x86_64' and sys_platform != 'darwin' and sys_platform != 'win32') or sys_platform == 'linux'" }, - { name = "nvidia-cusparse", marker = "(python_full_version >= '3.11' and platform_machine == 'x86_64' and sys_platform != 'darwin' and sys_platform != 'win32') or (platform_machine != 'aarch64' and platform_machine != 'x86_64' and sys_platform != 'darwin' and sys_platform != 'win32') or sys_platform == 'linux'" }, - { name = "nvidia-nvjitlink", marker = "(python_full_version >= '3.11' and platform_machine == 'x86_64' and sys_platform != 'darwin' and sys_platform != 'win32') or (platform_machine != 'aarch64' and platform_machine != 'x86_64' and sys_platform != 'darwin' and sys_platform != 'win32') or sys_platform == 'linux'" }, + { name = "nvidia-cublas" }, + { name = "nvidia-cusparse" }, + { name = "nvidia-nvjitlink" }, ] wheels = [ { url = "https://files.pythonhosted.org/packages/13/bd/61dcd7917f64c9e81a35f939f1bf4ab6535adf712192e915fb472e5bbf6c/nvidia_cusolver-12.2.6.9-py3-none-manylinux_2_27_aarch64.whl", hash = "sha256:658ce9e6ed8a321033419fe6e55faf0f245a15aa23669eda03d7f2dcce436d73", size = 267275679, upload-time = "2026-06-29T16:59:54.924Z" }, @@ -6244,9 +6244,9 @@ name = "nvidia-cusolver-cu12" version = "11.7.2.55" source = { registry = "https://pypi.org/simple" } dependencies = [ - { name = "nvidia-cublas-cu12", marker = "platform_machine == 'x86_64' and sys_platform == 'linux'" }, - { name = "nvidia-cusparse-cu12", marker = "platform_machine == 'x86_64' and sys_platform == 'linux'" }, - { name = "nvidia-nvjitlink-cu12", marker = "platform_machine == 'x86_64' and sys_platform == 'linux'" }, + { name = "nvidia-cublas-cu12" }, + { name = "nvidia-cusparse-cu12" }, + { name = "nvidia-nvjitlink-cu12" }, ] wheels = [ { url = "https://files.pythonhosted.org/packages/c2/08/953675873a136d96bb12f93b49ba045d1107bc94d2551c52b12fa6c7dec3/nvidia_cusolver_cu12-11.7.2.55-py3-none-manylinux_2_27_x86_64.whl", hash = "sha256:4d1354102f1e922cee9db51920dba9e2559877cf6ff5ad03a00d853adafb191b", size = 260373342, upload-time = "2025-01-23T17:58:56.406Z" }, @@ -6257,7 +6257,7 @@ name = "nvidia-cusparse" version = "12.8.2.51" source = { registry = "https://pypi.org/simple" } dependencies = [ - { name = "nvidia-nvjitlink", marker = "(python_full_version >= '3.11' and platform_machine == 'x86_64' and sys_platform != 'darwin' and sys_platform != 'win32') or (platform_machine != 'aarch64' and platform_machine != 'x86_64' and sys_platform != 'darwin' and sys_platform != 'win32') or sys_platform == 'linux'" }, + { name = "nvidia-nvjitlink" }, ] wheels = [ { url = "https://files.pythonhosted.org/packages/ac/bc/925058f9343d93426864cbb113a3f5cf6db97a7d11642aaf4159b6f0c57a/nvidia_cusparse-12.8.2.51-py3-none-manylinux2014_aarch64.manylinux_2_17_aarch64.whl", hash = "sha256:00469fcf62c4d464a1225abd9b20864ecff35e3fbc9fb992572e83d358927755", size = 172868683, upload-time = "2026-06-29T17:01:23.244Z" }, @@ -6269,7 +6269,7 @@ name = "nvidia-cusparse-cu12" version = "12.5.7.53" source = { registry = "https://pypi.org/simple" } dependencies = [ - { name = "nvidia-nvjitlink-cu12", marker = "platform_machine == 'x86_64' and sys_platform == 'linux'" }, + { name = "nvidia-nvjitlink-cu12" }, ] wheels = [ { url = "https://files.pythonhosted.org/packages/c2/ab/31e8149c66213b846c082a3b41b1365b831f41191f9f40c6ddbc8a7d550e/nvidia_cusparse_cu12-12.5.7.53-py3-none-manylinux2014_x86_64.manylinux_2_17_x86_64.whl", hash = "sha256:3c1b61eb8c85257ea07e9354606b26397612627fdcd327bfd91ccf6155e7c86d", size = 292064180, upload-time = "2025-01-23T18:00:23.233Z" }, @@ -6369,13 +6369,13 @@ resolution-markers = [ "python_full_version < '3.11' and platform_machine == 'aarch64' and sys_platform != 'darwin' and sys_platform != 'linux' and sys_platform != 'win32'", ] dependencies = [ - { name = "coloredlogs", marker = "python_full_version < '3.11'" }, - { name = "flatbuffers", marker = "python_full_version < '3.11'" }, - { name = "numpy", version = "2.2.6", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version < '3.11'" }, - { name = "packaging", marker = "python_full_version < '3.11'" }, - { name = "protobuf", marker = "python_full_version < '3.11'" }, - { name = "sympy", version = "1.13.1", source = { registry = "https://pypi.org/simple" }, marker = "(python_full_version < '3.11' and platform_machine != 'x86_64' and sys_platform == 'linux') or (python_full_version < '3.11' and sys_platform != 'linux' and sys_platform != 'win32')" }, - { name = "sympy", version = "1.14.0", source = { registry = "https://pypi.org/simple" }, marker = "(python_full_version < '3.11' and platform_machine == 'x86_64' and sys_platform == 'linux') or (python_full_version < '3.11' and sys_platform == 'win32')" }, + { name = "coloredlogs" }, + { name = "flatbuffers" }, + { name = "numpy", version = "2.2.6", source = { registry = "https://pypi.org/simple" } }, + { name = "packaging" }, + { name = "protobuf" }, + { name = "sympy", version = "1.13.1", source = { registry = "https://pypi.org/simple" }, marker = "(platform_machine != 'x86_64' and sys_platform == 'linux') or (sys_platform != 'linux' and sys_platform != 'win32')" }, + { name = "sympy", version = "1.14.0", source = { registry = "https://pypi.org/simple" }, marker = "(platform_machine == 'x86_64' and sys_platform == 'linux') or sys_platform == 'win32'" }, ] wheels = [ { url = "https://files.pythonhosted.org/packages/35/d6/311b1afea060015b56c742f3531168c1644650767f27ef40062569960587/onnxruntime-1.23.2-cp310-cp310-macosx_13_0_arm64.whl", hash = "sha256:a7730122afe186a784660f6ec5807138bf9d792fa1df76556b27307ea9ebcbe3", size = 17195934, upload-time = "2025-10-27T23:06:14.143Z" }, @@ -6418,12 +6418,12 @@ resolution-markers = [ "python_full_version == '3.11.*' and platform_machine == 'aarch64' and sys_platform != 'darwin' and sys_platform != 'linux' and sys_platform != 'win32'", ] dependencies = [ - { name = "flatbuffers", marker = "python_full_version >= '3.11'" }, - { name = "numpy", version = "2.3.5", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version >= '3.11'" }, - { name = "packaging", marker = "python_full_version >= '3.11'" }, - { name = "protobuf", marker = "python_full_version >= '3.11'" }, - { name = "sympy", version = "1.13.1", source = { registry = "https://pypi.org/simple" }, marker = "(python_full_version >= '3.11' and platform_machine != 'x86_64' and sys_platform == 'linux') or (python_full_version >= '3.11' and sys_platform != 'linux' and sys_platform != 'win32')" }, - { name = "sympy", version = "1.14.0", source = { registry = "https://pypi.org/simple" }, marker = "(python_full_version >= '3.11' and platform_machine == 'x86_64' and sys_platform == 'linux') or (python_full_version >= '3.11' and sys_platform == 'win32')" }, + { name = "flatbuffers" }, + { name = "numpy", version = "2.3.5", source = { registry = "https://pypi.org/simple" } }, + { name = "packaging" }, + { name = "protobuf" }, + { name = "sympy", version = "1.13.1", source = { registry = "https://pypi.org/simple" }, marker = "(platform_machine != 'x86_64' and sys_platform == 'linux') or (sys_platform != 'linux' and sys_platform != 'win32')" }, + { name = "sympy", version = "1.14.0", source = { registry = "https://pypi.org/simple" }, marker = "(platform_machine == 'x86_64' and sys_platform == 'linux') or sys_platform == 'win32'" }, ] wheels = [ { url = "https://files.pythonhosted.org/packages/d2/88/d9757c62a0f96b5193f8d447a141eefd14498c404cc5caf1a6f3233cf102/onnxruntime-1.24.1-cp311-cp311-macosx_14_0_arm64.whl", hash = "sha256:79b3119ab9f4f3817062e6dbe7f4a44937de93905e3a31ba34313d18cb49e7be", size = 17212018, upload-time = "2026-02-05T17:32:13.986Z" }, @@ -6447,13 +6447,13 @@ resolution-markers = [ "python_full_version < '3.11' and platform_machine == 'x86_64' and sys_platform != 'darwin' and sys_platform != 'linux' and sys_platform != 'win32'", ] dependencies = [ - { name = "coloredlogs", marker = "python_full_version < '3.11' and platform_machine == 'x86_64'" }, - { name = "flatbuffers", marker = "python_full_version < '3.11' and platform_machine == 'x86_64'" }, - { name = "numpy", version = "2.2.6", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version < '3.11' and platform_machine == 'x86_64'" }, - { name = "packaging", marker = "python_full_version < '3.11' and platform_machine == 'x86_64'" }, - { name = "protobuf", marker = "python_full_version < '3.11' and platform_machine == 'x86_64'" }, - { name = "sympy", version = "1.13.1", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version < '3.11' and platform_machine == 'x86_64' and sys_platform != 'linux' and sys_platform != 'win32'" }, - { name = "sympy", version = "1.14.0", source = { registry = "https://pypi.org/simple" }, marker = "(python_full_version < '3.11' and platform_machine == 'x86_64' and sys_platform == 'linux') or (python_full_version < '3.11' and platform_machine == 'x86_64' and sys_platform == 'win32')" }, + { name = "coloredlogs" }, + { name = "flatbuffers" }, + { name = "numpy", version = "2.2.6", source = { registry = "https://pypi.org/simple" } }, + { name = "packaging" }, + { name = "protobuf" }, + { name = "sympy", version = "1.13.1", source = { registry = "https://pypi.org/simple" }, marker = "sys_platform != 'linux' and sys_platform != 'win32'" }, + { name = "sympy", version = "1.14.0", source = { registry = "https://pypi.org/simple" }, marker = "sys_platform == 'linux' or sys_platform == 'win32'" }, ] wheels = [ { url = "https://files.pythonhosted.org/packages/cb/ae/39283748c68a96be4f5f8a9561e0e3ca92af1eae6c2b1c07fb1da5f65cd1/onnxruntime_gpu-1.23.2-cp310-cp310-manylinux_2_27_x86_64.manylinux_2_28_x86_64.whl", hash = "sha256:18de50c6c8eea50acc405ea13d299aec593e46478d7a22cd32cdbbdf7c42899d", size = 300525411, upload-time = "2025-10-22T16:56:08.415Z" }, @@ -6479,12 +6479,12 @@ resolution-markers = [ "(python_full_version == '3.11.*' and platform_machine != 'aarch64' and platform_machine != 'x86_64' and sys_platform == 'linux') or (python_full_version == '3.11.*' and platform_machine != 'aarch64' and sys_platform != 'darwin' and sys_platform != 'linux' and sys_platform != 'win32')", ] dependencies = [ - { name = "flatbuffers", marker = "python_full_version >= '3.11' and platform_machine != 'aarch64'" }, - { name = "numpy", version = "2.3.5", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version >= '3.11' and platform_machine != 'aarch64'" }, - { name = "packaging", marker = "python_full_version >= '3.11' and platform_machine != 'aarch64'" }, - { name = "protobuf", marker = "python_full_version >= '3.11' and platform_machine != 'aarch64'" }, - { name = "sympy", version = "1.13.1", source = { registry = "https://pypi.org/simple" }, marker = "(python_full_version >= '3.11' and platform_machine != 'aarch64' and platform_machine != 'x86_64' and sys_platform == 'linux') or (python_full_version >= '3.11' and platform_machine != 'aarch64' and sys_platform != 'linux' and sys_platform != 'win32')" }, - { name = "sympy", version = "1.14.0", source = { registry = "https://pypi.org/simple" }, marker = "(python_full_version >= '3.11' and platform_machine == 'x86_64' and sys_platform == 'linux') or (python_full_version >= '3.11' and platform_machine != 'aarch64' and sys_platform == 'win32')" }, + { name = "flatbuffers" }, + { name = "numpy", version = "2.3.5", source = { registry = "https://pypi.org/simple" } }, + { name = "packaging" }, + { name = "protobuf" }, + { name = "sympy", version = "1.13.1", source = { registry = "https://pypi.org/simple" }, marker = "sys_platform != 'linux' and sys_platform != 'win32'" }, + { name = "sympy", version = "1.14.0", source = { registry = "https://pypi.org/simple" }, marker = "sys_platform == 'linux' or sys_platform == 'win32'" }, ] wheels = [ { url = "https://files.pythonhosted.org/packages/ca/c7/07d06175f1124fc89e8b7da30d70eb8e0e1400d90961ae1cbea9da69e69b/onnxruntime_gpu-1.24.1-cp311-cp311-manylinux_2_27_x86_64.manylinux_2_28_x86_64.whl", hash = "sha256:ac4bfc90c376516b13d709764ab257e4e3d78639bf6a2ccfc826e9db4a5c7ddf", size = 252616647, upload-time = "2026-02-05T17:24:02.993Z" }, @@ -6519,23 +6519,23 @@ name = "open3d" version = "0.19.0" source = { registry = "https://pypi.org/simple" } dependencies = [ - { name = "addict", marker = "platform_machine != 'aarch64' or sys_platform != 'linux'" }, - { name = "configargparse", marker = "platform_machine != 'aarch64' or sys_platform != 'linux'" }, - { name = "dash", marker = "platform_machine != 'aarch64' or sys_platform != 'linux'" }, - { name = "flask", marker = "platform_machine != 'aarch64' or sys_platform != 'linux'" }, - { name = "matplotlib", marker = "platform_machine != 'aarch64' or sys_platform != 'linux'" }, - { name = "nbformat", marker = "platform_machine != 'aarch64' or sys_platform != 'linux'" }, - { name = "numpy", version = "2.2.6", source = { registry = "https://pypi.org/simple" }, marker = "(python_full_version < '3.11' and platform_machine != 'aarch64') or (python_full_version < '3.11' and sys_platform != 'linux')" }, - { name = "numpy", version = "2.3.5", source = { registry = "https://pypi.org/simple" }, marker = "(python_full_version >= '3.11' and platform_machine != 'aarch64') or (python_full_version >= '3.11' and sys_platform != 'linux')" }, - { name = "pandas", version = "2.3.3", source = { registry = "https://pypi.org/simple" }, marker = "(python_full_version < '3.11' and platform_machine != 'aarch64') or (python_full_version < '3.11' and sys_platform != 'linux')" }, - { name = "pandas", version = "3.0.0", source = { registry = "https://pypi.org/simple" }, marker = "(python_full_version >= '3.11' and platform_machine != 'aarch64') or (python_full_version >= '3.11' and sys_platform != 'linux')" }, - { name = "pillow", marker = "platform_machine != 'aarch64' or sys_platform != 'linux'" }, - { name = "pyquaternion", marker = "platform_machine != 'aarch64' or sys_platform != 'linux'" }, - { name = "pyyaml", marker = "platform_machine != 'aarch64' or sys_platform != 'linux'" }, - { name = "scikit-learn", version = "1.7.2", source = { registry = "https://pypi.org/simple" }, marker = "(python_full_version < '3.11' and platform_machine != 'aarch64') or (python_full_version < '3.11' and sys_platform != 'linux')" }, - { name = "scikit-learn", version = "1.8.0", source = { registry = "https://pypi.org/simple" }, marker = "(python_full_version >= '3.11' and platform_machine != 'aarch64') or (python_full_version >= '3.11' and sys_platform != 'linux')" }, - { name = "tqdm", marker = "platform_machine != 'aarch64' or sys_platform != 'linux'" }, - { name = "werkzeug", marker = "platform_machine != 'aarch64' or sys_platform != 'linux'" }, + { name = "addict" }, + { name = "configargparse" }, + { name = "dash" }, + { name = "flask" }, + { name = "matplotlib" }, + { name = "nbformat" }, + { name = "numpy", version = "2.2.6", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version < '3.11'" }, + { name = "numpy", version = "2.3.5", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version >= '3.11'" }, + { name = "pandas", version = "2.3.3", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version < '3.11'" }, + { name = "pandas", version = "3.0.0", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version >= '3.11'" }, + { name = "pillow" }, + { name = "pyquaternion" }, + { name = "pyyaml" }, + { name = "scikit-learn", version = "1.7.2", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version < '3.11'" }, + { name = "scikit-learn", version = "1.8.0", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version >= '3.11'" }, + { name = "tqdm" }, + { name = "werkzeug" }, ] wheels = [ { url = "https://files.pythonhosted.org/packages/5c/4b/91e8a4100adf0ccd2f7ad21dd24c2e3d8f12925396528d0462cfb1735e5a/open3d-0.19.0-cp310-cp310-macosx_10_15_universal2.whl", hash = "sha256:f7128ded206e07987cc29d0917195fb64033dea31e0d60dead3629b33d3c175f", size = 103086005, upload-time = "2025-01-08T07:25:56.755Z" }, @@ -6554,13 +6554,13 @@ name = "open3d-unofficial-arm" version = "0.19.0.post9" source = { registry = "https://pypi.org/simple" } dependencies = [ - { name = "configargparse", marker = "platform_machine == 'aarch64' and sys_platform == 'linux'" }, - { name = "dash", marker = "platform_machine == 'aarch64' and sys_platform == 'linux'" }, - { name = "flask", marker = "platform_machine == 'aarch64' and sys_platform == 'linux'" }, - { name = "nbformat", marker = "platform_machine == 'aarch64' and sys_platform == 'linux'" }, - { name = "numpy", version = "2.2.6", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version < '3.11' and platform_machine == 'aarch64' and sys_platform == 'linux'" }, - { name = "numpy", version = "2.3.5", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version >= '3.11' and platform_machine == 'aarch64' and sys_platform == 'linux'" }, - { name = "werkzeug", marker = "platform_machine == 'aarch64' and sys_platform == 'linux'" }, + { name = "configargparse" }, + { name = "dash" }, + { name = "flask" }, + { name = "nbformat" }, + { name = "numpy", version = "2.2.6", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version < '3.11'" }, + { name = "numpy", version = "2.3.5", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version >= '3.11'" }, + { name = "werkzeug" }, ] sdist = { url = "https://files.pythonhosted.org/packages/ec/f9/edcfaa213800ea278804402baa65693840bc7a323b3de8a31c54ce4e42c8/open3d_unofficial_arm-0.19.0.post9.tar.gz", hash = "sha256:ee300bd557f04750db6e47ccb6c6867c6dd6cfc04169dddeb92505da9ea739ef", size = 5327, upload-time = "2026-04-16T21:21:11.152Z" } wheels = [ @@ -6633,8 +6633,8 @@ name = "opencv-python" version = "4.13.0.92" source = { registry = "https://pypi.org/simple" } dependencies = [ - { name = "numpy", version = "2.2.6", source = { registry = "https://pypi.org/simple" }, marker = "(python_full_version < '3.11' and platform_machine != 'aarch64' and platform_machine != 'x86_64' and sys_platform == 'linux') or (python_full_version < '3.11' and sys_platform != 'darwin' and sys_platform != 'linux' and sys_platform != 'win32')" }, - { name = "numpy", version = "2.3.5", source = { registry = "https://pypi.org/simple" }, marker = "(python_full_version >= '3.11' and platform_machine != 'aarch64' and platform_machine != 'x86_64' and sys_platform == 'linux') or (python_full_version >= '3.11' and sys_platform != 'darwin' and sys_platform != 'linux' and sys_platform != 'win32')" }, + { name = "numpy", version = "2.2.6", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version < '3.11'" }, + { name = "numpy", version = "2.3.5", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version >= '3.11'" }, ] [[package]] @@ -6814,15 +6814,15 @@ name = "orbax-export" version = "0.0.8" source = { registry = "https://pypi.org/simple" } dependencies = [ - { name = "absl-py", marker = "python_full_version >= '3.11'" }, - { name = "dataclasses-json", marker = "python_full_version >= '3.11'" }, - { name = "etils", marker = "python_full_version >= '3.11'" }, - { name = "jax", version = "0.9.0.1", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version >= '3.11'" }, - { name = "jaxlib", version = "0.9.0.1", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version >= '3.11'" }, - { name = "jaxtyping", marker = "python_full_version >= '3.11'" }, - { name = "numpy", version = "2.3.5", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version >= '3.11'" }, - { name = "orbax-checkpoint", marker = "python_full_version >= '3.11'" }, - { name = "protobuf", marker = "python_full_version >= '3.11'" }, + { name = "absl-py" }, + { name = "dataclasses-json" }, + { name = "etils" }, + { name = "jax", version = "0.9.0.1", source = { registry = "https://pypi.org/simple" } }, + { name = "jaxlib", version = "0.9.0.1", source = { registry = "https://pypi.org/simple" } }, + { name = "jaxtyping" }, + { name = "numpy", version = "2.3.5", source = { registry = "https://pypi.org/simple" } }, + { name = "orbax-checkpoint" }, + { name = "protobuf" }, ] sdist = { url = "https://files.pythonhosted.org/packages/1c/c8/ed7ac3c3c687bf129d7469b016c2b3d8777379f4ea453474e50ee41ce5cb/orbax_export-0.0.8.tar.gz", hash = "sha256:544eef564e2a6f17cd11b1167febe348b7b7cf56d9575de994a33d5613dd568a", size = 124980, upload-time = "2025-09-17T15:41:14.264Z" } wheels = [ @@ -6959,10 +6959,10 @@ resolution-markers = [ "python_full_version < '3.11' and platform_machine == 'aarch64' and sys_platform != 'darwin' and sys_platform != 'linux' and sys_platform != 'win32'", ] dependencies = [ - { name = "numpy", version = "2.2.6", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version < '3.11'" }, - { name = "python-dateutil", marker = "python_full_version < '3.11'" }, - { name = "pytz", marker = "python_full_version < '3.11'" }, - { name = "tzdata", marker = "python_full_version < '3.11'" }, + { name = "numpy", version = "2.2.6", source = { registry = "https://pypi.org/simple" } }, + { name = "python-dateutil" }, + { name = "pytz" }, + { name = "tzdata" }, ] sdist = { url = "https://files.pythonhosted.org/packages/33/01/d40b85317f86cf08d853a4f495195c73815fdf205eef3993821720274518/pandas-2.3.3.tar.gz", hash = "sha256:e05e1af93b977f7eafa636d043f9f94c7ee3ac81af99c13508215942e64c993b", size = 4495223, upload-time = "2025-09-29T23:34:51.853Z" } wheels = [ @@ -7012,9 +7012,9 @@ resolution-markers = [ "python_full_version == '3.11.*' and platform_machine == 'aarch64' and sys_platform != 'darwin' and sys_platform != 'linux' and sys_platform != 'win32'", ] dependencies = [ - { name = "numpy", version = "2.3.5", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version >= '3.11'" }, - { name = "python-dateutil", marker = "python_full_version >= '3.11'" }, - { name = "tzdata", marker = "(python_full_version >= '3.11' and sys_platform == 'emscripten') or (python_full_version >= '3.11' and sys_platform == 'win32')" }, + { name = "numpy", version = "2.3.5", source = { registry = "https://pypi.org/simple" } }, + { name = "python-dateutil" }, + { name = "tzdata", marker = "sys_platform == 'emscripten' or sys_platform == 'win32'" }, ] sdist = { url = "https://files.pythonhosted.org/packages/de/da/b1dc0481ab8d55d0f46e343cfe67d4551a0e14fcee52bd38ca1bd73258d8/pandas-3.0.0.tar.gz", hash = "sha256:0facf7e87d38f721f0af46fe70d97373a37701b1c09f7ed7aeeb292ade5c050f", size = 4633005, upload-time = "2026-01-21T15:52:04.726Z" } wheels = [ @@ -7073,7 +7073,7 @@ name = "pexpect" version = "4.9.0" source = { registry = "https://pypi.org/simple" } dependencies = [ - { name = "ptyprocess", marker = "sys_platform != 'win32'" }, + { name = "ptyprocess" }, ] sdist = { url = "https://files.pythonhosted.org/packages/42/92/cc564bf6381ff43ce1f4d06852fc19a2f11d180f23dc32d9588bee2f149d/pexpect-4.9.0.tar.gz", hash = "sha256:ee7d41123f3c9911050ea2c2dac107568dc43b2d3b0c7557a33212c398ead30f", size = 166450, upload-time = "2023-11-25T09:07:26.339Z" } wheels = [ @@ -7881,7 +7881,7 @@ name = "pydot" version = "4.0.1" source = { registry = "https://pypi.org/simple" } dependencies = [ - { name = "pyparsing", marker = "platform_machine != 'aarch64'" }, + { name = "pyparsing" }, ] sdist = { url = "https://files.pythonhosted.org/packages/50/35/b17cb89ff865484c6a20ef46bf9d95a5f07328292578de0b295f4a6beec2/pydot-4.0.1.tar.gz", hash = "sha256:c2148f681c4a33e08bf0e26a9e5f8e4099a82e0e2a068098f32ce86577364ad5", size = 162594, upload-time = "2025-06-17T20:09:56.454Z" } wheels = [ @@ -8114,7 +8114,7 @@ name = "pyobjc-framework-cocoa" version = "12.1" source = { registry = "https://pypi.org/simple" } dependencies = [ - { name = "pyobjc-core", marker = "sys_platform == 'darwin'" }, + { name = "pyobjc-core" }, ] sdist = { url = "https://files.pythonhosted.org/packages/02/a3/16ca9a15e77c061a9250afbae2eae26f2e1579eb8ca9462ae2d2c71e1169/pyobjc_framework_cocoa-12.1.tar.gz", hash = "sha256:5556c87db95711b985d5efdaaf01c917ddd41d148b1e52a0c66b1a2e2c5c1640", size = 2772191, upload-time = "2025-11-14T10:13:02.069Z" } wheels = [ @@ -8128,8 +8128,8 @@ name = "pyobjc-framework-corebluetooth" version = "12.1" source = { registry = "https://pypi.org/simple" } dependencies = [ - { name = "pyobjc-core", marker = "sys_platform == 'darwin'" }, - { name = "pyobjc-framework-cocoa", marker = "sys_platform == 'darwin'" }, + { name = "pyobjc-core" }, + { name = "pyobjc-framework-cocoa" }, ] sdist = { url = "https://files.pythonhosted.org/packages/4b/25/d21d6cb3fd249c2c2aa96ee54279f40876a0c93e7161b3304bf21cbd0bfe/pyobjc_framework_corebluetooth-12.1.tar.gz", hash = "sha256:8060c1466d90bbb9100741a1091bb79975d9ba43911c9841599879fc45c2bbe0", size = 33157, upload-time = "2025-11-14T10:13:28.064Z" } wheels = [ @@ -8143,8 +8143,8 @@ name = "pyobjc-framework-libdispatch" version = "12.1" source = { registry = "https://pypi.org/simple" } dependencies = [ - { name = "pyobjc-core", marker = "sys_platform == 'darwin'" }, - { name = "pyobjc-framework-cocoa", marker = "sys_platform == 'darwin'" }, + { name = "pyobjc-core" }, + { name = "pyobjc-framework-cocoa" }, ] sdist = { url = "https://files.pythonhosted.org/packages/26/e8/75b6b9b3c88b37723c237e5a7600384ea2d84874548671139db02e76652b/pyobjc_framework_libdispatch-12.1.tar.gz", hash = "sha256:4035535b4fae1b5e976f3e0e38b6e3442ffea1b8aa178d0ca89faa9b8ecdea41", size = 38277, upload-time = "2025-11-14T10:16:46.235Z" } wheels = [ @@ -8210,8 +8210,8 @@ name = "pyquaternion" version = "0.9.9" source = { registry = "https://pypi.org/simple" } dependencies = [ - { name = "numpy", version = "2.2.6", source = { registry = "https://pypi.org/simple" }, marker = "(python_full_version < '3.11' and platform_machine != 'aarch64') or (python_full_version < '3.11' and sys_platform != 'linux')" }, - { name = "numpy", version = "2.3.5", source = { registry = "https://pypi.org/simple" }, marker = "(python_full_version >= '3.11' and platform_machine != 'aarch64') or (python_full_version >= '3.11' and sys_platform != 'linux')" }, + { name = "numpy", version = "2.2.6", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version < '3.11'" }, + { name = "numpy", version = "2.3.5", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version >= '3.11'" }, ] sdist = { url = "https://files.pythonhosted.org/packages/7d/0d/3d092aa20efaedacb89c3221a92c6491be5b28f618a2c36b52b53e7446c2/pyquaternion-0.9.9.tar.gz", hash = "sha256:b1f61af219cb2fe966b5fb79a192124f2e63a3f7a777ac3cadf2957b1a81bea8", size = 15530, upload-time = "2020-10-05T01:31:30.327Z" } wheels = [ @@ -9082,10 +9082,10 @@ resolution-markers = [ "python_full_version < '3.11' and platform_machine == 'aarch64' and sys_platform != 'darwin' and sys_platform != 'linux' and sys_platform != 'win32'", ] dependencies = [ - { name = "joblib", marker = "(python_full_version < '3.11' and platform_machine != 'aarch64') or (python_full_version < '3.11' and sys_platform != 'linux')" }, - { name = "numpy", version = "2.2.6", source = { registry = "https://pypi.org/simple" }, marker = "(python_full_version < '3.11' and platform_machine != 'aarch64') or (python_full_version < '3.11' and sys_platform != 'linux')" }, - { name = "scipy", version = "1.15.3", source = { registry = "https://pypi.org/simple" }, marker = "(python_full_version < '3.11' and platform_machine != 'aarch64') or (python_full_version < '3.11' and sys_platform != 'linux')" }, - { name = "threadpoolctl", marker = "(python_full_version < '3.11' and platform_machine != 'aarch64') or (python_full_version < '3.11' and sys_platform != 'linux')" }, + { name = "joblib" }, + { name = "numpy", version = "2.2.6", source = { registry = "https://pypi.org/simple" } }, + { name = "scipy", version = "1.15.3", source = { registry = "https://pypi.org/simple" } }, + { name = "threadpoolctl" }, ] sdist = { url = "https://files.pythonhosted.org/packages/98/c2/a7855e41c9d285dfe86dc50b250978105dce513d6e459ea66a6aeb0e1e0c/scikit_learn-1.7.2.tar.gz", hash = "sha256:20e9e49ecd130598f1ca38a1d85090e1a600147b9c02fa6f15d69cb53d968fda", size = 7193136, upload-time = "2025-09-09T08:21:29.075Z" } wheels = [ @@ -9127,10 +9127,10 @@ resolution-markers = [ "python_full_version == '3.11.*' and platform_machine == 'aarch64' and sys_platform != 'darwin' and sys_platform != 'linux' and sys_platform != 'win32'", ] dependencies = [ - { name = "joblib", marker = "(python_full_version >= '3.11' and platform_machine != 'aarch64') or (python_full_version >= '3.11' and sys_platform != 'linux')" }, - { name = "numpy", version = "2.3.5", source = { registry = "https://pypi.org/simple" }, marker = "(python_full_version >= '3.11' and platform_machine != 'aarch64') or (python_full_version >= '3.11' and sys_platform != 'linux')" }, - { name = "scipy", version = "1.17.0", source = { registry = "https://pypi.org/simple" }, marker = "(python_full_version >= '3.11' and platform_machine != 'aarch64') or (python_full_version >= '3.11' and sys_platform != 'linux')" }, - { name = "threadpoolctl", marker = "(python_full_version >= '3.11' and platform_machine != 'aarch64') or (python_full_version >= '3.11' and sys_platform != 'linux')" }, + { name = "joblib" }, + { name = "numpy", version = "2.3.5", source = { registry = "https://pypi.org/simple" } }, + { name = "scipy", version = "1.17.0", source = { registry = "https://pypi.org/simple" } }, + { name = "threadpoolctl" }, ] sdist = { url = "https://files.pythonhosted.org/packages/0e/d4/40988bf3b8e34feec1d0e6a051446b1f66225f8529b9309becaeef62b6c4/scikit_learn-1.8.0.tar.gz", hash = "sha256:9bccbb3b40e3de10351f8f5068e105d0f4083b1a65fa07b6634fbc401a6287fd", size = 7335585, upload-time = "2025-12-10T07:08:53.618Z" } wheels = [ @@ -9166,7 +9166,7 @@ resolution-markers = [ "python_full_version < '3.11' and platform_machine == 'aarch64' and sys_platform != 'darwin' and sys_platform != 'linux' and sys_platform != 'win32'", ] dependencies = [ - { name = "numpy", version = "2.2.6", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version < '3.11'" }, + { name = "numpy", version = "2.2.6", source = { registry = "https://pypi.org/simple" } }, ] sdist = { url = "https://files.pythonhosted.org/packages/0f/37/6964b830433e654ec7485e45a00fc9a27cf868d622838f6b6d9c5ec0d532/scipy-1.15.3.tar.gz", hash = "sha256:eae3cf522bc7df64b42cad3925c876e1b0b6c35c1337c93e12c0f366f55b0eaf", size = 59419214, upload-time = "2025-05-08T16:13:05.955Z" } wheels = [ @@ -9222,7 +9222,7 @@ resolution-markers = [ "python_full_version == '3.11.*' and platform_machine == 'aarch64' and sys_platform != 'darwin' and sys_platform != 'linux' and sys_platform != 'win32'", ] dependencies = [ - { name = "numpy", version = "2.3.5", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version >= '3.11'" }, + { name = "numpy", version = "2.3.5", source = { registry = "https://pypi.org/simple" } }, ] sdist = { url = "https://files.pythonhosted.org/packages/56/3e/9cca699f3486ce6bc12ff46dc2031f1ec8eb9ccc9a320fdaf925f1417426/scipy-1.17.0.tar.gz", hash = "sha256:2591060c8e648d8b96439e111ac41fd8342fdeff1876be2e19dea3fe8930454e", size = 30396830, upload-time = "2026-01-10T21:34:23.009Z" } wheels = [ @@ -9253,8 +9253,8 @@ name = "secretstorage" version = "3.5.0" source = { registry = "https://pypi.org/simple" } dependencies = [ - { name = "cryptography", marker = "(python_full_version >= '3.11' and platform_machine == 'x86_64' and sys_platform != 'darwin' and sys_platform != 'win32') or (platform_machine != 'aarch64' and platform_machine != 'x86_64' and sys_platform != 'darwin' and sys_platform != 'win32') or sys_platform == 'linux'" }, - { name = "jeepney", marker = "(python_full_version >= '3.11' and platform_machine == 'x86_64' and sys_platform != 'darwin' and sys_platform != 'win32') or (platform_machine != 'aarch64' and platform_machine != 'x86_64' and sys_platform != 'darwin' and sys_platform != 'win32') or sys_platform == 'linux'" }, + { name = "cryptography" }, + { name = "jeepney" }, ] sdist = { url = "https://files.pythonhosted.org/packages/1c/03/e834bcd866f2f8a49a85eaff47340affa3bfa391ee9912a952a1faa68c7b/secretstorage-3.5.0.tar.gz", hash = "sha256:f04b8e4689cbce351744d5537bf6b1329c6fc68f91fa666f60a380edddcd11be", size = 19884, upload-time = "2025-11-23T19:02:53.191Z" } wheels = [ @@ -9676,7 +9676,7 @@ resolution-markers = [ "python_full_version < '3.11' and platform_machine == 'aarch64' and sys_platform != 'darwin' and sys_platform != 'linux' and sys_platform != 'win32'", ] dependencies = [ - { name = "mpmath", marker = "(platform_machine != 'x86_64' and sys_platform == 'linux') or (sys_platform != 'linux' and sys_platform != 'win32')" }, + { name = "mpmath" }, ] sdist = { url = "https://files.pythonhosted.org/packages/ca/99/5a5b6f19ff9f083671ddf7b9632028436167cd3d33e11015754e41b249a4/sympy-1.13.1.tar.gz", hash = "sha256:9cebf7e04ff162015ce31c9c6c9144daa34a93bd082f54fd8f12deca4f47515f", size = 7533040, upload-time = "2024-07-19T09:26:51.238Z" } wheels = [ @@ -9700,7 +9700,7 @@ resolution-markers = [ "python_full_version < '3.11' and platform_machine == 'aarch64' and sys_platform == 'win32'", ] dependencies = [ - { name = "mpmath", marker = "(platform_machine == 'x86_64' and sys_platform == 'linux') or sys_platform == 'win32'" }, + { name = "mpmath" }, ] sdist = { url = "https://files.pythonhosted.org/packages/83/d3/803453b36afefb7c2bb238361cd4ae6125a569b4db67cd9e79846ba2d68c/sympy-1.14.0.tar.gz", hash = "sha256:d3d3fe8df1e5a0b42f0e7bdf50541697dbe7d23746e894990c030e2b05e72517", size = 7793921, upload-time = "2025-04-27T18:05:01.611Z" } wheels = [ @@ -9780,8 +9780,8 @@ resolution-markers = [ "python_full_version < '3.11' and platform_machine == 'aarch64' and sys_platform != 'darwin' and sys_platform != 'linux' and sys_platform != 'win32'", ] dependencies = [ - { name = "ml-dtypes", marker = "python_full_version < '3.11'" }, - { name = "numpy", version = "2.2.6", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version < '3.11'" }, + { name = "ml-dtypes" }, + { name = "numpy", version = "2.2.6", source = { registry = "https://pypi.org/simple" } }, ] sdist = { url = "https://files.pythonhosted.org/packages/9f/ee/05eb424437f4db63331c90e4605025eedc0f71da3faff97161d5d7b405af/tensorstore-0.1.78.tar.gz", hash = "sha256:e26074ffe462394cf54197eb76d6569b500f347573cd74da3f4dd5f510a4ad7c", size = 6913502, upload-time = "2025-10-06T17:44:29.649Z" } wheels = [ @@ -9825,8 +9825,8 @@ resolution-markers = [ "python_full_version == '3.11.*' and platform_machine == 'aarch64' and sys_platform != 'darwin' and sys_platform != 'linux' and sys_platform != 'win32'", ] dependencies = [ - { name = "ml-dtypes", marker = "python_full_version >= '3.11'" }, - { name = "numpy", version = "2.3.5", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version >= '3.11'" }, + { name = "ml-dtypes" }, + { name = "numpy", version = "2.3.5", source = { registry = "https://pypi.org/simple" } }, ] sdist = { url = "https://files.pythonhosted.org/packages/43/f6/e2403fc05b97ba74ad408a98a42c288e6e1b8eacc23780c153b0e5166179/tensorstore-0.1.81.tar.gz", hash = "sha256:687546192ea6f6c8ae28d18f13103336f68017d928b9f5a00325e9b0548d9c25", size = 7120819, upload-time = "2026-02-06T18:56:12.535Z" } wheels = [ @@ -10036,14 +10036,14 @@ resolution-markers = [ "python_full_version < '3.11' and platform_machine == 'aarch64' and sys_platform != 'darwin' and sys_platform != 'linux' and sys_platform != 'win32'", ] dependencies = [ - { name = "filelock", marker = "(platform_machine != 'x86_64' and sys_platform == 'linux') or (sys_platform != 'linux' and sys_platform != 'win32')" }, - { name = "fsspec", marker = "(platform_machine != 'x86_64' and sys_platform == 'linux') or (sys_platform != 'linux' and sys_platform != 'win32')" }, - { name = "jinja2", marker = "(platform_machine != 'x86_64' and sys_platform == 'linux') or (sys_platform != 'linux' and sys_platform != 'win32')" }, - { name = "networkx", version = "3.4.2", source = { registry = "https://pypi.org/simple" }, marker = "(python_full_version < '3.11' and platform_machine != 'x86_64' and sys_platform == 'linux') or (python_full_version < '3.11' and sys_platform != 'linux' and sys_platform != 'win32')" }, - { name = "networkx", version = "3.6.1", source = { registry = "https://pypi.org/simple" }, marker = "(python_full_version >= '3.11' and platform_machine != 'x86_64' and sys_platform == 'linux') or (python_full_version >= '3.11' and sys_platform != 'linux' and sys_platform != 'win32')" }, - { name = "setuptools", marker = "(python_full_version >= '3.12' and platform_machine != 'x86_64' and sys_platform == 'linux') or (python_full_version >= '3.12' and sys_platform != 'linux' and sys_platform != 'win32')" }, - { name = "sympy", version = "1.13.1", source = { registry = "https://pypi.org/simple" }, marker = "(platform_machine != 'x86_64' and sys_platform == 'linux') or (sys_platform != 'linux' and sys_platform != 'win32')" }, - { name = "typing-extensions", marker = "(platform_machine != 'x86_64' and sys_platform == 'linux') or (sys_platform != 'linux' and sys_platform != 'win32')" }, + { name = "filelock" }, + { name = "fsspec" }, + { name = "jinja2" }, + { name = "networkx", version = "3.4.2", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version < '3.11'" }, + { name = "networkx", version = "3.6.1", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version >= '3.11'" }, + { name = "setuptools", marker = "python_full_version >= '3.12'" }, + { name = "sympy", version = "1.13.1", source = { registry = "https://pypi.org/simple" } }, + { name = "typing-extensions" }, ] wheels = [ { url = "https://files.pythonhosted.org/packages/37/81/aa9ab58ec10264c1abe62c8b73f5086c3c558885d6beecebf699f0dbeaeb/torch-2.6.0-cp310-cp310-manylinux1_x86_64.whl", hash = "sha256:6860df13d9911ac158f4c44031609700e1eba07916fff62e21e6ffa0a9e01961", size = 766685561, upload-time = "2025-01-29T16:19:12.12Z" }, @@ -10074,29 +10074,29 @@ resolution-markers = [ "python_full_version < '3.11' and platform_machine == 'aarch64' and sys_platform == 'win32'", ] dependencies = [ - { name = "filelock", marker = "(platform_machine == 'x86_64' and sys_platform == 'linux') or sys_platform == 'win32'" }, - { name = "fsspec", marker = "(platform_machine == 'x86_64' and sys_platform == 'linux') or sys_platform == 'win32'" }, - { name = "jinja2", marker = "(platform_machine == 'x86_64' and sys_platform == 'linux') or sys_platform == 'win32'" }, - { name = "networkx", version = "3.4.2", source = { registry = "https://pypi.org/simple" }, marker = "(python_full_version < '3.11' and platform_machine == 'x86_64' and sys_platform == 'linux') or (python_full_version < '3.11' and sys_platform == 'win32')" }, - { name = "networkx", version = "3.6.1", source = { registry = "https://pypi.org/simple" }, marker = "(python_full_version >= '3.11' and platform_machine == 'x86_64' and sys_platform == 'linux') or (python_full_version >= '3.11' and sys_platform == 'win32')" }, - { name = "nvidia-cublas-cu12", marker = "platform_machine == 'x86_64' and sys_platform == 'linux'" }, - { name = "nvidia-cuda-cupti-cu12", marker = "platform_machine == 'x86_64' and sys_platform == 'linux'" }, - { name = "nvidia-cuda-nvrtc-cu12", marker = "platform_machine == 'x86_64' and sys_platform == 'linux'" }, - { name = "nvidia-cuda-runtime-cu12", marker = "platform_machine == 'x86_64' and sys_platform == 'linux'" }, - { name = "nvidia-cudnn-cu12", marker = "platform_machine == 'x86_64' and sys_platform == 'linux'" }, - { name = "nvidia-cufft-cu12", marker = "platform_machine == 'x86_64' and sys_platform == 'linux'" }, - { name = "nvidia-cufile-cu12", marker = "platform_machine == 'x86_64' and sys_platform == 'linux'" }, - { name = "nvidia-curand-cu12", marker = "platform_machine == 'x86_64' and sys_platform == 'linux'" }, - { name = "nvidia-cusolver-cu12", marker = "platform_machine == 'x86_64' and sys_platform == 'linux'" }, - { name = "nvidia-cusparse-cu12", marker = "platform_machine == 'x86_64' and sys_platform == 'linux'" }, - { name = "nvidia-cusparselt-cu12", marker = "platform_machine == 'x86_64' and sys_platform == 'linux'" }, - { name = "nvidia-nccl-cu12", marker = "platform_machine == 'x86_64' and sys_platform == 'linux'" }, - { name = "nvidia-nvjitlink-cu12", marker = "platform_machine == 'x86_64' and sys_platform == 'linux'" }, - { name = "nvidia-nvtx-cu12", marker = "platform_machine == 'x86_64' and sys_platform == 'linux'" }, - { name = "setuptools", marker = "(python_full_version >= '3.12' and platform_machine == 'x86_64' and sys_platform == 'linux') or (python_full_version >= '3.12' and sys_platform == 'win32')" }, - { name = "sympy", version = "1.14.0", source = { registry = "https://pypi.org/simple" }, marker = "(platform_machine == 'x86_64' and sys_platform == 'linux') or sys_platform == 'win32'" }, - { name = "triton", version = "3.3.1", source = { registry = "https://pypi.org/simple" }, marker = "platform_machine == 'x86_64' and sys_platform == 'linux'" }, - { name = "typing-extensions", marker = "(platform_machine == 'x86_64' and sys_platform == 'linux') or sys_platform == 'win32'" }, + { name = "filelock" }, + { name = "fsspec" }, + { name = "jinja2" }, + { name = "networkx", version = "3.4.2", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version < '3.11'" }, + { name = "networkx", version = "3.6.1", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version >= '3.11'" }, + { name = "nvidia-cublas-cu12", marker = "sys_platform != 'win32'" }, + { name = "nvidia-cuda-cupti-cu12", marker = "sys_platform != 'win32'" }, + { name = "nvidia-cuda-nvrtc-cu12", marker = "sys_platform != 'win32'" }, + { name = "nvidia-cuda-runtime-cu12", marker = "sys_platform != 'win32'" }, + { name = "nvidia-cudnn-cu12", marker = "sys_platform != 'win32'" }, + { name = "nvidia-cufft-cu12", marker = "sys_platform != 'win32'" }, + { name = "nvidia-cufile-cu12", marker = "sys_platform != 'win32'" }, + { name = "nvidia-curand-cu12", marker = "sys_platform != 'win32'" }, + { name = "nvidia-cusolver-cu12", marker = "sys_platform != 'win32'" }, + { name = "nvidia-cusparse-cu12", marker = "sys_platform != 'win32'" }, + { name = "nvidia-cusparselt-cu12", marker = "sys_platform != 'win32'" }, + { name = "nvidia-nccl-cu12", marker = "sys_platform != 'win32'" }, + { name = "nvidia-nvjitlink-cu12", marker = "sys_platform != 'win32'" }, + { name = "nvidia-nvtx-cu12", marker = "sys_platform != 'win32'" }, + { name = "setuptools", marker = "python_full_version >= '3.12'" }, + { name = "sympy", version = "1.14.0", source = { registry = "https://pypi.org/simple" } }, + { name = "triton", version = "3.3.1", source = { registry = "https://pypi.org/simple" }, marker = "sys_platform != 'win32'" }, + { name = "typing-extensions" }, ] wheels = [ { url = "https://download-r2.pytorch.org/whl/cu128/torch-2.7.1%2Bcu128-cp310-cp310-manylinux_2_28_x86_64.whl", hash = "sha256:d6c3cba198dc93f93422a8545f48a6697890366e4b9701f54351fc27e2304bd3", upload-time = "2025-06-03T18:30:47Z" }, @@ -10137,10 +10137,10 @@ resolution-markers = [ "python_full_version < '3.11' and platform_machine == 'aarch64' and sys_platform != 'darwin' and sys_platform != 'linux' and sys_platform != 'win32'", ] dependencies = [ - { name = "numpy", version = "2.2.6", source = { registry = "https://pypi.org/simple" }, marker = "(python_full_version < '3.11' and platform_machine != 'x86_64' and sys_platform == 'linux') or (python_full_version < '3.11' and sys_platform != 'linux' and sys_platform != 'win32')" }, - { name = "numpy", version = "2.3.5", source = { registry = "https://pypi.org/simple" }, marker = "(python_full_version >= '3.11' and platform_machine != 'x86_64' and sys_platform == 'linux') or (python_full_version >= '3.11' and sys_platform != 'linux' and sys_platform != 'win32')" }, - { name = "pillow", marker = "(platform_machine != 'x86_64' and sys_platform == 'linux') or (sys_platform != 'linux' and sys_platform != 'win32')" }, - { name = "torch", version = "2.6.0", source = { registry = "https://pypi.org/simple" }, marker = "(platform_machine != 'x86_64' and sys_platform == 'linux') or (sys_platform != 'linux' and sys_platform != 'win32')" }, + { name = "numpy", version = "2.2.6", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version < '3.11'" }, + { name = "numpy", version = "2.3.5", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version >= '3.11'" }, + { name = "pillow" }, + { name = "torch", version = "2.6.0", source = { registry = "https://pypi.org/simple" } }, ] wheels = [ { url = "https://files.pythonhosted.org/packages/a9/20/72eb0b5b08fa293f20fc41c374e37cf899f0033076f0144d2cdc48f9faee/torchvision-0.21.0-1-cp310-cp310-manylinux_2_28_aarch64.whl", hash = "sha256:5568c5a1ff1b2ec33127b629403adb530fab81378d9018ca4ed6508293f76e2b", size = 2327643, upload-time = "2025-03-18T17:25:51.165Z" }, @@ -10174,10 +10174,10 @@ resolution-markers = [ "python_full_version < '3.11' and platform_machine == 'aarch64' and sys_platform == 'win32'", ] dependencies = [ - { name = "numpy", version = "2.2.6", source = { registry = "https://pypi.org/simple" }, marker = "(python_full_version < '3.11' and platform_machine == 'x86_64' and sys_platform == 'linux') or (python_full_version < '3.11' and sys_platform == 'win32')" }, - { name = "numpy", version = "2.3.5", source = { registry = "https://pypi.org/simple" }, marker = "(python_full_version >= '3.11' and platform_machine == 'x86_64' and sys_platform == 'linux') or (python_full_version >= '3.11' and sys_platform == 'win32')" }, - { name = "pillow", marker = "(platform_machine == 'x86_64' and sys_platform == 'linux') or sys_platform == 'win32'" }, - { name = "torch", version = "2.7.1+cu128", source = { registry = "https://download.pytorch.org/whl/cu128" }, marker = "(platform_machine == 'x86_64' and sys_platform == 'linux') or sys_platform == 'win32'" }, + { name = "numpy", version = "2.2.6", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version < '3.11'" }, + { name = "numpy", version = "2.3.5", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version >= '3.11'" }, + { name = "pillow" }, + { name = "torch", version = "2.7.1+cu128", source = { registry = "https://download.pytorch.org/whl/cu128" } }, ] wheels = [ { url = "https://download-r2.pytorch.org/whl/cu128/torchvision-0.22.1%2Bcu128-cp310-cp310-manylinux_2_28_x86_64.whl", hash = "sha256:538f4db667286d939b4eee0a66d31ed21b51186668006b0e0ffe20338ecc7e00", upload-time = "2025-06-03T18:37:28Z" }, @@ -10326,7 +10326,7 @@ resolution-markers = [ "python_full_version < '3.11' and platform_machine == 'x86_64' and sys_platform == 'linux'", ] dependencies = [ - { name = "setuptools", marker = "platform_machine == 'x86_64' and sys_platform == 'linux'" }, + { name = "setuptools" }, ] wheels = [ { url = "https://files.pythonhosted.org/packages/8d/a9/549e51e9b1b2c9b854fd761a1d23df0ba2fbc60bd0c13b489ffa518cfcb7/triton-3.3.1-cp310-cp310-manylinux_2_27_x86_64.manylinux_2_28_x86_64.whl", hash = "sha256:b74db445b1c562844d3cfad6e9679c72e93fdfb1a90a24052b03bb5c49d1242e", size = 155600257, upload-time = "2025-05-29T23:39:36.085Z" }, @@ -10344,7 +10344,7 @@ resolution-markers = [ "python_full_version < '3.11' and platform_machine == 'aarch64' and sys_platform != 'darwin' and sys_platform != 'linux' and sys_platform != 'win32'", ] dependencies = [ - { name = "importlib-metadata", marker = "platform_machine == 'aarch64' and sys_platform != 'darwin' and sys_platform != 'linux' and sys_platform != 'win32'" }, + { name = "importlib-metadata" }, ] [[package]] @@ -10424,8 +10424,8 @@ name = "typing-inspect" version = "0.9.0" source = { registry = "https://pypi.org/simple" } dependencies = [ - { name = "mypy-extensions", marker = "python_full_version >= '3.11'" }, - { name = "typing-extensions", marker = "python_full_version >= '3.11'" }, + { name = "mypy-extensions" }, + { name = "typing-extensions" }, ] sdist = { url = "https://files.pythonhosted.org/packages/dc/74/1789779d91f1961fa9438e9a8710cdae6bd138c80d7303996933d117264a/typing_inspect-0.9.0.tar.gz", hash = "sha256:b23fc42ff6f6ef6954e4852c1fb512cdd18dbea03134f91f856a95ccc9461f78", size = 13825, upload-time = "2023-05-24T20:25:47.612Z" } wheels = [ @@ -10992,12 +10992,24 @@ wheels = [ { url = "https://files.pythonhosted.org/packages/8e/93/af1d6ccb69ab6b5a00e03fa0cefa563f9862412667776ea15dd4eece3a90/whatthepatch-1.0.7-py3-none-any.whl", hash = "sha256:1b6f655fd31091c001c209529dfaabbabdbad438f5de14e3951266ea0fc6e7ed", size = 11964, upload-time = "2024-11-16T17:21:20.761Z" }, ] +[[package]] +name = "wheel" +version = "0.48.0" +source = { registry = "https://pypi.org/simple" } +dependencies = [ + { name = "packaging" }, +] +sdist = { url = "https://files.pythonhosted.org/packages/d0/20/50ed6bdf27dec98b568a8ae25dc599f35baa3d9709f9e83fd1edb56b9a90/wheel-0.48.0.tar.gz", hash = "sha256:94800765601e9171bf5d58d066e640662842bcedcbab982b2c90787a2c987322", size = 66471, upload-time = "2026-08-11T22:02:27.327Z" } +wheels = [ + { url = "https://files.pythonhosted.org/packages/2e/29/69cfbb602cd91690c55d38ba9fe53e6a7e76a6fa647bf38f19c138d25449/wheel-0.48.0-py3-none-any.whl", hash = "sha256:3217dcc807155e45db462d7ef2431f5ddda0d7273b700d05a67b271ceb1287ab", size = 33320, upload-time = "2026-08-11T22:02:26.1Z" }, +] + [[package]] name = "winrt-runtime" version = "3.2.1" source = { registry = "https://pypi.org/simple" } dependencies = [ - { name = "typing-extensions", marker = "sys_platform == 'win32'" }, + { name = "typing-extensions" }, ] sdist = { url = "https://files.pythonhosted.org/packages/16/dd/acdd527c1d890c8f852cc2af644aa6c160974e66631289420aa871b05e65/winrt_runtime-3.2.1.tar.gz", hash = "sha256:c8dca19e12b234ae6c3dadf1a4d0761b51e708457492c13beb666556958801ea", size = 21721, upload-time = "2025-06-06T14:40:27.593Z" } wheels = [ @@ -11017,7 +11029,7 @@ name = "winrt-windows-devices-bluetooth" version = "3.2.1" source = { registry = "https://pypi.org/simple" } dependencies = [ - { name = "winrt-runtime", marker = "sys_platform == 'win32'" }, + { name = "winrt-runtime" }, ] sdist = { url = "https://files.pythonhosted.org/packages/b2/a0/1c8a0c469abba7112265c6cb52f0090d08a67c103639aee71fc690e614b8/winrt_windows_devices_bluetooth-3.2.1.tar.gz", hash = "sha256:db496d2d92742006d5a052468fc355bf7bb49e795341d695c374746113d74505", size = 23732, upload-time = "2025-06-06T14:41:20.489Z" } wheels = [ @@ -11037,7 +11049,7 @@ name = "winrt-windows-devices-bluetooth-advertisement" version = "3.2.1" source = { registry = "https://pypi.org/simple" } dependencies = [ - { name = "winrt-runtime", marker = "sys_platform == 'win32'" }, + { name = "winrt-runtime" }, ] sdist = { url = "https://files.pythonhosted.org/packages/06/fc/7ffe66ca4109b9e994b27c00f3d2d506e6e549e268791f755287ad9106d8/winrt_windows_devices_bluetooth_advertisement-3.2.1.tar.gz", hash = "sha256:0223852a7b7fa5c8dea3c6a93473bd783df4439b1ed938d9871f947933e574cc", size = 16906, upload-time = "2025-06-06T14:41:21.448Z" } wheels = [ @@ -11057,7 +11069,7 @@ name = "winrt-windows-devices-bluetooth-genericattributeprofile" version = "3.2.1" source = { registry = "https://pypi.org/simple" } dependencies = [ - { name = "winrt-runtime", marker = "sys_platform == 'win32'" }, + { name = "winrt-runtime" }, ] sdist = { url = "https://files.pythonhosted.org/packages/44/21/aeeddc0eccdfbd25e543360b5cc093233e2eab3cdfb53ad3cabae1b5d04d/winrt_windows_devices_bluetooth_genericattributeprofile-3.2.1.tar.gz", hash = "sha256:cdf6ddc375e9150d040aca67f5a17c41ceaf13a63f3668f96608bc1d045dde71", size = 38896, upload-time = "2025-06-06T14:41:22.687Z" } wheels = [ @@ -11077,7 +11089,7 @@ name = "winrt-windows-devices-enumeration" version = "3.2.1" source = { registry = "https://pypi.org/simple" } dependencies = [ - { name = "winrt-runtime", marker = "sys_platform == 'win32'" }, + { name = "winrt-runtime" }, ] sdist = { url = "https://files.pythonhosted.org/packages/9e/dd/75835bfbd063dffa152109727dedbd80f6e92ea284855f7855d48cdf31c9/winrt_windows_devices_enumeration-3.2.1.tar.gz", hash = "sha256:df316899e39bfc0ffc1f3cb0f5ee54d04e1d167fbbcc1484d2d5121449a935cf", size = 23538, upload-time = "2025-06-06T14:41:26.787Z" } wheels = [ @@ -11097,7 +11109,7 @@ name = "winrt-windows-devices-radios" version = "3.2.1" source = { registry = "https://pypi.org/simple" } dependencies = [ - { name = "winrt-runtime", marker = "sys_platform == 'win32'" }, + { name = "winrt-runtime" }, ] sdist = { url = "https://files.pythonhosted.org/packages/5e/02/9704ea359ad8b0d6faa1011f98fb477e8fb6eac5201f39d19e73c2407e7b/winrt_windows_devices_radios-3.2.1.tar.gz", hash = "sha256:4dc9b9d1501846049eb79428d64ec698d6476c27a357999b78a8331072e18a0b", size = 5908, upload-time = "2025-06-06T14:41:44.868Z" } wheels = [ @@ -11117,7 +11129,7 @@ name = "winrt-windows-foundation" version = "3.2.1" source = { registry = "https://pypi.org/simple" } dependencies = [ - { name = "winrt-runtime", marker = "sys_platform == 'win32'" }, + { name = "winrt-runtime" }, ] sdist = { url = "https://files.pythonhosted.org/packages/0c/55/098ce7ea0679efcc1298b269c48768f010b6c68f90c588f654ec874c8a74/winrt_windows_foundation-3.2.1.tar.gz", hash = "sha256:ad2f1fcaa6c34672df45527d7c533731fdf65b67c4638c2b4aca949f6eec0656", size = 30485, upload-time = "2025-06-06T14:41:53.344Z" } wheels = [ @@ -11137,7 +11149,7 @@ name = "winrt-windows-foundation-collections" version = "3.2.1" source = { registry = "https://pypi.org/simple" } dependencies = [ - { name = "winrt-runtime", marker = "sys_platform == 'win32'" }, + { name = "winrt-runtime" }, ] sdist = { url = "https://files.pythonhosted.org/packages/ef/62/d21e3f1eeb8d47077887bbf0c3882c49277a84d8f98f7c12bda64d498a07/winrt_windows_foundation_collections-3.2.1.tar.gz", hash = "sha256:0eff1ad0d8d763ad17e9e7bbd0c26a62b27215016393c05b09b046d6503ae6d5", size = 16043, upload-time = "2025-06-06T14:41:53.983Z" } wheels = [ @@ -11157,7 +11169,7 @@ name = "winrt-windows-storage-streams" version = "3.2.1" source = { registry = "https://pypi.org/simple" } dependencies = [ - { name = "winrt-runtime", marker = "sys_platform == 'win32'" }, + { name = "winrt-runtime" }, ] sdist = { url = "https://files.pythonhosted.org/packages/00/50/f4488b07281566e3850fcae1021f0285c9653992f60a915e15567047db63/winrt_windows_storage_streams-3.2.1.tar.gz", hash = "sha256:476f522722751eb0b571bc7802d85a82a3cae8b1cce66061e6e758f525e7b80f", size = 34335, upload-time = "2025-06-06T14:43:23.905Z" } wheels = [ diff --git a/web/cockpit/src/App.test.tsx b/web/cockpit/src/App.test.tsx index a1b8c0d0231..4c390a79a2f 100644 --- a/web/cockpit/src/App.test.tsx +++ b/web/cockpit/src/App.test.tsx @@ -35,17 +35,17 @@ const IMAGE: ChannelSpec = { requiredScope: null, }; -// A *.lcm.v1 channel decodes from the schema in its params; one without a +// A *.cdr.v1 channel decodes from the schema in its params; one without a // usable schema has no decoder at all. -const LCM_POSE: ChannelSpec = { +const CDR_POSE: ChannelSpec = { ...ODOM, - ch: "lcm_pose", - encoding: "geometry_msgs.PoseStamped.lcm.v1", + ch: "cdr_pose", + encoding: "t/msg/P.cdr.v1", params: { - lcm: { type: "t.P", fp: "0011223344556677", structs: { "t.P": [["x", "double", null]] } }, + cdr: { type: "t/msg/P", definition: "float64 x" }, }, }; -const LCM_BROKEN: ChannelSpec = { ...ODOM, ch: "lcm_bad", encoding: "t.Q.lcm.v1" }; +const CDR_BROKEN: ChannelSpec = { ...ODOM, ch: "cdr_bad", encoding: "t/msg/Q.cdr.v1" }; function mf(channels: ChannelSpec[], panels: PanelSpec[] = []): Manifest { return { version: 1, channels, panels, layout: null, pages: [] }; @@ -107,13 +107,13 @@ describe("App session states", () => { const switchButton = () => container.querySelector('[data-testid="switch-robot"]'); const panel = () => container.querySelector('[data-testid="panel-cam"]'); - it("shows *.lcm.v1 rows from their manifest schema, not a registered decoder", () => { + it("shows *.cdr.v1 rows from their manifest schema, not a registered decoder", () => { act(() => { status.update({ watchedRobot: ROBOT, robots: [ROBOT] }); - status.update({ manifest: mf([LCM_POSE, LCM_BROKEN]) }); + status.update({ manifest: mf([CDR_POSE, CDR_BROKEN]) }); channels.ingest( - "lcm_pose", - { ch: "lcm_pose", seq: 3, ts: 0.3, delivery: "reliable" }, + "cdr_pose", + { ch: "cdr_pose", seq: 3, ts: 0.3, delivery: "reliable" }, { x: 1.5 }, true, "{x: 1.5}", @@ -121,12 +121,12 @@ describe("App session states", () => { channels.publishUi(); }); view("channels"); - expect(container.querySelector('[data-testid="ch-lcm_pose-seq"]')!.textContent).toBe("3"); - expect(container.querySelector('[data-testid="ch-lcm_pose-value"]')!.textContent).toContain( + expect(container.querySelector('[data-testid="ch-cdr_pose-seq"]')!.textContent).toBe("3"); + expect(container.querySelector('[data-testid="ch-cdr_pose-value"]')!.textContent).toContain( "{x: 1.5}", ); - expect(container.querySelector('[data-testid="ch-lcm_bad-value"]')!.textContent).toContain( - "no decoder for t.Q.lcm.v1", + expect(container.querySelector('[data-testid="ch-cdr_bad-value"]')!.textContent).toContain( + "no decoder for t/msg/Q.cdr.v1", ); }); diff --git a/web/cockpit/src/subscriptions.test.ts b/web/cockpit/src/subscriptions.test.ts index 6108d119285..4e4b77ac2bd 100644 --- a/web/cockpit/src/subscriptions.test.ts +++ b/web/cockpit/src/subscriptions.test.ts @@ -38,23 +38,22 @@ const odom = spec(); const jpeg = spec({ ch: "color_image", encoding: "jpeg.v1", delivery: "latest" }); const costmap = spec({ ch: "global_costmap", encoding: "costmap.zlib.v1", delivery: "latest" }); const future = spec({ ch: "voxels", encoding: "voxels.bin.v9", delivery: "latest" }); -const lcm = spec({ - ch: "lcm_pose", - encoding: "geometry_msgs.PoseStamped.lcm.v1", +const cdr = spec({ + ch: "cdr_pose", + encoding: "t/msg/P.cdr.v1", params: { - lcm: { type: "t.P", fp: "0011223344556677", structs: { "t.P": [["x", "double", null]] } }, + cdr: { type: "t/msg/P", definition: "float64 x" }, }, }); -const lcmBroken = spec({ ch: "lcm_bad", encoding: "t.Q.lcm.v1", params: {} }); +const cdrBroken = spec({ ch: "cdr_bad", encoding: "t/msg/Q.cdr.v1", params: {} }); // A variable-length array: every frame costs the message's full size. -const lcmCloud = spec({ - ch: "lcm_cloud", - encoding: "sensor_msgs.PointCloud2.lcm.v1", +const cdrCloud = spec({ + ch: "cdr_cloud", + encoding: "t/msg/C.cdr.v1", params: { - lcm: { - type: "t.C", - fp: "0011223344556677", - structs: { "t.C": [["n", "int32_t", null], ["data", "byte", ["n"]]] }, + cdr: { + type: "t/msg/C", + definition: "uint8[] data", }, }, }); @@ -67,15 +66,15 @@ describe("subscribableChannels", () => { expect(subscribableChannels([future], [])).toEqual([]); }); - it("subscribes bounded *.lcm.v1 schemas by itself, bulk ones only through a panel", () => { - expect(channelSubscribable(lcm, [])).toBe(true); - expect(channelSubscribable(lcm, [videoPanel])).toBe(true); - expect(channelSubscribable(lcmCloud, [])).toBe(false); - const cloudPanel = panel({ id: "cloud", kind: "video", channels: ["lcm_cloud"] }); - expect(channelSubscribable(lcmCloud, [cloudPanel])).toBe(true); + it("subscribes bounded *.cdr.v1 schemas by itself, bulk ones only through a panel", () => { + expect(channelSubscribable(cdr, [])).toBe(true); + expect(channelSubscribable(cdr, [videoPanel])).toBe(true); + expect(channelSubscribable(cdrCloud, [])).toBe(false); + const cloudPanel = panel({ id: "cloud", kind: "video", channels: ["cdr_cloud"] }); + expect(channelSubscribable(cdrCloud, [cloudPanel])).toBe(true); // No schema in params: nothing can decode it, so nothing subscribes. - expect(channelSubscribable(lcmBroken, [])).toBe(false); - expect(subscribableChannels([lcm, lcmCloud, lcmBroken], [])).toEqual([lcm]); + expect(channelSubscribable(cdrBroken, [])).toBe(false); + expect(subscribableChannels([cdr, cdrCloud, cdrBroken], [])).toEqual([cdr]); }); it("never subscribes tx channels", () => { diff --git a/web/cockpit/src/subscriptions.ts b/web/cockpit/src/subscriptions.ts index 172447d25a2..e3c85d1dd11 100644 --- a/web/cockpit/src/subscriptions.ts +++ b/web/cockpit/src/subscriptions.ts @@ -5,7 +5,7 @@ // manifest. import type { ChannelSpec, DecoderRegistry, Session } from "@dimos/sdk"; -import { createDecoderRegistry, isLcmSchema } from "@dimos/sdk"; +import { cdrHasSequences, createDecoderRegistry, isCdrSchema } from "@dimos/sdk"; import type { PanelSpec } from "@dimos/shared"; import { getPanel } from "./panels/registry.tsx"; @@ -17,18 +17,15 @@ export const cockpitDecoders = createDecoderRegistry(); // subscribed only when a panel this build can render binds them. const PANEL_ONLY_ENCODINGS = new Set(["jpeg.v1", "costmap.zlib.v1"]); -/** Channels only a panel may subscribe: the encodings above, and a *.lcm.v1 +/** Channels only a panel may subscribe: the encodings above, and a *.cdr.v1 * channel whose schema has a variable-length array (a point cloud, a scan, a * path): every frame costs the message's full size, far more than the channel * table's preview is worth. A bounded message (a pose, an odometry) * subscribes like json.v1 so the table can show it. */ function panelOnly(spec: ChannelSpec): boolean { if (PANEL_ONLY_ENCODINGS.has(spec.encoding)) return true; - const lcm = spec.params.lcm; - return isLcmSchema(lcm) && - Object.values(lcm.structs).some((rows) => - rows.some(([, , dims]) => dims !== null && dims.some((d) => typeof d === "string")) - ); + const cdr = spec.params.cdr; + return isCdrSchema(cdr) && cdrHasSequences(cdr); } /** True when this build can put the channel to use: rx only, it has a diff --git a/web/deno.lock b/web/deno.lock index 997758d8707..3219ca5a94e 100644 --- a/web/deno.lock +++ b/web/deno.lock @@ -8,11 +8,13 @@ "jsr:@std/internal@^1.0.12": "1.0.12", "npm:@fontsource-variable/inter@^5.3.0": "5.3.0", "npm:@fontsource-variable/jetbrains-mono@^5.3.0": "5.3.0", + "npm:@foxglove/rosmsg2-serialization@3.1.2": "3.1.2", + "npm:@foxglove/rosmsg@5.0.5": "5.0.5", "npm:@peculiar/x509@2": "2.0.0", "npm:@types/node@^24.10.1": "24.12.2", "npm:@types/react-dom@^19.2.0": "19.2.3_@types+react@19.2.17", "npm:@types/react@^19.2.0": "19.2.17", - "npm:@vitejs/plugin-react@^5.1.0": "5.2.0_vite@7.3.2__@types+node@24.12.2__picomatch@4.0.5_@babel+core@7.29.7_@types+node@24.12.2", + "npm:@vitejs/plugin-react@^5.1.0": "5.2.0_vite@7.3.2_@babel+core@7.29.7_@types+node@24.12.2", "npm:happy-dom@^20.11.1": "20.11.1", "npm:react-dom@^19.2.0": "19.2.8_react@19.2.8", "npm:react@^19.2.0": "19.2.8", @@ -21,8 +23,8 @@ "npm:typescript@~5.9.2": "5.9.3", "npm:vite@*": "7.3.2_@types+node@24.12.2_picomatch@4.0.5", "npm:vite@^7.1.0": "7.3.2_@types+node@24.12.2_picomatch@4.0.5", - "npm:vitest@*": "3.2.7_@types+node@24.12.2_happy-dom@20.11.1_vite@7.3.2__@types+node@24.12.2__picomatch@4.0.5", - "npm:vitest@^3.2.4": "3.2.7_@types+node@24.12.2_happy-dom@20.11.1_vite@7.3.2__@types+node@24.12.2__picomatch@4.0.5" + "npm:vitest@*": "3.2.7_@types+node@24.12.2_happy-dom@20.11.1_vite@7.3.2", + "npm:vitest@^3.2.4": "3.2.7_@types+node@24.12.2_happy-dom@20.11.1_vite@7.3.2" }, "jsr": { "@std/assert@1.0.19": { @@ -322,6 +324,30 @@ "@fontsource-variable/jetbrains-mono@5.3.0": { "integrity": "sha512-F32xpS2NsGYoQi2ADSkKTgpJj7ozajsGgDJ8woTnqjmIB+dxDIqImjl4pXZVEExu8UFZ2ndhmX18EBS/hdz3Lw==" }, + "@foxglove/cdr@3.5.1": { + "integrity": "sha512-Fubm04/xC1SkeNaviwUhbFk8nAiiNvIqlmP5MW7r6IVGFa35rsA0E3/Rg/pGdE77ZXf4XNwShZcQoEmje62ddQ==" + }, + "@foxglove/message-definition@0.5.0": { + "integrity": "sha512-B/2GvWn4wRSsPDywcmy6uNXgAfZRhiOAnmy3qAaS6MHVqiHQqs/RoDsoW6Gw9sXSweJpeH5qAqAihvm1pR9Thg==" + }, + "@foxglove/rosmsg2-serialization@3.1.2": { + "integrity": "sha512-KKtmHuuIQy6I0JLUi01LFhV7hrZb5xd+LegtuGz2+9L9pftBxtVVKhoxO6B/kQREGocIUOLP8494kxLF58LMWw==", + "dependencies": [ + "@foxglove/cdr", + "@foxglove/message-definition", + "@foxglove/rostime" + ] + }, + "@foxglove/rosmsg@5.0.5": { + "integrity": "sha512-+qY6xW+4HdbPI3ItAlFFMjXgAiplXyRsv+kqdBk8OcWGH38BmEd7s6v/VnCkbYovAoB5rfR5giuKfmPRNzwbrA==", + "dependencies": [ + "@foxglove/message-definition", + "md5-typescript" + ] + }, + "@foxglove/rostime@1.1.3": { + "integrity": "sha512-Gr3Xy4sNbo2NcXg0tBHb/jVUqIcDyZ1kgHj9dB7K+Y1A7AQ0oSCmSXIuf8oqeteSIs0pl8BJOWEwAtLNC99Izg==" + }, "@jridgewell/gen-mapping@0.3.13": { "integrity": "sha512-2kkt/7niJ6MgEPxF0bYdQ6etZaA+fQvDcLKckhy1yIQOzaoKjBBjSj63/aLVjYE3qhRt5dvM+uUyfCg6UKCBbA==", "dependencies": [ @@ -663,7 +689,7 @@ "@types/node" ] }, - "@vitejs/plugin-react@5.2.0_vite@7.3.2__@types+node@24.12.2__picomatch@4.0.5_@babel+core@7.29.7_@types+node@24.12.2": { + "@vitejs/plugin-react@5.2.0_vite@7.3.2_@babel+core@7.29.7_@types+node@24.12.2": { "integrity": "sha512-YmKkfhOAi3wsB1PhJq5Scj3GXMn3WvtQ/JC0xoopuHoXSdmtdStOpFrYaT1kie2YgFBcIe64ROzMYRjCrYOdYw==", "dependencies": [ "@babel/core", @@ -685,7 +711,7 @@ "tinyrainbow" ] }, - "@vitest/mocker@3.2.7_vite@7.3.2__@types+node@24.12.2__picomatch@4.0.5_@types+node@24.12.2": { + "@vitest/mocker@3.2.7_vite@7.3.2_@types+node@24.12.2": { "integrity": "sha512-Trr0hYO9CM3Wj6ksWHRhK9IZpIY6wTMO5u/MqXurMxT57sWBaOPEtP3Oq60ihZuh5JsiagKfz95OcxdEP6dBrA==", "dependencies": [ "@vitest/spy", @@ -911,6 +937,9 @@ "@jridgewell/sourcemap-codec" ] }, + "md5-typescript@1.0.5": { + "integrity": "sha512-ovAc4EtiNt2dY8JPhPr/wkC9h4U5k/nuClNVcG0Ga3V1rMlYpAY24ZaaymFXJlz+ccJ6UMPo3FSaVKe7czBsXw==" + }, "ms@2.1.3": { "integrity": "sha512-6FlzubTLZG3J2a/NVCAleEhjzq5oxgHyaCU9yYXvcLsvoVaHJq/s5xXI6/XXP6tz7R9xAOtHnSO/tXtF3WRTlA==" }, @@ -1106,7 +1135,7 @@ ], "bin": true }, - "vitest@3.2.7_@types+node@24.12.2_happy-dom@20.11.1_vite@7.3.2__@types+node@24.12.2__picomatch@4.0.5": { + "vitest@3.2.7_@types+node@24.12.2_happy-dom@20.11.1_vite@7.3.2": { "integrity": "sha512-KrxIJ62Fd89gfysR4WotlgZABiz2dqFPgqGzX7s+CwsqLFomRH7777ZcrOD6+WVAh7khPQP41A+BKbpcJFrdEg==", "dependencies": [ "@types/chai", @@ -1193,6 +1222,8 @@ "sdk": { "packageJson": { "dependencies": [ + "npm:@foxglove/rosmsg2-serialization@3.1.2", + "npm:@foxglove/rosmsg@5.0.5", "npm:@types/node@^24.10.1", "npm:@types/react-dom@^19.2.0", "npm:@types/react@^19.2.0", diff --git a/web/sdk/fixture/demo_cdr.html b/web/sdk/fixture/demo_cdr.html new file mode 100644 index 00000000000..9e891fec8c5 --- /dev/null +++ b/web/sdk/fixture/demo_cdr.html @@ -0,0 +1,77 @@ + + + + + + DimOS generated CDR demo + + + +

Generated messages in the browser

+

+ Python-generated CDR bytes, decoded by the DimOS browser SDK from their embedded ROS2 + definitions. No ROS installation or browser message package. +

+ + +
+
+

+ + 

+      

+        Embedded ROS2 definition
+        

+      

+    

+    
+  
+
diff --git a/web/sdk/fixture/demo_cdr.ts b/web/sdk/fixture/demo_cdr.ts
new file mode 100644
index 00000000000..6e13812c927
--- /dev/null
+++ b/web/sdk/fixture/demo_cdr.ts
@@ -0,0 +1,59 @@
+import frames from "../../shared/fixtures/cdr_frames.json";
+import { createDecoderRegistry } from "@dimos/sdk";
+import type { ChannelSpec } from "@dimos/shared";
+
+const registry = createDecoderRegistry();
+const messages = document.querySelector("#message")!;
+const endian = document.querySelector("#endian")!;
+for (const [index, vector] of frames.vectors.entries()) {
+  messages.add(new Option(vector.name, String(index)));
+}
+function render(): void {
+  const vector = frames.vectors[Number(messages.value)];
+  const channel: ChannelSpec = {
+    ch: vector.name,
+    dir: "rx",
+    encoding: vector.encoding,
+    delivery: "reliable",
+    maxHz: 1,
+    params: { cdr: vector.schema },
+    publish: "none",
+    requiredScope: null,
+  };
+  const encoded = endian.value === "big" ? vector.big_endian_b64 : vector.payload_b64;
+  const payload = Uint8Array.from(atob(encoded), (c) => c.charCodeAt(0));
+  const decoded = registry.resolve(channel)!(payload, {
+    ch: vector.name,
+    seq: 1,
+    ts: 0,
+    delivery: "reliable",
+  });
+  document.querySelector("#status")!.textContent =
+    `Decoded ${payload.length} CDR bytes · ${endian.value} endian`;
+  document.querySelector("#type")!.textContent = vector.schema.type;
+  document.querySelector("#schema")!.textContent = vector.schema.definition;
+  document.querySelector("#value")!.textContent = JSON.stringify(
+    decoded.value,
+    (_key, value) =>
+      typeof value === "bigint"
+        ? `${value}n`
+        : ArrayBuffer.isView(value)
+        ? Array.from(value as unknown as ArrayLike)
+        : value,
+    2,
+  );
+  const canvas = document.querySelector("#image")!;
+  canvas.hidden = vector.name !== "image";
+  if (!canvas.hidden) {
+    const image = decoded.value as { data: Uint8Array };
+    const rgba = new Uint8ClampedArray(8);
+    for (let i = 0; i < 2; i++) {
+      rgba.set(image.data.subarray(i * 3, i * 3 + 3), i * 4);
+      rgba[i * 4 + 3] = 255;
+    }
+    canvas.getContext("2d")!.putImageData(new ImageData(rgba, 2, 1), 0, 0);
+  }
+}
+messages.onchange = render;
+endian.onchange = render;
+render();
diff --git a/web/sdk/package.json b/web/sdk/package.json
index d3521895cda..a67fa16b3e4 100644
--- a/web/sdk/package.json
+++ b/web/sdk/package.json
@@ -21,5 +21,9 @@
     "typescript": "~5.9.2",
     "vite": "^7.1.0",
     "vitest": "^3.2.4"
+  },
+  "dependencies": {
+    "@foxglove/rosmsg": "5.0.5",
+    "@foxglove/rosmsg2-serialization": "3.1.2"
   }
 }
diff --git a/web/sdk/src/decoders/cdr.test.ts b/web/sdk/src/decoders/cdr.test.ts
new file mode 100644
index 00000000000..822efef3366
--- /dev/null
+++ b/web/sdk/src/decoders/cdr.test.ts
@@ -0,0 +1,45 @@
+import { describe, expect, it } from "vitest";
+import frames from "../../../shared/fixtures/cdr_frames.json";
+import { cdrDecoder, cdrDecoderFor, cdrPreview } from "./cdr.ts";
+const HEADER = { ch: "message", seq: 1, ts: 0, delivery: "latest" as const };
+const bytes = (value: string) => Uint8Array.from(atob(value), (c) => c.charCodeAt(0));
+function plain(value: unknown): unknown {
+  if (typeof value === "bigint") return String(value);
+  if (ArrayBuffer.isView(value)) return Array.from(value as unknown as ArrayLike, plain);
+  if (Array.isArray(value)) return value.map(plain);
+  if (value !== null && typeof value === "object") {
+    return Object.fromEntries(
+      Object.entries(value).map(([key, item]) => [key, plain(item)]),
+    );
+  }
+  return value;
+}
+describe("generated CDR browser decoding", () => {
+  for (const vector of frames.vectors) {
+    it(`decodes ${vector.name} in both byte orders from its embedded schema`, () => {
+      const decode = cdrDecoder(vector.schema);
+      for (const encoded of [vector.payload_b64, vector.big_endian_b64]) {
+        expect(plain(decode(bytes(encoded), HEADER).value)).toEqual(vector.value);
+      }
+    });
+  }
+  it("rejects truncation, invalid encapsulation and trailing bytes", () => {
+    const vector = frames.vectors[0];
+    const decode = cdrDecoder(vector.schema);
+    const payload = bytes(vector.payload_b64);
+    expect(() => decode(payload.subarray(0, payload.length - 1), HEADER)).toThrow();
+    expect(() => decode(new Uint8Array([...payload, 0]), HEADER)).toThrow(/trailing/);
+    payload[0] = 255;
+    expect(() => decode(payload, HEADER)).toThrow(/header/);
+  });
+  it("declines missing and malformed schemas", () => {
+    expect(cdrDecoderFor(undefined)).toBeNull();
+    expect(cdrDecoderFor({ type: "p/msg/T", definition: "not a definition !" })).toBeNull();
+    expect(cdrDecoderFor({ type: "p.T", definition: "float64 x" })).toBeNull();
+  });
+  it("bounds previews of large images and arrays", () => {
+    const text = cdrPreview({ data: new Uint8Array(1000000), label: "x".repeat(10000) });
+    expect(text.length).toBeLessThan(550);
+    expect(text).toContain("Uint8Array(1000000)");
+  });
+});
diff --git a/web/sdk/src/decoders/cdr.ts b/web/sdk/src/decoders/cdr.ts
new file mode 100644
index 00000000000..10c244c780d
--- /dev/null
+++ b/web/sdk/src/decoders/cdr.ts
@@ -0,0 +1,106 @@
+// Decode generated messages from the manifest's complete ROS2 definition.
+import { parse } from "@foxglove/rosmsg";
+import { MessageReader } from "@foxglove/rosmsg2-serialization";
+import type { Decoder } from "./index.ts";
+
+export interface CdrSchema {
+  type: string;
+  definition: string;
+}
+export const CDR_ENCODING_RE = /\.cdr\.v1$/;
+export const CDR_PREVIEW_MAX_CHARS = 512;
+const PREVIEW_MAX_ITEMS = 8;
+const PREVIEW_MAX_STRING = 64;
+
+export function isCdrSchema(value: unknown): value is CdrSchema {
+  if (value === null || typeof value !== "object") return false;
+  const schema = value as Record;
+  return typeof schema.type === "string" &&
+    /^[A-Za-z][A-Za-z0-9_]*\/msg\/[A-Za-z][A-Za-z0-9_]*$/.test(schema.type) &&
+    typeof schema.definition === "string";
+}
+
+export function cdrDecoder(schema: CdrSchema): Decoder {
+  const definitions = parse(schema.definition, { ros2: true });
+  const reader = new MessageReader(definitions);
+  return (payload) => {
+    // Generated codecs use encapsulated XCDR1 with no trailing bytes.
+    if (
+      payload.length < 4 || payload[0] !== 0 || payload[1] > 1 ||
+      payload[2] !== 0 || payload[3] !== 0
+    ) throw new Error("invalid XCDR1 header");
+    const value = reader.readMessage(payload);
+    if (reader.lastReadByteLength() !== payload.byteLength) throw new Error("trailing CDR bytes");
+    return { value, preview: cdrPreview(value) };
+  };
+}
+
+export function cdrDecoderFor(value: unknown): Decoder | null {
+  if (!isCdrSchema(value)) return null;
+  try {
+    return cdrDecoder(value);
+  } catch {
+    return null;
+  }
+}
+
+/** Variable-length arrays require an explicit panel subscription in the cockpit. */
+export function cdrHasSequences(schema: CdrSchema): boolean {
+  return parse(schema.definition, { ros2: true }).some((definition) =>
+    definition.definitions.some((field) => field.isArray && field.arrayLength === undefined)
+  );
+}
+
+export function cdrPreview(value: unknown, budget = CDR_PREVIEW_MAX_CHARS): string {
+  const parts: string[] = [];
+  let len = 0;
+  let full = false;
+  const push = (s: string) => {
+    parts.push(s);
+    len += s.length;
+    if (len >= budget) full = true;
+  };
+  const items = (n: number, at: (i: number) => unknown) => {
+    for (let i = 0; i < n && i < PREVIEW_MAX_ITEMS && !full; i++) {
+      if (i > 0) push(", ");
+      walk(at(i));
+    }
+    if (n > PREVIEW_MAX_ITEMS && !full) push(", ...");
+    push("]");
+  };
+  const walk = (v: unknown): void => {
+    if (full) return;
+    if (v === null || v === undefined) {
+      push("null");
+    } else if (typeof v === "string") {
+      push(
+        JSON.stringify(v.length > PREVIEW_MAX_STRING ? `${v.slice(0, PREVIEW_MAX_STRING)}...` : v),
+      );
+    } else if (typeof v === "number" || typeof v === "boolean" || typeof v === "bigint") {
+      push(String(v));
+    } else if (ArrayBuffer.isView(v)) {
+      const a = v as unknown as ArrayLike;
+      push(`${v.constructor.name}(${a.length})[`);
+      items(a.length, (i) => a[i]);
+    } else if (Array.isArray(v)) {
+      push("[");
+      items(v.length, (i) => v[i]);
+    } else if (typeof v === "object") {
+      push("{");
+      let first = true;
+      for (const [key, item] of Object.entries(v)) {
+        if (full) break;
+        if (!first) push(", ");
+        first = false;
+        push(`${key}: `);
+        walk(item);
+      }
+      push("}");
+    } else {
+      push(String(v));
+    }
+  };
+  walk(value);
+  const text = parts.join("");
+  return full ? `${text.slice(0, budget)} ... (truncated)` : text;
+}
diff --git a/web/sdk/src/decoders/decoders.test.ts b/web/sdk/src/decoders/decoders.test.ts
index 803bb784fa2..f6a7aedf10f 100644
--- a/web/sdk/src/decoders/decoders.test.ts
+++ b/web/sdk/src/decoders/decoders.test.ts
@@ -1,7 +1,7 @@
 import { describe, expect, it } from "vitest";
 import type { FrameHeader } from "@dimos/shared";
 import costmapFrames from "../../../shared/fixtures/costmap_frames.json";
-import lcmFrames from "../../../shared/fixtures/lcm_frames.json";
+import cdrFrames from "../../../shared/fixtures/cdr_frames.json";
 import { spec } from "../testing/fakeRelay.ts";
 import {
   type CostmapValue,
@@ -23,14 +23,14 @@ function b64ToBytes(b64: string): Uint8Array {
   return Uint8Array.from(atob(b64), (c) => c.charCodeAt(0));
 }
 
-// The pose_stamped golden vector (Python-generated, see lcm.test.ts).
-const POSE = (lcmFrames as { vectors: { name: string; schema: unknown; payload_b64: string }[] })
+// The pose_stamped golden vector (Python-generated, see cdr.test.ts).
+const POSE = (cdrFrames as { vectors: { name: string; schema: unknown; payload_b64: string }[] })
   .vectors[0];
 const poseSpec = () =>
   spec({
-    ch: "lcm_pose",
-    encoding: "geometry_msgs.PoseStamped.lcm.v1",
-    params: { lcm: POSE.schema },
+    ch: "cdr_pose",
+    encoding: "geometry_msgs/msg/PoseStamped.cdr.v1",
+    params: { cdr: POSE.schema },
   });
 
 /** Minimal scannable JPEG: SOI + SOF0 declaring w x h (no scan data). */
@@ -59,7 +59,7 @@ describe("decoder registry", () => {
     expect(registry.get(undefined)).toBeUndefined();
   });
 
-  it("resolve() compiles *.lcm.v1 from params.lcm and caches per manifest record", () => {
+  it("resolve() compiles *.cdr.v1 from params.cdr and caches per manifest record", () => {
     expect(POSE.name).toBe("pose_stamped");
     const s = poseSpec();
     const decode = registry.resolve(s);
@@ -78,10 +78,10 @@ describe("decoder registry", () => {
     expect(registry.get(s.encoding)).toBeUndefined();
   });
 
-  it("resolve() lets an exact registration beat the lcm family rule", () => {
+  it("resolve() lets an exact registration beat the cdr family rule", () => {
     const own = createDecoderRegistry();
     const mine: Decoder = () => ({ value: "mine" });
-    own.register("geometry_msgs.PoseStamped.lcm.v1", mine);
+    own.register("geometry_msgs/msg/PoseStamped.cdr.v1", mine);
     expect(own.resolve(poseSpec())).toBe(mine);
   });
 
@@ -89,12 +89,12 @@ describe("decoder registry", () => {
     for (
       const params of [
         {},
-        { lcm: null },
-        { lcm: { type: "t.P", fp: "zz", structs: {} } },
-        { lcm: { type: "t.P", fp: "0011223344556677", structs: {} } },
+        { cdr: null },
+        { cdr: { type: "t.P", fp: "zz", structs: {} } },
+        { cdr: { type: "t.P", fp: "0011223344556677", structs: {} } },
       ]
     ) {
-      const s = spec({ ch: "x", encoding: "t.P.lcm.v1", params });
+      const s = spec({ ch: "x", encoding: "t/msg/P.cdr.v1", params });
       expect(registry.resolve(s)).toBeUndefined();
       expect(registry.resolve(s)).toBeUndefined();
     }
diff --git a/web/sdk/src/decoders/index.ts b/web/sdk/src/decoders/index.ts
index f315e9b07f4..9c1f5170135 100644
--- a/web/sdk/src/decoders/index.ts
+++ b/web/sdk/src/decoders/index.ts
@@ -1,5 +1,5 @@
 // Payload decoder registry. resolve(spec) finds a manifest channel's decoder:
-// a registered encoding id, the *.json.vN convention, or a *.lcm.v1 decoder
+// a registered encoding id, the *.json.vN convention, or a *.cdr.v1 decoder
 // compiled from the schema the robot put in the channel's params. An encoding
 // without a decoder is not an error: the channel renders as "unsupported"
 // (forward compatibility with newer bridges). Binary decoders (h264.v1, ...)
@@ -11,7 +11,7 @@ import type { ChannelSpec, FrameHeader } from "@dimos/shared";
 import { costmapDecoder } from "./costmap.ts";
 import { jpegDecoder } from "./jpeg.ts";
 import { jsonDecoder } from "./json.ts";
-import { LCM_ENCODING_RE, lcmDecoderFor } from "./lcm.ts";
+import { CDR_ENCODING_RE, cdrDecoderFor } from "./cdr.ts";
 
 export interface Decoded {
   value: unknown;
@@ -23,9 +23,9 @@ export type Decoder = (payload: Uint8Array, header: FrameHeader) => Decoded;
 
 export class DecoderRegistry {
   #decoders = new Map();
-  // Compiled *.lcm.v1 decoders per adopted manifest record (null: params.lcm
+  // Compiled *.cdr.v1 decoders per adopted manifest record (null: params.cdr
   // unusable); entries die with the manifest object that carried them.
-  #lcm = new WeakMap();
+  #cdr = new WeakMap();
 
   /** Duplicate registration is an error unless `replace` is set. */
   register(encoding: string, decoder: Decoder, opts: { replace?: boolean } = {}): void {
@@ -45,16 +45,16 @@ export class DecoderRegistry {
   }
 
   /** The decoder for a manifest channel: get(spec.encoding) first (registered
-   * ids and the JSON convention win), else a *.lcm.v1 decoder compiled once
-   * from the schema in spec.params.lcm. */
+   * ids and the JSON convention win), else a *.cdr.v1 decoder compiled once
+   * from the schema in spec.params.cdr. */
   resolve(spec: ChannelSpec): Decoder | undefined {
     const known = this.get(spec.encoding);
     if (known !== undefined) return known;
-    if (!LCM_ENCODING_RE.test(spec.encoding)) return undefined;
-    let compiled = this.#lcm.get(spec);
+    if (!CDR_ENCODING_RE.test(spec.encoding)) return undefined;
+    let compiled = this.#cdr.get(spec);
     if (compiled === undefined) {
-      compiled = lcmDecoderFor(spec.params.lcm);
-      this.#lcm.set(spec, compiled);
+      compiled = cdrDecoderFor(spec.params.cdr);
+      this.#cdr.set(spec, compiled);
     }
     return compiled ?? undefined;
   }
diff --git a/web/sdk/src/decoders/lcm.test.ts b/web/sdk/src/decoders/lcm.test.ts
deleted file mode 100644
index e490de9577f..00000000000
--- a/web/sdk/src/decoders/lcm.test.ts
+++ /dev/null
@@ -1,374 +0,0 @@
-import { describe, expect, it } from "vitest";
-import type { FrameHeader } from "@dimos/shared";
-import lcmFrames from "../../../shared/fixtures/lcm_frames.json";
-import {
-  compileLcmDecoder,
-  isLcmSchema,
-  LCM_PREVIEW_MAX_CHARS,
-  lcmDecoder,
-  lcmDecoderFor,
-  LcmOversizedError,
-  lcmPreview,
-  type LcmSchema,
-  type LcmValue,
-  MAX_LCM_ARRAY_ELEMENTS,
-} from "./lcm.ts";
-
-const HEADER: FrameHeader = { ch: "p", seq: 1, ts: 0, delivery: "reliable" };
-
-interface LcmVector {
-  name: string;
-  encoding: string;
-  schema: LcmSchema;
-  payload_b64: string;
-  value: unknown;
-}
-
-// Python is the reference (dimos/web/relay_bridge/gen_lcm_fixtures.py):
-// pytest re-encodes these, vitest decodes them.
-const VECTORS = (lcmFrames as unknown as { vectors: LcmVector[] }).vectors;
-
-function b64ToBytes(b64: string): Uint8Array {
-  return Uint8Array.from(atob(b64), (c) => c.charCodeAt(0));
-}
-
-/** The fixture's JSON form of a decoded value: byte views as base64, other
- * typed arrays as plain arrays, bigints as decimal strings. */
-function plain(v: unknown): unknown {
-  if (v instanceof Uint8Array) return btoa(String.fromCharCode(...v));
-  if (typeof v === "bigint") return v.toString();
-  if (v instanceof BigInt64Array) return Array.from(v, (x) => x.toString());
-  if (ArrayBuffer.isView(v)) return Array.from(v as unknown as ArrayLike);
-  if (Array.isArray(v)) return v.map(plain);
-  if (typeof v === "object" && v !== null) {
-    return Object.fromEntries(Object.entries(v).map(([k, x]) => [k, plain(x)]));
-  }
-  return v;
-}
-
-function vector(name: string): LcmVector {
-  const vec = VECTORS.find((v) => v.name === name);
-  if (vec === undefined) throw new Error(`no fixture vector ${name}`);
-  return vec;
-}
-
-function decodeVector(name: string): { payload: Uint8Array; value: LcmValue } {
-  const vec = vector(name);
-  const payload = b64ToBytes(vec.payload_b64);
-  return { payload, value: compileLcmDecoder(vec.schema)(payload) };
-}
-
-// A hand-built schema for the error cases: x, then n strings and n floats.
-const POINT: LcmSchema = {
-  type: "t.P",
-  fp: "0011223344556677",
-  structs: {
-    "t.P": [["x", "double", null], ["n", "int32_t", null], ["xs", "float", ["n"]], [
-      "tags",
-      "string",
-      ["n"],
-    ]],
-  },
-};
-
-/** A zeroed frame of `size` bytes that starts with the fingerprint. */
-function stamped(size: number, fp = POINT.fp): Uint8Array {
-  const bytes = new Uint8Array(size);
-  for (let i = 0; i < 8; i++) bytes[i] = parseInt(fp.slice(i * 2, i * 2 + 2), 16);
-  return bytes;
-}
-
-/** A t.P frame: fingerprint, x = 1.5, n, two floats, two "a" strings. */
-function pointFrame(fp = POINT.fp, n = 2, strLen = 2): Uint8Array {
-  const bytes = stamped(8 + 8 + 4 + 8 + 12, fp);
-  const view = new DataView(bytes.buffer, 8);
-  view.setFloat64(0, 1.5, false);
-  view.setInt32(8, n, false);
-  view.setFloat32(12, 0.5, false);
-  view.setFloat32(16, -0.25, false);
-  for (const at of [20, 26]) {
-    view.setInt32(at, strLen, false);
-    bytes[8 + at + 4] = 0x61; // "a"
-    bytes[8 + at + 5] = 0;
-  }
-  return bytes;
-}
-
-describe("lcm golden vectors", () => {
-  for (const vec of VECTORS) {
-    it(`decodes ${vec.name} (${vec.encoding})`, () => {
-      const decode = compileLcmDecoder(vec.schema);
-      expect(plain(decode(b64ToBytes(vec.payload_b64)))).toEqual(vec.value);
-    });
-  }
-});
-
-describe("lcm value shape", () => {
-  it("keeps the LCM field names in wire order, count fields included", () => {
-    expect(Object.keys(decodeVector("laser_scan").value)).toEqual([
-      "ranges_length",
-      "intensities_length",
-      "header",
-      "angle_min",
-      "angle_max",
-      "angle_increment",
-      "time_increment",
-      "scan_time",
-      "range_min",
-      "range_max",
-      "ranges",
-      "intensities",
-    ]);
-  });
-
-  it("views byte[] and int8_t[] into the frame, copies other arrays as typed arrays", () => {
-    const cloud = decodeVector("point_cloud");
-    expect(cloud.value.data).toBeInstanceOf(Uint8Array);
-    expect((cloud.value.data as Uint8Array).buffer).toBe(cloud.payload.buffer);
-    expect(cloud.value.is_dense).toBe(true);
-    expect(cloud.value.fields).toHaveLength(3);
-    const grid = decodeVector("occupancy_grid");
-    expect(grid.value.data).toBeInstanceOf(Int8Array);
-    expect((grid.value.data as Int8Array).buffer).toBe(grid.payload.buffer);
-    expect(Array.from(grid.value.data as Int8Array)).toEqual([0, 100, -1, 50, 0, 7]);
-    const scan = decodeVector("laser_scan").value;
-    expect(scan.ranges).toBeInstanceOf(Float32Array);
-    expect(scan.intensities).toBeInstanceOf(Float32Array);
-    expect((scan.intensities as Float32Array).length).toBe(0);
-    expect(decodeVector("imu").value.orientation_covariance).toBeInstanceOf(Float64Array);
-    expect(decodeVector("joint_state").value.name).toEqual(["shoulder", "elbow", "wrist_é"]);
-  });
-
-  it("decodes int64_t to a bigint, losslessly", () => {
-    expect(decodeVector("int64").value.data).toBe(-9007199254740993n);
-  });
-});
-
-describe("lcm decoder errors", () => {
-  const decode = compileLcmDecoder(POINT);
-
-  it("decodes the hand-built frame", () => {
-    expect(plain(decode(pointFrame()))).toEqual({
-      x: 1.5,
-      n: 2,
-      xs: [0.5, -0.25],
-      tags: ["a", "a"],
-    });
-  });
-
-  it("rejects the wrong fingerprint and a frame shorter than one", () => {
-    expect(() => decode(pointFrame("0011223344556678"))).toThrow(/fingerprint mismatch for t\.P/);
-    expect(() => decode(new Uint8Array(4))).toThrow(/shorter than the fingerprint/);
-  });
-
-  it("rejects truncated, overlong and malformed frames", () => {
-    expect(() => decode(pointFrame().subarray(0, 24))).toThrow();
-    expect(() => decode(pointFrame().subarray(0, 8 + 8 + 4 + 8 + 4))).toThrow(/bad string length/);
-    const trailing = new Uint8Array(pointFrame().byteLength + 1);
-    trailing.set(pointFrame());
-    expect(() => decode(trailing)).toThrow(/trailing bytes: 1/);
-    expect(() => decode(pointFrame(POINT.fp, 2, 0))).toThrow(/bad string length 0/);
-  });
-
-  it("rejects negative and oversized counts before allocating", () => {
-    expect(() => decode(pointFrame(POINT.fp, -1))).toThrow(/bad array length -1/);
-    expect(() => decode(pointFrame(POINT.fp, 1_000_000))).toThrow(/overruns the payload/);
-  });
-
-  it("refuses schemas it cannot serve at compile time", () => {
-    const withRoot = (rows: unknown): LcmSchema =>
-      ({ ...POINT, structs: { "t.P": rows } }) as LcmSchema;
-    // A type that is neither a primitive nor a struct key reads as a missing struct.
-    expect(() => compileLcmDecoder(withRoot([["x", "int128_t", null]]))).toThrow(
-      /schema has no struct int128_t/,
-    );
-    expect(() => compileLcmDecoder(withRoot([["h", "t.H", null]]))).toThrow(
-      /schema has no struct t\.H/,
-    );
-    expect(() => compileLcmDecoder(withRoot([["m", "double", [3, 3]]]))).toThrow(
-      /multi-dimensional arrays are not supported \(t\.P\.m\)/,
-    );
-    // A computed __proto__ key would set the record's prototype in browsers.
-    expect(() => compileLcmDecoder(withRoot([["__proto__", "double", null]]))).toThrow(
-      /field __proto__ is not supported \(t\.P\)/,
-    );
-  });
-
-  it("rejects recursive schemas at compile time", () => {
-    const direct: LcmSchema = { ...POINT, structs: { "t.P": [["self", "t.P", null]] } };
-    expect(() => compileLcmDecoder(direct)).toThrow(/recursive struct t\.P/);
-    // Through an array: n = 1 at every level would nest until the stack overflows.
-    const indirect: LcmSchema = {
-      ...POINT,
-      structs: {
-        "t.P": [["n", "int32_t", null], ["kids", "t.K", ["n"]]],
-        "t.K": [["parent", "t.P", null]],
-      },
-    };
-    expect(() => compileLcmDecoder(indirect)).toThrow(/recursive struct t\.P/);
-    expect(lcmDecoderFor(direct)).toBeNull();
-  });
-
-  it("keeps a leading BOM, as Python's decoder does", () => {
-    const text = new TextEncoder().encode("\ufeffrobot");
-    const bytes = stamped(8 + 4 + text.length + 1);
-    new DataView(bytes.buffer).setInt32(8, text.length + 1, false);
-    bytes.set(text, 12);
-    const decode = compileLcmDecoder({ ...POINT, structs: { "t.P": [["s", "string", null]] } });
-    expect(decode(bytes).s).toBe("\ufeffrobot");
-  });
-});
-
-describe("lcm schema gate and decoder factory", () => {
-  it("accepts the fixture schemas and rejects the rest", () => {
-    for (const vec of VECTORS) expect(isLcmSchema(vec.schema)).toBe(true);
-    for (
-      const bad of [
-        null,
-        undefined,
-        "t.P",
-        {},
-        { type: "t.P", fp: "zz", structs: {} },
-        { type: "t.P", fp: POINT.fp, structs: null },
-        { type: 1, fp: POINT.fp, structs: {} },
-      ]
-    ) {
-      expect(isLcmSchema(bad)).toBe(false);
-    }
-  });
-
-  it("returns null for an unusable schema and a previewing decoder otherwise", () => {
-    expect(lcmDecoderFor(undefined)).toBeNull();
-    expect(lcmDecoderFor({ ...POINT, structs: {} })).toBeNull();
-    expect(() => lcmDecoder({ ...POINT, structs: {} })).toThrow(/schema has no struct t\.P/);
-    const decoder = lcmDecoderFor(POINT);
-    expect(decoder).not.toBeNull();
-    const decoded = decoder!(pointFrame(), HEADER);
-    expect(plain(decoded.value)).toEqual({ x: 1.5, n: 2, xs: [0.5, -0.25], tags: ["a", "a"] });
-    expect(decoded.preview).toBe(
-      '{x: 1.5, n: 2, xs: Float32Array(2)[0.5, -0.25], tags: ["a", "a"]}',
-    );
-  });
-});
-
-// Two struct arrays and a string array, each with its own count field.
-const CLOUD: LcmSchema = {
-  type: "t.C",
-  fp: POINT.fp,
-  structs: {
-    "t.C": [
-      ["n", "int32_t", null],
-      ["m", "int32_t", null],
-      ["k", "int32_t", null],
-      ["a", "t.X", ["n"]],
-      ["b", "t.X", ["m"]],
-      ["tags", "string", ["k"]],
-    ],
-    "t.X": [["x", "float", null]],
-  },
-};
-
-function cloudFrame(n: number, m: number, k: number): Uint8Array {
-  const bytes = stamped(8 + 12 + 4 * (n + m) + 6 * k);
-  const view = new DataView(bytes.buffer, 8);
-  view.setInt32(0, n, false);
-  view.setInt32(4, m, false);
-  view.setInt32(8, k, false);
-  // Floats stay zero; every tag is "a".
-  for (let i = 0, at = 12 + 4 * (n + m); i < k; i++, at += 6) {
-    view.setInt32(at, 2, false);
-    bytes[8 + at + 4] = 0x61;
-  }
-  return bytes;
-}
-
-// n booleans behind their count: the one array kind that is a plain JS array.
-const FLAGS: LcmSchema = {
-  type: "t.F",
-  fp: POINT.fp,
-  structs: { "t.F": [["n", "int32_t", null], ["flags", "boolean", ["n"]]] },
-};
-
-function flagsFrame(n: number): Uint8Array {
-  const bytes = stamped(8 + 4 + n);
-  new DataView(bytes.buffer).setInt32(8, n, false);
-  bytes.fill(1, 12);
-  return bytes;
-}
-
-describe("lcm element budget", () => {
-  const decode = compileLcmDecoder(CLOUD);
-  const MAX = MAX_LCM_ARRAY_ELEMENTS;
-
-  it("decodes a frame at the budget", () => {
-    expect((decode(cloudFrame(MAX, 0, 0)).a as unknown[]).length).toBe(MAX);
-    expect((decode(cloudFrame(0, 0, MAX)).tags as string[]).length).toBe(MAX);
-  });
-
-  it("reports a frame over the budget as oversized instead of expanding it", () => {
-    expect(() => decode(cloudFrame(MAX + 1, 0, 0))).toThrow(LcmOversizedError);
-    expect(() => decode(cloudFrame(0, 0, MAX + 1))).toThrow(/over 100000 array elements \(tags\)/);
-    // The budget is per frame: sibling arrays add up.
-    expect(() => decode(cloudFrame(MAX / 2, MAX / 2 + 1, 0))).toThrow(/\(b\)/);
-    const frame = cloudFrame(MAX + 1, 0, 0);
-    expect(lcmDecoderFor(CLOUD)!(frame, HEADER)).toEqual({
-      value: undefined,
-      preview: `(oversized lcm message: over 100000 array elements (a), ${frame.byteLength} B)`,
-    });
-  });
-
-  it("charges boolean arrays against the budget too", () => {
-    const flags = compileLcmDecoder(FLAGS);
-    const value = flags(flagsFrame(MAX)).flags as boolean[];
-    expect(value).toHaveLength(MAX);
-    expect(value[MAX - 1]).toBe(true);
-    expect(() => flags(flagsFrame(MAX + 1))).toThrow(/over 100000 array elements \(flags\)/);
-  });
-
-  it("takes a page's own budget", () => {
-    const small = lcmDecoder(CLOUD, { maxArrayElements: 10 });
-    expect((small(cloudFrame(10, 0, 0), HEADER).value as LcmValue).a).toHaveLength(10);
-    expect(small(cloudFrame(11, 0, 0), HEADER).preview).toMatch(/over 10 array elements \(a\)/);
-    const big = compileLcmDecoder(CLOUD, { maxArrayElements: MAX + 1 });
-    expect(big(cloudFrame(MAX + 1, 0, 0)).a).toHaveLength(MAX + 1);
-  });
-});
-
-describe("lcmPreview", () => {
-  it("prints nested fields, typed arrays and bigints", () => {
-    const value = {
-      a: 1,
-      s: "x",
-      b: true,
-      big: 5n,
-      arr: new Float64Array([1.5, 2]),
-      nested: { q: null },
-    };
-    expect(lcmPreview(value)).toBe(
-      '{a: 1, s: "x", b: true, big: 5, arr: Float64Array(2)[1.5, 2], nested: {q: null}}',
-    );
-  });
-
-  it("shows the head of long arrays and cuts long strings", () => {
-    expect(lcmPreview({ r: new Float32Array(20) })).toBe(
-      "{r: Float32Array(20)[0, 0, 0, 0, 0, 0, 0, 0, ...]}",
-    );
-    expect(lcmPreview(["y".repeat(100)])).toBe(`["${"y".repeat(64)}..."]`);
-  });
-
-  it("stays short on a huge array and stops at its budget on a wide message", () => {
-    const poses = Array.from({ length: 100_000 }, (_, i) => ({ x: i, y: -i }));
-    const long = lcmPreview({ poses_length: poses.length, poses });
-    expect(long.startsWith("{poses_length: 100000, poses: [{x: 0, y: 0}, {x: 1, y: -1}, ")).toBe(
-      true,
-    );
-    expect(long.endsWith(", ...]}")).toBe(true);
-    expect(long.length).toBeLessThan(200);
-    const wide = Object.fromEntries(
-      Array.from({ length: 200 }, (_, i) => [`field_${i}`, "v".repeat(20)]),
-    );
-    const text = lcmPreview(wide);
-    expect(text.endsWith(" ... (truncated)")).toBe(true);
-    expect(text.length).toBeLessThanOrEqual(LCM_PREVIEW_MAX_CHARS + 20);
-  });
-});
diff --git a/web/sdk/src/decoders/lcm.ts b/web/sdk/src/decoders/lcm.ts
deleted file mode 100644
index 3c926a87dba..00000000000
--- a/web/sdk/src/decoders/lcm.ts
+++ /dev/null
@@ -1,424 +0,0 @@
-// Schema-driven LCM decoder for `.lcm.v1` channels. The robot ships
-// each channel's schema (exported from the generated dimos_lcm class) in the
-// manifest record's params.lcm; compileLcmDecoder turns it into closures once
-// per adopted manifest, so any DimOS message decodes with no generated code.
-// Values are plain objects with the LCM fields in wire order (including the
-// *_length count fields); byte[] and int8_t[] are zero-copy views into the
-// frame, other primitive arrays are fresh typed arrays, int64_t is a bigint.
-// Decoding runs on the ingest path, so a frame that would expand into more
-// struct, string or boolean array elements than the budget (a page's own,
-// else MAX_LCM_ARRAY_ELEMENTS) is reported as oversized instead (the json.v1
-// decoder's approach to a payload above its cap).
-
-import type { Decoder } from "./index.ts";
-
-export type LcmDim = number | string;
-export type LcmField = [name: string, type: string, dims: LcmDim[] | null];
-export interface LcmSchema {
-  type: string;
-  /** 16 hex chars: the big-endian packed fingerprint every frame starts with. */
-  fp: string;
-  structs: Record;
-}
-export type LcmValue = Record;
-
-export const LCM_ENCODING_RE = /\.lcm\.v1$/;
-// Struct, string and boolean array elements one frame may expand into JS
-// values (typed arrays and views cost what the payload weighs and need no
-// budget). A Path with 1000 poses is 1000; a page that needs more registers
-// lcmDecoder(schema, { maxArrayElements }) for that encoding.
-export const MAX_LCM_ARRAY_ELEMENTS = 100_000;
-
-export interface LcmDecoderOptions {
-  /** Per-frame budget of struct, string and boolean array elements. */
-  maxArrayElements?: number;
-}
-
-/** Thrown mid-decode by a compiled decoder for a frame over its element budget. */
-export class LcmOversizedError extends Error {}
-
-interface Budget {
-  max: number;
-  left: number;
-}
-// Longest preview the raw-value UI mounts for an LCM channel.
-export const LCM_PREVIEW_MAX_CHARS = 512;
-const PREVIEW_MAX_ITEMS = 8;
-const PREVIEW_MAX_STRING = 64;
-
-type FieldDecoder = (view: DataView, bytes: Uint8Array, off: number, out: LcmValue) => number;
-
-// fatal: corrupted bytes must fail decode, not U+FFFD their way onto screen.
-// ignoreBOM: a leading U+FEFF is part of the string (Python's decoder keeps it).
-const utf8 = new TextDecoder("utf-8", { fatal: true, ignoreBOM: true });
-const SIZE: Record = {
-  int8_t: 1,
-  int16_t: 2,
-  int32_t: 4,
-  int64_t: 8,
-  float: 4,
-  double: 8,
-  boolean: 1,
-  byte: 1,
-};
-
-/** Shape check for a manifest's params.lcm; the compiler validates the rows. */
-export function isLcmSchema(x: unknown): x is LcmSchema {
-  if (typeof x !== "object" || x === null) return false;
-  const s = x as Record;
-  return typeof s.type === "string" && typeof s.fp === "string" &&
-    /^[0-9a-f]{16}$/.test(s.fp) && typeof s.structs === "object" && s.structs !== null;
-}
-
-function readString(view: DataView, bytes: Uint8Array, off: number): [string, number] {
-  const len = view.getInt32(off, false); // counts the trailing NUL
-  off += 4;
-  if (len < 1 || off + len > bytes.byteLength) {
-    throw new Error(`bad string length ${len} at ${off}`);
-  }
-  return [utf8.decode(bytes.subarray(off, off + len - 1)), off + len];
-}
-
-function scalar(name: string, type: string): FieldDecoder {
-  switch (type) {
-    case "double":
-      return (v, _b, off, out) => {
-        out[name] = v.getFloat64(off, false);
-        return off + 8;
-      };
-    case "float":
-      return (v, _b, off, out) => {
-        out[name] = v.getFloat32(off, false);
-        return off + 4;
-      };
-    case "int32_t":
-      return (v, _b, off, out) => {
-        out[name] = v.getInt32(off, false);
-        return off + 4;
-      };
-    case "int16_t":
-      return (v, _b, off, out) => {
-        out[name] = v.getInt16(off, false);
-        return off + 2;
-      };
-    case "int8_t":
-      return (v, _b, off, out) => {
-        out[name] = v.getInt8(off);
-        return off + 1;
-      };
-    case "int64_t":
-      return (v, _b, off, out) => {
-        out[name] = v.getBigInt64(off, false);
-        return off + 8;
-      };
-    case "byte":
-      return (v, _b, off, out) => {
-        out[name] = v.getUint8(off);
-        return off + 1;
-      };
-    case "boolean":
-      return (v, _b, off, out) => {
-        out[name] = v.getUint8(off) !== 0;
-        return off + 1;
-      };
-    case "string":
-      return (v, b, off, out) => {
-        const [text, next] = readString(v, b, off);
-        out[name] = text;
-        return next;
-      };
-    default:
-      throw new Error(`unknown primitive ${type}`);
-  }
-}
-
-/** Array length: the fixed size, or the value of the count field already
- * decoded into the same struct (LCM puts the count before the array). */
-function count(dim: LcmDim, out: LcmValue): number {
-  const n = typeof dim === "number" ? dim : out[dim];
-  if (typeof n !== "number" || n < 0 || !Number.isInteger(n)) {
-    throw new Error(`bad array length ${String(n)}`);
-  }
-  return n;
-}
-
-function reserve(budget: Budget, name: string, n: number): void {
-  if (n > budget.left) {
-    throw new LcmOversizedError(`over ${budget.max} array elements (${name})`);
-  }
-  budget.left -= n;
-}
-
-function checked(name: string, type: string, size: number) {
-  return (bytes: Uint8Array, off: number, n: number) => {
-    if (off + n * size > bytes.byteLength) {
-      throw new Error(`${name}: ${n} x ${type} overruns the payload`);
-    }
-  };
-}
-
-function primitiveArray(name: string, type: string, dim: LcmDim, budget: Budget): FieldDecoder {
-  const check = checked(name, type, SIZE[type] ?? 1);
-  switch (type) {
-    case "byte":
-      return (_v, b, off, out) => {
-        const n = count(dim, out);
-        check(b, off, n);
-        out[name] = b.subarray(off, off + n);
-        return off + n;
-      };
-    case "int8_t":
-      return (_v, b, off, out) => {
-        const n = count(dim, out);
-        check(b, off, n);
-        out[name] = new Int8Array(b.buffer, b.byteOffset + off, n);
-        return off + n;
-      };
-    case "boolean":
-      return (_v, b, off, out) => {
-        const n = count(dim, out);
-        check(b, off, n);
-        reserve(budget, name, n);
-        const a = new Array(n);
-        for (let i = 0; i < n; i++) a[i] = b[off + i] !== 0;
-        out[name] = a;
-        return off + n;
-      };
-    case "float":
-      return (v, b, off, out) => {
-        const n = count(dim, out);
-        check(b, off, n);
-        const a = new Float32Array(n);
-        for (let i = 0; i < n; i++, off += 4) a[i] = v.getFloat32(off, false);
-        out[name] = a;
-        return off;
-      };
-    case "double":
-      return (v, b, off, out) => {
-        const n = count(dim, out);
-        check(b, off, n);
-        const a = new Float64Array(n);
-        for (let i = 0; i < n; i++, off += 8) a[i] = v.getFloat64(off, false);
-        out[name] = a;
-        return off;
-      };
-    case "int16_t":
-      return (v, b, off, out) => {
-        const n = count(dim, out);
-        check(b, off, n);
-        const a = new Int16Array(n);
-        for (let i = 0; i < n; i++, off += 2) a[i] = v.getInt16(off, false);
-        out[name] = a;
-        return off;
-      };
-    case "int32_t":
-      return (v, b, off, out) => {
-        const n = count(dim, out);
-        check(b, off, n);
-        const a = new Int32Array(n);
-        for (let i = 0; i < n; i++, off += 4) a[i] = v.getInt32(off, false);
-        out[name] = a;
-        return off;
-      };
-    case "int64_t":
-      return (v, b, off, out) => {
-        const n = count(dim, out);
-        check(b, off, n);
-        const a = new BigInt64Array(n);
-        for (let i = 0; i < n; i++, off += 8) a[i] = v.getBigInt64(off, false);
-        out[name] = a;
-        return off;
-      };
-    case "string":
-      return (v, b, off, out) => {
-        const n = count(dim, out);
-        check(b, off, n); // every string is at least 5 bytes: no giant Array from a bad count
-        reserve(budget, name, n);
-        const a = new Array(n);
-        for (let i = 0; i < n; i++) [a[i], off] = readString(v, b, off);
-        out[name] = a;
-        return off;
-      };
-    default:
-      throw new Error(`unknown primitive ${type}`);
-  }
-}
-
-function struct(name: string, fields: FieldDecoder[]): FieldDecoder {
-  return (v, b, off, out) => {
-    const o: LcmValue = {};
-    for (let i = 0; i < fields.length; i++) off = fields[i](v, b, off, o);
-    out[name] = o;
-    return off;
-  };
-}
-
-function structArray(
-  name: string,
-  fields: FieldDecoder[],
-  dim: LcmDim,
-  budget: Budget,
-): FieldDecoder {
-  const check = checked(name, "struct", 1);
-  return (v, b, off, out) => {
-    const n = count(dim, out);
-    check(b, off, n); // a corrupt count must not allocate a giant Array first
-    reserve(budget, name, n);
-    const a = new Array(n);
-    for (let j = 0; j < n; j++) {
-      const o: LcmValue = {};
-      for (let i = 0; i < fields.length; i++) off = fields[i](v, b, off, o);
-      a[j] = o;
-    }
-    out[name] = a;
-    return off;
-  };
-}
-
-/** Compile a schema into a decoder of whole frames (fingerprint included).
- * Throws on a schema it cannot serve: unknown primitives, missing or
- * recursive structs, multi-dimensional arrays, a field named __proto__. The
- * decoder throws LcmOversizedError, before expanding the array that crosses
- * it, for a frame over the element budget. */
-export function compileLcmDecoder(
-  schema: LcmSchema,
-  { maxArrayElements = MAX_LCM_ARRAY_ELEMENTS }: LcmDecoderOptions = {},
-): (payload: Uint8Array) => LcmValue {
-  // null: the struct is being compiled, so a reference to it is a cycle.
-  const compiled = new Map();
-  const budget: Budget = { max: maxArrayElements, left: 0 };
-  const structDecoders = (name: string): FieldDecoder[] => {
-    const done = compiled.get(name);
-    if (done) return done;
-    if (done === null) throw new Error(`recursive struct ${name}`);
-    const rows = schema.structs[name];
-    if (!Array.isArray(rows)) throw new Error(`schema has no struct ${name}`);
-    compiled.set(name, null);
-    const fields: FieldDecoder[] = [];
-    for (const [fname, ftype, dims] of rows) {
-      // A computed __proto__ key sets a record's prototype in browsers.
-      if (fname === "__proto__") throw new Error(`field __proto__ is not supported (${name})`);
-      const prim = ftype === "string" || Object.hasOwn(SIZE, ftype);
-      if (dims === null) {
-        fields.push(prim ? scalar(fname, ftype) : struct(fname, structDecoders(ftype)));
-      } else if (dims.length === 1) {
-        fields.push(
-          prim
-            ? primitiveArray(fname, ftype, dims[0], budget)
-            : structArray(fname, structDecoders(ftype), dims[0], budget),
-        );
-      } else {
-        throw new Error(`multi-dimensional arrays are not supported (${name}.${fname})`);
-      }
-    }
-    compiled.set(name, fields);
-    return fields;
-  };
-  const root = structDecoders(schema.type);
-  const fp = Uint8Array.from(
-    { length: 8 },
-    (_, i) => parseInt(schema.fp.slice(i * 2, i * 2 + 2), 16),
-  );
-  return (payload: Uint8Array): LcmValue => {
-    if (payload.byteLength < 8) throw new Error("payload shorter than the fingerprint");
-    for (let i = 0; i < 8; i++) {
-      if (payload[i] !== fp[i]) throw new Error(`fingerprint mismatch for ${schema.type}`);
-    }
-    const view = new DataView(payload.buffer, payload.byteOffset, payload.byteLength);
-    const out: LcmValue = {};
-    budget.left = maxArrayElements;
-    let off = 8;
-    for (let i = 0; i < root.length; i++) off = root[i](view, payload, off, out);
-    if (off !== payload.byteLength) throw new Error(`trailing bytes: ${payload.byteLength - off}`);
-    return out;
-  };
-}
-
-/** Bounded text form of a decoded value for the raw channel table: the first
- * elements of arrays and typed arrays, cut strings, bigint digits. Cost is
- * proportional to the budget, never to the message. */
-export function lcmPreview(value: unknown, budget = LCM_PREVIEW_MAX_CHARS): string {
-  const parts: string[] = [];
-  let len = 0;
-  let full = false;
-  const push = (s: string) => {
-    parts.push(s);
-    len += s.length;
-    if (len >= budget) full = true;
-  };
-  const items = (n: number, at: (i: number) => unknown) => {
-    for (let i = 0; i < n && i < PREVIEW_MAX_ITEMS && !full; i++) {
-      if (i > 0) push(", ");
-      walk(at(i));
-    }
-    if (n > PREVIEW_MAX_ITEMS && !full) push(", ...");
-    push("]");
-  };
-  const walk = (v: unknown): void => {
-    if (full) return;
-    if (v === null || v === undefined) {
-      push("null");
-    } else if (typeof v === "string") {
-      push(
-        JSON.stringify(v.length > PREVIEW_MAX_STRING ? `${v.slice(0, PREVIEW_MAX_STRING)}...` : v),
-      );
-    } else if (typeof v === "number" || typeof v === "boolean" || typeof v === "bigint") {
-      push(String(v));
-    } else if (ArrayBuffer.isView(v)) {
-      const a = v as unknown as ArrayLike;
-      push(`${v.constructor.name}(${a.length})[`);
-      items(a.length, (i) => a[i]);
-    } else if (Array.isArray(v)) {
-      push("[");
-      items(v.length, (i) => v[i]);
-    } else if (typeof v === "object") {
-      push("{");
-      let first = true;
-      for (const [key, item] of Object.entries(v)) {
-        if (full) break;
-        if (!first) push(", ");
-        first = false;
-        push(`${key}: `);
-        walk(item);
-      }
-      push("}");
-    } else {
-      push(String(v));
-    }
-  };
-  walk(value);
-  const text = parts.join("");
-  return full ? `${text.slice(0, budget)} ... (truncated)` : text;
-}
-
-/** The Decoder for a schema (a manifest channel's params.lcm): the value and
- * its preview, or an oversized preview for a frame over the budget. Throws on
- * a schema compileLcmDecoder cannot serve. A page that needs a bigger budget
- * registers this under the channel's encoding. */
-export function lcmDecoder(schema: LcmSchema, opts?: LcmDecoderOptions): Decoder {
-  const decode = compileLcmDecoder(schema, opts);
-  return (payload) => {
-    let value: LcmValue;
-    try {
-      value = decode(payload);
-    } catch (e) {
-      if (!(e instanceof LcmOversizedError)) throw e;
-      return {
-        value: undefined,
-        preview: `(oversized lcm message: ${e.message}, ${payload.byteLength} B)`,
-      };
-    }
-    return { value, preview: lcmPreview(value) };
-  };
-}
-
-/** lcmDecoder for a channel's params.lcm, or null when the schema is missing
- * or unusable (the channel then renders as one without a decoder). */
-export function lcmDecoderFor(lcm: unknown): Decoder | null {
-  if (!isLcmSchema(lcm)) return null;
-  try {
-    return lcmDecoder(lcm);
-  } catch {
-    return null;
-  }
-}
diff --git a/web/sdk/src/index.ts b/web/sdk/src/index.ts
index 2073fe9ccf9..6516e8d7f0a 100644
--- a/web/sdk/src/index.ts
+++ b/web/sdk/src/index.ts
@@ -14,14 +14,7 @@ export type { ChannelSnapshot, ChannelStats, SessionStatus, Slot } from "./store
 export { createDecoderRegistry, DecoderRegistry } from "./decoders/index.ts";
 export type { Decoded, Decoder } from "./decoders/index.ts";
 export { type CostmapValue, inflateCostmap } from "./decoders/costmap.ts";
-export {
-  isLcmSchema,
-  lcmDecoder,
-  type LcmDecoderOptions,
-  type LcmSchema,
-  type LcmValue,
-  MAX_LCM_ARRAY_ELEMENTS,
-} from "./decoders/lcm.ts";
+export { cdrDecoder, cdrHasSequences, type CdrSchema, isCdrSchema } from "./decoders/cdr.ts";
 export {
   PublishError,
   type PublishOutcome,
diff --git a/web/sdk/src/session.test.ts b/web/sdk/src/session.test.ts
index f80715177e1..0c4a8cf76a6 100644
--- a/web/sdk/src/session.test.ts
+++ b/web/sdk/src/session.test.ts
@@ -17,13 +17,13 @@ import {
   type Session,
 } from "./session.ts";
 import { PublishError } from "./errors.ts";
-import lcmFrames from "../../shared/fixtures/lcm_frames.json";
+import cdrFrames from "../../shared/fixtures/cdr_frames.json";
 
 // The pose_stamped golden vector (Python-generated): the schema a robot puts
-// in params.lcm plus the exact frame bytes.
-const POSE_LCM = (lcmFrames as { vectors: { schema: unknown; payload_b64: string }[] })
+// in params.cdr plus the exact frame bytes.
+const POSE_CDR = (cdrFrames as { vectors: { schema: unknown; payload_b64: string }[] })
   .vectors[0];
-const poseLcmPayload = () => Uint8Array.from(atob(POSE_LCM.payload_b64), (c) => c.charCodeAt(0));
+const poseCdrPayload = () => Uint8Array.from(atob(POSE_CDR.payload_b64), (c) => c.charCodeAt(0));
 import {
   FakeRelayEnd,
   INFO,
@@ -761,51 +761,51 @@ describe("Session over a fake WebTransport", () => {
     expect(slot.preview).toBe("2 points");
   });
 
-  it("decodes a *.lcm.v1 channel with the schema from the manifest", async () => {
+  it("decodes a *.cdr.v1 channel with the schema from the manifest", async () => {
     const { relay, handle } = start();
     await goLive(relay, handle, ROBOT_A, [
       spec({
-        ch: "lcm_pose",
-        encoding: "geometry_msgs.PoseStamped.lcm.v1",
-        params: { lcm: POSE_LCM.schema },
+        ch: "cdr_pose",
+        encoding: "geometry_msgs/msg/PoseStamped.cdr.v1",
+        params: { cdr: POSE_CDR.schema },
       }),
     ]);
-    handle.subscribe("lcm_pose", () => {});
-    await until(() => relay.subs().includes("lcm_pose"), "sub");
-    relay.pushRaw(1, poseLcmPayload(), "lcm_pose");
-    await until(() => handle.store.get("lcm_pose") !== null, "frame");
-    const slot = handle.store.get("lcm_pose")!;
+    handle.subscribe("cdr_pose", () => {});
+    await until(() => relay.subs().includes("cdr_pose"), "sub");
+    relay.pushRaw(1, poseCdrPayload(), "cdr_pose");
+    await until(() => handle.store.get("cdr_pose") !== null, "frame");
+    const slot = handle.store.get("cdr_pose")!;
     const value = slot.value as { pose: { position: { x: number } }; header: { frame_id: string } };
     expect(value.pose.position.x).toBe(1.5);
     expect(value.header.frame_id).toBe("map");
     expect(slot.preview).toContain("position: {x: 1.5, y: -2.5, z: 0.25}");
 
-    // Another fingerprint is a decode error; the last good value stays.
-    const foreign = poseLcmPayload();
+    // An invalid encapsulation header is a decode error; the last good value stays.
+    const foreign = poseCdrPayload();
     foreign[0] ^= 0xff;
-    relay.pushRaw(2, foreign, "lcm_pose");
+    relay.pushRaw(2, foreign, "cdr_pose");
     await until(() => {
       handle.store.publishUi();
-      return handle.store.getUiSnapshot("lcm_pose").stats.frames === 2;
+      return handle.store.getUiSnapshot("cdr_pose").stats.frames === 2;
     }, "bad frame counted");
-    expect(handle.store.get("lcm_pose")!.seq).toBe(1);
-    expect(handle.store.getUiSnapshot("lcm_pose").stats.decodeErrors).toBe(1);
+    expect(handle.store.get("cdr_pose")!.seq).toBe(1);
+    expect(handle.store.getUiSnapshot("cdr_pose").stats.decodeErrors).toBe(1);
   });
 
-  it("counts frames of an lcm channel without a usable schema, storing no value", async () => {
+  it("counts frames of an cdr channel without a usable schema, storing no value", async () => {
     const { relay, handle } = start();
     await goLive(relay, handle, ROBOT_A, [
-      spec({ ch: "lcm_pose", encoding: "geometry_msgs.PoseStamped.lcm.v1", params: {} }),
+      spec({ ch: "cdr_pose", encoding: "geometry_msgs/msg/PoseStamped.cdr.v1", params: {} }),
     ]);
-    handle.subscribe("lcm_pose", () => {});
-    await until(() => relay.subs().includes("lcm_pose"), "sub");
-    relay.pushRaw(1, poseLcmPayload(), "lcm_pose");
+    handle.subscribe("cdr_pose", () => {});
+    await until(() => relay.subs().includes("cdr_pose"), "sub");
+    relay.pushRaw(1, poseCdrPayload(), "cdr_pose");
     await until(() => {
       handle.store.publishUi();
-      return handle.store.getUiSnapshot("lcm_pose").stats.frames === 1;
+      return handle.store.getUiSnapshot("cdr_pose").stats.frames === 1;
     }, "frame counted");
-    expect(handle.store.get("lcm_pose")?.value).toBeUndefined();
-    expect(handle.store.getUiSnapshot("lcm_pose").stats.decodeErrors).toBe(0);
+    expect(handle.store.get("cdr_pose")?.value).toBeUndefined();
+    expect(handle.store.getUiSnapshot("cdr_pose").stats.decodeErrors).toBe(0);
   });
 
   it("retries the watch when the robot reappears after unknown_robot", async () => {
diff --git a/web/shared/fixtures/cdr_frames.json b/web/shared/fixtures/cdr_frames.json
new file mode 100644
index 00000000000..f3fb8e1839b
--- /dev/null
+++ b/web/shared/fixtures/cdr_frames.json
@@ -0,0 +1,236 @@
+{
+  "vectors": [
+    {
+      "name": "pose_stamped",
+      "encoding": "geometry_msgs/msg/PoseStamped.cdr.v1",
+      "schema": {
+        "type": "geometry_msgs/msg/PoseStamped",
+        "definition": "# A Pose with reference coordinate frame and timestamp\n\nstd_msgs/Header header\nPose pose\n================================================================================\nMSG: builtin_interfaces/Time\n# This message communicates ROS Time defined here:\n# https://design.ros2.org/articles/clock_and_time.html\n\n# The seconds component, valid over all int32 values.\nint32 sec\n\n# The nanoseconds component, valid in the range [0, 1e9), to be added to the seconds component. \n# e.g.\n# The time -1.7 seconds is represented as {sec: -2, nanosec: 3e8}\n# The time 1.7 seconds is represented as {sec: 1, nanosec: 7e8}\nuint32 nanosec\n================================================================================\nMSG: geometry_msgs/Point\n# This contains the position of a point in free space\nfloat64 x\nfloat64 y\nfloat64 z\n================================================================================\nMSG: geometry_msgs/Pose\n# A representation of pose in free space, composed of position and orientation.\n\nPoint position\nQuaternion orientation\n================================================================================\nMSG: geometry_msgs/Quaternion\n# This represents an orientation in free space in quaternion form.\n\nfloat64 x 0\nfloat64 y 0\nfloat64 z 0\nfloat64 w 1\n================================================================================\nMSG: std_msgs/Header\n# Standard metadata for higher-level stamped data types.\n# This is generally used to communicate timestamped data\n# in a particular coordinate frame.\n\n# Two-integer timestamp that is expressed as seconds and nanoseconds.\nbuiltin_interfaces/Time stamp\n\n# Transform frame with which this data is associated.\nstring frame_id\n"
+      },
+      "value": {
+        "header": {
+          "stamp": {
+            "sec": 1757400000,
+            "nanosec": 500000123
+          },
+          "frame_id": "map"
+        },
+        "pose": {
+          "position": {
+            "x": 1.5,
+            "y": -2.5,
+            "z": 0.25
+          },
+          "orientation": {
+            "x": 0.0,
+            "y": 0.0,
+            "z": 0.6,
+            "w": 0.8
+          }
+        }
+      },
+      "payload_b64": "AAEAAMDLv2h7Zc0dBAAAAG1hcAAAAAAAAAD4PwAAAAAAAATAAAAAAAAA0D8AAAAAAAAAAAAAAAAAAAAAMzMzMzMz4z+amZmZmZnpPw==",
+      "big_endian_b64": "AAAAAGi/y8AdzWV7AAAABG1hcAA/+AAAAAAAAMAEAAAAAAAAP9AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAP+MzMzMzMzM/6ZmZmZmZmg=="
+    },
+    {
+      "name": "custom_segments",
+      "encoding": "dimos_msgs/msg/LineSegments3D.cdr.v1",
+      "schema": {
+        "type": "dimos_msgs/msg/LineSegments3D",
+        "definition": "# Copyright 2026 Dimensional Inc.\n# SPDX-License-Identifier: Apache-2.0\n\n# Weighted segments in a common coordinate frame.\nstd_msgs/Header header\nLineSegment3D[] segments\n================================================================================\nMSG: builtin_interfaces/Time\n# This message communicates ROS Time defined here:\n# https://design.ros2.org/articles/clock_and_time.html\n\n# The seconds component, valid over all int32 values.\nint32 sec\n\n# The nanoseconds component, valid in the range [0, 1e9), to be added to the seconds component. \n# e.g.\n# The time -1.7 seconds is represented as {sec: -2, nanosec: 3e8}\n# The time 1.7 seconds is represented as {sec: 1, nanosec: 7e8}\nuint32 nanosec\n================================================================================\nMSG: dimos_msgs/LineSegment3D\n# Copyright 2026 Dimensional Inc.\n# SPDX-License-Identifier: Apache-2.0\n\ngeometry_msgs/Point start\ngeometry_msgs/Point end\nfloat64 weight 1.0\n================================================================================\nMSG: geometry_msgs/Point\n# This contains the position of a point in free space\nfloat64 x\nfloat64 y\nfloat64 z\n================================================================================\nMSG: std_msgs/Header\n# Standard metadata for higher-level stamped data types.\n# This is generally used to communicate timestamped data\n# in a particular coordinate frame.\n\n# Two-integer timestamp that is expressed as seconds and nanoseconds.\nbuiltin_interfaces/Time stamp\n\n# Transform frame with which this data is associated.\nstring frame_id\n"
+      },
+      "value": {
+        "header": {
+          "stamp": {
+            "sec": 1757400000,
+            "nanosec": 500000123
+          },
+          "frame_id": "map"
+        },
+        "segments": [
+          {
+            "start": {
+              "x": 1.0,
+              "y": 0.0,
+              "z": 0.0
+            },
+            "end": {
+              "x": 0.0,
+              "y": 2.0,
+              "z": 0.0
+            },
+            "weight": 4.0
+          }
+        ]
+      },
+      "payload_b64": "AAEAAMDLv2h7Zc0dBAAAAG1hcAABAAAAAAAAAAAAAAAAAPA/AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAEAAAAAAAAAAAAAAAAAAABBA",
+      "big_endian_b64": "AAAAAGi/y8AdzWV7AAAABG1hcAAAAAABAAAAAD/wAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAQAAAAAAAAAAAAAAAAAAAAEAQAAAAAAAA"
+    },
+    {
+      "name": "image",
+      "encoding": "sensor_msgs/msg/Image.cdr.v1",
+      "schema": {
+        "type": "sensor_msgs/msg/Image",
+        "definition": "# This message contains an uncompressed image\n# (0, 0) is at top-left corner of image\n\nstd_msgs/Header header # Header timestamp should be acquisition time of image\n                             # Header frame_id should be optical frame of camera\n                             # origin of frame should be optical center of cameara\n                             # +x should point to the right in the image\n                             # +y should point down in the image\n                             # +z should point into to plane of the image\n                             # If the frame_id here and the frame_id of the CameraInfo\n                             # message associated with the image conflict\n                             # the behavior is undefined\n\nuint32 height                # image height, that is, number of rows\nuint32 width                 # image width, that is, number of columns\n\n# The legal values for encoding are in file include/sensor_msgs/image_encodings.hpp\n# If you want to standardize a new string format, join\n# ros-users@lists.ros.org and send an email proposing a new encoding.\n\nstring encoding       # Encoding of pixels -- channel meaning, ordering, size\n                      # taken from the list of strings in include/sensor_msgs/image_encodings.hpp\n\nuint8 is_bigendian    # is this data bigendian?\nuint32 step           # Full row length in bytes\nuint8[] data          # actual matrix data, size is (step * rows)\n================================================================================\nMSG: builtin_interfaces/Time\n# This message communicates ROS Time defined here:\n# https://design.ros2.org/articles/clock_and_time.html\n\n# The seconds component, valid over all int32 values.\nint32 sec\n\n# The nanoseconds component, valid in the range [0, 1e9), to be added to the seconds component. \n# e.g.\n# The time -1.7 seconds is represented as {sec: -2, nanosec: 3e8}\n# The time 1.7 seconds is represented as {sec: 1, nanosec: 7e8}\nuint32 nanosec\n================================================================================\nMSG: std_msgs/Header\n# Standard metadata for higher-level stamped data types.\n# This is generally used to communicate timestamped data\n# in a particular coordinate frame.\n\n# Two-integer timestamp that is expressed as seconds and nanoseconds.\nbuiltin_interfaces/Time stamp\n\n# Transform frame with which this data is associated.\nstring frame_id\n"
+      },
+      "value": {
+        "header": {
+          "stamp": {
+            "sec": 1757400000,
+            "nanosec": 500000123
+          },
+          "frame_id": "map"
+        },
+        "height": 1,
+        "width": 2,
+        "encoding": "rgb8",
+        "is_bigendian": 0,
+        "step": 6,
+        "data": [
+          0,
+          1,
+          2,
+          253,
+          254,
+          255
+        ]
+      },
+      "payload_b64": "AAEAAMDLv2h7Zc0dBAAAAG1hcAABAAAAAgAAAAUAAAByZ2I4AAAAAAYAAAAGAAAAAAEC/f7/",
+      "big_endian_b64": "AAAAAGi/y8AdzWV7AAAABG1hcAAAAAABAAAAAgAAAAVyZ2I4AAAAAAAAAAYAAAAGAAEC/f7/"
+    },
+    {
+      "name": "imu",
+      "encoding": "sensor_msgs/msg/Imu.cdr.v1",
+      "schema": {
+        "type": "sensor_msgs/msg/Imu",
+        "definition": "# This is a message to hold data from an IMU (Inertial Measurement Unit)\n#\n# Accelerations should be in m/s^2 (not in g's), and rotational velocity should be in rad/sec\n#\n# If the covariance of the measurement is known, it should be filled in (if all you know is the\n# variance of each measurement, e.g. from the datasheet, just put those along the diagonal)\n# A covariance matrix of all zeros will be interpreted as \"covariance unknown\", and to use the\n# data a covariance will have to be assumed or gotten from some other source\n#\n# If you have no estimate for one of the data elements (e.g. your IMU doesn't produce an\n# orientation estimate), please set element 0 of the associated covariance matrix to -1\n# If you are interpreting this message, please check for a value of -1 in the first element of each\n# covariance matrix, and disregard the associated estimate.\n\nstd_msgs/Header header\n\ngeometry_msgs/Quaternion orientation\nfloat64[9] orientation_covariance # Row major about x, y, z axes\n\ngeometry_msgs/Vector3 angular_velocity\nfloat64[9] angular_velocity_covariance # Row major about x, y, z axes\n\ngeometry_msgs/Vector3 linear_acceleration\nfloat64[9] linear_acceleration_covariance # Row major x, y z\n================================================================================\nMSG: builtin_interfaces/Time\n# This message communicates ROS Time defined here:\n# https://design.ros2.org/articles/clock_and_time.html\n\n# The seconds component, valid over all int32 values.\nint32 sec\n\n# The nanoseconds component, valid in the range [0, 1e9), to be added to the seconds component. \n# e.g.\n# The time -1.7 seconds is represented as {sec: -2, nanosec: 3e8}\n# The time 1.7 seconds is represented as {sec: 1, nanosec: 7e8}\nuint32 nanosec\n================================================================================\nMSG: geometry_msgs/Quaternion\n# This represents an orientation in free space in quaternion form.\n\nfloat64 x 0\nfloat64 y 0\nfloat64 z 0\nfloat64 w 1\n================================================================================\nMSG: geometry_msgs/Vector3\n# This represents a vector in free space.\n\n# This is semantically different than a point.\n# A vector is always anchored at the origin.\n# When a transform is applied to a vector, only the rotational component is applied.\n\nfloat64 x\nfloat64 y\nfloat64 z\n================================================================================\nMSG: std_msgs/Header\n# Standard metadata for higher-level stamped data types.\n# This is generally used to communicate timestamped data\n# in a particular coordinate frame.\n\n# Two-integer timestamp that is expressed as seconds and nanoseconds.\nbuiltin_interfaces/Time stamp\n\n# Transform frame with which this data is associated.\nstring frame_id\n"
+      },
+      "value": {
+        "header": {
+          "stamp": {
+            "sec": 1757400000,
+            "nanosec": 500000123
+          },
+          "frame_id": "map"
+        },
+        "orientation": {
+          "x": 0.0,
+          "y": 0.0,
+          "z": 0.0,
+          "w": 1.0
+        },
+        "orientation_covariance": [
+          0.0,
+          1.0,
+          2.0,
+          3.0,
+          4.0,
+          5.0,
+          6.0,
+          7.0,
+          8.0
+        ],
+        "angular_velocity": {
+          "x": 0.0,
+          "y": 0.0,
+          "z": 0.0
+        },
+        "angular_velocity_covariance": [
+          0.0,
+          0.0,
+          0.0,
+          0.0,
+          0.0,
+          0.0,
+          0.0,
+          0.0,
+          0.0
+        ],
+        "linear_acceleration": {
+          "x": 0.0,
+          "y": 0.0,
+          "z": 0.0
+        },
+        "linear_acceleration_covariance": [
+          0.0,
+          0.0,
+          0.0,
+          0.0,
+          0.0,
+          0.0,
+          0.0,
+          0.0,
+          0.0
+        ]
+      },
+      "payload_b64": "AAEAAMDLv2h7Zc0dBAAAAG1hcAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAADwPwAAAAAAAAAAAAAAAAAA8D8AAAAAAAAAQAAAAAAAAAhAAAAAAAAAEEAAAAAAAAAUQAAAAAAAABhAAAAAAAAAHEAAAAAAAAAgQAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA==",
+      "big_endian_b64": "AAAAAGi/y8AdzWV7AAAABG1hcAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA/8AAAAAAAAAAAAAAAAAAAP/AAAAAAAABAAAAAAAAAAEAIAAAAAAAAQBAAAAAAAABAFAAAAAAAAEAYAAAAAAAAQBwAAAAAAABAIAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA=="
+    },
+    {
+      "name": "joints",
+      "encoding": "sensor_msgs/msg/JointState.cdr.v1",
+      "schema": {
+        "type": "sensor_msgs/msg/JointState",
+        "definition": "# This is a message that holds data to describe the state of a set of torque controlled joints.\n#\n# The state of each joint (revolute or prismatic) is defined by:\n#  * the position of the joint (rad or m),\n#  * the velocity of the joint (rad/s or m/s) and\n#  * the effort that is applied in the joint (Nm or N).\n#\n# Each joint is uniquely identified by its name\n# The header specifies the time at which the joint states were recorded. All the joint states\n# in one message have to be recorded at the same time.\n#\n# This message consists of a multiple arrays, one for each part of the joint state.\n# The goal is to make each of the fields optional. When e.g. your joints have no\n# effort associated with them, you can leave the effort array empty.\n#\n# All arrays in this message should have the same size, or be empty.\n# This is the only way to uniquely associate the joint name with the correct\n# states.\n\nstd_msgs/Header header\n\nstring[] name\nfloat64[] position\nfloat64[] velocity\nfloat64[] effort\n================================================================================\nMSG: builtin_interfaces/Time\n# This message communicates ROS Time defined here:\n# https://design.ros2.org/articles/clock_and_time.html\n\n# The seconds component, valid over all int32 values.\nint32 sec\n\n# The nanoseconds component, valid in the range [0, 1e9), to be added to the seconds component. \n# e.g.\n# The time -1.7 seconds is represented as {sec: -2, nanosec: 3e8}\n# The time 1.7 seconds is represented as {sec: 1, nanosec: 7e8}\nuint32 nanosec\n================================================================================\nMSG: std_msgs/Header\n# Standard metadata for higher-level stamped data types.\n# This is generally used to communicate timestamped data\n# in a particular coordinate frame.\n\n# Two-integer timestamp that is expressed as seconds and nanoseconds.\nbuiltin_interfaces/Time stamp\n\n# Transform frame with which this data is associated.\nstring frame_id\n"
+      },
+      "value": {
+        "header": {
+          "stamp": {
+            "sec": 1757400000,
+            "nanosec": 500000123
+          },
+          "frame_id": "map"
+        },
+        "name": [
+          "joint_a",
+          "joint_b"
+        ],
+        "position": [
+          1.5,
+          -2.5
+        ],
+        "velocity": [],
+        "effort": []
+      },
+      "payload_b64": "AAEAAMDLv2h7Zc0dBAAAAG1hcAACAAAACAAAAGpvaW50X2EACAAAAGpvaW50X2IAAgAAAAAAAAAAAPg/AAAAAAAABMAAAAAAAAAAAA==",
+      "big_endian_b64": "AAAAAGi/y8AdzWV7AAAABG1hcAAAAAACAAAACGpvaW50X2EAAAAACGpvaW50X2IAAAAAAj/4AAAAAAAAwAQAAAAAAAAAAAAAAAAAAA=="
+    },
+    {
+      "name": "int64",
+      "encoding": "std_msgs/msg/Int64.cdr.v1",
+      "schema": {
+        "type": "std_msgs/msg/Int64",
+        "definition": "# This was originally provided as an example message.\n# It is deprecated as of Foxy\n# It is recommended to create your own semantically meaningful message.\n# However if you would like to continue using this please use the equivalent in example_msgs.\n\nint64 data\n"
+      },
+      "value": {
+        "data": "-9007199254740993"
+      },
+      "payload_b64": "AAEAAP///////9//",
+      "big_endian_b64": "AAAAAP/f////////"
+    },
+    {
+      "name": "empty",
+      "encoding": "std_msgs/msg/Empty.cdr.v1",
+      "schema": {
+        "type": "std_msgs/msg/Empty",
+        "definition": "\n"
+      },
+      "value": {},
+      "payload_b64": "AAEAAAA=",
+      "big_endian_b64": "AAAAAAA="
+    },
+    {
+      "name": "string",
+      "encoding": "std_msgs/msg/String.cdr.v1",
+      "schema": {
+        "type": "std_msgs/msg/String",
+        "definition": "# This was originally provided as an example message.\n# It is deprecated as of Foxy\n# It is recommended to create your own semantically meaningful message.\n# However if you would like to continue using this please use the equivalent in example_msgs.\n\nstring data\n"
+      },
+      "value": {
+        "data": "robot 🦾 café"
+      },
+      "payload_b64": "AAEAABEAAAByb2JvdCDwn6a+IGNhZsOpAA==",
+      "big_endian_b64": "AAAAAAAAABFyb2JvdCDwn6a+IGNhZsOpAA=="
+    }
+  ]
+}
diff --git a/web/shared/fixtures/costmap_frames.json b/web/shared/fixtures/costmap_frames.json
index e9ffa3d305c..9a48edb53ee 100644
--- a/web/shared/fixtures/costmap_frames.json
+++ b/web/shared/fixtures/costmap_frames.json
@@ -5,7 +5,7 @@
       "meta": {
         "w": 5,
         "h": 4,
-        "res": 0.05,
+        "res": 0.05000000074505806,
         "origin": [
           -1.25,
           2.5,
@@ -20,7 +20,7 @@
       "meta": {
         "w": 3,
         "h": 3,
-        "res": 0.1,
+        "res": 0.10000000149011612,
         "origin": [
           1.5,
           -0.75,
diff --git a/web/shared/fixtures/lcm_frames.json b/web/shared/fixtures/lcm_frames.json
deleted file mode 100644
index 692e42df4c0..00000000000
--- a/web/shared/fixtures/lcm_frames.json
+++ /dev/null
@@ -1,1114 +0,0 @@
-{
-  "vectors": [
-    {
-      "name": "pose_stamped",
-      "encoding": "geometry_msgs.PoseStamped.lcm.v1",
-      "schema": {
-        "type": "geometry_msgs.PoseStamped",
-        "fp": "6a82696458c279a0",
-        "structs": {
-          "geometry_msgs.PoseStamped": [
-            [
-              "header",
-              "std_msgs.Header",
-              null
-            ],
-            [
-              "pose",
-              "geometry_msgs.Pose",
-              null
-            ]
-          ],
-          "std_msgs.Header": [
-            [
-              "seq",
-              "int32_t",
-              null
-            ],
-            [
-              "stamp",
-              "std_msgs.Time",
-              null
-            ],
-            [
-              "frame_id",
-              "string",
-              null
-            ]
-          ],
-          "std_msgs.Time": [
-            [
-              "sec",
-              "int32_t",
-              null
-            ],
-            [
-              "nsec",
-              "int32_t",
-              null
-            ]
-          ],
-          "geometry_msgs.Pose": [
-            [
-              "position",
-              "geometry_msgs.Point",
-              null
-            ],
-            [
-              "orientation",
-              "geometry_msgs.Quaternion",
-              null
-            ]
-          ],
-          "geometry_msgs.Point": [
-            [
-              "x",
-              "double",
-              null
-            ],
-            [
-              "y",
-              "double",
-              null
-            ],
-            [
-              "z",
-              "double",
-              null
-            ]
-          ],
-          "geometry_msgs.Quaternion": [
-            [
-              "x",
-              "double",
-              null
-            ],
-            [
-              "y",
-              "double",
-              null
-            ],
-            [
-              "z",
-              "double",
-              null
-            ],
-            [
-              "w",
-              "double",
-              null
-            ]
-          ]
-        }
-      },
-      "payload_b64": "aoJpZFjCeaAAAAAAaL/LwB3NZQAAAAAEbWFwAD/4AAAAAAAAwAQAAAAAAAA/0AAAAAAAAAAAAAAAAAAAAAAAAAAAAAA/4zMzMzMzMz/pmZmZmZma",
-      "value": {
-        "header": {
-          "seq": 0,
-          "stamp": {
-            "sec": 1757400000,
-            "nsec": 500000000
-          },
-          "frame_id": "map"
-        },
-        "pose": {
-          "position": {
-            "x": 1.5,
-            "y": -2.5,
-            "z": 0.25
-          },
-          "orientation": {
-            "x": 0.0,
-            "y": 0.0,
-            "z": 0.6,
-            "w": 0.8
-          }
-        }
-      }
-    },
-    {
-      "name": "laser_scan",
-      "encoding": "sensor_msgs.LaserScan.lcm.v1",
-      "schema": {
-        "type": "sensor_msgs.LaserScan",
-        "fp": "8de5754e00f25f00",
-        "structs": {
-          "sensor_msgs.LaserScan": [
-            [
-              "ranges_length",
-              "int32_t",
-              null
-            ],
-            [
-              "intensities_length",
-              "int32_t",
-              null
-            ],
-            [
-              "header",
-              "std_msgs.Header",
-              null
-            ],
-            [
-              "angle_min",
-              "float",
-              null
-            ],
-            [
-              "angle_max",
-              "float",
-              null
-            ],
-            [
-              "angle_increment",
-              "float",
-              null
-            ],
-            [
-              "time_increment",
-              "float",
-              null
-            ],
-            [
-              "scan_time",
-              "float",
-              null
-            ],
-            [
-              "range_min",
-              "float",
-              null
-            ],
-            [
-              "range_max",
-              "float",
-              null
-            ],
-            [
-              "ranges",
-              "float",
-              [
-                "ranges_length"
-              ]
-            ],
-            [
-              "intensities",
-              "float",
-              [
-                "intensities_length"
-              ]
-            ]
-          ],
-          "std_msgs.Header": [
-            [
-              "seq",
-              "int32_t",
-              null
-            ],
-            [
-              "stamp",
-              "std_msgs.Time",
-              null
-            ],
-            [
-              "frame_id",
-              "string",
-              null
-            ]
-          ],
-          "std_msgs.Time": [
-            [
-              "sec",
-              "int32_t",
-              null
-            ],
-            [
-              "nsec",
-              "int32_t",
-              null
-            ]
-          ]
-        }
-      },
-      "payload_b64": "jeV1TgDyXwAAAAAFAAAAAAAAAAdov8vAHc1lAAAAAAZsYXNlcgC/wAAAP8AAAD9AAAAAAAAAPczMzT6AAABB8AAAPwAAAD+gAABAMAAAQfAAAEFIAAA=",
-      "value": {
-        "ranges_length": 5,
-        "intensities_length": 0,
-        "header": {
-          "seq": 7,
-          "stamp": {
-            "sec": 1757400000,
-            "nsec": 500000000
-          },
-          "frame_id": "laser"
-        },
-        "angle_min": -1.5,
-        "angle_max": 1.5,
-        "angle_increment": 0.75,
-        "time_increment": 0.0,
-        "scan_time": 0.10000000149011612,
-        "range_min": 0.25,
-        "range_max": 30.0,
-        "ranges": [
-          0.5,
-          1.25,
-          2.75,
-          30.0,
-          12.5
-        ],
-        "intensities": []
-      }
-    },
-    {
-      "name": "imu",
-      "encoding": "sensor_msgs.Imu.lcm.v1",
-      "schema": {
-        "type": "sensor_msgs.Imu",
-        "fp": "31ab205c8dd57aa8",
-        "structs": {
-          "sensor_msgs.Imu": [
-            [
-              "header",
-              "std_msgs.Header",
-              null
-            ],
-            [
-              "orientation",
-              "geometry_msgs.Quaternion",
-              null
-            ],
-            [
-              "orientation_covariance",
-              "double",
-              [
-                9
-              ]
-            ],
-            [
-              "angular_velocity",
-              "geometry_msgs.Vector3",
-              null
-            ],
-            [
-              "angular_velocity_covariance",
-              "double",
-              [
-                9
-              ]
-            ],
-            [
-              "linear_acceleration",
-              "geometry_msgs.Vector3",
-              null
-            ],
-            [
-              "linear_acceleration_covariance",
-              "double",
-              [
-                9
-              ]
-            ]
-          ],
-          "std_msgs.Header": [
-            [
-              "seq",
-              "int32_t",
-              null
-            ],
-            [
-              "stamp",
-              "std_msgs.Time",
-              null
-            ],
-            [
-              "frame_id",
-              "string",
-              null
-            ]
-          ],
-          "std_msgs.Time": [
-            [
-              "sec",
-              "int32_t",
-              null
-            ],
-            [
-              "nsec",
-              "int32_t",
-              null
-            ]
-          ],
-          "geometry_msgs.Quaternion": [
-            [
-              "x",
-              "double",
-              null
-            ],
-            [
-              "y",
-              "double",
-              null
-            ],
-            [
-              "z",
-              "double",
-              null
-            ],
-            [
-              "w",
-              "double",
-              null
-            ]
-          ],
-          "geometry_msgs.Vector3": [
-            [
-              "x",
-              "double",
-              null
-            ],
-            [
-              "y",
-              "double",
-              null
-            ],
-            [
-              "z",
-              "double",
-              null
-            ]
-          ]
-        }
-      },
-      "payload_b64": "MasgXI3VeqgAAAAAaL/LwB3NZQAAAAAEaW11AAAAAAAAAAAAAAAAAAAAAAA/4zMzMzMzMz/pmZmZmZmaP4R64UeuFHs/hHrhR64Uez+EeuFHrhR7P4R64UeuFHs/hHrhR64Uez+EeuFHrhR7P4R64UeuFHs/hHrhR64Uez+EeuFHrhR7P7mZmZmZmZq/yZmZmZmZmj/TMzMzMzMzAAAAAAAAAAA/8AAAAAAAAEAAAAAAAAAAQAgAAAAAAABAEAAAAAAAAEAUAAAAAAAAQBgAAAAAAABAHAAAAAAAAEAgAAAAAAAAAAAAAAAAAAAAAAAAAAAAAEAjAAAAAAAAP564UeuFHrg/nrhR64UeuD+euFHrhR64P564UeuFHrg/nrhR64UeuD+euFHrhR64P564UeuFHrg/nrhR64UeuD+euFHrhR64",
-      "value": {
-        "header": {
-          "seq": 0,
-          "stamp": {
-            "sec": 1757400000,
-            "nsec": 500000000
-          },
-          "frame_id": "imu"
-        },
-        "orientation": {
-          "x": 0.0,
-          "y": 0.0,
-          "z": 0.6,
-          "w": 0.8
-        },
-        "orientation_covariance": [
-          0.01,
-          0.01,
-          0.01,
-          0.01,
-          0.01,
-          0.01,
-          0.01,
-          0.01,
-          0.01
-        ],
-        "angular_velocity": {
-          "x": 0.1,
-          "y": -0.2,
-          "z": 0.3
-        },
-        "angular_velocity_covariance": [
-          0.0,
-          1.0,
-          2.0,
-          3.0,
-          4.0,
-          5.0,
-          6.0,
-          7.0,
-          8.0
-        ],
-        "linear_acceleration": {
-          "x": 0.0,
-          "y": 0.0,
-          "z": 9.5
-        },
-        "linear_acceleration_covariance": [
-          0.03,
-          0.03,
-          0.03,
-          0.03,
-          0.03,
-          0.03,
-          0.03,
-          0.03,
-          0.03
-        ]
-      }
-    },
-    {
-      "name": "occupancy_grid",
-      "encoding": "nav_msgs.OccupancyGrid.lcm.v1",
-      "schema": {
-        "type": "nav_msgs.OccupancyGrid",
-        "fp": "e7dfd179cdfc3b65",
-        "structs": {
-          "nav_msgs.OccupancyGrid": [
-            [
-              "data_length",
-              "int32_t",
-              null
-            ],
-            [
-              "header",
-              "std_msgs.Header",
-              null
-            ],
-            [
-              "info",
-              "nav_msgs.MapMetaData",
-              null
-            ],
-            [
-              "data",
-              "int8_t",
-              [
-                "data_length"
-              ]
-            ]
-          ],
-          "std_msgs.Header": [
-            [
-              "seq",
-              "int32_t",
-              null
-            ],
-            [
-              "stamp",
-              "std_msgs.Time",
-              null
-            ],
-            [
-              "frame_id",
-              "string",
-              null
-            ]
-          ],
-          "std_msgs.Time": [
-            [
-              "sec",
-              "int32_t",
-              null
-            ],
-            [
-              "nsec",
-              "int32_t",
-              null
-            ]
-          ],
-          "nav_msgs.MapMetaData": [
-            [
-              "map_load_time",
-              "std_msgs.Time",
-              null
-            ],
-            [
-              "resolution",
-              "float",
-              null
-            ],
-            [
-              "width",
-              "int32_t",
-              null
-            ],
-            [
-              "height",
-              "int32_t",
-              null
-            ],
-            [
-              "origin",
-              "geometry_msgs.Pose",
-              null
-            ]
-          ],
-          "geometry_msgs.Pose": [
-            [
-              "position",
-              "geometry_msgs.Point",
-              null
-            ],
-            [
-              "orientation",
-              "geometry_msgs.Quaternion",
-              null
-            ]
-          ],
-          "geometry_msgs.Point": [
-            [
-              "x",
-              "double",
-              null
-            ],
-            [
-              "y",
-              "double",
-              null
-            ],
-            [
-              "z",
-              "double",
-              null
-            ]
-          ],
-          "geometry_msgs.Quaternion": [
-            [
-              "x",
-              "double",
-              null
-            ],
-            [
-              "y",
-              "double",
-              null
-            ],
-            [
-              "z",
-              "double",
-              null
-            ],
-            [
-              "w",
-              "double",
-              null
-            ]
-          ]
-        }
-      },
-      "payload_b64": "59/Rec38O2UAAAAGAAAAAGi/y8AdzWUAAAAABG1hcABov8vAHc1lAD1MzM0AAAADAAAAAsAUAAAAAAAAwBQAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAD/wAAAAAAAAAGT/MgAH",
-      "value": {
-        "data_length": 6,
-        "header": {
-          "seq": 0,
-          "stamp": {
-            "sec": 1757400000,
-            "nsec": 500000000
-          },
-          "frame_id": "map"
-        },
-        "info": {
-          "map_load_time": {
-            "sec": 1757400000,
-            "nsec": 500000000
-          },
-          "resolution": 0.05000000074505806,
-          "width": 3,
-          "height": 2,
-          "origin": {
-            "position": {
-              "x": -5.0,
-              "y": -5.0,
-              "z": 0.0
-            },
-            "orientation": {
-              "x": 0.0,
-              "y": 0.0,
-              "z": 0.0,
-              "w": 1.0
-            }
-          }
-        },
-        "data": [
-          0,
-          100,
-          -1,
-          50,
-          0,
-          7
-        ]
-      }
-    },
-    {
-      "name": "joint_state",
-      "encoding": "sensor_msgs.JointState.lcm.v1",
-      "schema": {
-        "type": "sensor_msgs.JointState",
-        "fp": "ccefa8a492b967e3",
-        "structs": {
-          "sensor_msgs.JointState": [
-            [
-              "name_length",
-              "int32_t",
-              null
-            ],
-            [
-              "position_length",
-              "int32_t",
-              null
-            ],
-            [
-              "velocity_length",
-              "int32_t",
-              null
-            ],
-            [
-              "effort_length",
-              "int32_t",
-              null
-            ],
-            [
-              "header",
-              "std_msgs.Header",
-              null
-            ],
-            [
-              "name",
-              "string",
-              [
-                "name_length"
-              ]
-            ],
-            [
-              "position",
-              "double",
-              [
-                "position_length"
-              ]
-            ],
-            [
-              "velocity",
-              "double",
-              [
-                "velocity_length"
-              ]
-            ],
-            [
-              "effort",
-              "double",
-              [
-                "effort_length"
-              ]
-            ]
-          ],
-          "std_msgs.Header": [
-            [
-              "seq",
-              "int32_t",
-              null
-            ],
-            [
-              "stamp",
-              "std_msgs.Time",
-              null
-            ],
-            [
-              "frame_id",
-              "string",
-              null
-            ]
-          ],
-          "std_msgs.Time": [
-            [
-              "sec",
-              "int32_t",
-              null
-            ],
-            [
-              "nsec",
-              "int32_t",
-              null
-            ]
-          ]
-        }
-      },
-      "payload_b64": "zO+opJK5Z+MAAAADAAAAAwAAAAMAAAADAAAAAGi/y8AdzWUAAAAABGFybQAAAAAJc2hvdWxkZXIAAAAABmVsYm93AAAAAAl3cmlzdF/DqQA/uZmZmZmZmr/JmZmZmZmaP9MzMzMzMzM/hHrhR64Uez+UeuFHrhR7P564UeuFHrg/8AAAAAAAAEAAAAAAAAAAQAgAAAAAAAA=",
-      "value": {
-        "name_length": 3,
-        "position_length": 3,
-        "velocity_length": 3,
-        "effort_length": 3,
-        "header": {
-          "seq": 0,
-          "stamp": {
-            "sec": 1757400000,
-            "nsec": 500000000
-          },
-          "frame_id": "arm"
-        },
-        "name": [
-          "shoulder",
-          "elbow",
-          "wrist_\u00e9"
-        ],
-        "position": [
-          0.1,
-          -0.2,
-          0.3
-        ],
-        "velocity": [
-          0.01,
-          0.02,
-          0.03
-        ],
-        "effort": [
-          1.0,
-          2.0,
-          3.0
-        ]
-      }
-    },
-    {
-      "name": "point_cloud",
-      "encoding": "sensor_msgs.PointCloud2.lcm.v1",
-      "schema": {
-        "type": "sensor_msgs.PointCloud2",
-        "fp": "f5eb3da1c2853175",
-        "structs": {
-          "sensor_msgs.PointCloud2": [
-            [
-              "fields_length",
-              "int32_t",
-              null
-            ],
-            [
-              "data_length",
-              "int32_t",
-              null
-            ],
-            [
-              "header",
-              "std_msgs.Header",
-              null
-            ],
-            [
-              "height",
-              "int32_t",
-              null
-            ],
-            [
-              "width",
-              "int32_t",
-              null
-            ],
-            [
-              "fields",
-              "sensor_msgs.PointField",
-              [
-                "fields_length"
-              ]
-            ],
-            [
-              "is_bigendian",
-              "boolean",
-              null
-            ],
-            [
-              "point_step",
-              "int32_t",
-              null
-            ],
-            [
-              "row_step",
-              "int32_t",
-              null
-            ],
-            [
-              "data",
-              "byte",
-              [
-                "data_length"
-              ]
-            ],
-            [
-              "is_dense",
-              "boolean",
-              null
-            ]
-          ],
-          "std_msgs.Header": [
-            [
-              "seq",
-              "int32_t",
-              null
-            ],
-            [
-              "stamp",
-              "std_msgs.Time",
-              null
-            ],
-            [
-              "frame_id",
-              "string",
-              null
-            ]
-          ],
-          "std_msgs.Time": [
-            [
-              "sec",
-              "int32_t",
-              null
-            ],
-            [
-              "nsec",
-              "int32_t",
-              null
-            ]
-          ],
-          "sensor_msgs.PointField": [
-            [
-              "name",
-              "string",
-              null
-            ],
-            [
-              "offset",
-              "int32_t",
-              null
-            ],
-            [
-              "datatype",
-              "byte",
-              null
-            ],
-            [
-              "count",
-              "int32_t",
-              null
-            ]
-          ]
-        }
-      },
-      "payload_b64": "9es9ocKFMXUAAAADAAAAGAAAAABov8vAHc1lAAAAAAZsaWRhcgAAAAABAAAAAgAAAAJ4AAAAAAAHAAAAAQAAAAJ5AAAAAAQHAAAAAQAAAAJ6AAAAAAgHAAAAAQAAAAAMAAAAGAAAgD8AAABAAABAQAAAgMAAALBAAADIQAE=",
-      "value": {
-        "fields_length": 3,
-        "data_length": 24,
-        "header": {
-          "seq": 0,
-          "stamp": {
-            "sec": 1757400000,
-            "nsec": 500000000
-          },
-          "frame_id": "lidar"
-        },
-        "height": 1,
-        "width": 2,
-        "fields": [
-          {
-            "name": "x",
-            "offset": 0,
-            "datatype": 7,
-            "count": 1
-          },
-          {
-            "name": "y",
-            "offset": 4,
-            "datatype": 7,
-            "count": 1
-          },
-          {
-            "name": "z",
-            "offset": 8,
-            "datatype": 7,
-            "count": 1
-          }
-        ],
-        "is_bigendian": false,
-        "point_step": 12,
-        "row_step": 24,
-        "data": "AACAPwAAAEAAAEBAAACAwAAAsEAAAMhA",
-        "is_dense": true
-      }
-    },
-    {
-      "name": "tf_message",
-      "encoding": "tf2_msgs.TFMessage.lcm.v1",
-      "schema": {
-        "type": "tf2_msgs.TFMessage",
-        "fp": "c2b8a1c33a8923ec",
-        "structs": {
-          "tf2_msgs.TFMessage": [
-            [
-              "transforms_length",
-              "int32_t",
-              null
-            ],
-            [
-              "transforms",
-              "geometry_msgs.TransformStamped",
-              [
-                "transforms_length"
-              ]
-            ]
-          ],
-          "geometry_msgs.TransformStamped": [
-            [
-              "header",
-              "std_msgs.Header",
-              null
-            ],
-            [
-              "child_frame_id",
-              "string",
-              null
-            ],
-            [
-              "transform",
-              "geometry_msgs.Transform",
-              null
-            ]
-          ],
-          "std_msgs.Header": [
-            [
-              "seq",
-              "int32_t",
-              null
-            ],
-            [
-              "stamp",
-              "std_msgs.Time",
-              null
-            ],
-            [
-              "frame_id",
-              "string",
-              null
-            ]
-          ],
-          "std_msgs.Time": [
-            [
-              "sec",
-              "int32_t",
-              null
-            ],
-            [
-              "nsec",
-              "int32_t",
-              null
-            ]
-          ],
-          "geometry_msgs.Transform": [
-            [
-              "translation",
-              "geometry_msgs.Vector3",
-              null
-            ],
-            [
-              "rotation",
-              "geometry_msgs.Quaternion",
-              null
-            ]
-          ],
-          "geometry_msgs.Vector3": [
-            [
-              "x",
-              "double",
-              null
-            ],
-            [
-              "y",
-              "double",
-              null
-            ],
-            [
-              "z",
-              "double",
-              null
-            ]
-          ],
-          "geometry_msgs.Quaternion": [
-            [
-              "x",
-              "double",
-              null
-            ],
-            [
-              "y",
-              "double",
-              null
-            ],
-            [
-              "z",
-              "double",
-              null
-            ],
-            [
-              "w",
-              "double",
-              null
-            ]
-          ]
-        }
-      },
-      "payload_b64": "wrihwzqJI+wAAAACAAAAAWi/y8AdzWUAAAAABG1hcAAAAAAKYmFzZV9saW5rAD+5mZmZmZmaAAAAAAAAAAA/4AAAAAAAAAAAAAAAAAAAAAAAAAAAAAA/4zMzMzMzMz/pmZmZmZmaAAAAAWi/y8AdzWUAAAAACmJhc2VfbGluawAAAAAHY2FtZXJhAL/wAAAAAAAAQAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAD/wAAAAAAAA",
-      "value": {
-        "transforms_length": 2,
-        "transforms": [
-          {
-            "header": {
-              "seq": 1,
-              "stamp": {
-                "sec": 1757400000,
-                "nsec": 500000000
-              },
-              "frame_id": "map"
-            },
-            "child_frame_id": "base_link",
-            "transform": {
-              "translation": {
-                "x": 0.1,
-                "y": 0.0,
-                "z": 0.5
-              },
-              "rotation": {
-                "x": 0.0,
-                "y": 0.0,
-                "z": 0.6,
-                "w": 0.8
-              }
-            }
-          },
-          {
-            "header": {
-              "seq": 1,
-              "stamp": {
-                "sec": 1757400000,
-                "nsec": 500000000
-              },
-              "frame_id": "base_link"
-            },
-            "child_frame_id": "camera",
-            "transform": {
-              "translation": {
-                "x": -1.0,
-                "y": 2.0,
-                "z": 0.0
-              },
-              "rotation": {
-                "x": 0.0,
-                "y": 0.0,
-                "z": 0.0,
-                "w": 1.0
-              }
-            }
-          }
-        ]
-      }
-    },
-    {
-      "name": "int64",
-      "encoding": "std_msgs.Int64.lcm.v1",
-      "schema": {
-        "type": "std_msgs.Int64",
-        "fp": "2cbcf9fdee91023e",
-        "structs": {
-          "std_msgs.Int64": [
-            [
-              "data",
-              "int64_t",
-              null
-            ]
-          ]
-        }
-      },
-      "payload_b64": "LLz5/e6RAj7/3////////w==",
-      "value": {
-        "data": "-9007199254740993"
-      }
-    }
-  ]
-}