diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..b2be92b --- /dev/null +++ b/.gitignore @@ -0,0 +1 @@ +result diff --git a/CMakeLists.txt b/CMakeLists.txt deleted file mode 100644 index 68bb066..0000000 --- a/CMakeLists.txt +++ /dev/null @@ -1,102 +0,0 @@ -# Copyright 2026 Dimensional Inc. -# SPDX-License-Identifier: Apache-2.0 - -cmake_minimum_required(VERSION 3.14) -project(dimos_cuvslam CXX) - -set(CMAKE_CXX_STANDARD 20) -set(CMAKE_CXX_STANDARD_REQUIRED ON) - -include(FetchContent) - -# Generated LCM message headers. 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) - -# Transform algebra and the tf client (src/utils/tf.hpp). Header-only; nix provides -# it via buildInputs, other builds via FetchContent. -include(FetchContent) -find_package(Eigen3 3.4 QUIET) -if(NOT Eigen3_FOUND) - FetchContent_Declare(eigen - URL https://gitlab.com/libeigen/eigen/-/archive/3.4.1/eigen-3.4.1.tar.gz - URL_HASH SHA256=b93c667d1b69265cdb4d9f30ec21f8facbbe8b307cf34c0b9942834c6d4fdbe2 - DOWNLOAD_EXTRACT_TIMESTAMP TRUE - ) - set(EIGEN_BUILD_DOC OFF) - set(EIGEN_BUILD_TESTING OFF) - set(EIGEN_BUILD_PKGCONFIG OFF) - FetchContent_MakeAvailable(eigen) -endif() - -# Header-only dimos native C++ SDK, overridable so nix can point at the flake input. -if(NOT DEFINED DIMOS_NATIVE_CPP_DIR) - message(FATAL_ERROR - "DIMOS_NATIVE_CPP_DIR is not set. Build via `nix build` (which pins dimos and " - "passes its native/cpp), or point it at a dimos checkout's native/cpp.") -endif() -add_subdirectory(${DIMOS_NATIVE_CPP_DIR} ${CMAKE_BINARY_DIR}/dimos_native) - -if(NOT DEFINED CUVSLAM_SDK_DIR) - message(FATAL_ERROR - "CUVSLAM_SDK_DIR is not set. Build via `nix build` (which packages NVIDIA's " - "official C++ SDK), or pass -DCUVSLAM_SDK_DIR=/path/to/extracted/sdk.") -endif() -find_path(CUVSLAM_INCLUDE_DIR cuvslam/cuvslam2.h HINTS ${CUVSLAM_SDK_DIR}/include REQUIRED) -find_library(CUVSLAM_LIBRARY cuvslam HINTS ${CUVSLAM_SDK_DIR}/lib REQUIRED) -message(STATUS "cuVSLAM headers: ${CUVSLAM_INCLUDE_DIR}") -message(STATUS "cuVSLAM library: ${CUVSLAM_LIBRARY}") - -add_executable(cuvslam_odometry src/cuvslam_main.cpp) -target_include_directories(cuvslam_odometry PRIVATE - ${dimos_lcm_SOURCE_DIR}/generated/cpp_lcm_msgs - # Vendored lcm-gen output for types dimos-lcm does not ship yet (ImuInfo). - ${CMAKE_CURRENT_SOURCE_DIR}/src/vendor - ${LCM_INCLUDE_DIRS} - ${CUVSLAM_INCLUDE_DIR} -) -target_link_libraries(cuvslam_odometry PRIVATE - dimos_native - Eigen3::Eigen - ${LCM_LIBRARIES} - ${CUVSLAM_LIBRARY} -) -target_link_directories(cuvslam_odometry PRIVATE ${LCM_LIBRARY_DIRS}) -if(APPLE) - # The SDK's install name is @rpath/libcuvslam.dylib, so the directory it was linked - # from has to survive into the installed binary. - get_filename_component(CUVSLAM_LIBRARY_DIR ${CUVSLAM_LIBRARY} DIRECTORY) - set_target_properties(cuvslam_odometry PROPERTIES - INSTALL_RPATH ${CUVSLAM_LIBRARY_DIR} - BUILD_WITH_INSTALL_RPATH TRUE - ) -endif() -install(TARGETS cuvslam_odometry RUNTIME DESTINATION bin) - -# The tf client's test suite (a mirror of the rust one's; tf.hpp is std-only, so the -# tests need doctest alone). Off by default; flip on to run them locally. -option(CUVSLAM_UTIL_TESTS "Build the cuvslam util tests" OFF) -if(CUVSLAM_UTIL_TESTS) - enable_testing() - FetchContent_Declare(doctest - GIT_REPOSITORY https://github.com/doctest/doctest.git - GIT_TAG 1da23a3e8119ec5cce4f9388e91b065e20bf06f5 # v2.4.12 - ) - FetchContent_MakeAvailable(doctest) - add_executable(cuvslam_util_tests src/utils/test_tf.cpp) - target_compile_features(cuvslam_util_tests PRIVATE cxx_std_20) - target_compile_definitions(cuvslam_util_tests - PRIVATE DOCTEST_CONFIG_IMPLEMENT_WITH_MAIN) - target_compile_options(cuvslam_util_tests PRIVATE -Wall -Wextra -Werror) - target_link_libraries(cuvslam_util_tests PRIVATE doctest::doctest) - include(${doctest_SOURCE_DIR}/scripts/cmake/doctest.cmake) - doctest_discover_tests(cuvslam_util_tests) -endif() diff --git a/README.md b/README.md index 96e503b..f97e098 100644 --- a/README.md +++ b/README.md @@ -4,12 +4,13 @@ SLAM stack for dimos. - `cuvslam/` — our cuVSLAM lineage (NVIDIA cuVSLAM v17 + odometry state serialization). Licensed under the NVIDIA Community License (`cuvslam/LICENSE`). -- `src/`, `CMakeLists.txt` — the dimos native odometry module (`cuvslam_odometry`) - built on top of it. -- `flake.nix` — builds both: `nix build .#` (variants: `x86_64-cuda12`, - `x86_64-cuda13`, `orin`, `thor`, `metal`; fork-source builds for x86_64-cuda12 - and orin, NVIDIA release tarballs for the rest). The dimos header-only native - C++ SDK comes in as the pinned `dimos-src` flake input. +- `rust/` — the `dim_slam` dimos module: cuVSLAM visual odometry (via a small + extern-"C" shim in `shim/`, the only C++) feeding an error-state Kalman fusion + filter in-process. Needs `CUVSLAM_SDK_DIR` (a dir with + `include/cuvslam/cuvslam2.h` and `lib/libcuvslam.*`) to build. +- `flake.nix` — builds everything: `nix build .#` (variants: + `x86_64-cuda12`, `x86_64-cuda13`, `orin`, `thor`, `metal`; fork-source builds + for x86_64-cuda12 and orin, NVIDIA release tarballs for the rest). dimos consumes this repo by git tag: `nix build github:dimensionalOS/dimSLAM/#` diff --git a/flake.lock b/flake.lock index df42282..0160bf2 100644 --- a/flake.lock +++ b/flake.lock @@ -1,39 +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" - } - }, - "dimos-src": { - "flake": false, - "locked": { - "lastModified": 1786605362, - "narHash": "sha256-7JVuQyC5LtQaocNEKzihf3KBnDn+Idy9x56az5B67xA=", - "owner": "dimensionalOS", - "repo": "dimos", - "rev": "bc5605aa5de303b557d40215e4e608a204713fa5", - "type": "github" - }, - "original": { - "owner": "dimensionalOS", - "repo": "dimos", - "rev": "bc5605aa5de303b557d40215e4e608a204713fa5", - "type": "github" - } - }, "flake-utils": { "inputs": { "systems": "systems" @@ -52,29 +18,6 @@ "type": "github" } }, - "lcm-extended": { - "inputs": { - "flake-utils": [ - "flake-utils" - ], - "nixpkgs": [ - "nixpkgs" - ] - }, - "locked": { - "lastModified": 1774902379, - "narHash": "sha256-gRFvEkbXCEoG4jEmsT+i0bMZ5kDHOtAaPsrbStXjdu4=", - "owner": "jeff-hykin", - "repo": "lcm_extended", - "rev": "7d12ad8546d3daae30528a6c28f2c9ff5b10baf7", - "type": "github" - }, - "original": { - "owner": "jeff-hykin", - "repo": "lcm_extended", - "type": "github" - } - }, "nixpkgs": { "locked": { "lastModified": 1785571196, @@ -91,31 +34,10 @@ "type": "github" } }, - "pfr": { - "flake": false, - "locked": { - "lastModified": 1750486035, - "narHash": "sha256-Evp15skYDclMZ3IIV7VLwYGwu9BsZMCDKIP0k9cb+6E=", - "owner": "apolukhin", - "repo": "pfr_non_boost", - "rev": "b1a9798677adc191e21dd7904fb42cbe2f2889a7", - "type": "github" - }, - "original": { - "owner": "apolukhin", - "ref": "2.3.2", - "repo": "pfr_non_boost", - "type": "github" - } - }, "root": { "inputs": { - "dimos-lcm": "dimos-lcm", - "dimos-src": "dimos-src", "flake-utils": "flake-utils", - "lcm-extended": "lcm-extended", - "nixpkgs": "nixpkgs", - "pfr": "pfr" + "nixpkgs": "nixpkgs" } }, "systems": { diff --git a/flake.nix b/flake.nix index 7686b50..234b1dd 100644 --- a/flake.nix +++ b/flake.nix @@ -1,33 +1,12 @@ { - description = "dimSLAM: cuVSLAM (in cuvslam/) plus the dimos odometry module built on it"; + description = "dimSLAM: cuVSLAM (in cuvslam/) plus the Rust dim_slam module built on it"; inputs = { nixpkgs.url = "github:NixOS/nixpkgs/nixos-unstable"; flake-utils.url = "github:numtide/flake-utils"; - lcm-extended = { - url = "github:jeff-hykin/lcm_extended"; - inputs.nixpkgs.follows = "nixpkgs"; - inputs.flake-utils.follows = "flake-utils"; - }; - # Generated LCM message headers, via a FetchContent source override. - dimos-lcm = { - url = "github:dimensionalOS/dimos-lcm/main"; - flake = false; - }; - # Standalone Boost.PFR, the same way. - pfr = { - url = "github:apolukhin/pfr_non_boost/2.3.2"; - flake = false; - }; - # The dimos header-only native C++ SDK (module/transport/config headers). - # Pinned to a dimos main revision; bump when native/cpp changes. - dimos-src = { - url = "github:dimensionalOS/dimos/bc5605aa5de303b557d40215e4e608a204713fa5"; - flake = false; - }; }; - outputs = { self, nixpkgs, flake-utils, lcm-extended, dimos-lcm, pfr, dimos-src, ... }: + outputs = { self, nixpkgs, flake-utils, ... }: flake-utils.lib.eachDefaultSystem (system: let isDarwin = nixpkgs.lib.hasSuffix "-darwin" system; @@ -35,7 +14,6 @@ inherit system; config = { allowUnfree = true; cudaSupport = !isDarwin; }; }; - lcm = lcm-extended.packages.${system}.lcm; # Every C++ build NVIDIA ships for this release. The ubuntu flavor does not # matter under autoPatchelf. `cuda` is the matching nixpkgs set: a runtime @@ -264,42 +242,39 @@ # the module's runtime libs. sdkPackageFor = name: sdk: if forkBuilds ? ${name} then forkSdkFor name forkBuilds.${name} else sdkFor name sdk; - runtimeCudaFor = name: sdk: - if forkBuilds ? ${name} then { cuda = forkBuilds.${name}.cuda; } else sdk; - moduleFor = name: sdk: let sdkPackage = sdkPackageFor name sdk; in pkgs.stdenv.mkDerivation { - pname = "dimos-cuvslam-native"; - version = "0.1.0"; - # cmake inputs only. Editing cuvslam.py must not rebuild the C++. - src = pkgs.lib.fileset.toSource { - root = ./.; - fileset = pkgs.lib.fileset.unions [ ./CMakeLists.txt ./src ]; - }; - - nativeBuildInputs = [ pkgs.cmake pkgs.pkg-config ] - ++ pkgs.lib.optionals isDarwin [ pkgs.makeWrapper ] - ++ pkgs.lib.optionals (!isDarwin) [ pkgs.autoPatchelfHook ]; - buildInputs = [ lcm pkgs.glib pkgs.nlohmann_json pkgs.eigen sdkPackage ] - ++ cudaLibs (runtimeCudaFor name sdk); - - cmakeFlags = [ - # cmake otherwise defaults to no optimisation at all. - "-DCMAKE_BUILD_TYPE=Release" - "-DCMAKE_POLICY_VERSION_MINIMUM=3.5" - "-DFETCHCONTENT_SOURCE_DIR_DIMOS_LCM=${dimos-lcm}" - "-DFETCHCONTENT_SOURCE_DIR_PFR=${pfr}" - "-DDIMOS_NATIVE_CPP_DIR=${dimos-src}/native/cpp" - "-DCUVSLAM_SDK_DIR=${sdkPackage}" - ]; - - # The archive ships every kernel already compiled, but its store path is - # read-only so CuMetal cannot use it as the normal cache; this is the read-only - # lookup it consults first. - postInstall = pkgs.lib.optionalString isDarwin '' - wrapProgram $out/bin/cuvslam_odometry \ - --set-default CUMETAL_PREBUILT_CACHE_DIR ${sdkPackage}/share/cumetal-cache - ''; + # The crates' git dependencies, pre-fetched so the sandboxed cargo build + # needs no network. Both dimos-module crates come from the same dimos + # checkout, so they share a hash. Bump alongside Cargo.lock. + rustGitDepHashes = { + "dimos-module-0.1.0" = "sha256-nMnCk9RbDKhOUZRCQUCjZVVPeQY8HrwFLItaZ64Q1KY="; + "dimos-module-macros-0.1.0" = "sha256-nMnCk9RbDKhOUZRCQUCjZVVPeQY8HrwFLItaZ64Q1KY="; + "lcm-msgs-0.1.0" = "sha256-GGkx4Mn6NYP6KZecmoRLKGWIih/+y8OgNn12DeXX6n8="; }; + + moduleFor = name: sdk: let sdkPackage = sdkPackageFor name sdk; in + pkgs.rustPlatform.buildRustPackage { + pname = "dim_slam"; + version = "0.1.0"; + src = ./rust; + cargoLock = { + lockFile = ./rust/Cargo.lock; + outputHashes = rustGitDepHashes; + }; + # build.rs compiles the shim against this SDK and embeds lib/ as an rpath. + env.CUVSLAM_SDK_DIR = sdkPackage; + nativeBuildInputs = pkgs.lib.optionals isDarwin [ pkgs.makeWrapper ]; + # The test binary links libcuvslam, whose CUDA runtime wants a GPU + # driver the build sandbox lacks; unit tests run via plain cargo test. + doCheck = false; + # The archive ships every kernel already compiled, but its store path is + # read-only so CuMetal cannot use it as the normal cache; this is the + # read-only lookup it consults first. + postInstall = pkgs.lib.optionalString isDarwin '' + wrapProgram $out/bin/dim_slam \ + --set-default CUMETAL_PREBUILT_CACHE_DIR ${sdkPackage}/share/cumetal-cache + ''; + }; in { # One package per build NVIDIA ships for this arch. packages = pkgs.lib.mapAttrs moduleFor forThisSystem @@ -310,7 +285,8 @@ // { default = moduleFor defaultVariant forThisSystem.${defaultVariant}; }; devShells.default = pkgs.mkShell { - inputsFrom = [ self.packages.${system}.default ]; + packages = [ pkgs.cargo pkgs.rustc pkgs.clippy pkgs.rustfmt ]; + CUVSLAM_SDK_DIR = sdkPackageFor defaultVariant forThisSystem.${defaultVariant}; }; }); } diff --git a/result b/result deleted file mode 120000 index 9299ee6..0000000 --- a/result +++ /dev/null @@ -1 +0,0 @@ -/nix/store/fbfixh57nycbcxgz1rq213576wfz16mm-dimos-cuvslam-native-0.1.0 \ No newline at end of file diff --git a/rust/.cargo/config.toml b/rust/.cargo/config.toml new file mode 100644 index 0000000..3a06773 --- /dev/null +++ b/rust/.cargo/config.toml @@ -0,0 +1,3 @@ +# dimos is a private repo: fetch with the system git so its credentials apply. +[net] +git-fetch-with-cli = true diff --git a/rust/.gitignore b/rust/.gitignore new file mode 100644 index 0000000..2f7896d --- /dev/null +++ b/rust/.gitignore @@ -0,0 +1 @@ +target/ diff --git a/rust/Cargo.lock b/rust/Cargo.lock new file mode 100644 index 0000000..27e01d7 --- /dev/null +++ b/rust/Cargo.lock @@ -0,0 +1,4333 @@ +# 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 = "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 2.0.20", + "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.3", +] + +[[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 = "base64ct" +version = "1.8.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "2af50177e190e07a26ab74f8b1efbfe2ef87da2116221318cb1c2e82baf7de06" + +[[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 = "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" + +[[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.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "509591b7bcd67f4ef775afad7662703b4935daaa6ec0e5605cfb1090b32a2b6d" +dependencies = [ + "find-msvc-tools", + "jobserver", + "libc", + "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.2.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f079e83a288787bcd14a6aea84cee5c87a67c5a3e660c30f557a3d24761b3527" + +[[package]] +name = "chacha20" +version = "0.10.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "d524456ba66e72eb8b115ff89e01e497f8e6d11d78b70b1aa13c0fbd97540a81" +dependencies = [ + "cfg-if", + "cpufeatures 0.3.0", + "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.7" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ba5a308b75df32fe02788e748662718f03fde005016435c444eea572398219fd" +dependencies = [ + "bytes", + "memchr", +] + +[[package]] +name = "const-oid" +version = "0.9.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "c2459377285ad874054d797f3ccebf984978aa39129f6eafde5cdc8315b612f8" + +[[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.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8b2a41393f66f16b0823bb79094d54ac5fbd34ab292ddafb9a0456ac9f87d201" +dependencies = [ + "libc", +] + +[[package]] +name = "crc32fast" +version = "1.5.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9481c1c90cbf2ac953f07c8d4a58aa3945c425b7185c9154d67a65e4230da511" +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 2.0.20", +] + +[[package]] +name = "der" +version = "0.7.10" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e7c1832837b905bbfb5101e07cc24c8deddf52f93225eee6ead5f4d63d53ddcb" +dependencies = [ + "const-oid", + "pem-rfc7468", + "zeroize", +] + +[[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", + "const-oid", + "crypto-common", + "subtle", +] + +[[package]] +name = "dim_slam" +version = "0.1.0" +dependencies = [ + "cc", + "dimos-module", + "lcm-msgs", + "serde", + "tokio", + "tracing", + "validator", +] + +[[package]] +name = "dimos-lcm" +version = "0.1.0" +source = "git+https://github.com/dimensionalOS/dimos-lcm.git?branch=rust-codegen#04d78e8622500244123ba9cefa4c51b4cb454549" +dependencies = [ + "byteorder", + "socket2 0.5.10", + "tokio", +] + +[[package]] +name = "dimos-module" +version = "0.1.0" +source = "git+https://github.com/dimensionalOS/dimos.git?branch=main#5dc691323d257a4e39f38a66c7164bf02c3aa784" +dependencies = [ + "dimos-lcm", + "dimos-module-macros", + "lcm-msgs", + "nalgebra", + "serde", + "serde_json", + "tokio", + "tracing", + "tracing-subscriber", + "validator", + "zenoh", +] + +[[package]] +name = "dimos-module-macros" +version = "0.1.0" +source = "git+https://github.com/dimensionalOS/dimos.git?branch=main#5dc691323d257a4e39f38a66c7164bf02c3aa784" +dependencies = [ + "proc-macro2", + "quote", + "syn 2.0.119", +] + +[[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.3", +] + +[[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.17.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9e5e8f6c15a24b9a3ee5efec809ccd006d3b30e8b3bb63c39af737c7f87daa1d" + +[[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.9" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "843fba2746e448b37e26a819579957415c8cef339bf08564fe8b7ddbd959573c" +dependencies = [ + "crc32fast", + "miniz_oxide", +] + +[[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.3", +] + +[[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.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "7360bd2cd76e0cd9032d42cf2922155cecea2685b0cfa4630c3246df030bcfd6" + +[[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.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "fc0fef456e4baa96da950455cd02c081ca953b141298e41db3fc7e36b1da849c" + +[[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 = "http" +version = "1.5.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "918d3568bebf352712bc2ef3d46a8bcf1a75b373be6539de198e9105cbbf9ce0" +dependencies = [ + "bytes", + "itoa", +] + +[[package]] +name = "httparse" +version = "1.10.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "6dbf3de79e51f3d586ab4cb9d5c3e2c14aa28ed23d180cf89b4df0454a69cc87" + +[[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.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "92a7ed671a6aad807a8651a2e1782a6598fda9ce5185dd8158549e95a91c6428" +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.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "d466e9454f08e4a911e14806c24e16fba1b4c121d1ea474396f396069cf949d9" +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 2.0.20", + "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 = "jobserver" +version = "0.1.35" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1c00acbd29eabad4a2392fa0e921c874934dbbf4194312ad20f04a0ed67a3cb3" +dependencies = [ + "getrandom 0.4.3", + "libc", +] + +[[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" +dependencies = [ + "spin 0.9.9", +] + +[[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" +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.20" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "28d0a00925a9f930d679b6789b721e3a7f9ed110f41b86d2497caa780c3a070a" +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 = "log" +version = "0.4.33" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0ceec5bc11778974d1bcb055b18002eba7f4b3518b6a0081b3af5f21666da9ad" + +[[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.8.9" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1fa76a2c86f704bdb222d66965fb3d63269ce38518b83cb0575fca855ebb6316" +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.3", + "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", + "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-bigint-dig" +version = "0.8.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e661dda6640fad38e827a6d4a310ff4763082116fe217f279885c97f511bb0b7" +dependencies = [ + "lazy_static", + "libm", + "num-integer", + "num-iter", + "num-traits", + "rand 0.8.7", + "smallvec", + "zeroize", +] + +[[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-iter" +version = "0.1.46" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "c92800bd69a1eac91786bcfe9da64a897eb72911b8dc3095decbd07429e8048b" +dependencies = [ + "num-integer", + "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", + "libm", +] + +[[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 = "parking" +version = "2.2.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f38d5652c16fde515bb1ecef450ab0f6a219d619a7274976324d5e377f7dceba" + +[[package]] +name = "paste" +version = "1.0.15" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "57c0d7b74b563b49d38dae00a0c37d4d6de9b432382b2892f0574ddcae73fd0a" + +[[package]] +name = "pem" +version = "3.0.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1d30c53c26bc5b31a98cd02d20f25a7c8567146caf63ed593a9d87b2775291be" +dependencies = [ + "base64", + "serde_core", +] + +[[package]] +name = "pem-rfc7468" +version = "0.7.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "88b39c9bfcfc231068454382784bb460aae594343fb030d46e9f50a645418412" +dependencies = [ + "base64ct", +] + +[[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.0", + "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 = "pkcs1" +version = "0.7.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "c8ffb9f10fa047879315e6625af03c164b16962a5368d724ed16323b68ace47f" +dependencies = [ + "der", + "pkcs8", + "spki", +] + +[[package]] +name = "pkcs8" +version = "0.10.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f950b2377845cebe5cf8b5165cb3cc1a5e0fa5cfa3e1f7f55707d8fd82e0a7b7" +dependencies = [ + "der", + "spki", +] + +[[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.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b0084e6206a967a2dad822180626b2f6b07a3b379325e8f1ec0438e33a469ba7" +dependencies = [ + "proc-macro2", + "quote", +] + +[[package]] +name = "proc-macro-error3" +version = "3.1.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0cf066225f2373bc711684792b69bdeac0356019b007e721090c24d92d5d5a50" +dependencies = [ + "proc-macro-error-attr3", + "proc-macro2", + "quote", + "syn 3.0.3", +] + +[[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 = "quinn" +version = "0.11.11" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0c1a41e437b6bbd489372cd4971de128e85c855f56c57f283d20ff016cf7c0a8" +dependencies = [ + "bytes", + "cfg_aliases", + "pin-project-lite", + "quinn-proto", + "quinn-udp", + "rustc-hash", + "rustls", + "socket2 0.6.5", + "thiserror 2.0.20", + "tokio", + "tracing", + "web-time", +] + +[[package]] +name = "quinn-proto" +version = "0.11.16" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "2f4bfc015262b9df63c8845072ce59068853ff5872180c2ce2f13038b970e560" +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 2.0.20", + "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", + "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.7" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "22f6172bdec972074665ed81ed53b71da00bfc44b65a753cfde883ec4c702a1a" +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 = "rcgen" +version = "0.14.9" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "091e7a8e7d86e6feb87a27ce8e2cba29d49eff9507afeebefab7eeb2ca667fb4" +dependencies = [ + "pem", + "ring", + "rustls-pki-types", + "time", + "x509-parser", + "yasna", +] + +[[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 2.0.20", +] + +[[package]] +name = "ref-cast" +version = "1.0.26" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "216e8f773d7923bcba9ceb86a86c93cabb3903a11872fc3f138c49630e50b96d" +dependencies = [ + "ref-cast-impl", +] + +[[package]] +name = "ref-cast-impl" +version = "1.0.26" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "2c9283685feec7d69af75fb0e858d5e7378f33fe4fc699383b2916ab9273e03c" +dependencies = [ + "proc-macro2", + "quote", + "syn 3.0.3", +] + +[[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 = "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 = "rsa" +version = "0.9.10" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b8573f03f5883dcaebdfcf4725caa1ecb9c15b2ef50c43a07b816e06799bb12d" +dependencies = [ + "const-oid", + "digest", + "num-bigint-dig", + "num-integer", + "num-traits", + "pkcs1", + "pkcs8", + "rand_core 0.6.4", + "signature", + "spki", + "subtle", + "zeroize", +] + +[[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.14" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0527518605e68109d875e248ea259b6758801cf165e4b2c2733ae3b51f12535a" +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.3", +] + +[[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_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.3", +] + +[[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.3", +] + +[[package]] +name = "serde_json" +version = "1.0.151" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "c841b55ecdae098c80dcae9cf767f6f8a0c2cdb3416bbef72181df4d0fe73f14" +dependencies = [ + "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", + "bs58", + "chrono", + "hex", + "indexmap 1.9.3", + "indexmap 2.14.0", + "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.0", + "itoa", + "ryu", + "serde", + "unsafe-libyaml", +] + +[[package]] +name = "sha1" +version = "0.10.7" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a978451301f4db1d02937a4ab3ccce137717b81826e79b7d49ffe3244a13c3b8" +dependencies = [ + "cfg-if", + "cpufeatures 0.2.17", + "digest", +] + +[[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 = "signature" +version = "2.2.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "77549399552de45a898a580c1b41d445bf730df867cc44e6c0233bbc4b8329de" +dependencies = [ + "digest", + "rand_core 0.6.4", +] + +[[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 = "spki" +version = "0.7.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "d91ed6c858b01f942cd56b37a94b3e0a1798290327d1236e4d9cf4eaca44d29d" +dependencies = [ + "base64ct", + "der", +] + +[[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 = "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 = "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.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "53e9bae58849f64dfa4f5d5ae372c8341f7305f82a3868709269343628b659a3" +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 = "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", +] + +[[package]] +name = "thiserror-impl" +version = "2.0.20" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "bc04cd3e1236dd4a98afca4569f2deb3f120e5422a4023be2cb683f8486292af" +dependencies = [ + "proc-macro2", + "quote", + "syn 3.0.3", +] + +[[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 = "tls-listener" +version = "0.11.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1461056cc1ef47003f7ee16e4cef3741068d4c7f6b627bfce49b7c00c120a530" +dependencies = [ + "futures-util", + "pin-project-lite", + "thiserror 2.0.20", + "tokio", + "tokio-rustls", +] + +[[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.3", +] + +[[package]] +name = "tokio-rustls" +version = "0.26.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1729aa945f29d91ba541258c8df89027d5792d85a8841fb65e8bf0f4ede4ef61" +dependencies = [ + "rustls", + "tokio", +] + +[[package]] +name = "tokio-tungstenite" +version = "0.24.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "edc5f74e248dc973e0dbb7b74c7e0d6fcc301c694ff50049504004ef4d0cdcd9" +dependencies = [ + "futures-util", + "log", + "tokio", + "tungstenite", +] + +[[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.0", + "serde_core", + "serde_spanned", + "toml_datetime 0.7.5+spec-1.1.0", + "toml_parser", + "toml_writer", + "winnow 0.7.15", +] + +[[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.0", + "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 = "tungstenite" +version = "0.24.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "18e5b8366ee7a95b16d32197d0b2604b43a0be89dc5fac9f8e96ccafbaedda8a" +dependencies = [ + "byteorder", + "bytes", + "data-encoding", + "http", + "httparse", + "log", + "rand 0.8.7", + "sha1", + "thiserror 1.0.69", + "utf-8", +] + +[[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.7", + "serde", + "spin 0.10.1", +] + +[[package]] +name = "unicode-ident" +version = "1.0.24" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e6e4313cd5fcd3dad5cafa179702e2b244f760991f45397d14d4ebf38247da75" + +[[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 = "utf-8" +version = "0.7.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "09cc8ee72d2a9becf2f2febe0205bbed8fc6615b7cb429ad062dc7b7ddd036a9" + +[[package]] +name = "utf8_iter" +version = "1.0.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b6c140620e7ffbb22c2dee59cafe6084a59b5ffc27a8859a5f0d494b5d52b6be" + +[[package]] +name = "uuid" +version = "1.24.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "2cefc03fd367c0c6d4305de1b312cf00248c4114f4a0418ce6a6af769e3b0bd9" +dependencies = [ + "getrandom 0.4.3", + "js-sys", + "wasm-bindgen", +] + +[[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.20.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "43fb22e1a008ece370ce08a3e9e4447a910e92621bb49b85d6e48a45397e7cfa" +dependencies = [ + "idna", + "once_cell", + "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.6.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "de2aaf408e58689c2096682331b1f42bb2d9f2ed6b11560407d023cd0a6c634e" +dependencies = [ + "bytemuck", + "safe_arch", +] + +[[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-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", + "windows_i686_gnu", + "windows_i686_gnullvm", + "windows_i686_msvc", + "windows_x86_64_gnu", + "windows_x86_64_gnullvm", + "windows_x86_64_msvc", +] + +[[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.52.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "09ec2a7bb152e2252b53fa7803150007879548bc709c039df7627cabbd05d469" + +[[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.52.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "240948bc05c5e7c6dabba28bf89d89ffce3e303022809e73deaefe4f6ec56c66" + +[[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.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 2.0.20", + "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.7", + "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-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.7", + "tracing", + "uhlc", + "zenoh-buffers", + "zenoh-protocol", +] + +[[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", + "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.7", + "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.7", + "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-quic", + "zenoh-link-quic_datagram", + "zenoh-link-tcp", + "zenoh-link-tls", + "zenoh-link-udp", + "zenoh-link-unixsock_stream", + "zenoh-link-ws", + "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", + "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" +version = "1.10.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1038b5c97aeca14bbba31d6bed629bd2d354488850c27eddde07726e90ce9a48" +dependencies = [ + "async-trait", + "rustls-webpki", + "time", + "tracing", + "zenoh-core", + "zenoh-link-commons", + "zenoh-link-quic_datagram", + "zenoh-protocol", + "zenoh-result", +] + +[[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-tls" +version = "1.10.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a123220c7aedf285cfeaaaf2f05d88787a164a92fa5c64c09156b861feb16134" +dependencies = [ + "async-trait", + "base64", + "rustls", + "rustls-pemfile", + "rustls-pki-types", + "rustls-webpki", + "secrecy", + "socket2 0.5.10", + "time", + "tls-listener", + "tokio", + "tokio-rustls", + "tokio-util", + "tracing", + "webpki-roots", + "x509-parser", + "zenoh-config", + "zenoh-core", + "zenoh-link-commons", + "zenoh-protocol", + "zenoh-result", + "zenoh-runtime", +] + +[[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-link-unixsock_stream" +version = "1.10.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e000c7d101a3d2e5e3350a19ead2f7914276583d9d3a5787fde005465a1271d7" +dependencies = [ + "async-trait", + "nix", + "tokio", + "tokio-util", + "tracing", + "uuid", + "zenoh-core", + "zenoh-link-commons", + "zenoh-protocol", + "zenoh-result", + "zenoh-runtime", +] + +[[package]] +name = "zenoh-link-ws" +version = "1.10.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "be8a5dc8ab07d0ccb46b187f58b9ee5e38a1cacaaf780ad38398f6c2669dfb53" +dependencies = [ + "async-trait", + "futures-util", + "tokio", + "tokio-tungstenite", + "tokio-util", + "tracing", + "url", + "zenoh-core", + "zenoh-link-commons", + "zenoh-protocol", + "zenoh-result", + "zenoh-runtime", + "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.7", + "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-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", + "lz4_flex", + "rand 0.8.7", + "ringbuffer-spsc", + "rsa", + "serde", + "sha3", + "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-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.7" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "94b5c6b5976d66c1d703c4fd17d3f5e43c8cedaacf604961b171adc7130896d8" +dependencies = [ + "yoke", + "zerofrom", + "zerovec-derive", +] + +[[package]] +name = "zerovec-derive" +version = "0.11.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "47402523226a02bfe5230160dc3ccc089aa6f6f19e7fcbb4e6f824bbb1b4aa62" +dependencies = [ + "proc-macro2", + "quote", + "syn 3.0.3", +] + +[[package]] +name = "zmij" +version = "1.0.23" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "29666d0abbfad1e3dc4dcf6144730dd3a3ab225bbbdac83319345b1b44ccfc1b" diff --git a/rust/Cargo.toml b/rust/Cargo.toml new file mode 100644 index 0000000..20f30cc --- /dev/null +++ b/rust/Cargo.toml @@ -0,0 +1,23 @@ +# Copyright 2026 Dimensional Inc. +# SPDX-License-Identifier: Apache-2.0 + +[package] +name = "dim_slam" +version = "0.1.0" +edition = "2021" +license = "Apache-2.0" + +[dependencies] +dimos-module = { git = "https://github.com/dimensionalOS/dimos.git", branch = "main" } +lcm-msgs = { git = "https://github.com/dimensionalOS/dimos-lcm.git", branch = "rust-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. +serde = { version = "1", features = ["derive"] } +validator = { version = "0.20", features = ["derive"] } + +[build-dependencies] +cc = { version = "1", features = ["parallel"] } + +[profile.release] +lto = true diff --git a/rust/build.rs b/rust/build.rs new file mode 100644 index 0000000..371b2b6 --- /dev/null +++ b/rust/build.rs @@ -0,0 +1,25 @@ +use std::env; +use std::path::PathBuf; + +fn main() { + println!("cargo:rerun-if-changed=shim/cuvslam_shim.h"); + println!("cargo:rerun-if-changed=shim/cuvslam_shim.cpp"); + println!("cargo:rerun-if-env-changed=CUVSLAM_SDK_DIR"); + + let sdk_dir = PathBuf::from( + env::var("CUVSLAM_SDK_DIR").expect("set CUVSLAM_SDK_DIR to a cuVSLAM SDK (include/cuvslam/cuvslam2.h + lib/)"), + ); + + cc::Build::new() + .cpp(true) + .std("c++20") + .file("shim/cuvslam_shim.cpp") + .include(sdk_dir.join("include")) + .compile("cuvslam_shim"); + + let lib_dir = sdk_dir.join("lib"); + println!("cargo:rustc-link-search=native={}", lib_dir.display()); + println!("cargo:rustc-link-lib=dylib=cuvslam"); + // libcuvslam's install name is @rpath/libcuvslam.dylib on macOS; embed the SDK path. + println!("cargo:rustc-link-arg=-Wl,-rpath,{}", lib_dir.display()); +} diff --git a/rust/shim/cuvslam_shim.cpp b/rust/shim/cuvslam_shim.cpp new file mode 100644 index 0000000..d46ca9d --- /dev/null +++ b/rust/shim/cuvslam_shim.cpp @@ -0,0 +1,155 @@ +#include "cuvslam_shim.h" + +#include + +#include +#include +#include +#include +#include + +namespace { + +void write_error(char* error_message, int32_t capacity, const char* text) { + if (error_message == nullptr || capacity <= 0) { + return; + } + std::snprintf(error_message, static_cast(capacity), "%s", text); +} + +cuvslam::Pose to_cuvslam_pose(const CuvPose& pose) { + cuvslam::Pose result; + std::copy(pose.rotation_xyzw, pose.rotation_xyzw + 4, result.rotation.begin()); + std::copy(pose.translation, pose.translation + 3, result.translation.begin()); + return result; +} + +cuvslam::Image to_cuvslam_image(const CuvImage& image) { + cuvslam::Image result{}; + result.pixels = image.pixels; + result.width = image.width; + result.height = image.height; + result.pitch = 0; + result.encoding = static_cast(image.encoding); + result.data_type = static_cast(image.data_type); + result.is_gpu_mem = false; + result.timestamp_ns = image.timestamp_ns; + result.camera_index = image.camera_index; + return result; +} + +} // namespace + +struct CuvTracker { + cuvslam::Odometry odometry; +}; + +extern "C" { + +int32_t cuv_tracker_create(const CuvCamera* cameras, int32_t camera_count, const CuvImuCalibration* imu_or_null, + const CuvConfig* config, CuvTracker** out_tracker, char* error_message, + int32_t error_message_capacity) { + try { + cuvslam::Rig rig; + for (int32_t index = 0; index < camera_count; index++) { + const CuvCamera& source = cameras[index]; + cuvslam::Camera camera; + camera.size = {source.width, source.height}; + camera.principal = {source.principal[0], source.principal[1]}; + camera.focal = {source.focal[0], source.focal[1]}; + camera.rig_from_camera = to_cuvslam_pose(source.rig_from_camera); + camera.distortion.model = static_cast(source.distortion_model); + camera.distortion.parameters.assign(source.distortion_parameters, + source.distortion_parameters + source.distortion_parameter_count); + rig.cameras.push_back(camera); + } + if (imu_or_null != nullptr) { + cuvslam::ImuCalibration imu; + imu.rig_from_imu = to_cuvslam_pose(imu_or_null->rig_from_imu); + imu.gyroscope_noise_density = imu_or_null->gyroscope_noise_density; + imu.gyroscope_random_walk = imu_or_null->gyroscope_random_walk; + imu.accelerometer_noise_density = imu_or_null->accelerometer_noise_density; + imu.accelerometer_random_walk = imu_or_null->accelerometer_random_walk; + imu.frequency = imu_or_null->frequency; + rig.imus.push_back(imu); + } + + cuvslam::Odometry::Config cuvslam_config = cuvslam::Odometry::GetDefaultConfig(); + cuvslam_config.odometry_mode = static_cast(config->odometry_mode); + cuvslam_config.use_gpu = config->use_gpu; + // Async SBA throws from cuVSLAM's own thread on failure, which would crash the process. + cuvslam_config.async_sba = false; + cuvslam_config.rectified_stereo_camera = config->rectified_stereo_camera; + cuvslam_config.rgbd_settings.depth_scale_factor = config->rgbd_depth_scale_factor; + cuvslam_config.rgbd_settings.depth_camera_id = config->rgbd_depth_camera_id; + + *out_tracker = new CuvTracker{cuvslam::Odometry(rig, cuvslam_config)}; + return 0; + } catch (const std::exception& error) { + write_error(error_message, error_message_capacity, error.what()); + return 1; + } catch (...) { + write_error(error_message, error_message_capacity, "unknown C++ exception"); + return 1; + } +} + +int32_t cuv_tracker_track(CuvTracker* tracker, const CuvImage* images, int32_t image_count, const CuvImage* depths, + int32_t depth_count, CuvPoseEstimate* out_estimate, char* error_message, + int32_t error_message_capacity) { + try { + cuvslam::Odometry::ImageSet image_set; + for (int32_t index = 0; index < image_count; index++) { + image_set.push_back(to_cuvslam_image(images[index])); + } + cuvslam::Odometry::ImageSet depth_set; + for (int32_t index = 0; index < depth_count; index++) { + depth_set.push_back(to_cuvslam_image(depths[index])); + } + + const cuvslam::PoseEstimate estimate = tracker->odometry.Track(image_set, {}, depth_set); + + std::memset(out_estimate, 0, sizeof(*out_estimate)); + out_estimate->timestamp_ns = estimate.timestamp_ns; + out_estimate->has_pose = estimate.world_from_rig.has_value(); + if (estimate.world_from_rig.has_value()) { + const cuvslam::PoseWithCovariance& pose_with_covariance = *estimate.world_from_rig; + std::copy(pose_with_covariance.pose.rotation.begin(), pose_with_covariance.pose.rotation.end(), + out_estimate->world_from_rig.rotation_xyzw); + std::copy(pose_with_covariance.pose.translation.begin(), pose_with_covariance.pose.translation.end(), + out_estimate->world_from_rig.translation); + std::copy(pose_with_covariance.covariance_xyz_rpy.begin(), pose_with_covariance.covariance_xyz_rpy.end(), + out_estimate->covariance_xyz_rpy); + } + return 0; + } catch (const std::exception& error) { + write_error(error_message, error_message_capacity, error.what()); + return 1; + } catch (...) { + write_error(error_message, error_message_capacity, "unknown C++ exception"); + return 1; + } +} + +int32_t cuv_tracker_register_imu(CuvTracker* tracker, const CuvImuMeasurement* measurement, char* error_message, + int32_t error_message_capacity) { + try { + cuvslam::ImuMeasurement imu{}; + imu.timestamp_ns = measurement->timestamp_ns; + std::copy(measurement->linear_accelerations, measurement->linear_accelerations + 3, + imu.linear_accelerations.begin()); + std::copy(measurement->angular_velocities, measurement->angular_velocities + 3, + imu.angular_velocities.begin()); + tracker->odometry.RegisterImuMeasurement(0, imu); + return 0; + } catch (const std::exception& error) { + write_error(error_message, error_message_capacity, error.what()); + return 1; + } catch (...) { + write_error(error_message, error_message_capacity, "unknown C++ exception"); + return 1; + } +} + +void cuv_tracker_destroy(CuvTracker* tracker) { delete tracker; } +} diff --git a/rust/shim/cuvslam_shim.h b/rust/shim/cuvslam_shim.h new file mode 100644 index 0000000..871068a --- /dev/null +++ b/rust/shim/cuvslam_shim.h @@ -0,0 +1,115 @@ +// Thin extern-"C" shim over the cuVSLAM C++ API (cuvslam2.h). +// Only wraps the surface used by the odometry module: construct, Track, RegisterImuMeasurement. +#pragma once + +#include +#include + +#ifdef __cplusplus +extern "C" { +#endif + +typedef struct CuvTracker CuvTracker; + +typedef struct { + float rotation_xyzw[4]; + float translation[3]; +} CuvPose; + +// distortion_model values match cuvslam::Distortion::Model +enum { + CUV_DISTORTION_PINHOLE = 0, + CUV_DISTORTION_FISHEYE = 1, + CUV_DISTORTION_BROWN = 2, + CUV_DISTORTION_POLYNOMIAL = 3, +}; + +typedef struct { + int32_t width; + int32_t height; + float principal[2]; + float focal[2]; + CuvPose rig_from_camera; + uint8_t distortion_model; + const float* distortion_parameters; + int32_t distortion_parameter_count; +} CuvCamera; + +typedef struct { + CuvPose rig_from_imu; + float gyroscope_noise_density; + float gyroscope_random_walk; + float accelerometer_noise_density; + float accelerometer_random_walk; + float frequency; +} CuvImuCalibration; + +// odometry_mode values match cuvslam::Odometry::OdometryMode +enum { + CUV_ODOMETRY_MULTICAMERA = 0, + CUV_ODOMETRY_INERTIAL = 1, + CUV_ODOMETRY_RGBD = 2, + CUV_ODOMETRY_MONO = 3, +}; + +typedef struct { + uint8_t odometry_mode; + bool use_gpu; + bool rectified_stereo_camera; + float rgbd_depth_scale_factor; + int32_t rgbd_depth_camera_id; +} CuvConfig; + +// encoding values match cuvslam::ImageData::Encoding, data_type matches DataType +enum { + CUV_ENCODING_MONO = 0, + CUV_ENCODING_RGB = 1, +}; +enum { + CUV_DATA_UINT8 = 0, + CUV_DATA_UINT16 = 1, + CUV_DATA_FLOAT32 = 2, +}; + +typedef struct { + const void* pixels; + int32_t width; + int32_t height; + uint8_t encoding; + uint8_t data_type; + int64_t timestamp_ns; + uint32_t camera_index; +} CuvImage; + +typedef struct { + int64_t timestamp_ns; + float linear_accelerations[3]; + float angular_velocities[3]; +} CuvImuMeasurement; + +typedef struct { + int64_t timestamp_ns; + bool has_pose; + CuvPose world_from_rig; + double covariance_xyz_rpy[36]; // row-major 6x6, valid only when has_pose +} CuvPoseEstimate; + +// All functions return 0 on success. On failure they return nonzero and write a +// NUL-terminated message into error_message (truncated to error_message_capacity). + +int32_t cuv_tracker_create(const CuvCamera* cameras, int32_t camera_count, const CuvImuCalibration* imu_or_null, + const CuvConfig* config, CuvTracker** out_tracker, char* error_message, + int32_t error_message_capacity); + +int32_t cuv_tracker_track(CuvTracker* tracker, const CuvImage* images, int32_t image_count, const CuvImage* depths, + int32_t depth_count, CuvPoseEstimate* out_estimate, char* error_message, + int32_t error_message_capacity); + +int32_t cuv_tracker_register_imu(CuvTracker* tracker, const CuvImuMeasurement* measurement, char* error_message, + int32_t error_message_capacity); + +void cuv_tracker_destroy(CuvTracker* tracker); + +#ifdef __cplusplus +} +#endif diff --git a/rust/src/cuvslam_odometry.rs b/rust/src/cuvslam_odometry.rs new file mode 100644 index 0000000..a9c8fc4 --- /dev/null +++ b/rust/src/cuvslam_odometry.rs @@ -0,0 +1,887 @@ +// Copyright 2026 Dimensional Inc. +// SPDX-License-Identifier: Apache-2.0 +// +// NVIDIA cuVSLAM visual odometry, Rust port of src/cuvslam_main.cpp. cuVSLAM itself +// stays C++; shim/cuvslam_shim.cpp wraps the three calls used (construct, Track, +// RegisterImuMeasurement) behind extern "C". +// +// Nothing is emitted while tracking is lost. cuVSLAM keeps one world frame for the +// life of the tracker, so it resumes in the same frame. The rig comes from the tf tree. + +mod depth_cloud; +mod depth_reproject; +mod ffi; +pub mod imu_info; +mod msg_convert; +mod tracker; + +use std::collections::HashMap; +use std::time::Duration; + +use dimos_module::nalgebra::{Isometry3, Matrix3, Matrix6, Vector3}; +use dimos_module::{error_throttled, native_config, warn_throttled, Tf}; +use lcm_msgs::nav_msgs::Odometry; +use lcm_msgs::sensor_msgs::{CameraInfo, Image, Imu, PointCloud2}; +use tracing::{error, info, warn}; + +use self::depth_cloud::depth_cloud; +use self::depth_reproject::reproject_depth; +use self::imu_info::ImuInfo; +use self::msg_convert::{ + cuv_pose_to_isometry, stamp_to_ns, to_cuv_pose, to_distortion, to_stamp, transform_to_isometry, +}; +use self::tracker::{CameraParams, ImageRef, Tracker}; + +/// cuVSLAM's Track() contract asks for stereo stamps within 1 ms. +const MAX_PAIR_SKEW_NS: i64 = 1_000_000; + +#[derive(Clone, Copy, PartialEq)] +enum Mode { + Stereo, + Mono, + Rgbd, +} + +/// SE(3) adjoint on (xyz, rpy) coordinates: how a small motion expressed in the child +/// frame reads in the parent frame. First order in the rotation block, where fixed-axis +/// rpy rates and rotation-vector components coincide. +fn se3_adjoint(parent_from_child: &Isometry3) -> Matrix6 { + let rotation = parent_from_child.rotation.to_rotation_matrix().into_inner(); + let translation = parent_from_child.translation.vector; + let translation_skew = Matrix3::new( + 0.0, + -translation.z, + translation.y, + translation.z, + 0.0, + -translation.x, + -translation.y, + translation.x, + 0.0, + ); + let mut adjoint = Matrix6::zeros(); + adjoint.fixed_view_mut::<3, 3>(0, 0).copy_from(&rotation); + adjoint + .fixed_view_mut::<3, 3>(0, 3) + .copy_from(&(translation_skew * rotation)); + adjoint.fixed_view_mut::<3, 3>(3, 3).copy_from(&rotation); + adjoint +} + +/// cuVSLAM wants camera 0 to be the left of a pair, which is the one whose +/// partner sits at +x (optical convention, x to the right). +fn needs_left_swap(first_rig_from_camera: &Isometry3, second_rig_from_camera: &Isometry3) -> bool { + (first_rig_from_camera.inverse() * second_rig_from_camera).translation.x < 0.0 +} + +fn odometry_mode(mode: Mode, enable_imu: bool) -> u8 { + match mode { + Mode::Rgbd => ffi::CUV_ODOMETRY_RGBD, + Mode::Mono => ffi::CUV_ODOMETRY_MONO, + // Inertial is the stereo pair plus an IMU; there is no inertial mono or rgbd. + Mode::Stereo => { + if enable_imu { + ffi::CUV_ODOMETRY_INERTIAL + } else { + ffi::CUV_ODOMETRY_MULTICAMERA + } + } + } +} + +/// A three-channel image has to be declared as such: fed as MONO, cuVSLAM +/// reads a third of each row and tracks nothing. +fn image_encoding(encoding: &str) -> u8 { + if encoding == "mono8" { + ffi::CUV_ENCODING_MONO + } else { + ffi::CUV_ENCODING_RGB + } +} + +#[native_config] +#[derive(Clone)] +pub struct CuvslamOdometryConfig { + /// "stereo", "mono" or "rgbd". Mono is accurate only up to scale. + pub camera_mode: String, + /// One tf frame per camera, in cuVSLAM's index order. Empty discovers them off + /// camera_info. + pub camera_frames: Vec, + /// Images arrive rectified: no distortion, rows aligned. + pub rectified: bool, + /// Off runs the tracker on the CPU. Needs a libcuvslam built with ENFORCE_GPU=OFF + /// (the jeff-hykin/cuVSLAM fork build); NVIDIA's stock SDK binaries are GPU-only. + pub use_gpu: bool, + /// Frame stamped on the emitted odometry; the tracker's world, drifting freely. + pub odom_frame: String, + pub base_frame: String, + /// Frame the cuVSLAM rig is expressed in. Empty means base_frame. Setting it to a camera's + /// optical frame reproduces NVIDIA's examples, whose rig IS the left camera; the emitted + /// odometry stays on base_frame either way, since the two differ by a fixed transform. + pub rig_frame: String, + /// Rebase guard. A frame whose translation standard deviation (root of the largest + /// translation term of cuVSLAM's covariance) exceeds this is unconstrained: its motion + /// is dropped, the pose holds, and later frames are rebased onto the held pose so the + /// published path never carries the teleport. Meters; 0 disables the guard and the raw + /// integrator is published untouched. + pub covariance_gate_translation_std: f64, + /// Rebase guard on physically implausible frame-to-frame motion, sharing the + /// covariance gate's hold-and-rebase machinery but trusting kinematics instead of the + /// tracker's self-report: a teleport with confident covariance still cannot claim the + /// rig moved faster than the platform can. Linear is metres/second, angular is + /// radians/second, both measured on the raw pose against the previous tracked frame; + /// 0 disables that limit. + pub speed_gate_max_linear: f64, + pub speed_gate_max_angular: f64, + /// cuVSLAM's Inertial mode is stereo plus one IMU. The noise model and frame come + /// from the imu_info stream, published by the driver the way camera_info is. + pub enable_imu: bool, + /// rgbd only: raw depth units per metre. 1000 for sixteen-bit millimetres. + pub depth_units_per_meter: f64, + /// Range gate on the published depth_cloud, metres. Stereo depth error grows as range + /// squared, so the far gate is what decides whether the cloud is worth mapping with; + /// 0 leaves it open. + pub depth_cloud_min_range: f64, + pub depth_cloud_max_range: f64, +} + +struct RigCamera { + frame: String, + rig_from_camera: Isometry3, + info: CameraInfo, + image: Option, +} + +enum DepthChoice { + /// Depth already recorded against the rig camera. + Passthrough, + /// Reprojected into `aligned_depth`. + Aligned, +} + +/// The tracker pipeline, free of any transport; the module in main.rs drives it. +pub struct VoCore { + config: CuvslamOdometryConfig, + /// The rig, in cuVSLAM's camera order. + cameras: Vec, + camera_info_by_frame: HashMap, + imu_model: Option, + /// Buffered. cuVSLAM requires Track() and RegisterImuMeasurement() in + /// non-decreasing timestamp order; the round-robin dispatcher lets images + /// overtake a 400 Hz IMU. + pending_imu: Vec, + vslam: Option, + + depth: Option, + depth_info: Option, + aligned_depth: Image, + camera_from_depth: Option>, + last_ts_ns: Option, + + // tracking state; identity transforms are set in ensure_tracker before first use + /// last published pose, on base_frame + world_from_base: Option>, + base_from_rig: Option>, + rig_from_base: Option>, + /// Adjoint of base_from_rig, fixed with it at tracker creation. + covariance_adjoint: Matrix6, + /// identity until the gate first fires + rebase: Option>, + covariance: Matrix6, + previous_raw: Option>, + previous_raw_ns: i64, + was_tracking: bool, + + frames: u64, + tracked: u64, + segment_id: u64, + imu_samples: u64, + imu_dropped: u64, + skew_rejects: u64, + depth_reprojected: u64, + covariance_gated: u64, + speed_gated: u64, + unplaced_images: u64, +} + +impl VoCore { + pub fn new(config: CuvslamOdometryConfig) -> Self { + Self { + config, + cameras: Vec::new(), + camera_info_by_frame: HashMap::new(), + imu_model: None, + pending_imu: Vec::new(), + vslam: None, + depth: None, + depth_info: None, + aligned_depth: Image::default(), + camera_from_depth: None, + last_ts_ns: None, + world_from_base: None, + base_from_rig: None, + rig_from_base: None, + covariance_adjoint: Matrix6::zeros(), + rebase: None, + covariance: Matrix6::zeros(), + previous_raw: None, + previous_raw_ns: 0, + was_tracking: false, + frames: 0, + tracked: 0, + segment_id: 0, + imu_samples: 0, + imu_dropped: 0, + skew_rejects: 0, + depth_reprojected: 0, + covariance_gated: 0, + speed_gated: 0, + unplaced_images: 0, + } + } + + fn mode(&self) -> Mode { + match self.config.camera_mode.as_str() { + "stereo" => Mode::Stereo, + "rgbd" => Mode::Rgbd, + _ => Mode::Mono, + } + } + + /// The frame cuVSLAM's rig is expressed in, which need not be the published frame. + fn rig_frame(&self) -> &str { + if self.config.rig_frame.is_empty() { + &self.config.base_frame + } else { + &self.config.rig_frame + } + } + + pub fn handle_camera_info(&mut self, info: CameraInfo, tf: &Tf) { + if self.vslam.is_some() { + return; // rig is fixed once the tracker exists + } + self.camera_info_by_frame.insert(info.header.frame_id.clone(), info); + self.resolve_rig(tf); + } + + /// Place every camera against the rig frame, or nothing until they all resolve. + /// + /// The C++ module retried this on every tf message; here tf intake belongs to the + /// SDK, so the retry rides on camera_info and image arrivals instead. + fn resolve_rig(&mut self, tf: &Tf) { + if !self.cameras.is_empty() { + return; + } + let discovered = self.config.camera_frames.is_empty(); + let mut frames = self.config.camera_frames.clone(); + if discovered { + frames.extend(self.camera_info_by_frame.keys().cloned()); + // camera_info has no order of its own, and the rig is indexed. + frames.sort(); + let expected = if self.mode() == Mode::Stereo { 2 } else { 1 }; + if frames.len() != expected { + if frames.len() > expected { + error_throttled!( + Duration::from_secs(10), + found = frames.join(", "), + mode = %self.config.camera_mode, + "cuvslam found more cameras on camera_info than camera_mode uses, \ + and they have no discoverable order. Set camera_frames.", + ); + } + return; + } + } + + let mut cameras = Vec::new(); + for frame in &frames { + let Some(rig_from_camera) = tf.get_latest(self.rig_frame(), frame) else { + return; + }; + let Some(info) = self.camera_info_by_frame.get(frame) else { + return; + }; + cameras.push(RigCamera { + frame: frame.clone(), + rig_from_camera: transform_to_isometry(&rig_from_camera), + info: info.clone(), + image: None, + }); + } + if discovered + && cameras.len() == 2 + && needs_left_swap(&cameras[0].rig_from_camera, &cameras[1].rig_from_camera) + { + cameras.swap(0, 1); + } + self.cameras = cameras; + } + + /// Which camera in the rig a frame_id names. + fn camera_index(&self, frame_id: &str) -> Option { + self.cameras.iter().position(|camera| camera.frame == frame_id) + } + + pub fn handle_imu_info(&mut self, info: ImuInfo, tf: &Tf) { + if self.vslam.is_some() { + return; // rig is fixed once the tracker exists + } + self.imu_model = Some(info); + self.resolve_rig(tf); + } + + pub fn handle_imu(&mut self, msg: &Imu) { + if self.vslam.is_none() { + // No tracker yet; this is the window inertial init needs. + self.imu_dropped += 1; + return; + } + // Track() has already consumed everything up to last_ts_ns. + self.pending_imu.push(ffi::CuvImuMeasurement { + timestamp_ns: stamp_to_ns(&msg.header), + linear_accelerations: [ + msg.linear_acceleration.x as f32, + msg.linear_acceleration.y as f32, + msg.linear_acceleration.z as f32, + ], + angular_velocities: [ + msg.angular_velocity.x as f32, + msg.angular_velocity.y as f32, + msg.angular_velocity.z as f32, + ], + }); + self.imu_samples += 1; + } + + pub fn handle_image(&mut self, img: Image, tf: &Tf) -> Option { + if self.cameras.is_empty() { + self.resolve_rig(tf); + } + let Some(index) = self.camera_index(&img.header.frame_id) else { + self.unplaced_images += 1; + warn_throttled!( + Duration::from_secs(10), + frame_id = %img.header.frame_id, + dropped = self.unplaced_images, + rig_cameras = self.cameras.len(), + "cuvslam dropping image with a frame_id not on the rig", + ); + return None; + }; + self.cameras[index].image = Some(img); + self.try_track(tf) + } + + pub fn handle_depth_image( + &mut self, + img: Image, + tf: &Tf, + ) -> (Option, Option) { + let cloud = self.depth_cloud_msg(&img); + self.depth = Some(img); + (cloud, self.try_track(tf)) + } + + /// The depth sensor's own points, gated by range, in the sensor's frame. A driver's + /// cloud carries every far, noisy pixel, and the tracker already holds the intrinsics and + /// the depth scale a clean one needs. + fn depth_cloud_msg(&self, depth: &Image) -> Option { + let info = self.depth_info.as_ref().or_else(|| { + self.cameras + .iter() + .find(|camera| camera.frame == depth.header.frame_id) + .map(|camera| &camera.info) + })?; + let mut cloud = PointCloud2::default(); + depth_cloud( + depth, + info, + self.config.depth_units_per_meter, + self.config.depth_cloud_min_range, + self.config.depth_cloud_max_range, + &mut cloud, + ); + Some(cloud) + } + + pub fn handle_depth_camera_info(&mut self, info: CameraInfo) { + if self.depth_info.is_none() { + self.depth_info = Some(info); + } + } + + /// Depth pixel-aligned with the rig camera, as cuVSLAM's RGBD contract requires. + /// Passthrough when it was recorded against that camera; reprojected through the depth + /// intrinsics and the tf between the two sensors when it was not (a D455 records depth + /// against the left IR camera, not the color camera). None while the pieces to + /// reproject are still missing. + fn align_depth(&mut self, tf: &Tf) -> Option { + let depth = self.depth.as_ref().expect("checked by try_track"); + let camera = &self.cameras[0]; + if depth.header.frame_id == camera.frame { + return Some(DepthChoice::Passthrough); + } + let Some(depth_info) = self.depth_info.as_ref() else { + warn_throttled!( + Duration::from_secs(10), + depth_frame = %depth.header.frame_id, + camera_frame = %camera.frame, + "cuvslam: depth is in another frame than the camera and needs \ + depth_camera_info to reproject", + ); + return None; + }; + if self.camera_from_depth.is_none() { + let Some(camera_from_depth) = tf.get_latest(&camera.frame, &depth.header.frame_id) else { + warn_throttled!( + Duration::from_secs(10), + depth_frame = %depth.header.frame_id, + camera_frame = %camera.frame, + "cuvslam: tf does not connect the depth frame to the camera", + ); + return None; + }; + self.camera_from_depth = Some(transform_to_isometry(&camera_from_depth)); + info!( + depth_frame = %depth.header.frame_id, + camera_frame = %camera.frame, + "cuvslam reprojecting depth onto the rig camera" + ); + } + let mut aligned = std::mem::take(&mut self.aligned_depth); + reproject_depth( + depth, + depth_info, + &camera.info, + self.camera_from_depth.as_ref().expect("set above"), + self.config.depth_units_per_meter, + &mut aligned, + ); + self.aligned_depth = aligned; + self.depth_reprojected += 1; + Some(DepthChoice::Aligned) + } + + /// Builds the tracker against rig_frame(). Poses are converted back onto base_frame on + /// publish, so rig_frame is an internal choice with no effect on the output contract. + fn ensure_tracker(&mut self, tf: &Tf) { + if self.vslam.is_some() { + return; + } + let Some(base_from_rig) = tf.get_latest(&self.config.base_frame, self.rig_frame()) else { + warn_throttled!( + Duration::from_secs(10), + rig_frame = self.rig_frame(), + base_frame = %self.config.base_frame, + "cuvslam: tf does not place the rig frame against base_frame", + ); + return; + }; + let base_from_rig = transform_to_isometry(&base_from_rig); + self.base_from_rig = Some(base_from_rig); + self.rig_from_base = Some(base_from_rig.inverse()); + self.covariance_adjoint = se3_adjoint(&base_from_rig); + + let cameras: Vec = self + .cameras + .iter() + .map(|rig_camera| { + let info = &rig_camera.info; + let (distortion_model, distortion_parameters) = to_distortion(info); + CameraParams { + width: info.width, + height: info.height, + principal: [info.K[2] as f32, info.K[5] as f32], + focal: [info.K[0] as f32, info.K[4] as f32], + rig_from_camera: to_cuv_pose(&rig_camera.rig_from_camera), + distortion_model, + distortion_parameters, + } + }) + .collect(); + + let mut imu_calibration = None; + if self.config.enable_imu { + let Some(imu_model) = &self.imu_model else { + warn_throttled!( + Duration::from_secs(10), + "cuvslam: enable_imu is on but no imu_info has arrived", + ); + return; + }; + let imu_frame = &imu_model.header.frame_id; + let Some(rig_from_imu) = tf.get_latest(self.rig_frame(), imu_frame) else { + warn_throttled!( + Duration::from_secs(10), + imu_frame = %imu_frame, + "cuvslam: enable_imu is on but tf does not place the IMU", + ); + return; + }; + imu_calibration = Some(ffi::CuvImuCalibration { + rig_from_imu: to_cuv_pose(&transform_to_isometry(&rig_from_imu)), + gyroscope_noise_density: imu_model.gyro_noise_density as f32, + gyroscope_random_walk: imu_model.gyro_random_walk as f32, + accelerometer_noise_density: imu_model.accel_noise_density as f32, + accelerometer_random_walk: imu_model.accel_random_walk as f32, + frequency: imu_model.frequency as f32, + }); + } + + let mode = self.mode(); + let mut tracker_config = ffi::CuvConfig { + odometry_mode: odometry_mode(mode, self.config.enable_imu), + use_gpu: self.config.use_gpu, + // cuVSLAM rejects this outright unless the rig has a stereo pair: "Rectified + // stereo camera mode only works with 1+ stereo cameras". + rectified_stereo_camera: self.config.rectified && mode == Mode::Stereo, + rgbd_depth_scale_factor: 1.0, + rgbd_depth_camera_id: -1, + }; + if mode == Mode::Rgbd { + tracker_config.rgbd_depth_scale_factor = self.config.depth_units_per_meter as f32; + // The default of -1 means no camera, and depth belonging to nothing is silently + // ignored. A depth stream usually reports its own frame, so unrecognised means 0. + let depth_frame = self.depth.as_ref().map(|depth| depth.header.frame_id.clone()); + tracker_config.rgbd_depth_camera_id = depth_frame + .and_then(|frame| self.camera_index(&frame)) + .map_or(0, |index| index as i32); + } + + // The C++ module let a construction failure abort the process; here it is logged + // and retried on the next frame set instead. + match Tracker::new(&cameras, imu_calibration.as_ref(), &tracker_config) { + Ok(vslam) => { + self.vslam = Some(vslam); + info!( + cameras = cameras.len(), + width = cameras[0].width, + height = cameras[0].height, + rig_frames = self + .cameras + .iter() + .map(|camera| camera.frame.as_str()) + .collect::>() + .join(", "), + "cuvslam tracker created" + ); + } + Err(message) => { + error_throttled!(Duration::from_secs(10), error = %message, "cuvslam tracker construction failed"); + } + } + } + + fn clear_frame_set(&mut self) { + for camera in &mut self.cameras { + camera.image = None; + } + self.depth = None; + } + + fn try_track(&mut self, tf: &Tf) -> Option { + let mode = self.mode(); + if self.cameras.is_empty() || (mode == Mode::Rgbd && self.depth.is_none()) { + return None; + } + if self.cameras.iter().any(|camera| camera.image.is_none()) { + return None; + } + self.ensure_tracker(tf); + self.vslam.as_ref()?; // no tf placement yet + + let stamps: Vec = self + .cameras + .iter() + .map(|camera| stamp_to_ns(&camera.image.as_ref().expect("checked above").header)) + .collect(); + let oldest = *stamps.iter().min().expect("nonempty"); + let newest = *stamps.iter().max().expect("nonempty"); + if newest - oldest > MAX_PAIR_SKEW_NS { + self.skew_rejects += 1; + warn_throttled!( + Duration::from_secs(10), + rejected = self.skew_rejects, + skew_ms = (newest - oldest) as f64 / 1.0e6, + "cuvslam frame sets exceed the 1 ms skew limit", + ); + return None; + } + // cuVSLAM rejects a frame that is not strictly newer than the last one. + if self.last_ts_ns.is_some_and(|last| newest <= last) { + self.clear_frame_set(); + return None; + } + + let depth_choice = if mode == Mode::Rgbd { + // Reprojection inputs still missing means retry on the next message. + Some(self.align_depth(tf)?) + } else { + None + }; + + // Hand cuVSLAM every sample that precedes the frame about to be tracked. + let vslam = self.vslam.as_mut().expect("checked above"); + let consumed = self + .pending_imu + .partition_point(|measurement| measurement.timestamp_ns <= newest); + for measurement in self.pending_imu.drain(..consumed) { + if let Err(message) = vslam.register_imu(&measurement) { + warn_throttled!(Duration::from_secs(10), error = %message, "cuvslam rejected an IMU sample"); + } + } + + let result = { + let images: Vec = self + .cameras + .iter() + .enumerate() + .map(|(index, camera)| { + let image = camera.image.as_ref().expect("checked above"); + ImageRef { + pixels: &image.data, + width: image.width, + height: image.height, + encoding: image_encoding(&image.encoding), + data_type: ffi::CUV_DATA_UINT8, + timestamp_ns: newest, + camera_index: index as u32, + } + }) + .collect(); + let depths: Vec = depth_choice + .iter() + .map(|choice| { + let depth = match choice { + DepthChoice::Passthrough => self.depth.as_ref().expect("checked above"), + DepthChoice::Aligned => &self.aligned_depth, + }; + ImageRef { + pixels: &depth.data, + width: depth.width, + height: depth.height, + encoding: ffi::CUV_ENCODING_MONO, + data_type: ffi::CUV_DATA_UINT16, + timestamp_ns: newest, + camera_index: images[0].camera_index, + } + }) + .collect(); + vslam.track(&images, &depths) + }; + self.frames += 1; + self.last_ts_ns = Some(newest); + self.clear_frame_set(); + + let estimate = match result { + Ok(estimate) => estimate, + Err(message) => { + // Track() throwing aborted the C++ module; the shim catches it instead + // and the frame is skipped. + error!(error = %message, "cuvslam Track failed"); + return None; + } + }; + + let Some((pose, rig_covariance)) = estimate.world_from_rig else { + if self.was_tracking { + self.segment_id += 1; + self.was_tracking = false; + // The next tracked frame restarts across an unmeasured gap; its speed + // against the pre-loss pose would be meaningless. + self.previous_raw = None; + warn!(segment = self.segment_id, "cuvslam tracking lost"); + } + return None; + }; + // cuVSLAM tracks rig_frame(); the contract is base_frame starting at identity. Both + // collapse to the raw pose when the two frames are the same. + let base_from_rig = self.base_from_rig.expect("set with the tracker"); + let rig_from_base = self.rig_from_base.expect("set with the tracker"); + let raw_pose = base_from_rig * cuv_pose_to_isometry(&pose) * rig_from_base; + // cuVSLAM reports the 6x6 on the rig frame; the published pose is on base_frame, + // so the covariance moves through the same fixed transform. NaN (the tracker's + // unconstrained marker) survives the product and still trips the gate below. + self.covariance = self.covariance_adjoint + * Matrix6::from_row_slice(&rig_covariance) + * self.covariance_adjoint.transpose(); + let translation_variances = [ + self.covariance[(0, 0)], + self.covariance[(1, 1)], + self.covariance[(2, 2)], + ]; + let translation_std = translation_variances + .iter() + .fold(f64::NEG_INFINITY, |a, b| a.max(*b)) + .sqrt(); + // NaN covariance is the tracker's own way of saying unconstrained; a NaN never + // exceeds a threshold, so it has to be gated explicitly. (f64::max skips NaN, + // hence the separate finiteness check.) + let translation_finite = translation_variances.iter().all(|variance| variance.is_finite()); + let mut gate_frame = false; + if self.config.covariance_gate_translation_std > 0.0 + && self.was_tracking + && (!translation_finite || translation_std > self.config.covariance_gate_translation_std) + { + gate_frame = true; + self.covariance_gated += 1; + warn_throttled!( + Duration::from_secs(5), + translation_std, + gated = self.covariance_gated, + "cuvslam covariance gate holding pose", + ); + } + // Speed is judged against the previous raw pose even when that frame was gated: + // after a teleport the tracker keeps integrating from the far side, so later + // frames are near the teleported pose and only the jump frame itself trips. + if self.config.speed_gate_max_linear > 0.0 || self.config.speed_gate_max_angular > 0.0 { + if let Some(previous_raw) = &self.previous_raw { + // Track() rejects non-increasing stamps, so dt is strictly positive here. + let dt = (estimate.timestamp_ns - self.previous_raw_ns) as f64 / 1.0e9; + let linear_speed = (raw_pose.translation.vector - previous_raw.translation.vector).norm() / dt; + let angular_speed = previous_raw.rotation.angle_to(&raw_pose.rotation) / dt; + if (self.config.speed_gate_max_linear > 0.0 && linear_speed > self.config.speed_gate_max_linear) + || (self.config.speed_gate_max_angular > 0.0 + && angular_speed > self.config.speed_gate_max_angular) + { + gate_frame = true; + self.speed_gated += 1; + warn_throttled!( + Duration::from_secs(5), + linear_mps = linear_speed, + angular_rps = angular_speed, + gated = self.speed_gated, + "cuvslam speed gate holding pose", + ); + } + } + } + self.previous_raw = Some(raw_pose); + self.previous_raw_ns = estimate.timestamp_ns; + let rebase = self.rebase.unwrap_or_else(Isometry3::identity); + let world_from_base = self.world_from_base.unwrap_or_else(Isometry3::identity); + if gate_frame { + // Implausible frame (blank wall, repeated texture, teleport): drop its motion + // and keep rebasing onto the held pose, so recovery continues from here with + // only the delta measured after tracking became sane again. + self.rebase = Some(world_from_base * raw_pose.inverse()); + } else { + self.world_from_base = Some(rebase * raw_pose); + } + self.was_tracking = true; + self.tracked += 1; + Some(self.output(estimate.timestamp_ns)) + } + + fn output(&self, timestamp_ns: i64) -> Odometry { + let world_from_base = self.world_from_base.unwrap_or_else(Isometry3::identity); + let mut msg = Odometry::default(); + msg.header.stamp = to_stamp(timestamp_ns); + msg.header.frame_id = self.config.odom_frame.clone(); + msg.child_frame_id = self.config.base_frame.clone(); + let translation: Vector3 = world_from_base.translation.vector; + msg.pose.pose.position.x = translation.x; + msg.pose.pose.position.y = translation.y; + msg.pose.pose.position.z = translation.z; + let rotation = world_from_base.rotation.quaternion(); + msg.pose.pose.orientation.x = rotation.i; + msg.pose.pose.orientation.y = rotation.j; + msg.pose.pose.orientation.z = rotation.k; + msg.pose.pose.orientation.w = rotation.w; + // cuVSLAM's own 6x6, row-major xyz-rpy, the order ROS uses, already moved onto + // base_frame with the pose. + for row in 0..6 { + for column in 0..6 { + msg.pose.covariance[row * 6 + column] = self.covariance[(row, column)]; + } + } + msg + } + + pub fn report(&self) { + info!( + frames = self.frames, + tracked = self.tracked, + resets = self.segment_id, + imu_samples = self.imu_samples, + imu_dropped_before_start = self.imu_dropped, + skew_rejects = self.skew_rejects, + depth_reprojected = self.depth_reprojected, + covariance_gated = self.covariance_gated, + speed_gated = self.speed_gated, + unmatched_images = self.unplaced_images, + "cuvslam shutting down" + ); + } +} + +#[cfg(test)] +mod tests { + use super::*; + use dimos_module::nalgebra::{Translation3, UnitQuaternion}; + + #[test] + fn adjoint_of_identity_is_identity() { + assert_eq!(se3_adjoint(&Isometry3::identity()), Matrix6::identity()); + } + + #[test] + fn adjoint_pure_rotation_has_no_coupling() { + let rotation = UnitQuaternion::from_euler_angles(0.0, 0.0, std::f64::consts::FRAC_PI_2); + let adjoint = se3_adjoint(&Isometry3::from_parts(Translation3::identity(), rotation)); + let rotation_matrix = rotation.to_rotation_matrix().into_inner(); + assert!((adjoint.fixed_view::<3, 3>(0, 0) - rotation_matrix).norm() < 1.0e-12); + assert!(adjoint.fixed_view::<3, 3>(0, 3).norm() < 1.0e-12); + assert!((adjoint.fixed_view::<3, 3>(3, 3) - rotation_matrix).norm() < 1.0e-12); + assert!(adjoint.fixed_view::<3, 3>(3, 0).norm() < 1.0e-12); + } + + #[test] + fn adjoint_translation_couples_rotation_into_translation() { + // A yaw-rate uncertainty at a 2 m lever arm reads as lateral translation + // uncertainty: the (x, yaw) coupling term is skew(t) * R with R = I. + let lever_arm = Isometry3::from_parts(Translation3::new(2.0, 0.0, 0.0), UnitQuaternion::identity()); + let adjoint = se3_adjoint(&lever_arm); + assert!((adjoint[(1, 5)] + 2.0).abs() < 1.0e-12); // y <- yaw + assert!((adjoint[(2, 4)] - 2.0).abs() < 1.0e-12); // z <- pitch + assert_eq!(adjoint.fixed_view::<3, 3>(0, 0), Matrix3::identity()); + } + + #[test] + fn adjoint_moves_covariance_between_frames() { + // Pure yaw variance on the rig maps into y-translation variance on a base + // displaced 2 m along x: sigma_y = arm^2 * sigma_yaw. + let lever_arm = Isometry3::from_parts(Translation3::new(2.0, 0.0, 0.0), UnitQuaternion::identity()); + let adjoint = se3_adjoint(&lever_arm); + let mut rig_covariance = Matrix6::zeros(); + rig_covariance[(5, 5)] = 0.01; // yaw + let base_covariance = adjoint * rig_covariance * adjoint.transpose(); + assert!((base_covariance[(1, 1)] - 0.04).abs() < 1.0e-12); + assert!((base_covariance[(5, 5)] - 0.01).abs() < 1.0e-12); + } + + #[test] + fn left_swap_when_partner_sits_at_negative_x() { + let left = Isometry3::from_parts(Translation3::new(0.0, 0.0, 0.0), UnitQuaternion::identity()); + let right = Isometry3::from_parts(Translation3::new(0.12, 0.0, 0.0), UnitQuaternion::identity()); + assert!(!needs_left_swap(&left, &right)); + assert!(needs_left_swap(&right, &left)); + } + + #[test] + fn odometry_mode_selection() { + assert_eq!(odometry_mode(Mode::Rgbd, true), ffi::CUV_ODOMETRY_RGBD); + assert_eq!(odometry_mode(Mode::Mono, false), ffi::CUV_ODOMETRY_MONO); + assert_eq!(odometry_mode(Mode::Stereo, false), ffi::CUV_ODOMETRY_MULTICAMERA); + assert_eq!(odometry_mode(Mode::Stereo, true), ffi::CUV_ODOMETRY_INERTIAL); + } + + #[test] + fn encoding_declares_color_as_rgb() { + assert_eq!(image_encoding("mono8"), ffi::CUV_ENCODING_MONO); + assert_eq!(image_encoding("rgb8"), ffi::CUV_ENCODING_RGB); + assert_eq!(image_encoding("bgr8"), ffi::CUV_ENCODING_RGB); + } +} diff --git a/rust/src/cuvslam_odometry/depth_cloud.rs b/rust/src/cuvslam_odometry/depth_cloud.rs new file mode 100644 index 0000000..dcd9ab3 --- /dev/null +++ b/rust/src/cuvslam_odometry/depth_cloud.rs @@ -0,0 +1,66 @@ +// Copyright 2026 Dimensional Inc. +// SPDX-License-Identifier: Apache-2.0 +// +// Range-gated point cloud from a depth image, in the depth sensor's own frame. + +use lcm_msgs::sensor_msgs::{CameraInfo, Image, PointCloud2, PointField}; + +const BYTES_PER_POINT: i32 = 12; + +fn xyz_field(name: &str, offset: i32) -> PointField { + PointField { + name: name.to_string(), + offset, + datatype: PointField::FLOAT32 as u8, + count: 1, + } +} + +/// Deprojects every depth pixel through the depth intrinsics and keeps the ones inside the +/// range gate. Stereo depth error grows as range squared, so the far gate is what makes the +/// cloud usable for mapping; `max_range` of 0 leaves it open. Points come out in +/// `depth.header.frame_id`, unrotated, for a consumer that already knows where the sensor is. +pub fn depth_cloud( + depth: &Image, + depth_info: &CameraInfo, + units_per_meter: f64, + min_range: f64, + max_range: f64, + out: &mut PointCloud2, +) { + let (fx, fy) = (depth_info.K[0], depth_info.K[4]); + let (cx, cy) = (depth_info.K[2], depth_info.K[5]); + let far = if max_range > 0.0 { max_range } else { f64::INFINITY }; + + out.data.clear(); + let mut points = 0; + for v in 0..depth.height { + let row_start = v as usize * depth.step as usize; + for u in 0..depth.width { + let pixel_start = row_start + u as usize * 2; + let raw = u16::from_ne_bytes([depth.data[pixel_start], depth.data[pixel_start + 1]]); + if raw == 0 { + continue; + } + let z = raw as f64 / units_per_meter; + if z < min_range || z > far { + continue; + } + let x = (u as f64 - cx) / fx * z; + let y = (v as f64 - cy) / fy * z; + out.data.extend_from_slice(&(x as f32).to_le_bytes()); + out.data.extend_from_slice(&(y as f32).to_le_bytes()); + out.data.extend_from_slice(&(z as f32).to_le_bytes()); + points += 1; + } + } + + out.header = depth.header.clone(); + out.height = 1; + out.width = points; + out.fields = vec![xyz_field("x", 0), xyz_field("y", 4), xyz_field("z", 8)]; + out.is_bigendian = false; + out.point_step = BYTES_PER_POINT; + out.row_step = BYTES_PER_POINT * points; + out.is_dense = true; +} diff --git a/rust/src/cuvslam_odometry/depth_reproject.rs b/rust/src/cuvslam_odometry/depth_reproject.rs new file mode 100644 index 0000000..680943a --- /dev/null +++ b/rust/src/cuvslam_odometry/depth_reproject.rs @@ -0,0 +1,179 @@ +// Copyright 2026 Dimensional Inc. +// SPDX-License-Identifier: Apache-2.0 +// +// Depth-to-camera reprojection for the cuVSLAM module's rgbd mode. + +use dimos_module::nalgebra::Isometry3; +use lcm_msgs::sensor_msgs::{CameraInfo, Image}; + +/// Depth recorded against one camera, reprojected onto another: deproject through the depth +/// intrinsics, move through camera_from_depth, project through the target intrinsics. Where +/// several depth pixels land on one target pixel the nearest surface wins, matching what the +/// target camera would have seen. Unhit pixels stay 0, which cuVSLAM reads as no depth. +pub fn reproject_depth( + depth: &Image, + depth_info: &CameraInfo, + camera_info: &CameraInfo, + camera_from_depth: &Isometry3, + units_per_meter: f64, + out: &mut Image, +) { + out.header = depth.header.clone(); + out.header.frame_id = camera_info.header.frame_id.clone(); + out.width = camera_info.width; + out.height = camera_info.height; + out.encoding = depth.encoding.clone(); + out.is_bigendian = depth.is_bigendian; + out.step = out.width * std::mem::size_of::() as i32; + out.data.clear(); + out.data.resize(out.step as usize * out.height as usize, 0); + + let rotation = camera_from_depth.rotation.to_rotation_matrix(); + let column_x = rotation.matrix().column(0).into_owned(); + let column_y = rotation.matrix().column(1).into_owned(); + let column_z = rotation.matrix().column(2).into_owned(); + let origin = camera_from_depth.translation.vector; + let (fx_d, fy_d) = (depth_info.K[0], depth_info.K[4]); + let (cx_d, cy_d) = (depth_info.K[2], depth_info.K[5]); + let (fx_c, fy_c) = (camera_info.K[0], camera_info.K[4]); + let (cx_c, cy_c) = (camera_info.K[2], camera_info.K[5]); + let (out_width, out_height) = (out.width, out.height); + + for v in 0..depth.height { + let row_start = v as usize * depth.step as usize; + for u in 0..depth.width { + let pixel_start = row_start + u as usize * 2; + let raw = u16::from_ne_bytes([depth.data[pixel_start], depth.data[pixel_start + 1]]); + if raw == 0 { + continue; + } + let z = raw as f64 / units_per_meter; + let x = (u as f64 - cx_d) / fx_d * z; + let y = (v as f64 - cy_d) / fy_d * z; + let z_c = column_x[2] * x + column_y[2] * y + column_z[2] * z + origin[2]; + if z_c <= 0.0 { + continue; + } + let x_c = column_x[0] * x + column_y[0] * y + column_z[0] * z + origin[0]; + let y_c = column_x[1] * x + column_y[1] * y + column_z[1] * z + origin[1]; + let u_c = (fx_c * x_c / z_c + cx_c).round() as i64; + let v_c = (fy_c * y_c / z_c + cy_c).round() as i64; + if u_c < 0 || u_c >= out_width as i64 || v_c < 0 || v_c >= out_height as i64 { + continue; + } + let value = ((z_c * units_per_meter).round() as i64).min(u16::MAX as i64) as u16; + let slot_start = (v_c as usize * out_width as usize + u_c as usize) * 2; + let slot = u16::from_ne_bytes([out.data[slot_start], out.data[slot_start + 1]]); + if slot == 0 || value < slot { + out.data[slot_start..slot_start + 2].copy_from_slice(&value.to_ne_bytes()); + } + } + } +} + +#[cfg(test)] +mod tests { + use super::*; + use dimos_module::nalgebra::{Translation3, UnitQuaternion}; + + const UNITS_PER_METER: f64 = 1000.0; + + fn make_info(width: i32, height: i32, focal: f64) -> CameraInfo { + let mut info = CameraInfo { + width, + height, + ..Default::default() + }; + info.K[0] = focal; + info.K[4] = focal; + info.K[2] = width as f64 / 2.0; + info.K[5] = height as f64 / 2.0; + info + } + + fn make_depth(width: i32, height: i32) -> Image { + Image { + width, + height, + step: width * 2, + data: vec![0; (width * height * 2) as usize], + encoding: "16UC1".into(), + ..Default::default() + } + } + + fn set_pixel(image: &mut Image, u: i32, v: i32, value: u16) { + let start = (v * image.step + u * 2) as usize; + image.data[start..start + 2].copy_from_slice(&value.to_ne_bytes()); + } + + fn get_pixel(image: &Image, u: i32, v: i32) -> u16 { + let start = (v * image.step + u * 2) as usize; + u16::from_ne_bytes([image.data[start], image.data[start + 1]]) + } + + #[test] + fn identity_transform_is_passthrough() { + let info = make_info(8, 6, 100.0); + let mut depth = make_depth(8, 6); + set_pixel(&mut depth, 3, 2, 1500); + let mut out = Image::default(); + reproject_depth(&depth, &info, &info, &Isometry3::identity(), UNITS_PER_METER, &mut out); + assert_eq!(get_pixel(&out, 3, 2), 1500); + let total: u64 = (0..6) + .flat_map(|v| (0..8).map(move |u| (u, v))) + .map(|(u, v)| get_pixel(&out, u, v) as u64) + .sum(); + assert_eq!(total, 1500); + } + + #[test] + fn translation_shifts_projection() { + // Moving the target camera +x by 0.1 m at 1 m depth with f=100 shifts u by -10 px. + let info = make_info(32, 32, 100.0); + let mut depth = make_depth(32, 32); + set_pixel(&mut depth, 16, 16, 1000); + let camera_from_depth = + Isometry3::from_parts(Translation3::new(-0.1, 0.0, 0.0), UnitQuaternion::identity()); + let mut out = Image::default(); + reproject_depth(&depth, &info, &info, &camera_from_depth, UNITS_PER_METER, &mut out); + assert_eq!(get_pixel(&out, 6, 16), 1000); + } + + #[test] + fn nearest_surface_wins_on_collision() { + // Two depth pixels, one behind the other, forced onto one target pixel by a + // low-resolution target camera. + let depth_info = make_info(4, 4, 1000.0); + // Principal point at the corner so every ray lands on pixel (0, 0). + let mut target_info = make_info(1, 1, 0.001); + target_info.K[2] = 0.0; + target_info.K[5] = 0.0; + let mut depth = make_depth(4, 4); + set_pixel(&mut depth, 1, 1, 3000); + set_pixel(&mut depth, 2, 2, 1200); + let mut out = Image::default(); + reproject_depth( + &depth, + &depth_info, + &target_info, + &Isometry3::identity(), + UNITS_PER_METER, + &mut out, + ); + assert_eq!(get_pixel(&out, 0, 0), 1200); + } + + #[test] + fn behind_camera_is_dropped() { + let info = make_info(8, 8, 100.0); + let mut depth = make_depth(8, 8); + set_pixel(&mut depth, 4, 4, 500); + // Push the point 1 m behind the target camera. + let camera_from_depth = + Isometry3::from_parts(Translation3::new(0.0, 0.0, -1.0), UnitQuaternion::identity()); + let mut out = Image::default(); + reproject_depth(&depth, &info, &info, &camera_from_depth, UNITS_PER_METER, &mut out); + assert!(out.data.iter().all(|byte| *byte == 0)); + } +} diff --git a/rust/src/cuvslam_odometry/ffi.rs b/rust/src/cuvslam_odometry/ffi.rs new file mode 100644 index 0000000..068755d --- /dev/null +++ b/rust/src/cuvslam_odometry/ffi.rs @@ -0,0 +1,132 @@ +// Copyright 2026 Dimensional Inc. +// SPDX-License-Identifier: Apache-2.0 +// +// Raw bindings to shim/cuvslam_shim.h, the extern-"C" wrapper over the cuVSLAM C++ API. + +use std::os::raw::{c_char, c_void}; + +#[repr(C)] +#[derive(Clone, Copy, Debug)] +pub struct CuvPose { + pub rotation_xyzw: [f32; 4], + pub translation: [f32; 3], +} + +impl Default for CuvPose { + fn default() -> Self { + Self { + rotation_xyzw: [0.0, 0.0, 0.0, 1.0], + translation: [0.0, 0.0, 0.0], + } + } +} + +pub const CUV_DISTORTION_PINHOLE: u8 = 0; +pub const CUV_DISTORTION_BROWN: u8 = 2; + +#[repr(C)] +pub struct CuvCamera { + pub width: i32, + pub height: i32, + pub principal: [f32; 2], + pub focal: [f32; 2], + pub rig_from_camera: CuvPose, + pub distortion_model: u8, + pub distortion_parameters: *const f32, + pub distortion_parameter_count: i32, +} + +#[repr(C)] +#[derive(Clone, Copy, Debug, Default)] +pub struct CuvImuCalibration { + pub rig_from_imu: CuvPose, + pub gyroscope_noise_density: f32, + pub gyroscope_random_walk: f32, + pub accelerometer_noise_density: f32, + pub accelerometer_random_walk: f32, + pub frequency: f32, +} + +pub const CUV_ODOMETRY_MULTICAMERA: u8 = 0; +pub const CUV_ODOMETRY_INERTIAL: u8 = 1; +pub const CUV_ODOMETRY_RGBD: u8 = 2; +pub const CUV_ODOMETRY_MONO: u8 = 3; + +#[repr(C)] +#[derive(Clone, Copy, Debug)] +pub struct CuvConfig { + pub odometry_mode: u8, + pub use_gpu: bool, + pub rectified_stereo_camera: bool, + pub rgbd_depth_scale_factor: f32, + pub rgbd_depth_camera_id: i32, +} + +pub const CUV_ENCODING_MONO: u8 = 0; +pub const CUV_ENCODING_RGB: u8 = 1; +pub const CUV_DATA_UINT8: u8 = 0; +pub const CUV_DATA_UINT16: u8 = 1; + +#[repr(C)] +pub struct CuvImage { + pub pixels: *const c_void, + pub width: i32, + pub height: i32, + pub encoding: u8, + pub data_type: u8, + pub timestamp_ns: i64, + pub camera_index: u32, +} + +#[repr(C)] +#[derive(Clone, Copy, Debug, Default)] +pub struct CuvImuMeasurement { + pub timestamp_ns: i64, + pub linear_accelerations: [f32; 3], + pub angular_velocities: [f32; 3], +} + +#[repr(C)] +pub struct CuvPoseEstimate { + pub timestamp_ns: i64, + pub has_pose: bool, + pub world_from_rig: CuvPose, + pub covariance_xyz_rpy: [f64; 36], +} + +#[repr(C)] +pub struct CuvTracker { + _private: [u8; 0], +} + +extern "C" { + pub fn cuv_tracker_create( + cameras: *const CuvCamera, + camera_count: i32, + imu_or_null: *const CuvImuCalibration, + config: *const CuvConfig, + out_tracker: *mut *mut CuvTracker, + error_message: *mut c_char, + error_message_capacity: i32, + ) -> i32; + + pub fn cuv_tracker_track( + tracker: *mut CuvTracker, + images: *const CuvImage, + image_count: i32, + depths: *const CuvImage, + depth_count: i32, + out_estimate: *mut CuvPoseEstimate, + error_message: *mut c_char, + error_message_capacity: i32, + ) -> i32; + + pub fn cuv_tracker_register_imu( + tracker: *mut CuvTracker, + measurement: *const CuvImuMeasurement, + error_message: *mut c_char, + error_message_capacity: i32, + ) -> i32; + + pub fn cuv_tracker_destroy(tracker: *mut CuvTracker); +} diff --git a/rust/src/cuvslam_odometry/imu_info.rs b/rust/src/cuvslam_odometry/imu_info.rs new file mode 100644 index 0000000..c1ebd33 --- /dev/null +++ b/rust/src/cuvslam_odometry/imu_info.rs @@ -0,0 +1,127 @@ +// Copyright 2026 Dimensional Inc. +// SPDX-License-Identifier: Apache-2.0 + +//! Vendored `sensor_msgs.ImuInfo` until dimos-lcm ships the type, mirroring the +//! C++ module's vendored lcm-gen output. The fingerprint is the lcm hash chain +//! (ImuInfo -> Header -> Time) precomputed; a test pins it against the constants +//! of the generated types this decodes alongside. + +use std::io::{self, Cursor, Read}; +#[cfg(test)] +use std::io::Write; + +use lcm_msgs::std_msgs::Header; + +/// The IMU's noise model, published by the driver the way camera_info is. +#[derive(Debug, Clone, Default, PartialEq)] +pub struct ImuInfo { + pub header: Header, + pub gyro_noise_density: f64, + pub gyro_random_walk: f64, + pub accel_noise_density: f64, + pub accel_random_walk: f64, + pub frequency: f64, +} + +impl ImuInfo { + #[cfg(test)] + pub const HASH: i64 = 0x437a196cbd6b4e49u64 as i64; + /// rot1(HASH + rot1(Header::HASH + rot1(Time::HASH))), the lcm recursive hash. + const PACKED_FINGERPRINT: u64 = 0xE6AA5563F2A33280; + + #[cfg(test)] + pub fn encode(&self) -> Vec { + let mut buf = Vec::new(); + buf.extend_from_slice(&Self::PACKED_FINGERPRINT.to_be_bytes()); + self.encode_one(&mut buf).unwrap(); + buf + } + + pub fn decode(data: &[u8]) -> io::Result { + let mut cursor = Cursor::new(data); + let mut hash = [0u8; 8]; + cursor.read_exact(&mut hash)?; + let hash = u64::from_be_bytes(hash); + if hash != Self::PACKED_FINGERPRINT { + return Err(io::Error::new( + io::ErrorKind::InvalidData, + format!( + "Hash mismatch: expected {:016x}, got {:016x}", + Self::PACKED_FINGERPRINT, + hash + ), + )); + } + Self::decode_one(&mut cursor) + } + + #[cfg(test)] + fn encode_one(&self, buf: &mut W) -> io::Result<()> { + self.header.encode_one(buf)?; + for value in [ + self.gyro_noise_density, + self.gyro_random_walk, + self.accel_noise_density, + self.accel_random_walk, + self.frequency, + ] { + buf.write_all(&value.to_be_bytes())?; + } + Ok(()) + } + + fn decode_one(buf: &mut R) -> io::Result { + let header = Header::decode_one(buf)?; + let mut fields = [0f64; 5]; + for value in &mut fields { + let mut bytes = [0u8; 8]; + buf.read_exact(&mut bytes)?; + *value = f64::from_be_bytes(bytes); + } + let [gyro_noise_density, gyro_random_walk, accel_noise_density, accel_random_walk, frequency] = + fields; + Ok(Self { + header, + gyro_noise_density, + gyro_random_walk, + accel_noise_density, + accel_random_walk, + frequency, + }) + } +} + +#[cfg(test)] +mod tests { + use lcm_msgs::std_msgs::Time; + + use super::*; + + fn rot1(hash: u64) -> u64 { + hash.rotate_left(1) + } + + #[test] + fn fingerprint_matches_the_hash_chain() { + let header = rot1((Header::HASH as u64).wrapping_add(rot1(Time::HASH as u64))); + let expected = rot1((ImuInfo::HASH as u64).wrapping_add(header)); + assert_eq!(ImuInfo::PACKED_FINGERPRINT, expected); + } + + #[test] + fn roundtrips() { + let message = ImuInfo { + header: Header { + seq: 7, + stamp: Time { sec: 12, nsec: 34 }, + frame_id: "imu_link".to_string(), + }, + gyro_noise_density: 1e-3, + gyro_random_walk: 2e-5, + accel_noise_density: 3e-3, + accel_random_walk: 4e-4, + frequency: 400.0, + }; + assert_eq!(ImuInfo::decode(&message.encode()).unwrap(), message); + } +} diff --git a/rust/src/cuvslam_odometry/msg_convert.rs b/rust/src/cuvslam_odometry/msg_convert.rs new file mode 100644 index 0000000..2fd39eb --- /dev/null +++ b/rust/src/cuvslam_odometry/msg_convert.rs @@ -0,0 +1,129 @@ +// Copyright 2026 Dimensional Inc. +// SPDX-License-Identifier: Apache-2.0 +// +// Converters between dimos-lcm messages, cuVSLAM shim types and nalgebra: +// stamps, poses, distortion models. + +use dimos_module::nalgebra::{Isometry3, Translation3, UnitQuaternion}; +use dimos_module::Transform; +use lcm_msgs::sensor_msgs::CameraInfo; +use lcm_msgs::std_msgs::{Header, Time}; + +use super::ffi::{CuvPose, CUV_DISTORTION_BROWN, CUV_DISTORTION_PINHOLE}; + +pub const NS_PER_SEC: i64 = 1_000_000_000; + +pub fn stamp_to_ns(header: &Header) -> i64 { + header.stamp.sec as i64 * NS_PER_SEC + header.stamp.nsec as i64 +} + +pub fn to_stamp(timestamp_ns: i64) -> Time { + Time { + sec: (timestamp_ns / NS_PER_SEC) as i32, + nsec: (timestamp_ns % NS_PER_SEC) as i32, + } +} + +pub fn to_cuv_pose(iso: &Isometry3) -> CuvPose { + let rotation = iso.rotation.quaternion(); + CuvPose { + rotation_xyzw: [ + rotation.i as f32, + rotation.j as f32, + rotation.k as f32, + rotation.w as f32, + ], + translation: [ + iso.translation.x as f32, + iso.translation.y as f32, + iso.translation.z as f32, + ], + } +} + +pub fn cuv_pose_to_isometry(pose: &CuvPose) -> Isometry3 { + let [x, y, z, w] = pose.rotation_xyzw; + Isometry3::from_parts( + Translation3::new( + pose.translation[0] as f64, + pose.translation[1] as f64, + pose.translation[2] as f64, + ), + UnitQuaternion::from_quaternion(dimos_module::nalgebra::Quaternion::new( + w as f64, x as f64, y as f64, z as f64, + )), + ) +} + +pub fn transform_to_isometry(transform: &Transform) -> Isometry3 { + Isometry3::from_parts(Translation3::from(transform.translation()), transform.rotation()) +} + +/// Pinhole for a rectified camera, Brown for one whose camera_info carries real plumb_bob +/// coefficients (a D455's color stream is raw; its IR streams are rectified with D all zero). +/// ROS orders plumb_bob (k1, k2, p1, p2, k3); cuVSLAM's Brown wants (k1, k2, k3, p1, p2). +pub fn to_distortion(info: &CameraInfo) -> (u8, Vec) { + let d = &info.D; + let distorted = d.len() >= 5 && d.iter().any(|coefficient| *coefficient != 0.0); + if !distorted { + return (CUV_DISTORTION_PINHOLE, Vec::new()); + } + ( + CUV_DISTORTION_BROWN, + vec![d[0] as f32, d[1] as f32, d[4] as f32, d[2] as f32, d[3] as f32], + ) +} + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn stamp_round_trip() { + let stamp = to_stamp(1_234_567_890_123_456_789); + assert_eq!(stamp.sec, 1_234_567_890); + assert_eq!(stamp.nsec, 123_456_789); + let header = Header { + stamp, + ..Default::default() + }; + assert_eq!(stamp_to_ns(&header), 1_234_567_890_123_456_789); + } + + #[test] + fn distortion_all_zero_is_pinhole() { + let info = CameraInfo { + D: vec![0.0; 5], + ..Default::default() + }; + assert_eq!(to_distortion(&info), (CUV_DISTORTION_PINHOLE, Vec::new())); + } + + #[test] + fn distortion_empty_is_pinhole() { + let info = CameraInfo::default(); + assert_eq!(to_distortion(&info), (CUV_DISTORTION_PINHOLE, Vec::new())); + } + + #[test] + fn distortion_plumb_bob_reorders_to_brown() { + let info = CameraInfo { + D: vec![0.1, 0.2, 0.3, 0.4, 0.5], // ROS: k1 k2 p1 p2 k3 + ..Default::default() + }; + let (model, parameters) = to_distortion(&info); + assert_eq!(model, CUV_DISTORTION_BROWN); + assert_eq!(parameters, vec![0.1, 0.2, 0.5, 0.3, 0.4]); // cuVSLAM: k1 k2 k3 p1 p2 + } + + #[test] + fn pose_round_trip() { + let iso = Isometry3::from_parts( + Translation3::new(1.0, -2.0, 3.0), + UnitQuaternion::from_euler_angles(0.1, 0.2, 0.3), + ); + let back = cuv_pose_to_isometry(&to_cuv_pose(&iso)); + assert!((back.translation.vector - iso.translation.vector).norm() < 1.0e-6); + assert!(back.rotation.angle_to(&iso.rotation) < 1.0e-6); + } +} diff --git a/rust/src/cuvslam_odometry/tracker.rs b/rust/src/cuvslam_odometry/tracker.rs new file mode 100644 index 0000000..5873b20 --- /dev/null +++ b/rust/src/cuvslam_odometry/tracker.rs @@ -0,0 +1,153 @@ +// Copyright 2026 Dimensional Inc. +// SPDX-License-Identifier: Apache-2.0 +// +// Safe wrapper around the cuVSLAM shim: owns the tracker handle, turns error +// buffers into Result<_, String>, keeps the raw pointers behind slices. + +use std::ffi::CStr; +use std::os::raw::{c_char, c_void}; + +use super::ffi; + +const ERROR_CAPACITY: usize = 512; + +pub struct CameraParams { + pub width: i32, + pub height: i32, + pub principal: [f32; 2], + pub focal: [f32; 2], + pub rig_from_camera: ffi::CuvPose, + pub distortion_model: u8, + pub distortion_parameters: Vec, +} + +pub struct ImageRef<'a> { + pub pixels: &'a [u8], + pub width: i32, + pub height: i32, + pub encoding: u8, + pub data_type: u8, + pub timestamp_ns: i64, + pub camera_index: u32, +} + +pub struct PoseEstimate { + pub timestamp_ns: i64, + /// `None` while tracking is lost. + pub world_from_rig: Option<(ffi::CuvPose, [f64; 36])>, +} + +pub struct Tracker { + raw: *mut ffi::CuvTracker, +} + +// The module serializes all tracker calls on its one event loop; cuVSLAM itself +// only forbids concurrent calls, not cross-thread ownership. +unsafe impl Send for Tracker {} + +fn error_string(buffer: &[c_char]) -> String { + unsafe { CStr::from_ptr(buffer.as_ptr()) } + .to_string_lossy() + .into_owned() +} + +fn to_ffi_image(image: &ImageRef) -> ffi::CuvImage { + ffi::CuvImage { + pixels: image.pixels.as_ptr() as *const c_void, + width: image.width, + height: image.height, + encoding: image.encoding, + data_type: image.data_type, + timestamp_ns: image.timestamp_ns, + camera_index: image.camera_index, + } +} + +impl Tracker { + pub fn new( + cameras: &[CameraParams], + imu: Option<&ffi::CuvImuCalibration>, + config: &ffi::CuvConfig, + ) -> Result { + let ffi_cameras: Vec = cameras + .iter() + .map(|camera| ffi::CuvCamera { + width: camera.width, + height: camera.height, + principal: camera.principal, + focal: camera.focal, + rig_from_camera: camera.rig_from_camera, + distortion_model: camera.distortion_model, + distortion_parameters: camera.distortion_parameters.as_ptr(), + distortion_parameter_count: camera.distortion_parameters.len() as i32, + }) + .collect(); + let mut error = [0 as c_char; ERROR_CAPACITY]; + let mut raw: *mut ffi::CuvTracker = std::ptr::null_mut(); + let status = unsafe { + ffi::cuv_tracker_create( + ffi_cameras.as_ptr(), + ffi_cameras.len() as i32, + imu.map_or(std::ptr::null(), |calibration| calibration as *const _), + config, + &mut raw, + error.as_mut_ptr(), + ERROR_CAPACITY as i32, + ) + }; + if status != 0 { + return Err(error_string(&error)); + } + Ok(Self { raw }) + } + + pub fn track(&mut self, images: &[ImageRef], depths: &[ImageRef]) -> Result { + let ffi_images: Vec = images.iter().map(to_ffi_image).collect(); + let ffi_depths: Vec = depths.iter().map(to_ffi_image).collect(); + let mut error = [0 as c_char; ERROR_CAPACITY]; + let mut estimate = ffi::CuvPoseEstimate { + timestamp_ns: 0, + has_pose: false, + world_from_rig: ffi::CuvPose::default(), + covariance_xyz_rpy: [0.0; 36], + }; + let status = unsafe { + ffi::cuv_tracker_track( + self.raw, + ffi_images.as_ptr(), + ffi_images.len() as i32, + ffi_depths.as_ptr(), + ffi_depths.len() as i32, + &mut estimate, + error.as_mut_ptr(), + ERROR_CAPACITY as i32, + ) + }; + if status != 0 { + return Err(error_string(&error)); + } + Ok(PoseEstimate { + timestamp_ns: estimate.timestamp_ns, + world_from_rig: estimate + .has_pose + .then_some((estimate.world_from_rig, estimate.covariance_xyz_rpy)), + }) + } + + pub fn register_imu(&mut self, measurement: &ffi::CuvImuMeasurement) -> Result<(), String> { + let mut error = [0 as c_char; ERROR_CAPACITY]; + let status = unsafe { + ffi::cuv_tracker_register_imu(self.raw, measurement, error.as_mut_ptr(), ERROR_CAPACITY as i32) + }; + if status != 0 { + return Err(error_string(&error)); + } + Ok(()) + } +} + +impl Drop for Tracker { + fn drop(&mut self) { + unsafe { ffi::cuv_tracker_destroy(self.raw) }; + } +} diff --git a/rust/src/main.rs b/rust/src/main.rs new file mode 100644 index 0000000..17270dd --- /dev/null +++ b/rust/src/main.rs @@ -0,0 +1,243 @@ +// Copyright 2026 Dimensional Inc. +// SPDX-License-Identifier: Apache-2.0 +// +// dimSLAM: cuVSLAM visual odometry feeding an error-state Kalman fusion filter +// in-process. The tracker's pose stream never touches the wire; it enters the +// filter as a drifting source under visual_odom_frame, alongside any external +// sources on the `sources` input. +// +// in: image, camera_info, depth_image, depth_camera_info, imu, imu_info, sources +// out: odometry (fused, odom_frame -> base_frame), depth_cloud +// tf: odom_frame -> base_frame, and identity map_frame -> odom_frame until a map +// correction exists. + +mod cuvslam_odometry; +mod odometry_fusion; + +use cuvslam_odometry::imu_info::ImuInfo; +use cuvslam_odometry::{CuvslamOdometryConfig, VoCore}; +use dimos_module::{ + native_config, run_with_transport, warn_throttled, Input, Module, Output, Tf, Transform, +}; +use lcm_msgs::nav_msgs::Odometry; +use lcm_msgs::sensor_msgs::{CameraInfo, Image, Imu, PointCloud2}; +use odometry_fusion::{FusionCore, OdometryFusionConfig}; + +#[native_config] +struct DimSlamConfig { + // --- cuVSLAM (see CuvslamOdometryConfig for the full field docs) --- + camera_mode: String, + camera_frames: Vec, + rectified: bool, + use_gpu: bool, + /// Frame the tracker's internal odometry drifts in; fused as a drifting source, + /// so it must appear in source_frames. + visual_odom_frame: String, + rig_frame: String, + covariance_gate_translation_std: f64, + speed_gate_max_linear: f64, + speed_gate_max_angular: f64, + /// cuVSLAM's own inertial mode, separate from the filter's use_imu. + cuvslam_enable_imu: bool, + depth_units_per_meter: f64, + depth_cloud_min_range: f64, + depth_cloud_max_range: f64, + + // --- fusion (see OdometryFusionConfig for the full field docs) --- + odom_frame: String, + base_frame: String, + map_frame: String, + publish_map_to_odom: bool, + publish_tf: bool, + publish_rate: f64, + replay_buffer_seconds: f64, + mahalanobis_gate: f64, + use_imu: bool, + imu_gyro_noise_density: f64, + imu_gyro_random_walk: f64, + imu_accel_noise_density: f64, + imu_accel_random_walk: f64, + gravity: f64, + imu_init_samples: i64, + initial_position_std: f64, + initial_velocity_std: f64, + initial_rotation_std: f64, + initial_bias_std: f64, + source_frames: Vec, + source_pose_variances: Vec, + source_twist_variances: Vec, + constraint_twist_variances: Vec, +} + +#[derive(Module)] +#[module(setup = init, teardown = report)] +struct DimSlam { + #[input(decode = CameraInfo::decode)] + camera_info: Input, + #[input(decode = Image::decode)] + image: Input, + /// rgbd only; unconnected otherwise. + #[input(decode = Image::decode)] + depth_image: Input, + /// Only needed when depth has to be reprojected onto the rig camera. + #[input(decode = CameraInfo::decode)] + depth_camera_info: Input, + #[input(decode = Imu::decode)] + imu: Input, + /// cuvslam_enable_imu only: the IMU's noise model and frame. + #[input(decode = ImuInfo::decode)] + imu_info: Input, + /// External odometry sources, told apart by header.frame_id. + #[input(decode = Odometry::decode)] + sources: Input, + #[output(encode = Odometry::encode)] + odometry: Output, + /// rgbd only: the depth sensor's own points, range-gated, in the depth frame. + #[output(encode = PointCloud2::encode)] + depth_cloud: Output, + #[tf] + tf: Tf, + #[config] + config: DimSlamConfig, + + vo: Option, + fusion: Option, +} + +impl DimSlam { + async fn init(&mut self) { + self.vo = Some(VoCore::new(CuvslamOdometryConfig { + camera_mode: self.config.camera_mode.clone(), + camera_frames: self.config.camera_frames.clone(), + rectified: self.config.rectified, + use_gpu: self.config.use_gpu, + odom_frame: self.config.visual_odom_frame.clone(), + base_frame: self.config.base_frame.clone(), + rig_frame: self.config.rig_frame.clone(), + covariance_gate_translation_std: self.config.covariance_gate_translation_std, + speed_gate_max_linear: self.config.speed_gate_max_linear, + speed_gate_max_angular: self.config.speed_gate_max_angular, + enable_imu: self.config.cuvslam_enable_imu, + depth_units_per_meter: self.config.depth_units_per_meter, + depth_cloud_min_range: self.config.depth_cloud_min_range, + depth_cloud_max_range: self.config.depth_cloud_max_range, + })); + self.fusion = Some(FusionCore::new(OdometryFusionConfig { + odom_frame: self.config.odom_frame.clone(), + base_frame: self.config.base_frame.clone(), + publish_rate: self.config.publish_rate, + replay_buffer_seconds: self.config.replay_buffer_seconds, + mahalanobis_gate: self.config.mahalanobis_gate, + use_imu: self.config.use_imu, + imu_gyro_noise_density: self.config.imu_gyro_noise_density, + imu_gyro_random_walk: self.config.imu_gyro_random_walk, + imu_accel_noise_density: self.config.imu_accel_noise_density, + imu_accel_random_walk: self.config.imu_accel_random_walk, + gravity: self.config.gravity, + imu_init_samples: self.config.imu_init_samples, + initial_position_std: self.config.initial_position_std, + initial_velocity_std: self.config.initial_velocity_std, + initial_rotation_std: self.config.initial_rotation_std, + initial_bias_std: self.config.initial_bias_std, + source_frames: self.config.source_frames.clone(), + source_pose_variances: self.config.source_pose_variances.clone(), + source_twist_variances: self.config.source_twist_variances.clone(), + constraint_twist_variances: self.config.constraint_twist_variances.clone(), + })); + } + + async fn handle_camera_info(&mut self, info: CameraInfo) { + self.vo.as_mut().expect("setup ran").handle_camera_info(info, &self.tf); + } + + async fn handle_image(&mut self, img: Image) { + let tracked = self.vo.as_mut().expect("setup ran").handle_image(img, &self.tf); + self.fuse(tracked).await; + } + + async fn handle_depth_image(&mut self, img: Image) { + let (cloud, tracked) = self.vo.as_mut().expect("setup ran").handle_depth_image(img, &self.tf); + if let Some(cloud) = cloud { + self.depth_cloud.publish(&cloud).await.ok(); + } + self.fuse(tracked).await; + } + + async fn handle_depth_camera_info(&mut self, info: CameraInfo) { + self.vo.as_mut().expect("setup ran").handle_depth_camera_info(info); + } + + async fn handle_imu(&mut self, msg: Imu) { + if self.config.cuvslam_enable_imu { + self.vo.as_mut().expect("setup ran").handle_imu(&msg); + } + if !self.config.use_imu { + return; + } + let Some(base_from_imu) = self + .tf + .get_latest(&self.config.base_frame, &msg.header.frame_id) + else { + warn_throttled!( + std::time::Duration::from_secs(10), + imu_frame = %msg.header.frame_id, + "imu dropped: tf does not place the IMU in the base frame", + ); + return; + }; + self.fusion.as_mut().expect("setup ran").handle_imu(&msg, &base_from_imu); + self.publish().await; + } + + async fn handle_imu_info(&mut self, info: ImuInfo) { + self.vo.as_mut().expect("setup ran").handle_imu_info(info, &self.tf); + } + + async fn handle_sources(&mut self, msg: Odometry) { + self.fusion.as_mut().expect("setup ran").handle_source(&msg); + self.publish().await; + } + + /// The tracker's pose enters the filter like any other source, without a wire hop. + async fn fuse(&mut self, tracked: Option) { + let Some(visual_odometry) = tracked else { return }; + self.fusion.as_mut().expect("setup ran").handle_source(&visual_odometry); + self.publish().await; + } + + async fn publish(&mut self) { + let Some((msg, odom_from_base)) = self.fusion.as_mut().expect("setup ran").maybe_publish() + else { + return; + }; + self.odometry.publish(&msg).await.ok(); + if !self.config.publish_tf { + return; + } + let ts_secs = odom_from_base.ts; + let mut transforms = vec![odom_from_base]; + if self.config.publish_map_to_odom { + transforms.push(Transform::new( + self.config.map_frame.clone(), + self.config.odom_frame.clone(), + ts_secs, + dimos_module::nalgebra::Isometry3::identity(), + )); + } + self.tf.publish(&transforms).await.ok(); + } + + async fn report(&mut self) { + if let Some(vo) = &self.vo { + vo.report(); + } + if let Some(fusion) = &self.fusion { + fusion.report(); + } + } +} + +#[tokio::main] +async fn main() { + run_with_transport::().await; +} diff --git a/rust/src/odometry_fusion.rs b/rust/src/odometry_fusion.rs new file mode 100644 index 0000000..0e1e77c --- /dev/null +++ b/rust/src/odometry_fusion.rs @@ -0,0 +1,801 @@ +// Copyright 2026 Dimensional Inc. +// SPDX-License-Identifier: Apache-2.0 +// +// Error-state Kalman filter fusing IMU propagation with any number of +// nav_msgs/Odometry sources, told apart by header.frame_id. +// +// Source semantics come from the frame ids alone: a source whose header.frame_id +// equals odom_frame is fused absolutely; any other frame drifts on its own, so +// consecutive poses are fused as filter-anchored deltas. Twist is fused in the +// body frame either way. Per-source, per-dimension variances pick the trust: +// negative uses the message covariance, zero drops the dimension, positive is a +// fixed variance. Late measurements roll the filter back and replay. +// +// Without an IMU there is no process model, and a Kalman update cannot both pass a +// lone source through (gain 1) and average several (gain 1 is last-writer-wins). So +// the no-IMU mode is not a Kalman filter: each message's pose increment is composed +// onto the estimate scaled by its inverse-variance share among the active sources. +// One source gets share 1 and passes through exactly; several blend their drifts. + +mod eskf; + +use std::collections::{HashMap, VecDeque}; + +use dimos_module::nalgebra::{DVector, Isometry3, Translation3, UnitQuaternion, Vector3}; +use dimos_module::{native_config, warn_throttled, Transform}; +use lcm_msgs::nav_msgs::Odometry; +use lcm_msgs::sensor_msgs::Imu; +use tracing::{info, warn}; + +use eskf::{Filter, Jacobian, Mat15, State, Vec15}; + +const NS_PER_SEC: i64 = 1_000_000_000; + +/// Above this the robot is rotating for real: MEMS gyro bias sits around 0.01 rad/s, +/// so a bias-calibration window containing such samples would swallow true motion. +const STATIONARY_GYRO_LIMIT: f64 = 0.05; + +/// First-order low-pass on the differentiated gyro (~6 Hz at 400 Hz sampling); raw +/// finite differences are too noisy to use in the tangential lever-arm term. +const ANGULAR_ACCEL_SMOOTHING: f64 = 0.1; + +fn stamp_to_ns(header: &lcm_msgs::std_msgs::Header) -> i64 { + header.stamp.sec as i64 * NS_PER_SEC + header.stamp.nsec as i64 +} + +fn to_stamp(timestamp_ns: i64) -> lcm_msgs::std_msgs::Time { + lcm_msgs::std_msgs::Time { + sec: (timestamp_ns / NS_PER_SEC) as i32, + nsec: (timestamp_ns % NS_PER_SEC) as i32, + } +} + +fn vector3(v: &lcm_msgs::geometry_msgs::Vector3) -> Vector3 { + Vector3::new(v.x, v.y, v.z) +} + +/// Fixed and message policies share a guard: a non-finite or non-positive +/// variance can only drop the dimension, never divide by it. +fn resolve_variance(configured: f64, from_message: f64) -> f64 { + let variance = if configured < 0.0 { + from_message + } else { + configured + }; + if variance.is_finite() && variance > 0.0 { + variance + } else { + 0.0 + } +} + +#[native_config] +#[derive(Clone)] +pub struct OdometryFusionConfig { + pub odom_frame: String, + pub base_frame: String, + /// Output cadence; the filter itself runs at the IMU rate. + pub publish_rate: f64, + /// How far back a late measurement can reach before it is dropped. + pub replay_buffer_seconds: f64, + /// Outlier gate in standard deviations per measurement dimension; 0 disables. + pub mahalanobis_gate: f64, + /// With no IMU there is no process model and the Kalman machinery is bypassed: + /// the filter is seeded level from the first source message and pose increments + /// are blended by inverse-variance share, so a lone source passes through exactly. + pub use_imu: bool, + pub imu_gyro_noise_density: f64, + pub imu_gyro_random_walk: f64, + pub imu_accel_noise_density: f64, + pub imu_accel_random_walk: f64, + pub gravity: f64, + /// Samples averaged while stationary to level the filter and take the gyro bias. + pub imu_init_samples: i64, + pub initial_position_std: f64, + pub initial_velocity_std: f64, + pub initial_rotation_std: f64, + pub initial_bias_std: f64, + /// One entry per source: the header.frame_id its messages carry. + pub source_frames: Vec, + /// 6 per source, [x y z roll pitch yaw]: <0 message covariance, 0 drop, >0 fixed. + /// For a drifting (non-odom_frame) source a fixed variance is usually the right + /// choice, since its message covariance describes accumulated drift, not the delta. + pub source_pose_variances: Vec, + /// 6 per source, [vx vy vz wx wy wz], same convention, body frame. + pub source_twist_variances: Vec, + /// Virtual zero-twist measurement [vx vy vz wx wy wz] applied with every source + /// message: >0 constrains that dimension toward zero with this variance (e.g. the + /// non-holonomic vy, vz of a differential-drive base), 0 leaves it free. + pub constraint_twist_variances: Vec, +} + +#[derive(Clone, Debug)] +struct Measurement { + source: usize, + absolute: bool, + /// Absolute pose, or `None` when the message carried no usable pose. + pose: Option>, + /// Consecutive-message delta when the source drifts. + delta: Option>, + pose_variance: [f64; 6], // resolved; <=0 means dropped + twist_variance: [f64; 6], + linear: Vector3, + angular: Vector3, +} + +/// What a source last contributed, for the no-IMU blend: a source keeps claiming its +/// inverse-variance share of every increment until it has been silent for the timeout, +/// after which the remaining sources scale back up to a full share between them. +#[derive(Clone, Debug, Default)] +struct SourceActivity { + last_ns: i64, + inverse_variance: [f64; 6], +} + +const SOURCE_ACTIVITY_TIMEOUT_SECONDS: f64 = 0.5; + +// The snapshot alongside the kind dwarfs any variant, so boxing buys nothing. +#[allow(clippy::large_enum_variant)] +#[derive(Clone, Debug)] +enum EventKind { + Seed, + Imu { + gyro: Vector3, + accel: Vector3, + }, + Measurement(Measurement), +} + +/// One filter step, with the state it left behind so a late arrival can roll +/// back to just before its own slot and replay everything after it. +#[derive(Clone, Debug)] +struct Event { + ts_ns: i64, + kind: EventKind, + // snapshot after processing + state: State, + covariance: Mat15, + anchors: Vec>>, + activity: Vec, + last_imu_ns: i64, + last_gyro: Vector3, +} + +/// The filter itself, free of any transport; the module in main.rs drives it. +pub struct FusionCore { + config: OdometryFusionConfig, + filter: Filter, + initialized: bool, + init_gyro_sum: Vector3, + init_accel_sum: Vector3, + init_samples: u64, + events: VecDeque, + anchors: Vec>>, + activity: Vec, + last_imu_ns: i64, + last_gyro: Vector3, + lever_gyro: Vector3, + lever_gyro_ns: i64, + lever_alpha: Vector3, + /// Per-source last raw pose; deltas are a property of the message stream, so they + /// are computed once at arrival and survive replay untouched. + previous_source_pose: HashMap>, + last_publish_ns: i64, + imu_samples: u64, + measurements: u64, + gated: u64, + replayed: u64, + too_late: u64, +} + +impl FusionCore { + pub fn new(config: OdometryFusionConfig) -> Self { + Self { + config, + filter: Filter::default(), + initialized: false, + init_gyro_sum: Vector3::zeros(), + init_accel_sum: Vector3::zeros(), + init_samples: 0, + events: VecDeque::new(), + anchors: Vec::new(), + activity: Vec::new(), + last_imu_ns: 0, + last_gyro: Vector3::zeros(), + lever_gyro: Vector3::zeros(), + lever_gyro_ns: 0, + lever_alpha: Vector3::zeros(), + previous_source_pose: HashMap::new(), + last_publish_ns: 0, + imu_samples: 0, + measurements: 0, + gated: 0, + replayed: 0, + too_late: 0, + } + } + + /// `base_from_imu` places the IMU's mount frame (e.g. a camera optical frame) in + /// base_frame, where the filter state lives; both vectors are rotated through it. + pub fn handle_imu(&mut self, imu: &Imu, base_from_imu: &Transform) { + let lever = base_from_imu.translation(); + let base_from_imu = base_from_imu.rotation(); + let gyro = base_from_imu * vector3(&imu.angular_velocity); + let mut accel = base_from_imu * vector3(&imu.linear_acceleration); + // The IMU rides half a metre off the base origin, so any rotation adds + // centripetal and tangential acceleration at its location -- kinematics of the + // mount, not base motion. ~1.2 m/s^2 during this robot's in-place spins. + let ts_ns = stamp_to_ns(&imu.header); + if self.lever_gyro_ns != 0 { + let dt = (ts_ns - self.lever_gyro_ns) as f64 / NS_PER_SEC as f64; + if dt > 0.0 { + let raw_alpha = (gyro - self.lever_gyro) / dt; + self.lever_alpha += (raw_alpha - self.lever_alpha) * ANGULAR_ACCEL_SMOOTHING; + } + } + self.lever_gyro = gyro; + self.lever_gyro_ns = ts_ns; + accel -= gyro.cross(&gyro.cross(&lever)) + self.lever_alpha.cross(&lever); + if !self.initialized { + self.check_config(); + // Gyro bias is only observable while stationary; a sample this large is real + // rotation (MEMS bias is ~0.01 rad/s), so restart the window until it is over. + if gyro.norm() > STATIONARY_GYRO_LIMIT { + warn_throttled!( + std::time::Duration::from_secs(5), + gyro_norm = gyro.norm(), + "imu init deferred: waiting for the robot to hold still", + ); + self.init_gyro_sum = Vector3::zeros(); + self.init_accel_sum = Vector3::zeros(); + self.init_samples = 0; + return; + } + self.init_gyro_sum += gyro; + self.init_accel_sum += accel; + self.init_samples += 1; + if self.init_samples >= self.config.imu_init_samples.max(1) as u64 { + let mean_accel = self.init_accel_sum / self.init_samples as f64; + let mean_gyro = self.init_gyro_sum / self.init_samples as f64; + // Stationary accel reads the specific force R^T (0,0,g): level so it maps to +z. + let world_from_body = UnitQuaternion::rotation_between(&mean_accel, &Vector3::z()) + .unwrap_or_default(); + // Whatever magnitude the sensor reads beyond g while stationary is accel + // bias; left at zero it dead-reckons z away and tilts pitch to compensate. + let accel_bias = mean_accel + - world_from_body.inverse_transform_vector(&Vector3::new( + 0.0, + 0.0, + self.config.gravity, + )); + self.initialize(ts_ns, world_from_body, mean_gyro, accel_bias); + } + return; + } + self.insert(Event { + ts_ns, + kind: EventKind::Imu { gyro, accel }, + state: State::default(), + covariance: Mat15::identity(), + anchors: Vec::new(), + activity: Vec::new(), + last_imu_ns: 0, + last_gyro: Vector3::zeros(), + }); + self.imu_samples += 1; + } + + pub fn handle_source(&mut self, msg: &Odometry) { + if !self.initialized { + if self.config.use_imu { + return; + } + self.check_config(); + self.initialize( + stamp_to_ns(&msg.header), + UnitQuaternion::identity(), + Vector3::zeros(), + Vector3::zeros(), + ); + } + let Some(source) = self + .config + .source_frames + .iter() + .position(|frame| *frame == msg.header.frame_id) + else { + warn!(frame_id = %msg.header.frame_id, "odometry from unconfigured frame_id dropped"); + return; + }; + + let absolute = msg.header.frame_id == self.config.odom_frame; + let mut pose_variance = [0.0; 6]; + let mut twist_variance = [0.0; 6]; + for dim in 0..6 { + pose_variance[dim] = resolve_variance( + self.config.source_pose_variances[source * 6 + dim], + msg.pose.covariance[dim * 7], + ); + twist_variance[dim] = resolve_variance( + self.config.source_twist_variances[source * 6 + dim], + msg.twist.covariance[dim * 7], + ); + } + + let orientation = UnitQuaternion::from_quaternion(dimos_module::nalgebra::Quaternion::new( + msg.pose.pose.orientation.w, + msg.pose.pose.orientation.x, + msg.pose.pose.orientation.y, + msg.pose.pose.orientation.z, + )); + let raw_norm = (msg.pose.pose.orientation.w.powi(2) + + msg.pose.pose.orientation.x.powi(2) + + msg.pose.pose.orientation.y.powi(2) + + msg.pose.pose.orientation.z.powi(2)) + .sqrt(); + let pose_valid = (raw_norm - 1.0).abs() < 0.1; + let pose_wanted = pose_variance.iter().any(|v| *v > 0.0); + + let mut measurement = Measurement { + source, + absolute, + pose: None, + delta: None, + pose_variance, + twist_variance, + linear: Vector3::new( + msg.twist.twist.linear.x, + msg.twist.twist.linear.y, + msg.twist.twist.linear.z, + ), + angular: Vector3::new( + msg.twist.twist.angular.x, + msg.twist.twist.angular.y, + msg.twist.twist.angular.z, + ), + }; + if pose_wanted && pose_valid { + let pose = Isometry3::from_parts( + Translation3::new( + msg.pose.pose.position.x, + msg.pose.pose.position.y, + msg.pose.pose.position.z, + ), + orientation, + ); + if absolute { + measurement.pose = Some(pose); + } else { + if let Some(previous) = self.previous_source_pose.get(&source) { + measurement.delta = Some(previous.inverse() * pose); + } + self.previous_source_pose.insert(source, pose); + } + } + + self.insert(Event { + ts_ns: stamp_to_ns(&msg.header), + kind: EventKind::Measurement(measurement), + state: State::default(), + covariance: Mat15::identity(), + anchors: Vec::new(), + activity: Vec::new(), + last_imu_ns: 0, + last_gyro: Vector3::zeros(), + }); + self.measurements += 1; + } + + fn check_config(&self) { + let sources = self.config.source_frames.len(); + assert!( + self.config.source_pose_variances.len() == 6 * sources + && self.config.source_twist_variances.len() == 6 * sources, + "source_pose_variances and source_twist_variances need 6 entries per source_frames entry" + ); + assert!( + self.config.constraint_twist_variances.len() == 6, + "constraint_twist_variances needs exactly 6 entries" + ); + } + + fn initialize( + &mut self, + timestamp_ns: i64, + world_from_body: UnitQuaternion, + gyro_bias: Vector3, + accel_bias: Vector3, + ) { + self.filter.noise = eskf::Noise { + gyro_noise_density: self.config.imu_gyro_noise_density, + gyro_random_walk: self.config.imu_gyro_random_walk, + accel_noise_density: self.config.imu_accel_noise_density, + accel_random_walk: self.config.imu_accel_random_walk, + }; + self.filter.gravity = self.config.gravity; + self.filter.init( + world_from_body, + gyro_bias, + accel_bias, + self.config.initial_position_std, + self.config.initial_velocity_std, + self.config.initial_rotation_std, + self.config.initial_bias_std, + ); + self.anchors = vec![None; self.config.source_frames.len()]; + self.activity = vec![SourceActivity::default(); self.config.source_frames.len()]; + self.initialized = true; + self.last_publish_ns = timestamp_ns; + let mut seed = Event { + ts_ns: timestamp_ns, + kind: EventKind::Seed, + state: State::default(), + covariance: Mat15::identity(), + anchors: Vec::new(), + activity: Vec::new(), + last_imu_ns: 0, + last_gyro: Vector3::zeros(), + }; + self.snapshot(&mut seed); + self.events.push_back(seed); + info!( + gyro_bias = gyro_bias.norm(), + use_imu = self.config.use_imu, + "odometry_fusion initialized" + ); + } + + fn insert(&mut self, event: Event) { + let newest = self.events.back().expect("seeded at init").ts_ns; + if event.ts_ns >= newest { + self.events.push_back(event); + let index = self.events.len() - 1; + self.process_at(index); + } else if event.ts_ns <= self.events.front().expect("seeded at init").ts_ns { + self.too_late += 1; + warn!("measurement older than the replay buffer dropped"); + return; + } else { + // Roll back to the event just before the late slot and replay forward. + let slot = self.events.partition_point(|e| e.ts_ns <= event.ts_ns); + self.events.insert(slot, event); + for index in slot..self.events.len() { + self.process_at(index); + } + self.replayed += 1; + } + let horizon = self.events.back().expect("nonempty").ts_ns + - (self.config.replay_buffer_seconds * 1.0e9) as i64; + while self.events.len() > 1 && self.events.front().expect("nonempty").ts_ns < horizon { + self.events.pop_front(); + } + } + + /// Runs the event at `index` against the filter restored from its predecessor's + /// snapshot, then snapshots into it. Replay is just calling this again in order. + fn process_at(&mut self, index: usize) { + { + let previous = &self.events[index - 1]; + self.filter.x = previous.state.clone(); + self.filter.p_cov = previous.covariance; + self.anchors = previous.anchors.clone(); + self.activity = previous.activity.clone(); + self.last_imu_ns = previous.last_imu_ns; + self.last_gyro = previous.last_gyro; + } + let ts_ns = self.events[index].ts_ns; + let kind = self.events[index].kind.clone(); + match kind { + EventKind::Seed => {} + EventKind::Imu { gyro, accel } => { + let dt = (ts_ns - self.last_imu_ns) as f64 / 1.0e9; + if dt > 0.0 && dt < 1.0 { + self.filter.propagate(dt, &gyro, &accel); + } + self.last_imu_ns = ts_ns; + self.last_gyro = gyro; + } + EventKind::Measurement(measurement) => { + if self.config.use_imu { + self.apply(&measurement); + } else { + self.blend(&measurement, ts_ns); + } + } + } + let mut event = std::mem::replace( + &mut self.events[index], + Event { + ts_ns, + kind: EventKind::Seed, + state: State::default(), + covariance: Mat15::identity(), + anchors: Vec::new(), + activity: Vec::new(), + last_imu_ns: 0, + last_gyro: Vector3::zeros(), + }, + ); + self.snapshot(&mut event); + self.events[index] = event; + } + + fn apply(&mut self, measurement: &Measurement) { + let mut rows: Vec = Vec::new(); + let mut residuals: Vec = Vec::new(); + let mut variances: Vec = Vec::new(); + + let target = if measurement.absolute { + measurement.pose + } else if let Some(delta) = measurement.delta { + match self.anchors[measurement.source] { + Some(anchor) => Some(anchor * delta), + None => { + // First delta after (re)start: adopt the filter pose, fuse nothing. + self.anchor(measurement.source); + return; + } + } + } else { + None + }; + if let Some(target) = target { + let position_residual = target.translation.vector - self.filter.x.p; + let rotation_residual = (self.filter.x.q.inverse() * target.rotation).scaled_axis(); + for dim in 0..3 { + if measurement.pose_variance[dim] > 0.0 { + let mut row = Vec15::zeros(); + row[dim] = 1.0; + rows.push(row); + residuals.push(position_residual[dim]); + variances.push(measurement.pose_variance[dim]); + } + } + for dim in 0..3 { + if measurement.pose_variance[3 + dim] > 0.0 { + let mut row = Vec15::zeros(); + row[6 + dim] = 1.0; + rows.push(row); + residuals.push(rotation_residual[dim]); + variances.push(measurement.pose_variance[3 + dim]); + } + } + } + + self.add_twist_rows( + &measurement.linear, + &measurement.angular, + &measurement.twist_variance, + &mut rows, + &mut residuals, + &mut variances, + ); + let accepted = self.update(&rows, &residuals, &variances); + if accepted && !measurement.absolute && target.is_some() { + // Re-basing to the corrected pose keeps every source's anchor on the one fused + // trajectory. Anchoring each source to its own integrated chain instead makes + // drifting sources diverge without bound and the filter oscillate between them. + self.anchor(measurement.source); + } + + // The configured constraints ride along with every source message. + let mut constraint_variance = [0.0; 6]; + constraint_variance.copy_from_slice(&self.config.constraint_twist_variances); + let mut constraint_rows = Vec::new(); + let mut constraint_residuals = Vec::new(); + let mut constraint_variances = Vec::new(); + self.add_twist_rows( + &Vector3::zeros(), + &Vector3::zeros(), + &constraint_variance, + &mut constraint_rows, + &mut constraint_residuals, + &mut constraint_variances, + ); + self.update( + &constraint_rows, + &constraint_residuals, + &constraint_variances, + ); + } + + /// No-IMU fusion. Each message contributes a body-frame pose increment — a drifting + /// source its consecutive-message delta, an absolute one the offset from the current + /// estimate to its pose — composed onto the estimate scaled per dimension by the + /// source's inverse-variance share among the currently active sources. Shares sum to + /// one, so the blend advances at true scale, a lone source passes through exactly, + /// and a source falling silent releases its share to the others. + fn blend(&mut self, measurement: &Measurement, ts_ns: i64) { + let activity = &mut self.activity[measurement.source]; + activity.last_ns = ts_ns; + for dim in 0..6 { + activity.inverse_variance[dim] = if measurement.pose_variance[dim] > 0.0 { + 1.0 / measurement.pose_variance[dim] + } else { + 0.0 + }; + } + + let increment = if measurement.absolute { + let Some(pose) = measurement.pose else { return }; + Isometry3::from_parts(Translation3::from(self.filter.x.p), self.filter.x.q).inverse() + * pose + } else { + let Some(delta) = measurement.delta else { return }; + delta + }; + + let horizon = ts_ns - (SOURCE_ACTIVITY_TIMEOUT_SECONDS * 1.0e9) as i64; + let mut share = [0.0; 6]; + for (dim, share_dim) in share.iter_mut().enumerate() { + let total: f64 = self + .activity + .iter() + .filter(|a| a.last_ns >= horizon) + .map(|a| a.inverse_variance[dim]) + .sum(); + if total > 0.0 { + *share_dim = self.activity[measurement.source].inverse_variance[dim] / total; + } + } + + let translation = increment.translation.vector; + let rotation = increment.rotation.scaled_axis(); + self.filter.x.p += self.filter.x.q + * Vector3::new( + share[0] * translation.x, + share[1] * translation.y, + share[2] * translation.z, + ); + self.filter.x.q *= UnitQuaternion::from_scaled_axis(Vector3::new( + share[3] * rotation.x, + share[4] * rotation.y, + share[5] * rotation.z, + )); + } + + /// Body-frame twist rows. Linear velocity observes v and theta; angular velocity + /// is measured against the latest gyro sample, so it observes the gyro bias. + fn add_twist_rows( + &self, + linear: &Vector3, + angular: &Vector3, + variance: &[f64; 6], + rows: &mut Vec, + residuals: &mut Vec, + variances: &mut Vec, + ) { + let body_from_world = self.filter.x.q.inverse().to_rotation_matrix(); + let body_velocity = body_from_world * self.filter.x.v; + let velocity_skew = eskf::skew(&body_velocity); + for dim in 0..3 { + if variance[dim] > 0.0 { + let mut row = Vec15::zeros(); + for col in 0..3 { + row[3 + col] = body_from_world.matrix()[(dim, col)]; + row[6 + col] = velocity_skew[(dim, col)]; + } + rows.push(row); + residuals.push(linear[dim] - body_velocity[dim]); + variances.push(variance[dim]); + } + } + let predicted_angular = self.last_gyro - self.filter.x.bg; + for dim in 0..3 { + if variance[3 + dim] > 0.0 { + let mut row = Vec15::zeros(); + row[9 + dim] = -1.0; + rows.push(row); + residuals.push(angular[dim] - predicted_angular[dim]); + variances.push(variance[3 + dim]); + } + } + } + + fn update(&mut self, rows: &[Vec15], residuals: &[f64], variances: &[f64]) -> bool { + if rows.is_empty() { + return false; + } + let residual = DVector::from_row_slice(residuals); + let variance = DVector::from_row_slice(variances); + let mut jacobian = Jacobian::zeros(rows.len()); + for (index, row) in rows.iter().enumerate() { + jacobian.row_mut(index).copy_from(&row.transpose()); + } + let accepted = self.filter.update( + &residual, + &jacobian, + &variance, + self.config.mahalanobis_gate, + ); + if !accepted { + self.gated += 1; + } + accepted + } + + fn anchor(&mut self, source: usize) { + self.anchors[source] = Some(Isometry3::from_parts( + Translation3::from(self.filter.x.p), + self.filter.x.q, + )); + } + + fn snapshot(&self, event: &mut Event) { + event.state = self.filter.x.clone(); + event.covariance = self.filter.p_cov; + event.anchors = self.anchors.clone(); + event.activity = self.activity.clone(); + event.last_imu_ns = self.last_imu_ns; + event.last_gyro = self.last_gyro; + } + + /// Rate-gated: the fused Odometry and the odom->base transform, when due. + pub fn maybe_publish(&mut self) -> Option<(Odometry, Transform)> { + let latest = self.events.back()?; + let period = (1.0e9 / self.config.publish_rate.max(1.0)) as i64; + if latest.ts_ns - self.last_publish_ns < period { + return None; + } + self.last_publish_ns = latest.ts_ns; + let state = latest.state.clone(); + let covariance = latest.covariance; + let last_gyro = latest.last_gyro; + let ts_ns = latest.ts_ns; + + let body_from_world = state.q.inverse().to_rotation_matrix(); + let body_velocity = body_from_world * state.v; + let body_angular = last_gyro - state.bg; + + let mut msg = Odometry::default(); + msg.header.stamp = to_stamp(ts_ns); + msg.header.frame_id = self.config.odom_frame.clone(); + msg.child_frame_id = self.config.base_frame.clone(); + msg.pose.pose.position.x = state.p.x; + msg.pose.pose.position.y = state.p.y; + msg.pose.pose.position.z = state.p.z; + msg.pose.pose.orientation.x = state.q.i; + msg.pose.pose.orientation.y = state.q.j; + msg.pose.pose.orientation.z = state.q.k; + msg.pose.pose.orientation.w = state.q.w; + msg.twist.twist.linear.x = body_velocity.x; + msg.twist.twist.linear.y = body_velocity.y; + msg.twist.twist.linear.z = body_velocity.z; + msg.twist.twist.angular.x = body_angular.x; + msg.twist.twist.angular.y = body_angular.y; + msg.twist.twist.angular.z = body_angular.z; + let velocity_world = covariance.fixed_view::<3, 3>(3, 3).into_owned(); + let velocity_body = + body_from_world.matrix() * velocity_world * body_from_world.matrix().transpose(); + for row in 0..3 { + for col in 0..3 { + msg.pose.covariance[row * 6 + col] = covariance[(row, col)]; + msg.pose.covariance[row * 6 + col + 3] = covariance[(row, col + 6)]; + msg.pose.covariance[(row + 3) * 6 + col] = covariance[(row + 6, col)]; + msg.pose.covariance[(row + 3) * 6 + col + 3] = covariance[(row + 6, col + 6)]; + msg.twist.covariance[row * 6 + col] = velocity_body[(row, col)]; + msg.twist.covariance[(row + 3) * 6 + col + 3] = covariance[(row + 9, col + 9)]; + } + } + + let pose = Isometry3::from_parts(Translation3::from(state.p), state.q); + let transform = Transform::new( + self.config.odom_frame.clone(), + self.config.base_frame.clone(), + ts_ns as f64 / 1.0e9, + pose, + ); + Some((msg, transform)) + } + + pub fn report(&self) { + info!( + imu_samples = self.imu_samples, + measurements = self.measurements, + gated = self.gated, + replayed = self.replayed, + too_late = self.too_late, + "odometry_fusion shutting down" + ); + } +} diff --git a/rust/src/odometry_fusion/eskf.rs b/rust/src/odometry_fusion/eskf.rs new file mode 100644 index 0000000..172e5b2 --- /dev/null +++ b/rust/src/odometry_fusion/eskf.rs @@ -0,0 +1,365 @@ +// Copyright 2026 Dimensional Inc. +// SPDX-License-Identifier: Apache-2.0 +// +// Error-state Kalman filter for odometry fusion. The IMU is the process model: +// it propagates position, velocity, orientation, and both biases, while the +// covariance carries the cross-terms that let position-space measurements +// correct velocity and bias. Measurements arrive as stacked scalar rows with a +// diagonal noise, so callers pick dimensions freely. +// +// Error-state order: p(0:3) v(3:6) theta(6:9) bg(9:12) ba(12:15), with theta a +// right (body-frame) rotation perturbation: q_true = q_est * Exp(theta). + +use dimos_module::nalgebra::{DVector, Dyn, Matrix3, OMatrix, SMatrix, UnitQuaternion, Vector3, U15}; + +pub type Mat15 = SMatrix; +pub type Vec15 = SMatrix; +pub type Jacobian = OMatrix; + +pub fn skew(v: &Vector3) -> Matrix3 { + v.cross_matrix() +} + +#[derive(Clone, Copy, Debug, Default)] +pub struct Noise { + pub gyro_noise_density: f64, // rad/s/sqrt(Hz) + pub gyro_random_walk: f64, // rad/s^2/sqrt(Hz) + pub accel_noise_density: f64, // m/s^2/sqrt(Hz) + pub accel_random_walk: f64, // m/s^3/sqrt(Hz) +} + +#[derive(Clone, Debug)] +pub struct State { + pub p: Vector3, // world position of the body + pub v: Vector3, // world velocity + pub q: UnitQuaternion, // world_from_body + pub bg: Vector3, // gyro bias + pub ba: Vector3, // accel bias +} + +impl Default for State { + fn default() -> Self { + Self { + p: Vector3::zeros(), + v: Vector3::zeros(), + q: UnitQuaternion::identity(), + bg: Vector3::zeros(), + ba: Vector3::zeros(), + } + } +} + +#[derive(Clone, Debug)] +pub struct Filter { + pub noise: Noise, + pub gravity: f64, + pub x: State, + pub p_cov: Mat15, +} + +impl Default for Filter { + fn default() -> Self { + Self { + noise: Noise::default(), + gravity: 9.80665, + x: State::default(), + p_cov: Mat15::identity(), + } + } +} + +impl Filter { + #[allow(clippy::too_many_arguments)] + pub fn init( + &mut self, + world_from_body: UnitQuaternion, + gyro_bias: Vector3, + accel_bias: Vector3, + position_std: f64, + velocity_std: f64, + rotation_std: f64, + bias_std: f64, + ) { + self.x = State::default(); + self.x.q = world_from_body; + self.x.bg = gyro_bias; + self.x.ba = accel_bias; + self.p_cov = Mat15::zeros(); + for (block, std) in [ + (0, position_std), + (3, velocity_std), + (6, rotation_std), + (9, bias_std), + (12, bias_std), + ] { + self.p_cov + .fixed_view_mut::<3, 3>(block, block) + .copy_from(&(Matrix3::identity() * std * std)); + } + } + + pub fn propagate(&mut self, dt: f64, gyro: &Vector3, accel: &Vector3) { + let rotation = self.x.q.to_rotation_matrix(); + let unbiased_gyro = gyro - self.x.bg; + let unbiased_accel = accel - self.x.ba; + let gravity_vector = Vector3::new(0.0, 0.0, -self.gravity); + let world_accel = rotation * unbiased_accel + gravity_vector; + + let mut f = Mat15::identity(); + f.fixed_view_mut::<3, 3>(0, 3) + .copy_from(&(Matrix3::identity() * dt)); + f.fixed_view_mut::<3, 3>(3, 6) + .copy_from(&(-(rotation.matrix() * skew(&unbiased_accel)) * dt)); + f.fixed_view_mut::<3, 3>(3, 12) + .copy_from(&(-rotation.matrix() * dt)); + f.fixed_view_mut::<3, 3>(6, 6) + .copy_from(&(Matrix3::identity() - skew(&unbiased_gyro) * dt)); + f.fixed_view_mut::<3, 3>(6, 9) + .copy_from(&(-Matrix3::identity() * dt)); + + self.x.p += self.x.v * dt + 0.5 * world_accel * dt * dt; + self.x.v += world_accel * dt; + self.x.q *= UnitQuaternion::from_scaled_axis(unbiased_gyro * dt); + + let mut q_noise = Mat15::zeros(); + for (block, density) in [ + (3, self.noise.accel_noise_density), + (6, self.noise.gyro_noise_density), + (9, self.noise.gyro_random_walk), + (12, self.noise.accel_random_walk), + ] { + q_noise + .fixed_view_mut::<3, 3>(block, block) + .copy_from(&(Matrix3::identity() * density * density * dt)); + } + self.p_cov = f * self.p_cov * f.transpose() + q_noise; + } + + /// One stacked update with a diagonal noise. `gate` is a Mahalanobis + /// threshold in standard deviations per degree of freedom; 0 disables the + /// gate. Returns false when the measurement was rejected. + // The negated comparison is deliberate: a NaN distance must reject. + #[allow(clippy::neg_cmp_op_on_partial_ord)] + pub fn update( + &mut self, + residual: &DVector, + jacobian: &Jacobian, + variance: &DVector, + gate: f64, + ) -> bool { + let noise = OMatrix::::from_diagonal(variance); + let innovation = jacobian * self.p_cov * jacobian.transpose() + &noise; + let Some(innovation_inverse) = innovation.try_inverse() else { + return false; + }; + if gate > 0.0 { + let mahalanobis_sq = (residual.transpose() * &innovation_inverse * residual)[(0, 0)]; + if !(mahalanobis_sq < gate * gate * residual.len() as f64) { + return false; + } + } + let kalman_gain = self.p_cov * jacobian.transpose() * &innovation_inverse; + let dx: Vec15 = &kalman_gain * residual; + let identity_minus_kh: Mat15 = Mat15::identity() - &kalman_gain * jacobian; + self.p_cov = identity_minus_kh * self.p_cov * identity_minus_kh.transpose() + + &kalman_gain * noise * kalman_gain.transpose(); + self.inject(&dx); + true + } + + fn inject(&mut self, dx: &Vec15) { + self.x.p += dx.fixed_rows::<3>(0); + self.x.v += dx.fixed_rows::<3>(3); + self.x.q *= UnitQuaternion::from_scaled_axis(dx.fixed_rows::<3>(6).into_owned()); + self.x.bg += dx.fixed_rows::<3>(9); + self.x.ba += dx.fixed_rows::<3>(12); + } +} + +#[cfg(test)] +mod tests { + use super::*; + + const GRAVITY: f64 = 9.80665; + + fn level_filter() -> Filter { + let mut filter = Filter { + noise: Noise { + gyro_noise_density: 0.01, + gyro_random_walk: 0.001, + accel_noise_density: 0.1, + accel_random_walk: 0.01, + }, + ..Filter::default() + }; + filter.init( + UnitQuaternion::identity(), + Vector3::zeros(), + Vector3::zeros(), + 0.1, + 0.1, + 0.05, + 0.01, + ); + filter + } + + /// Gravity-compensating accel for a level, stationary body. + fn static_accel() -> Vector3 { + Vector3::new(0.0, 0.0, GRAVITY) + } + + fn position_jacobian(rows: usize) -> Jacobian { + let mut jacobian = Jacobian::zeros(rows); + jacobian.fixed_view_mut::<3, 3>(0, 0).fill_with_identity(); + jacobian + } + + #[test] + fn skew_is_antisymmetric_and_reproduces_the_cross_product() { + let a = Vector3::new(1.0, 2.0, 3.0); + let b = Vector3::new(-0.5, 4.0, 0.25); + let a_skew = skew(&a); + assert!((a_skew + a_skew.transpose()).norm() < 1e-12); + assert!((a_skew * b - a.cross(&b)).norm() < 1e-12); + } + + #[test] + fn exp_and_log_of_so3_round_trip() { + let rotation = Vector3::new(0.3, -0.2, 0.9); + let q = UnitQuaternion::from_scaled_axis(rotation); + assert!((q.scaled_axis() - rotation).norm() < 1e-9); + } + + #[test] + fn exp_of_a_tiny_rotation_stays_finite_and_normalized() { + let q = UnitQuaternion::from_scaled_axis(Vector3::new(1e-14, 0.0, 0.0)); + let norm: f64 = q.norm(); + assert!((norm - 1.0).abs() < 1e-12); + } + + #[test] + fn stationary_propagation_holds_the_state() { + let mut filter = level_filter(); + for _ in 0..100 { + filter.propagate(0.01, &Vector3::zeros(), &static_accel()); + } + assert!(filter.x.p.norm() < 1e-9); + assert!(filter.x.v.norm() < 1e-9); + } + + #[test] + fn propagation_integrates_constant_velocity() { + let mut filter = level_filter(); + filter.x.v = Vector3::new(1.0, 0.0, 0.0); + for _ in 0..100 { + filter.propagate(0.01, &Vector3::zeros(), &static_accel()); + } + assert!((filter.x.p.x - 1.0).abs() < 1e-9); + } + + #[test] + fn propagation_grows_uncertainty() { + let mut filter = level_filter(); + let before = filter.p_cov.trace(); + for _ in 0..100 { + filter.propagate(0.01, &Vector3::zeros(), &static_accel()); + } + assert!(filter.p_cov.trace() > before); + } + + #[test] + fn a_position_update_pulls_the_estimate() { + let mut filter = level_filter(); + filter.propagate(0.01, &Vector3::zeros(), &static_accel()); + let residual = DVector::from_vec(vec![1.0, 0.0, 0.0]); + let variance = DVector::from_element(3, 1e-4); + assert!(filter.update(&residual, &position_jacobian(3), &variance, 0.0)); + assert!(filter.x.p.x > 0.9); + assert!(filter.p_cov.trace() < 15.0); + } + + #[test] + fn position_updates_correct_velocity_through_the_cross_covariance() { + let mut filter = level_filter(); + filter.x.v = Vector3::new(1.0, 0.0, 0.0); // true velocity is zero; the filter is wrong + for _ in 0..50 { + filter.propagate(0.01, &Vector3::zeros(), &static_accel()); + // The truth stays at the origin, so the residual is minus the estimate. + let residual = DVector::from_vec(vec![-filter.x.p.x, -filter.x.p.y, -filter.x.p.z]); + let variance = DVector::from_element(3, 1e-6); + filter.update(&residual, &position_jacobian(3), &variance, 0.0); + } + assert!(filter.x.v.x.abs() < 0.1); + } + + #[test] + fn the_mahalanobis_gate_rejects_an_outlier_and_passes_an_inlier() { + let mut filter = level_filter(); + filter.propagate(0.01, &Vector3::zeros(), &static_accel()); + let variance = DVector::from_element(3, 1e-4); + let outlier = DVector::from_vec(vec![100.0, 0.0, 0.0]); + assert!(!filter.update(&outlier, &position_jacobian(3), &variance, 3.0)); + let inlier = DVector::from_vec(vec![0.01, 0.0, 0.0]); + assert!(filter.update(&inlier, &position_jacobian(3), &variance, 3.0)); + } + + #[test] + fn a_gate_of_zero_disables_rejection() { + let mut filter = level_filter(); + let variance = DVector::from_element(3, 1e-4); + let outlier = DVector::from_vec(vec![100.0, 0.0, 0.0]); + assert!(filter.update(&outlier, &position_jacobian(3), &variance, 0.0)); + } + + #[test] + fn a_gyro_bias_propagates_into_a_tilt_the_accel_residual_can_expose() { + let mut filter = level_filter(); + let true_bias = Vector3::new(0.02, 0.0, 0.0); + // The gyro reads only its bias while the body is still: the estimate tilts. + for _ in 0..100 { + filter.propagate(0.01, &true_bias, &static_accel()); + } + let tilt = filter.x.q.scaled_axis(); + assert!((tilt.x - 0.02).abs() < 1e-3); + } + + #[test] + fn an_angular_rate_update_observes_the_gyro_bias() { + let mut filter = level_filter(); + let gyro_reading = Vector3::new(0.05, 0.0, 0.0); // still body, biased gyro + for _ in 0..50 { + filter.propagate(0.01, &gyro_reading, &static_accel()); + // A perfect rate sensor says the body is not rotating: residual is + // 0 - (gyro - bg), jacobian -I on the bias block. + let predicted = gyro_reading - filter.x.bg; + let residual = DVector::from_vec(vec![-predicted.x, -predicted.y, -predicted.z]); + let mut jacobian = Jacobian::zeros(3); + jacobian + .fixed_view_mut::<3, 3>(0, 9) + .copy_from(&(-Matrix3::identity())); + let variance = DVector::from_element(3, 1e-6); + filter.update(&residual, &jacobian, &variance, 0.0); + } + assert!((filter.x.bg.x - 0.05).abs() < 1e-3); + } + + #[test] + fn init_seeds_the_covariance_from_the_stds() { + let mut filter = Filter::default(); + filter.init( + UnitQuaternion::identity(), + Vector3::new(0.1, 0.0, 0.0), + Vector3::zeros(), + 2.0, + 3.0, + 0.5, + 0.01, + ); + assert!((filter.p_cov[(0, 0)] - 4.0).abs() < 1e-12); + assert!((filter.p_cov[(3, 3)] - 9.0).abs() < 1e-12); + assert!((filter.p_cov[(6, 6)] - 0.25).abs() < 1e-12); + assert!((filter.p_cov[(9, 9)] - 1e-4).abs() < 1e-12); + assert!((filter.x.bg.x - 0.1).abs() < 1e-12); + } +} diff --git a/src/cuvslam_main.cpp b/src/cuvslam_main.cpp deleted file mode 100644 index 377a130..0000000 --- a/src/cuvslam_main.cpp +++ /dev/null @@ -1,826 +0,0 @@ -// Copyright 2026 Dimensional Inc. -// SPDX-License-Identifier: Apache-2.0 -// -// NVIDIA cuVSLAM visual odometry as a dimos native module. -// -// in: image, camera_info (every camera on the one stream, told apart by frame_id), -// tf, depth_image, imu, imu_info (the IMU's noise model, published like camera_info) -// out: odometry, corrected_odometry, tf -// -// Nothing is published while tracking is lost. cuVSLAM keeps one world frame for the -// life of the tracker, so it resumes in the same frame. The rig comes from the tf tree. - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "cuvslam/cuvslam2.h" -#include "dimos/native.hpp" -#include "nav_msgs/Odometry.hpp" -#include "sensor_msgs/CameraInfo.hpp" -#include "sensor_msgs/Image.hpp" -#include "sensor_msgs/Imu.hpp" -#include "sensor_msgs/ImuInfo.hpp" -#include "tf2_msgs/TFMessage.hpp" -#include "utils/depth_reproject.hpp" -#include - -#include "utils/tf.hpp" -#include "utils/msg_convert.hpp" - -using dimos::native::Builder; -using dimos::native::Config; -using dimos::native::Module; -using dimos::native::Output; -namespace logging = dimos::native::log; -using namespace depth_reproject; -using namespace msg_convert; -using tf_client::Tf; -using Transform = Eigen::Isometry3d; - -namespace { - -/// cuVSLAM's Track() contract asks for stereo stamps within 1 ms. -constexpr std::int64_t MAX_PAIR_SKEW_NS = 1000000LL; // 1 ms - -std::string join(const std::vector& parts) { - std::string joined; - for (const std::string& part : parts) { - joined += joined.empty() ? part : ", " + part; - } - return joined; -} - -struct RigCamera { - std::string frame; - Transform rig_from_camera = Transform::Identity(); - sensor_msgs::CameraInfo info; - sensor_msgs::Image image; - bool have_image{false}; -}; - - -} // namespace - -struct CuvslamConfig { - /// "stereo", "mono" or "rgbd". Mono is accurate only up to scale. - std::string camera_mode; - /// One tf frame per camera, in cuVSLAM's index order. Empty discovers them off - /// camera_info. - std::vector camera_frames; - /// Images arrive rectified: no distortion, rows aligned. - bool rectified; - /// Off runs the tracker on the CPU. Needs a libcuvslam built with ENFORCE_GPU=OFF - /// (the jeff-hykin/cuVSLAM fork build); NVIDIA's stock SDK binaries are GPU-only. - bool use_gpu; - std::string odom_frame; - std::string base_frame; - /// Frame the cuVSLAM rig is expressed in. Empty means base_frame. Setting it to a camera's - /// optical frame reproduces NVIDIA's examples, whose rig IS the left camera; the published - /// odometry stays on base_frame either way, since the two differ by a fixed transform. - std::string rig_frame; - std::string map_frame; - /// Only read when Slam is off, where map->odom can only be identity. - bool publish_map_to_odom; - /// Pose graph + loop closure. - bool enable_slam; - /// GetPose() carries no timestamp, so a Slam thread running behind cannot be - /// matched to the odometry pose it corrects. - bool slam_async; - /// Poses kept in the graph; 0 is unlimited. - int slam_max_poses; - /// Floor on the interval between loop closures, milliseconds. - int slam_throttling_ms; - /// Rebase guard. A frame whose translation standard deviation (root of the largest - /// translation term of cuVSLAM's covariance) exceeds this is unconstrained: its motion - /// is dropped, the pose holds, and later frames are rebased onto the held pose so the - /// published path never carries the teleport. Meters; 0 disables the guard and the raw - /// integrator is published untouched. - double covariance_gate_translation_std; - /// Rebase guard on physically implausible frame-to-frame motion, sharing the - /// covariance gate's hold-and-rebase machinery but trusting kinematics instead of the - /// tracker's self-report: a teleport with confident covariance still cannot claim the - /// rig moved faster than the platform can. Linear is metres/second, angular is - /// radians/second, both measured on the raw pose against the previous tracked frame; - /// 0 disables that limit. - double speed_gate_max_linear; - double speed_gate_max_angular; - /// cuVSLAM's Inertial mode is stereo plus one IMU. The noise model arrives on the - /// imu_info stream, the way camera intrinsics arrive on camera_info. - bool enable_imu; - /// rgbd only: raw depth units per metre. 1000 for sixteen-bit millimetres. - double depth_units_per_meter; -}; - -class CuvslamOdometry : public Module { -public: - enum class Mode { Stereo, Mono, Rgbd }; - - void build(Builder& builder, Config& config) override { - cfg_ = config.parse(); - mode_ = cfg_.camera_mode == "stereo" ? Mode::Stereo - : cfg_.camera_mode == "rgbd" ? Mode::Rgbd - : Mode::Mono; - - builder.input("tf", &CuvslamOdometry::on_tf, this); - builder.input("camera_info", &CuvslamOdometry::on_camera_info, - this); - builder.input("image", &CuvslamOdometry::on_image, this); - if (mode_ == Mode::Rgbd) { - builder.input("depth_image", &CuvslamOdometry::on_depth, this); - builder.input("depth_camera_info", - &CuvslamOdometry::on_depth_camera_info, this); - } - if (cfg_.enable_imu) { - builder.input("imu", &CuvslamOdometry::on_imu, this); - builder.input("imu_info", &CuvslamOdometry::on_imu_info, this); - } - - odometry_ = builder.output("odometry"); - corrected_odometry_ = builder.output("corrected_odometry"); - tf_ = builder.output("tf"); - tf_client_.set_publish_sink([this](const std::vector& transforms) { - tf2_msgs::TFMessage message{}; - for (const tf_client::Transform& transform : transforms) { - geometry_msgs::TransformStamped stamped{}; - stamped.header.stamp = to_stamp( - static_cast(std::llround(transform.ts * 1.0e9))); - stamped.header.frame_id = transform.parent; - stamped.child_frame_id = transform.child; - stamped.transform.translation.x = transform.translation()[0]; - stamped.transform.translation.y = transform.translation()[1]; - stamped.transform.translation.z = transform.translation()[2]; - stamped.transform.rotation.x = transform.rotation()[0]; - stamped.transform.rotation.y = transform.rotation()[1]; - stamped.transform.rotation.z = transform.rotation()[2]; - stamped.transform.rotation.w = transform.rotation()[3]; - message.transforms.push_back(stamped); - } - message.transforms_length = static_cast(message.transforms.size()); - tf_.publish(message); - }); - tf_client_.set_warn_sink([](const std::string& message) { - DIMOS_LOG_THROTTLED(logging::Level::Warn, logging::from_secs(1), message.c_str()); - }); - } - - void teardown() override { - logging::info("cuvslam shutting down", - {logging::Field("frames", static_cast(frames_)), - logging::Field("tracked", static_cast(tracked_)), - logging::Field("resets", static_cast(segment_id_)), - logging::Field("loop_closures", static_cast(loop_closures_)), - logging::Field("imu_samples", static_cast(imu_samples_)), - logging::Field("imu_dropped_before_start", - static_cast(imu_dropped_)), - logging::Field("skew_rejects", static_cast(skew_rejects_)), - logging::Field("depth_reprojected", - static_cast(depth_reprojected_)), - logging::Field("covariance_gated", - static_cast(covariance_gated_)), - logging::Field("speed_gated", - static_cast(speed_gated_)), - logging::Field("unmatched_images", - static_cast(unplaced_images_))}); - } - -private: - void on_camera_info(const sensor_msgs::CameraInfo& info) { - if (tracker_) { - return; // rig is fixed once the tracker exists - } - camera_info_[info.header.frame_id] = info; - resolve_rig(); - } - - /// The mount tree, which is the whole of the extrinsic calibration. - void on_tf(const tf2_msgs::TFMessage& message) { - if (tracker_) { - return; // rig is fixed once the tracker exists - } - for (const geometry_msgs::TransformStamped& stamped : message.transforms) { - // Skip what this module publishes itself: odom and map are poses, not mount. - if (stamped.header.frame_id == cfg_.map_frame || - stamped.header.frame_id == cfg_.odom_frame) { - continue; - } - tf_client_.receive( - stamped.header.frame_id, stamped.child_frame_id, - static_cast(stamp_to_ns(stamped.header)) / 1.0e9, - {stamped.transform.rotation.x, stamped.transform.rotation.y, - stamped.transform.rotation.z, stamped.transform.rotation.w}, - {stamped.transform.translation.x, stamped.transform.translation.y, - stamped.transform.translation.z}); - } - resolve_rig(); - } - - /// The frame cuVSLAM's rig is expressed in, which need not be the published frame. - const std::string& rig_frame() const { - return cfg_.rig_frame.empty() ? cfg_.base_frame : cfg_.rig_frame; - } - - /// Place every camera against the rig frame, or nothing until they all resolve. - void resolve_rig() { - if (!cameras_.empty()) { - return; - } - const bool discovered = cfg_.camera_frames.empty(); - std::vector frames = cfg_.camera_frames; - if (discovered) { - for (const auto& [frame, info] : camera_info_) { - frames.push_back(frame); - } - // camera_info_ has no order of its own, and the rig is indexed. - std::sort(frames.begin(), frames.end()); - const std::size_t expected = mode_ == Mode::Stereo ? 2 : 1; - if (frames.size() != expected) { - if (frames.size() > expected) { - DIMOS_LOG_THROTTLED( - logging::Level::Error, logging::from_secs(10), - "cuvslam found more cameras on camera_info than camera_mode uses, " - "and they have no discoverable order. Set camera_frames.", - logging::Field("found", join(frames)), - logging::Field("mode", cfg_.camera_mode)); - } - return; - } - } - - std::vector cameras; - for (const std::string& frame : frames) { - const std::optional rig_from_camera = - tf_client_.get_latest(rig_frame(), frame); - const auto info = camera_info_.find(frame); - if (!rig_from_camera || info == camera_info_.end()) { - return; - } - cameras.push_back( - RigCamera{frame, to_isometry(rig_from_camera->rigid), info->second}); - } - // cuVSLAM wants camera 0 to be the left of a pair, which is the one whose - // partner sits at +x (optical convention, x to the right). - if (discovered && cameras.size() == 2 && - (cameras[0].rig_from_camera.inverse() * cameras[1].rig_from_camera) - .translation() - .x() < 0.0) { - std::swap(cameras[0], cameras[1]); - } - cameras_ = std::move(cameras); - } - - std::vector rig_frames() const { - std::vector frames; - for (const RigCamera& camera : cameras_) { - frames.push_back(camera.frame); - } - return frames; - } - - /// Which camera in the rig a frame_id names, or -1 for one that is not on it. - int camera_index(const std::string& frame_id) const { - for (std::size_t i = 0; i < cameras_.size(); ++i) { - if (cameras_[i].frame == frame_id) { - return static_cast(i); - } - } - return -1; - } - - /// The IMU's noise model, published by the driver the way camera_info is. - void on_imu_info(const sensor_msgs::ImuInfo& info) { - if (tracker_) { - return; // rig is fixed once the tracker exists - } - imu_info_ = info; - resolve_rig(); - } - - /// Buffered. cuVSLAM requires Track() and RegisterImuMeasurement() in - /// non-decreasing timestamp order; the round-robin dispatcher lets images - /// overtake a 400 Hz IMU. - void on_imu(const sensor_msgs::Imu& msg) { - if (!tracker_) { - // No tracker yet; this is the window inertial init needs. - ++imu_dropped_; - return; - } - cuvslam::ImuMeasurement measurement{}; - measurement.timestamp_ns = stamp_to_ns(msg.header); - // Track() has already consumed everything up to last_ts_ns_. - measurement.linear_accelerations = {static_cast(msg.linear_acceleration.x), - static_cast(msg.linear_acceleration.y), - static_cast(msg.linear_acceleration.z)}; - measurement.angular_velocities = {static_cast(msg.angular_velocity.x), - static_cast(msg.angular_velocity.y), - static_cast(msg.angular_velocity.z)}; - pending_imu_.push_back(measurement); - ++imu_samples_; - } - - void on_image(const sensor_msgs::Image& img) { - const int index = camera_index(img.header.frame_id); - if (index < 0) { - ++unplaced_images_; - DIMOS_LOG_THROTTLED(logging::Level::Warn, logging::from_secs(10), - "cuvslam dropping image with a frame_id not on the rig", - logging::Field("frame_id", img.header.frame_id), - logging::Field("dropped", - static_cast(unplaced_images_)), - logging::Field("rig_cameras", - static_cast(cameras_.size()))); - return; - } - cameras_[index].image = img; - cameras_[index].have_image = true; - try_track(); - } - - void on_depth(const sensor_msgs::Image& img) { - depth_ = img; - have_depth_ = true; - try_track(); - } - - /// Intrinsics of the depth sensor itself, only needed when depth has to be reprojected - /// onto the rig camera. Kept off `camera_info` so it cannot join the rig discovery. - void on_depth_camera_info(const sensor_msgs::CameraInfo& info) { - if (!have_depth_info_) { - depth_info_ = info; - have_depth_info_ = true; - } - } - - /// Depth pixel-aligned with the rig camera, as cuVSLAM's RGBD contract requires. - /// Passthrough when it was recorded against that camera; reprojected through the depth - /// intrinsics and the tf between the two sensors when it was not (a D455 records depth - /// against the left IR camera, not the color camera). Null while the pieces to - /// reproject are still missing. - const sensor_msgs::Image* align_depth() { - const RigCamera& camera = cameras_[0]; - if (depth_.header.frame_id == camera.frame) { - return &depth_; - } - if (!have_depth_info_) { - DIMOS_LOG_THROTTLED(logging::Level::Warn, logging::from_secs(10), - "cuvslam: depth is in another frame than the camera and needs " - "depth_camera_info to reproject", - logging::Field("depth_frame", depth_.header.frame_id), - logging::Field("camera_frame", camera.frame)); - return nullptr; - } - if (!camera_from_depth_) { - const auto camera_from_depth = - tf_client_.get_latest(camera.frame, depth_.header.frame_id); - camera_from_depth_ = camera_from_depth.has_value() - ? std::optional(to_isometry(camera_from_depth->rigid)) - : std::nullopt; - if (!camera_from_depth_) { - DIMOS_LOG_THROTTLED(logging::Level::Warn, logging::from_secs(10), - "cuvslam: tf does not connect the depth frame to the camera", - logging::Field("depth_frame", depth_.header.frame_id), - logging::Field("camera_frame", camera.frame)); - return nullptr; - } - logging::info("cuvslam reprojecting depth onto the rig camera", - {logging::Field("depth_frame", depth_.header.frame_id), - logging::Field("camera_frame", camera.frame)}); - } - reproject_depth(depth_, depth_info_, camera.info, *camera_from_depth_, - cfg_.depth_units_per_meter, aligned_depth_); - ++depth_reprojected_; - return &aligned_depth_; - } - - cuvslam::Odometry::OdometryMode odometry_mode() const { - using OdometryMode = cuvslam::Odometry::OdometryMode; - if (mode_ == Mode::Rgbd) { - return OdometryMode::RGBD; - } - if (mode_ == Mode::Mono) { - return OdometryMode::Mono; - } - // Inertial is the stereo pair plus an IMU; there is no inertial mono or rgbd. - return cfg_.enable_imu ? OdometryMode::Inertial : OdometryMode::Multicamera; - } - - /// Builds the tracker against rig_frame(). Poses are converted back onto base_frame on - /// publish, so rig_frame is an internal choice with no effect on the output contract. - void ensure_tracker() { - if (tracker_) { - return; - } - const std::optional base_from_rig = - tf_client_.get_latest(cfg_.base_frame, rig_frame()); - if (!base_from_rig) { - DIMOS_LOG_THROTTLED(logging::Level::Warn, logging::from_secs(10), - "cuvslam: tf does not place the rig frame against base_frame", - logging::Field("rig_frame", rig_frame()), - logging::Field("base_frame", cfg_.base_frame)); - return; - } - base_from_rig_ = to_isometry(base_from_rig->rigid); - rig_from_base_ = base_from_rig_.inverse(); - cuvslam::Rig rig; - for (const RigCamera& rig_camera : cameras_) { - const sensor_msgs::CameraInfo& info = rig_camera.info; - cuvslam::Camera camera{}; - camera.size = {info.width, info.height}; - camera.principal = {static_cast(info.K[2]), static_cast(info.K[5])}; - camera.focal = {static_cast(info.K[0]), static_cast(info.K[4])}; - camera.rig_from_camera = to_pose(rig_camera.rig_from_camera); - camera.distortion = to_distortion(info); - rig.cameras.push_back(camera); - } - if (cfg_.enable_imu) { - if (!imu_info_) { - DIMOS_LOG_THROTTLED(logging::Level::Warn, logging::from_secs(10), - "cuvslam: enable_imu is on but no imu_info has arrived"); - return; - } - const std::string& imu_frame = imu_info_->header.frame_id; - const std::optional rig_from_imu = - imu_frame.empty() ? std::nullopt : tf_client_.get_latest(rig_frame(), imu_frame); - if (!rig_from_imu) { - DIMOS_LOG_THROTTLED(logging::Level::Warn, logging::from_secs(10), - "cuvslam: enable_imu is on but tf does not place the IMU", - logging::Field("imu_frame", imu_frame)); - return; - } - cuvslam::ImuCalibration imu{}; - imu.rig_from_imu = to_pose(to_isometry(rig_from_imu->rigid)); - imu.gyroscope_noise_density = static_cast(imu_info_->gyro_noise_density); - imu.gyroscope_random_walk = static_cast(imu_info_->gyro_random_walk); - imu.accelerometer_noise_density = static_cast(imu_info_->accel_noise_density); - imu.accelerometer_random_walk = static_cast(imu_info_->accel_random_walk); - imu.frequency = static_cast(imu_info_->frequency); - rig.imus = {imu}; - } - - cuvslam::Odometry::Config odometry_cfg = cuvslam::Odometry::GetDefaultConfig(); - odometry_cfg.odometry_mode = odometry_mode(); - odometry_cfg.use_gpu = cfg_.use_gpu; - // cuVSLAM rejects this outright unless the rig has a stereo pair: "Rectified - // stereo camera mode only works with 1+ stereo cameras". - odometry_cfg.rectified_stereo_camera = cfg_.rectified && mode_ == Mode::Stereo; - odometry_cfg.enable_landmarks_export = cfg_.enable_slam; - // Slam reads the tracker's State, which GetState() only fills when the - // export flags are on; without them it throws instead of returning empty. - odometry_cfg.enable_observations_export = cfg_.enable_slam; - // On, cuVSLAM's bundle adjustment throws from its own thread, where no - // caller-side handler can catch it, and the process aborts. - odometry_cfg.async_sba = false; - if (mode_ == Mode::Rgbd) { - odometry_cfg.rgbd_settings.depth_scale_factor = - static_cast(cfg_.depth_units_per_meter); - // The default of -1 means no camera, and depth belonging to nothing is silently - // ignored. A depth stream usually reports its own frame, so unrecognised means 0. - odometry_cfg.rgbd_settings.depth_camera_id = - std::max(camera_index(depth_.header.frame_id), 0); - } - - tracker_.emplace(rig, odometry_cfg); - if (cfg_.enable_slam) { - cuvslam::Slam::Config slam_cfg = cuvslam::Slam::GetDefaultConfig(); - slam_cfg.sync_mode = !cfg_.slam_async; - slam_cfg.use_gpu = cfg_.use_gpu; - slam_cfg.max_map_size = static_cast(cfg_.slam_max_poses); - slam_cfg.throttling_time_ms = static_cast(cfg_.slam_throttling_ms); - slam_.emplace(rig, tracker_->GetPrimaryCameras(), slam_cfg); - } - logging::info("cuvslam tracker created", - {logging::Field("cameras", static_cast(rig.cameras.size())), - logging::Field("width", static_cast(rig.cameras[0].size[0])), - logging::Field("height", static_cast(rig.cameras[0].size[1])), - logging::Field("rig_frames", join(rig_frames()))}); - } - - /// Hand cuVSLAM every sample that precedes the frame about to be tracked. - void register_imu_through(std::int64_t timestamp_ns) { - auto after = std::find_if(pending_imu_.begin(), pending_imu_.end(), - [timestamp_ns](const cuvslam::ImuMeasurement& measurement) { - return measurement.timestamp_ns > timestamp_ns; - }); - for (auto it = pending_imu_.begin(); it != after; ++it) { - tracker_->RegisterImuMeasurement(0, *it); - } - pending_imu_.erase(pending_imu_.begin(), after); - } - - static cuvslam::Image to_cuvslam_image(const sensor_msgs::Image& img, - std::int64_t timestamp_ns, std::uint32_t camera_index) { - cuvslam::Image out{}; - out.pixels = img.data.data(); - out.width = img.width; - out.height = img.height; - out.pitch = img.step; - // A three-channel image has to be declared as such: fed as MONO, cuVSLAM - // reads a third of each row and tracks nothing. - out.encoding = img.encoding == "mono8" ? cuvslam::ImageData::Encoding::MONO - : cuvslam::ImageData::Encoding::RGB; - out.data_type = cuvslam::ImageData::DataType::UINT8; - out.is_gpu_mem = false; - out.timestamp_ns = timestamp_ns; - out.camera_index = camera_index; - return out; - } - - void clear_frame_set() { - for (RigCamera& camera : cameras_) { - camera.have_image = false; - } - have_depth_ = false; - } - - void try_track() { - if (cameras_.empty() || (mode_ == Mode::Rgbd && !have_depth_)) { - return; - } - for (const RigCamera& camera : cameras_) { - if (!camera.have_image) { - return; - } - } - ensure_tracker(); - if (!tracker_) { - return; // no camera_info yet - } - - std::int64_t oldest = stamp_to_ns(cameras_[0].image.header); - std::int64_t newest = oldest; - for (const RigCamera& camera : cameras_) { - const std::int64_t ts = stamp_to_ns(camera.image.header); - oldest = std::min(oldest, ts); - newest = std::max(newest, ts); - } - if (newest - oldest > MAX_PAIR_SKEW_NS) { - ++skew_rejects_; - DIMOS_LOG_THROTTLED(logging::Level::Warn, logging::from_secs(10), - "cuvslam frame sets exceed the 1 ms skew limit", - logging::Field("rejected", static_cast(skew_rejects_)), - logging::Field("skew_ms", (newest - oldest) / 1.0e6)); - return; - } - // cuVSLAM rejects a frame that is not strictly newer than the last one. - if (last_ts_ns_ && newest <= *last_ts_ns_) { - clear_frame_set(); - return; - } - - cuvslam::Odometry::ImageSet images; - for (std::size_t i = 0; i < cameras_.size(); ++i) { - images.push_back( - to_cuvslam_image(cameras_[i].image, newest, static_cast(i))); - } - cuvslam::Odometry::ImageSet depths; - if (mode_ == Mode::Rgbd) { - const sensor_msgs::Image* aligned = align_depth(); - if (aligned == nullptr) { - return; // reprojection inputs still missing; retry on the next message - } - cuvslam::Image depth = to_cuvslam_image(*aligned, newest, images[0].camera_index); - depth.encoding = cuvslam::ImageData::Encoding::MONO; - depth.data_type = cuvslam::ImageData::DataType::UINT16; - depths.push_back(depth); - } - - register_imu_through(newest); - const cuvslam::PoseEstimate est = tracker_->Track(images, {}, depths); - ++frames_; - last_ts_ns_ = newest; - clear_frame_set(); - - if (!est.world_from_rig.has_value()) { - if (was_tracking_) { - ++segment_id_; - was_tracking_ = false; - // The next tracked frame restarts across an unmeasured gap; its speed - // against the pre-loss pose would be meaningless. - previous_raw_.reset(); - logging::warn("cuvslam tracking lost", - {logging::Field("segment", static_cast(segment_id_))}); - } - return; - } - // cuVSLAM tracks rig_frame(); the contract is base_frame starting at identity. Both - // collapse to the raw pose when the two frames are the same. - const Transform raw_pose = - base_from_rig_ * to_isometry(est.world_from_rig->pose) * rig_from_base_; - covariance_ = est.world_from_rig->covariance_xyz_rpy; - const double translation_std = std::sqrt(static_cast( - std::max({covariance_[0], covariance_[7], covariance_[14]}))); - // NaN covariance is the tracker's own way of saying unconstrained; a NaN never - // exceeds a threshold, so it has to be gated explicitly. - bool gate_frame = false; - if (cfg_.covariance_gate_translation_std > 0.0 && was_tracking_ && - (!std::isfinite(translation_std) || - translation_std > cfg_.covariance_gate_translation_std)) { - gate_frame = true; - ++covariance_gated_; - DIMOS_LOG_THROTTLED(logging::Level::Warn, logging::from_secs(5), - "cuvslam covariance gate holding pose", - logging::Field("translation_std", translation_std), - logging::Field("gated", - static_cast(covariance_gated_))); - } - // Speed is judged against the previous raw pose even when that frame was gated: - // after a teleport the tracker keeps integrating from the far side, so later - // frames are near the teleported pose and only the jump frame itself trips. - if ((cfg_.speed_gate_max_linear > 0.0 || cfg_.speed_gate_max_angular > 0.0) && - previous_raw_.has_value()) { - // Track() rejects non-increasing stamps, so dt is strictly positive here. - const double dt = static_cast(est.timestamp_ns - previous_raw_ns_) / 1.0e9; - const double linear_speed = - (raw_pose.translation() - previous_raw_->translation()).norm() / dt; - const double angular_speed = - Eigen::Quaterniond(previous_raw_->linear()) - .angularDistance(Eigen::Quaterniond(raw_pose.linear())) / - dt; - if ((cfg_.speed_gate_max_linear > 0.0 && - linear_speed > cfg_.speed_gate_max_linear) || - (cfg_.speed_gate_max_angular > 0.0 && - angular_speed > cfg_.speed_gate_max_angular)) { - gate_frame = true; - ++speed_gated_; - DIMOS_LOG_THROTTLED(logging::Level::Warn, logging::from_secs(5), - "cuvslam speed gate holding pose", - logging::Field("linear_mps", linear_speed), - logging::Field("angular_rps", angular_speed), - logging::Field("gated", - static_cast(speed_gated_))); - } - } - previous_raw_ = raw_pose; - previous_raw_ns_ = est.timestamp_ns; - if (gate_frame) { - // Implausible frame (blank wall, repeated texture, teleport): drop its motion - // and keep rebasing onto the held pose, so recovery continues from here with - // only the delta measured after tracking became sane again. - rebase_ = world_from_rig_ * raw_pose.inverse(); - frame_gated_ = true; - } else { - world_from_rig_ = rebase_ * raw_pose; - frame_gated_ = false; - } - was_tracking_ = true; - ++tracked_; - publish(est.timestamp_ns); - if (slam_) { - run_slam(est.timestamp_ns); - } - } - - void run_slam(std::int64_t timestamp_ns) { - cuvslam::Odometry::State state; - tracker_->GetState(state); - slam_->Track(state); - - // Identity until Slam has a keyframe, which would read as a huge correction. - slam_started_ = slam_started_ || state.keyframe; - if (!slam_started_) { - return; - } - // Slam tracks the same rig, so its pose needs the same move onto base_frame as the - // odometry one before it can be compared with world_from_rig_ or published. - const Transform slam_pose = to_isometry(slam_->GetPose()) * rig_from_base_; - // The gate's decision carries over: an unconstrained frame poisons the slam pose - // through the same estimate, so the loop-closed stream holds and rebases with the - // VO stream. A real loop-closure snap has ordinary covariance and passes through. - if (frame_gated_) { - corrected_rebase_ = map_from_base_ * slam_pose.inverse(); - } else { - map_from_base_ = corrected_rebase_ * slam_pose; - } - const Transform& map_from_base = map_from_base_; - - const Transform map_from_odom_raw = map_from_base * world_from_rig_.inverse(); - - nav_msgs::Odometry corrected{}; - fill_pose(corrected, map_from_base, timestamp_ns, cfg_.map_frame, cfg_.base_frame); - corrected_odometry_.publish(corrected); - - tf_client_.publish({tf_client::Transform{cfg_.map_frame, cfg_.odom_frame, - static_cast(timestamp_ns) / 1.0e9, - to_rigid(map_from_odom_raw)}}); - - cuvslam::Slam::Metrics metrics{}; - slam_->GetSlamMetrics(metrics); - // The metrics are retained between Slam steps, which happen per keyframe, so a - // closure reads back on every frame until the next one. Key on the step's own stamp. - if (metrics.lc_status && metrics.timestamp_ns != last_closure_ns_) { - ++loop_closures_; - last_closure_ns_ = metrics.timestamp_ns; - logging::info("cuvslam loop closure", - {logging::Field("count", static_cast(loop_closures_)), - logging::Field("timestamp_ns", metrics.timestamp_ns), - logging::Field("tracked_landmarks", - static_cast(metrics.lc_tracked_landmarks_count))}); - } - } - - static void fill_pose(nav_msgs::Odometry& msg, const Transform& pose, std::int64_t timestamp_ns, - const std::string& frame, const std::string& child) { - msg.header.stamp = to_stamp(timestamp_ns); - msg.header.frame_id = frame; - msg.child_frame_id = child; - const Eigen::Quaterniond rotation(pose.linear()); - msg.pose.pose.position.x = pose.translation().x(); - msg.pose.pose.position.y = pose.translation().y(); - msg.pose.pose.position.z = pose.translation().z(); - msg.pose.pose.orientation.x = rotation.x(); - msg.pose.pose.orientation.y = rotation.y(); - msg.pose.pose.orientation.z = rotation.z(); - msg.pose.pose.orientation.w = rotation.w(); - } - - - void publish(std::int64_t timestamp_ns) { - nav_msgs::Odometry msg{}; - fill_pose(msg, world_from_rig_, timestamp_ns, cfg_.odom_frame, cfg_.base_frame); - // cuVSLAM's own 6x6, row-major xyz-rpy, the order ROS uses. Reported against the - // rig frame, which is base_frame unless rig_frame overrides it. - for (std::size_t i = 0; i < covariance_.size(); ++i) { - msg.pose.covariance[i] = covariance_[i]; - } - odometry_.publish(msg); - tf_client_.publish({tf_client::Transform{cfg_.odom_frame, cfg_.base_frame, - static_cast(timestamp_ns) / 1.0e9, - to_rigid(world_from_rig_)}}); - // With Slam running, map->odom is its correction; only one publisher per edge. - if (!cfg_.enable_slam && cfg_.publish_map_to_odom) { - tf_client_.publish({tf_client::Transform{cfg_.map_frame, cfg_.odom_frame, - static_cast(timestamp_ns) / 1.0e9, - to_rigid(Transform::Identity())}}); - } - } - - CuvslamConfig cfg_{}; - /// camera_mode, parsed once. - Mode mode_{Mode::Stereo}; - - // slam - std::optional slam_; - std::vector pending_imu_; - std::uint64_t imu_samples_{0}; - std::uint64_t imu_dropped_{0}; - bool slam_started_{false}; - std::uint64_t loop_closures_{0}; - std::int64_t last_closure_ns_{-1}; - - std::uint64_t skew_rejects_{0}; - - /// The rig, in cuVSLAM's camera order. - std::vector cameras_; - std::unordered_map camera_info_; - std::uint64_t unplaced_images_{0}; - std::optional imu_info_; - - /// child frame -> (its parent, parent_from_child). The bound is a cycle guard. - Tf tf_client_; - - /// covariance gate - cuvslam::PoseCovariance covariance_{}; - Transform rebase_ = Transform::Identity(); ///< identity until the gate first fires - bool frame_gated_{false}; - Transform corrected_rebase_ = Transform::Identity(); - Transform map_from_base_ = Transform::Identity(); ///< last published loop-closed pose - std::uint64_t covariance_gated_{0}; - std::optional previous_raw_; ///< raw pose of the last tracked frame - std::int64_t previous_raw_ns_{0}; - std::uint64_t speed_gated_{0}; - - sensor_msgs::Image depth_{}; - bool have_depth_{false}; - sensor_msgs::CameraInfo depth_info_{}; - bool have_depth_info_{false}; - sensor_msgs::Image aligned_depth_{}; - std::optional camera_from_depth_; - std::uint64_t depth_reprojected_{0}; - std::optional last_ts_ns_; - - // tracking state - std::optional tracker_; - Transform world_from_rig_ = Transform::Identity(); ///< last published pose, on base_frame - Transform base_from_rig_ = Transform::Identity(); ///< fixed; identity when rig_frame is base_frame - Transform rig_from_base_ = Transform::Identity(); - bool was_tracking_{false}; - std::uint64_t segment_id_{0}; - std::uint64_t frames_{0}; - std::uint64_t tracked_{0}; - - Output odometry_; - Output corrected_odometry_; - Output tf_; -}; - -int main() { - dimos::native::run_with_transport(); - return 0; -} diff --git a/src/utils/depth_reproject.hpp b/src/utils/depth_reproject.hpp deleted file mode 100644 index 5114f5b..0000000 --- a/src/utils/depth_reproject.hpp +++ /dev/null @@ -1,83 +0,0 @@ -// Copyright 2026 Dimensional Inc. -// SPDX-License-Identifier: Apache-2.0 -// -// Depth-to-camera reprojection for the cuVSLAM module's rgbd mode. - -#pragma once - -#include -#include -#include -#include - -#include "sensor_msgs/CameraInfo.hpp" -#include "sensor_msgs/Image.hpp" -#include - -namespace depth_reproject { - -/// Depth recorded against one camera, reprojected onto another: deproject through the depth -/// intrinsics, move through camera_from_depth, project through the target intrinsics. Where -/// several depth pixels land on one target pixel the nearest surface wins, matching what the -/// target camera would have seen. Unhit pixels stay 0, which cuVSLAM reads as no depth. -inline void reproject_depth(const sensor_msgs::Image& depth, - const sensor_msgs::CameraInfo& depth_info, - const sensor_msgs::CameraInfo& camera_info, - const Eigen::Isometry3d& camera_from_depth, - double units_per_meter, sensor_msgs::Image& out) { - - out.header = depth.header; - out.header.frame_id = camera_info.header.frame_id; - out.width = camera_info.width; - out.height = camera_info.height; - out.encoding = depth.encoding; - out.is_bigendian = depth.is_bigendian; - out.step = out.width * static_cast(sizeof(std::uint16_t)); - out.data.assign(static_cast(out.step) * out.height, 0); - out.data_length = static_cast(out.data.size()); - - // The rotation as columns, so each point costs multiplies rather than a full transform. - const Eigen::Matrix3d rotation = camera_from_depth.linear(); - const Eigen::Vector3d col_x = rotation.col(0); - const Eigen::Vector3d col_y = rotation.col(1); - const Eigen::Vector3d col_z = rotation.col(2); - const Eigen::Vector3d origin = camera_from_depth.translation(); - const double fx_d = depth_info.K[0], fy_d = depth_info.K[4]; - const double cx_d = depth_info.K[2], cy_d = depth_info.K[5]; - const double fx_c = camera_info.K[0], fy_c = camera_info.K[4]; - const double cx_c = camera_info.K[2], cy_c = camera_info.K[5]; - - auto* aligned = reinterpret_cast(out.data.data()); - for (std::int32_t v = 0; v < depth.height; ++v) { - const auto* row = reinterpret_cast( - depth.data.data() + static_cast(v) * depth.step); - for (std::int32_t u = 0; u < depth.width; ++u) { - const std::uint16_t raw = row[u]; - if (raw == 0) { - continue; - } - const double z = raw / units_per_meter; - const double x = (u - cx_d) / fx_d * z; - const double y = (v - cy_d) / fy_d * z; - const double z_c = col_x[2] * x + col_y[2] * y + col_z[2] * z + origin[2]; - if (z_c <= 0.0) { - continue; - } - const double x_c = col_x[0] * x + col_y[0] * y + col_z[0] * z + origin[0]; - const double y_c = col_x[1] * x + col_y[1] * y + col_z[1] * z + origin[1]; - const auto u_c = static_cast(std::lround(fx_c * x_c / z_c + cx_c)); - const auto v_c = static_cast(std::lround(fy_c * y_c / z_c + cy_c)); - if (u_c < 0 || u_c >= out.width || v_c < 0 || v_c >= out.height) { - continue; - } - const auto value = static_cast( - std::min(std::lround(z_c * units_per_meter), 65535L)); - std::uint16_t& slot = aligned[static_cast(v_c) * out.width + u_c]; - if (slot == 0 || value < slot) { - slot = value; - } - } - } -} - -} // namespace depth_reproject diff --git a/src/utils/msg_convert.hpp b/src/utils/msg_convert.hpp deleted file mode 100644 index e450567..0000000 --- a/src/utils/msg_convert.hpp +++ /dev/null @@ -1,104 +0,0 @@ -// Copyright 2026 Dimensional Inc. -// SPDX-License-Identifier: Apache-2.0 -// -// Boilerplate converters between dimos-lcm messages, cuVSLAM SDK types and -// Eigen: stamps, poses, distortion models. - -#pragma once - -#include -#include -#include - -#include - -#include "cuvslam/cuvslam2.h" -#include "geometry_msgs/Transform.hpp" -#include "sensor_msgs/CameraInfo.hpp" -#include "std_msgs/Header.hpp" -#include "std_msgs/Time.hpp" -#include "tf.hpp" - -namespace msg_convert { - -constexpr std::int64_t NS_PER_SEC = 1000000000LL; - -inline std::int64_t stamp_to_ns(const std_msgs::Header& header) { - return static_cast(header.stamp.sec) * NS_PER_SEC + - static_cast(header.stamp.nsec); -} - -inline std_msgs::Time to_stamp(std::int64_t timestamp_ns) { - std_msgs::Time stamp{}; - stamp.sec = static_cast(timestamp_ns / NS_PER_SEC); - stamp.nsec = static_cast(timestamp_ns % NS_PER_SEC); - return stamp; -} - -inline Eigen::Isometry3d to_isometry(const cuvslam::Pose& pose) { - Eigen::Isometry3d iso = Eigen::Isometry3d::Identity(); - // cuvslam::Pose rotation is xyzw; Eigen's quaternion constructor takes w first. - iso.linear() = Eigen::Quaterniond(pose.rotation[3], pose.rotation[0], pose.rotation[1], - pose.rotation[2]) - .toRotationMatrix(); - iso.translation() = - Eigen::Vector3d(pose.translation[0], pose.translation[1], pose.translation[2]); - return iso; -} - -inline Eigen::Isometry3d to_isometry(const geometry_msgs::Transform& message) { - Eigen::Isometry3d iso = Eigen::Isometry3d::Identity(); - iso.linear() = Eigen::Quaterniond(message.rotation.w, message.rotation.x, - message.rotation.y, message.rotation.z) - .toRotationMatrix(); - iso.translation() = Eigen::Vector3d(message.translation.x, message.translation.y, - message.translation.z); - return iso; -} - -inline Eigen::Isometry3d to_isometry(const tf_client::Rigid& rigid) { - Eigen::Isometry3d iso = Eigen::Isometry3d::Identity(); - iso.linear() = Eigen::Quaterniond(rigid.rotation[3], rigid.rotation[0], rigid.rotation[1], - rigid.rotation[2]) - .toRotationMatrix(); - iso.translation() = Eigen::Vector3d(rigid.translation[0], rigid.translation[1], - rigid.translation[2]); - return iso; -} - -inline tf_client::Rigid to_rigid(const Eigen::Isometry3d& iso) { - const Eigen::Quaterniond rotation(iso.linear()); - return tf_client::Rigid{ - {rotation.x(), rotation.y(), rotation.z(), rotation.w()}, - {iso.translation().x(), iso.translation().y(), iso.translation().z()}}; -} - -inline cuvslam::Pose to_pose(const Eigen::Isometry3d& iso) { - const Eigen::Quaterniond rotation(iso.linear()); - const Eigen::Vector3d& translation = iso.translation(); - cuvslam::Pose pose{}; - pose.rotation = {static_cast(rotation.x()), static_cast(rotation.y()), - static_cast(rotation.z()), static_cast(rotation.w())}; - pose.translation = {static_cast(translation.x()), static_cast(translation.y()), - static_cast(translation.z())}; - return pose; -} - -/// Pinhole for a rectified camera, Brown for one whose camera_info carries real plumb_bob -/// coefficients (a D455's color stream is raw; its IR streams are rectified with D all zero). -/// ROS orders plumb_bob (k1, k2, p1, p2, k3); cuVSLAM's Brown wants (k1, k2, k3, p1, p2). -inline cuvslam::Distortion to_distortion(const sensor_msgs::CameraInfo& info) { - const std::vector& d = info.D; - const bool distorted = - d.size() >= 5 && std::any_of(d.begin(), d.end(), [](double c) { return c != 0.0; }); - if (!distorted) { - return cuvslam::Distortion{cuvslam::Distortion::Model::Pinhole}; - } - cuvslam::Distortion distortion{cuvslam::Distortion::Model::Brown}; - distortion.parameters = {static_cast(d[0]), static_cast(d[1]), - static_cast(d[4]), static_cast(d[2]), - static_cast(d[3])}; - return distortion; -} - -} // namespace msg_convert diff --git a/src/utils/test_tf.cpp b/src/utils/test_tf.cpp deleted file mode 100644 index e207f0c..0000000 --- a/src/utils/test_tf.cpp +++ /dev/null @@ -1,345 +0,0 @@ -// Copyright 2026 Dimensional Inc. -// SPDX-License-Identifier: Apache-2.0 -// -// Mirror of the rust tf client's test suite (native/rust/dimos-module/src/tf.rs). - -#include - -#include -#include -#include -#include -#include -#include - -#include "tf.hpp" - -using tf_client::DEFAULT_TF_WINDOW_SECS; -using tf_client::Rigid; -using tf_client::TBuffer; -using tf_client::Tf; -using tf_client::Transform; - -namespace { - -constexpr double PI = 3.14159265358979323846; - -Rigid rigid_of(double x, double y, double z, double yaw) { - return Rigid{{0.0, 0.0, std::sin(yaw / 2.0), std::cos(yaw / 2.0)}, {x, y, z}}; -} - -void add(Tf& tf, const std::string& parent, const std::string& child, double ts, - double x, double y, double z, double yaw = 0.0) { - const Rigid rigid = rigid_of(x, y, z, yaw); - tf.receive(parent, child, ts, rigid.rotation, rigid.translation); -} - -double yaw_of(const Transform& transform) { - const std::array& q = transform.rotation(); - return std::atan2(2.0 * (q[3] * q[2] + q[0] * q[1]), - 1.0 - 2.0 * (q[1] * q[1] + q[2] * q[2])); -} - -} // namespace - -TEST_CASE("direct edge") { - Tf tf; - add(tf, "base_link", "arm", 1.0, 1.0, -1.0, 0.0); - const auto t = tf.get_latest("base_link", "arm"); - REQUIRE(t.has_value()); - CHECK(std::abs(t->translation()[0] - 1.0) < 1e-9); - CHECK(std::abs(t->translation()[1] + 1.0) < 1e-9); - CHECK(t->parent == "base_link"); - CHECK(t->child == "arm"); -} - -TEST_CASE("reverse edge returns inverse") { - Tf tf; - add(tf, "base_link", "arm", 1.0, 1.0, 2.0, 3.0); - const auto inv = tf.get_latest("arm", "base_link"); - REQUIRE(inv.has_value()); - CHECK(std::abs(inv->translation()[0] + 1.0) < 1e-9); - CHECK(std::abs(inv->translation()[1] + 2.0) < 1e-9); - CHECK(std::abs(inv->translation()[2] + 3.0) < 1e-9); - CHECK(inv->parent == "arm"); - CHECK(inv->child == "base_link"); -} - -TEST_CASE("reverse edge inverts rotation and translation") { - // Inverse of a rotated edge is t' = -R^T t, the classic sign/order trap. - Tf tf; - add(tf, "base_link", "arm", 1.0, 1.0, 2.0, 3.0, PI / 2.0); - const auto inv = tf.get_latest("arm", "base_link"); - REQUIRE(inv.has_value()); - CHECK(std::abs(inv->translation()[0] + 2.0) < 1e-9); - CHECK(std::abs(inv->translation()[1] - 1.0) < 1e-9); - CHECK(std::abs(inv->translation()[2] + 3.0) < 1e-9); - CHECK(std::abs(yaw_of(*inv) + PI / 2.0) < 1e-9); -} - -TEST_CASE("composes the ros example chain") { - Tf tf; - add(tf, "base_link", "arm", 1.0, 1.0, -1.0, 0.0, PI / 6.0); - add(tf, "arm", "end_effector", 1.0, 1.0, 1.0, 0.0); - const auto t = tf.get_latest("base_link", "end_effector"); - REQUIRE(t.has_value()); - CHECK(std::abs(t->translation()[0] - 1.366) < 1e-3); - CHECK(std::abs(t->translation()[1] - 0.366) < 1e-3); - CHECK(t->parent == "base_link"); - CHECK(t->child == "end_effector"); -} - -TEST_CASE("composes a multi-hop chain") { - Tf tf; - add(tf, "world", "robot", 1.0, 1.0, 2.0, 3.0); - add(tf, "robot", "sensor", 1.0, 0.5, 0.0, 0.2, PI / 2.0); - const auto t = tf.get_latest("world", "sensor"); - REQUIRE(t.has_value()); - CHECK(std::abs(t->translation()[0] - 1.5) < 1e-3); - CHECK(std::abs(t->translation()[1] - 2.0) < 1e-3); - CHECK(std::abs(t->translation()[2] - 3.2) < 1e-3); -} - -TEST_CASE("composed chain accumulates rotation") { - Tf tf; - add(tf, "a", "b", 1.0, 0.0, 0.0, 0.0, PI / 6.0); - add(tf, "b", "c", 1.0, 0.0, 0.0, 0.0, PI / 6.0); - const auto t = tf.get_latest("a", "c"); - REQUIRE(t.has_value()); - CHECK(std::abs(yaw_of(*t) - PI / 3.0) < 1e-9); -} - -TEST_CASE("composed stamp is the stalest edge") { - Tf tf; - add(tf, "world", "robot", 700.0, 1.0, 0.0, 0.0); - add(tf, "robot", "sensor", 1000.0, 0.5, 0.0, 0.0); - REQUIRE(tf.get_latest("world", "sensor").has_value()); - CHECK(tf.get_latest("world", "sensor")->ts == 700.0); - // Both directions, so the answer does not depend on which end is queried. - CHECK(tf.get_latest("sensor", "world")->ts == 700.0); -} - -TEST_CASE("missing path returns nullopt") { - Tf tf; - add(tf, "world", "robot", 1.0, 1.0, 0.0, 0.0); - CHECK_FALSE(tf.get_latest("world", "unconnected").has_value()); -} - -TEST_CASE("identity for the same frame") { - Tf tf; - // No query time: identity is stamped now, not the epoch. - const auto t = tf.get_latest("base_link", "base_link"); - REQUIRE(t.has_value()); - CHECK(std::abs(t->translation()[0]) < 1e-12); - CHECK(std::abs(t->translation()[1]) < 1e-12); - CHECK(std::abs(t->translation()[2]) < 1e-12); - CHECK(t->ts > 1.0e9); -} - -TEST_CASE("bfs takes the fewest hops on a branching graph") { - // Two routes to d: three hops through b, c and two through x. BFS must - // compose the two-hop route. - Tf tf; - add(tf, "a", "b", 1.0, 1.0, 0.0, 0.0); - add(tf, "b", "c", 1.0, 1.0, 0.0, 0.0); - add(tf, "c", "d", 1.0, 1.0, 0.0, 0.0); - add(tf, "a", "x", 1.0, 10.0, 0.0, 0.0); - add(tf, "x", "d", 1.0, 1.0, 0.0, 0.0); - const auto t = tf.get_latest("a", "d"); - REQUIRE(t.has_value()); - CHECK(std::abs(t->translation()[0] - 11.0) < 1e-9); -} - -TEST_CASE("time query picks the nearest sample") { - Tf tf; - add(tf, "a", "b", 10.0, 1.0, 0.0, 0.0); - add(tf, "a", "b", 20.0, 2.0, 0.0, 0.0); - const auto near_10 = tf.lookup("a", "b").at(11.0).get(); - REQUIRE(near_10.has_value()); - CHECK(std::abs(near_10->translation()[0] - 1.0) < 1e-9); - const auto near_20 = tf.lookup("a", "b").at(18.0).get(); - REQUIRE(near_20.has_value()); - CHECK(std::abs(near_20->translation()[0] - 2.0) < 1e-9); -} - -TEST_CASE("tie prefers the later sample") { - Tf tf; - add(tf, "a", "b", 10.0, 1.0, 0.0, 0.0); - add(tf, "a", "b", 12.0, 2.0, 0.0, 0.0); - const auto t = tf.lookup("a", "b").at(11.0).get(); - REQUIRE(t.has_value()); - CHECK(std::abs(t->translation()[0] - 2.0) < 1e-9); -} - -TEST_CASE("time query outside tolerance returns nullopt") { - Tf tf; - add(tf, "a", "b", 10.0, 1.0, 0.0, 0.0); - CHECK_FALSE(tf.lookup("a", "b").at(50.0).tolerance(1.0).get().has_value()); - CHECK(tf.lookup("a", "b").at(10.5).tolerance(1.0).get().has_value()); -} - -TEST_CASE("time query beyond the window returns nullopt without a tolerance") { - Tf tf; - add(tf, "a", "b", 100.0, 1.0, 0.0, 0.0); - CHECK_FALSE(tf.lookup("a", "b").at(50.0).get().has_value()); -} - -TEST_CASE("time query inside the window resolves without a tolerance") { - Tf tf; - add(tf, "a", "b", 100.0, 1.0, 0.0, 0.0); - const auto t = tf.lookup("a", "b").at(95.0).get(); - REQUIRE(t.has_value()); - CHECK(std::abs(t->translation()[0] - 1.0) < 1e-9); -} - -TEST_CASE("an explicit tolerance reaches past the window") { - // An explicit tolerance is the caller opting into staleness, so it widens - // past the window rather than being clamped by it. - Tf tf; - add(tf, "a", "b", 100.0, 1.0, 0.0, 0.0); - CHECK(tf.lookup("a", "b").at(50.0).tolerance(60.0).get().has_value()); -} - -TEST_CASE("latest is not bounded by the window") { - // The window bounds queries against a stamp, not the latest sample. With no - // requested time, the newest edge is returned however old it is. - Tf tf; - add(tf, "a", "b", 100.0, 1.0, 0.0, 0.0); - CHECK(tf.get_latest("a", "b").has_value()); -} - -TEST_CASE("within returns without waiting when already buffered") { - Tf tf; - add(tf, "a", "b", 5.0, 1.0, 0.0, 0.0); - const auto t = tf.lookup("a", "b").within(std::chrono::seconds(30)); - REQUIRE(t.has_value()); - CHECK(std::abs(t->translation()[0] - 1.0) < 1e-9); -} - -TEST_CASE("within resolves when the transform arrives late") { - // Returns as soon as the transform lands, not at the deadline. - Tf tf; - std::thread writer([&tf] { - std::this_thread::sleep_for(std::chrono::milliseconds(30)); - add(tf, "a", "b", 5.0, 1.0, 0.0, 0.0); - }); - const auto start = std::chrono::steady_clock::now(); - const auto t = tf.lookup("a", "b").within(std::chrono::seconds(10)); - const auto elapsed = std::chrono::steady_clock::now() - start; - writer.join(); - REQUIRE(t.has_value()); - CHECK(elapsed < std::chrono::seconds(5)); -} - -TEST_CASE("within times out when nothing arrives") { - Tf tf; - const auto start = std::chrono::steady_clock::now(); - const auto t = tf.lookup("a", "b").within(std::chrono::milliseconds(50)); - const auto elapsed = std::chrono::steady_clock::now() - start; - CHECK_FALSE(t.has_value()); - CHECK(elapsed >= std::chrono::milliseconds(50)); -} - -TEST_CASE("a zero rotation on the wire is dropped rather than stored as nan") { - Tf tf; - tf.receive("a", "b", 1.0, {0.0, 0.0, 0.0, 0.0}, {1.0, 0.0, 0.0}); - CHECK_FALSE(tf.get_latest("a", "b").has_value()); - tf.receive("a", "b", 2.0, {0.0, 0.0, 0.0, 1.0}, {1.0, 0.0, 0.0}); - const auto t = tf.get_latest("a", "b"); - REQUIRE(t.has_value()); - for (double component : t->rotation()) { - CHECK(std::isfinite(component)); - } -} - -TEST_CASE("a lookup that finds nothing warns with the frames") { - Tf tf; - std::vector warnings; - tf.set_warn_sink([&warnings](const std::string& message) { warnings.push_back(message); }); - add(tf, "world", "robot", 1.0, 1.0, 0.0, 0.0); - CHECK_FALSE(tf.get_latest("world", "gripper").has_value()); - REQUIRE(warnings.size() == 1); - CHECK(warnings[0].find("gripper") != std::string::npos); -} - -TEST_CASE("repeated misses warn once per frame pair") { - Tf tf; - std::vector warnings; - tf.set_warn_sink([&warnings](const std::string& message) { warnings.push_back(message); }); - for (int i = 0; i < 5; ++i) { - CHECK_FALSE(tf.get_latest("world", "gripper").has_value()); - } - CHECK(warnings.size() == 1); - // A pair that is throttled must not mute an unrelated one. - CHECK_FALSE(tf.get_latest("world", "camera").has_value()); - REQUIRE(warnings.size() == 2); - CHECK(warnings[1].find("camera") != std::string::npos); -} - -TEST_CASE("a resolved lookup stays quiet") { - Tf tf; - std::vector warnings; - tf.set_warn_sink([&warnings](const std::string& message) { warnings.push_back(message); }); - add(tf, "world", "robot", 1.0, 1.0, 0.0, 0.0); - CHECK(tf.get_latest("world", "robot").has_value()); - CHECK(warnings.empty()); -} - -TEST_CASE("publish feeds the local graph and the sink") { - Tf tf; - std::vector seen; - tf.set_publish_sink( - [&seen](const std::vector& transforms) { seen = transforms; }); - tf.publish({Transform{"odom", "base", 7.0, rigid_of(1.0, 2.0, 3.0, 0.0)}}); - REQUIRE(seen.size() == 1); - CHECK(seen[0].parent == "odom"); - const auto t = tf.get_latest("odom", "base"); - REQUIRE(t.has_value()); - CHECK(std::abs(t->translation()[0] - 1.0) < 1e-9); - CHECK(std::abs(t->translation()[1] - 2.0) < 1e-9); - CHECK(std::abs(t->translation()[2] - 3.0) < 1e-9); - CHECK(std::abs(t->ts - 7.0) < 1e-9); -} - -TEST_CASE("tbuffer prunes samples outside the window") { - TBuffer buffer(5.0); - buffer.add(1.0, Rigid{}); - buffer.add(2.0, Rigid{}); - buffer.add(10.0, Rigid{}); - CHECK(buffer.samples().size() == 1); - CHECK(std::abs(buffer.last()->ts - 10.0) < 1e-9); -} - -TEST_CASE("tbuffer late sample does not spare ones the window has aged out") { - TBuffer buffer(5.0); - buffer.add(10.0, Rigid{}); - buffer.add(11.0, Rigid{}); - buffer.add(7.0, Rigid{}); - CHECK(std::abs(buffer.samples().front().ts - 7.0) < 1e-9); - buffer.add(13.0, Rigid{}); - // 7.0 is now older than newest - window. - CHECK(std::abs(buffer.samples().front().ts - 10.0) < 1e-9); -} - -TEST_CASE("tbuffer clock reset drops the pre-jump samples") { - TBuffer buffer(5.0); - for (int i = 0; i < 5; ++i) { - buffer.add(1000.0 + i, Rigid{}); - } - for (int i = 0; i < 20; ++i) { - buffer.add(100.0 + i, Rigid{}); - } - CHECK(buffer.samples().size() <= 6); - CHECK(std::abs(buffer.last()->ts - 119.0) < 1e-9); -} - -TEST_CASE("tbuffer add out of order keeps samples sorted") { - TBuffer buffer(DEFAULT_TF_WINDOW_SECS); - buffer.add(3.0, Rigid{}); - buffer.add(1.0, Rigid{}); - buffer.add(2.0, Rigid{}); - CHECK(std::abs(buffer.last()->ts - 3.0) < 1e-9); - const auto* closest = buffer.find_closest(1.9, std::nullopt); - REQUIRE(closest != nullptr); - CHECK(std::abs(closest->ts - 2.0) < 1e-9); -} diff --git a/src/utils/tf.hpp b/src/utils/tf.hpp deleted file mode 100644 index 7aff642..0000000 --- a/src/utils/tf.hpp +++ /dev/null @@ -1,484 +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. - -// Transform client, a C++ mirror of the rust one (native/rust/dimos-module/src/tf.rs). -// -// Each tf edge is buffered per (parent, child), and Tf::lookup composes the shortest -// path through the frame graph. Lookups are nearest-in-time within a tolerance, not -// interpolated. Tf::publish feeds the local graph and forwards to a caller-provided -// sink (the module wires its tf output port there). Standard library only — rotation -// algebra is a self-contained xyzw quaternion; warnings go through an injectable sink. - -#pragma once - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -namespace tf_client { - -/// How many seconds of history each edge keeps. -constexpr double DEFAULT_TF_WINDOW_SECS = 10.0; - -inline double now_secs() { - return std::chrono::duration( - std::chrono::system_clock::now().time_since_epoch()) - .count(); -} - -/// A rigid body transform: rotation as an xyzw quaternion plus a translation. -/// Self-contained so tf.hpp depends on the standard library alone. -struct Rigid { - std::array rotation{0.0, 0.0, 0.0, 1.0}; // xyzw - std::array translation{0.0, 0.0, 0.0}; - - static std::array quat_multiply(const std::array& left, - const std::array& right) { - return { - left[3] * right[0] + left[0] * right[3] + left[1] * right[2] - left[2] * right[1], - left[3] * right[1] - left[0] * right[2] + left[1] * right[3] + left[2] * right[0], - left[3] * right[2] + left[0] * right[1] - left[1] * right[0] + left[2] * right[3], - left[3] * right[3] - left[0] * right[0] - left[1] * right[1] - left[2] * right[2]}; - } - - static std::array quat_rotate(const std::array& rotation, - const std::array& vector) { - const std::array axis{rotation[0], rotation[1], rotation[2]}; - std::array inner{axis[1] * vector[2] - axis[2] * vector[1], - axis[2] * vector[0] - axis[0] * vector[2], - axis[0] * vector[1] - axis[1] * vector[0]}; - for (int component = 0; component < 3; ++component) { - inner[component] += rotation[3] * vector[component]; - } - const std::array outer{axis[1] * inner[2] - axis[2] * inner[1], - axis[2] * inner[0] - axis[0] * inner[2], - axis[0] * inner[1] - axis[1] * inner[0]}; - return {vector[0] + 2.0 * outer[0], vector[1] + 2.0 * outer[1], - vector[2] + 2.0 * outer[2]}; - } - - Rigid operator*(const Rigid& other) const { - const std::array rotated = quat_rotate(rotation, other.translation); - return Rigid{quat_multiply(rotation, other.rotation), - {translation[0] + rotated[0], translation[1] + rotated[1], - translation[2] + rotated[2]}}; - } - - Rigid inverse() const { - const std::array conjugate{-rotation[0], -rotation[1], -rotation[2], - rotation[3]}; - const std::array rotated = quat_rotate(conjugate, translation); - return Rigid{conjugate, {-rotated[0], -rotated[1], -rotated[2]}}; - } -}; - -/// A rigid transform from `parent` to `child` at a point in time. It maps a point -/// expressed in `child` coordinates into `parent` coordinates. -struct Transform { - std::string parent; - std::string child; - double ts = 0.0; - Rigid rigid; - - const std::array& translation() const { return rigid.translation; } - const std::array& rotation() const { return rigid.rotation; } - - Transform inverse() const { return Transform{child, parent, ts, rigid.inverse()}; } - - Transform compose(const Transform& other) const { - return Transform{parent, other.child, ts, rigid * other.rigid}; - } -}; - -/// One edge's time-sorted history, capped to a fixed-duration window. Public so its -/// windowing behaviour is directly testable, like the rust module's own tests do. -class TBuffer { -public: - struct Sample { - double ts; - Rigid rigid; - }; - - explicit TBuffer(double window_secs) : window_secs_(window_secs) {} - - void add(double ts, const Rigid& rigid) { - // A stamp a whole window behind the newest is a clock reset, not jitter. - if (!samples_.empty() && ts < samples_.back().ts - window_secs_) { - samples_.clear(); - } - const auto position = std::upper_bound( - samples_.begin(), samples_.end(), ts, - [](double value, const Sample& sample) { return value < sample.ts; }); - samples_.insert(position, Sample{ts, rigid}); - // Anchored to the newest sample so a late message cannot widen the window. - const double newest = samples_.back().ts; - prune(newest - window_secs_); - } - - const std::deque& samples() const { return samples_; } - - const Sample* last() const { return samples_.empty() ? nullptr : &samples_.back(); } - - /// Nearest sample in time. On a tie, prefer the later sample. Null when the - /// closest sample is further than tolerance from ts. - const Sample* find_closest(double ts, std::optional tolerance) const { - const auto position = std::lower_bound( - samples_.begin(), samples_.end(), ts, - [](const Sample& sample, double value) { return sample.ts < value; }); - const Sample* next = position == samples_.end() ? nullptr : &*position; - const Sample* previous = position == samples_.begin() ? nullptr : &*(position - 1); - const Sample* best = nullptr; - if (previous != nullptr && next != nullptr) { - best = std::abs(next->ts - ts) <= std::abs(ts - previous->ts) ? next : previous; - } else if (previous != nullptr) { - best = previous; - } else if (next != nullptr) { - best = next; - } else { - return nullptr; - } - if (tolerance.has_value() && std::abs(best->ts - ts) > *tolerance) { - return nullptr; - } - return best; - } - - std::optional sample(const std::string& parent, const std::string& child, - std::optional time, - std::optional tolerance) const { - const Sample* found = nullptr; - if (!time.has_value()) { - found = last(); - } else { - found = find_closest(*time, tolerance.value_or(window_secs_)); - } - if (found == nullptr) { - return std::nullopt; - } - return Transform{parent, child, found->ts, found->rigid}; - } - -private: - void prune(double min_ts) { - while (!samples_.empty() && samples_.front().ts < min_ts) { - samples_.pop_front(); - } - } - - double window_secs_; - std::deque samples_; -}; - -/// A cheap handle for querying and publishing transforms. -/// -/// Mirrors the rust Tf: `lookup(parent, child)` starts a builder refined with `at()` -/// and `tolerance()` and finished with `get()` or a blocking `within()`; -/// `get_latest()` is the no-refinement shorthand; `receive()` ingests wire traffic; -/// `publish()` stores and forwards through the publish sink. Thread-safe. -class Tf { -public: - /// Receives everything publish() sends, e.g. to encode onto the module's tf port. - using PublishSink = std::function&)>; - /// Receives unresolved-lookup warnings (throttled to one per second per pair). - using WarnSink = std::function; - - explicit Tf(double window_secs = DEFAULT_TF_WINDOW_SECS) : window_secs_(window_secs) {} - - void set_publish_sink(PublishSink sink) { - const std::lock_guard guard(mutex_); - publish_sink_ = std::move(sink); - } - - void set_warn_sink(WarnSink sink) { - const std::lock_guard guard(mutex_); - warn_sink_ = std::move(sink); - } - - /// Ingest one wire edge, rotation as xyzw. A zero or non-finite rotation would - /// normalize to NaN and poison every lookup through it, so it is dropped instead - /// of stored. - void receive(const std::string& parent, const std::string& child, double ts, - const std::array& rotation_xyzw, - const std::array& translation) { - double norm_squared = 0.0; - bool finite = true; - for (double component : rotation_xyzw) { - norm_squared += component * component; - finite = finite && std::isfinite(component); - } - for (double component : translation) { - finite = finite && std::isfinite(component); - } - if (!finite || norm_squared < 1e-24) { - return; - } - const double norm = std::sqrt(norm_squared); - Rigid rigid{{rotation_xyzw[0] / norm, rotation_xyzw[1] / norm, - rotation_xyzw[2] / norm, rotation_xyzw[3] / norm}, - translation}; - { - const std::lock_guard guard(mutex_); - edge_buffer(parent, child).add(ts, rigid); - } - changed_.notify_all(); - } - - /// The latest transform from `parent` to `child`; shorthand for lookup().get(). - std::optional get_latest(const std::string& parent, const std::string& child); - - class Lookup; - /// Start a lookup of the transform from `parent` to `child`. Refine it with - /// Lookup::at and Lookup::tolerance, then finish with Lookup::get. - Lookup lookup(const std::string& parent, const std::string& child); - - /// Publish transforms: they feed the local graph, so a lookup right after sees - /// them without waiting for a transport round trip, and go to the publish sink. - void publish(const std::vector& transforms) { - PublishSink sink; - { - const std::lock_guard guard(mutex_); - for (const Transform& transform : transforms) { - edge_buffer(transform.parent, transform.child).add(transform.ts, transform.rigid); - } - sink = publish_sink_; - } - changed_.notify_all(); - if (sink) { - sink(transforms); - } - } - -private: - friend class Lookup; - - TBuffer& edge_buffer(const std::string& parent, const std::string& child) { - const auto key = std::make_pair(parent, child); - auto found = buffers_.find(key); - if (found == buffers_.end()) { - found = buffers_.emplace(key, TBuffer(window_secs_)).first; - } - return found->second; - } - - std::vector connections(const std::string& frame) const { - std::vector out; - for (const auto& [key, buffer] : buffers_) { - if (key.first == frame) { - out.push_back(key.second); - } - if (key.second == frame) { - out.push_back(key.first); - } - } - return out; - } - - std::optional edge(const std::string& parent, const std::string& child, - std::optional time, - std::optional tolerance) const { - if (parent == child) { - return Transform{parent, child, time.value_or(now_secs()), Rigid{}}; - } - auto direct = buffers_.find(std::make_pair(parent, child)); - if (direct != buffers_.end()) { - return direct->second.sample(parent, child, time, tolerance); - } - auto reversed = buffers_.find(std::make_pair(child, parent)); - if (reversed != buffers_.end()) { - auto sample = reversed->second.sample(child, parent, time, tolerance); - if (sample.has_value()) { - return sample->inverse(); - } - } - return std::nullopt; - } - - std::optional> bfs(const std::string& parent, - const std::string& child, - std::optional time, - std::optional tolerance) const { - std::deque>> queue{{parent, {}}}; - std::set visited{parent}; - while (!queue.empty()) { - auto [frame, path] = std::move(queue.front()); - queue.pop_front(); - if (frame == child) { - return path; - } - for (const std::string& next : connections(frame)) { - if (visited.count(next) != 0) { - continue; - } - const auto step = edge(frame, next, time, tolerance); - if (!step.has_value()) { - continue; - } - visited.insert(next); - std::vector extended = path; - extended.push_back(*step); - queue.emplace_back(next, std::move(extended)); - } - } - return std::nullopt; - } - - std::optional resolve_locked(const std::string& parent, - const std::string& child, - std::optional time, - std::optional tolerance) const { - const auto direct = edge(parent, child, time, tolerance); - if (direct.has_value()) { - return direct; - } - const auto path = bfs(parent, child, time, tolerance); - if (!path.has_value() || path->empty()) { - return std::nullopt; - } - // A composition is only as fresh as its stalest edge. - double oldest = path->front().ts; - Transform composed = path->front(); - for (std::size_t i = 1; i < path->size(); ++i) { - oldest = std::min(oldest, (*path)[i].ts); - composed = composed.compose((*path)[i]); - } - composed.ts = oldest; - return composed; - } - - /// Once per second per pair, so one noisy lookup cannot mute another pair's miss. - bool should_warn(const std::string& parent, const std::string& child) { - const auto now = std::chrono::steady_clock::now(); - auto& last = warned_[std::make_pair(parent, child)]; - if (last.has_value() && now - *last < std::chrono::seconds(1)) { - return false; - } - last = now; - return true; - } - - void warn_unresolved(const std::string& parent, const std::string& child, - std::optional time, std::optional tolerance) { - WarnSink sink; - { - const std::lock_guard guard(mutex_); - if (!should_warn(parent, child)) { - return; - } - sink = warn_sink_; - } - if (sink) { - sink("No transform found between frames '" + parent + "' and '" + child + - "' at " + std::to_string(time.value_or(now_secs())) + " tolerance " + - (tolerance.has_value() ? std::to_string(*tolerance) : "none")); - } - } - - double window_secs_; - mutable std::mutex mutex_; - std::condition_variable changed_; - std::map, TBuffer> buffers_; - std::map, - std::optional> - warned_; - PublishSink publish_sink_; - WarnSink warn_sink_; -}; - -/// A transform lookup being built. Created by Tf::lookup. -class Tf::Lookup { -public: - /// Take the sample nearest `time` rather than the latest one. - Lookup& at(double time) { - time_ = time; - return *this; - } - - /// Bound how far, in seconds, the chosen sample may sit from at(). - Lookup& tolerance(double tolerance) { - tolerance_ = tolerance; - return *this; - } - - /// Resolve against the transforms buffered so far. Nullopt when no path connects - /// the frames, or when the nearest sample is outside the tolerance. - std::optional get() { - std::optional found; - { - const std::lock_guard guard(tf_.mutex_); - found = tf_.resolve_locked(parent_, child_, time_, tolerance_); - } - if (!found.has_value()) { - tf_.warn_unresolved(parent_, child_, time_, tolerance_); - } - return found; - } - - /// Resolve the lookup, blocking up to `timeout` for a late transform. Returns as - /// soon as the lookup succeeds, or nullopt at the deadline. Calling this on the - /// module's dispatch thread parks the whole dispatch loop; prefer get() there. - std::optional within(std::chrono::steady_clock::duration timeout) { - const auto deadline = std::chrono::steady_clock::now() + timeout; - std::unique_lock lock(tf_.mutex_); - while (true) { - auto found = tf_.resolve_locked(parent_, child_, time_, tolerance_); - if (found.has_value()) { - return found; - } - // Only the deadline warns. An intermediate miss is the normal state of a - // wait, not a failure. - if (tf_.changed_.wait_until(lock, deadline) == std::cv_status::timeout) { - auto last = tf_.resolve_locked(parent_, child_, time_, tolerance_); - lock.unlock(); - if (!last.has_value()) { - tf_.warn_unresolved(parent_, child_, time_, tolerance_); - } - return last; - } - } - } - -private: - friend class Tf; - Lookup(Tf& tf, std::string parent, std::string child) - : tf_(tf), parent_(std::move(parent)), child_(std::move(child)) {} - - Tf& tf_; - std::string parent_; - std::string child_; - std::optional time_; - std::optional tolerance_; -}; - -inline Tf::Lookup Tf::lookup(const std::string& parent, const std::string& child) { - return Lookup(*this, parent, child); -} - -inline std::optional Tf::get_latest(const std::string& parent, - const std::string& child) { - return lookup(parent, child).get(); -} - -} // namespace tf_client diff --git a/src/vendor/sensor_msgs/ImuInfo.hpp b/src/vendor/sensor_msgs/ImuInfo.hpp deleted file mode 100644 index a55c6f7..0000000 --- a/src/vendor/sensor_msgs/ImuInfo.hpp +++ /dev/null @@ -1,202 +0,0 @@ -/** THIS IS AN AUTOMATICALLY GENERATED FILE. DO NOT MODIFY - * BY HAND!! - * - * Generated by lcm-gen 1.5.2 - **/ - -#ifndef __sensor_msgs_ImuInfo_hpp__ -#define __sensor_msgs_ImuInfo_hpp__ - -#include - -#include "std_msgs/Header.hpp" - -namespace sensor_msgs -{ - -class ImuInfo -{ - public: - std_msgs::Header header; - - double gyro_noise_density; - - double gyro_random_walk; - - double accel_noise_density; - - double accel_random_walk; - - double frequency; - - public: - /** - * Encode a message into binary form. - * - * @param buf The output buffer. - * @param offset Encoding starts at thie byte offset into @p buf. - * @param maxlen Maximum number of bytes to write. This should generally be - * equal to getEncodedSize(). - * @return The number of bytes encoded, or <0 on error. - */ - inline int encode(void *buf, int offset, int maxlen) const; - - /** - * Check how many bytes are required to encode this message. - */ - inline int getEncodedSize() const; - - /** - * Decode a message from binary form into this instance. - * - * @param buf The buffer containing the encoded message. - * @param offset The byte offset into @p buf where the encoded message starts. - * @param maxlen The maximum number of bytes to read while decoding. - * @return The number of bytes decoded, or <0 if an error occured. - */ - inline int decode(const void *buf, int offset, int maxlen); - - /** - * Retrieve the 64-bit fingerprint identifying the structure of the message. - * Note that the fingerprint is the same for all instances of the same - * message type, and is a fingerprint on the message type definition, not on - * the message contents. - */ - inline static int64_t getHash(); - - /** - * Returns "ImuInfo" - */ - inline static const char* getTypeName(); - - // LCM support functions. Users should not call these - inline int _encodeNoHash(void *buf, int offset, int maxlen) const; - inline int _getEncodedSizeNoHash() const; - inline int _decodeNoHash(const void *buf, int offset, int maxlen); - inline static uint64_t _computeHash(const __lcm_hash_ptr *p); -}; - -int ImuInfo::encode(void *buf, int offset, int maxlen) const -{ - int pos = 0, tlen; - int64_t hash = getHash(); - - tlen = __int64_t_encode_array(buf, offset + pos, maxlen - pos, &hash, 1); - if(tlen < 0) return tlen; else pos += tlen; - - tlen = this->_encodeNoHash(buf, offset + pos, maxlen - pos); - if (tlen < 0) return tlen; else pos += tlen; - - return pos; -} - -int ImuInfo::decode(const void *buf, int offset, int maxlen) -{ - int pos = 0, thislen; - - int64_t msg_hash; - thislen = __int64_t_decode_array(buf, offset + pos, maxlen - pos, &msg_hash, 1); - if (thislen < 0) return thislen; else pos += thislen; - if (msg_hash != getHash()) return -1; - - thislen = this->_decodeNoHash(buf, offset + pos, maxlen - pos); - if (thislen < 0) return thislen; else pos += thislen; - - return pos; -} - -int ImuInfo::getEncodedSize() const -{ - return 8 + _getEncodedSizeNoHash(); -} - -int64_t ImuInfo::getHash() -{ - static int64_t hash = static_cast(_computeHash(NULL)); - return hash; -} - -const char* ImuInfo::getTypeName() -{ - return "ImuInfo"; -} - -int ImuInfo::_encodeNoHash(void *buf, int offset, int maxlen) const -{ - int pos = 0, tlen; - - tlen = this->header._encodeNoHash(buf, offset + pos, maxlen - pos); - if(tlen < 0) return tlen; else pos += tlen; - - tlen = __double_encode_array(buf, offset + pos, maxlen - pos, &this->gyro_noise_density, 1); - if(tlen < 0) return tlen; else pos += tlen; - - tlen = __double_encode_array(buf, offset + pos, maxlen - pos, &this->gyro_random_walk, 1); - if(tlen < 0) return tlen; else pos += tlen; - - tlen = __double_encode_array(buf, offset + pos, maxlen - pos, &this->accel_noise_density, 1); - if(tlen < 0) return tlen; else pos += tlen; - - tlen = __double_encode_array(buf, offset + pos, maxlen - pos, &this->accel_random_walk, 1); - if(tlen < 0) return tlen; else pos += tlen; - - tlen = __double_encode_array(buf, offset + pos, maxlen - pos, &this->frequency, 1); - if(tlen < 0) return tlen; else pos += tlen; - - return pos; -} - -int ImuInfo::_decodeNoHash(const void *buf, int offset, int maxlen) -{ - int pos = 0, tlen; - - tlen = this->header._decodeNoHash(buf, offset + pos, maxlen - pos); - if(tlen < 0) return tlen; else pos += tlen; - - tlen = __double_decode_array(buf, offset + pos, maxlen - pos, &this->gyro_noise_density, 1); - if(tlen < 0) return tlen; else pos += tlen; - - tlen = __double_decode_array(buf, offset + pos, maxlen - pos, &this->gyro_random_walk, 1); - if(tlen < 0) return tlen; else pos += tlen; - - tlen = __double_decode_array(buf, offset + pos, maxlen - pos, &this->accel_noise_density, 1); - if(tlen < 0) return tlen; else pos += tlen; - - tlen = __double_decode_array(buf, offset + pos, maxlen - pos, &this->accel_random_walk, 1); - if(tlen < 0) return tlen; else pos += tlen; - - tlen = __double_decode_array(buf, offset + pos, maxlen - pos, &this->frequency, 1); - if(tlen < 0) return tlen; else pos += tlen; - - return pos; -} - -int ImuInfo::_getEncodedSizeNoHash() const -{ - int enc_size = 0; - enc_size += this->header._getEncodedSizeNoHash(); - enc_size += __double_encoded_array_size(NULL, 1); - enc_size += __double_encoded_array_size(NULL, 1); - enc_size += __double_encoded_array_size(NULL, 1); - enc_size += __double_encoded_array_size(NULL, 1); - enc_size += __double_encoded_array_size(NULL, 1); - return enc_size; -} - -uint64_t ImuInfo::_computeHash(const __lcm_hash_ptr *p) -{ - const __lcm_hash_ptr *fp; - for(fp = p; fp != NULL; fp = fp->parent) - if(fp->v == ImuInfo::getHash) - return 0; - const __lcm_hash_ptr cp = { p, ImuInfo::getHash }; - - uint64_t hash = 0x437a196cbd6b4e49LL + - std_msgs::Header::_computeHash(&cp); - - return (hash<<1) + ((hash>>63)&1); -} - -} - -#endif