From 4a41339923e08d2bb5ec6d079d558c43c6de2f94 Mon Sep 17 00:00:00 2001 From: u5-4 Date: Mon, 20 Jul 2026 10:15:43 +0800 Subject: [PATCH 1/4] [fix] Patch Isaac ROS 3.2 IMU timestamps Keep the fix out of the cuVSLAM core and pin it to the exact NVIDIA wrapper and NITROS package revisions. Add a verifier and documented apply, rebuild, update, and rollback boundaries. Signed-off-by: u5-4 --- .../isaac_ros_3_2_yopo/COMPATIBILITY.md | 40 ++++ integrations/isaac_ros_3_2_yopo/README.md | 204 ++++++++++++++++++ ...os_visual_slam_v3_2_15_imu_timestamp.patch | 23 ++ .../scripts/verify_visual_slam_patch.sh | 153 +++++++++++++ 4 files changed, 420 insertions(+) create mode 100644 integrations/isaac_ros_3_2_yopo/COMPATIBILITY.md create mode 100644 integrations/isaac_ros_3_2_yopo/README.md create mode 100644 integrations/isaac_ros_3_2_yopo/patches/isaac_ros_visual_slam_v3_2_15_imu_timestamp.patch create mode 100755 integrations/isaac_ros_3_2_yopo/scripts/verify_visual_slam_patch.sh diff --git a/integrations/isaac_ros_3_2_yopo/COMPATIBILITY.md b/integrations/isaac_ros_3_2_yopo/COMPATIBILITY.md new file mode 100644 index 0000000..8c9d5f2 --- /dev/null +++ b/integrations/isaac_ros_3_2_yopo/COMPATIBILITY.md @@ -0,0 +1,40 @@ +# Compatibility contract + +This adapter is intentionally pinned. Do not apply it to another Isaac ROS +Visual SLAM revision without reviewing the upstream wrapper again. + +| Component | Required value | +| --- | --- | +| NVIDIA repository | `NVIDIA-ISAAC-ROS/isaac_ros_visual_slam` | +| NVIDIA tag | `v3.2-15` | +| NVIDIA commit | `e31f4cc1d41a329a01946e5fe63669f8b15da677` | +| ROS distribution | ROS 2 Humble | +| Patched package | `isaac_ros_visual_slam` | +| NITROS package | `ros-humble-isaac-ros-nitros=3.2.5-0jammy` | +| SDK interface checked | `CUVSLAM_RegisterImuMeasurement` from the Isaac ROS 3.2 NITROS cuVSLAM header | + +NVIDIA tags `v3.2-14` and `v3.2-15` may both describe the same commit in a +shallow checkout. This integration uses the `v3.2-15` release label and treats +the full commit ID above as the authoritative compatibility check. + +## Ownership boundary + +- The patch changes only + `isaac_ros_visual_slam/src/impl/visual_slam_impl.cpp` in NVIDIA's wrapper. +- NVIDIA's packaged `libcuvslam.so` remains the runtime implementation. +- No library under this fork's `libs/` tree is built or linked. +- `core17` is not built, linked, copied, or redistributed by this adapter. +- The NVIDIA SDK binary and header are not vendored here. +- The verifier checks the installed Debian package version, its file-integrity + metadata, and the required timestamp contract in `cuvslam.h` before applying + the source patch. It does not claim behavioral IMU-fusion coverage. + +## Revalidation triggers + +Repeat the source audit and runtime validation before using the adapter with: + +- a different Isaac ROS Visual SLAM commit; +- a different NITROS/cuVSLAM SDK package; +- a different timestamp clock or FCU bridge; +- a different camera/FCU-IMU frame convention; +- a new camera-to-FCU-IMU calibration or time-offset estimate. diff --git a/integrations/isaac_ros_3_2_yopo/README.md b/integrations/isaac_ros_3_2_yopo/README.md new file mode 100644 index 0000000..9c483c3 --- /dev/null +++ b/integrations/isaac_ros_3_2_yopo/README.md @@ -0,0 +1,204 @@ +# Isaac ROS 3.2 external-IMU wrapper patch + +This directory carries an out-of-tree patch for the Isaac ROS Visual SLAM +wrapper used by the YOPO integration. It does not modify, build, or link the +cuVSLAM implementation in this repository. In particular, `core17` is not part +of this integration. + +"Out-of-tree" describes ownership of the patch: the patch is maintained in +this fork instead of NVIDIA's repository. Applying it creates one reviewed +tracked modification in the NVIDIA checkout. That modification is the approved +exception to the otherwise read-only NVIDIA-source policy; no commit is made in +the NVIDIA repository. + +The runtime remains NVIDIA's `libcuvslam.so` distributed with Isaac ROS 3.2. +Only the open-source ROS wrapper at the exact revision listed in +[`COMPATIBILITY.md`](COMPATIBILITY.md) is patched. + +## Why this patch is required + +Isaac ROS Visual SLAM 3.2 receives IMU and image timestamps in nanoseconds, but +the unpatched wrapper has two independent timestamp defects: + +1. It computes each IMU message's timestamp (`imu_ts`) and then passes the + current image timestamp (`latest_ts`) to + `CUVSLAM_RegisterImuMeasurement`. When several IMU samples arrive between + two image frames, they are registered with the same timestamp instead of + their own strictly increasing timestamps. +2. `MessageStreamSequencer` receives nanosecond timestamps, while its image and + IMU jitter thresholds are ROS parameters expressed in milliseconds. The + unpatched constructor passes those thresholds without converting units. + +The patch therefore passes `imu_ts` to the SDK and converts both sequencer +thresholds from milliseconds to nanoseconds. Both changes are required to +remove the two wrapper blockers, but they do not by themselves establish a +complete external-IMU data path. + +The SDK contract was verified from the Isaac ROS 3.2 container header at: + +```text +/opt/ros/humble/share/isaac_ros_nitros/cuvslam/include/cuvslam.h +``` + +That header requires the timestamp passed to +`CUVSLAM_RegisterImuMeasurement` to be in nanoseconds and to always increase. + +## Obtain only this integration in the container + +Run these commands inside the Isaac ROS development container: + +```bash +cd /workspaces/isaac_ros-dev/src + +git clone \ + --depth 1 \ + --branch u5-4/isaac-ros-3.2-yopo-adapter \ + --filter=blob:none \ + --sparse \ + https://github.com/u5-4/cuVSLAM.git \ + cuvslam-yopo-adapter + +git -C cuvslam-yopo-adapter sparse-checkout set \ + integrations/isaac_ros_3_2_yopo +``` + +The three exported paths below are shell-local. Set them again after opening a new +container terminal: + +```bash +export VSLAM_SOURCE=/workspaces/isaac_ros-dev/src/isaac_ros_visual_slam +export YOPO_ADAPTER=/workspaces/isaac_ros-dev/src/cuvslam-yopo-adapter/integrations/isaac_ros_3_2_yopo +export VSLAM_PATCH="$YOPO_ADAPTER/patches/isaac_ros_visual_slam_v3_2_15_imu_timestamp.patch" +``` + +## Verify and apply + +The verification script requires a clean NVIDIA checkout. It applies the patch, +checks both corrections, verifies the installed NITROS SDK package/header, and +reverses the patch before it exits: + +```bash +"$YOPO_ADAPTER/scripts/verify_visual_slam_patch.sh" "$VSLAM_SOURCE" +``` + +`--source-only` skips the installed SDK check and exists only for offline patch +development. Do not use it for Jetson acceptance. + +Apply the patch for the actual build: + +```bash +test -f /workspaces/isaac_ros-dev/install/setup.bash + +# Stop the running Visual SLAM launch before changing or rebuilding its library. +git -C "$VSLAM_SOURCE" apply --check \ + "$VSLAM_PATCH" + +git -C "$VSLAM_SOURCE" apply \ + "$VSLAM_PATCH" +``` + +Confirm that the expected patch is the only tracked source change, then rebuild +only the affected ROS package. This package-select build assumes the existing +Isaac ROS workspace overlay has already been built successfully: + +```bash +cd /workspaces/isaac_ros-dev +set +u +set -eo pipefail +source /opt/ros/humble/setup.bash +source /workspaces/isaac_ros-dev/install/setup.bash + +git -C "$VSLAM_SOURCE" diff --check +test "$(git -C "$VSLAM_SOURCE" status --short --untracked-files=no)" = \ + " M isaac_ros_visual_slam/src/impl/visual_slam_impl.cpp" +git -C "$VSLAM_SOURCE" apply --reverse --check "$VSLAM_PATCH" + +colcon build \ + --packages-select isaac_ros_visual_slam \ + --cmake-args -DBUILD_TESTING=OFF + +source /workspaces/isaac_ros-dev/install/setup.bash +test "$(ros2 pkg prefix isaac_ros_visual_slam)" = \ + /workspaces/isaac_ros-dev/install/isaac_ros_visual_slam +``` + +Restart the Visual SLAM launch after the build and verify that its package prefix +resolves to `/workspaces/isaac_ros-dev/install/isaac_ros_visual_slam`. + +The clone, patch, build, and `install/` output are in the host bind-mounted +workspace. They persist on the host, but they are not embedded in +`isaac_ros_dev-aarch64:latest`; a production image is a separate later task. + +## Update this integration + +Never pull a new adapter revision while its current patch is applied. Stop +Visual SLAM, reverse the current patch with the current patch file, pull, run +the verifier, apply the new patch, rebuild, and restart: + +```bash +git -C "$VSLAM_SOURCE" apply --reverse --check "$VSLAM_PATCH" +git -C "$VSLAM_SOURCE" apply --reverse "$VSLAM_PATCH" + +git -C /workspaces/isaac_ros-dev/src/cuvslam-yopo-adapter pull --ff-only + +"$YOPO_ADAPTER/scripts/verify_visual_slam_patch.sh" "$VSLAM_SOURCE" +git -C "$VSLAM_SOURCE" apply --check "$VSLAM_PATCH" +git -C "$VSLAM_SOURCE" apply "$VSLAM_PATCH" + +cd /workspaces/isaac_ros-dev +set +u +set -eo pipefail +source /opt/ros/humble/setup.bash +source /workspaces/isaac_ros-dev/install/setup.bash +colcon build --packages-select isaac_ros_visual_slam --cmake-args -DBUILD_TESTING=OFF +source /workspaces/isaac_ros-dev/install/setup.bash +``` + +## Roll back + +Stop Visual SLAM first. Reversing the source patch does not replace an already +installed or already loaded library, so rebuild and restart are mandatory: + +```bash +git -C "$VSLAM_SOURCE" apply --reverse --check "$VSLAM_PATCH" +git -C "$VSLAM_SOURCE" apply --reverse "$VSLAM_PATCH" + +cd /workspaces/isaac_ros-dev +set +u +set -eo pipefail +source /opt/ros/humble/setup.bash +source /workspaces/isaac_ros-dev/install/setup.bash +colcon build --packages-select isaac_ros_visual_slam --cmake-args -DBUILD_TESTING=OFF +source /workspaces/isaac_ros-dev/install/setup.bash +``` + +Restart Visual SLAM only after this build succeeds. + +## Validation boundary + +A successful build proves only that the wrapper compiles and links. Before +external FCU IMU fusion is accepted, validate at runtime that: + +- the FCU IMU topic uses the intended clock and strictly increasing timestamps; +- duplicate and non-monotonic FCU timestamps are rejected and counted by the + future adapter health gate instead of being forwarded to this wrapper; +- the Visual SLAM node has exactly the intended FCU `sensor_msgs/msg/Imu` + subscription/remap and is not still consuming `/camera/imu` from the D435i; +- the calibrated constant camera/FCU clock offset is applied with its documented + sign convention before SDK registration; +- all IMU samples use the expected axes and the calibrated transform direction + is converted into the documented ROS TF chain; for the selected rectified + stereo input, the Kalibr result is treated as `T_Crect0_I`, not as a raw + optical-camera transform; +- noise density and random-walk values come from the selected FCU IMU, with the + units expected by the Isaac ROS parameters, rather than D435i/default values; +- Visual SLAM logs do not report failed IMU registrations; +- tracking and timestamp checks pass on a new recording made after this patch. + +The earlier 10-minute visual tracking test used the D435i onboard IMU and +predates this patch. It does not prove that FCU IMU measurements are routed, +calibrated, or registered with correct timestamps. + +The included verifier is a source and SDK-contract check, not a behavioral +fusion test. Production acceptance still requires the rebuilt Jetson package +and a new FCU-IMU rosbag/runtime validation. diff --git a/integrations/isaac_ros_3_2_yopo/patches/isaac_ros_visual_slam_v3_2_15_imu_timestamp.patch b/integrations/isaac_ros_3_2_yopo/patches/isaac_ros_visual_slam_v3_2_15_imu_timestamp.patch new file mode 100644 index 0000000..f388b30 --- /dev/null +++ b/integrations/isaac_ros_3_2_yopo/patches/isaac_ros_visual_slam_v3_2_15_imu_timestamp.patch @@ -0,0 +1,23 @@ +diff --git a/isaac_ros_visual_slam/src/impl/visual_slam_impl.cpp b/isaac_ros_visual_slam/src/impl/visual_slam_impl.cpp +--- a/isaac_ros_visual_slam/src/impl/visual_slam_impl.cpp ++++ b/isaac_ros_visual_slam/src/impl/visual_slam_impl.cpp +@@ -99,8 +99,8 @@ VisualSlamNode::VisualSlamImpl::VisualSlamImpl(VisualSlamNode & vslam_node) + : node(vslam_node), + sync(node.num_cameras_, 1e6 * node.sync_matching_threshold_ms_, node.min_num_images_, + node.image_buffer_size_), +- sequencer(node.imu_buffer_size_, node.imu_jitter_threshold_ms_, node.image_buffer_size_, +- node.image_jitter_threshold_ms_), ++ sequencer(node.imu_buffer_size_, 1e6 * node.imu_jitter_threshold_ms_, ++ node.image_buffer_size_, 1e6 * node.image_jitter_threshold_ms_), + tf_buffer(std::make_unique(node.get_clock())), + tf_listener(std::make_unique(*tf_buffer)), + tf_publisher(std::make_unique(&node)), +@@ -685,7 +685,7 @@ void VisualSlamNode::VisualSlamImpl::UpdatePose( + int64_t imu_ts = static_cast(timestamp.nanoseconds()); + RCLCPP_DEBUG(node.get_logger(), "Using imu msg timestamp [%ld]", imu_ts); + const CUVSLAM_Status status = CUVSLAM_RegisterImuMeasurement( +- cuvslam_handle, latest_ts, &imu_measurement); ++ cuvslam_handle, imu_ts, &imu_measurement); + if (status != CUVSLAM_SUCCESS) { + RCLCPP_WARN(node.get_logger(), "CUVSLAM has failed to register an IMU measurement"); + } diff --git a/integrations/isaac_ros_3_2_yopo/scripts/verify_visual_slam_patch.sh b/integrations/isaac_ros_3_2_yopo/scripts/verify_visual_slam_patch.sh new file mode 100755 index 0000000..f37e1e1 --- /dev/null +++ b/integrations/isaac_ros_3_2_yopo/scripts/verify_visual_slam_patch.sh @@ -0,0 +1,153 @@ +#!/usr/bin/env bash + +set -euo pipefail + +EXPECTED_COMMIT="e31f4cc1d41a329a01946e5fe63669f8b15da677" +EXPECTED_NITROS_PACKAGE="ros-humble-isaac-ros-nitros" +EXPECTED_NITROS_VERSION="3.2.5-0jammy" +SDK_HEADER="/opt/ros/humble/share/isaac_ros_nitros/cuvslam/include/cuvslam.h" +SCRIPT_DIR="$(cd -- "$(dirname -- "${BASH_SOURCE[0]}")" && pwd)" +INTEGRATION_DIR="$(cd -- "$SCRIPT_DIR/.." && pwd)" +PATCH_FILE="$INTEGRATION_DIR/patches/isaac_ros_visual_slam_v3_2_15_imu_timestamp.patch" +SOURCE_FILE="isaac_ros_visual_slam/src/impl/visual_slam_impl.cpp" + +usage() { + echo "Usage: $0 [--source-only] /path/to/isaac_ros_visual_slam" >&2 +} + +source_only=0 +if [[ "${1:-}" == "--source-only" ]]; then + source_only=1 + shift +fi + +if [[ $# -ne 1 ]]; then + usage + exit 2 +fi + +repo_input="$(cd -- "$1" && pwd)" + +if ! repo="$(git -C "$repo_input" rev-parse --show-toplevel 2>/dev/null)"; then + echo "[STOP] Not a Git worktree: $repo_input" >&2 + exit 1 +fi + +verify_sdk_contract() { + local installed_version + local verification_output + + if ! command -v dpkg-query >/dev/null 2>&1 || ! command -v dpkg >/dev/null 2>&1; then + echo "[STOP] dpkg and dpkg-query are required for the SDK compatibility check." >&2 + exit 1 + fi + + if ! installed_version="$(dpkg-query -W -f='${Version}' "$EXPECTED_NITROS_PACKAGE" 2>/dev/null)"; then + echo "[STOP] Required package is not installed: $EXPECTED_NITROS_PACKAGE" >&2 + exit 1 + fi + + if [[ "$installed_version" != "$EXPECTED_NITROS_VERSION" ]]; then + echo "[STOP] Unsupported NITROS package version." >&2 + echo "expected=$EXPECTED_NITROS_VERSION" >&2 + echo "actual =$installed_version" >&2 + exit 1 + fi + + if [[ ! -f "$SDK_HEADER" ]]; then + echo "[STOP] cuVSLAM SDK header not found: $SDK_HEADER" >&2 + exit 1 + fi + + if ! verification_output="$(dpkg --verify "$EXPECTED_NITROS_PACKAGE" 2>&1)"; then + echo "[STOP] Unable to verify installed NITROS package files:" >&2 + echo "$verification_output" >&2 + exit 1 + fi + if [[ -n "$verification_output" ]]; then + echo "[STOP] Installed NITROS package files differ from package metadata:" >&2 + echo "$verification_output" >&2 + exit 1 + fi + + if ! grep -Fq \ + '* @param[in] timestamp timestamp is in nanoseconds and should always increment' \ + "$SDK_HEADER" || \ + ! grep -Fq \ + 'CUVSLAM_Status CUVSLAM_RegisterImuMeasurement(CUVSLAM_TrackerHandle tracker, int64_t timestamp,' \ + "$SDK_HEADER" + then + echo "[STOP] Installed cuVSLAM header does not contain the required timestamp contract." >&2 + exit 1 + fi +} + +if [[ "$source_only" -eq 0 ]]; then + verify_sdk_contract +fi + +actual_commit="$(git -C "$repo" rev-parse HEAD)" +if [[ "$actual_commit" != "$EXPECTED_COMMIT" ]]; then + echo "[STOP] Unsupported NVIDIA revision." >&2 + echo "expected=$EXPECTED_COMMIT" >&2 + echo "actual =$actual_commit" >&2 + exit 1 +fi + +if [[ -n "$(git -C "$repo" status --porcelain --untracked-files=no)" ]]; then + echo "[STOP] NVIDIA checkout has tracked changes; verification will not modify it." >&2 + exit 1 +fi + +if [[ ! -f "$PATCH_FILE" ]]; then + echo "[STOP] Patch not found: $PATCH_FILE" >&2 + exit 1 +fi + +if [[ ! -f "$repo/$SOURCE_FILE" ]]; then + echo "[STOP] Expected NVIDIA source file not found: $repo/$SOURCE_FILE" >&2 + exit 1 +fi + +patch_applied=0 +cleanup() { + if [[ "$patch_applied" -eq 1 ]]; then + git -C "$repo" apply --reverse "$PATCH_FILE" + patch_applied=0 + fi +} +trap cleanup EXIT +trap 'exit 130' INT +trap 'exit 143' TERM + +git -C "$repo" apply --check "$PATCH_FILE" +git -C "$repo" apply "$PATCH_FILE" +patch_applied=1 + +target="$repo/$SOURCE_FILE" + +grep -Fq \ + 'sequencer(node.imu_buffer_size_, 1e6 * node.imu_jitter_threshold_ms_,' \ + "$target" +grep -Fq \ + 'node.image_buffer_size_, 1e6 * node.image_jitter_threshold_ms_),' \ + "$target" +grep -Fq 'cuvslam_handle, imu_ts, &imu_measurement);' "$target" + +if grep -Fq 'cuvslam_handle, latest_ts, &imu_measurement);' "$target"; then + echo "[STOP] The incorrect image timestamp call is still present." >&2 + exit 1 +fi + +git -C "$repo" diff --check + +git -C "$repo" apply --reverse "$PATCH_FILE" +patch_applied=0 + +if [[ -n "$(git -C "$repo" status --porcelain --untracked-files=no)" ]]; then + echo "[STOP] Verification did not restore the NVIDIA checkout." >&2 + exit 1 +fi + +trap - EXIT INT TERM +echo "[PASS] Patch matches NVIDIA v3.2-15, both corrections are present, and the checkout was restored." From 9ca71902ec8d50d87390072a213f8ec211b88817 Mon Sep 17 00:00:00 2001 From: u5-4 Date: Tue, 21 Jul 2026 15:19:53 +0800 Subject: [PATCH 2/4] [feat] Add calibrated PX4 IMU cuVSLAM bringup --- .../isaac_ros_3_2_yopo/COMPATIBILITY.md | 7 +- integrations/isaac_ros_3_2_yopo/README.md | 204 ---- .../isaac_ros_3_2_yopo/README_set_up.md | 239 +++++ .../STARTUP_RUNBOOK.zh-CN.md | 969 ++++++++++++++++++ .../isaac_ros_yopo_bringup/README.md | 106 ++ .../config/d435i_243622070369_fcu_imu.yaml | 86 ++ .../config/px4_imu_noise_allan.template.yaml | 26 + .../config/px4_imu_noise_unvalidated.yaml | 24 + .../isaac_ros_yopo_bringup/__init__.py | 1 + .../aligned_imu_relay.py | 502 +++++++++ .../isaac_ros_yopo_bringup/calibration.py | 576 +++++++++++ .../runtime_health_monitor.py | 483 +++++++++ .../isaac_ros_yopo_bringup/time_alignment.py | 161 +++ .../launch/d435i_fcu_imu_cuvslam.launch.py | 379 +++++++ .../isaac_ros_yopo_bringup/package.xml | 31 + .../resource/isaac_ros_yopo_bringup | 1 + .../isaac_ros_yopo_bringup/setup.cfg | 5 + .../isaac_ros_yopo_bringup/setup.py | 41 + .../test/test_calibration.py | 295 ++++++ .../test/test_launch_source.py | 103 ++ .../test/test_relay_source.py | 122 +++ .../test_runtime_health_monitor_source.py | 170 +++ .../test/test_time_alignment.py | 190 ++++ ...os_visual_slam_v3_2_15_imu_timestamp.patch | 13 +- .../scripts/verify_visual_slam_patch.sh | 4 +- 25 files changed, 4531 insertions(+), 207 deletions(-) delete mode 100644 integrations/isaac_ros_3_2_yopo/README.md create mode 100644 integrations/isaac_ros_3_2_yopo/README_set_up.md create mode 100644 integrations/isaac_ros_3_2_yopo/STARTUP_RUNBOOK.zh-CN.md create mode 100644 integrations/isaac_ros_3_2_yopo/isaac_ros_yopo_bringup/README.md create mode 100644 integrations/isaac_ros_3_2_yopo/isaac_ros_yopo_bringup/config/d435i_243622070369_fcu_imu.yaml create mode 100644 integrations/isaac_ros_3_2_yopo/isaac_ros_yopo_bringup/config/px4_imu_noise_allan.template.yaml create mode 100644 integrations/isaac_ros_3_2_yopo/isaac_ros_yopo_bringup/config/px4_imu_noise_unvalidated.yaml create mode 100644 integrations/isaac_ros_3_2_yopo/isaac_ros_yopo_bringup/isaac_ros_yopo_bringup/__init__.py create mode 100644 integrations/isaac_ros_3_2_yopo/isaac_ros_yopo_bringup/isaac_ros_yopo_bringup/aligned_imu_relay.py create mode 100644 integrations/isaac_ros_3_2_yopo/isaac_ros_yopo_bringup/isaac_ros_yopo_bringup/calibration.py create mode 100644 integrations/isaac_ros_3_2_yopo/isaac_ros_yopo_bringup/isaac_ros_yopo_bringup/runtime_health_monitor.py create mode 100644 integrations/isaac_ros_3_2_yopo/isaac_ros_yopo_bringup/isaac_ros_yopo_bringup/time_alignment.py create mode 100644 integrations/isaac_ros_3_2_yopo/isaac_ros_yopo_bringup/launch/d435i_fcu_imu_cuvslam.launch.py create mode 100644 integrations/isaac_ros_3_2_yopo/isaac_ros_yopo_bringup/package.xml create mode 100644 integrations/isaac_ros_3_2_yopo/isaac_ros_yopo_bringup/resource/isaac_ros_yopo_bringup create mode 100644 integrations/isaac_ros_3_2_yopo/isaac_ros_yopo_bringup/setup.cfg create mode 100644 integrations/isaac_ros_3_2_yopo/isaac_ros_yopo_bringup/setup.py create mode 100644 integrations/isaac_ros_3_2_yopo/isaac_ros_yopo_bringup/test/test_calibration.py create mode 100644 integrations/isaac_ros_3_2_yopo/isaac_ros_yopo_bringup/test/test_launch_source.py create mode 100644 integrations/isaac_ros_3_2_yopo/isaac_ros_yopo_bringup/test/test_relay_source.py create mode 100644 integrations/isaac_ros_3_2_yopo/isaac_ros_yopo_bringup/test/test_runtime_health_monitor_source.py create mode 100644 integrations/isaac_ros_3_2_yopo/isaac_ros_yopo_bringup/test/test_time_alignment.py diff --git a/integrations/isaac_ros_3_2_yopo/COMPATIBILITY.md b/integrations/isaac_ros_3_2_yopo/COMPATIBILITY.md index 8c9d5f2..4fe9815 100644 --- a/integrations/isaac_ros_3_2_yopo/COMPATIBILITY.md +++ b/integrations/isaac_ros_3_2_yopo/COMPATIBILITY.md @@ -12,6 +12,8 @@ Visual SLAM revision without reviewing the upstream wrapper again. | Patched package | `isaac_ros_visual_slam` | | NITROS package | `ros-humble-isaac-ros-nitros=3.2.5-0jammy` | | SDK interface checked | `CUVSLAM_RegisterImuMeasurement` from the Isaac ROS 3.2 NITROS cuVSLAM header | +| Patched binary marker | `ISAAC_ROS_YOPO_IMU_TIMESTAMP_PATCH_V1` | +| Runtime calibration | `d435i_243622070369_factory_rectified_px4_imu_20260720` | NVIDIA tags `v3.2-14` and `v3.2-15` may both describe the same commit in a shallow checkout. This integration uses the `v3.2-15` release label and treats @@ -27,7 +29,10 @@ the full commit ID above as the authoritative compatibility check. - The NVIDIA SDK binary and header are not vendored here. - The verifier checks the installed Debian package version, its file-integrity metadata, and the required timestamp contract in `cuvslam.h` before applying - the source patch. It does not claim behavioral IMU-fusion coverage. + the source patch. The patch also embeds the marker above in + `libvisual_slam_node.so`; runtime bringup refuses an installed overlay that + lacks it. This proves the current binary was rebuilt from the marked wrapper, + but does not claim behavioral IMU-fusion coverage. ## Revalidation triggers diff --git a/integrations/isaac_ros_3_2_yopo/README.md b/integrations/isaac_ros_3_2_yopo/README.md deleted file mode 100644 index 9c483c3..0000000 --- a/integrations/isaac_ros_3_2_yopo/README.md +++ /dev/null @@ -1,204 +0,0 @@ -# Isaac ROS 3.2 external-IMU wrapper patch - -This directory carries an out-of-tree patch for the Isaac ROS Visual SLAM -wrapper used by the YOPO integration. It does not modify, build, or link the -cuVSLAM implementation in this repository. In particular, `core17` is not part -of this integration. - -"Out-of-tree" describes ownership of the patch: the patch is maintained in -this fork instead of NVIDIA's repository. Applying it creates one reviewed -tracked modification in the NVIDIA checkout. That modification is the approved -exception to the otherwise read-only NVIDIA-source policy; no commit is made in -the NVIDIA repository. - -The runtime remains NVIDIA's `libcuvslam.so` distributed with Isaac ROS 3.2. -Only the open-source ROS wrapper at the exact revision listed in -[`COMPATIBILITY.md`](COMPATIBILITY.md) is patched. - -## Why this patch is required - -Isaac ROS Visual SLAM 3.2 receives IMU and image timestamps in nanoseconds, but -the unpatched wrapper has two independent timestamp defects: - -1. It computes each IMU message's timestamp (`imu_ts`) and then passes the - current image timestamp (`latest_ts`) to - `CUVSLAM_RegisterImuMeasurement`. When several IMU samples arrive between - two image frames, they are registered with the same timestamp instead of - their own strictly increasing timestamps. -2. `MessageStreamSequencer` receives nanosecond timestamps, while its image and - IMU jitter thresholds are ROS parameters expressed in milliseconds. The - unpatched constructor passes those thresholds without converting units. - -The patch therefore passes `imu_ts` to the SDK and converts both sequencer -thresholds from milliseconds to nanoseconds. Both changes are required to -remove the two wrapper blockers, but they do not by themselves establish a -complete external-IMU data path. - -The SDK contract was verified from the Isaac ROS 3.2 container header at: - -```text -/opt/ros/humble/share/isaac_ros_nitros/cuvslam/include/cuvslam.h -``` - -That header requires the timestamp passed to -`CUVSLAM_RegisterImuMeasurement` to be in nanoseconds and to always increase. - -## Obtain only this integration in the container - -Run these commands inside the Isaac ROS development container: - -```bash -cd /workspaces/isaac_ros-dev/src - -git clone \ - --depth 1 \ - --branch u5-4/isaac-ros-3.2-yopo-adapter \ - --filter=blob:none \ - --sparse \ - https://github.com/u5-4/cuVSLAM.git \ - cuvslam-yopo-adapter - -git -C cuvslam-yopo-adapter sparse-checkout set \ - integrations/isaac_ros_3_2_yopo -``` - -The three exported paths below are shell-local. Set them again after opening a new -container terminal: - -```bash -export VSLAM_SOURCE=/workspaces/isaac_ros-dev/src/isaac_ros_visual_slam -export YOPO_ADAPTER=/workspaces/isaac_ros-dev/src/cuvslam-yopo-adapter/integrations/isaac_ros_3_2_yopo -export VSLAM_PATCH="$YOPO_ADAPTER/patches/isaac_ros_visual_slam_v3_2_15_imu_timestamp.patch" -``` - -## Verify and apply - -The verification script requires a clean NVIDIA checkout. It applies the patch, -checks both corrections, verifies the installed NITROS SDK package/header, and -reverses the patch before it exits: - -```bash -"$YOPO_ADAPTER/scripts/verify_visual_slam_patch.sh" "$VSLAM_SOURCE" -``` - -`--source-only` skips the installed SDK check and exists only for offline patch -development. Do not use it for Jetson acceptance. - -Apply the patch for the actual build: - -```bash -test -f /workspaces/isaac_ros-dev/install/setup.bash - -# Stop the running Visual SLAM launch before changing or rebuilding its library. -git -C "$VSLAM_SOURCE" apply --check \ - "$VSLAM_PATCH" - -git -C "$VSLAM_SOURCE" apply \ - "$VSLAM_PATCH" -``` - -Confirm that the expected patch is the only tracked source change, then rebuild -only the affected ROS package. This package-select build assumes the existing -Isaac ROS workspace overlay has already been built successfully: - -```bash -cd /workspaces/isaac_ros-dev -set +u -set -eo pipefail -source /opt/ros/humble/setup.bash -source /workspaces/isaac_ros-dev/install/setup.bash - -git -C "$VSLAM_SOURCE" diff --check -test "$(git -C "$VSLAM_SOURCE" status --short --untracked-files=no)" = \ - " M isaac_ros_visual_slam/src/impl/visual_slam_impl.cpp" -git -C "$VSLAM_SOURCE" apply --reverse --check "$VSLAM_PATCH" - -colcon build \ - --packages-select isaac_ros_visual_slam \ - --cmake-args -DBUILD_TESTING=OFF - -source /workspaces/isaac_ros-dev/install/setup.bash -test "$(ros2 pkg prefix isaac_ros_visual_slam)" = \ - /workspaces/isaac_ros-dev/install/isaac_ros_visual_slam -``` - -Restart the Visual SLAM launch after the build and verify that its package prefix -resolves to `/workspaces/isaac_ros-dev/install/isaac_ros_visual_slam`. - -The clone, patch, build, and `install/` output are in the host bind-mounted -workspace. They persist on the host, but they are not embedded in -`isaac_ros_dev-aarch64:latest`; a production image is a separate later task. - -## Update this integration - -Never pull a new adapter revision while its current patch is applied. Stop -Visual SLAM, reverse the current patch with the current patch file, pull, run -the verifier, apply the new patch, rebuild, and restart: - -```bash -git -C "$VSLAM_SOURCE" apply --reverse --check "$VSLAM_PATCH" -git -C "$VSLAM_SOURCE" apply --reverse "$VSLAM_PATCH" - -git -C /workspaces/isaac_ros-dev/src/cuvslam-yopo-adapter pull --ff-only - -"$YOPO_ADAPTER/scripts/verify_visual_slam_patch.sh" "$VSLAM_SOURCE" -git -C "$VSLAM_SOURCE" apply --check "$VSLAM_PATCH" -git -C "$VSLAM_SOURCE" apply "$VSLAM_PATCH" - -cd /workspaces/isaac_ros-dev -set +u -set -eo pipefail -source /opt/ros/humble/setup.bash -source /workspaces/isaac_ros-dev/install/setup.bash -colcon build --packages-select isaac_ros_visual_slam --cmake-args -DBUILD_TESTING=OFF -source /workspaces/isaac_ros-dev/install/setup.bash -``` - -## Roll back - -Stop Visual SLAM first. Reversing the source patch does not replace an already -installed or already loaded library, so rebuild and restart are mandatory: - -```bash -git -C "$VSLAM_SOURCE" apply --reverse --check "$VSLAM_PATCH" -git -C "$VSLAM_SOURCE" apply --reverse "$VSLAM_PATCH" - -cd /workspaces/isaac_ros-dev -set +u -set -eo pipefail -source /opt/ros/humble/setup.bash -source /workspaces/isaac_ros-dev/install/setup.bash -colcon build --packages-select isaac_ros_visual_slam --cmake-args -DBUILD_TESTING=OFF -source /workspaces/isaac_ros-dev/install/setup.bash -``` - -Restart Visual SLAM only after this build succeeds. - -## Validation boundary - -A successful build proves only that the wrapper compiles and links. Before -external FCU IMU fusion is accepted, validate at runtime that: - -- the FCU IMU topic uses the intended clock and strictly increasing timestamps; -- duplicate and non-monotonic FCU timestamps are rejected and counted by the - future adapter health gate instead of being forwarded to this wrapper; -- the Visual SLAM node has exactly the intended FCU `sensor_msgs/msg/Imu` - subscription/remap and is not still consuming `/camera/imu` from the D435i; -- the calibrated constant camera/FCU clock offset is applied with its documented - sign convention before SDK registration; -- all IMU samples use the expected axes and the calibrated transform direction - is converted into the documented ROS TF chain; for the selected rectified - stereo input, the Kalibr result is treated as `T_Crect0_I`, not as a raw - optical-camera transform; -- noise density and random-walk values come from the selected FCU IMU, with the - units expected by the Isaac ROS parameters, rather than D435i/default values; -- Visual SLAM logs do not report failed IMU registrations; -- tracking and timestamp checks pass on a new recording made after this patch. - -The earlier 10-minute visual tracking test used the D435i onboard IMU and -predates this patch. It does not prove that FCU IMU measurements are routed, -calibrated, or registered with correct timestamps. - -The included verifier is a source and SDK-contract check, not a behavioral -fusion test. Production acceptance still requires the rebuilt Jetson package -and a new FCU-IMU rosbag/runtime validation. diff --git a/integrations/isaac_ros_3_2_yopo/README_set_up.md b/integrations/isaac_ros_3_2_yopo/README_set_up.md new file mode 100644 index 0000000..8410e8e --- /dev/null +++ b/integrations/isaac_ros_3_2_yopo/README_set_up.md @@ -0,0 +1,239 @@ +# Isaac ROS 3.2 外部 IMU 包装层补丁 + +中文日常运行与后续扩展手册：[`STARTUP_RUNBOOK.zh-CN.md`](STARTUP_RUNBOOK.zh-CN.md) + +外部飞控 IMU 运行包：[`isaac_ros_yopo_bringup/README.md`](isaac_ros_yopo_bringup/README.md) + +本目录提供 YOPO 集成所使用的 Isaac ROS Visual SLAM 包装层树外补丁。本集成不会修改、 +构建或链接本仓库中的 cuVSLAM 实现；尤其是，`core17` 不属于本集成的一部分。 + +“树外（out-of-tree）”描述的是补丁的维护归属：该补丁维护在本 fork 中，而不是 NVIDIA +仓库中。应用补丁后，NVIDIA 源码检出目录中会产生一处已经审核的受 Git 跟踪修改。 +这是 NVIDIA 源码通常只读策略中经过批准的唯一例外；不会在 NVIDIA 仓库中创建提交。 + +运行时仍使用 Isaac ROS 3.2 随附的 NVIDIA `libcuvslam.so`。本补丁只修改 +[`COMPATIBILITY.md`](COMPATIBILITY.md) 中列出的精确版本所对应的开源 ROS 包装层。 + +## 为什么需要此补丁 + +Isaac ROS Visual SLAM 3.2 接收的 IMU 与图像时间戳以纳秒为单位，但未打补丁的包装层 +存在两个相互独立的时间戳缺陷： + +1. 包装层会计算每条 IMU 消息的时间戳（`imu_ts`），却将当前图像时间戳 + （`latest_ts`）传给 `CUVSLAM_RegisterImuMeasurement`。当两帧图像之间到达多条 + IMU 样本时，这些样本会以相同时间戳注册，而不是使用各自严格递增的时间戳。 +2. `MessageStreamSequencer` 接收纳秒时间戳，而它的图像和 IMU 抖动阈值是以毫秒表示的 + ROS 参数。未打补丁的构造函数直接传入这些阈值，没有进行单位换算。 + +因此，本补丁将 `imu_ts` 传给 SDK，并将两个序列器阈值从毫秒转换为纳秒。两项修改 +缺一不可，能够消除包装层中的两个阻塞问题，但仅凭这些修改还不能建立完整的外部 IMU +数据链路。 + +补丁还会在跟踪器成功初始化的日志中嵌入固定字符串 +`ISAAC_ROS_YOPO_IMU_TIMESTAMP_PATCH_V1`。运行时启动包会在启动前检查已安装的 +`libvisual_slam_node.so` 是否包含该 marker，并在运行期间等待同一 marker。这样既能 +拒绝未打补丁的 overlay，也能拒绝源码虽已打补丁但尚未重新构建的情况。 + +SDK 契约已依据 Isaac ROS 3.2 容器中的以下头文件完成核验： + +```text +/opt/ros/humble/share/isaac_ros_nitros/cuvslam/include/cuvslam.h +``` + +该头文件要求传给 `CUVSLAM_RegisterImuMeasurement` 的时间戳以纳秒为单位，并且必须 +始终递增。 + +## 在容器中仅获取本集成 + +在 Isaac ROS 开发容器内执行以下命令： + +```bash +cd /workspaces/isaac_ros-dev/src + +git clone \ + --depth 1 \ + --branch u5-4/fcu-imu-cuvslam-integration \ + --filter=blob:none \ + --sparse \ + https://github.com/u5-4/cuVSLAM.git \ + cuvslam-yopo-adapter + +git -C cuvslam-yopo-adapter sparse-checkout set \ + integrations/isaac_ros_3_2_yopo +``` + +以下三个导出路径仅在当前 shell 中有效。每次打开新的容器终端后都需要重新设置： + +```bash +export VSLAM_SOURCE=/workspaces/isaac_ros-dev/src/isaac_ros_visual_slam +export YOPO_ADAPTER=/workspaces/isaac_ros-dev/src/cuvslam-yopo-adapter/integrations/isaac_ros_3_2_yopo +export VSLAM_PATCH="$YOPO_ADAPTER/patches/isaac_ros_visual_slam_v3_2_15_imu_timestamp.patch" +``` + +## 验证并应用补丁 + +验证脚本要求 NVIDIA 源码检出目录处于干净状态。脚本会应用补丁、检查两项修正、核验 +已安装的 NITROS SDK 软件包及头文件，并在退出前反向撤销补丁： + +```bash +"$YOPO_ADAPTER/scripts/verify_visual_slam_patch.sh" "$VSLAM_SOURCE" +``` + +`--source-only` 会跳过已安装 SDK 的检查，仅用于离线开发补丁。Jetson 验收时不得使用 +该选项。 + +为实际构建应用补丁： + +```bash +test -f /workspaces/isaac_ros-dev/install/setup.bash + +# 修改或重新构建库之前，先停止正在运行的 Visual SLAM launch。 +git -C "$VSLAM_SOURCE" apply --check \ + "$VSLAM_PATCH" + +git -C "$VSLAM_SOURCE" apply \ + "$VSLAM_PATCH" +``` + +确认预期补丁是源码中唯一受 Git 跟踪的修改，然后仅重新构建受影响的 ROS 软件包。以下 +按软件包选择的构建命令假定现有 Isaac ROS 工作空间 overlay 已经成功完成过构建： + +```bash +cd /workspaces/isaac_ros-dev +set +u +set -eo pipefail +source /opt/ros/humble/setup.bash +source /workspaces/isaac_ros-dev/install/setup.bash + +git -C "$VSLAM_SOURCE" diff --check +test "$(git -C "$VSLAM_SOURCE" status --short --untracked-files=no)" = \ + " M isaac_ros_visual_slam/src/impl/visual_slam_impl.cpp" +git -C "$VSLAM_SOURCE" apply --reverse --check "$VSLAM_PATCH" + +colcon build \ + --packages-select isaac_ros_visual_slam \ + --cmake-args -DBUILD_TESTING=OFF + +source /workspaces/isaac_ros-dev/install/setup.bash +test "$(ros2 pkg prefix isaac_ros_visual_slam)" = \ + /workspaces/isaac_ros-dev/install/isaac_ros_visual_slam + +grep -aFq \ + ISAAC_ROS_YOPO_IMU_TIMESTAMP_PATCH_V1 \ + /workspaces/isaac_ros-dev/install/isaac_ros_visual_slam/lib/libvisual_slam_node.so +``` + +构建完成后重新启动 Visual SLAM launch，并确认其软件包前缀解析为 +`/workspaces/isaac_ros-dev/install/isaac_ros_visual_slam`。 + +确认打过补丁的包装层构建成功后，再构建运行时启动包： + +```bash +cd /workspaces/isaac_ros-dev +colcon build --packages-select isaac_ros_yopo_bringup +source /workspaces/isaac_ros-dev/install/setup.bash +ros2 pkg prefix isaac_ros_yopo_bringup +``` + +软件包前缀必须解析到 `/workspaces/isaac_ros-dev/install/` 下。运行命令以及宿主机与 +容器的执行边界记录在 +[`STARTUP_RUNBOOK.zh-CN.md`](STARTUP_RUNBOOK.zh-CN.md) 第 8 节中。 + +克隆内容、补丁、构建结果和 `install/` 输出都位于宿主机绑定挂载的工作空间内，因此会 +持久保存在宿主机上，但不会嵌入 `isaac_ros_dev-aarch64:latest` 镜像。制作生产镜像是 +后续单独的任务。 + +## 更新本集成 + +当前补丁仍应用在 NVIDIA 源码上时，绝不能拉取新版适配器。应先停止 Visual SLAM， +使用当前版本的补丁文件反向撤销当前补丁，再依次执行拉取、验证、应用新补丁、重新构建 +和重启： + +```bash +git -C "$VSLAM_SOURCE" apply --reverse --check "$VSLAM_PATCH" +git -C "$VSLAM_SOURCE" apply --reverse "$VSLAM_PATCH" + +git -C /workspaces/isaac_ros-dev/src/cuvslam-yopo-adapter pull --ff-only + +"$YOPO_ADAPTER/scripts/verify_visual_slam_patch.sh" "$VSLAM_SOURCE" +git -C "$VSLAM_SOURCE" apply --check "$VSLAM_PATCH" +git -C "$VSLAM_SOURCE" apply "$VSLAM_PATCH" + +cd /workspaces/isaac_ros-dev +set +u +set -eo pipefail +source /opt/ros/humble/setup.bash +source /workspaces/isaac_ros-dev/install/setup.bash +colcon build --packages-select isaac_ros_visual_slam --cmake-args -DBUILD_TESTING=OFF +source /workspaces/isaac_ros-dev/install/setup.bash +``` + +### 从仅含补丁的分支一次性迁移 + +运行时分支在包装层补丁中增加了二进制 marker。如果 Jetson 当前应用的是早期 +`u5-4/isaac-ros-3.2-yopo-adapter` 分支中的补丁，必须趁旧补丁文件仍在当前检出内容中， +在 fetch 或切换分支**之前**反向撤销它： + +```bash +export VSLAM_SOURCE=/workspaces/isaac_ros-dev/src/isaac_ros_visual_slam +export ADAPTER_REPO=/workspaces/isaac_ros-dev/src/cuvslam-yopo-adapter +export OLD_PATCH="$ADAPTER_REPO/integrations/isaac_ros_3_2_yopo/patches/isaac_ros_visual_slam_v3_2_15_imu_timestamp.patch" + +git -C "$VSLAM_SOURCE" apply --reverse --check "$OLD_PATCH" +git -C "$VSLAM_SOURCE" apply --reverse "$OLD_PATCH" + +git -C "$ADAPTER_REPO" fetch origin \ + u5-4/fcu-imu-cuvslam-integration +git -C "$ADAPTER_REPO" switch -c \ + u5-4/fcu-imu-cuvslam-integration \ + --track origin/u5-4/fcu-imu-cuvslam-integration +``` + +随后根据新分支重新设置 `YOPO_ADAPTER`/`VSLAM_PATCH`，运行验证脚本，应用新补丁，并 +重新构建 `isaac_ros_visual_slam` 和 `isaac_ros_yopo_bringup`。不得使用新的 marker +补丁反向撤销旧的双区块补丁；二者的补丁上下文本来就不同。 + +## 回滚 + +首先停止 Visual SLAM。反向撤销源码补丁不会替换已经安装或已经加载的库，因此必须 +重新构建并重启： + +```bash +git -C "$VSLAM_SOURCE" apply --reverse --check "$VSLAM_PATCH" +git -C "$VSLAM_SOURCE" apply --reverse "$VSLAM_PATCH" + +cd /workspaces/isaac_ros-dev +set +u +set -eo pipefail +source /opt/ros/humble/setup.bash +source /workspaces/isaac_ros-dev/install/setup.bash +colcon build --packages-select isaac_ros_visual_slam --cmake-args -DBUILD_TESTING=OFF +source /workspaces/isaac_ros-dev/install/setup.bash +``` + +只有在此次构建成功后，才能重新启动 Visual SLAM。 + +## 验收边界 + +构建成功只能证明包装层能够编译和链接。在接受外部飞控 IMU 融合方案之前，必须在 +运行时验证以下事项： + +- 飞控 IMU 话题使用预期的时钟，且时间戳严格递增； +- 重复、非单调、跨时钟域和过期的飞控时间戳会由 `aligned_fcu_imu_relay` 拒绝并计数， + 而不会转发给此包装层； +- Visual SLAM 节点恰好具有预期的飞控 `sensor_msgs/msg/Imu` 订阅或重映射，并且不再 + 使用 D435i 的 `/camera/imu`； +- SDK 注册之前，已经按照文档规定的符号约定应用标定得到的相机与飞控固定时钟偏移； +- 所有 IMU 样本均使用预期坐标轴，且标定变换方向已经转换为文档规定的 ROS TF 链； + 对于所选的校正后双目输入，Kalibr 结果应被视为 `T_Crect0_I`，而不是原始光学相机 + 变换； +- 噪声密度与随机游走数值来自选定的飞控 IMU，并通过一个可追溯的 Allan YAML 使用 + Isaac ROS 参数所要求的单位，而不是使用相互独立的 CLI 数值或 D435i/默认数值； +- Visual SLAM 日志中没有 IMU 注册失败记录； +- 使用本补丁后新录制的数据通过跟踪与时间戳检查。 + +此前 10 分钟的视觉跟踪测试使用的是 D435i 板载 IMU，且测试时间早于本补丁。该测试 +不能证明飞控 IMU 测量已经以正确时间戳完成路由、标定和注册。 + +随附的验证脚本只检查源码和 SDK 契约，并不执行融合行为测试。生产验收仍需在 Jetson +上重新构建软件包，并使用新的飞控 IMU rosbag 或运行时数据完成验证。 diff --git a/integrations/isaac_ros_3_2_yopo/STARTUP_RUNBOOK.zh-CN.md b/integrations/isaac_ros_3_2_yopo/STARTUP_RUNBOOK.zh-CN.md new file mode 100644 index 0000000..fb578f4 --- /dev/null +++ b/integrations/isaac_ros_3_2_yopo/STARTUP_RUNBOOK.zh-CN.md @@ -0,0 +1,969 @@ +# Jetson Isaac ROS / cuVSLAM / YOPO 系统启动手册 + +本文档是 Jetson 机载感知与规划链路的长期启动手册。每完成并验收一个阶段，就把该阶段的启动、验证和停止命令追加到这里，使后续使用者可以按顺序直接启动系统。 + +当前文档覆盖： + +- Isaac ROS 开发容器的首次创建、冷启动和重复进入； +- Intel RealSense D435i 正常运行节点的独立启动； +- NVIDIA Isaac ROS Visual SLAM（cuVSLAM）的独立启动； +- D435i 双红外与 PX4/MAVROS 飞控 IMU 的候选统一启动； +- 相机、IMU、cuVSLAM 输出的运行检查； +- 正确停止顺序和常见故障； +- 飞控 IMU、状态适配器和 YOPO PASSIVE 的后续章节边界。 + +本文档不是标定录包手册。正常运行时不要启动 `d435_ir_calibration.launch.py` 或 `d435_fcu_imu_record.launch.py`。 + +## 当前状态 + +最后更新：2026-07-21 + +| 模块 | 当前状态 | 本文档是否提供启动命令 | +| --- | --- | --- | +| Isaac ROS 3.2 开发容器 | 已验证 | 是 | +| D435i + 内置 IMU 正常运行源 | 官方组合链路已验证；拆分命令已按同一源码参数整理 | 是 | +| cuVSLAM + D435i 内置 IMU | 官方组合链路已验证；拆分命令需在 Jetson 再次复核 | 是 | +| cuVSLAM + PX4/MAVROS 飞控 IMU | 运行源码已完成；PX4 Allan 参数和 Jetson 联合验收待完成 | 候选命令 | +| cuVSLAM 到 `/state/odom` 适配 | 尚未完成联合验收 | 否 | +| YOPO PASSIVE | 尚未完成联合验收 | 否 | +| YOPO 控制输出 | 禁止启用 | 否 | + +固定环境： + +| 项目 | 值 | +| --- | --- | +| Jetson 工作空间 | `$HOME/workspaces/isaac_ros_3_2` | +| 容器工作空间 | `/workspaces/isaac_ros-dev` | +| ROS 版本 | ROS 2 Humble | +| Isaac ROS Visual SLAM | `v3.2-15` / `e31f4cc1d41a329a01946e5fe63669f8b15da677` | +| ROS Domain | `42` | +| D435i 序列号 | `243622070369` | +| D435i 固件 | `5.15.1.55` | +| 双目配置 | `640x360@90 Hz` | + +## 当前推荐日常启动 + +当前已经完成真机验证的主路径是 NVIDIA 官方 RealSense + cuVSLAM 组合启动： + +1. 在 `H1` 按 1.1 使用已有镜像创建并进入容器； +2. 在 `C1` 按 1.4 初始化 ROS 环境； +3. 在 `C1` 按第 2 节启动官方组合 launch，并保持终端运行； +4. 在 `H2` 按 1.2 进入同一个容器，在 `C2` 按第 5 节检查运行状态。 + +第 3、4 节提供“相机与 cuVSLAM 分终端启动”的候选命令。它们已按 NVIDIA 源码逐项核对，但必须先通过一次 Jetson 冒烟测试，才能提升为日常主路径。 + +第 8 节提供飞控 IMU 候选链路。该链路尚未取代上述日常主路径：没有 PX4 Allan 噪声结果时只能做明确标记的接线冒烟，不能据此验收导航精度。 + +## 执行位置与终端约定 + +本文统一使用以下标签。执行命令前必须先确认所在环境。 + +| 标签 | 执行位置 | 用途 | +| --- | --- | --- | +| `H1` | Jetson 宿主机终端 1 | 创建或启动容器 | +| `C1` | 由 H1 进入的主容器终端 | 启动并保持 RealSense 相机运行 | +| `H2` | Jetson 宿主机终端 2 | 进入已经运行的容器 | +| `C2` | 由 H2 进入的附加容器终端 | 启动并保持 cuVSLAM 运行 | +| `H3` | Jetson 宿主机终端 3 | 再次进入已经运行的容器 | +| `C3` | 由 H3 进入的附加容器终端 | 检查话题、频率和 RViz | + +宿主机提示符通常类似： + +```text +nvidia@tegra-ubuntu:~$ +``` + +容器提示符通常类似： + +```text +admin@tegra-ubuntu:/workspaces/isaac_ros-dev$ +``` + +> [!IMPORTANT] +> 不要在同一个代码块中混用宿主机和容器命令。看到 `admin@...:/workspaces/isaac_ros-dev$` 后，才表示已经进入容器。 + +## 安全边界 + +- 同一时刻只能运行一份 `realsense2_camera_node`。重复启动会导致 `RS2_USB_STATUS_BUSY`。 +- 本手册的正常运行链路与标定录包链路互斥。 +- 拆分启动模式下，不要再运行 `isaac_ros_visual_slam_realsense.launch.py`；该官方启动文件会再次启动一份 RealSense 驱动。 +- 所有终端必须使用相同的 `ROS_DOMAIN_ID=42`。 +- 每个新容器终端都必须重新加载 ROS 环境。 +- 在 `source /opt/ros/humble/setup.bash` 前先执行 `set +u`。 +- 当前不得启动 YOPO 控制输出、PX4 OFFBOARD、解锁或起飞命令。 + +## 1. 创建或进入 Isaac ROS 容器 + +### 1.1 使用已有镜像创建并进入新容器 + +执行位置：`H1`，Jetson 宿主机。 + +这是当前日常冷启动命令。适用情况：容器当前没有运行，并且已验证的开发镜像已经存在。 + +```bash +conda deactivate 2>/dev/null || true +unset PYTHONHOME + +export ISAAC_ROS_WS="$HOME/workspaces/isaac_ros_3_2" +export ROS_DOMAIN_ID=42 + +cd "$ISAAC_ROS_WS/src/isaac_ros_common" +./scripts/run_dev.sh -b -d "$ISAAC_ROS_WS" +``` + +`-b` 是 `--skip_image_build`，含义是跳过镜像构建，不是构建镜像。该命令使用已有镜像创建一个新的临时容器并进入 `C1`，不是通过 `docker start` 重启旧容器。 + +工作空间通过 bind mount 保存在 Jetson 主机上。容器内 `/workspaces/isaac_ros-dev` 之外的临时修改不会自动保存到下一次新容器中。 + +### 1.2 从新终端进入已经运行的容器 + +执行位置：`H2` 或 `H3`，Jetson 宿主机。 + +保持 H1/C1 不退出，然后在新的宿主机终端执行： + +```bash +conda deactivate 2>/dev/null || true +unset PYTHONHOME + +export ISAAC_ROS_WS="$HOME/workspaces/isaac_ros_3_2" +export ROS_DOMAIN_ID=42 + +cd "$ISAAC_ROS_WS/src/isaac_ros_common" +./scripts/run_dev.sh -d "$ISAAC_ROS_WS" +``` + +正常复用时应先看到： + +```text +Attaching to running container: ... +Docker workspace: /workspaces/isaac_ros-dev +``` + +`run_dev.sh` 会自动通过 `docker exec` 进入同一个容器，不会重新构建镜像，也不会创建第二个容器。如果第二个终端显示的是 `Launching Isaac ROS Dev container`，说明原容器不存在或容器名称配置已变化，不要继续把它当成 `C2/C3`。 + +`-d` 只在创建新容器时决定主机目录的挂载位置。容器已经运行时，脚本不会重新挂载工作空间。若要切换工作空间，必须先按第 6 节停止原容器，再使用新的 `-d` 创建容器。运行期间不要修改 `CONFIG_CONTAINER_NAME_SUFFIX`。 + +不要在正式启动流程中硬编码： + +```text +docker exec ... isaac_ros_dev-aarch64-container ... +``` + +容器名可能受到 `CONFIG_CONTAINER_NAME_SUFFIX` 影响。统一使用 `run_dev.sh` 可以避免多数 `No such container` 问题。 + +### 1.3 完整镜像构建（维护操作，非日常启动） + +当前完整 cuVSLAM 开发环境包含已经制备的依赖和工作空间构建结果。仅运行基础镜像构建流程不能保证复现全部环境，因此本节不是日常启动步骤。 + +需要维护镜像时，在 `H1` 执行： + +```bash +conda deactivate 2>/dev/null || true +unset PYTHONHOME + +export ISAAC_ROS_WS="$HOME/workspaces/isaac_ros_3_2" +export ROS_DOMAIN_ID=42 + +cd "$ISAAC_ROS_WS/src/isaac_ros_common" +./scripts/run_dev.sh -d "$ISAAC_ROS_WS" +``` + +只有在同名容器未运行，并且 `SKIP_DOCKER_BUILD` 与 `CONFIG_SKIP_IMAGE_BUILD` 均未启用时，不带 `-b` 才会执行镜像构建。配置可能来自 `isaac_ros_common/scripts/.isaac_ros_common-config` 或 `~/.isaac_ros_common-config`。 + +构建后必须重新验证 GPU、RealSense、NITROS、cuVSLAM 动态库、工作空间 overlay 和本手册第 2 节的完整链路。新机器的完整环境制备仍需单独安装/构建文档支持。 + +### 1.4 初始化每个容器终端 + +执行位置：每一个新打开的容器终端 `C1`、`C2`、`C3`。 + +```bash +set +u +set +e +set -o pipefail +export ROS_DOMAIN_ID=42 + +source /opt/ros/humble/setup.bash +source /workspaces/isaac_ros-dev/install/setup.bash + +cd /workspaces/isaac_ros-dev +``` + +快速确认： + +```bash +echo "ROS_DOMAIN_ID=$ROS_DOMAIN_ID" +ros2 pkg prefix realsense2_camera +ros2 pkg prefix isaac_ros_visual_slam +test "$(ros2 pkg prefix isaac_ros_visual_slam)" = \ + /workspaces/isaac_ros-dev/install/isaac_ros_visual_slam +``` + +最后一项必须成功，确保当前使用的是工作空间 overlay，而不是错误的系统包。如果 `/workspaces/isaac_ros-dev/install/setup.bash` 不存在，或包前缀不匹配，应先停止，不要继续启动传感器。 + +## 2. 当前推荐：官方组合启动 + +这是当前已经完成 Jetson 真机验证的日常主路径。 + +执行位置：`C1`。 + +保持该终端持续运行： + +```bash +ros2 launch isaac_ros_visual_slam \ + isaac_ros_visual_slam_realsense.launch.py +``` + +该命令会同时启动： + +- 一份 `realsense2_camera_node`； +- D435i 双红外和内置 IMU； +- 一份 cuVSLAM 节点。 + +运行该命令前不得已有 `/camera/camera`，否则会争用 USB 设备。该官方基线不使用飞控 IMU，并且启动文件本身不提供相机序列号参数；连接多台 RealSense 时不能保证选择 `243622070369`。 + +需要检查运行状态时，在 `H2` 按 1.2 进入同一个容器，得到 `C2`，再按第 5 节执行检查。 + +## 3. 拆分启动候选：正常 D435i 相机 + +> [!WARNING] +> 第 3、4 节是按 NVIDIA 官方参数拆分出的候选路径，尚需在 Jetson 完成一次冒烟测试。当前日常运行优先使用第 2 节。 + +本节启动正常运行用的 D435i 数据源，不启动标定节点，不录包。参数与 NVIDIA Isaac ROS 3.2 的 RealSense Visual SLAM 基线一致： + +- IR1、IR2：启用，`640x360@90 Hz`； +- D435i 陀螺仪、加速度计：启用，`200 Hz`； +- 合并 IMU：启用，发布 `/camera/imu`； +- 彩色、深度：关闭； +- 红外发射器：关闭。 + +执行位置：`C1`。 + +保持该终端持续运行： + +```bash +ros2 run realsense2_camera realsense2_camera_node --ros-args \ + -r __node:=camera \ + -r __ns:=/camera \ + -p serial_no:="'243622070369'" \ + -p enable_infra1:=true \ + -p enable_infra2:=true \ + -p enable_color:=false \ + -p enable_depth:=false \ + -p depth_module.emitter_enabled:=0 \ + -p depth_module.profile:="'640x360x90'" \ + -p enable_gyro:=true \ + -p enable_accel:=true \ + -p gyro_fps:=200 \ + -p accel_fps:=200 \ + -p unite_imu_method:=2 +``` + +启动成功后应存在以下主要输入话题： + +```text +/camera/infra1/image_rect_raw +/camera/infra2/image_rect_raw +/camera/infra1/camera_info +/camera/infra2/camera_info +/camera/imu +/tf_static +``` + +> [!NOTE] +> 这条命令是正常运行相机命令，不是 `image_bag_recorder` 中的标定相机命令。 + +## 4. 拆分启动候选：cuVSLAM + +当前拆分启动命令使用 D435i 内置 IMU，仅用于复现 NVIDIA 官方 RealSense 基线。飞控 IMU 的受控运行入口已经在第 8 节单独实现；不能在本命令中把 `/camera/imu` 手工替换成 `/mavros/imu/data_raw`，否则会绕过时间偏移、frame、TF、数据质量和运行健康门禁。 + +### 4.1 进入第二个容器终端 + +在 `H2` 按 1.2 的方法进入同一个容器，得到 `C2`，然后按 1.4 初始化 ROS 环境。 + +### 4.2 启动 cuVSLAM 节点 + +执行位置：`C2`。 + +先确认独立可执行文件已安装： + +```bash +ros2 pkg executables isaac_ros_visual_slam | \ + grep -F 'isaac_ros_visual_slam isaac_ros_visual_slam' +``` + +保持该终端持续运行： + +```bash +ros2 run isaac_ros_visual_slam isaac_ros_visual_slam --ros-args \ + -r __node:=visual_slam_node \ + -p enable_image_denoising:=false \ + -p rectified_images:=true \ + -p num_cameras:=2 \ + -p enable_imu_fusion:=true \ + -p gyro_noise_density:=0.000244 \ + -p gyro_random_walk:=0.000019393 \ + -p accel_noise_density:=0.001862 \ + -p accel_random_walk:=0.003 \ + -p calibration_frequency:=200.0 \ + -p image_jitter_threshold_ms:=22.0 \ + -p base_frame:=camera_link \ + -p imu_frame:=camera_gyro_optical_frame \ + -p camera_optical_frames:="['camera_infra1_optical_frame','camera_infra2_optical_frame']" \ + -p enable_localization_n_mapping:=true \ + -p enable_slam_visualization:=true \ + -p enable_landmarks_view:=true \ + -p enable_observations_view:=true \ + -r visual_slam/image_0:=/camera/infra1/image_rect_raw \ + -r visual_slam/camera_info_0:=/camera/infra1/camera_info \ + -r visual_slam/image_1:=/camera/infra2/image_rect_raw \ + -r visual_slam/camera_info_1:=/camera/infra2/camera_info \ + -r visual_slam/imu:=/camera/imu +``` + +该命令复现 NVIDIA `isaac_ros_visual_slam_realsense.launch.py` 中 VisualSlamNode 的节点名、参数与话题映射，但不会再次启动 RealSense 驱动。独立可执行文件使用自身的多线程 executor，而官方 launch 使用 component container，因此仍需进行 Jetson 冒烟测试。 + +## 5. 检查运行状态 + +### 5.1 进入检查终端 + +官方组合模式：在 `H2` 按 1.2 进入同一个容器，得到 `C2`。 + +拆分候选模式：由于 `C2` 正在运行 cuVSLAM，在 `H3` 按 1.2 进入同一个容器，得到 `C3`。 + +进入后按 1.4 初始化 ROS 环境。 + +### 5.2 检查节点和发布端 + +执行位置：官方组合模式使用 `C2`；拆分候选模式使用 `C3`。 + +```bash +echo "========== Nodes ==========" +ros2 node list | sort + +CAMERA_NODE_COUNT="$( + ros2 node list 2>/dev/null | + grep -cx '/camera/camera' || true +)" +echo "camera_node_count=$CAMERA_NODE_COUNT" + +echo "========== cuVSLAM node ==========" +ros2 node info /visual_slam_node +ros2 param get /visual_slam_node camera_optical_frames + +echo "========== Camera inputs ==========" +ros2 topic info -v /camera/infra1/image_rect_raw +ros2 topic info -v /camera/infra2/image_rect_raw +ros2 topic info -v /camera/infra1/camera_info +ros2 topic info -v /camera/infra2/camera_info +ros2 topic info -v /camera/imu +ros2 topic info -v /tf_static + +echo "========== cuVSLAM outputs ==========" +ros2 topic info -v /visual_slam/status +ros2 topic info -v /visual_slam/tracking/odometry +``` + +必须确认： + +- `camera_node_count=1`； +- 左右 Image、两路 CameraInfo 和 `/camera/imu` 均为一个发布端； +- 上述五个输入话题都能看到 cuVSLAM 的一个订阅端； +- `/visual_slam_node` 的订阅已映射到预期的 `/camera/...` 话题； +- `camera_optical_frames` 正确解析为左右两个光学坐标系； +- `/visual_slam/status` 和 `/visual_slam/tracking/odometry` 有发布端。 + +### 5.3 检查 CameraInfo 和 TF + +执行位置：官方组合模式使用 `C2`；拆分候选模式使用 `C3`。 + +```bash +timeout 8s ros2 topic echo \ + /camera/infra1/camera_info --once || true + +timeout 8s ros2 topic echo \ + /camera/infra2/camera_info --once || true + +timeout --signal=INT 5s ros2 run tf2_ros tf2_echo \ + camera_link camera_infra1_optical_frame || true + +timeout --signal=INT 5s ros2 run tf2_ros tf2_echo \ + camera_link camera_infra2_optical_frame || true + +timeout --signal=INT 5s ros2 run tf2_ros tf2_echo \ + camera_link camera_gyro_optical_frame || true +``` + +两路 CameraInfo 必须包含非零 `K/P`，三个 TF 查询必须得到有效变换。RealSense ROS 4.51.1 可能让右 CameraInfo 消息头误用左光学 frame；cuVSLAM 命令已通过 `camera_optical_frames` 显式固定左右物理坐标系，但 TF 本身仍必须存在。 + +### 5.4 检查频率 + +执行位置：官方组合模式使用 `C2`；拆分候选模式使用 `C3`。 + +```bash +timeout --signal=INT 8s ros2 topic hz /camera/infra1/image_rect_raw || true +timeout --signal=INT 8s ros2 topic hz /camera/infra2/image_rect_raw || true +timeout --signal=INT 8s ros2 topic hz /camera/imu || true +timeout --signal=INT 8s ros2 topic hz /visual_slam/tracking/odometry || true +``` + +参考值： + +| 话题 | 参考频率 | +| --- | --- | +| `/camera/infra1/image_rect_raw` | 约 `89.8 Hz` | +| `/camera/infra2/image_rect_raw` | 约 `89.8 Hz` | +| `/camera/imu` | 约 `199.8 Hz` | +| `/visual_slam/tracking/odometry` | 约 `89.8 Hz` | + +### 5.5 读取状态和里程计样本 + +执行位置：官方组合模式使用 `C2`；拆分候选模式使用 `C3`。 + +```bash +timeout 8s ros2 topic echo /visual_slam/status --once || true +timeout 8s ros2 topic echo /visual_slam/tracking/odometry --once || true +``` + +正常状态必须满足： + +- `/visual_slam/status` 中 `vo_state: 1`； +- 里程计时间戳非零并持续更新； +- `header.frame_id` 为 `odom`； +- `child_frame_id` 为 `camera_link`。 + +只有图像、CameraInfo、TF 和 IMU 持续有效，并且 cuVSLAM 完成初始化后，里程计才会正常输出。 + +### 5.6 使用 RViz + +执行位置：官方组合模式使用 `C2`；拆分候选模式使用 `C3`。需要可用的图形显示环境。 + +```bash +RVIZ_CONFIG="$(ros2 pkg prefix --share isaac_ros_visual_slam)/rviz/realsense.cfg.rviz" +rviz2 -d "$RVIZ_CONFIG" +``` + +### 5.7 使用 rqt 查看左右红外图像 + +执行位置：已初始化 ROS 环境且有图形显示的容器终端。左右 cuVSLAM 输入分别为： + +```text +/camera/infra1/image_rect_raw +/camera/infra2/image_rect_raw +``` + +可以在两个终端各启动一个 Image View： + +```bash +ros2 run rqt_image_view rqt_image_view \ + /camera/infra1/image_rect_raw +``` + +```bash +ros2 run rqt_image_view rqt_image_view \ + /camera/infra2/image_rect_raw +``` + +两幅图应同时、连续更新，均为 `640x360` rectified 红外图。rqt 只用于短时检查；正式运行和性能测试前关闭两个窗口，避免额外订阅、显示与拷贝负载。 + +## 6. 正确停止顺序 + +### 6.1 官方组合模式 + +1. 在持有官方组合 launch 的 `C1` 中按一次 `Ctrl+C`。 +2. 等待 RealSense 与 cuVSLAM 节点一起退出并释放 USB 设备。 +3. 关闭 RViz，在 `C2` 等附加容器终端执行 `exit`。 +4. 确认不再需要容器后，最后在主容器终端 `C1` 执行 `exit`。 + +### 6.2 拆分候选模式 + +1. 在 `C2` 中按一次 `Ctrl+C`，等待 cuVSLAM 完全退出。 +2. 在 `C1` 中按一次 `Ctrl+C`，等待 RealSense 节点释放 USB 设备。 +3. 关闭 RViz，然后在 `C3`、`C2` 等附加容器终端执行 `exit`。 +4. 确认不再需要容器后，最后在主容器终端 `C1` 执行 `exit`。 + +### 6.3 退出主容器前检查残留进程 + +执行位置：`C1`。 + +```bash +pgrep -af realsense2_camera_node || true +pgrep -af isaac_ros_visual_slam || true +``` + +确认没有残留的相机或 cuVSLAM 进程后，再退出主容器 shell。 + +`run_dev.sh` 使用 `docker run --rm` 创建主容器。退出主容器 shell 后，容器会停止并自动删除；所有附加容器终端和容器内进程也会结束。退出附加终端本身不会停止主容器。 + +## 7. 常见故障 + +### `No such container` + +可能原因： + +- 固定使用了过期或带不同后缀的容器名； +- 主容器终端 `C1` 已退出，`--rm` 已删除容器； +- H1 的 SSH 会话或终端被关闭； +- Docker 服务已经重启； +- H1 与 H2 读取了不同的 `CONFIG_CONTAINER_NAME_SUFFIX`； +- 只有名称相似的容器，触发了 `run_dev.sh` 的 Docker 名称过滤边界。 + +检查位置：Jetson 宿主机。 + +```bash +docker ps -a \ + --filter 'name=isaac_ros_dev-' \ + --format 'table {{.Names}}\t{{.Status}}\t{{.Image}}' +``` + +如果目标容器仍在运行，按 1.2 进入。如果容器不存在但已验证镜像仍在，按 1.1 创建新容器。如果 `run_dev.sh` 先显示 `Attaching to running container`，随后又报 `No such container`，重点检查是否只有名称相似而非完全相同的容器。 + +### `No built image found` + +原因：使用 `-b` 跳过了镜像构建，但当前配置所对应的镜像不存在；修改容器后缀或镜像 key 也可能改变所需镜像名。 + +检查位置：Jetson 宿主机。 + +```bash +docker image ls 'isaac_ros_dev-*' +env | grep -E 'SKIP_DOCKER_BUILD|ISAAC_ROS_WS' || true +grep -H -E 'CONFIG_(IMAGE_KEY|SKIP_IMAGE_BUILD|CONTAINER_NAME_SUFFIX)' \ + "$HOME/.isaac_ros_common-config" \ + "$HOME/workspaces/isaac_ros_3_2/src/isaac_ros_common/scripts/.isaac_ros_common-config" \ + 2>/dev/null || true +``` + +不要为了消除该报错而盲目重建并覆盖当前已验证镜像。确认配置后，按项目的镜像制备记录执行；只有明确进行环境维护时才使用 1.3。 + +### `RS2_USB_STATUS_BUSY` 或 `failed to claim usb interface` + +原因：另一份 RealSense 节点已经占用 D435i。 + +检查位置：任一已经初始化的容器终端。 + +```bash +ros2 node list | grep -E '^/camera/camera$' || true +pgrep -af realsense2_camera_node || true +``` + +停止旧节点并确认 USB 释放后，只选择一种模式：运行第 2 节官方组合 launch，或者运行第 3、4 节拆分候选；不得同时运行。 + +### `Package '...' not found` + +原因：新的容器终端尚未加载工作空间 overlay。 + +处理：重新执行 1.4 的完整环境初始化命令。 + +### `setup.bash` 因未定义变量退出 + +原因：在 source ROS 环境前启用了 `set -u`。 + +处理：先执行 `set +u`，再 source 两个 `setup.bash`。 + +### 相机有图像但没有里程计 + +依次检查： + +```bash +ros2 topic info /camera/infra1/image_rect_raw +ros2 topic info /camera/infra2/image_rect_raw +ros2 topic info /camera/infra1/camera_info +ros2 topic info /camera/infra2/camera_info +ros2 topic info /camera/imu +ros2 topic echo /visual_slam/status --once +``` + +同时查看 `C2` 中是否存在 TF、时间戳、IMU 注册或 cuVSLAM 初始化错误。 + +## 8. 飞控 IMU 运行链路（源码已完成，Jetson 待验收） + +> [!CAUTION] +> 本节是候选链路，不是当前生产主路径。现有 `seeker_imu.yaml` 的四项值是 Kalibr 输入假设，不是 PX4 Allan 标定结果。没有真实 Allan 参数时，只能按 8.6 做接线冒烟，禁止据此验收 VIO 精度或接入飞行控制。 + +本链路固定为： + +```text +[Jetson 宿主机 H4] +PX4 -> /dev/ttyTHS2:921600 -> MAVROS -> /mavros/imu/data_raw + +[Isaac ROS 容器 C1] +/mavros/imu/data_raw + -> aligned_fcu_imu_relay + -> /fcu/imu/data_raw_aligned + -> cuVSLAM + +D435i IR1 + IR2 + CameraInfo + factory TF -> cuVSLAM +camera_infra1_optical_frame -> fcu_imu -> 标定静态 TF +``` + +宿主机只负责 UART/MAVROS；容器统一 launch 负责唯一一份 RealSense 驱动、时间对齐节点、标定 TF 和 cuVSLAM。该模式与第 2 节官方组合 launch、以及第 3、4 节拆分链路互斥。 + +### 8.1 固定标定合同 + +版本化配置： + +```text +isaac_ros_yopo_bringup/config/d435i_243622070369_fcu_imu.yaml +``` + +运行时固定： + +- 相机原厂 `K/D/R/P` 由 RealSense 两路 `CameraInfo` 提供，不硬编码进 cuVSLAM； +- 左右物理 frame 显式为 `camera_infra1_optical_frame`、`camera_infra2_optical_frame`； +- 原始飞控 IMU 保留 `/mavros/imu/data_raw` 与 `base_link` 不变； +- 对齐节点只把时间戳增加 `1,737,987 ns`，并把同一数值坐标轴命名为 `fcu_imu`； +- 原始 IMU stamp 与宿主 system time 的接收残差必须不超过 `250 ms`，PX4 boot-time 会被拒绝； +- 对齐节点不旋转、不滤波、不插值、不重采样，也不修改任何测量或 covariance； +- 静态 TF 发布 `camera_infra1_optical_frame -> fcu_imu`； +- cuVSLAM 只订阅 `/fcu/imu/data_raw_aligned`； +- `calibration_frequency=170.0` 使用实测频率，不使用 MAVLink 请求值 `200 Hz`。 + +这里有意选择“原厂 rectified CameraInfo + 使用同一相机模型求得的联合外参”。另一组自由估计内参的 Kalibr 结果为左目约 `[324.5671, 325.0486, 322.0116, 184.0997]`、右目约 `[324.9723, 325.4113, 321.3716, 184.1345]`，并估计了非零 radtan 畸变和 `49.9168 mm` 基线；它不能直接与当前 RealSense 已校正图像发布的原厂 `K/D/R/P` 混用。若以后选择该模型，必须同时实现并验收新的图像校正与 CameraInfo 发布链路，再重新生成匹配的联合外参。 + +两组联合结果的 IMU/相机旋转接近，但相机原点在 IMU 坐标系中的位置相差约 `6.83 mm`。因此当前 factory-rectified 结果保持候选状态，不能只因为单次优化收敛就标记为 `approved`。 + +Kalibr 时移定义为 `t_imu = t_cam + timeshift_cam_imu`，其中 `timeshift_cam_imu=-0.001737986760008108 s`。保持相机时间戳不变时，正确实现就是给原始 IMU 时间戳增加 `1,737,987 ns`，不能再次使用负号。 + +### 8.2 `H4`：启动并保持 MAVROS + +执行位置：Jetson 宿主机新终端 `H4`，不在容器内。 + +```bash +conda deactivate 2>/dev/null || true +unset PYTHONHOME +set +u +source /opt/ros/humble/setup.bash +export ROS_DOMAIN_ID=42 + +FCU_DEV=/dev/ttyTHS2 +FCU_BAUD=921600 + +if [ ! -c "$FCU_DEV" ]; then + echo "[STOP] $FCU_DEV does not exist" + exit 1 +fi + +if ! id -nG | tr ' ' '\n' | grep -qx dialout; then + echo "[STOP] current user is not in dialout" + exit 1 +fi + +if fuser "$FCU_DEV" >/dev/null 2>&1; then + echo "[STOP] $FCU_DEV is already in use" + fuser -v "$FCU_DEV" + exit 1 +fi + +ros2 launch mavros px4.launch \ + fcu_url:="${FCU_DEV}:${FCU_BAUD}" +``` + +保持 `H4` 持续运行。不要传空的 `gcs_url:=""`；Humble 会把它解析成 malformed launch argument。正常日志必须出现 `Got HEARTBEAT, connected. FCU: PX4 Autopilot`。 + +### 8.3 `H5`：请求 HIGHRES_IMU 并检查原始流 + +执行位置：Jetson 宿主机另一个终端 `H5`，不在容器内。 + +```bash +conda deactivate 2>/dev/null || true +unset PYTHONHOME +set +u +source /opt/ros/humble/setup.bash +export ROS_DOMAIN_ID=42 + +STATE="$( + timeout 10s ros2 topic echo \ + /mavros/state mavros_msgs/msg/State --once +)" +printf '%s\n' "$STATE" +if ! printf '%s\n' "$STATE" | grep -q '^connected: true$'; then + echo "[STOP] MAVROS is not connected to PX4" + exit 1 +fi + +ros2 service call \ + /mavros/set_message_interval \ + mavros_msgs/srv/MessageInterval \ + "{message_id: 105, message_rate: 200.0}" + +IMU_TYPE="$(ros2 topic type /mavros/imu/data_raw 2>/dev/null || true)" +if [ "$IMU_TYPE" != sensor_msgs/msg/Imu ]; then + echo "[STOP] unexpected or missing FCU IMU type: $IMU_TYPE" + exit 1 +fi + +ros2 topic info -v /mavros/imu/data_raw + +timeout 8s ros2 topic echo \ + /mavros/imu/data_raw sensor_msgs/msg/Imu \ + --once --qos-reliability best_effort + +timeout --signal=INT 10s ros2 topic hz \ + /mavros/imu/data_raw || true + +timeout 8s ros2 topic echo \ + /mavros/timesync_status mavros_msgs/msg/TimesyncStatus \ + --once --qos-reliability best_effort +``` + +必须确认 service 返回 `success=True`、IMU 约 `170 Hz`、stamp 非零且递增、静止时加速度模长约 `9.8 m/s^2`，并且 timesync 持续稳定。请求 `200 Hz` 不代表实际一定达到 `200 Hz`。 + +### 8.4 `C1`：构建和核对运行包 + +执行位置：已按 1.1、1.4 进入并初始化的容器主终端 `C1`。 + +先确认 NVIDIA wrapper 的 v3.2-15 时间戳补丁已应用，且新包能被 colcon 发现： + +```bash +export VSLAM_SOURCE=/workspaces/isaac_ros-dev/src/isaac_ros_visual_slam +export YOPO_ADAPTER=/workspaces/isaac_ros-dev/src/cuvslam-yopo-adapter/integrations/isaac_ros_3_2_yopo +export VSLAM_PATCH="$YOPO_ADAPTER/patches/isaac_ros_visual_slam_v3_2_15_imu_timestamp.patch" + +if ! git -C "$VSLAM_SOURCE" apply --reverse --check "$VSLAM_PATCH"; then + echo "[STOP] expected Isaac ROS Visual SLAM timestamp patch is not applied" + exit 1 +fi + +if ! colcon list | grep -q '^isaac_ros_yopo_bringup[[:space:]]'; then + echo "[STOP] isaac_ros_yopo_bringup is not visible to colcon" + exit 1 +fi + +cd /workspaces/isaac_ros-dev +colcon build --packages-select isaac_ros_yopo_bringup +source /workspaces/isaac_ros-dev/install/setup.bash + +if [ "$(ros2 pkg prefix isaac_ros_visual_slam)" != \ + /workspaces/isaac_ros-dev/install/isaac_ros_visual_slam ]; then + echo "[STOP] patched visual_slam workspace overlay is not active" + exit 1 +fi + +if ! grep -aFq \ + ISAAC_ROS_YOPO_IMU_TIMESTAMP_PATCH_V1 \ + /workspaces/isaac_ros-dev/install/isaac_ros_visual_slam/lib/libvisual_slam_node.so; then + echo "[STOP] installed Visual SLAM binary was not rebuilt with the required patch" + exit 1 +fi + +ros2 pkg prefix isaac_ros_yopo_bringup +ros2 launch isaac_ros_yopo_bringup \ + d435i_fcu_imu_cuvslam.launch.py --show-args +``` + +源码补丁本身的首次应用和 `isaac_ros_visual_slam` 重编译仍按 [`README_set_up.md`](README_set_up.md) 执行。仅编译本 bringup 包不会自动重编译 NVIDIA wrapper。 + +### 8.5 `C1`：使用真实 Allan 参数启动候选链路 + +执行位置：容器主终端 `C1`。运行前必须停止所有旧 RealSense 和 Visual SLAM 节点，并确保 `H4` 中 MAVROS 正常连接。 + +先把真实 PX4 Allan 结果写成受版本控制的 schema YAML，并设置其容器内绝对路径： + +```bash +export PX4_IMU_NOISE_FILE=/absolute/path/to/px4_imu_allan.yaml +``` + +该文件必须使用 `schema_version: 1`，并包含 `validated: true`、匹配的 `hardware_id`、约 `170 Hz` 采样率、`method.name: allan_deviation`、源文件 SHA-256、四项精确单位和四项数值。launch 从同一个文件读取来源与数值，不接受独立 CLI 数值覆盖。 + +正式启动前执行硬门禁： + +```bash +if [ -z "${PX4_IMU_NOISE_FILE:-}" ] || [ ! -s "$PX4_IMU_NOISE_FILE" ]; then + echo "[STOP] valid PX4 Allan parameters are required" + exit 1 +fi + +ros2 launch isaac_ros_yopo_bringup \ + d435i_fcu_imu_cuvslam.launch.py \ + imu_noise_file:="$PX4_IMU_NOISE_FILE" \ + allow_candidate_calibration:=true +``` + +当前版本化联合标定状态仍是候选，因此必须显式设置 `allow_candidate_calibration:=true`。完成真实 Allan 参数和独立重复性验收、把版本化状态改为 `approved` 后，应删除这个开关。该终端会同时保持 RealSense、aligned IMU、标定 TF、cuVSLAM 和运行健康监视节点。不要再单独启动相机节点或官方组合 launch。 + +### 8.6 `C1`：仅做接线冒烟的临时命令 + +> [!WARNING] +> 下列命令明确使用未通过 Allan 标定的旧输入假设，只能检查节点、话题、TF、时间戳和订阅关系。它不能证明 IMU 权重正确，不能用于 VIO 精度结论，更不能用于飞行。 + +```bash +UNVALIDATED_NOISE="$( + ros2 pkg prefix --share isaac_ros_yopo_bringup +)/config/px4_imu_noise_unvalidated.yaml" + +ros2 launch isaac_ros_yopo_bringup \ + d435i_fcu_imu_cuvslam.launch.py \ + imu_noise_file:="$UNVALIDATED_NOISE" \ + allow_candidate_calibration:=true \ + allow_unvalidated_imu_noise:=true +``` + +launch 会打印 `[WARNING] Unvalidated IMU noise was explicitly allowed`。看不到该警告时，不要把当前运行当作本节的临时冒烟。 + +### 8.7 `C2`：运行验收 + +在 `H2` 按 1.2 进入同一个容器，得到 `C2`，再按 1.4 初始化 ROS 环境。 + +先检查 D435i 内置 IMU 确实关闭： + +```bash +ros2 param get /camera/camera enable_gyro +ros2 param get /camera/camera enable_accel +ros2 param get /camera/camera unite_imu_method + +if ros2 topic list | grep -qx /camera/imu; then + if timeout 6s ros2 topic echo \ + /camera/imu sensor_msgs/msg/Imu \ + --once --qos-reliability best_effort \ + >/tmp/d435_runtime_imu_probe.log 2>&1 + then + cat /tmp/d435_runtime_imu_probe.log + echo "[STOP] D435i IMU produced a forbidden sample" + exit 1 + else + RC=$? + if [ "$RC" -eq 124 ]; then + echo "[PASS] /camera/imu exists but remained silent" + else + cat /tmp/d435_runtime_imu_probe.log + echo "[STOP] D435i IMU silence probe failed with rc=$RC" + exit 1 + fi + fi +else + echo "[PASS] /camera/imu is absent" +fi +``` + +三个参数必须依次为 `False`、`False`、`0`。RealSense ROS 4.51.1 可能保留一个没有实际样本的 `/camera/imu` publisher，因此必须检查 6 秒内是否收到消息，不能只看话题是否存在。 + +检查 raw/aligned IMU、参数与 TF： + +```bash +timeout 8s ros2 topic echo \ + /camera/infra1/camera_info sensor_msgs/msg/CameraInfo --once +timeout 8s ros2 topic echo \ + /camera/infra2/camera_info sensor_msgs/msg/CameraInfo --once + +ros2 topic info -v /mavros/imu/data_raw +ros2 topic info -v /fcu/imu/data_raw_aligned + +ros2 param get /aligned_fcu_imu_relay imu_to_camera_offset_ns +ros2 param get /aligned_fcu_imu_relay output_frame_id +ros2 param get /visual_slam_node imu_frame +ros2 param get /visual_slam_node calibration_frequency + +timeout 8s ros2 topic echo \ + /fcu/imu/data_raw_aligned sensor_msgs/msg/Imu \ + --once --qos-reliability best_effort + +timeout --signal=INT 5s ros2 run tf2_ros tf2_echo \ + camera_infra1_optical_frame fcu_imu || true +``` + +RealSense 启动日志必须报告序列号 `243622070369` 和固件 `5.15.1.55`。两路 CameraInfo 必须为 `640x360`，并与版本化 YAML 中的 factory `K/D/R/P` 一致；右 CameraInfo 的消息头允许存在已知的左 frame 复用问题。还必须看到 offset `1737987`、两个 IMU frame 均为 `fcu_imu`、频率参数 `170.0`。TF 平移应接近 `[0.02736293, 0.05285189, -0.06214162] m`，四元数 xyzw 应接近 `[0.50184877, -0.50942523, 0.49064963, 0.49789224]`。 + +检查 cuVSLAM 只有一个正确 IMU 输入： + +```bash +VSLAM_INFO="$(ros2 node info /visual_slam_node)" +printf '%s\n' "$VSLAM_INFO" + +if ! printf '%s\n' "$VSLAM_INFO" | \ + grep -q '/fcu/imu/data_raw_aligned: sensor_msgs/msg/Imu'; then + echo "[STOP] cuVSLAM is not subscribed to the aligned FCU IMU" + exit 1 +fi + +if printf '%s\n' "$VSLAM_INFO" | \ + grep -Eq '/camera/imu|/mavros/imu/data_raw'; then + echo "[STOP] cuVSLAM has a forbidden raw or D435i IMU subscription" + exit 1 +fi +``` + +检查频率、diagnostics 和里程计： + +```bash +timeout --signal=INT 10s ros2 topic hz /camera/infra1/image_rect_raw || true +timeout --signal=INT 10s ros2 topic hz /camera/infra2/image_rect_raw || true +timeout --signal=INT 10s ros2 topic hz /fcu/imu/data_raw_aligned || true +timeout --signal=INT 10s ros2 topic hz /visual_slam/tracking/odometry || true + +timeout 8s ros2 topic echo /diagnostics \ + diagnostic_msgs/msg/DiagnosticArray --once || true +timeout 8s ros2 topic echo /visual_slam/status --once || true +timeout 8s ros2 topic echo /visual_slam/tracking/odometry --once || true +``` + +参考值为左右图像约 `89.8 Hz`、aligned IMU 约 `170 Hz`、里程计约 `89.8 Hz`。aligned relay diagnostics 必须最终为 `OK`，`last_clock_residual_ms` 的绝对值必须小于 `250`，累计 `zero_stamp`、`invalid_stamp`、`duplicate`、`nonmonotonic`、`frame_mismatch`、`clock_domain_mismatch`、`aligned_out_of_range`、`nonfinite_measurement` 都应为 `0`；单次 IMU 间隔门限约为 `29.4 ms`，持续频率或间隔异常仍会在三个诊断周期后退出。运行健康监视 diagnostics 也必须为 `OK`，odometry 必须使用 `odom -> camera_link`、非零严格递增时间戳和有限数值，`vo_state` 应为 `1`。持续 duplicate、跨时间域、非有限测量、异常频率、CameraInfo 不匹配、D435i IMU 实际出数或里程计中断时，相应守护节点必须非零退出并触发整套 launch 关闭。 + +### 8.8 停止顺序和验收边界 + +1. 在 `C1` 中按一次 `Ctrl+C`，等待健康监视、cuVSLAM、TF、aligned relay 和 RealSense 全部退出。 +2. 在 `H4` 中按一次 `Ctrl+C`，等待 MAVROS 退出并释放 `/dev/ttyTHS2`。 +3. 在 `C2` 等附加容器终端执行 `exit`，最后退出主容器终端。 + +源码与单元测试完成不等于真机生产验收。正式替换第 2 节日常主路径前，仍需： + +- 用 PX4 原始静止数据完成 Allan 标定并固化四项噪声参数及来源； +- 在相同 factory-rectified 模型下做一次独立重复联合标定，解释当前约 `6.83 mm` 的新旧相机原点差； +- 在 Jetson 验证真实 ROS `Imu` 深拷贝、BEST_EFFORT QoS、拒绝计数和 output-stale diagnostics； +- 完成时间戳、轨迹质量、失跟恢复、资源占用和长时间稳定性测试。 + +## 9. cuVSLAM 状态适配器（待完成） + +目标链路： + +```text +/visual_slam/tracking/odometry + /visual_slam/status + -> 状态与坐标适配器 + -> /state/odom +``` + +只有在 frame、位置、姿态、速度语义和 tracking-lost 门控完成真机验收后，才在本节加入正式启动命令。 + +## 10. YOPO PASSIVE（待联合验收） + +本节当前不提供启动命令，避免把孤立可运行的 YOPO 命令误写成已经打通的完整系统入口。 + +加入命令前必须满足： + +- `/state/odom` 的 frame 和速度语义已经验收； +- Depth 与 CameraInfo 的分辨率、单位、时间戳和话题契约已经固定； +- YOPO 配置明确设置 `output_enabled=false`； +- tracking lost、里程计超时或深度失效时，YOPO 不使用陈旧数据； +- 被动联合运行期间控制话题实际消息数为零； +- 完成长时间资源和稳定性测试。 + +完成后，本节按以下结构追加： + +1. `[Jetson 宿主机]` YOPO 环境初始化； +2. YOPO PASSIVE 启动命令； +3. 输入、输出和零控制消息验证； +4. 正确停止顺序； +5. 常见故障。 + +## 11. 文档维护规则 + +后续每加入一个模块，都必须同时写明： + +- 当前状态：已验证、待验证或禁止； +- 精确执行位置：Jetson 宿主机或 Isaac ROS 容器； +- 使用的终端编号； +- 完整环境初始化命令； +- 启动命令； +- 必须持续运行的终端； +- 输入输出话题和正常参考值； +- 验证命令； +- 停止顺序； +- 固定版本或提交； +- 已知限制和安全边界。 + +不得把尚未通过真机联合验收的命令写成可直接运行的正式步骤。 + +## 变更记录 + +| 日期 | 内容 | +| --- | --- | +| 2026-07-21 | 加入 D435i factory-rectified + PX4/MAVROS 外部 IMU + cuVSLAM 候选统一启动、噪声门禁、验收与停止流程。 | +| 2026-07-21 | 创建系统启动手册；加入容器生命周期、正常 D435i、cuVSLAM、验证、停止和后续 YOPO 章节边界。 | diff --git a/integrations/isaac_ros_3_2_yopo/isaac_ros_yopo_bringup/README.md b/integrations/isaac_ros_3_2_yopo/isaac_ros_yopo_bringup/README.md new file mode 100644 index 0000000..bf340bb --- /dev/null +++ b/integrations/isaac_ros_3_2_yopo/isaac_ros_yopo_bringup/README.md @@ -0,0 +1,106 @@ +# D435i + PX4 IMU + Isaac ROS cuVSLAM 启动包 + +本 ROS 2 Humble 软件包把已完成的 factory-rectified 双目/飞控 IMU 标定结果接入 Isaac ROS Visual SLAM 3.2。当前标定被版本化为运行候选：内参、外参和时间偏移已经固定，但 PX4 Allan 噪声标定与独立重复性验收尚未完成。 + +完整的宿主机、容器、MAVROS、构建、启动、检查和停止顺序见父目录的 [`STARTUP_RUNBOOK.zh-CN.md`](../STARTUP_RUNBOOK.zh-CN.md) 第 8 节。 + +## 运行链路 + +```text +Jetson 宿主机 + PX4 -> /dev/ttyTHS2:921600 -> MAVROS -> /mavros/imu/data_raw + +Isaac ROS 容器 + /mavros/imu/data_raw + -> aligned_fcu_imu_relay + - 拒绝零、重复、倒退和错误 frame 的时间戳 + - 拒绝与宿主 system time 相差超过 250 ms 的陈旧/boot-time 时间戳 + - stamp 固定增加 1,737,987 ns + - base_link 同轴重命名为 fcu_imu + - 不旋转、不滤波、不插值、不修改 covariance + -> /fcu/imu/data_raw_aligned + -> Isaac ROS cuVSLAM + + D435i IR1/IR2 + CameraInfo + factory TF + -> Isaac ROS cuVSLAM + + camera_infra1_optical_frame -> fcu_imu + -> 已标定静态 TF +``` + +统一 launch 会启动一份且仅一份 RealSense 驱动、时间对齐节点、标定静态 TF、官方 cuVSLAM component 和运行健康监视节点。D435i 的 gyro/accel 被明确关闭，cuVSLAM 的 IMU 订阅只映射到 `/fcu/imu/data_raw_aligned`。 + +relay 在 15 秒内收不到首条可发布样本，或运行中 raw/aligned 流持续中断超过 2 秒时会以错误退出；统一 launch 随即停止相机、TF 和 cuVSLAM，避免留下表面存活但不再融合 IMU 的进程。 + +频率或最大时间间隔单次越界先报告 WARN；连续 3 个诊断周期仍不健康时升级为 ERROR 并退出。最大间隔门限为实测周期的 5 倍，约 `29.4 ms`，为已录制数据的 `16.592 ms` 峰值保留 Jetson 负载余量。`/clock` 或 `use_sim_time` 不会替代该 system-time 新鲜度门禁。 + +统一 launch 还会检查已安装 `libvisual_slam_node.so` 中的 patch marker，并等待 patched cuVSLAM 打印 tracker 成功初始化 marker。常驻健康监视节点严格比对两路 CameraInfo 的尺寸、模型和 `K/D/R/P`，禁止 D435i 自带 IMU 产生实际样本，并要求 `/visual_slam/tracking/odometry` 首帧及后续数据持续在线；任何合同失效都会非零退出并关闭整套 launch。 + +## 标定来源 + +版本化记录位于 [`config/d435i_243622070369_fcu_imu.yaml`](config/d435i_243622070369_fcu_imu.yaml)： + +- D435i：序列号 `243622070369`，固件 `5.15.1.55`； +- 图像：原厂 rectified `640x360@90 Hz`； +- 原厂内参：由运行中的 RealSense `CameraInfo` 提供，YAML 中的 `K/D/R/P` 只用于审计； +- 飞控 IMU：`/mavros/imu/data_raw`，实测约 `170 Hz`； +- 时移：`t_aligned = t_imu_raw + 1,737,987 ns`； +- TF：`camera_infra1_optical_frame -> fcu_imu`，使用 `T_Crect0_I`。 + +右侧 RealSense `CameraInfo.header.frame_id` 可能错误复用左光学 frame。launch 已显式设置左右 `camera_optical_frames`，不依赖该错误消息头推断右相机坐标系。 + +## 标定状态门禁 + +当前 YAML 状态为 `runtime_candidate_pending_allan_and_independent_repeatability`。launch 默认只接受 `approved`；现阶段真机验证必须明确传入 `allow_candidate_calibration:=true`。`rejected` 状态永远不能被命令行覆盖。 + +这个开关只承认“操作者知道当前仍是候选标定”，不会跳过 CameraInfo、TF、时间戳、IMU 数据质量或运行新鲜度检查，也不会把未验证的 IMU 噪声参数变成已验证参数。 + +## IMU 噪声门禁 + +launch 不接受四个彼此独立的 CLI 噪声数值，而是要求一个无默认值的 `imu_noise_file`。该 YAML 必须同时记录： + +- `validated` 状态； +- PX4 IMU 硬件/传输 ID； +- Allan 数据采样率； +- 方法名 `allan_deviation`； +- 源数据文件名与 SHA-256； +- 四项参数及精确单位。 + +填写入口为 [`config/px4_imu_noise_allan.template.yaml`](config/px4_imu_noise_allan.template.yaml)。模板中的所有 `REPLACE_WITH_...` 都必须替换，不能把模板本身传给 launch。 + +四项单位为： + +- `gyro_noise_density`：`rad/(s*sqrt(Hz))`； +- `gyro_random_walk`：`rad/(s^2*sqrt(Hz))`； +- `accel_noise_density`：`m/(s^2*sqrt(Hz))`； +- `accel_random_walk`：`m/(s^3*sqrt(Hz))`。 + +现有 `seeker_imu.yaml` 中的 `0.09 / 0.05 / 0.06 / 0.001` 是 Kalibr 联合标定的输入假设，不是 Allan 标定输出。D435i 官方默认四元组也不属于 PX4 IMU。仓库中的 `config/px4_imu_noise_unvalidated.yaml` 明确记录了旧值并设置 `validated: false`；launch 默认拒绝它，`allow_unvalidated_imu_noise:=true` 只用于验证接线，不能作为导航验收结果。真实 Allan YAML 引用的源数据文件必须实际存在，且记录的 SHA-256 必须与该文件字节完全一致。 + +## 本地逻辑测试 + +在本软件包目录执行： + +```bash +python3 -m unittest discover -s test -v +``` + +测试固定了整数纳秒运算、ROS Time 边界、严格递增状态机、消息透明转发、标定矩阵/四元数一致性，以及 launch 的关键订阅和噪声门禁合同。 + +## Jetson 构建边界 + +本包不修改、构建或链接仓库中的 cuVSLAM 核心库。运行时仍使用 Isaac ROS 3.2 的 NVIDIA `libcuvslam.so`，并要求工作空间中的 `isaac_ros_visual_slam` 已应用父目录的 v3.2-15 IMU 时间戳补丁。 + +Jetson 上只构建本包： + +```bash +cd /workspaces/isaac_ros-dev +set +u +source /opt/ros/humble/setup.bash +source /workspaces/isaac_ros-dev/install/setup.bash + +colcon build --packages-select isaac_ros_yopo_bringup +source /workspaces/isaac_ros-dev/install/setup.bash +``` + +编译成功只能证明 ROS 包结构和依赖成立。真机验收必须继续检查频率、frame、TF、时间戳、diagnostics、cuVSLAM 唯一 IMU 订阅、`vo_state` 和长时间运行稳定性。 diff --git a/integrations/isaac_ros_3_2_yopo/isaac_ros_yopo_bringup/config/d435i_243622070369_fcu_imu.yaml b/integrations/isaac_ros_3_2_yopo/isaac_ros_yopo_bringup/config/d435i_243622070369_fcu_imu.yaml new file mode 100644 index 0000000..dd1022c --- /dev/null +++ b/integrations/isaac_ros_3_2_yopo/isaac_ros_yopo_bringup/config/d435i_243622070369_fcu_imu.yaml @@ -0,0 +1,86 @@ +schema_version: 1 +calibration_id: d435i_243622070369_factory_rectified_px4_imu_20260720 +status: runtime_candidate_pending_allan_and_independent_repeatability + +provenance: + camera_serial: "243622070369" + camera_firmware: "5.15.1.55" + joint_calibration_bag: d435_fcu_imu_kalibr_20260720_075102 + kalibr_camchain_result: d435_fcu_imu_kalibr_ros1-camchain-imucam.yaml + camera_model: factory_rectified_camera_info + imu_clock_source: mavros_timesync_px4_boot_to_ros_system_time + fcu_bridge: mavros_2.14.0_ttyTHS2_921600_highres_imu105_requested_200hz + +camera: + model: Intel RealSense D435I + width: 640 + height: 360 + rate_hz: 90.0 + distortion_model: plumb_bob + runtime_intrinsics_source: realsense_camera_info + left: + frame_id: camera_infra1_optical_frame + D: [0.0, 0.0, 0.0, 0.0, 0.0] + K: [323.1030578613281, 0.0, 319.8547058105469, + 0.0, 323.1030578613281, 184.5459442138672, + 0.0, 0.0, 1.0] + R: [1.0, 0.0, 0.0, + 0.0, 1.0, 0.0, + 0.0, 0.0, 1.0] + P: [323.1030578613281, 0.0, 319.8547058105469, 0.0, + 0.0, 323.1030578613281, 184.5459442138672, 0.0, + 0.0, 0.0, 1.0, 0.0] + right: + frame_id: camera_infra2_optical_frame + recorded_camera_info_frame_bug: camera_infra1_optical_frame + D: [0.0, 0.0, 0.0, 0.0, 0.0] + K: [323.1030578613281, 0.0, 319.8547058105469, + 0.0, 323.1030578613281, 184.5459442138672, + 0.0, 0.0, 1.0] + R: [1.0, 0.0, 0.0, + 0.0, 1.0, 0.0, + 0.0, 0.0, 1.0] + P: [323.1030578613281, 0.0, 319.8547058105469, -16.174942016601562, + 0.0, 323.1030578613281, 184.5459442138672, 0.0, + 0.0, 0.0, 1.0, 0.0] + stereo: + baseline_m: 0.050061247094552876 + T_right_left: + [1.0, 0.0, 0.0, -0.050061247094552876, + 0.0, 1.0, 0.0, 0.0, + 0.0, 0.0, 1.0, 0.0, + 0.0, 0.0, 0.0, 1.0] + +imu: + source_topic: /mavros/imu/data_raw + aligned_topic: /fcu/imu/data_raw_aligned + source_frame_id: base_link + runtime_frame_id: fcu_imu + measured_rate_hz: 170.0 + hardware_id: px4-highres-imu-105-ttyTHS2 + axes_contract: source measurements are already expressed in the Kalibr IMU axes; relay renames but does not rotate + time_alignment: + kalibr_timeshift_cam_imu_sec: -0.001737986760008108 + kalibr_equation: t_imu = t_cam + timeshift_cam_imu + runtime_equation: t_aligned = t_imu_raw + imu_to_camera_offset_ns + imu_to_camera_offset_ns: 1737987 + noise: + status: required_external_allan_calibration + gyroscope_noise_density: null + gyroscope_random_walk: null + accelerometer_noise_density: null + accelerometer_random_walk: null + +extrinsics: + convention: p_camera_infra1_optical = T_camera_infra1_optical_fcu_imu * p_fcu_imu + parent_frame: camera_infra1_optical_frame + child_frame: fcu_imu + translation_m: [0.027362927932, 0.052851887430, -0.062141618519] + rotation_xyzw: + [0.5018487706479298, -0.5094252342586629, + 0.4906496324298315, 0.4978922376339477] + T_parent_child: + [-0.000502262206, -0.999890141869, -0.014813909877, 0.027362927932, + -0.022727568330, 0.014821499191, -0.999631822622, 0.052851887430, + 0.999741569292, -0.000165393136, -0.022732515801, -0.062141618519, + 0.0, 0.0, 0.0, 1.0] diff --git a/integrations/isaac_ros_3_2_yopo/isaac_ros_yopo_bringup/config/px4_imu_noise_allan.template.yaml b/integrations/isaac_ros_3_2_yopo/isaac_ros_yopo_bringup/config/px4_imu_noise_allan.template.yaml new file mode 100644 index 0000000..4086ed0 --- /dev/null +++ b/integrations/isaac_ros_3_2_yopo/isaac_ros_yopo_bringup/config/px4_imu_noise_allan.template.yaml @@ -0,0 +1,26 @@ +schema_version: 1 +calibration_id: REPLACE_WITH_UNIQUE_ALLAN_CALIBRATION_ID +validated: true + +sensor: + hardware_id: px4-highres-imu-105-ttyTHS2 + sample_rate_hz: 170.0 + +method: + name: allan_deviation + # This file must exist. Relative paths are resolved from this YAML directory. + source_artifact: REPLACE_WITH_SOURCE_BAG_OR_CSV + # SHA-256 must match the exact bytes of source_artifact. + source_sha256: REPLACE_WITH_64_HEX_DIGIT_SHA256 + +units: + gyroscope_noise_density: rad/(s*sqrt(Hz)) + gyroscope_random_walk: rad/(s^2*sqrt(Hz)) + accelerometer_noise_density: m/(s^2*sqrt(Hz)) + accelerometer_random_walk: m/(s^3*sqrt(Hz)) + +parameters: + gyroscope_noise_density: REPLACE_WITH_ALLAN_VALUE + gyroscope_random_walk: REPLACE_WITH_ALLAN_VALUE + accelerometer_noise_density: REPLACE_WITH_ALLAN_VALUE + accelerometer_random_walk: REPLACE_WITH_ALLAN_VALUE diff --git a/integrations/isaac_ros_3_2_yopo/isaac_ros_yopo_bringup/config/px4_imu_noise_unvalidated.yaml b/integrations/isaac_ros_3_2_yopo/isaac_ros_yopo_bringup/config/px4_imu_noise_unvalidated.yaml new file mode 100644 index 0000000..7cb41af --- /dev/null +++ b/integrations/isaac_ros_3_2_yopo/isaac_ros_yopo_bringup/config/px4_imu_noise_unvalidated.yaml @@ -0,0 +1,24 @@ +schema_version: 1 +calibration_id: seeker_imu_kalibr_input_assumption_unvalidated +validated: false + +sensor: + hardware_id: px4-highres-imu-105-ttyTHS2 + sample_rate_hz: 170.0 + +method: + name: kalibr_input_assumption + source_artifact: seeker_imu.yaml + source_sha256: null + +units: + gyroscope_noise_density: rad/(s*sqrt(Hz)) + gyroscope_random_walk: rad/(s^2*sqrt(Hz)) + accelerometer_noise_density: m/(s^2*sqrt(Hz)) + accelerometer_random_walk: m/(s^3*sqrt(Hz)) + +parameters: + gyroscope_noise_density: 0.06 + gyroscope_random_walk: 0.001 + accelerometer_noise_density: 0.09 + accelerometer_random_walk: 0.05 diff --git a/integrations/isaac_ros_3_2_yopo/isaac_ros_yopo_bringup/isaac_ros_yopo_bringup/__init__.py b/integrations/isaac_ros_3_2_yopo/isaac_ros_yopo_bringup/isaac_ros_yopo_bringup/__init__.py new file mode 100644 index 0000000..602765b --- /dev/null +++ b/integrations/isaac_ros_3_2_yopo/isaac_ros_yopo_bringup/isaac_ros_yopo_bringup/__init__.py @@ -0,0 +1 @@ +"""Isaac ROS 3.2 bringup for D435i stereo and an external PX4 IMU.""" diff --git a/integrations/isaac_ros_3_2_yopo/isaac_ros_yopo_bringup/isaac_ros_yopo_bringup/aligned_imu_relay.py b/integrations/isaac_ros_3_2_yopo/isaac_ros_yopo_bringup/isaac_ros_yopo_bringup/aligned_imu_relay.py new file mode 100644 index 0000000..ea534f6 --- /dev/null +++ b/integrations/isaac_ros_3_2_yopo/isaac_ros_yopo_bringup/isaac_ros_yopo_bringup/aligned_imu_relay.py @@ -0,0 +1,502 @@ +"""Publish a strictly monotonic, time-aligned copy of the PX4 IMU stream.""" + +from collections import deque +import math +import time +from typing import Deque, Dict, Optional + +from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus, KeyValue +import rclpy +from rcl_interfaces.msg import ParameterDescriptor +from rclpy.node import Node +from rclpy.qos import qos_profile_sensor_data +from sensor_msgs.msg import Imu + +from .time_alignment import ( + ConsecutiveFailureGate, + StampOrder, + StrictStampGuard, + TimestampContractError, + add_offset_nanoseconds, + clock_residual_nanoseconds, + clone_with_aligned_stamp, + normalize_absolute_topic, + stamp_to_nanoseconds, + validate_frame_id, +) + + +def imu_measurements_are_finite(message: Imu) -> bool: + """Return whether every IMU measurement and covariance value is finite.""" + values = ( + message.orientation.x, + message.orientation.y, + message.orientation.z, + message.orientation.w, + *message.orientation_covariance, + message.angular_velocity.x, + message.angular_velocity.y, + message.angular_velocity.z, + *message.angular_velocity_covariance, + message.linear_acceleration.x, + message.linear_acceleration.y, + message.linear_acceleration.z, + *message.linear_acceleration_covariance, + ) + try: + return all(math.isfinite(value) for value in values) + except (TypeError, ValueError): + return False + + +class AlignedImuRelay(Node): + """Apply one audited constant clock offset without modifying IMU samples.""" + + def __init__(self) -> None: + super().__init__("aligned_fcu_imu_relay") + + self._input_topic = normalize_absolute_topic( + self._read_only_parameter( + "input_topic", + "/mavros/imu/data_raw", + "Absolute raw MAVROS IMU topic.", + ) + ) + self._output_topic = normalize_absolute_topic( + self._read_only_parameter( + "output_topic", + "/fcu/imu/data_raw_aligned", + "Absolute aligned IMU topic consumed by cuVSLAM.", + ) + ) + if self._input_topic == self._output_topic: + raise RuntimeError("input_topic and output_topic must differ") + + self._expected_input_frame = validate_frame_id( + self._read_only_parameter( + "expected_input_frame_id", + "base_link", + "Frame that names the calibrated MAVROS IMU measurement axes.", + ) + ) + self._output_frame = validate_frame_id( + self._read_only_parameter( + "output_frame_id", + "fcu_imu", + "Dedicated runtime name for the same calibrated IMU axes.", + ) + ) + self._offset_ns = self._read_only_parameter( + "imu_to_camera_offset_ns", + 1_737_987, + "Integer offset in t_aligned = t_imu_raw + offset.", + ) + if isinstance(self._offset_ns, bool) or not isinstance(self._offset_ns, int): + raise RuntimeError("imu_to_camera_offset_ns must be an integer") + if abs(self._offset_ns) > 100_000_000: + raise RuntimeError("imu_to_camera_offset_ns exceeds the 100 ms safety bound") + + self._expected_rate_hz = float( + self._read_only_parameter( + "expected_rate_hz", + 170.0, + "Measured external IMU rate used by the health diagnostic.", + ) + ) + self._rate_tolerance_ratio = float( + self._read_only_parameter( + "rate_tolerance_ratio", + 0.15, + "Allowed relative stamp-rate error before diagnostics warn.", + ) + ) + self._maximum_gap_ratio = float( + self._read_only_parameter( + "maximum_gap_ratio", + 3.0, + "Maximum accepted stamp gap as a multiple of the expected period.", + ) + ) + self._diagnostic_period_sec = float( + self._read_only_parameter( + "diagnostic_period_sec", + 1.0, + "Diagnostic publication period.", + ) + ) + self._stale_after_sec = float( + self._read_only_parameter( + "stale_after_sec", + 2.0, + "Maximum input silence before diagnostics report an error.", + ) + ) + self._startup_timeout_sec = float( + self._read_only_parameter( + "startup_timeout_sec", + 15.0, + "Maximum startup wait for the first publishable FCU IMU sample.", + ) + ) + self._maximum_receipt_time_residual_sec = float( + self._read_only_parameter( + "maximum_receipt_time_residual_sec", + 0.25, + "Maximum raw IMU stamp residual against host system time.", + ) + ) + self._hardware_id = str( + self._read_only_parameter( + "hardware_id", + "px4-highres-imu-105-ttyTHS2", + "Stable external IMU hardware/transport identifier.", + ) + ).strip() + numeric_values = ( + self._expected_rate_hz, + self._rate_tolerance_ratio, + self._maximum_gap_ratio, + self._diagnostic_period_sec, + self._stale_after_sec, + self._startup_timeout_sec, + self._maximum_receipt_time_residual_sec, + ) + if not all(math.isfinite(value) for value in numeric_values): + raise RuntimeError("relay numeric parameters must be finite") + if self._expected_rate_hz <= 0.0: + raise RuntimeError("expected_rate_hz must be positive") + if not 0.0 < self._rate_tolerance_ratio < 1.0: + raise RuntimeError("rate_tolerance_ratio must be in (0, 1)") + if not 1.0 < self._maximum_gap_ratio <= 10.0: + raise RuntimeError("maximum_gap_ratio must be in (1, 10]") + if self._diagnostic_period_sec <= 0.0: + raise RuntimeError("diagnostic_period_sec must be positive") + if self._stale_after_sec <= self._diagnostic_period_sec: + raise RuntimeError("stale_after_sec must exceed diagnostic_period_sec") + if not self._stale_after_sec < self._startup_timeout_sec <= 120.0: + raise RuntimeError("startup_timeout_sec must exceed stale_after_sec and be <= 120") + if not 0.0 < self._maximum_receipt_time_residual_sec <= 1.0: + raise RuntimeError("maximum_receipt_time_residual_sec must be in (0, 1]") + if not self._hardware_id: + raise RuntimeError("hardware_id must be non-empty") + + self._guard = StrictStampGuard() + self._counts: Dict[str, int] = { + "received": 0, + "published": 0, + "zero_stamp": 0, + "invalid_stamp": 0, + "duplicate": 0, + "nonmonotonic": 0, + "frame_mismatch": 0, + "nonfinite_measurement": 0, + "clock_domain_mismatch": 0, + "aligned_out_of_range": 0, + } + self._recent_rejects: Dict[str, int] = { + name: 0 for name in self._counts if name not in {"received", "published"} + } + history_size = max(32, int(math.ceil(self._expected_rate_hz * 3.0))) + self._accepted_stamp_ns: Deque[int] = deque(maxlen=history_size) + self._last_input_stamp_ns: Optional[int] = None + self._last_output_stamp_ns: Optional[int] = None + self._last_clock_residual_ns: Optional[int] = None + self._last_receive_monotonic_ns: Optional[int] = None + self._last_publish_monotonic_ns: Optional[int] = None + self._maximum_recent_gap_ns = 0 + self._rate_gap_failure_gate = ConsecutiveFailureGate(3) + self._reject_failure_gate = ConsecutiveFailureGate(3) + self._started_monotonic_ns = time.monotonic_ns() + + self._output_publisher = self.create_publisher( + Imu, + self._output_topic, + qos_profile_sensor_data, + ) + self._diagnostic_publisher = self.create_publisher( + DiagnosticArray, + "/diagnostics", + 10, + ) + self._input_subscription = self.create_subscription( + Imu, + self._input_topic, + self._on_imu, + qos_profile_sensor_data, + ) + self._diagnostic_timer = self.create_timer( + self._diagnostic_period_sec, + self._publish_diagnostics, + ) + + self.get_logger().info( + "Aligned FCU IMU relay ready: " + f"{self._input_topic} [{self._expected_input_frame}] -> " + f"{self._output_topic} [{self._output_frame}], " + f"timestamp offset {self._offset_ns:+d} ns" + ) + + def _read_only_parameter(self, name: str, default_value, description: str): + descriptor = ParameterDescriptor(description=description, read_only=True) + return self.declare_parameter(name, default_value, descriptor).value + + def _reject(self, reason: str, message: str) -> None: + self._counts[reason] += 1 + self._recent_rejects[reason] += 1 + count = self._counts[reason] + if count == 1 or count & (count - 1) == 0: + self.get_logger().warning(f"{message}; rejected {reason} count={count}") + + def _on_imu(self, message: Imu) -> None: + now_monotonic_ns = time.monotonic_ns() + self._counts["received"] += 1 + self._last_receive_monotonic_ns = now_monotonic_ns + + if message.header.frame_id != self._expected_input_frame: + self._reject( + "frame_mismatch", + "input frame mismatch: " + f"expected {self._expected_input_frame!r}, got {message.header.frame_id!r}", + ) + return + + if not imu_measurements_are_finite(message): + self._reject( + "nonfinite_measurement", + "input IMU contains a non-finite measurement or covariance value", + ) + return + + try: + input_stamp_ns = stamp_to_nanoseconds( + message.header.stamp.sec, + message.header.stamp.nanosec, + ) + except TimestampContractError as error: + self._reject("invalid_stamp", str(error)) + return + if input_stamp_ns == 0: + self._reject("zero_stamp", "input timestamp is zero") + return + + try: + clock_residual_ns = clock_residual_nanoseconds( + input_stamp_ns, + time.time_ns(), + ) + except TimestampContractError as error: + self._reject("clock_domain_mismatch", str(error)) + return + self._last_clock_residual_ns = clock_residual_ns + if abs(clock_residual_ns) > round( + self._maximum_receipt_time_residual_sec * 1_000_000_000.0 + ): + self._reject( + "clock_domain_mismatch", + "raw IMU timestamp is not in the ROS system-time domain: " + f"residual={clock_residual_ns / 1e9:.6f}s", + ) + return + + order = self._guard.classify(input_stamp_ns) + if order is StampOrder.DUPLICATE: + self._reject("duplicate", f"duplicate input timestamp {input_stamp_ns}") + return + if order is StampOrder.NONMONOTONIC: + self._reject( + "nonmonotonic", + "input timestamp moved backwards: " + f"{input_stamp_ns} <= {self._guard.last_accepted_ns}", + ) + return + + try: + output_stamp_ns = add_offset_nanoseconds(input_stamp_ns, self._offset_ns) + if output_stamp_ns == 0: + raise TimestampContractError("aligned timestamp is zero") + output = clone_with_aligned_stamp( + message, + self._output_frame, + self._offset_ns, + ) + except TimestampContractError as error: + self._reject("aligned_out_of_range", str(error)) + return + + if self._last_input_stamp_ns is not None: + gap_ns = input_stamp_ns - self._last_input_stamp_ns + self._maximum_recent_gap_ns = max(self._maximum_recent_gap_ns, gap_ns) + self._output_publisher.publish(output) + self._guard.commit(input_stamp_ns) + self._last_input_stamp_ns = input_stamp_ns + self._last_output_stamp_ns = output_stamp_ns + self._last_publish_monotonic_ns = now_monotonic_ns + self._accepted_stamp_ns.append(input_stamp_ns) + self._counts["published"] += 1 + + def _stamp_rate_hz(self) -> Optional[float]: + if len(self._accepted_stamp_ns) < 2: + return None + duration_ns = self._accepted_stamp_ns[-1] - self._accepted_stamp_ns[0] + if duration_ns <= 0: + return None + return ( + (len(self._accepted_stamp_ns) - 1) + * 1_000_000_000.0 + / duration_ns + ) + + def _publish_diagnostics(self) -> None: + now_monotonic_ns = time.monotonic_ns() + startup_age_sec = ( + now_monotonic_ns - self._started_monotonic_ns + ) / 1_000_000_000.0 + if self._last_receive_monotonic_ns is None: + receive_age_sec = startup_age_sec + else: + receive_age_sec = ( + now_monotonic_ns - self._last_receive_monotonic_ns + ) / 1_000_000_000.0 + if self._last_publish_monotonic_ns is None: + publish_age_sec = startup_age_sec + else: + publish_age_sec = ( + now_monotonic_ns - self._last_publish_monotonic_ns + ) / 1_000_000_000.0 + stamp_rate_hz = self._stamp_rate_hz() + maximum_allowed_gap_ns = round( + self._maximum_gap_ratio * 1_000_000_000.0 / self._expected_rate_hz + ) + + recent_errors = sum( + self._recent_rejects[name] + for name in ( + "invalid_stamp", + "nonmonotonic", + "frame_mismatch", + "nonfinite_measurement", + "clock_domain_mismatch", + "aligned_out_of_range", + "zero_stamp", + ) + ) + recent_rejects = recent_errors + self._recent_rejects["duplicate"] + gap_is_bad = self._maximum_recent_gap_ns > maximum_allowed_gap_ns + rate_is_bad = stamp_rate_hz is not None and ( + abs(stamp_rate_hz - self._expected_rate_hz) / self._expected_rate_hz + > self._rate_tolerance_ratio + ) + fatal_reason = None + if self._counts["received"] == 0: + level = DiagnosticStatus.STALE + summary = "waiting for raw FCU IMU" + if startup_age_sec > self._startup_timeout_sec: + level = DiagnosticStatus.ERROR + summary = "raw FCU IMU did not appear before startup timeout" + fatal_reason = summary + elif receive_age_sec > self._stale_after_sec: + level = DiagnosticStatus.ERROR + summary = "raw FCU IMU input is stale" + fatal_reason = summary + elif self._counts["published"] == 0: + level = DiagnosticStatus.ERROR + summary = "raw IMU is present but no aligned sample has been published" + if startup_age_sec > self._startup_timeout_sec: + fatal_reason = summary + elif publish_age_sec > self._stale_after_sec: + level = DiagnosticStatus.ERROR + summary = "aligned FCU IMU output is stale" + fatal_reason = summary + elif recent_errors: + level = DiagnosticStatus.ERROR + summary = "unsafe IMU samples were rejected" + elif self._recent_rejects["duplicate"]: + level = DiagnosticStatus.WARN + summary = "duplicate IMU samples were rejected" + elif gap_is_bad: + level = DiagnosticStatus.WARN + summary = "aligned IMU stamp gap exceeded the configured limit" + elif rate_is_bad: + level = DiagnosticStatus.WARN + summary = "aligned IMU stamp rate is outside tolerance" + elif stamp_rate_hz is None: + level = DiagnosticStatus.WARN + summary = "waiting for enough aligned samples to measure rate" + else: + level = DiagnosticStatus.OK + summary = "aligned FCU IMU stream is healthy" + + reject_failure = self._reject_failure_gate.observe(recent_rejects > 0) + rate_gap_failure = self._rate_gap_failure_gate.observe(gap_is_bad or rate_is_bad) + if fatal_reason is None and reject_failure: + level = DiagnosticStatus.ERROR + summary = "unsafe IMU samples were rejected for three diagnostics" + fatal_reason = summary + elif fatal_reason is None and rate_gap_failure: + level = DiagnosticStatus.ERROR + summary = "aligned IMU rate/gap remained unhealthy for three diagnostics" + fatal_reason = summary + + status = DiagnosticStatus() + status.level = level + status.name = f"{self.get_fully_qualified_name()}: aligned FCU IMU" + status.message = summary + status.hardware_id = self._hardware_id + values = { + "input_topic": self._input_topic, + "output_topic": self._output_topic, + "expected_input_frame": self._expected_input_frame, + "output_frame": self._output_frame, + "imu_to_camera_offset_ns": self._offset_ns, + "offset_equation": "t_aligned = t_imu_raw + offset", + "expected_rate_hz": f"{self._expected_rate_hz:.6f}", + "maximum_receipt_time_residual_sec": ( + f"{self._maximum_receipt_time_residual_sec:.6f}" + ), + "last_clock_residual_ms": ( + "not_available" + if self._last_clock_residual_ns is None + else f"{self._last_clock_residual_ns / 1e6:.6f}" + ), + "stamp_rate_hz": ( + "not_available" if stamp_rate_hz is None else f"{stamp_rate_hz:.6f}" + ), + "last_receive_age_sec": f"{receive_age_sec:.6f}", + "last_publish_age_sec": f"{publish_age_sec:.6f}", + "maximum_recent_stamp_gap_ms": f"{self._maximum_recent_gap_ns / 1e6:.6f}", + "maximum_allowed_stamp_gap_ms": f"{maximum_allowed_gap_ns / 1e6:.6f}", + "consecutive_rate_gap_warning_cycles": self._rate_gap_failure_gate.count, + "consecutive_reject_warning_cycles": self._reject_failure_gate.count, + "last_input_stamp_ns": self._last_input_stamp_ns, + "last_output_stamp_ns": self._last_output_stamp_ns, + } + values.update(self._counts) + status.values = [KeyValue(key=key, value=str(value)) for key, value in values.items()] + + diagnostics = DiagnosticArray() + diagnostics.header.stamp = self.get_clock().now().to_msg() + diagnostics.status = [status] + self._diagnostic_publisher.publish(diagnostics) + + for name in self._recent_rejects: + self._recent_rejects[name] = 0 + self._maximum_recent_gap_ns = 0 + if fatal_reason is not None: + self.get_logger().fatal(f"{fatal_reason}; shutting down runtime bringup") + raise RuntimeError(fatal_reason) + + +def main(args=None) -> None: + rclpy.init(args=args) + node = None + try: + node = AlignedImuRelay() + rclpy.spin(node) + finally: + if node is not None: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/integrations/isaac_ros_3_2_yopo/isaac_ros_yopo_bringup/isaac_ros_yopo_bringup/calibration.py b/integrations/isaac_ros_3_2_yopo/isaac_ros_yopo_bringup/isaac_ros_yopo_bringup/calibration.py new file mode 100644 index 0000000..5109b79 --- /dev/null +++ b/integrations/isaac_ros_3_2_yopo/isaac_ros_yopo_bringup/isaac_ros_yopo_bringup/calibration.py @@ -0,0 +1,576 @@ +"""Load and validate the audited D435i/PX4 runtime calibration record.""" + +from dataclasses import dataclass +import hashlib +import math +import os +import re +from typing import Any, Mapping, Sequence, Tuple + +import yaml + +from .time_alignment import normalize_absolute_topic, validate_frame_id + + +APPROVED_CALIBRATION_STATUS = "approved" +CANDIDATE_CALIBRATION_STATUS = ( + "runtime_candidate_pending_allan_and_independent_repeatability" +) +REJECTED_CALIBRATION_STATUS = "rejected" +SUPPORTED_CALIBRATION_STATUSES = { + APPROVED_CALIBRATION_STATUS, + CANDIDATE_CALIBRATION_STATUS, + REJECTED_CALIBRATION_STATUS, +} + + +class UniqueKeySafeLoader(yaml.SafeLoader): + """Safe YAML loader that rejects ambiguous duplicate mapping keys.""" + + +def _construct_unique_mapping(loader, node, deep=False): + mapping = {} + for key_node, value_node in node.value: + key = loader.construct_object(key_node, deep=deep) + try: + duplicate = key in mapping + except TypeError as error: + raise ValueError("YAML mapping keys must be hashable") from error + if duplicate: + raise ValueError(f"duplicate YAML key: {key!r}") + mapping[key] = loader.construct_object(value_node, deep=deep) + return mapping + + +UniqueKeySafeLoader.add_constructor( + yaml.resolver.BaseResolver.DEFAULT_MAPPING_TAG, + _construct_unique_mapping, +) + + +@dataclass(frozen=True) +class StereoCameraCalibration: + width: int + height: int + rate_hz: float + distortion_model: str + left_frame: str + right_frame: str + right_recorded_frame_bug: str + left_d: Tuple[float, ...] + left_k: Tuple[float, ...] + left_r: Tuple[float, ...] + left_p: Tuple[float, ...] + right_d: Tuple[float, ...] + right_k: Tuple[float, ...] + right_r: Tuple[float, ...] + right_p: Tuple[float, ...] + + +@dataclass(frozen=True) +class RuntimeCalibration: + calibration_id: str + status: str + camera_serial: str + camera_firmware: str + camera: StereoCameraCalibration + imu_input_topic: str + imu_output_topic: str + imu_source_frame: str + imu_runtime_frame: str + imu_rate_hz: float + imu_hardware_id: str + imu_to_camera_offset_ns: int + kalibr_timeshift_cam_imu_sec: float + tf_parent_frame: str + tf_child_frame: str + tf_translation_m: Tuple[float, float, float] + tf_rotation_xyzw: Tuple[float, float, float, float] + transform_row_major: Tuple[float, ...] + + @property + def left_camera_frame(self) -> str: + return self.camera.left_frame + + @property + def right_camera_frame(self) -> str: + return self.camera.right_frame + + +@dataclass(frozen=True) +class ImuNoiseCalibration: + calibration_id: str + validated: bool + sensor_hardware_id: str + sample_rate_hz: float + method: str + source_artifact: str + source_sha256: str + gyroscope_noise_density: float + gyroscope_random_walk: float + accelerometer_noise_density: float + accelerometer_random_walk: float + + +def _mapping(value: Any, name: str) -> Mapping[str, Any]: + if not isinstance(value, Mapping): + raise ValueError(f"{name} must be a mapping") + return value + + +def _nonempty_string(value: Any, name: str) -> str: + if not isinstance(value, str) or not value.strip(): + raise ValueError(f"{name} must be a non-empty string") + return value.strip() + + +def _finite_number(value: Any, name: str) -> float: + if isinstance(value, bool) or not isinstance(value, (int, float)): + raise ValueError(f"{name} must be a YAML integer or floating-point value") + result = float(value) + if not math.isfinite(result): + raise ValueError(f"{name} must be finite") + return result + + +def _positive_integer(value: Any, name: str) -> int: + if isinstance(value, bool) or not isinstance(value, int) or value <= 0: + raise ValueError(f"{name} must be a positive integer") + return value + + +def _finite_sequence(value: Any, length: int, name: str) -> Tuple[float, ...]: + if isinstance(value, (str, bytes)) or not isinstance(value, Sequence): + raise ValueError(f"{name} must be a sequence") + if len(value) != length: + raise ValueError(f"{name} must contain {length} values") + return tuple( + _finite_number(item, f"{name}[{index}]") + for index, item in enumerate(value) + ) + + +def _determinant_3x3(matrix: Sequence[float]) -> float: + return ( + matrix[0] * (matrix[4] * matrix[8] - matrix[5] * matrix[7]) + - matrix[1] * (matrix[3] * matrix[8] - matrix[5] * matrix[6]) + + matrix[2] * (matrix[3] * matrix[7] - matrix[4] * matrix[6]) + ) + + +def _quaternion_matrix(quaternion: Sequence[float]) -> Tuple[float, ...]: + x, y, z, w = quaternion + return ( + 1.0 - 2.0 * (y * y + z * z), + 2.0 * (x * y - z * w), + 2.0 * (x * z + y * w), + 2.0 * (x * y + z * w), + 1.0 - 2.0 * (x * x + z * z), + 2.0 * (y * z - x * w), + 2.0 * (x * z - y * w), + 2.0 * (y * z + x * w), + 1.0 - 2.0 * (x * x + y * y), + ) + + +def _validate_rotation(matrix: Sequence[float], quaternion: Sequence[float]) -> None: + rows = (matrix[0:3], matrix[3:6], matrix[6:9]) + for row_index in range(3): + for column_index in range(3): + dot = sum( + rows[item][row_index] * rows[item][column_index] + for item in range(3) + ) + expected = 1.0 if row_index == column_index else 0.0 + if not math.isclose(dot, expected, abs_tol=1.0e-8): + raise ValueError("T_parent_child rotation is not orthonormal") + if not math.isclose(_determinant_3x3(matrix), 1.0, abs_tol=1.0e-8): + raise ValueError("T_parent_child rotation determinant is not +1") + + norm = math.sqrt(sum(value * value for value in quaternion)) + if not math.isclose(norm, 1.0, abs_tol=1.0e-8): + raise ValueError("rotation_xyzw is not a unit quaternion") + quaternion_matrix = _quaternion_matrix(quaternion) + if any( + not math.isclose(actual, expected, abs_tol=2.0e-9) + for actual, expected in zip(matrix, quaternion_matrix) + ): + raise ValueError("rotation_xyzw and T_parent_child disagree") + + +def _validate_factory_camera(camera: Mapping[str, Any]) -> StereoCameraCalibration: + if camera.get("model") != "Intel RealSense D435I": + raise ValueError("camera.model must identify the calibrated Intel RealSense D435I") + if camera.get("runtime_intrinsics_source") != "realsense_camera_info": + raise ValueError("runtime camera intrinsics must come from RealSense CameraInfo") + width = _positive_integer(camera.get("width"), "camera.width") + height = _positive_integer(camera.get("height"), "camera.height") + if width != 640 or height != 360: + raise ValueError("calibration is only valid for 640x360 rectified images") + rate_hz = _finite_number(camera.get("rate_hz"), "camera.rate_hz") + if not math.isclose(rate_hz, 90.0, abs_tol=1.0e-12): + raise ValueError("calibration is only valid for 90 Hz rectified images") + distortion_model = _nonempty_string( + camera.get("distortion_model"), + "camera.distortion_model", + ) + if distortion_model != "plumb_bob": + raise ValueError("factory rectified distortion_model must be plumb_bob") + + left = _mapping(camera.get("left"), "camera.left") + right = _mapping(camera.get("right"), "camera.right") + left_frame = validate_frame_id(_nonempty_string(left.get("frame_id"), "camera.left.frame_id")) + right_frame = validate_frame_id( + _nonempty_string(right.get("frame_id"), "camera.right.frame_id") + ) + if left_frame == right_frame: + raise ValueError("left and right physical camera frames must differ") + right_recorded_frame_bug = validate_frame_id( + _nonempty_string( + right.get("recorded_camera_info_frame_bug"), + "camera.right.recorded_camera_info_frame_bug", + ) + ) + if right_recorded_frame_bug != left_frame: + raise ValueError( + "the documented right CameraInfo frame bug must reuse the left optical frame" + ) + + left_k = _finite_sequence(left.get("K"), 9, "camera.left.K") + right_k = _finite_sequence(right.get("K"), 9, "camera.right.K") + left_p = _finite_sequence(left.get("P"), 12, "camera.left.P") + right_p = _finite_sequence(right.get("P"), 12, "camera.right.P") + identity_rotation = (1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0) + left_d = _finite_sequence(left.get("D"), 5, "camera.left.D") + right_d = _finite_sequence(right.get("D"), 5, "camera.right.D") + left_r = _finite_sequence(left.get("R"), 9, "camera.left.R") + right_r = _finite_sequence(right.get("R"), 9, "camera.right.R") + for side_name, distortion, rectification in ( + ("left", left_d, left_r), + ("right", right_d, right_r), + ): + if any(abs(value) > 1.0e-12 for value in distortion): + raise ValueError(f"camera.{side_name}.D must be zero for rectified images") + if any( + not math.isclose(actual, expected, abs_tol=1.0e-12) + for actual, expected in zip(rectification, identity_rotation) + ): + raise ValueError(f"camera.{side_name}.R must be identity") + if any( + not math.isclose(actual, expected, abs_tol=1.0e-12) + for actual, expected in zip(left_k, right_k) + ): + raise ValueError("factory rectified left and right K matrices must match") + if left_k[0] <= 0.0 or left_k[4] <= 0.0: + raise ValueError("factory rectified focal lengths must be positive") + if not 0.0 <= left_k[2] < width or not 0.0 <= left_k[5] < height: + raise ValueError("factory rectified principal point lies outside the image") + if not math.isclose(left_k[8], 1.0, abs_tol=1.0e-12): + raise ValueError("camera K homogeneous scale must be 1") + + expected_left_p = ( + left_k[0], left_k[1], left_k[2], 0.0, + left_k[3], left_k[4], left_k[5], 0.0, + left_k[6], left_k[7], left_k[8], 0.0, + ) + if any( + not math.isclose(actual, expected, abs_tol=1.0e-12) + for actual, expected in zip(left_p, expected_left_p) + ): + raise ValueError("camera.left.P disagrees with the rectified K matrix") + + stereo = _mapping(camera.get("stereo"), "camera.stereo") + baseline = _finite_number(stereo.get("baseline_m"), "camera.stereo.baseline_m") + if baseline <= 0.0: + raise ValueError("camera.stereo.baseline_m must be positive") + if not math.isclose(-right_p[3] / right_p[0], baseline, abs_tol=1.0e-12): + raise ValueError("right projection matrix and stereo baseline disagree") + expected_right_p = list(expected_left_p) + expected_right_p[3] = -right_p[0] * baseline + if any( + not math.isclose(actual, expected, abs_tol=1.0e-12) + for actual, expected in zip(right_p, expected_right_p) + ): + raise ValueError("camera.right.P disagrees with K and baseline") + + right_from_left = _finite_sequence( + stereo.get("T_right_left"), + 16, + "camera.stereo.T_right_left", + ) + expected_right_from_left = ( + 1.0, 0.0, 0.0, -baseline, + 0.0, 1.0, 0.0, 0.0, + 0.0, 0.0, 1.0, 0.0, + 0.0, 0.0, 0.0, 1.0, + ) + if any( + not math.isclose(actual, expected, abs_tol=1.0e-12) + for actual, expected in zip(right_from_left, expected_right_from_left) + ): + raise ValueError("T_right_left disagrees with the factory stereo baseline") + return StereoCameraCalibration( + width=width, + height=height, + rate_hz=rate_hz, + distortion_model=distortion_model, + left_frame=left_frame, + right_frame=right_frame, + right_recorded_frame_bug=right_recorded_frame_bug, + left_d=left_d, + left_k=left_k, + left_r=left_r, + left_p=left_p, + right_d=right_d, + right_k=right_k, + right_r=right_r, + right_p=right_p, + ) + + +def load_calibration(path) -> RuntimeCalibration: + """Load one version-controlled runtime calibration and reject ambiguity.""" + with open(path, "r", encoding="utf-8") as stream: + root = _mapping(yaml.load(stream, Loader=UniqueKeySafeLoader), "calibration") + + if root.get("schema_version") != 1: + raise ValueError("unsupported calibration schema_version") + calibration_id = _nonempty_string(root.get("calibration_id"), "calibration_id") + status = _nonempty_string(root.get("status"), "status") + if status not in SUPPORTED_CALIBRATION_STATUSES: + raise ValueError(f"unsupported calibration status: {status}") + + provenance = _mapping(root.get("provenance"), "provenance") + camera_serial = _nonempty_string( + provenance.get("camera_serial"), + "provenance.camera_serial", + ) + camera_firmware = _nonempty_string( + provenance.get("camera_firmware"), + "provenance.camera_firmware", + ) + for field in ( + "joint_calibration_bag", + "kalibr_camchain_result", + "camera_model", + "imu_clock_source", + "fcu_bridge", + ): + _nonempty_string(provenance.get(field), f"provenance.{field}") + + camera = _mapping(root.get("camera"), "camera") + camera_calibration = _validate_factory_camera(camera) + + imu = _mapping(root.get("imu"), "imu") + input_topic = normalize_absolute_topic( + _nonempty_string(imu.get("source_topic"), "imu.source_topic") + ) + output_topic = normalize_absolute_topic( + _nonempty_string(imu.get("aligned_topic"), "imu.aligned_topic") + ) + if input_topic == output_topic: + raise ValueError("raw and aligned IMU topics must differ") + source_frame = validate_frame_id( + _nonempty_string(imu.get("source_frame_id"), "imu.source_frame_id") + ) + runtime_frame = validate_frame_id( + _nonempty_string(imu.get("runtime_frame_id"), "imu.runtime_frame_id") + ) + imu_rate_hz = _finite_number(imu.get("measured_rate_hz"), "imu.measured_rate_hz") + if imu_rate_hz <= 0.0: + raise ValueError("imu.measured_rate_hz must be positive") + hardware_id = _nonempty_string(imu.get("hardware_id"), "imu.hardware_id") + + time_alignment = _mapping(imu.get("time_alignment"), "imu.time_alignment") + offset_ns = time_alignment.get("imu_to_camera_offset_ns") + if isinstance(offset_ns, bool) or not isinstance(offset_ns, int): + raise ValueError("imu_to_camera_offset_ns must be an integer") + kalibr_shift = _finite_number( + time_alignment.get("kalibr_timeshift_cam_imu_sec"), + "imu.time_alignment.kalibr_timeshift_cam_imu_sec", + ) + if round(-kalibr_shift * 1_000_000_000) != offset_ns: + raise ValueError("runtime offset has the wrong Kalibr sign or rounding") + + extrinsics = _mapping(root.get("extrinsics"), "extrinsics") + parent_frame = validate_frame_id( + _nonempty_string(extrinsics.get("parent_frame"), "extrinsics.parent_frame") + ) + child_frame = validate_frame_id( + _nonempty_string(extrinsics.get("child_frame"), "extrinsics.child_frame") + ) + if parent_frame != camera_calibration.left_frame or child_frame != runtime_frame: + raise ValueError("extrinsic frames disagree with camera/IMU runtime frames") + translation = _finite_sequence(extrinsics.get("translation_m"), 3, "translation_m") + quaternion = _finite_sequence(extrinsics.get("rotation_xyzw"), 4, "rotation_xyzw") + transform = _finite_sequence(extrinsics.get("T_parent_child"), 16, "T_parent_child") + if any( + not math.isclose(actual, expected, abs_tol=1.0e-12) + for actual, expected in zip(transform[12:16], (0.0, 0.0, 0.0, 1.0)) + ): + raise ValueError("T_parent_child has an invalid homogeneous bottom row") + matrix_rotation = ( + transform[0], transform[1], transform[2], + transform[4], transform[5], transform[6], + transform[8], transform[9], transform[10], + ) + matrix_translation = (transform[3], transform[7], transform[11]) + if any( + not math.isclose(actual, expected, abs_tol=1.0e-12) + for actual, expected in zip(matrix_translation, translation) + ): + raise ValueError("translation_m and T_parent_child disagree") + _validate_rotation(matrix_rotation, quaternion) + + return RuntimeCalibration( + calibration_id=calibration_id, + status=status, + camera_serial=camera_serial, + camera_firmware=camera_firmware, + camera=camera_calibration, + imu_input_topic=input_topic, + imu_output_topic=output_topic, + imu_source_frame=source_frame, + imu_runtime_frame=runtime_frame, + imu_rate_hz=imu_rate_hz, + imu_hardware_id=hardware_id, + imu_to_camera_offset_ns=offset_ns, + kalibr_timeshift_cam_imu_sec=kalibr_shift, + tf_parent_frame=parent_frame, + tf_child_frame=child_frame, + tf_translation_m=translation, + tf_rotation_xyzw=quaternion, + transform_row_major=transform, + ) + + +def assert_runtime_calibration_allowed( + calibration: RuntimeCalibration, + allow_candidate: bool, +) -> None: + """Reject unapproved calibration unless the one known candidate is explicit.""" + if not isinstance(allow_candidate, bool): + raise ValueError("allow_candidate must be a boolean") + if calibration.status == APPROVED_CALIBRATION_STATUS: + return + if calibration.status == CANDIDATE_CALIBRATION_STATUS: + if allow_candidate: + return + raise ValueError( + "calibration is a runtime candidate pending Allan calibration and " + "independent repeatability; explicitly set allow_candidate_calibration:=true" + ) + if calibration.status == REJECTED_CALIBRATION_STATUS: + raise ValueError("calibration status is rejected and cannot be overridden") + raise ValueError(f"unsupported calibration status: {calibration.status}") + + +def load_imu_noise( + path, + expected_hardware_id: str, + expected_rate_hz: float, +) -> ImuNoiseCalibration: + """Load one traceable IMU noise model instead of independent CLI values.""" + with open(path, "r", encoding="utf-8") as stream: + root = _mapping( + yaml.load(stream, Loader=UniqueKeySafeLoader), + "IMU noise calibration", + ) + if root.get("schema_version") != 1: + raise ValueError("unsupported IMU noise schema_version") + + calibration_id = _nonempty_string(root.get("calibration_id"), "calibration_id") + validated = root.get("validated") + if not isinstance(validated, bool): + raise ValueError("validated must be a boolean") + + sensor = _mapping(root.get("sensor"), "sensor") + hardware_id = _nonempty_string(sensor.get("hardware_id"), "sensor.hardware_id") + if hardware_id != expected_hardware_id: + raise ValueError( + f"IMU noise hardware mismatch: expected {expected_hardware_id}, got {hardware_id}" + ) + sample_rate_hz = _finite_number( + sensor.get("sample_rate_hz"), + "sensor.sample_rate_hz", + ) + if sample_rate_hz <= 0.0: + raise ValueError("sensor.sample_rate_hz must be positive") + if abs(sample_rate_hz - expected_rate_hz) / expected_rate_hz > 0.05: + raise ValueError("IMU noise sample rate differs from the runtime rate by more than 5%") + + method_record = _mapping(root.get("method"), "method") + method = _nonempty_string(method_record.get("name"), "method.name") + source_artifact = _nonempty_string( + method_record.get("source_artifact"), + "method.source_artifact", + ) + source_sha256_value = method_record.get("source_sha256") + if validated: + source_sha256 = _nonempty_string(source_sha256_value, "method.source_sha256") + if method != "allan_deviation": + raise ValueError("validated IMU noise method must be allan_deviation") + if not re.fullmatch(r"[0-9a-fA-F]{64}", source_sha256): + raise ValueError("validated method.source_sha256 must contain 64 hex digits") + artifact_path = source_artifact + if not os.path.isabs(artifact_path): + artifact_path = os.path.join(os.path.dirname(os.path.abspath(path)), artifact_path) + if not os.path.isfile(artifact_path): + raise ValueError(f"validated Allan source artifact does not exist: {artifact_path}") + digest = hashlib.sha256() + with open(artifact_path, "rb") as artifact_stream: + for chunk in iter(lambda: artifact_stream.read(1024 * 1024), b""): + digest.update(chunk) + if digest.hexdigest().lower() != source_sha256.lower(): + raise ValueError("validated Allan source artifact SHA-256 does not match") + else: + source_sha256 = "" if source_sha256_value is None else str(source_sha256_value).strip() + + expected_units = { + "gyroscope_noise_density": "rad/(s*sqrt(Hz))", + "gyroscope_random_walk": "rad/(s^2*sqrt(Hz))", + "accelerometer_noise_density": "m/(s^2*sqrt(Hz))", + "accelerometer_random_walk": "m/(s^3*sqrt(Hz))", + } + units = _mapping(root.get("units"), "units") + for name, expected_unit in expected_units.items(): + if units.get(name) != expected_unit: + raise ValueError(f"units.{name} must be {expected_unit}") + + parameters = _mapping(root.get("parameters"), "parameters") + values = tuple( + _finite_number(parameters.get(name), f"parameters.{name}") + for name in expected_units + ) + upper_bounds = (1.0, 1.0, 10.0, 10.0) + if any( + not 0.0 < value < upper_bound + for value, upper_bound in zip(values, upper_bounds) + ): + raise ValueError("IMU noise parameters are non-finite, non-positive, or out of bounds") + known_unvalidated = ( + (0.000244, 0.000019393, 0.001862, 0.003), + (0.06, 0.001, 0.09, 0.05), + ) + if validated and any( + all( + math.isclose(value, reference, rel_tol=1.0e-9, abs_tol=1.0e-12) + for value, reference in zip(values, known) + ) + for known in known_unvalidated + ): + raise ValueError("validated IMU noise cannot reuse a known default/input tuple") + + return ImuNoiseCalibration( + calibration_id=calibration_id, + validated=validated, + sensor_hardware_id=hardware_id, + sample_rate_hz=sample_rate_hz, + method=method, + source_artifact=source_artifact, + source_sha256=source_sha256, + gyroscope_noise_density=values[0], + gyroscope_random_walk=values[1], + accelerometer_noise_density=values[2], + accelerometer_random_walk=values[3], + ) diff --git a/integrations/isaac_ros_3_2_yopo/isaac_ros_yopo_bringup/isaac_ros_yopo_bringup/runtime_health_monitor.py b/integrations/isaac_ros_3_2_yopo/isaac_ros_yopo_bringup/isaac_ros_yopo_bringup/runtime_health_monitor.py new file mode 100644 index 0000000..0a193e8 --- /dev/null +++ b/integrations/isaac_ros_3_2_yopo/isaac_ros_yopo_bringup/isaac_ros_yopo_bringup/runtime_health_monitor.py @@ -0,0 +1,483 @@ +"""Fail closed when the calibrated D435i/cuVSLAM runtime contract breaks.""" + +import math +import os +import time +from typing import Dict, Optional, Sequence, Tuple + +from ament_index_python.packages import get_package_share_directory +from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus, KeyValue +from nav_msgs.msg import Odometry +import rclpy +from rcl_interfaces.msg import ParameterDescriptor +from rclpy.node import Node +from rclpy.qos import qos_profile_sensor_data +from sensor_msgs.msg import CameraInfo, Imu + +from .calibration import load_calibration +from .time_alignment import ( + TimestampContractError, + normalize_absolute_topic, + stamp_to_nanoseconds, + validate_frame_id, +) + + +PACKAGE_NAME = "isaac_ros_yopo_bringup" +DEFAULT_CALIBRATION_FILE = os.path.join( + get_package_share_directory(PACKAGE_NAME), + "config", + "d435i_243622070369_fcu_imu.yaml", +) + + +def odometry_measurements_are_finite(message: Odometry) -> bool: + """Return whether every pose, twist, and covariance value is finite.""" + values = ( + message.pose.pose.position.x, + message.pose.pose.position.y, + message.pose.pose.position.z, + message.pose.pose.orientation.x, + message.pose.pose.orientation.y, + message.pose.pose.orientation.z, + message.pose.pose.orientation.w, + *message.pose.covariance, + message.twist.twist.linear.x, + message.twist.twist.linear.y, + message.twist.twist.linear.z, + message.twist.twist.angular.x, + message.twist.twist.angular.y, + message.twist.twist.angular.z, + *message.twist.covariance, + ) + try: + return all(math.isfinite(value) for value in values) + except (TypeError, ValueError): + return False + + +class RuntimeHealthMonitor(Node): + """Continuously enforce live camera and cuVSLAM output invariants.""" + + def __init__(self) -> None: + super().__init__("d435i_cuvslam_runtime_health_monitor") + + calibration_file = str( + self._read_only_parameter( + "calibration_file", + DEFAULT_CALIBRATION_FILE, + "Version-controlled D435i/FCU runtime calibration YAML.", + ) + ).strip() + if not calibration_file: + raise RuntimeError("calibration_file must be non-empty") + self._calibration = load_calibration(calibration_file) + + self._left_topic = self._topic_parameter( + "left_camera_info_topic", + "/camera/infra1/camera_info", + "Rectified left infrared CameraInfo topic.", + ) + self._right_topic = self._topic_parameter( + "right_camera_info_topic", + "/camera/infra2/camera_info", + "Rectified right infrared CameraInfo topic.", + ) + self._camera_imu_topic = self._topic_parameter( + "camera_imu_topic", + "/camera/imu", + "D435i IMU topic that must remain silent.", + ) + self._odometry_topic = self._topic_parameter( + "odometry_topic", + "/visual_slam/tracking/odometry", + "cuVSLAM tracking odometry topic.", + ) + self._expected_odometry_frame = validate_frame_id( + str( + self._read_only_parameter( + "expected_odometry_frame_id", + "odom", + "Required cuVSLAM tracking odometry parent frame.", + ) + ) + ) + self._expected_odometry_child_frame = validate_frame_id( + str( + self._read_only_parameter( + "expected_odometry_child_frame_id", + "camera_link", + "Required cuVSLAM tracking odometry child frame.", + ) + ) + ) + topics = { + self._left_topic, + self._right_topic, + self._camera_imu_topic, + self._odometry_topic, + } + if len(topics) != 4: + raise RuntimeError("runtime health topics must be distinct") + + self._diagnostic_period_sec = self._positive_parameter( + "diagnostic_period_sec", + 1.0, + "Health evaluation and diagnostic publication period.", + ) + self._camera_info_stale_after_sec = self._positive_parameter( + "camera_info_stale_after_sec", + 2.0, + "Maximum silence allowed from either CameraInfo stream.", + ) + self._odometry_stale_after_sec = self._positive_parameter( + "odometry_stale_after_sec", + 2.0, + "Maximum silence allowed from cuVSLAM tracking odometry.", + ) + self._startup_timeout_sec = self._positive_parameter( + "startup_timeout_sec", + 30.0, + "Maximum wait for both CameraInfo streams and tracking odometry.", + ) + if self._diagnostic_period_sec >= min( + self._camera_info_stale_after_sec, + self._odometry_stale_after_sec, + ): + raise RuntimeError("stale thresholds must exceed diagnostic_period_sec") + if self._startup_timeout_sec <= max( + self._camera_info_stale_after_sec, + self._odometry_stale_after_sec, + ): + raise RuntimeError("startup_timeout_sec must exceed stale thresholds") + if self._startup_timeout_sec > 120.0: + raise RuntimeError("startup_timeout_sec must be <= 120 seconds") + + self._started_monotonic_ns = time.monotonic_ns() + self._last_seen_monotonic_ns: Dict[str, Optional[int]] = { + "left_camera_info": None, + "right_camera_info": None, + "odometry": None, + } + self._counts = { + "left_camera_info": 0, + "right_camera_info": 0, + "odometry": 0, + "forbidden_camera_imu": 0, + } + self._ready_logged = False + self._right_frame_bug_logged = False + self._fatal_reason: Optional[str] = None + self._last_odometry_stamp_ns: Optional[int] = None + + self._diagnostic_publisher = self.create_publisher( + DiagnosticArray, + "/diagnostics", + 10, + ) + self._left_subscription = self.create_subscription( + CameraInfo, + self._left_topic, + self._on_left_camera_info, + qos_profile_sensor_data, + ) + self._right_subscription = self.create_subscription( + CameraInfo, + self._right_topic, + self._on_right_camera_info, + qos_profile_sensor_data, + ) + self._camera_imu_subscription = self.create_subscription( + Imu, + self._camera_imu_topic, + self._on_forbidden_camera_imu, + qos_profile_sensor_data, + ) + self._odometry_subscription = self.create_subscription( + Odometry, + self._odometry_topic, + self._on_odometry, + qos_profile_sensor_data, + ) + self._diagnostic_timer = self.create_timer( + self._diagnostic_period_sec, + self._evaluate_health, + ) + + self.get_logger().info( + "Runtime health monitor armed for calibration " + f"{self._calibration.calibration_id}; D435i IMU samples are forbidden" + ) + + def _read_only_parameter(self, name: str, default_value, description: str): + descriptor = ParameterDescriptor(description=description, read_only=True) + return self.declare_parameter(name, default_value, descriptor).value + + def _topic_parameter(self, name: str, default_value: str, description: str) -> str: + value = str( + self._read_only_parameter(name, default_value, description) + ).strip() + return normalize_absolute_topic(value) + + def _positive_parameter( + self, + name: str, + default_value: float, + description: str, + ) -> float: + value = self._read_only_parameter(name, default_value, description) + if isinstance(value, bool) or not isinstance(value, (int, float)): + raise RuntimeError(f"{name} must be numeric") + result = float(value) + if not math.isfinite(result) or result <= 0.0: + raise RuntimeError(f"{name} must be finite and positive") + return result + + def _fail(self, reason: str) -> None: + if self._fatal_reason is None: + self._fatal_reason = reason + self.get_logger().fatal(f"{reason}; shutting down runtime bringup") + raise RuntimeError(self._fatal_reason) + + @staticmethod + def _sequence_matches( + actual: Sequence[float], + expected: Tuple[float, ...], + ) -> bool: + return len(actual) == len(expected) and all( + isinstance(value, (int, float)) + and not isinstance(value, bool) + and math.isfinite(float(value)) + and float(value) == reference + for value, reference in zip(actual, expected) + ) + + def _camera_info_mismatch( + self, + message: CameraInfo, + side: str, + ) -> Optional[str]: + camera = self._calibration.camera + if message.width != camera.width or message.height != camera.height: + return ( + f"{side} CameraInfo dimensions mismatch: expected " + f"{camera.width}x{camera.height}, got " + f"{message.width}x{message.height}" + ) + if message.distortion_model != camera.distortion_model: + return ( + f"{side} CameraInfo distortion_model mismatch: expected " + f"{camera.distortion_model!r}, got {message.distortion_model!r}" + ) + + expected_values = { + "D": camera.left_d if side == "left" else camera.right_d, + "K": camera.left_k if side == "left" else camera.right_k, + "R": camera.left_r if side == "left" else camera.right_r, + "P": camera.left_p if side == "left" else camera.right_p, + } + actual_values = { + "D": message.d, + "K": message.k, + "R": message.r, + "P": message.p, + } + for field_name, expected in expected_values.items(): + if not self._sequence_matches(actual_values[field_name], expected): + return f"{side} CameraInfo {field_name} mismatch" + + frame_id = message.header.frame_id + if side == "left": + if frame_id != camera.left_frame: + return ( + "left CameraInfo frame mismatch: expected " + f"{camera.left_frame!r}, got {frame_id!r}" + ) + else: + allowed_right_frames = { + camera.right_frame, + camera.right_recorded_frame_bug, + } + if frame_id not in allowed_right_frames: + return ( + "right CameraInfo frame mismatch: expected physical frame " + f"{camera.right_frame!r} or known RealSense reuse " + f"{camera.right_recorded_frame_bug!r}, got {frame_id!r}" + ) + return None + + def _accept_camera_info(self, message: CameraInfo, side: str) -> None: + mismatch = self._camera_info_mismatch(message, side) + if mismatch is not None: + self._fail(mismatch) + if ( + side == "right" + and message.header.frame_id + == self._calibration.camera.right_recorded_frame_bug + and not self._right_frame_bug_logged + ): + self.get_logger().warning( + "right CameraInfo reuses the left optical frame as expected for " + "this RealSense firmware; cuVSLAM uses the explicit physical frames" + ) + self._right_frame_bug_logged = True + key = f"{side}_camera_info" + self._counts[key] += 1 + self._last_seen_monotonic_ns[key] = time.monotonic_ns() + + def _on_left_camera_info(self, message: CameraInfo) -> None: + self._accept_camera_info(message, "left") + + def _on_right_camera_info(self, message: CameraInfo) -> None: + self._accept_camera_info(message, "right") + + def _on_forbidden_camera_imu(self, _message: Imu) -> None: + self._counts["forbidden_camera_imu"] += 1 + self._fail( + "received a D435i IMU sample although camera gyro/accel must be disabled" + ) + + def _on_odometry(self, message: Odometry) -> None: + if message.header.frame_id != self._expected_odometry_frame: + self._fail( + "odometry parent frame mismatch: expected " + f"{self._expected_odometry_frame!r}, got " + f"{message.header.frame_id!r}" + ) + if message.child_frame_id != self._expected_odometry_child_frame: + self._fail( + "odometry child frame mismatch: expected " + f"{self._expected_odometry_child_frame!r}, got " + f"{message.child_frame_id!r}" + ) + try: + stamp_ns = stamp_to_nanoseconds( + message.header.stamp.sec, + message.header.stamp.nanosec, + ) + except TimestampContractError as error: + self._fail(f"invalid odometry timestamp: {error}") + if stamp_ns == 0: + self._fail("odometry timestamp is zero") + if ( + self._last_odometry_stamp_ns is not None + and stamp_ns <= self._last_odometry_stamp_ns + ): + self._fail( + "odometry timestamp is not strictly increasing: " + f"{stamp_ns} <= {self._last_odometry_stamp_ns}" + ) + if not odometry_measurements_are_finite(message): + self._fail("odometry contains a non-finite pose, twist, or covariance value") + self._last_odometry_stamp_ns = stamp_ns + self._counts["odometry"] += 1 + self._last_seen_monotonic_ns["odometry"] = time.monotonic_ns() + + def _age_sec(self, key: str, now_monotonic_ns: int) -> Optional[float]: + last_seen = self._last_seen_monotonic_ns[key] + if last_seen is None: + return None + return (now_monotonic_ns - last_seen) / 1_000_000_000.0 + + def _publish_diagnostic( + self, + level: int, + summary: str, + ages: Dict[str, Optional[float]], + ) -> None: + status = DiagnosticStatus() + status.level = level + status.name = f"{self.get_fully_qualified_name()}: calibrated runtime" + status.message = summary + status.hardware_id = self._calibration.camera_serial + values = { + "calibration_id": self._calibration.calibration_id, + "left_camera_info_topic": self._left_topic, + "right_camera_info_topic": self._right_topic, + "camera_imu_topic": self._camera_imu_topic, + "odometry_topic": self._odometry_topic, + "expected_odometry_frame": self._expected_odometry_frame, + "expected_odometry_child_frame": self._expected_odometry_child_frame, + "last_odometry_stamp_ns": self._last_odometry_stamp_ns, + "known_right_frame_reuse_observed": self._right_frame_bug_logged, + } + values.update(self._counts) + values.update({ + f"{name}_age_sec": "not_received" if age is None else f"{age:.6f}" + for name, age in ages.items() + }) + status.values = [ + KeyValue(key=key, value=str(value)) for key, value in values.items() + ] + diagnostics = DiagnosticArray() + diagnostics.header.stamp = self.get_clock().now().to_msg() + diagnostics.status = [status] + self._diagnostic_publisher.publish(diagnostics) + + def _evaluate_health(self) -> None: + if self._fatal_reason is not None: + raise RuntimeError(self._fatal_reason) + + now_monotonic_ns = time.monotonic_ns() + startup_age_sec = ( + now_monotonic_ns - self._started_monotonic_ns + ) / 1_000_000_000.0 + ages = { + name: self._age_sec(name, now_monotonic_ns) + for name in self._last_seen_monotonic_ns + } + stale = [] + for name in ("left_camera_info", "right_camera_info"): + if ( + ages[name] is not None + and ages[name] > self._camera_info_stale_after_sec + ): + stale.append(name) + if ( + ages["odometry"] is not None + and ages["odometry"] > self._odometry_stale_after_sec + ): + stale.append("odometry") + if stale: + summary = "required runtime streams are stale: " + ", ".join(stale) + self._publish_diagnostic(DiagnosticStatus.ERROR, summary, ages) + self._fail(summary) + + missing = [name for name, age in ages.items() if age is None] + if missing: + summary = "waiting for required streams: " + ", ".join(missing) + self._publish_diagnostic(DiagnosticStatus.STALE, summary, ages) + if startup_age_sec > self._startup_timeout_sec: + self._fail( + "required runtime streams did not appear before startup timeout: " + + ", ".join(missing) + ) + return + + self._publish_diagnostic( + DiagnosticStatus.OK, + "calibrated D435i CameraInfo and cuVSLAM odometry are healthy", + ages, + ) + if not self._ready_logged: + self.get_logger().info( + "[PASS] Both CameraInfo streams match calibration and cuVSLAM " + "tracking odometry is live" + ) + self._ready_logged = True + + +def main(args=None) -> None: + rclpy.init(args=args) + node = None + try: + node = RuntimeHealthMonitor() + rclpy.spin(node) + finally: + if node is not None: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/integrations/isaac_ros_3_2_yopo/isaac_ros_yopo_bringup/isaac_ros_yopo_bringup/time_alignment.py b/integrations/isaac_ros_3_2_yopo/isaac_ros_yopo_bringup/isaac_ros_yopo_bringup/time_alignment.py new file mode 100644 index 0000000..e1ac465 --- /dev/null +++ b/integrations/isaac_ros_3_2_yopo/isaac_ros_yopo_bringup/isaac_ros_yopo_bringup/time_alignment.py @@ -0,0 +1,161 @@ +"""Pure contracts used by the aligned external-IMU relay.""" + +import copy +from dataclasses import dataclass +from enum import Enum +from typing import Any, Optional, Tuple + + +NANOSECONDS_PER_SECOND = 1_000_000_000 +MAX_TIME_SECOND = 2_147_483_647 +MAX_TIME_NANOSECONDS = ( + MAX_TIME_SECOND * NANOSECONDS_PER_SECOND + NANOSECONDS_PER_SECOND - 1 +) + + +class TimestampContractError(ValueError): + """Raised when a timestamp cannot satisfy the runtime clock contract.""" + + +class StampOrder(Enum): + """Relationship between an input stamp and the last accepted stamp.""" + + ACCEPT = "accept" + DUPLICATE = "duplicate" + NONMONOTONIC = "nonmonotonic" + + +def _require_integer(name: str, value: Any) -> int: + if isinstance(value, bool) or not isinstance(value, int): + raise TimestampContractError(f"{name} must be an integer") + return value + + +def stamp_to_nanoseconds(sec: int, nanosec: int) -> int: + """Convert a non-negative ROS system-time stamp using integer arithmetic.""" + sec = _require_integer("sec", sec) + nanosec = _require_integer("nanosec", nanosec) + if sec < 0: + raise TimestampContractError("sec must be non-negative for system time") + if not 0 <= nanosec < NANOSECONDS_PER_SECOND: + raise TimestampContractError("nanosec must be in [0, 1000000000)") + if sec > MAX_TIME_SECOND: + raise TimestampContractError("sec exceeds builtin_interfaces/Time range") + return sec * NANOSECONDS_PER_SECOND + nanosec + + +def add_offset_nanoseconds(stamp_ns: int, offset_ns: int) -> int: + """Add a signed constant offset and enforce builtin_interfaces/Time bounds.""" + stamp_ns = _require_integer("stamp_ns", stamp_ns) + offset_ns = _require_integer("offset_ns", offset_ns) + if not 0 <= stamp_ns <= MAX_TIME_NANOSECONDS: + raise TimestampContractError("input timestamp is outside ROS Time range") + aligned_ns = stamp_ns + offset_ns + if aligned_ns < 0: + raise TimestampContractError("aligned timestamp is negative") + if aligned_ns > MAX_TIME_NANOSECONDS: + raise TimestampContractError("aligned timestamp exceeds ROS Time range") + return aligned_ns + + +def split_nanoseconds(stamp_ns: int) -> Tuple[int, int]: + """Split checked nanoseconds into canonical ROS Time fields.""" + stamp_ns = _require_integer("stamp_ns", stamp_ns) + if not 0 <= stamp_ns <= MAX_TIME_NANOSECONDS: + raise TimestampContractError("timestamp is outside ROS Time range") + return divmod(stamp_ns, NANOSECONDS_PER_SECOND) + + +def clock_residual_nanoseconds(stamp_ns: int, reference_ns: int) -> int: + """Return a checked signed residual against the active ROS system clock.""" + stamp_ns = _require_integer("stamp_ns", stamp_ns) + reference_ns = _require_integer("reference_ns", reference_ns) + if not 0 <= stamp_ns <= MAX_TIME_NANOSECONDS: + raise TimestampContractError("sensor timestamp is outside ROS Time range") + if not 0 <= reference_ns <= MAX_TIME_NANOSECONDS: + raise TimestampContractError("reference timestamp is outside ROS Time range") + return stamp_ns - reference_ns + + +def validate_frame_id(frame_id: str) -> str: + """Validate an unambiguous tf2 frame identifier.""" + if not isinstance(frame_id, str): + raise ValueError("frame_id must be a string") + frame_id = frame_id.strip() + if not frame_id or frame_id.startswith("/") or any(char.isspace() for char in frame_id): + raise ValueError("frame_id must be non-empty, relative, and contain no whitespace") + return frame_id + + +def normalize_absolute_topic(topic: str) -> str: + """Validate and normalize an absolute ROS topic used by this integration.""" + if not isinstance(topic, str): + raise ValueError("topic must be a string") + topic = topic.strip() + if not topic.startswith("/"): + raise ValueError("topic must be absolute") + parts = [part for part in topic.split("/") if part] + if not parts or any(any(char.isspace() for char in part) for part in parts): + raise ValueError("topic must contain a name and no whitespace") + return "/" + "/".join(parts) + + +def clone_with_aligned_stamp(message: Any, output_frame_id: str, offset_ns: int) -> Any: + """Deep-copy an IMU-like message and modify only its header stamp and frame.""" + output_frame_id = validate_frame_id(output_frame_id) + input_ns = stamp_to_nanoseconds( + message.header.stamp.sec, + message.header.stamp.nanosec, + ) + output_ns = add_offset_nanoseconds(input_ns, offset_ns) + output_sec, output_nanosec = split_nanoseconds(output_ns) + + output = copy.deepcopy(message) + output.header.stamp.sec = output_sec + output.header.stamp.nanosec = output_nanosec + output.header.frame_id = output_frame_id + return output + + +@dataclass +class StrictStampGuard: + """Track the last accepted input stamp without advancing on rejected samples.""" + + last_accepted_ns: Optional[int] = None + + def classify(self, stamp_ns: int) -> StampOrder: + stamp_ns = _require_integer("stamp_ns", stamp_ns) + if self.last_accepted_ns is None or stamp_ns > self.last_accepted_ns: + return StampOrder.ACCEPT + if stamp_ns == self.last_accepted_ns: + return StampOrder.DUPLICATE + return StampOrder.NONMONOTONIC + + def commit(self, stamp_ns: int) -> None: + stamp_ns = _require_integer("stamp_ns", stamp_ns) + if self.classify(stamp_ns) is not StampOrder.ACCEPT: + raise TimestampContractError("cannot commit a non-increasing timestamp") + self.last_accepted_ns = stamp_ns + + +@dataclass +class ConsecutiveFailureGate: + """Trip only after a condition remains unhealthy for a fixed number of checks.""" + + threshold: int + count: int = 0 + + def __post_init__(self) -> None: + if ( + isinstance(self.threshold, bool) + or not isinstance(self.threshold, int) + or self.threshold <= 0 + ): + raise ValueError("threshold must be a positive integer") + + def observe(self, unhealthy: bool) -> bool: + if unhealthy: + self.count += 1 + else: + self.count = 0 + return self.count >= self.threshold diff --git a/integrations/isaac_ros_3_2_yopo/isaac_ros_yopo_bringup/launch/d435i_fcu_imu_cuvslam.launch.py b/integrations/isaac_ros_3_2_yopo/isaac_ros_yopo_bringup/launch/d435i_fcu_imu_cuvslam.launch.py new file mode 100644 index 0000000..e7497b8 --- /dev/null +++ b/integrations/isaac_ros_3_2_yopo/isaac_ros_yopo_bringup/launch/d435i_fcu_imu_cuvslam.launch.py @@ -0,0 +1,379 @@ +"""Bring up D435i rectified stereo and a time-aligned PX4 IMU for cuVSLAM.""" + +import os + +from ament_index_python.packages import get_package_prefix, get_package_share_directory +from launch import LaunchDescription +from launch.actions import ( + DeclareLaunchArgument, + EmitEvent, + LogInfo, + OpaqueFunction, + RegisterEventHandler, + TimerAction, +) +from launch.event_handlers import OnProcessExit, OnProcessIO +from launch.events import Shutdown +from launch.logging import get_logger +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import ComposableNodeContainer, Node +from launch_ros.descriptions import ComposableNode + +from isaac_ros_yopo_bringup.calibration import ( + assert_runtime_calibration_allowed, + load_calibration, + load_imu_noise, +) + + +PACKAGE_NAME = "isaac_ros_yopo_bringup" +EXPECTED_VISUAL_SLAM_PREFIX = "/workspaces/isaac_ros-dev/install/isaac_ros_visual_slam" +VISUAL_SLAM_PATCH_MARKER = b"ISAAC_ROS_YOPO_IMU_TIMESTAMP_PATCH_V1" + + +def _parse_bool(value: str, name: str) -> bool: + normalized = value.strip().lower() + if normalized in {"1", "true", "yes", "on"}: + return True + if normalized in {"0", "false", "no", "off"}: + return False + raise RuntimeError(f"{name} must be true or false, got {value!r}") + + +def _shutdown_if_process_exits(action, label: str) -> RegisterEventHandler: + return RegisterEventHandler( + OnProcessExit( + target_action=action, + on_exit=[ + EmitEvent( + event=Shutdown(reason=f"required process exited: {label}"), + ) + ], + ) + ) + + +def _build_runtime_actions(context): + calibration_path = LaunchConfiguration("calibration_file").perform(context) + calibration = load_calibration(calibration_path) + allow_candidate_calibration = _parse_bool( + LaunchConfiguration("allow_candidate_calibration").perform(context), + "allow_candidate_calibration", + ) + try: + assert_runtime_calibration_allowed( + calibration, + allow_candidate_calibration, + ) + except ValueError as error: + raise RuntimeError(str(error)) from error + + visual_slam_prefix = get_package_prefix("isaac_ros_visual_slam") + if visual_slam_prefix != EXPECTED_VISUAL_SLAM_PREFIX: + raise RuntimeError( + "isaac_ros_visual_slam must resolve to the patched workspace overlay; " + f"expected {EXPECTED_VISUAL_SLAM_PREFIX}, got {visual_slam_prefix}" + ) + visual_slam_library = os.path.join( + visual_slam_prefix, + "lib", + "libvisual_slam_node.so", + ) + try: + with open(visual_slam_library, "rb") as stream: + patched_binary = VISUAL_SLAM_PATCH_MARKER in stream.read() + except OSError as error: + raise RuntimeError( + f"cannot read installed Visual SLAM library: {visual_slam_library}" + ) from error + if not patched_binary: + raise RuntimeError( + "installed libvisual_slam_node.so does not contain the required " + "IMU timestamp patch marker; rebuild isaac_ros_visual_slam" + ) + + enable_visualization = _parse_bool( + LaunchConfiguration("enable_visualization").perform(context), + "enable_visualization", + ) + allow_unvalidated_noise = _parse_bool( + LaunchConfiguration("allow_unvalidated_imu_noise").perform(context), + "allow_unvalidated_imu_noise", + ) + noise_file = LaunchConfiguration("imu_noise_file").perform(context) + noise = load_imu_noise( + noise_file, + calibration.imu_hardware_id, + calibration.imu_rate_hz, + ) + if not noise.validated and not allow_unvalidated_noise: + raise RuntimeError( + "the supplied IMU noise file is not validated; provide a traceable PX4 " + "Allan result, or explicitly set " + "allow_unvalidated_imu_noise:=true for a non-production plumbing test" + ) + + camera_node = Node( + package="realsense2_camera", + executable="realsense2_camera_node", + name="camera", + namespace="camera", + output="screen", + parameters=[{ + "serial_no": calibration.camera_serial, + "enable_infra1": True, + "enable_infra2": True, + "enable_color": False, + "enable_depth": False, + "depth_module.emitter_enabled": 0, + "depth_module.profile": "640x360x90", + "enable_gyro": False, + "enable_accel": False, + "unite_imu_method": 0, + "publish_tf": True, + }], + ) + + aligned_imu_relay = Node( + package=PACKAGE_NAME, + executable="aligned_imu_relay", + name="aligned_fcu_imu_relay", + output="screen", + parameters=[{ + "input_topic": calibration.imu_input_topic, + "output_topic": calibration.imu_output_topic, + "expected_input_frame_id": calibration.imu_source_frame, + "output_frame_id": calibration.imu_runtime_frame, + "imu_to_camera_offset_ns": calibration.imu_to_camera_offset_ns, + "expected_rate_hz": calibration.imu_rate_hz, + "rate_tolerance_ratio": 0.15, + "maximum_gap_ratio": 5.0, + "diagnostic_period_sec": 1.0, + "stale_after_sec": 2.0, + "startup_timeout_sec": 15.0, + "maximum_receipt_time_residual_sec": 0.25, + "hardware_id": calibration.imu_hardware_id, + }], + ) + + translation = calibration.tf_translation_m + quaternion = calibration.tf_rotation_xyzw + calibrated_imu_tf = Node( + package="tf2_ros", + executable="static_transform_publisher", + name="calibrated_fcu_imu_static_transform", + output="screen", + arguments=[ + "--x", f"{translation[0]:.17g}", + "--y", f"{translation[1]:.17g}", + "--z", f"{translation[2]:.17g}", + "--qx", f"{quaternion[0]:.17g}", + "--qy", f"{quaternion[1]:.17g}", + "--qz", f"{quaternion[2]:.17g}", + "--qw", f"{quaternion[3]:.17g}", + "--frame-id", calibration.tf_parent_frame, + "--child-frame-id", calibration.tf_child_frame, + ], + ) + + visual_slam_component = ComposableNode( + package="isaac_ros_visual_slam", + plugin="nvidia::isaac_ros::visual_slam::VisualSlamNode", + name="visual_slam_node", + parameters=[{ + "enable_image_denoising": False, + "rectified_images": True, + "num_cameras": 2, + "enable_imu_fusion": True, + "gyro_noise_density": noise.gyroscope_noise_density, + "gyro_random_walk": noise.gyroscope_random_walk, + "accel_noise_density": noise.accelerometer_noise_density, + "accel_random_walk": noise.accelerometer_random_walk, + "calibration_frequency": calibration.imu_rate_hz, + "image_jitter_threshold_ms": 22.0, + "imu_jitter_threshold_ms": 10.0, + "base_frame": "camera_link", + "imu_frame": calibration.imu_runtime_frame, + "camera_optical_frames": [ + calibration.left_camera_frame, + calibration.right_camera_frame, + ], + "enable_localization_n_mapping": True, + "enable_slam_visualization": enable_visualization, + "enable_landmarks_view": enable_visualization, + "enable_observations_view": enable_visualization, + }], + remappings=[ + ("visual_slam/image_0", "/camera/infra1/image_rect_raw"), + ("visual_slam/camera_info_0", "/camera/infra1/camera_info"), + ("visual_slam/image_1", "/camera/infra2/image_rect_raw"), + ("visual_slam/camera_info_1", "/camera/infra2/camera_info"), + ("visual_slam/imu", calibration.imu_output_topic), + ], + ) + visual_slam_container = ComposableNodeContainer( + name="visual_slam_launch_container", + namespace="", + package="rclcpp_components", + executable="component_container", + composable_node_descriptions=[visual_slam_component], + output="screen", + ) + runtime_health_monitor = Node( + package=PACKAGE_NAME, + executable="runtime_health_monitor", + name="d435i_cuvslam_runtime_health_monitor", + output="screen", + parameters=[{ + "calibration_file": calibration_path, + "startup_timeout_sec": 35.0, + "camera_info_stale_after_sec": 2.0, + "odometry_stale_after_sec": 2.0, + "expected_odometry_frame_id": "odom", + "expected_odometry_child_frame_id": "camera_link", + }], + ) + + initialization_state = {"ready": False, "output_buffer": ""} + marker_text = VISUAL_SLAM_PATCH_MARKER.decode("ascii") + + def on_visual_slam_output(event): + text = event.text + if isinstance(text, bytes): + text = text.decode(errors="replace") + if initialization_state["ready"]: + return [] + initialization_state["output_buffer"] += str(text) + if marker_text not in initialization_state["output_buffer"]: + initialization_state["output_buffer"] = initialization_state[ + "output_buffer" + ][-2 * len(marker_text):] + return [] + initialization_state["ready"] = True + return [ + LogInfo( + msg=( + "[PASS] Patched cuVSLAM tracker was constructed; " + "continuing runtime health and odometry checks." + ) + ) + ] + + def on_visual_slam_initialization_timeout(_context): + if initialization_state["ready"]: + return [] + get_logger(PACKAGE_NAME).error( + "[STOP] Patched cuVSLAM tracker did not initialize within 30 seconds." + ) + return [ + EmitEvent( + event=Shutdown(reason="cuVSLAM initialization readiness timeout"), + ) + ] + + initialization_handler = RegisterEventHandler( + OnProcessIO( + target_action=visual_slam_container, + on_stdout=on_visual_slam_output, + on_stderr=on_visual_slam_output, + ) + ) + initialization_timeout = TimerAction( + period=30.0, + actions=[OpaqueFunction(function=on_visual_slam_initialization_timeout)], + ) + + messages = [ + LogInfo(msg=f"Calibration: {calibration.calibration_id}"), + LogInfo(msg=f"Calibration status: {calibration.status}"), + LogInfo( + msg=( + f"Expected D435i serial={calibration.camera_serial}, " + f"firmware={calibration.camera_firmware}" + ) + ), + LogInfo( + msg=( + "IMU alignment: raw stamp + " + f"{calibration.imu_to_camera_offset_ns} ns; " + f"{calibration.tf_parent_frame} -> {calibration.tf_child_frame}" + ) + ), + LogInfo( + msg=( + f"IMU noise: {noise.calibration_id}; method={noise.method}; " + f"source={noise.source_artifact}; validated={noise.validated}" + ) + ), + ] + if not noise.validated: + messages.append( + LogInfo( + msg=( + "[WARNING] Unvalidated IMU noise was explicitly allowed. " + "This run is for plumbing tests only, not navigation acceptance." + ) + ) + ) + + required_processes = ( + (camera_node, "RealSense D435i"), + (aligned_imu_relay, "aligned FCU IMU relay"), + (calibrated_imu_tf, "calibrated FCU IMU static TF"), + (visual_slam_container, "cuVSLAM component container"), + (runtime_health_monitor, "calibrated runtime health monitor"), + ) + process_exit_handlers = [ + _shutdown_if_process_exits(action, label) + for action, label in required_processes + ] + actions = messages + process_exit_handlers + [ + initialization_handler, + camera_node, + aligned_imu_relay, + calibrated_imu_tf, + visual_slam_container, + runtime_health_monitor, + initialization_timeout, + ] + return actions + + +def generate_launch_description() -> LaunchDescription: + default_calibration = str( + os.path.join( + get_package_share_directory(PACKAGE_NAME), + "config", + "d435i_243622070369_fcu_imu.yaml", + ) + ) + return LaunchDescription([ + DeclareLaunchArgument( + "calibration_file", + default_value=default_calibration, + description="Version-controlled factory-rectified camera/FCU IMU calibration.", + ), + DeclareLaunchArgument( + "imu_noise_file", + description="Required schema-validated PX4 IMU Allan YAML file.", + ), + DeclareLaunchArgument( + "allow_candidate_calibration", + default_value="false", + description=( + "Allow the current calibration candidate pending Allan and " + "independent repeatability checks." + ), + ), + DeclareLaunchArgument( + "allow_unvalidated_imu_noise", + default_value="false", + description="Allow known placeholder noise only for a non-production smoke test.", + ), + DeclareLaunchArgument( + "enable_visualization", + default_value="false", + description="Enable cuVSLAM observation/landmark visualization publishers.", + ), + OpaqueFunction(function=_build_runtime_actions), + ]) diff --git a/integrations/isaac_ros_3_2_yopo/isaac_ros_yopo_bringup/package.xml b/integrations/isaac_ros_3_2_yopo/isaac_ros_yopo_bringup/package.xml new file mode 100644 index 0000000..00d2b1c --- /dev/null +++ b/integrations/isaac_ros_3_2_yopo/isaac_ros_yopo_bringup/package.xml @@ -0,0 +1,31 @@ + + + + isaac_ros_yopo_bringup + 0.1.0 + D435i rectified stereo and time-aligned PX4 IMU bringup for Isaac ROS Visual SLAM 3.2. + u5-4 + Apache-2.0 + + ament_python + + ament_index_python + diagnostic_msgs + isaac_ros_visual_slam + launch + launch_ros + nav_msgs + python3-yaml + rcl_interfaces + rclcpp_components + rclpy + realsense2_camera + sensor_msgs + tf2_ros + + python3-pytest + + + ament_python + + diff --git a/integrations/isaac_ros_3_2_yopo/isaac_ros_yopo_bringup/resource/isaac_ros_yopo_bringup b/integrations/isaac_ros_3_2_yopo/isaac_ros_yopo_bringup/resource/isaac_ros_yopo_bringup new file mode 100644 index 0000000..8b13789 --- /dev/null +++ b/integrations/isaac_ros_3_2_yopo/isaac_ros_yopo_bringup/resource/isaac_ros_yopo_bringup @@ -0,0 +1 @@ + diff --git a/integrations/isaac_ros_3_2_yopo/isaac_ros_yopo_bringup/setup.cfg b/integrations/isaac_ros_3_2_yopo/isaac_ros_yopo_bringup/setup.cfg new file mode 100644 index 0000000..9c21615 --- /dev/null +++ b/integrations/isaac_ros_3_2_yopo/isaac_ros_yopo_bringup/setup.cfg @@ -0,0 +1,5 @@ +[develop] +script_dir=$base/lib/isaac_ros_yopo_bringup + +[install] +install_scripts=$base/lib/isaac_ros_yopo_bringup diff --git a/integrations/isaac_ros_3_2_yopo/isaac_ros_yopo_bringup/setup.py b/integrations/isaac_ros_3_2_yopo/isaac_ros_yopo_bringup/setup.py new file mode 100644 index 0000000..efce199 --- /dev/null +++ b/integrations/isaac_ros_3_2_yopo/isaac_ros_yopo_bringup/setup.py @@ -0,0 +1,41 @@ +from glob import glob +import os + +from setuptools import find_packages, setup + + +PACKAGE_NAME = "isaac_ros_yopo_bringup" + + +setup( + name=PACKAGE_NAME, + version="0.1.0", + packages=find_packages(exclude=("test",)), + data_files=[ + ( + "share/ament_index/resource_index/packages", + [f"resource/{PACKAGE_NAME}"], + ), + (f"share/{PACKAGE_NAME}", ["package.xml"]), + (os.path.join("share", PACKAGE_NAME, "config"), glob("config/*.yaml")), + (os.path.join("share", PACKAGE_NAME, "launch"), glob("launch/*.launch.py")), + ], + install_requires=["setuptools"], + tests_require=["pytest"], + zip_safe=True, + maintainer="u5-4", + maintainer_email="u5-4@users.noreply.github.com", + description=( + "D435i rectified stereo and time-aligned PX4 IMU bringup for " + "Isaac ROS Visual SLAM 3.2" + ), + license="Apache-2.0", + entry_points={ + "console_scripts": [ + "aligned_imu_relay = " + "isaac_ros_yopo_bringup.aligned_imu_relay:main", + "runtime_health_monitor = " + "isaac_ros_yopo_bringup.runtime_health_monitor:main", + ], + }, +) diff --git a/integrations/isaac_ros_3_2_yopo/isaac_ros_yopo_bringup/test/test_calibration.py b/integrations/isaac_ros_3_2_yopo/isaac_ros_yopo_bringup/test/test_calibration.py new file mode 100644 index 0000000..b63016c --- /dev/null +++ b/integrations/isaac_ros_3_2_yopo/isaac_ros_yopo_bringup/test/test_calibration.py @@ -0,0 +1,295 @@ +import copy +import hashlib +import os +import tempfile +import unittest + +import yaml + +from isaac_ros_yopo_bringup.calibration import ( + assert_runtime_calibration_allowed, + load_calibration, + load_imu_noise, +) + + +PACKAGE_ROOT = os.path.dirname(os.path.dirname(os.path.abspath(__file__))) +CALIBRATION_PATH = os.path.join( + PACKAGE_ROOT, + "config", + "d435i_243622070369_fcu_imu.yaml", +) +UNVALIDATED_NOISE_PATH = os.path.join( + PACKAGE_ROOT, + "config", + "px4_imu_noise_unvalidated.yaml", +) + + +class RuntimeCalibrationTest(unittest.TestCase): + @classmethod + def setUpClass(cls): + cls.calibration = load_calibration(CALIBRATION_PATH) + + def test_identifies_the_fixed_camera(self): + self.assertEqual("243622070369", self.calibration.camera_serial) + self.assertEqual("5.15.1.55", self.calibration.camera_firmware) + + def test_uses_distinct_physical_camera_frames(self): + self.assertEqual( + "camera_infra1_optical_frame", + self.calibration.left_camera_frame, + ) + self.assertEqual( + "camera_infra2_optical_frame", + self.calibration.right_camera_frame, + ) + + def test_fixes_time_offset_sign_and_integer_rounding(self): + self.assertAlmostEqual( + -0.001737986760008108, + self.calibration.kalibr_timeshift_cam_imu_sec, + ) + self.assertEqual(1_737_987, self.calibration.imu_to_camera_offset_ns) + + def test_uses_dedicated_runtime_imu_frame_and_topic(self): + self.assertEqual("base_link", self.calibration.imu_source_frame) + self.assertEqual("fcu_imu", self.calibration.imu_runtime_frame) + self.assertEqual("/mavros/imu/data_raw", self.calibration.imu_input_topic) + self.assertEqual( + "/fcu/imu/data_raw_aligned", + self.calibration.imu_output_topic, + ) + + def test_transform_direction_is_camera_parent_to_imu_child(self): + self.assertEqual( + "camera_infra1_optical_frame", + self.calibration.tf_parent_frame, + ) + self.assertEqual("fcu_imu", self.calibration.tf_child_frame) + self.assertEqual( + (0.027362927932, 0.052851887430, -0.062141618519), + self.calibration.tf_translation_m, + ) + self.assertAlmostEqual( + 1.0, + sum(value * value for value in self.calibration.tf_rotation_xyzw), + places=12, + ) + + def test_uses_measured_not_requested_imu_rate(self): + self.assertEqual(170.0, self.calibration.imu_rate_hz) + + def test_factory_camera_info_is_versioned_exactly(self): + camera = self.calibration.camera + self.assertEqual((0.0,) * 5, camera.left_d) + self.assertEqual((0.0,) * 5, camera.right_d) + self.assertEqual( + ( + 323.1030578613281, + 0.0, + 319.8547058105469, + 0.0, + 323.1030578613281, + 184.5459442138672, + 0.0, + 0.0, + 1.0, + ), + camera.left_k, + ) + self.assertEqual(-16.174942016601562, camera.right_p[3]) + self.assertEqual(camera.left_frame, camera.right_recorded_frame_bug) + + def test_candidate_requires_an_explicit_runtime_override(self): + with self.assertRaisesRegex(ValueError, "runtime candidate"): + assert_runtime_calibration_allowed(self.calibration, False) + assert_runtime_calibration_allowed(self.calibration, True) + + def test_approved_status_is_allowed_without_override(self): + calibration = self._load_modified(status="approved") + assert_runtime_calibration_allowed(calibration, False) + + def test_rejected_status_cannot_be_overridden(self): + calibration = self._load_modified(status="rejected") + with self.assertRaisesRegex(ValueError, "cannot be overridden"): + assert_runtime_calibration_allowed(calibration, True) + + def test_rejects_unknown_calibration_status(self): + with self.assertRaisesRegex(ValueError, "unsupported calibration status"): + self._load_modified(status="looks_good") + + def test_rejects_duplicate_yaml_keys(self): + with open(CALIBRATION_PATH, "r", encoding="utf-8") as stream: + source = stream.read() + source = source.replace( + "schema_version: 1\n", + "schema_version: 1\nschema_version: 1\n", + 1, + ) + with tempfile.NamedTemporaryFile( + mode="w", + suffix=".yaml", + encoding="utf-8", + delete=False, + ) as stream: + stream.write(source) + path = stream.name + try: + with self.assertRaisesRegex(ValueError, "duplicate YAML key"): + load_calibration(path) + finally: + os.unlink(path) + + def test_rejects_boolean_camera_rate(self): + with self.assertRaisesRegex(ValueError, "YAML integer or floating-point"): + self._load_modified(camera={"rate_hz": True}) + + def test_rejects_projection_baseline_disagreement(self): + with self.assertRaisesRegex(ValueError, "projection matrix"): + self._load_modified(camera={"right": {"P": [ + 323.1030578613281, 0.0, 319.8547058105469, -15.0, + 0.0, 323.1030578613281, 184.5459442138672, 0.0, + 0.0, 0.0, 1.0, 0.0, + ]}}) + + def test_rejects_explicit_null_in_required_string(self): + with open(CALIBRATION_PATH, "r", encoding="utf-8") as stream: + data = yaml.safe_load(stream) + data["provenance"]["camera_serial"] = None + path = self._temporary_yaml(data) + try: + with self.assertRaises(ValueError): + load_calibration(path) + finally: + os.unlink(path) + + @classmethod + def _load_modified(cls, **changes): + with open(CALIBRATION_PATH, "r", encoding="utf-8") as stream: + data = yaml.safe_load(stream) + cls._deep_update(data, changes) + path = cls._temporary_yaml(data) + try: + return load_calibration(path) + finally: + os.unlink(path) + + @classmethod + def _deep_update(cls, target, updates): + for key, value in updates.items(): + if isinstance(value, dict) and isinstance(target.get(key), dict): + cls._deep_update(target[key], value) + else: + target[key] = value + + @staticmethod + def _temporary_yaml(data): + with tempfile.NamedTemporaryFile( + mode="w", + suffix=".yaml", + encoding="utf-8", + delete=False, + ) as stream: + yaml.safe_dump(data, stream, sort_keys=False) + return stream.name + + +class ImuNoiseCalibrationTest(unittest.TestCase): + def test_loads_bundled_values_only_as_unvalidated(self): + noise = load_imu_noise( + UNVALIDATED_NOISE_PATH, + "px4-highres-imu-105-ttyTHS2", + 170.0, + ) + self.assertFalse(noise.validated) + self.assertEqual("kalibr_input_assumption", noise.method) + self.assertEqual(0.06, noise.gyroscope_noise_density) + self.assertEqual(0.001, noise.gyroscope_random_walk) + self.assertEqual(0.09, noise.accelerometer_noise_density) + self.assertEqual(0.05, noise.accelerometer_random_walk) + + def test_rejects_noise_for_another_hardware_id(self): + with self.assertRaises(ValueError): + load_imu_noise( + UNVALIDATED_NOISE_PATH, + "another-imu", + 170.0, + ) + + def test_rejects_noise_from_a_different_sample_rate(self): + with self.assertRaises(ValueError): + load_imu_noise( + UNVALIDATED_NOISE_PATH, + "px4-highres-imu-105-ttyTHS2", + 200.0, + ) + + def test_rejects_known_input_tuple_claimed_as_validated_allan(self): + with tempfile.TemporaryDirectory() as directory: + path = self._validated_noise_yaml(directory, preserve_parameters=True) + with self.assertRaises(ValueError): + load_imu_noise( + path, + "px4-highres-imu-105-ttyTHS2", + 170.0, + ) + + def test_loads_traceable_validated_allan_result(self): + with tempfile.TemporaryDirectory() as directory: + path = self._validated_noise_yaml(directory) + noise = load_imu_noise( + path, + "px4-highres-imu-105-ttyTHS2", + 170.0, + ) + self.assertTrue(noise.validated) + self.assertEqual("allan_deviation", noise.method) + + def test_rejects_validated_allan_hash_mismatch(self): + with tempfile.TemporaryDirectory() as directory: + path = self._validated_noise_yaml(directory) + with open(path, "r", encoding="utf-8") as stream: + data = yaml.safe_load(stream) + data["method"]["source_sha256"] = "0" * 64 + with open(path, "w", encoding="utf-8") as stream: + yaml.safe_dump(data, stream, sort_keys=False) + with self.assertRaisesRegex(ValueError, "SHA-256"): + load_imu_noise( + path, + "px4-highres-imu-105-ttyTHS2", + 170.0, + ) + + @staticmethod + def _validated_noise_yaml(directory, preserve_parameters=False): + artifact_name = "px4_stationary_imu.csv" + artifact_path = os.path.join(directory, artifact_name) + artifact_contents = b"timestamp_ns,gx,gy,gz,ax,ay,az\n1,0,0,0,0,0,9.81\n" + with open(artifact_path, "wb") as stream: + stream.write(artifact_contents) + + with open(UNVALIDATED_NOISE_PATH, "r", encoding="utf-8") as stream: + data = copy.deepcopy(yaml.safe_load(stream)) + data["calibration_id"] = "px4_allan_test" + data["validated"] = True + data["method"] = { + "name": "allan_deviation", + "source_artifact": artifact_name, + "source_sha256": hashlib.sha256(artifact_contents).hexdigest(), + } + if not preserve_parameters: + data["parameters"] = { + "gyroscope_noise_density": 0.01, + "gyroscope_random_walk": 0.002, + "accelerometer_noise_density": 0.1, + "accelerometer_random_walk": 0.01, + } + path = os.path.join(directory, "validated_allan.yaml") + with open(path, "w", encoding="utf-8") as stream: + yaml.safe_dump(data, stream, sort_keys=False) + return path + + +if __name__ == "__main__": + unittest.main() diff --git a/integrations/isaac_ros_3_2_yopo/isaac_ros_yopo_bringup/test/test_launch_source.py b/integrations/isaac_ros_3_2_yopo/isaac_ros_yopo_bringup/test/test_launch_source.py new file mode 100644 index 0000000..946bdca --- /dev/null +++ b/integrations/isaac_ros_3_2_yopo/isaac_ros_yopo_bringup/test/test_launch_source.py @@ -0,0 +1,103 @@ +import ast +import os +import unittest + + +PACKAGE_ROOT = os.path.dirname(os.path.dirname(os.path.abspath(__file__))) +LAUNCH_PATH = os.path.join(PACKAGE_ROOT, "launch", "d435i_fcu_imu_cuvslam.launch.py") + + +class LaunchSourceContractTest(unittest.TestCase): + @classmethod + def setUpClass(cls): + with open(LAUNCH_PATH, "r", encoding="utf-8") as stream: + cls.source = stream.read() + cls.tree = ast.parse(cls.source) + + def test_disables_d435i_imu(self): + self.assertIn('"enable_gyro": False', self.source) + self.assertIn('"enable_accel": False', self.source) + self.assertIn('"unite_imu_method": 0', self.source) + self.assertNotIn('"/camera/imu"', self.source) + + def test_uses_official_visual_slam_component(self): + self.assertIn( + 'plugin="nvidia::isaac_ros::visual_slam::VisualSlamNode"', + self.source, + ) + self.assertIn('"enable_imu_fusion": True', self.source) + self.assertIn('"rectified_images": True', self.source) + + def test_explicitly_overrides_both_camera_frames(self): + self.assertIn('"camera_optical_frames"', self.source) + self.assertIn("calibration.left_camera_frame", self.source) + self.assertIn("calibration.right_camera_frame", self.source) + + def test_visual_slam_consumes_only_aligned_imu(self): + self.assertIn( + '("visual_slam/imu", calibration.imu_output_topic)', + self.source, + ) + self.assertNotIn( + '("visual_slam/imu", calibration.imu_input_topic)', + self.source, + ) + + def test_noise_file_is_required_and_cli_values_are_not_accepted(self): + required = {"imu_noise_file"} + declarations = {} + for node in ast.walk(self.tree): + if not isinstance(node, ast.Call): + continue + function_name = getattr(node.func, "id", "") + if function_name != "DeclareLaunchArgument" or not node.args: + continue + if not isinstance(node.args[0], ast.Constant): + continue + declarations[node.args[0].value] = { + keyword.arg for keyword in node.keywords + } + self.assertTrue(required.issubset(declarations)) + for name in required: + self.assertNotIn("default_value", declarations[name]) + for forbidden in ( + 'DeclareLaunchArgument(\n "gyro_noise_density"', + 'DeclareLaunchArgument(\n "gyro_random_walk"', + 'DeclareLaunchArgument(\n "accel_noise_density"', + 'DeclareLaunchArgument(\n "accel_random_walk"', + ): + self.assertNotIn(forbidden, self.source) + + def test_requires_patched_workspace_overlay(self): + self.assertIn( + "/workspaces/isaac_ros-dev/install/isaac_ros_visual_slam", + self.source, + ) + self.assertIn("ISAAC_ROS_YOPO_IMU_TIMESTAMP_PATCH_V1", self.source) + self.assertIn("libvisual_slam_node.so", self.source) + + def test_requires_successful_tracker_initialization(self): + self.assertIn("OnProcessIO", self.source) + self.assertIn("initialization_timeout = TimerAction", self.source) + self.assertIn("Patched cuVSLAM tracker was constructed", self.source) + + def test_candidate_calibration_requires_explicit_override(self): + self.assertIn('"allow_candidate_calibration"', self.source) + self.assertIn("assert_runtime_calibration_allowed", self.source) + self.assertIn("allow_candidate_calibration", self.source) + + def test_runtime_monitor_is_required(self): + self.assertIn('executable="runtime_health_monitor"', self.source) + self.assertIn( + '(runtime_health_monitor, "calibrated runtime health monitor")', + self.source, + ) + self.assertIn('"camera_info_stale_after_sec": 2.0', self.source) + self.assertIn('"odometry_stale_after_sec": 2.0', self.source) + + def test_gap_threshold_has_margin_over_the_recorded_maximum(self): + self.assertIn('"maximum_gap_ratio": 5.0', self.source) + + +if __name__ == "__main__": + unittest.main() diff --git a/integrations/isaac_ros_3_2_yopo/isaac_ros_yopo_bringup/test/test_relay_source.py b/integrations/isaac_ros_3_2_yopo/isaac_ros_yopo_bringup/test/test_relay_source.py new file mode 100644 index 0000000..f65cce3 --- /dev/null +++ b/integrations/isaac_ros_3_2_yopo/isaac_ros_yopo_bringup/test/test_relay_source.py @@ -0,0 +1,122 @@ +import ast +import math +import os +from types import SimpleNamespace +import unittest + + +PACKAGE_ROOT = os.path.dirname(os.path.dirname(os.path.abspath(__file__))) +RELAY_PATH = os.path.join( + PACKAGE_ROOT, + "isaac_ros_yopo_bringup", + "aligned_imu_relay.py", +) +LAUNCH_PATH = os.path.join(PACKAGE_ROOT, "launch", "d435i_fcu_imu_cuvslam.launch.py") + + +def make_imu_message(): + return SimpleNamespace( + orientation=SimpleNamespace(x=0.0, y=0.0, z=0.0, w=1.0), + orientation_covariance=[0.0] * 9, + angular_velocity=SimpleNamespace(x=0.1, y=0.2, z=0.3), + angular_velocity_covariance=[0.0] * 9, + linear_acceleration=SimpleNamespace(x=0.0, y=0.0, z=9.81), + linear_acceleration_covariance=[0.0] * 9, + ) + + +class RelaySourceContractTest(unittest.TestCase): + @classmethod + def setUpClass(cls): + with open(RELAY_PATH, "r", encoding="utf-8") as stream: + cls.relay_source = stream.read() + with open(LAUNCH_PATH, "r", encoding="utf-8") as stream: + cls.launch_source = stream.read() + relay_tree = ast.parse(cls.relay_source, filename=RELAY_PATH) + finite_function = next( + node + for node in relay_tree.body + if isinstance(node, ast.FunctionDef) + and node.name == "imu_measurements_are_finite" + ) + function_module = ast.Module(body=[finite_function], type_ignores=[]) + namespace = {"Imu": object, "math": math} + exec(compile(function_module, RELAY_PATH, "exec"), namespace) + cls.measurements_are_finite = staticmethod( + namespace["imu_measurements_are_finite"] + ) + + def test_input_and_output_use_sensor_data_qos(self): + self.assertGreaterEqual( + self.relay_source.count("qos_profile_sensor_data"), + 3, + ) + + def test_clock_gate_uses_host_system_time(self): + self.assertIn("time.time_ns()", self.relay_source) + self.assertNotIn("self.get_clock().now().nanoseconds", self.relay_source) + self.assertIn( + '"maximum_receipt_time_residual_sec": 0.25', + self.launch_source, + ) + + def test_parameters_are_declared_read_only(self): + self.assertIn( + "ParameterDescriptor(description=description, read_only=True)", + self.relay_source, + ) + + def test_sustained_unhealthy_stream_exits_nonzero(self): + self.assertIn("ConsecutiveFailureGate(3)", self.relay_source) + self.assertIn("raise RuntimeError(fatal_reason)", self.relay_source) + + def test_callback_rejects_nonfinite_measurements(self): + self.assertIn( + "if not imu_measurements_are_finite(message):", + self.relay_source, + ) + self.assertGreaterEqual( + self.relay_source.count('"nonfinite_measurement"'), + 3, + ) + + def test_finite_gate_accepts_a_complete_finite_message(self): + self.assertTrue(self.measurements_are_finite(make_imu_message())) + + def test_finite_gate_checks_vectors_and_all_covariances(self): + vector_fields = ( + ("orientation", "x"), + ("orientation", "y"), + ("orientation", "z"), + ("orientation", "w"), + ("angular_velocity", "x"), + ("angular_velocity", "y"), + ("angular_velocity", "z"), + ("linear_acceleration", "x"), + ("linear_acceleration", "y"), + ("linear_acceleration", "z"), + ) + for vector_name, component_name in vector_fields: + with self.subTest(field=f"{vector_name}.{component_name}"): + message = make_imu_message() + setattr(getattr(message, vector_name), component_name, math.nan) + self.assertFalse(self.measurements_are_finite(message)) + + covariance_fields = ( + "orientation_covariance", + "angular_velocity_covariance", + "linear_acceleration_covariance", + ) + for covariance_name in covariance_fields: + for index in range(9): + with self.subTest(field=covariance_name, index=index): + message = make_imu_message() + getattr(message, covariance_name)[index] = math.inf + self.assertFalse(self.measurements_are_finite(message)) + + def test_relay_never_subscribes_to_d435i_imu(self): + self.assertNotIn("/camera/imu", self.relay_source) + + +if __name__ == "__main__": + unittest.main() diff --git a/integrations/isaac_ros_3_2_yopo/isaac_ros_yopo_bringup/test/test_runtime_health_monitor_source.py b/integrations/isaac_ros_3_2_yopo/isaac_ros_yopo_bringup/test/test_runtime_health_monitor_source.py new file mode 100644 index 0000000..675eeb5 --- /dev/null +++ b/integrations/isaac_ros_3_2_yopo/isaac_ros_yopo_bringup/test/test_runtime_health_monitor_source.py @@ -0,0 +1,170 @@ +import ast +import math +import os +from types import SimpleNamespace +import unittest + + +PACKAGE_ROOT = os.path.dirname(os.path.dirname(os.path.abspath(__file__))) +MONITOR_PATH = os.path.join( + PACKAGE_ROOT, + "isaac_ros_yopo_bringup", + "runtime_health_monitor.py", +) +SETUP_PATH = os.path.join(PACKAGE_ROOT, "setup.py") +PACKAGE_XML_PATH = os.path.join(PACKAGE_ROOT, "package.xml") + + +class RuntimeHealthMonitorSourceContractTest(unittest.TestCase): + @classmethod + def setUpClass(cls): + with open(MONITOR_PATH, "r", encoding="utf-8") as stream: + cls.source = stream.read() + with open(SETUP_PATH, "r", encoding="utf-8") as stream: + cls.setup_source = stream.read() + with open(PACKAGE_XML_PATH, "r", encoding="utf-8") as stream: + cls.package_xml = stream.read() + cls.tree = ast.parse(cls.source) + finite_function = next( + node + for node in cls.tree.body + if isinstance(node, ast.FunctionDef) + and node.name == "odometry_measurements_are_finite" + ) + namespace = {"Odometry": object, "math": math} + exec( + compile( + ast.Module(body=[finite_function], type_ignores=[]), + MONITOR_PATH, + "exec", + ), + namespace, + ) + cls.odometry_is_finite = staticmethod( + namespace["odometry_measurements_are_finite"] + ) + + def test_subscribes_to_all_runtime_health_inputs_with_sensor_qos(self): + for topic in ( + "/camera/infra1/camera_info", + "/camera/infra2/camera_info", + "/camera/imu", + "/visual_slam/tracking/odometry", + ): + self.assertIn(topic, self.source) + self.assertEqual(4, self.source.count("qos_profile_sensor_data,")) + self.assertIn("from nav_msgs.msg import Odometry", self.source) + + def test_compares_full_versioned_camera_info_contract(self): + for field in ( + "camera.width", + "camera.height", + "camera.distortion_model", + "camera.left_d", + "camera.left_k", + "camera.left_r", + "camera.left_p", + "camera.right_d", + "camera.right_k", + "camera.right_r", + "camera.right_p", + ): + self.assertIn(field, self.source) + self.assertIn("float(value) == reference", self.source) + + def test_allows_only_the_known_right_frame_reuse(self): + self.assertIn( + "camera.right_recorded_frame_bug", + self.source, + ) + self.assertIn("if frame_id not in allowed_right_frames", self.source) + self.assertIn("if frame_id != camera.left_frame", self.source) + + def test_any_d435i_imu_sample_is_fatal(self): + function = self._function("_on_forbidden_camera_imu") + calls = [node for node in ast.walk(function) if isinstance(node, ast.Call)] + self.assertTrue( + any(getattr(call.func, "attr", "") == "_fail" for call in calls) + ) + + def test_requires_first_and_continuous_odometry_and_camera_info(self): + self.assertIn('"left_camera_info": None', self.source) + self.assertIn('"right_camera_info": None', self.source) + self.assertIn('"odometry": None', self.source) + self.assertIn("startup_timeout_sec", self.source) + self.assertIn("camera_info_stale_after_sec", self.source) + self.assertIn("odometry_stale_after_sec", self.source) + + def test_odometry_must_be_structurally_valid(self): + self.assertIn("expected_odometry_frame_id", self.source) + self.assertIn("expected_odometry_child_frame_id", self.source) + self.assertIn("stamp_to_nanoseconds", self.source) + self.assertIn("odometry timestamp is not strictly increasing", self.source) + self.assertIn("odometry_measurements_are_finite", self.source) + self.assertIn("non-finite pose, twist, or covariance", self.source) + + def test_odometry_finite_gate_checks_pose_twist_and_covariance(self): + vector = lambda x=0.0, y=0.0, z=0.0: SimpleNamespace(x=x, y=y, z=z) + message = SimpleNamespace( + pose=SimpleNamespace( + pose=SimpleNamespace( + position=vector(), + orientation=SimpleNamespace(x=0.0, y=0.0, z=0.0, w=1.0), + ), + covariance=[0.0] * 36, + ), + twist=SimpleNamespace( + twist=SimpleNamespace(linear=vector(), angular=vector()), + covariance=[0.0] * 36, + ), + ) + self.assertTrue(self.odometry_is_finite(message)) + for mutate in ( + lambda: setattr(message.pose.pose.position, "x", math.nan), + lambda: setattr(message.pose.pose.orientation, "w", math.inf), + lambda: message.pose.covariance.__setitem__(35, -math.inf), + lambda: setattr(message.twist.twist.angular, "z", math.nan), + lambda: message.twist.covariance.__setitem__(35, math.inf), + ): + message.pose.pose.position.x = 0.0 + message.pose.pose.orientation.w = 1.0 + message.pose.covariance[35] = 0.0 + message.twist.twist.angular.z = 0.0 + message.twist.covariance[35] = 0.0 + mutate() + self.assertFalse(self.odometry_is_finite(message)) + + def test_failure_is_a_nonzero_process_exit_contract(self): + function = self._function("_fail") + self.assertTrue( + any(isinstance(node, ast.Raise) for node in ast.walk(function)) + ) + self.assertIn("raise RuntimeError(self._fatal_reason)", self.source) + + def test_installs_console_entry_point_and_odometry_dependency(self): + self.assertIn('"runtime_health_monitor = "', self.setup_source) + self.assertIn( + '"isaac_ros_yopo_bringup.runtime_health_monitor:main"', + self.setup_source, + ) + self.assertIn("nav_msgs", self.package_xml) + + def test_source_is_parseable_and_respects_python_line_length(self): + self.assertIsInstance(self.tree, ast.Module) + long_lines = [ + index + for index, line in enumerate(self.source.splitlines(), start=1) + if len(line) > 99 + ] + self.assertEqual([], long_lines) + + def _function(self, name): + for node in ast.walk(self.tree): + if isinstance(node, (ast.FunctionDef, ast.AsyncFunctionDef)): + if node.name == name: + return node + self.fail(f"function {name!r} was not found") + + +if __name__ == "__main__": + unittest.main() diff --git a/integrations/isaac_ros_3_2_yopo/isaac_ros_yopo_bringup/test/test_time_alignment.py b/integrations/isaac_ros_3_2_yopo/isaac_ros_yopo_bringup/test/test_time_alignment.py new file mode 100644 index 0000000..fa680ff --- /dev/null +++ b/integrations/isaac_ros_3_2_yopo/isaac_ros_yopo_bringup/test/test_time_alignment.py @@ -0,0 +1,190 @@ +import math +from types import SimpleNamespace +import unittest + +from isaac_ros_yopo_bringup.time_alignment import ( + ConsecutiveFailureGate, + MAX_TIME_NANOSECONDS, + StampOrder, + StrictStampGuard, + TimestampContractError, + add_offset_nanoseconds, + clock_residual_nanoseconds, + clone_with_aligned_stamp, + split_nanoseconds, + stamp_to_nanoseconds, +) + + +def make_message(sec=10, nanosec=20, frame_id="base_link"): + return SimpleNamespace( + header=SimpleNamespace( + stamp=SimpleNamespace(sec=sec, nanosec=nanosec), + frame_id=frame_id, + ), + orientation=SimpleNamespace(x=0.1, y=-0.2, z=0.3, w=0.9), + orientation_covariance=[-1.0, 2.0, math.nan, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0], + angular_velocity=SimpleNamespace(x=1.1, y=-2.2, z=3.3), + angular_velocity_covariance=list(range(9)), + linear_acceleration=SimpleNamespace(x=-4.4, y=5.5, z=9.81), + linear_acceleration_covariance=[value / 10.0 for value in range(9)], + ) + + +class TimestampArithmeticTest(unittest.TestCase): + def test_applies_audited_offset_exactly(self): + input_ns = stamp_to_nanoseconds(1_784_533_873, 524_838_125) + output_ns = add_offset_nanoseconds(input_ns, 1_737_987) + self.assertEqual(1_737_987, output_ns - input_ns) + + def test_nanosecond_carry(self): + input_ns = stamp_to_nanoseconds(10, 999_999_500) + self.assertEqual((11, 500), split_nanoseconds(input_ns + 1_000)) + + def test_negative_offset_borrows_from_second(self): + input_ns = stamp_to_nanoseconds(2, 100) + output_ns = add_offset_nanoseconds(input_ns, -200) + self.assertEqual((1, 999_999_900), split_nanoseconds(output_ns)) + + def test_zero_is_representable_but_negative_is_rejected(self): + self.assertEqual((0, 0), split_nanoseconds(add_offset_nanoseconds(10, -10))) + with self.assertRaises(TimestampContractError): + add_offset_nanoseconds(10, -11) + + def test_rejects_an_invalid_input_even_if_offset_would_recover_it(self): + with self.assertRaises(TimestampContractError): + add_offset_nanoseconds(-1, 1) + with self.assertRaises(TimestampContractError): + add_offset_nanoseconds(MAX_TIME_NANOSECONDS + 1, -1) + + def test_ros_time_upper_boundary(self): + self.assertEqual( + (2_147_483_647, 999_999_999), + split_nanoseconds(MAX_TIME_NANOSECONDS), + ) + with self.assertRaises(TimestampContractError): + add_offset_nanoseconds(MAX_TIME_NANOSECONDS, 1) + + def test_rejects_invalid_nanosecond_field(self): + self.assertEqual( + 1_999_999_999, + stamp_to_nanoseconds(1, 999_999_999), + ) + with self.assertRaises(TimestampContractError): + stamp_to_nanoseconds(1, 1_000_000_000) + + def test_large_epoch_keeps_single_nanosecond_precision(self): + first = stamp_to_nanoseconds(1_784_533_873, 999_999_998) + second = stamp_to_nanoseconds(1_784_533_873, 999_999_999) + self.assertEqual(1, second - first) + self.assertEqual( + 1, + add_offset_nanoseconds(second, 1_737_987) + - add_offset_nanoseconds(first, 1_737_987), + ) + + def test_clock_domain_residual_detects_boot_time(self): + system_time_ns = stamp_to_nanoseconds(1_784_533_873, 0) + boot_time_ns = stamp_to_nanoseconds(3_143, 0) + residual_ns = clock_residual_nanoseconds(boot_time_ns, system_time_ns) + self.assertLess(residual_ns, -1_000_000_000_000_000_000) + + def test_clock_domain_residual_preserves_submillisecond_sign(self): + reference_ns = stamp_to_nanoseconds(100, 500_000) + self.assertEqual( + -200_000, + clock_residual_nanoseconds(reference_ns - 200_000, reference_ns), + ) + + +class StrictStampGuardTest(unittest.TestCase): + def test_rejects_duplicate_and_backward_without_advancing(self): + guard = StrictStampGuard() + self.assertIs(StampOrder.ACCEPT, guard.classify(100)) + guard.commit(100) + self.assertIs(StampOrder.DUPLICATE, guard.classify(100)) + self.assertIs(StampOrder.NONMONOTONIC, guard.classify(99)) + self.assertEqual(100, guard.last_accepted_ns) + self.assertIs(StampOrder.ACCEPT, guard.classify(101)) + guard.commit(101) + self.assertEqual(101, guard.last_accepted_ns) + + def test_commit_refuses_non_increasing_stamp(self): + guard = StrictStampGuard(100) + with self.assertRaises(TimestampContractError): + guard.commit(100) + with self.assertRaises(TimestampContractError): + guard.commit(99) + + +class ConsecutiveFailureGateTest(unittest.TestCase): + def test_trips_on_third_consecutive_failure(self): + gate = ConsecutiveFailureGate(3) + self.assertFalse(gate.observe(True)) + self.assertFalse(gate.observe(True)) + self.assertTrue(gate.observe(True)) + self.assertEqual(3, gate.count) + + def test_healthy_observation_resets_the_streak(self): + gate = ConsecutiveFailureGate(3) + gate.observe(True) + gate.observe(True) + self.assertFalse(gate.observe(False)) + self.assertEqual(0, gate.count) + self.assertFalse(gate.observe(True)) + + +class MessageTransparencyTest(unittest.TestCase): + def test_only_header_stamp_and_frame_are_changed(self): + message = make_message() + output = clone_with_aligned_stamp(message, "fcu_imu", 1_737_987) + + self.assertIsNot(message, output) + self.assertEqual("base_link", message.header.frame_id) + self.assertEqual((10, 20), (message.header.stamp.sec, message.header.stamp.nanosec)) + self.assertEqual("fcu_imu", output.header.frame_id) + self.assertEqual( + 1_737_987, + stamp_to_nanoseconds( + output.header.stamp.sec, + output.header.stamp.nanosec, + ) - stamp_to_nanoseconds(10, 20), + ) + self.assertEqual(message.orientation, output.orientation) + self.assertEqual(message.angular_velocity, output.angular_velocity) + self.assertEqual(message.linear_acceleration, output.linear_acceleration) + self.assertEqual( + message.angular_velocity_covariance, + output.angular_velocity_covariance, + ) + self.assertEqual( + message.linear_acceleration_covariance, + output.linear_acceleration_covariance, + ) + for input_value, output_value in zip( + message.orientation_covariance, + output.orientation_covariance, + ): + if math.isnan(input_value): + self.assertTrue(math.isnan(output_value)) + else: + self.assertEqual(input_value, output_value) + + def test_constant_offset_preserves_sample_interval(self): + first = make_message(sec=10, nanosec=100) + second = make_message(sec=10, nanosec=200) + first_output = clone_with_aligned_stamp(first, "fcu_imu", 1_737_987) + second_output = clone_with_aligned_stamp(second, "fcu_imu", 1_737_987) + input_interval = stamp_to_nanoseconds(10, 200) - stamp_to_nanoseconds(10, 100) + output_interval = stamp_to_nanoseconds( + second_output.header.stamp.sec, + second_output.header.stamp.nanosec, + ) - stamp_to_nanoseconds( + first_output.header.stamp.sec, + first_output.header.stamp.nanosec, + ) + self.assertEqual(input_interval, output_interval) + + +if __name__ == "__main__": + unittest.main() diff --git a/integrations/isaac_ros_3_2_yopo/patches/isaac_ros_visual_slam_v3_2_15_imu_timestamp.patch b/integrations/isaac_ros_3_2_yopo/patches/isaac_ros_visual_slam_v3_2_15_imu_timestamp.patch index f388b30..fe96525 100644 --- a/integrations/isaac_ros_3_2_yopo/patches/isaac_ros_visual_slam_v3_2_15_imu_timestamp.patch +++ b/integrations/isaac_ros_3_2_yopo/patches/isaac_ros_visual_slam_v3_2_15_imu_timestamp.patch @@ -12,7 +12,18 @@ diff --git a/isaac_ros_visual_slam/src/impl/visual_slam_impl.cpp b/isaac_ros_vis tf_buffer(std::make_unique(node.get_clock())), tf_listener(std::make_unique(*tf_buffer)), tf_publisher(std::make_unique(&node)), -@@ -685,7 +685,7 @@ void VisualSlamNode::VisualSlamImpl::UpdatePose( +@@ -315,7 +315,8 @@ void VisualSlamNode::VisualSlamImpl::Initialize() + node.vis_localizer_loop_closure_pub_, tracker, canonical_pose_cuvslam, node.map_frame_); + } + cuvslam_handle = tracker; +- RCLCPP_INFO(node.get_logger(), "cuVSLAM tracker was successfully initialized."); ++ RCLCPP_INFO(node.get_logger(), ++ "cuVSLAM tracker was successfully initialized. ISAAC_ROS_YOPO_IMU_TIMESTAMP_PATCH_V1"); +- ++ + if (node.localize_on_startup_) { + // We don't care when this thread returns since we anyways can't do anything if the localization +@@ -685,7 +686,7 @@ void VisualSlamNode::VisualSlamImpl::UpdatePose( int64_t imu_ts = static_cast(timestamp.nanoseconds()); RCLCPP_DEBUG(node.get_logger(), "Using imu msg timestamp [%ld]", imu_ts); const CUVSLAM_Status status = CUVSLAM_RegisterImuMeasurement( diff --git a/integrations/isaac_ros_3_2_yopo/scripts/verify_visual_slam_patch.sh b/integrations/isaac_ros_3_2_yopo/scripts/verify_visual_slam_patch.sh index f37e1e1..005c572 100755 --- a/integrations/isaac_ros_3_2_yopo/scripts/verify_visual_slam_patch.sh +++ b/integrations/isaac_ros_3_2_yopo/scripts/verify_visual_slam_patch.sh @@ -10,6 +10,7 @@ SCRIPT_DIR="$(cd -- "$(dirname -- "${BASH_SOURCE[0]}")" && pwd)" INTEGRATION_DIR="$(cd -- "$SCRIPT_DIR/.." && pwd)" PATCH_FILE="$INTEGRATION_DIR/patches/isaac_ros_visual_slam_v3_2_15_imu_timestamp.patch" SOURCE_FILE="isaac_ros_visual_slam/src/impl/visual_slam_impl.cpp" +PATCH_MARKER="ISAAC_ROS_YOPO_IMU_TIMESTAMP_PATCH_V1" usage() { echo "Usage: $0 [--source-only] /path/to/isaac_ros_visual_slam" >&2 @@ -133,6 +134,7 @@ grep -Fq \ 'node.image_buffer_size_, 1e6 * node.image_jitter_threshold_ms_),' \ "$target" grep -Fq 'cuvslam_handle, imu_ts, &imu_measurement);' "$target" +grep -Fq "$PATCH_MARKER" "$target" if grep -Fq 'cuvslam_handle, latest_ts, &imu_measurement);' "$target"; then echo "[STOP] The incorrect image timestamp call is still present." >&2 @@ -150,4 +152,4 @@ if [[ -n "$(git -C "$repo" status --porcelain --untracked-files=no)" ]]; then fi trap - EXIT INT TERM -echo "[PASS] Patch matches NVIDIA v3.2-15, both corrections are present, and the checkout was restored." +echo "[PASS] Patch matches NVIDIA v3.2-15, both corrections and the runtime marker are present, and the checkout was restored." From 04d7b9cd1a4f6ebe324538a3892d32ccc794e650 Mon Sep 17 00:00:00 2001 From: u5-4 Date: Tue, 21 Jul 2026 18:01:14 +0800 Subject: [PATCH 3/4] [feat] Approve odometry-only FCU IMU runtime Signed-off-by: u5-4 --- .../PROJECT_TASKFLOW.zh-CN.md | 1208 +++++++++++++++++ .../isaac_ros_3_2_yopo/README_set_up.md | 7 +- .../STARTUP_RUNBOOK.zh-CN.md | 102 +- .../isaac_ros_yopo_bringup/README.md | 35 +- .../config/d435i_243622070369_fcu_imu.yaml | 9 +- .../config/px4_imu_noise_allan.template.yaml | 4 +- .../config/px4_imu_noise_unvalidated.yaml | 5 +- .../isaac_ros_yopo_bringup/calibration.py | 53 +- .../launch/d435i_fcu_imu_cuvslam.launch.py | 91 +- .../isaac_ros_yopo_bringup/package.xml | 2 +- .../isaac_ros_yopo_bringup/setup.py | 2 +- .../test/test_calibration.py | 123 +- .../test/test_launch_source.py | 26 +- 13 files changed, 1514 insertions(+), 153 deletions(-) create mode 100644 integrations/isaac_ros_3_2_yopo/PROJECT_TASKFLOW.zh-CN.md diff --git a/integrations/isaac_ros_3_2_yopo/PROJECT_TASKFLOW.zh-CN.md b/integrations/isaac_ros_3_2_yopo/PROJECT_TASKFLOW.zh-CN.md new file mode 100644 index 0000000..0882bc4 --- /dev/null +++ b/integrations/isaac_ros_3_2_yopo/PROJECT_TASKFLOW.zh-CN.md @@ -0,0 +1,1208 @@ +# YOPO / cuVSLAM / PX4 Jetson 项目任务流 + +> 桌面主文件：`C:\Users\10416\Desktop\agent.md` +> 仓库镜像：`integrations/isaac_ros_3_2_yopo/PROJECT_TASKFLOW.zh-CN.md` +> 最近更新：2026-07-21 +> 当前阶段：外部飞控 IMU cuVSLAM 首次联合运行验收完成，进入稳定化、图像质量与 odometry-only 验证 +> 安全状态：仅允许被动感知与规划验证；禁止 OFFBOARD、解锁、起飞和有效控制输出 + +## 0. 版本取舍与历史保留说明 + +本文件由两份资料合并而成： + +1. 桌面旧任务流 `agent.md`，主体形成于 2026-07-17，包含环境审计、官方 D435i 基线、P-001 至 P-010、D-001/D-002、早期两阶段路线和后续补充记录； +2. 2026-07-21 的 `PROJECT_TASKFLOW.zh-CN.md`，包含外部 FCU IMU 标定、wrapper 修复、统一 launch、坐标系、YOPO PASSIVE 和 SO3 后续任务。 + +取舍原则不是“新版本覆盖并删除旧版本”，而是： + +- 已经被新证据更新的旧结论，在下表中说明替代关系； +- 仍然成立的环境基线、问题记录和安全规则继续生效； +- 旧路线、旧分支和旧镜像继续作为回退、A/B 或审计记录； +- 全部旧文档原文保留在第 15 节历史快照中，但其中标注为“当前”的内容不再覆盖第 1 至 14 节的现行状态。 + +### 0.1 关键版本取舍 + +| 主题 | 旧文档状态/路线 | 当前生效选择 | 取舍原因与历史用途 | +| --- | --- | --- | --- | +| 项目范围 | 当前两阶段不包含 PX4、MAVROS 和控制器 | 已加入 PX4/MAVROS 外部 IMU；SO3 只进入参数与接口审计，不启用控制 | 用户在完成官方基线后明确扩展任务范围；旧的被动安全边界继续保留 | +| cuVSLAM 集成分支 | 计划 `u5-4/isaac-ros-3.2-yopo-adapter` | 当前 `u5-4/fcu-imu-cuvslam-integration@9ca7190`，本地后续提交待发布 | 旧分支 `4a41339` 保留为仅含 wrapper 时间戳补丁的历史基线；`9ca7190` 完成首次联合验收，后续提交收敛批准语义和 odometry-only | +| integration 路径白名单 | 早期只预留 CMake adapter 目录结构 | 仍只允许修改 `integrations/isaac_ros_3_2_yopo/`，但加入 Python bringup、标定 YAML、patch、测试和中文文档 | 所有新增内容仍与 core17 隔离；早期目录草案作为设计历史保留，不再限制具体 ROS 包构建类型 | +| Git 签署 | 旧规则要求后续提交使用 `git commit -s` | 已发布的 `9ca7190` 没有 `Signed-off-by`，不重写已推送历史；后续提交恢复 `-s`，除非仓库规则另有明确决定 | 如远端将来启用 DCO，再通过后续合规流程处理，禁止为补签而 force-push | +| NVIDIA wrapper 策略 | 官方源码保持只读，发现必须修改时停止决策 | 允许一个经过固定 commit、apply/reverse verifier 和 marker 约束的树外补丁 | 实测确认 wrapper 把图像时间戳用于 IMU 注册，并存在 jitter 单位错误；SDK 二进制仍不修改、不上传官方仓库 | +| cuVSLAM core17 | 不加入 Isaac ROS 3.2 构建 | 继续不构建、不链接、不运行 | 旧决策保持不变；Isaac ROS 3.2 继续使用配套 SDK ABI | +| IMU 路线 | 官方 D435i 内置 IMU fusion | 官方路线保留为 A/B 和回退；当前候选路线使用 PX4 FCU IMU | 官方路线已经完成约 10 分钟稳定基线；外部 IMU 更符合最终机载硬件设计，当前只把 Jetson 联合运行作为必需验收，Allan 为可选调优 | +| 相机模型 | D435i EEPROM 原厂内外参 | 运行时继续使用原厂 rectified `K/D/R/P` | 自由估计 Kalibr 内参作为对比保留；它不能与当前 RealSense rectified CameraInfo 直接混用 | +| 相机-FCU IMU 标定 | 未完成 | `T_Crect0_I` 与 `+1,737,987 ns` 已固化，Kalibr 联合标定已完成并在首次 Jetson 联合运行后转为项目 `approved` | `9ca7190` 的 runtime candidate 命名作为已验收历史保留；后续 schema 将项目批准与 Allan/重复性来源状态拆开 | +| IMU 噪声策略 | `9ca7190` 把 Allan 来源作为默认生产门禁 | 当前项目接受 Kalibr 配置中的四项噪声权重用于联合标定和第一版 cuVSLAM；Allan 改为可选增强 | Kalibr 可在近似权重下迭代轨迹、bias、外参和时移并收敛；它不会自动重新估计四个噪声系数，但这不构成当前部署阻塞 | +| 里程计参考点 | 官方 `base_frame=camera_link` | 当前联合冒烟暂时保持；最终目标改为 `base_link` | 旧相机参考输出可用于基线；接入规划/控制前必须补齐 `base_link -> fcu_imu` 并形成单父 TF 树 | +| 定位模式 | 官方 localization + mapping | mapping-on 联合冒烟已通过；后续源码已固定为 odometry-only，等待 Jetson A/B | 先隔离外部 IMU 链路风险，再关闭 mapping/回环、ground constraint、可视化和 `map -> odom` TF | +| Depth 与 emitter | emitter 策略、深度合同待验证 | 问题继续保留到 YOPO PASSIVE 阶段 | 当前外部 IMU launch 关闭 Depth；不能因为 VIO 已稳定就视为深度链已验收 | +| 阶段结构 | 官方基线 + YOPO PASSIVE 两阶段 | 部署、坐标、状态适配、Depth/YOPO、SO3 五个连续阶段 | 新增外部 IMU、机体坐标和控制合同后，需要更细门禁；旧两阶段结果映射到新阶段而不是删除 | +| 开发镜像 | 记录 `jp6.2` 与 `vslam3.2-dev-20260717` 快照 | 两个镜像继续作为恢复锚点，当前开发工作区使用 overlay 增量构建 | 镜像历史仍有恢复价值；当前补丁和 bringup 尚未声明已经固化进生产镜像 | + +### 0.2 历史结论的生效优先级 + +发生冲突时按以下顺序判断： + +1. 当前 Jetson 的真实命令输出、日志和设备数据； +2. 本文件第 1 至 14 节的当前任务流； +3. `STARTUP_RUNBOOK.zh-CN.md` 中与当前 commit 匹配的运行命令； +4. 第 15 节旧文档快照和早期补充记录。 + +低优先级内容不会被删除，但不能覆盖后来已经验证的新事实。 + +## 1. 文档维护规则 + +- 桌面的 `agent.md` 是用户日常查询的主任务流。 +- 仓库中的本文件是可审计、可提交的镜像，不替代桌面主文件。 +- 每完成一个检查点，应同步更新日期、实际 commit、验证证据、复选框和下一步。 +- 没有真实 Jetson 输出或日志证据的任务不得标记为 `[x]`。 +- 部分完成的任务保持 `[ ]`，并在后面注明“部分完成”和缺少的证据。 +- 运行命令集中维护在 [`STARTUP_RUNBOOK.zh-CN.md`](STARTUP_RUNBOOK.zh-CN.md)，本文件只维护目标、状态、依赖关系和验收门槛。 + +## 2. 最终目标 + +建立一条坐标、时间、单位和健康状态都明确的被动数据链： + +```text +D435i IR1 + IR2 + CameraInfo +PX4 FCU IMU -> MAVROS + -> Isaac ROS Visual SLAM / cuVSLAM + -> 标准定位输出与健康状态 + -> YOPO PASSIVE + +D435i Depth + CameraInfo + -> YOPO PASSIVE + +定位、深度和规划完成被动验收后 + -> 审计 SO3 状态/指令/动力学合同 +``` + +当前不允许把定位结果回灌 PX4，不允许 YOPO 或 SO3 发布有效控制命令。 + +## 3. 固定版本与硬件基线 + +| 项目 | 当前固定值 | 状态 | +| --- | --- | --- | +| Jetson | Orin NX 16GB / JetPack 6.2 / L4T 36.4.3 | 已确认 | +| 系统 | Ubuntu 22.04 / ROS 2 Humble | 已确认 | +| Isaac ROS Common | `v3.2-15@fcf4d9e` | 已固定 | +| Isaac ROS Visual SLAM | `v3.2-15@e31f4cc1d41a329a01946e5fe63669f8b15da677` | 已固定 | +| cuVSLAM 运行时 | Isaac ROS 3.2 配套二进制 SDK，不构建 core17 | 已固定 | +| 集成分支 | `u5-4/fcu-imu-cuvslam-integration` | 已发布 | +| 集成提交 | `9ca71902ec8d50d87390072a213f8ec211b88817` | 已拉取到 Jetson | +| wrapper marker | `ISAAC_ROS_YOPO_IMU_TIMESTAMP_PATCH_V1` | 已在源码补丁验证 | +| D435i | 序列号 `243622070369` / 固件 `5.15.1.55` | 已固定 | +| RealSense | librealsense `2.55.1` / ROS `4.51.1` | 已确认 | +| 相机 VIO 输入 | IR1/IR2 rectified `640x360@90 Hz`，emitter 关闭 | 已固定 | +| 飞控桥 | MAVROS `2.14.0` / `/dev/ttyTHS2:921600` | 已确认 | +| FCU IMU | MAVLink `HIGHRES_IMU(105)` 请求 200 Hz，实测约 170 Hz | 已确认 | +| ROS Domain | `42` | 已固定 | + +## 4. 已完成事实 + +### 4.1 环境与官方基线 + +- [x] 容器内确认 NVIDIA GPU、D435i USB3、librealsense 和 ROS 2 Humble。 +- [x] 完成 IR1/IR2/Depth/合并 D435i IMU 的短时帧率、时间戳和同步基线。 +- [x] 克隆 Visual SLAM 并精确固定到 `v3.2-15@e31f4cc`。 +- [x] 确认运行时使用 Isaac ROS 3.2 配套 cuVSLAM SDK，不构建或链接仓库 core17。 +- [x] 在容器中构建并运行 NVIDIA 官方 RealSense + D435i IMU fusion 基线。 +- [x] 完成官方 IMU fusion 与视觉-only A/B 基线检查。 +- [x] 官方链路连续运行约 10 分钟，`vo_state/status`、时间戳和 tracking 状态正常。 +- [x] 当前 D435i 固件 `5.15.1.55` 已运行官方链路，不需要为兼容性立即刷写。 + +### 4.2 标定与录包 + +- [x] 建立独立 `image_bag_recorder` 标定录包流程。 +- [x] 录制 D435i 原厂 rectified 双目内参包。 +- [x] 录制 D435i 双目 + PX4/MAVROS FCU IMU 联合标定包。 +- [x] 验证双目约 `89.8 Hz`、FCU IMU 约 `170 Hz`、时间戳非零递增和传输合同。 +- [x] 完成 Kalibr 双目内参与相机-FCU IMU 联合标定。 +- [x] 对比自由估计内参与 D435i 原厂 rectified CameraInfo，选择与实时图像链一致的原厂 rectified 模型。 +- [x] 固化 `T_Crect0_I`、双目 baseline 和相机/IMU 时移。 +- [x] 固化运行时补偿 `t_aligned = t_imu_raw + 1,737,987 ns`。 +- [x] 项目确认当前 Kalibr 四项 IMU 噪声权重可用于第一版联合标定与 cuVSLAM 验证，不以 Allan 作为强制前置。 +- [ ] 可选增强：完成 PX4 IMU Allan 噪声标定，用于独立测量噪声系数和后续调优。 +- [ ] 可选增强：在相同 factory-rectified 模型下完成一次独立重复联合标定，用于量化可重复性。 + +### 4.3 外部飞控 IMU cuVSLAM 源码 + +- [x] 修复 Isaac ROS 3.2 wrapper 将图像时间戳误用于 IMU 注册的问题。 +- [x] 修复 `MessageStreamSequencer` 抖动阈值毫秒/纳秒单位问题。 +- [x] 增加编译产物 marker 和固定 NVIDIA commit 校验脚本。 +- [x] 实现严格递增、时钟域检查、常量时移和有限数值门禁的 aligned IMU relay。 +- [x] 实现版本化相机 `K/D/R/P`、外参、时移和 IMU 噪声 schema 校验。 +- [x] 实现 D435i IMU 禁用、CameraInfo 核对和 cuVSLAM odometry 常驻健康监控。 +- [x] 实现 RealSense、aligned IMU、标定 TF、cuVSLAM 和健康监控统一 launch。 +- [x] 本地完成 `65/65` 逻辑测试、AST/XML/格式检查和补丁 apply/reverse 验证。 +- [x] 创建并发布 `u5-4/fcu-imu-cuvslam-integration@9ca7190`。 +- [x] Jetson 已拉取新分支并将新版 wrapper 补丁应用到固定 NVIDIA 源码。 +- [x] 首次真机验收后完成本地后续源码：项目批准与 Allan 来源拆分、默认 noise YAML、固定 odometry-only 和 `map -> odom` 所有权边界；`72/72` 逻辑测试通过。 + +## 5. 当前检查点：Jetson 构建与联合启动 + +### 5.1 构建状态 + +- [x] Jetson 上确认 adapter 为 `9ca7190`。 +- [x] 新补丁 verifier 通过并自动恢复干净源码。 +- [x] 新补丁实际应用后，NVIDIA 源码仅修改 `visual_slam_impl.cpp`。 +- [x] 源码中确认 `ISAAC_ROS_YOPO_IMU_TIMESTAMP_PATCH_V1`。 +- [x] 重新构建 `isaac_ros_visual_slam`。（Jetson 实测完成，耗时 57 分 20 秒） +- [x] 构建 `isaac_ros_yopo_bringup`。 +- [x] 确认安装后的 `libvisual_slam_node.so` 包含 marker。 +- [x] 确认安装 `aligned_imu_relay` 和 `runtime_health_monitor` 两个可执行文件。 +- [x] 确认统一 launch 的 `--show-args` 正常。 + +### 5.2 第一次联合冒烟 + +联合 launch 会独占启动一份 RealSense。不得同时运行官方 RealSense launch、标定录包 launch 或第二份相机节点。 + +- [x] 宿主机启动 MAVROS `/dev/ttyTHS2:921600`，确认 `connected: true`。 +- [x] 请求 `HIGHRES_IMU(105)` 200 Hz，确认实际约 169–171 Hz、时间戳有效且 timesync RTT 约 1.31 ms。 +- [x] 容器启动统一 `d435i_fcu_imu_cuvslam.launch.py`；RealSense、aligned relay、标定 TF、cuVSLAM 与 runtime monitor 均成功启动。 +- [x] 确认 D435i IR1/IR2 和两路 CameraInfo 稳定在约 89.9–90.4 Hz；左 CameraInfo 单次订阅瞬态复测后稳定为 89.86 Hz。 +- [x] 确认 D435i `enable_gyro=False`、`enable_accel=False`、`unite_imu_method=0`，且 `/camera/imu` 6 秒内没有实际样本。 +- [x] 确认 `/fcu/imu/data_raw_aligned` 约 171.3 Hz、frame 为 `fcu_imu`。 +- [x] 确认 cuVSLAM 只订阅 aligned FCU IMU，不订阅 raw IMU 或 D435i IMU。 +- [x] 确认 `camera_infra1_optical_frame -> fcu_imu` 标定 TF；平移匹配标定值，四元数以等价的整体反号形式发布。 +- [x] 确认 tracker marker、`/visual_slam/status` 和 tracking odometry；`vo_state=1`，状态 frame 为 `map`，odometry 为 `odom -> camera_link` 且数值有限。 +- [x] 确认 aligned relay 与 runtime monitor diagnostics 均为 `OK`；170.821 Hz、最大间隔 6.542 ms、时钟残差 -5.661 ms，所有 IMU 拒绝计数及 `forbidden_camera_imu` 均为 0。 +- [x] 完成约 17 分 48 秒首次联合运行；相机、aligned IMU、odometry 与两个健康守护持续在线，无 USB 断连、崩溃、CUDA OOM 或持续 tracking lost。 + +`9ca7190` 使用的两个显式确认开关已经完成其首次联合验收用途。当前本地后续源码把相机-FCU 标定设为项目 `approved`，noise schema v2 使用独立的 `project_status: approved` 与 `validated: false`，并移除两个临时放行参数；`validated: false` 仅表示当前 Kalibr 权重不声称独立 Allan 来源。该后续版本尚未在 Jetson 重建和 A/B，不能用本段本地状态覆盖上面的 `9ca7190` 真机证据。 + +## 6. 阶段一收尾：稳定部署与文档 + +- [x] 根据第一次联合冒烟修正 `/camera/imu` 检查的 BrokenPipe 噪声、批准语义和稳定启动文档。 +- [x] 后续源码固定为 odometry-only：关闭建图/回环、两类 ground constraint、三类内部可视化和 cuVSLAM `map -> odom` TF。 +- [ ] 在 Jetson 构建后续 `isaac_ros_yopo_bringup`，确认七个模式参数均为 `False`，重新完成频率、diagnostics、受控运动和 10 分钟 A/B。 +- [x] emitter 关闭时用两个 `rqt_image_view` 目视确认 IR1/IR2 无黑屏、冻结、明显曝光差异或异常拖影。 +- [ ] 归档 emitter 关闭时的代表性 IR 图像、曝光、模糊和特征质量。 +- [x] 完成受控刚体运动响应测试：18 秒内最大位移约 1.368 m、最大转角约 179.99°，917 个不同位姿，`vo_state` 全程为 1，odometry 时间戳无逆序。 +- [ ] 记录 emitter 关闭时的 VIO 频率、失跟、延迟和漂移结果。 +- [x] 完成 IR1/IR2/FCU IMU 的首次 10 分钟压力测试；连续运行约 17 分 48 秒并补充受控大幅运动，无守护退出。 +- [ ] 后续启用 Depth 后，完成 IR1/IR2/Depth/FCU IMU 的并发压力测试。 +- [ ] 将最终宿主机、容器、MAVROS、构建、启动、验收和停止命令同步到启动手册。 +- [ ] 更新桌面 `agent.md`、仓库任务流和变更记录。 + +## 7. 阶段二：统一坐标系与机体坐标 + +### 7.1 推荐坐标树 + +最终目标遵循 ROS REP-103/REP-105： + +```text +map 全局任务坐标，ENU，可接收低频全局校正 +└── odom 连续局部坐标，ENU，不允许跳变 + └── base_link 机体坐标，FLU，原点为最终控制参考点/质心 + ├── fcu_imu 飞控 IMU 物理坐标 + └── camera_link D435i 机身坐标 + ├── camera_infra1_optical_frame + ├── camera_infra2_optical_frame + └── camera_depth_optical_frame +``` + +PX4 内部继续使用 `NED + FRD`。ROS 感知、规划和状态接口使用 `ENU + FLU`。转换只能集中在 MAVROS/控制网关边界，不能分别散落在 cuVSLAM、YOPO 和 SO3 中。 + +### 7.2 必须确认的事实 + +- [ ] 确定 `base_link` 原点：车辆质心、控制参考点还是估计器参考点。 +- [ ] 确定 `base_link` 轴向为 FLU，并记录对应的 PX4 `base_frd` 关系。 +- [ ] 查明 MAVROS `/mavros/imu/data_raw` 的实际轴向转换，不只依赖 `frame_id` 名称。 +- [ ] 通过绕单轴和静态重力实验验证 FCU IMU 的 X/Y/Z 正负方向。 +- [ ] 获取或测量 `base_link -> fcu_imu` 的平移和旋转。 +- [ ] 使用已标定的 `camera_infra1_optical_frame -> fcu_imu` 推导并复核 `base_link -> camera_link`。 +- [ ] 重构 TF 为单父节点树，禁止同时给 `fcu_imu` 发布两个父 frame。 +- [ ] 将 cuVSLAM `base_frame` 从临时 `camera_link` 改为最终 `base_link`。 +- [ ] 确认 cuVSLAM odometry 的 header frame、child frame、四元数方向和 twist 表达坐标系。 +- [ ] 确定局部规划与 SO3 使用连续 `odom`，全局 `map` 只通过 `map -> odom` 提供校正。 +- [ ] 形成一份坐标系表：frame、父 frame、原点、轴向、单位、来源、发布节点和验证方法。 +- [ ] 将最终坐标树和转换公式明确写入桌面 `agent.md` 与启动/接口文档。 + +在这一步完成前，不得把 cuVSLAM odometry 直接当成无人机质心状态，也不得接入控制器。 + +## 8. 阶段三：定位状态适配器 + +目标接口： + +```text +/visual_slam/tracking/odometry + /visual_slam/status + diagnostics + -> localization/state adapter + -> /localization/odometry + -> YOPO remap /state/odom +``` + +- [ ] 确定 YOPO 对 pose、quaternion、linear velocity 和 angular velocity 的实际坐标语义。 +- [ ] 保留一条符合 `nav_msgs/msg/Odometry` 标准语义的 `/localization/odometry`。 +- [ ] 若 YOPO 历史接口要求世界系速度，必须在 adapter 中显式转换并单独记录该偏差。 +- [ ] 实现 tracking lost、odometry stale、时间戳回退、frame 错误和非有限数值门禁。 +- [ ] 确认 `/state/odom` 不发布陈旧或跨坐标系混合的数据。 +- [ ] 使用已知直线距离、单轴旋转和静止测试验证尺度、符号和速度方向。 +- [ ] 完成状态适配器单元测试、rosbag 回放和 Jetson 实时验收。 + +## 9. 阶段四：Depth 与 YOPO PASSIVE + +当前统一外部 IMU launch 明确关闭 Depth。Depth 阶段只能扩展这一份 RealSense 节点，不能启动第二份驱动。 + +- [ ] 在唯一 RealSense 节点中启用 Depth，同时保留 VIO 所需 IR1/IR2。 +- [ ] 固定 Depth topic、CameraInfo、分辨率、FPS、编码、单位和时间戳合同。 +- [ ] 验证 `16UC1` 毫米或 `32FC1` 米，禁止隐式猜测单位。 +- [ ] 比较 emitter 开/关对 VIO 和深度空洞率、有效距离、噪声的影响。 +- [ ] 选择不会破坏 VIO 的 emitter 策略并记录应用场景边界。 +- [ ] 将标准深度接口 remap/转换到 YOPO `/depth_image`。 +- [ ] 保持 YOPO `output_enabled=false`。 +- [ ] 验证 `/so3_control/pos_cmd` 实际消息数为零，而不是只检查 publisher 是否存在。 +- [ ] 联合运行定位、Depth 和 YOPO PASSIVE 至少 30 分钟。 +- [ ] 记录 GPU、CPU、内存、温度、功耗、频率、延迟和队列积压。 + +## 10. 阶段五：SO3 重力与动力学合同 + +只有坐标系和 YOPO PASSIVE 通过后才进入本阶段。该阶段首先是离线审计和被动观测，不启用控制。 + +### 10.1 坐标与消息合同 + +- [ ] 审查 SO3 源码实际要求的世界坐标、机体坐标和四元数方向。 +- [ ] 确认状态输入使用 `ENU/FLU` 还是 `NED/FRD`。 +- [ ] 确认位置、速度、加速度、jerk、yaw、yaw rate、body rate 和 thrust 的单位。 +- [ ] 明确重力向量与加速度计 specific force 的区别。 +- [ ] 固定唯一坐标转换边界，禁止 YOPO、MAVROS 和 SO3 重复转换。 + +### 10.2 车辆参数 + +- [ ] 车辆总质量与载荷配置。 +- [ ] 质心位置和 `base_link` 原点。 +- [ ] 完整惯量矩阵及其测量/辨识来源。 +- [ ] 重力常数和世界坐标中的重力方向。 +- [ ] 机臂长度、电机布局和旋转方向。 +- [ ] 推力系数、反扭矩系数、推力曲线和电机时间常数。 +- [ ] 最小/最大转速、总推力和力矩饱和范围。 +- [ ] 悬停推力、气动阻力参数和电池电压影响。 +- [ ] PX4 传感器位置、板载方向和估计器相关 offset 参数。 +- [ ] 控制频率、状态延迟、命令延迟和超时门限。 + +### 10.3 安全验收 + +- [ ] 建立只记录不发布执行指令的 SO3 dry-run 模式。 +- [ ] 对重力补偿、悬停推力和力矩符号做离线测试。 +- [ ] 对状态丢失、时间戳异常和坐标不一致实施 fail-closed。 +- [ ] 完成仿真、桨叶拆除测试、系留测试和独立安全审查后，才允许讨论主动控制。 + +## 11. 旧任务清单的当前判定 + +以下内容用于同步原桌面 `agent.md` 中的复选框： + +```markdown +- [x] 容器内确认 NVIDIA GPU、D435i USB3、librealsense 和 ROS2 Humble。 +- [x] 完成 IR1/IR2/Depth/合并 IMU 的短时帧率、时间戳和同步基线。 +- [x] 克隆 Visual SLAM 并精确固定到 `v3.2-15@e31f4cc`；不链接 core17。 +- [x] 在现有容器内解析依赖并构建官方 `isaac_ros_visual_slam` 与 interfaces 包。 +- [x] 运行官方 RealSense + IMU fusion 基线并完成视觉-only A/B 检查。 +- [ ] 使用 odometry-only 配置。（后续源码与本地测试已完成，Jetson A/B 待完成） +- [x] 验证当前固件 `5.15.1.55` 可以运行官方链路，无需立即刷写。 +- [ ] 归档 emitter 关闭时的图像和 VIO 质量。（VIO 部分已完成，图像证据待归档） +``` + +其他阶段一验收项： + +```markdown +- [ ] CameraInfo 长期队列积压和 PVA 设备节点已单独复核。（CameraInfo 时间戳已完成） +- [x] `/visual_slam/status` 正常，`/visual_slam/tracking/odometry` 连续输出。 +- [ ] 已知平移/旋转动作的方向、尺度和姿态符号符合预期。 +- [x] 官方 IR 双目 + D435i IMU 连续运行至少 10 分钟且没有 tracking lost。 +- [ ] IR 双目 + Depth + FCU IMU 连续并发至少 10 分钟。 +- [ ] 阶段日志、配置、commit 和未解决问题已经集中归档。 +``` + +## 12. 当前下一步 + +当前只执行以下顺序，不并行进入 YOPO 或控制： + +1. [x] Jetson 完成两个包的编译和安装验证。 +2. [x] 宿主机启动 MAVROS 并确认 FCU IMU 约 170 Hz。 +3. [x] 容器启动统一外部 IMU cuVSLAM launch。 +4. [x] 完成短时话题、TF、订阅、diagnostics 和 odometry 验收。 +5. [x] 完成至少 10 分钟联合稳定性测试。（首次连续运行约 17 分 48 秒） +6. [x] 修复首次真机发现的问题并更新后续稳定启动文档。 +7. [ ] 进入 odometry-only A/B。 +8. [ ] 进入统一坐标系和机体外参阶段。 + +## 13. 当前已知未解决项 + +| 编号 | 问题 | 当前影响 | +| --- | --- | --- | +| R-001 | PX4 IMU 四项噪声权重没有独立 Allan 来源 | 当前不阻塞部署；保留来源说明，运行质量不足时再执行 Allan 调优 | +| R-002 | factory-rectified 联合标定没有独立重复数据集 | 当前不阻塞部署；作为可选重复性量化，不能把“未重复”误写成“联合标定未完成” | +| R-003 | 当前 cuVSLAM `base_frame=camera_link` | 输出仍是相机参考点，不是最终机体/质心状态 | +| R-004 | 后续源码已设 `enable_localization_n_mapping=False`，但 Jetson A/B 尚未完成 | 不能把本地源码合同直接视为真机 odometry-only 验收 | +| R-005 | 当前统一外部 IMU launch 关闭 Depth | 尚不能作为 YOPO 完整感知输入 | +| R-006 | `base_link -> fcu_imu` 真实安装关系未固定 | 不能建立最终机体 TF 树 | +| R-007 | YOPO 对 odometry twist 的坐标语义待源码确认 | 不能直接发布最终 `/state/odom` | +| R-008 | SO3 车辆参数与坐标合同尚未审计 | 禁止启用控制 | + +## 14. 变更记录 + +| 日期 | 更新 | +| --- | --- | +| 2026-07-21 | 将任务流同步到外部 FCU IMU 集成现状；记录 `9ca7190`、wrapper 补丁、标定候选、Jetson 当前构建检查点；新增坐标系、状态适配、Depth/YOPO 和 SO3 阶段。 | +| 2026-07-21 | 根据项目决定，将 Allan 和独立重复联合标定调整为可选增强；当前 Kalibr 联合标定保持已完成，第一版运行使用现有四项噪声权重。 | +| 2026-07-21 | Jetson 用时 57 分 20 秒完成 `isaac_ros_visual_slam` 重编译和 `isaac_ros_yopo_bringup` 构建；安装节点、二进制 marker 与统一 launch 参数验证通过，检查点推进到首次联合运行。 | +| 2026-07-21 | 首次 D435i 双 IR + PX4 aligned IMU + cuVSLAM 联合运行验收通过：话题频率、TF、唯一 IMU 订阅、diagnostics、`vo_state=1` 和 `odom -> camera_link` 均满足合同，并连续运行约 17 分 48 秒。 | +| 2026-07-21 | 本地后续源码将标定项目批准与 Allan provenance 拆分，移除两个临时放行参数并提供默认 noise YAML；生产 launch 固定为 odometry-only 且不发布 `map -> odom`，`72/72` 逻辑测试通过，Jetson A/B 待执行。 | + +## 15. 桌面旧任务流完整内容快照 + +> 来源：`C:\Users\10416\Desktop\agent.md` +> 合并前 SHA-256：`699B49FB4CB0777E390FA711A0B1C4997EDF60BECB357097B649060C10CFEB2E` +> 快照用途：保留 2026-07-17 环境审计、问题/决策编号、早期阶段计划、镜像恢复信息和随后追加的讨论记录。 +> 保留方式：文字、代码和顺序全部保留；仅规范化历史原文中的行尾空格，未覆盖桌面源文件。 +> 注意：本快照中的“当前”“尚未完成”“不进入”等表述只代表当时状态；与第 0 节取舍表或第 1 至 14 节冲突时，以当前章节为准。 + +
+展开 2026-07-17 版 agent.md 完整原文 + + +# YOPO ROS2 与 cuVSLAM Jetson 部署工作文档 + +> 用途：记录项目边界、已验证事实、技术决策、问题和接下来两个阶段的任务。 +> 最近更新：2026-07-17 +> 当前原则：只推进被动部署，不进入控制、OFFBOARD 或飞行阶段。 + +## 1. 项目目标与当前位置 + +保留 YOPO 官方算法主体，完成 ROS2 Humble 接口和 Jetson Orin NX 被动部署，并验证以下真实数据链： + +```text +D435i IR1 + IR2 + IMU + -> Isaac ROS Visual SLAM / cuVSLAM + -> 坐标、速度与健康状态适配 + -> /state/odom + +D435i Depth + CameraInfo + -> /depth_image + -> YOPO + +/state/odom + /depth_image + goal + -> YOPO PASSIVE（output_enabled=false） +``` + +### 1.1 已完成 + +- [x] YOPO 官方算法主体的 ROS2 Humble 接口迁移。 +- [x] `quadrotor_msgs` 与 `yopo_planner` 从 `~/catkin_ws` 完整构建成功。 +- [x] `PositionCommand` 接口和 `yopo_node` 可执行文件验证成功。 +- [x] Jetson 加载官方 `epoch50.pth`，PyTorch CUDA 推理初始化成功。 +- [x] YOPO 以 `output_enabled=false` 被动启动成功。 +- [x] VINS-Fusion-ROS2 在 Jetson Humble 上完成 Ceres 构建兼容修复，保留为回退方案。 +- [x] JetPack、CUDA、Docker、NVIDIA Container Runtime 和 D435i USB3 链路完成基础审计。 +- [x] Isaac ROS Common 固定到 `v3.2-15`，RealSense Dev 镜像已构建并固定标签。 +- [x] NVIDIA GPU/PVA CDI 名称已注册，官方 `run_dev.sh -b` 可启动容器并识别 Orin GPU。 +- [x] D435i IR1、IR2、Depth 和合并 IMU 的短时帧率、时间戳及同步基线通过。 + +### 1.2 尚未完成 + +- [ ] Isaac ROS Visual SLAM `v3.2-15` 固定版本导入、构建与官方运行基线。 +- [ ] D435i IR 双目、IMU、深度至少 10 分钟并发流压力测试。 +- [ ] cuVSLAM 里程计到项目 `/state/odom` 契约的适配。 +- [ ] cuVSLAM 与 YOPO PASSIVE 联合运行和长期性能验收。 + +本文件当前只安排上述两类任务。ROS1/RflySim 仿真、PX4、MAVROS、控制器、TensorRT 和实飞均不属于当前两个阶段。 + +## 2. 工作范围与协作边界 + +### 2.1 代码、仓库与工作区 + +| 位置 | 当前状态 | 用途与规则 | +| --- | --- | --- | +| `D:\catkin_ws\src\YOPO` | `ros2-humble@80c0569`，干净 | 当前可部署 ROS2 基线，允许按真实问题修改 | +| `work\YOPO-phase2` | `jetson-passive-deployment@80c0569`，无 upstream | 阶段二本地草案克隆；`stash@{0}` 仅属于该克隆，不是可发布版本 | +| `D:\catkin_ws\src\YOPO-ROS2` | 第三方参考 | 只读参考，不覆盖官方算法主体 | +| `D:\catkin_ws\src\VINS-Fusion-ROS2` | `jetson-orin-build-fixes@11d64f1` | 已修改并推送的回退/A-B 基线，不是当前 cuVSLAM 集成对象 | +| `u5-4/cuVSLAM` | `main` 为 core 17；阶段二计划 `u5-4/isaac-ros-3.2-yopo-adapter` 分支 | 用户指定的适配代码托管仓库；core 保持只读，适配层与 core17 隔离 | +| Jetson `~/catkin_ws` | YOPO/VINS ROS2 工作区 | 已有包验证，不放入 Isaac ROS 全栈 | +| Jetson `~/workspaces/isaac_ros_3_2` | 已创建；Common 固定 `v3.2-15` | Isaac ROS 3.2 独立工作区；RealSense Dev 镜像已构建，Visual SLAM 源码尚未导入 | + +`stash@{0}` 的序号会变化，不能作为长期版本标识。草案只有在审查、测试并提交后才能视为可部署代码。 + +### 2.2 职责分工 + +- Codex：阅读和修改 Windows 本地源码、设计接口、根据用户返回的真实日志修复问题。 +- 用户：在 Jetson/WSL2 执行安装、构建和运行命令，并返回完整输出。 +- 不通过 SSH 操作飞行计算机；不替用户启动传感器、控制器或飞行任务。 +- Git 推送、分支发布或外部状态修改只在用户明确要求时执行。 + +### 2.3 安全门禁 + +- 不启动 RflySim、SITL、PX4、MAVROS、OFFBOARD、解锁或起飞脚本。 +- 不启用 YOPO 控制输出；`output_enabled` 必须保持 `false`。 +- 不将 cuVSLAM 输出回灌 PX4 external vision。 +- 不在 Jetson 上运行 ROS1/Noetic 仿真；如需仿真，只能在 PC/WSL 隔离环境另行安排。 +- 不盲目刷写 D435i 固件。 +- 宿主机与容器不能同时启动两份 RealSense 驱动。 +- 不为尚未出现的问题提前改代码、安装无关依赖或转换 TensorRT。 + +### 2.4 代码修改与上传边界 + +“适配 cuVSLAM”在当前项目中指适配其 ROS2 输出、坐标、速度、健康状态和 YOPO 接口，不等于修改或替换 cuVSLAM core。 + +| 仓库 | 允许做 | 禁止做 | 代码上传位置 | +| --- | --- | --- | --- | +| `u5-4/YOPO_ROS2` | 修复 YOPO ROS2 接口、被动安全和真实部署问题 | 覆盖官方算法主体、未经验证启用控制、直接改 `main` | 已验收的 YOPO 专用分支 | +| `u5-4/cuVSLAM` | 在隔离目录增加 ROS2 adapter、launch、配置、测试和兼容文档 | 修改 core17、根 CMake/README/LICENSE/VERSION、`libs/`、`python/`、`examples/` | `u5-4/isaac-ros-3.2-yopo-adapter` | +| `u5-4/VINS-Fusion-ROS2` | 维护回退/A-B 基线和真实 Jetson 构建修复 | 与 cuVSLAM 阶段混改或同时占用 D435i | `jetson-orin-build-fixes` | +| NVIDIA Isaac ROS 官方仓库 | 固定 `v3.2-15`、构建、运行和记录 commit | 修改、覆盖、向官方仓库上传或混用 4.x/main | 不上传；只作为固定外部依赖 | + +`u5-4/cuVSLAM` 中允许新增的唯一路径为： + +```text +integrations/isaac_ros_3_2_yopo/ + README.md + COMPATIBILITY.md + LICENSE # 仅覆盖本目录新增代码，具体许可证由用户确认 + config/ + launch/ + cuvslam_yopo_adapter/ + package.xml + CMakeLists.txt + include/ + src/ + test/ +``` + +该 adapter 必须满足： + +- 只通过 ROS2 topic 与外部 Isaac ROS Visual SLAM 通信。 +- 不包含或链接 core17 的头文件、共享库、Python 包或 CMake target。 +- 首选标准 ROS2 接口：`nav_msgs`、`diagnostic_msgs`、`tf2` 等。 +- 输入 `/visual_slam/tracking/odometry` 与标准 `/diagnostics`，输出 `/state/odom` 和 adapter diagnostics。 +- `/depth_image` 不属于该 adapter，由唯一 RealSense 驱动独立发布、remap 或转换。 +- README 明确运行时依赖外部 Isaac ROS `v3.2-15` 与 cuVSLAM SDK `12.6`；仓库内 core `17.0.0` 不构建、不链接、不运行。 + +Git 与上传规则： + +- `main` 始终跟随 NVIDIA 上游 core，不直接提交 YOPO 适配代码。 +- 分支遵守仓库规则，计划名称为 `u5-4/isaac-ros-3.2-yopo-adapter`，不使用容易冒充 NVIDIA 发行线的 `release-3.2` 等名称。 +- 任何 branch/commit 操作前先获得用户明确确认；提交使用 `git commit -s`。 +- 只允许 `git add integrations/isaac_ros_3_2_yopo/...`，禁止 `git add .`。 +- 提交前执行路径白名单检查，所有变更必须位于上述 integration 目录。 +- 禁止提交权重、固件、设备序列号、运行日志、容器缓存或构建产物。 +- 禁止 force-push、直接推送 `main` 或创建类似 NVIDIA release 的 tag。 +- cuVSLAM 仓库自身 `AGENTS.md` 规定 Codex 不执行 `git push`；Codex准备本地差异和经授权的签署提交，由用户审核后推送到自己的 fork。 +- 如果确认必须修改 Isaac ROS wrapper，本阶段立即停止并重新决策；不能把 wrapper patch 或 SDK12.6 二进制改动伪装成 core17 修改。 + +## 3. Git 与版本基线 + +| 组件 | 固定版本 | 说明 | +| --- | --- | --- | +| YOPO ROS2 | `ros2-humble@80c0569` | 当前 1.0 被动基线 | +| YOPO 阶段二草案 | `work\YOPO-phase2` 中的 `jetson-passive-deployment@80c0569` | 仅本地分支；尚无 upstream，stash 未发布 | +| VINS-Fusion-ROS2 | `jetson-orin-build-fixes@11d64f1` | 构建成功的回退版本 | +| Isaac ROS | `v3.2-15` | JetPack 6.2、Orin、Ubuntu 22.04、ROS2 Humble 维护线 | +| Isaac ROS Visual SLAM | 目标 `v3.2-15@e31f4cc` | 必须与 Common/NITROS 使用同一 3.2 更新代际；Jetson 尚未导入 | +| cuVSLAM SDK | Isaac ROS 3.2 配套 SDK `12.6` | 此处 12.6 是 SDK 版本，不是 CUDA 版本 | +| `u5-4/cuVSLAM` | core `17.0.0` 开发线 | 与 Isaac ROS 3.2 wrapper 不兼容，不加入当前构建 | +| cuVSLAM YOPO adapter | 计划 `u5-4/isaac-ros-3.2-yopo-adapter` | 只包含隔离 ROS2 integration 目录，不链接 core17 | + +当前路线是：隔离评估 cuVSLAM，达标后才允许将其晋升为 YOPO 主里程计；VINS 保留为回退和 A/B 对照。 + +## 4. 已验证环境基线 + +### 4.1 Jetson + +| 项目 | 当前值 | 状态 | +| --- | --- | --- | +| 设备 | Jetson Orin NX 16GB | 已确认 | +| JetPack / L4T | JetPack 6.2 / `36.4.3` | 与 Isaac ROS 3.2 Update 1+ 匹配 | +| Ubuntu | `22.04.5 LTS` | 已确认 | +| ROS2 | Humble | 当前目标；每个新终端仍需检查未混入 Noetic | +| CUDA | `12.6`，nvcc `12.6.68` | `/usr/local/cuda/bin/nvcc` 已验证 | +| cuDNN / TensorRT | `9.3` / `10.3` | 已确认 | +| YOPO PyTorch | `2.11.0`，Orin CUDA 可用 | 在 YOPO 运行环境验证成功 | +| Conda | 存在 `base`/`yopo` 环境 | 启动 Isaac ROS 脚本前先 `conda deactivate` | +| Docker | `29.4.3` | 已安装；用户已加入 `docker` 组，重启后的持久权限仍待复核 | +| Buildx | `0.34.0` | 已安装 | +| NVIDIA Container Toolkit | `1.16.2` | 已安装 | +| Docker Runtime | `nvidia` 已注册，默认 `runc` | 正常；无需改默认 Runtime | +| Isaac ROS Common | `v3.2-15@fcf4d9e` | 工作区和 RealSense Dev 镜像已完成；Visual SLAM 尚未构建 | +| RealSense Dev 镜像 | `isaac_ros_dev-aarch64:isaac3.2-rs2.55.1-jp6.2` | image ID `sha256:bceda07bd17756dc582693e00fc388fb5b2589c94d8267e6a253546640f8a933` | + +环境诊断原始日志：`jetson_env_20260716_074237.log`。 + +### 4.2 D435i + +| 项目 | 当前值 | 状态 | +| --- | --- | --- | +| USB ID | `8086:0b3a` | 已识别 | +| USB 链路 | D435i 所有接口 `5000M`；descriptor `3.2` | 已在 `lsusb -t` 与 librealsense 实测为 USB3 | +| 序列号 | `243622070369` | 已记录 | +| 固件 | `5.15.1.55` | 不属于 Isaac ROS 3.2 官方验证组合 | +| librealsense | `2.55.1` | 容器内 `pkg-config` 验证 | +| RealSense ROS | `4.51.1-0jammy` | camera、msgs、description 均已安装 | +| 图像流 | IR1 `29.99 Hz`、IR2 `29.99 Hz`、Depth `29.99 Hz` | 短时并发基线通过 | +| 惯性流 | Gyro `199.8 Hz`、合并 IMU `199.8 Hz` | `unite_imu_method=2`；加速度和角速度均已取样 | +| 时间戳 | IR1/IR2/Depth/IMU 均 `zero=0`、`nonmonotonic=0` | IR1-IR2、IR1-Depth 的 p95 均为 `0.000 ms` | +| 枚举与并发稳定性 | 短时通过 | 至少 10 分钟 Visual SLAM 联合压力测试仍待做 | + +Isaac ROS 3.2 官方 Quickstart 要求： + +```text +Firmware: 5.13.0.50 +librealsense: 2.55.1 +realsense-ros: 4.51.1-isaac +``` + +当前固件已通过枚举和短时多流测试，暂不刷写。它尚未证明 Visual SLAM 全链路兼容；只有在 USB、容器驱动和配置均确认正确后仍有明确版本证据，才评估有回退方案的固件变更。 + +`/camera/accel/sample` 在 `unite_imu_method=2` 下未独立发布不是故障；加速度已合入 `/camera/imu`，并已确认该消息同时含有效 `linear_acceleration` 与 `angular_velocity`。 + +### 4.3 YOPO 被动基线 + +- `quadrotor_msgs`、`yopo_planner` 已能从 `~/catkin_ws` 被 `colcon list` 发现和构建。 +- `ros2 interface show quadrotor_msgs/msg/PositionCommand` 已通过。 +- 官方 `epoch50.pth` 已在 Jetson CUDA 上加载。 +- `/yopo_net` 的 `output_enabled` 已验证为 `false`。 +- 当前实现即使在被动模式仍会创建 `/so3_control/pos_cmd` publisher 和控制 timer，只是在发布回调内返回。因此“ROS 图中存在 publisher”不等于“实际发布控制消息”。阶段二必须验证该 topic 实际消息数为零。 + +## 5. 已确认问题与技术决策 + +### P-001：`quadrotor_msgs` 曾无法发现 + +- 状态：已解决。 +- 原因：曾在包目录内构建，未从 `~/catkin_ws` 工作区根目录执行。 +- 验证：`colcon list`、消息接口和 `yopo_node` 均已通过。 + +### P-002：缺少 `policy/models/__init__.py` + +- 状态：未解决，当前非阻塞。 +- 现象：`setup.py` 声明 `policy.models`，文件实际不存在；`--symlink-install` 构建产生警告。 +- 处理原则：阶段二先增加普通安装验证；只有确认影响部署时再修复并提交。 + +### P-003：CUDA 编译器路径 + +- 状态：已确认。 +- `/usr/local/cuda/bin/nvcc` 为 CUDA `12.6.68`；是否加入 shell `PATH` 不是当前阻塞项。 + +### P-004：D435i 固件偏离官方组合 + +- 状态：短时 RealSense 基线通过；Visual SLAM 兼容性待验证。 +- 当前固件 `5.15.1.55`，官方 3.2 Quickstart 要求 `5.13.0.50`。 +- 当前固件已完成 USB3.2 枚举、IR 双目、Depth 和合并 IMU 短时并发验证。 +- 禁止在 USB 不稳定或没有恢复方案时刷写。 + +### P-005：RealSense 发射器策略未确定 + +- 状态：待阶段一验证。 +- NVIDIA Visual SLAM 文档要求 IR 跟踪时关闭 emitter，避免投射白点造成漂移；YOPO 深度在低纹理环境可能受益于主动纹理。 +- 必须分别记录 emitter 关闭时的 VIO 与深度质量，不可默认选择。 + +### P-006：深度与相机契约尚未验收 + +- 状态：待阶段二验证。 +- YOPO 当前支持 `16UC1` 毫米和 `32FC1` 米，错误单位可能造成 1000 倍尺度错误。 +- 目标配置需记录实际 RealSense topic、编码、单位、`480x270`、目标 FPS、CameraInfo、FOV 和时间戳。 +- `/depth_image` 由唯一的 RealSense 驱动独立 remap/转换，不由里程计适配节点生成。 + +### P-007：缺少 Git LFS 导致 `run_dev.sh` 静默退出 + +- 状态：已解决。 +- 现象：`run_dev.sh` 只打印调用目录后退出，没有显示预期错误。 +- 原因：Jetson 缺少 `git-lfs`；官方脚本启用 `set -e`，在静默执行 `git lfs`失败时提前退出，来不及执行后续错误提示。 +- 处理：安装 Ubuntu `git-lfs 3.0.2` 并执行 `git lfs install --skip-repo`。 +- 验证：重新运行后已进入 `aarch64.ros2_humble.realsense` 镜像构建流程。 + +### P-008：RealSense 镜像层未识别 librealsense `v2.55.1` + +- 状态：已解决。 +- 现象：`Dockerfile.realsense` 调用 `build-librealsense.sh -v v2.55.1`，最终报告该 tag 不可用并以 exit code 1 退出。 +- 原因：Docker build 内 GitHub/TLS 传输中断；上游 `v2.55.1@e196cefa` 始终存在，“tag 不可用”是 clone 失败后的连锁误报。 +- 处理：在官方源码外配置 Git transport，固定 HTTP/1.1、`http.maxRequests=1`、canonical `realsenseai/librealsense` URL rewrite，并在构建时显式传入 LAN 代理。 +- 边界：未修改 NVIDIA Isaac ROS 官方 Dockerfile、脚本或仓库内容。 +- 验证：镜像构建成功；容器内 `pkg-config --modversion realsense2` 返回 `2.55.1`，D435i 可由 `rs-enumerate-devices` 枚举。 + +### P-009：容器启动时缺少 GPU CDI 设备 + +- 状态：已解决。 +- 现象：`run_dev.sh -b` 报 `unresolvable CDI devices nvidia.com/gpu=all`。 +- 原因：宿主已有 `pva-allow-2` 提供的 PVA CDI，但缺少 GPU CDI spec。 +- 处理：生成 `/etc/cdi/nvidia.yaml`，保留 `/etc/cdi/nvidia-pva.yaml`。 +- 验证：`nvidia-ctk cdi list` 已包含 `nvidia.com/gpu={0,all}` 与 `nvidia.com/pva={0,all}`；`run_dev.sh -b` 可启动，容器内 `nvidia-smi` 识别 Orin，`/dev/nvidia0` 可见。 +- 未过度声明：尚未单独证明容器内 `/dev/nvhost-pva*` 节点可见；PVA 节点仍需在后续运行检查中复核。 + +### P-010：Docker 默认 bridge 的 `iptables raw` 错误 + +- 状态：当前路线不适用，非阻塞。 +- 现象：手工使用 Docker 默认 bridge 网络测试时报 `Unable to enable DIRECT ACCESS FILTERING`，宿主 legacy iptables 缺少 `raw` 表。 +- 判断：官方 `run_dev.sh` 使用 host network，且实际容器已成功启动；该错误不影响当前 Isaac ROS 路线。 +- 处理原则：不为无关的 bridge 测试修改 Jetson 内核或 iptables；只有未来明确需要 Docker bridge 时再单独处理。 + +### D-001：Isaac ROS 使用容器优先 + +- 固定 `isaac_ros_common`、`isaac_ros_visual_slam`、NITROS 到同一 `v3.2-15`。 +- `v3.2-15` 是仓库 tag；开发镜像由对应版本 `run_dev.sh` 分层构建，不是一个可随意混用的 core17 镜像标签。 +- 已构建镜像固定为 `isaac_ros_dev-aarch64:isaac3.2-rs2.55.1-jp6.2`，作为恢复锚点。`run_dev.sh -b` 实际读取 `isaac_ros_dev-aarch64:latest`，启动前需确认二者 image ID 一致；不一致时才将固定标签重新 tag 为 `latest`。 +- 使用官方 `run_dev.sh`，不手写不完整的 `docker run`。脚本负责 NVIDIA Runtime、host network、IPC、设备和工作区挂载。 +- Isaac ROS 官方源码在当前两阶段保持只读；配置和外部 adapter 保存在用户 fork 的隔离 integration 目录。 + +### D-002：容器与宿主暂时分离 + +- Isaac 容器：唯一一份 `realsense2_camera`、Visual SLAM，以及需要 Isaac 状态消息的适配逻辑。 +- Jetson 宿主：现有 YOPO Python/CUDA 环境。 +- 双方固定相同 `ROS_DOMAIN_ID`、RMW 和经过验证的 QoS。 +- 第一版不把 YOPO 安装进 Isaac 系统 Python，避免 Torch、OpenCV、Ceres 和 GXF/NITROS 依赖冲突。 + +## 6. 当前推荐架构 + +```text +Isaac ROS 3.2 容器（仓库统一 v3.2-15） + +D435i IR1 + IR2 + IMU + -> isaac_ros_visual_slam（配套 cuVSLAM SDK 12.6） + -> /visual_slam/tracking/odometry + /visual_slam/status + -> 状态/坐标适配节点 + -> /state/odom + +D435i Depth + CameraInfo + -> 独立 remap/必要的编码转换 + -> /depth_image + /depth_image/camera_info + + ↓ ROS2 DDS + +Jetson 宿主 + -> YOPO（output_enabled=false） + -> 仅记录候选轨迹、推理延迟和健康状态 +``` + +cuVSLAM release-3.2 不消费 YOPO 的深度图。深度与 VIO 是同一 RealSense 驱动发布的两条并行数据链。 + +## 7. 后续仅保留两个阶段 + +### 阶段一：Isaac ROS 3.2 / D435i 官方基线 + +**状态：进行中（2026-07-17 启动）** +**当前检查点：1C - 构建 Visual SLAM `v3.2-15` 并验证真实里程计输出。** + +检查点 1A 已于 2026-07-17 通过： + +- ROS 环境仅包含 Humble：`AMENT_PREFIX_PATH=/opt/ros/humble`。 +- `isaac_ros_common` HEAD 为 `fcf4d9e17f8f0a7f47f1d22d6a18421ce3768c01`。 +- `v3.2-14` 与 `v3.2-15` 同时指向该提交；HEAD 已与 `refs/tags/v3.2-15^{commit}` 精确比对通过。 +- `.isaac_ros_common-config` 已设置 `CONFIG_IMAGE_KEY=ros2_humble.realsense`。 +- NVMe 根分区剩余约 `60G`，当前可进入容器构建检查点。 + +检查点 1B 已于 2026-07-17 通过： + +- `ros2_humble.realsense` Dev 镜像已成功构建并固定为 `isaac_ros_dev-aarch64:isaac3.2-rs2.55.1-jp6.2`，image ID 为 `sha256:bceda07bd17756dc582693e00fc388fb5b2589c94d8267e6a253546640f8a933`。 +- GPU/PVA CDI 名称已注册；`run_dev.sh -b` 成功进入容器，`nvidia-smi` 识别 Orin GPU。PVA CDI 存在，但容器内 PVA 节点仍待单独复核。 +- 容器内 librealsense 为 `2.55.1`，RealSense ROS 为 `4.51.1-0jammy`，D435i 固件为 `5.15.1.55`，USB descriptor 为 `3.2`。 +- IR1、IR2、Depth 均约 `29.99 Hz`，Gyro 与合并 IMU 约 `199.8 Hz`；被测消息时间戳非零、单调，双红外和红外/深度同步偏差 p95 均为 `0.000 ms`。 +- `unite_imu_method=2` 下 `/camera/accel/sample` 不独立发布；加速度和角速度已在 `/camera/imu` 中确认。 + +检查点 1C 的源码预审结论： + +- Visual SLAM `v3.2-15` 目标 commit 为 `e31f4cc1d41a329a01946e5fe63669f8b15da677`；它与 `v3.2-14` 共用 commit，不能只依赖 `git describe`，必须比对完整 SHA。 +- Jetson 已将该仓库导入 `~/workspaces/isaac_ros_3_2/src/isaac_ros_visual_slam`；完整 SHA 比对通过，工作区源码固定完成。 +- 官方 `isaac_ros_visual_slam_realsense.launch.py` 会独占启动一份 RealSense 节点，配置 IR1/IR2 `640x360x90`、Gyro/Accel `200 Hz`、`unite_imu_method=2`、emitter 关闭、Depth 关闭并启用 IMU fusion。 +- 官方 launch 已 remap IR1、IR2、对应 CameraInfo 和 `/camera/imu`；启动前必须关闭现有 `/camera/camera`，禁止两份驱动争用 D435i。 +- 该 launch 适合官方功能基线，但不是最终低负载配置：它开启三类 SLAM 可视化，并沿用 `enable_localization_n_mapping=true` 默认值。基线通过后再从外部 integration 目录提供 odometry-only launch，不修改 NVIDIA 仓库。 +- wrapper 从 `isaac_ros_nitros` 配套资源链接 `libcuvslam.so` 并使用旧 `CUVSLAM_*` C API；不得链接 `u5-4/cuVSLAM` core17 C++ API。 + +#### 目标 + +在独立工作区中复现 NVIDIA 官方 RealSense Visual SLAM 基线，证明容器、D435i 双目、IMU 和 cuVSLAM SDK 在当前 JetPack 6.2 上可用。阶段一不接 YOPO，也不修改当前 YOPO/VINS 基线。 + +#### 任务 + +- [x] 新建 `~/workspaces/isaac_ros_3_2/src`。 +- [x] 克隆 `isaac_ros_common` 并固定 `v3.2-15`。 +- [x] 使用已配置的 `CONFIG_IMAGE_KEY=ros2_humble.realsense` 运行官方 `run_dev.sh` 并构建容器。 +- [x] 在容器内确认 NVIDIA GPU、D435i USB3、librealsense 和 ROS2 Humble。 +- [x] 完成 IR1/IR2/Depth/合并 IMU 的短时帧率、时间戳和同步基线。 +- [x] 克隆 Visual SLAM 并精确固定到 `v3.2-15@e31f4cc`；不链接 `u5-4/cuVSLAM` core17。 +- [x] 在现有容器内解析依赖并构建 `isaac_ros_visual_slam` 与 interfaces 包。 +- [x] 先运行官方 RealSense + IMU fusion 基线并保存 topic、频率、状态和日志；随后再做关闭 IMU 的视觉-only A/B 检查。 +- [ ] 使用 odometry-only 配置：关闭建图、回环、ground constraint 和非必要可视化。 +- [ ] 验证当前固件 `5.15.1.55` 是否能运行官方链路；失败时先定位版本证据，不立即刷固件。 +- [ ] 记录 emitter 关闭时的图像和 VIO 质量。 + +#### 验收标准 + +- [x] `isaac_ros_common` 的实际 tag/commit 已记录并精确固定到 `v3.2-15`。 +- [x] Visual SLAM 的 Jetson 实际 tag/commit 已记录并固定到 `v3.2-15@e31f4cc`。 +- [x] 容器能识别 NVIDIA Runtime、Orin GPU 和 D435i USB3 链路。 +- [x] IR1、IR2、Depth、合并 IMU 的短时时间戳单调且实际频率稳定。 +- [ ] CameraInfo 时间戳、长期队列积压和 PVA 设备节点已单独复核。 +- [ ] `/visual_slam/status` 正常，`/visual_slam/tracking/odometry` 连续输出。 +- [ ] 保持相机静止、前后左右上下和平移/旋转时，方向、尺度和姿态符号符合预期。 +- [ ] IR 双目 + IMU 连续运行至少 10 分钟，无 USB disconnect、进程崩溃、CUDA OOM 或持续 tracking lost。 +- [ ] 阶段一日志、配置和未解决问题已归档；未启动 YOPO、PX4 或任何控制组件。 + +#### 阶段一立即执行的两个动作 + +1. 重启或重新登录后复核 `docker` 组、CDI 清单，并确认 `latest` 与固定镜像 image ID 一致，再用 `run_dev.sh -b` 复用现有镜像；禁止触发无必要的 RealSense 重建。 +2. 在现有容器内构建 Visual SLAM，随后验证 `IR1 + IR2 + /camera/imu -> /visual_slam/status + /visual_slam/tracking/odometry`。 + +### 阶段二：cuVSLAM 到 YOPO 的联合 PASSIVE 验收 + +#### 进入条件 + +阶段一全部通过，且固件、emitter、双目/IMU频率和 Visual SLAM 配置已有固定记录。 + +#### 目标 + +建立并验收以下被动闭环，不产生任何有效控制消息： + +```text +/visual_slam/tracking/odometry + status + -> adapter -> /state/odom + +D435i depth + CameraInfo + -> /depth_image + +/state/odom + /depth_image + goal + -> YOPO PASSIVE +``` + +#### 任务 + +- [ ] 在 `u5-4/cuVSLAM` 的计划分支 `u5-4/isaac-ros-3.2-yopo-adapter` 下，仅在 `integrations/isaac_ros_3_2_yopo/` 新建独立 ROS2 适配包。 +- [ ] adapter 不构建或链接仓库 core17；明确输入、输出、frame、速度语义和健康门控。 +- [ ] 将 cuVSLAM 任意初始化世界通过唯一对齐关系转换到项目 `map`。 +- [ ] 输出 `map -> base_link` 语义，并验证四元数 `xyzw`。 +- [ ] 查明 cuVSLAM odometry twist 的实际参考系；如为机体系，旋转成 YOPO 要求的世界系速度。 +- [ ] 对 tracking lost、odometry 超时和时间戳回退实施阻断，禁止发布陈旧 `/state/odom`。 +- [ ] 在唯一 RealSense 驱动中启用深度，固定实际 topic、`480x270`、目标 FPS、编码、单位和 CameraInfo。 +- [ ] 验证 `16UC1` 为毫米或 `32FC1` 为米；禁止隐式单位猜测。 +- [ ] 保持 YOPO `output_enabled=false`，联合运行 cuVSLAM、depth 和 YOPO。 +- [ ] 记录 GPU、CPU、内存、温度、功耗、图像/IMU/odom频率、YOPO延迟和队列积压。 +- [ ] 只从 `work\YOPO-phase2` 中选择经过真实日志证明需要的草案改动；审查后提交，不直接发布 stash。 + +#### 验收标准 + +- [ ] `/state/odom` 的 pose 位于 `map`，child 为 `base_link`，位置、姿态和世界系速度方向全部正确。 +- [ ] D435i 深度编码、单位、分辨率、FOV、CameraInfo 和时间戳已归档并与 YOPO 配置一致。 +- [ ] VIO、深度和 YOPO 联合运行至少 30 分钟，无 USB disconnect、崩溃、CUDA OOM、持续掉帧或热失控。 +- [ ] tracking lost、里程计超时或深度失效时，YOPO 不使用陈旧状态。 +- [ ] `output_enabled=false`；虽然 ROS 图中存在控制 publisher，但 `/so3_control/pos_cmd` 的实际消息数为零。 +- [ ] 联合运行日志、配置、适配代码和固定 commit 已归档。 +- [ ] 阶段二通过只代表“真机被动导航链可用”，不代表允许 PX4 回灌、OFFBOARD 或飞行。 + +## 8. 当前不进入的工作 + +- ROS1/RflySim 仿真基线：如后续确有必要，在 PC/WSL 独立安排，不占用当前两阶段。 +- TensorRT：当前 PyTorch CUDA 已可运行；待被动数据链稳定后再评估，并必须在目标 Jetson 生成引擎。 +- VINS 实机定位：保留为回退/A-B 基线，不与阶段一容器同时争用 D435i。 +- PX4 external vision、MAVROS、控制器、OFFBOARD、拆桨、系留和实飞：全部延后，当前禁止启动。 +- 训练新模型：不属于当前部署阻塞项。 + +## 9. 进度与问题记录模板 + +### 9.1 进度记录 + +```markdown +#### YYYY-MM-DD：任务标题 + +- 所属阶段：一 / 二 +- 目标： +- 执行环境： +- 仓库、分支、tag、commit： +- 执行命令： +- 结果：成功 / 失败 / 部分成功 +- 关键输出与日志路径： +- 是否满足验收标准： +- 下一步： +``` + +### 9.2 问题记录 + +```markdown +#### P-XXX：问题标题 + +- 状态：待处理 / 处理中 / 已解决 / 暂缓 +- 所属阶段： +- 发现日期： +- 环境与版本： +- 复现步骤： +- 完整报错或日志路径： +- 原因分析： +- 最小处理方案： +- 验证结果： +``` + +## 10. 官方参考资料 + +- [Isaac ROS 3.2 Getting Started](https://nvidia-isaac-ros.github.io/v/release-3.2/getting_started/index.html) +- [Isaac ROS 3.2 Release Notes](https://nvidia-isaac-ros.github.io/v/release-3.2/releases/index.html) +- [Isaac ROS 3.2 Compute Setup](https://nvidia-isaac-ros.github.io/v/release-3.2/getting_started/hardware_setup/compute/index.html) +- [Isaac ROS 3.2 RealSense Setup](https://nvidia-isaac-ros.github.io/v/release-3.2/getting_started/hardware_setup/sensors/realsense_setup.html) +- [Isaac ROS 3.2 RealSense VSLAM](https://nvidia-isaac-ros.github.io/v/release-3.2/concepts/visual_slam/cuvslam/tutorial_realsense.html) + + + + + + + +补充‘： + +不是同一个东西，但关系很紧密： + +``` +isaac_ros_visual_slam + = ROS2 封装层、节点、launch、话题接口 + ↓ 调用 +libcuvslam.so + = 真正执行视觉惯性里程计的 cuVSLAM SDK 引擎 +``` + +我们现在要克隆的 `NVIDIA-ISAAC-ROS/isaac_ros_visual_slam@v3.2-15` 是 ROS2 封装源码，不是 cuVSLAM 算法核心源码。真正的 cuVSLAM SDK 12.6 已由 Isaac ROS 镜像配套提供。 + +而你的 `u5-4/cuVSLAM` 是更新的 core17 源码，使用新的 C++ API；Isaac ROS 3.2 wrapper 使用旧的 `CUVSLAM_*` C API，两者不能直接连接。 + +因此当前路线是： + +``` +D435i + -> RealSense ROS + -> isaac_ros_visual_slam v3.2-15 + -> 镜像内 cuVSLAM SDK 12.6 + -> /visual_slam/tracking/odometry +``` + +所以现在克隆 Visual SLAM wrapper 到 `~/workspaces/isaac_ros_3_2/src`；暂时不要把 `u5-4/cuVSLAM` core17 放进 `~/catkin_ws/src`。 + + + + + + + +镜像安排设计（待验证） + +更适合量产和算法迭代的架构。不过建议不要把“规划”和“飞控执行”塞进同一个镜像，应进一步拆开安全边界。 + +``` +固定基础层 +┌─────────────────────────────────────┐ +│ localization-runtime │ +│ RealSense + Isaac ROS + NITROS │ +│ Visual SLAM + cuVSLAM SDK 12.6 │ +│ 统一里程计、TF、状态、深度接口 │ +└─────────────────────────────────────┘ + │ + 标准 ROS2 DDS 接口 + │ + ┌────────────┼────────────┐ + ▼ ▼ ▼ + YOPO 镜像 EGO-Planner Super Planner + 镜像 镜像 +``` + +对于需要稠密地图的规划器，再插入可选模块： + +``` +localization-runtime + │ + ├── odometry + └── depth/pointcloud + │ + mapping-runtime + voxel / occupancy / ESDF + │ + EGO-Planner / Super Planner +``` + +典型组合就是： + +``` +YOPO： +localization + yopo + +EGO-Planner： +localization + mapping + ego-planner + +Super Planner： +localization + mapping + super-planner + +自研： +localization + 可选 mapping + custom-planner +``` + +关键是固定一套与规划器无关的接口契约： + +``` +/localization/odometry nav_msgs/msg/Odometry +/localization/status diagnostic_msgs/msg/DiagnosticArray +/camera/depth/image sensor_msgs/msg/Image +/camera/depth/camera_info sensor_msgs/msg/CameraInfo +/tf map / odom / base_link / camera_link +``` + +各规划器通过 remap 转成自己需要的话题，例如 YOPO 的： + +``` +/localization/odometry -> /state/odom +/camera/depth/image -> /depth_image +``` + +这样 localization 镜像完全不知道后面接的是 YOPO、EGO-Planner 还是其他算法。 + +量产镜像建议分成： + +- `isaac-vslam-runtime`：唯一拥有 D435i，发布定位、深度和健康状态。 +- `mapping-runtime`：可选，负责点云、体素地图、ESDF。 +- `yopo-runtime`、`ego-planner-runtime`、`super-planner-runtime`：互斥选择一个规划器。 +- `vehicle-control-gateway`：未来单独建立，负责 PX4/MAVROS/控制安全门；当前阶段不启用。 + +再通过 Docker Compose profiles 选择组合： + +``` +docker compose --profile yopo up +docker compose --profile ego up +docker compose --profile super up +``` + +这些镜像可以继承同一个 Jetson ROS2 基础镜像，因此公共层只保存一次，不会因为模块化就成倍占用磁盘。 + +需要特别明确：`isaac-vslam-runtime` 中使用的是 Isaac ROS 3.2 配套的 cuVSLAM SDK 12.6，不构建 `u5-4/cuVSLAM` core17。你的 fork 用于保存 adapter、Dockerfile、Compose、配置和兼容文档。 + +这个结构比一个大镜像更适合多规划器组合，也能让定位、建图、规划和控制分别升级、测试及回滚。 + + + + + + + + + +最终结果是 `[PASS]`。前面的 `[STOP]` 是粘贴过程混入的残留文本，实际统计显示： + +- 全部 `53868` 条 `vo_state=1` +- 全部 `53867` 条 `vo_status=0` +- 时间戳零值和回退均为 0 +- CameraInfo 完全同步 +- 只有一次 `22.255 ms` gap +- `66.348 ms` 是一次计算尖峰，未导致 tracking lost + +**当前用了什么标定** + +你虽然没有手工标定，但 D435i 出厂时已经写入了一套标定数据： + +| 参数 | 当前来源 | 是否在线重算 | +| ------------------------------- | ---------------------------------------- | ------------ | +| IR1/IR2 内参 `fx/fy/cx/cy` | D435i EEPROM，经 `CameraInfo` 发布 | 否 | +| 畸变、校正矩阵 | D435i EEPROM，经 `CameraInfo D/R/P` 发布 | 否 | +| IR1 与 IR2 双目外参/基线 | D435i 出厂标定和 TF | 否 | +| 相机与 D435i 内置 IMU 外参 | RealSense 静态 TF | 否 | +| IMU bias、重力、速度、运动状态 | cuVSLAM 跟踪过程 | 会在线估计 | +| D435i 到无人机 `base_link` 外参 | 当前没有 | 必须后续标定 | + +wrapper 会在初始化时读取一次 CameraInfo 和 TF，构造固定 camera rig，见 [visual_slam_impl.cpp (line 195)](/C:/Users/10416/Documents/Codex/2026-07-16/w-m/work/isaac_ros_visual_slam-v3.2-15/isaac_ros_visual_slam/src/impl/visual_slam_impl.cpp:195)。初始化完成后，后续 CameraInfo 不再更新；内参转换逻辑见 [cuvslam_ros_conversion.cpp (line 157)](/C:/Users/10416/Documents/Codex/2026-07-16/w-m/work/isaac_ros_visual_slam-v3.2-15/isaac_ros_visual_slam/src/impl/cuvslam_ros_conversion.cpp:157)。 + +`calibration_frequency=200` 容易误解。它只是 IMU 模型的采样频率，与噪声密度、随机游走和固定 `rig_from_imu` 一起传入 cuVSLAM，并不是在线重新标定内外参。 + +**现在验证的是什么** + +当前测试证明了： + +- D435i 出厂标定与 cuVSLAM 能正常配合。 +- 双目、IMU、时间戳、GPU 跟踪和 ROS 输出稳定。 +- 连续 10 分钟没有 tracking lost。 +- 当前固件无需为了兼容性立即刷写。 + +但它还不能证明“绝对定位很精准”，因为没有真值轨迹、已知距离或高精度姿态作为对照。 + +**后续必须标定的部分** + +最重要的是 D435i 安装到无人机后的刚体外参： + +``` +camera_link -> base_link +``` + +当前官方 launch 把 `base_frame` 直接设置为 `camera_link`，所以输出： + +``` +odom -> camera_link +``` + +这表示当前测得的是相机位姿，不是无人机机体中心位姿。接入 YOPO 前必须得到相机到机体的平移和旋转，并验证 15°/0° 相机支架的真实安装误差。 + +建议顺序： + +1. 保留 D435i 出厂内参、双目外参和相机-IMU 外参作为第一版。 +2. 用现有 rosbag 中的 CameraInfo 与 `/tf_static` 归档这套出厂数据。 +3. 标定或精确测量 `camera_link -> base_link`。 +4. 使用已知直线距离和已知角度验证尺度、方向和漂移。 +5. 只有发现双目重投影误差或 VIO 明显漂移时，才进行 Kalibr/RealSense 专用的相机-IMU或双目标定。 + +因此：cuVSLAM 有在线状态估计能力，但当前 Isaac ROS 3.2 wrapper不能替代完整的内外参标定。最关键的无人机安装外参仍需要我们后续单独完成。 + + + + + +开发容器快照 + + --filter name=isaac_ros_dev-aarch64-container + +echo "========== Preserved images ==========" +docker image inspect \ + isaac_ros_dev-aarch64:latest \ + isaac_ros_dev-aarch64:isaac3.2-rs2.55.1-vslam3.2-dev-20260717 \ + isaac_ros_dev-aarch64:isaac3.2-rs2.55.1-jp6.2 \ + --format 'tags={{json .RepoTags}} id={{.Id}}' +========== Container ========== +CONTAINER ID IMAGE COMMAND CREATED STATUS PORTS NAMES +========== Preserved images ========== +tags=["isaac_ros_dev-aarch64:isaac3.2-rs2.55.1-vslam3.2-dev-20260717","isaac_ros_dev-aarch64:latest"] id=sha256:ddd233211dc6cd29aaa6eaf6b317931e912f0ed057a762496e8df390904474dc +tags=["isaac_ros_dev-aarch64:isaac3.2-rs2.55.1-vslam3.2-dev-20260717","isaac_ros_dev-aarch64:latest"] id=sha256:ddd233211dc6cd29aaa6eaf6b317931e912f0ed057a762496e8df390904474dc +tags=["isaac_ros_dev-aarch64:isaac3.2-rs2.55.1-jp6.2"] id=sha256:bceda07bd17756dc582693e00fc388fb5b2589c94d8267e6a253546640f8a933 + + + + + + + + + +位置是对的。你没看到 `cuVSLAM` 源码是正常的，因为当前实际运行的是容器里的 cuVSLAM SDK 二进制，不是 `u5-4/cuVSLAM` 源码。 + +| 组件 | 位置 | 作用 | +| ----------------------- | ---------------------------------------------------------- | -------------------------------------- | +| `isaac_ros_common` | `~/workspaces/isaac_ros_3_2/src/` | 容器和开发环境 | +| `isaac_ros_visual_slam` | `~/workspaces/isaac_ros_3_2/src/` | ROS2 wrapper、launch、消息接口 | +| `libcuvslam.so` | 容器 `/opt/ros/humble/share/isaac_ros_nitros/cuvslam/lib/` | 真正执行 VIO 的 cuVSLAM SDK 12.6 | +| `u5-4/cuVSLAM` | 尚未克隆到 Jetson | core17 源码及后续 integration 托管仓库 | + +构建以后，SDK 还被复制到宿主工作区的 overlay： + +``` +ls -lh \ + ~/workspaces/isaac_ros_3_2/install/isaac_ros_visual_slam/lib/libcuvslam.so +``` + +容器内可以确认原始 SDK： + +``` +ls -lh \ + /opt/ros/humble/share/isaac_ros_nitros/cuvslam/lib/libcuvslam.so +``` + +所以当前结构没有缺文件： + +``` +isaac_ros_visual_slam 源码 + ↓ 调用 +容器内 libcuvslam.so SDK 12.6 +``` + +`u5-4/cuVSLAM` 不应直接克隆到 `isaac_ros_3_2/src`，否则容易让 core17 与 SDK 12.6 混入同一构建。下一阶段开始时，建议单独放在： + +``` +/home/nvidia/workspaces/cuVSLAM +``` + +只将其中的 `integrations/isaac_ros_3_2_yopo/` 作为 ROS2 集成代码参与 Isaac 工作区构建；core17 本身不构建、不链接。 + + + + + +前半部分正确，后半部分需要收紧一下。 + +Docker 镜像里已经包含 cuVSLAM SDK 12.6，`isaac_ros_visual_slam` wrapper 也已经构建并成功运行。因此不需要为了运行当前 cuVSLAM，再克隆和编译 `u5-4/cuVSLAM` 的 core17 源码。 + +当前状态是： + +| 项目 | 状态 | +| ---------------------------------- | ------------------------ | +| cuVSLAM SDK 安装、GPU 运行 | 已完成 | +| Isaac ROS Visual SLAM wrapper 构建 | 已完成 | +| D435i 双目 + D435i IMU 官方基线 | 已完成 | +| 里程计、状态、10 分钟稳定性 | 已完成 | +| 飞控 IMU 接入 | 未完成 | +| 相机—飞控 IMU 空间外参 | 未完成 | +| 相机—飞控时钟偏移和同步 | 未完成 | +| 飞控坐标系转换、噪声参数 | 未完成 | +| 外部 IMU 专用 launch | 未完成 | +| cuVSLAM 到 YOPO 的接口和健康门控 | 未完成 | +| 可复现生产 Dockerfile | 未完成，目前只是开发快照 | + +所以准确结论是： + +> cuVSLAM 的开发环境部署和 NVIDIA 官方 D435i 基线已经完成；面向实际飞行系统的适配尚未完成。 + +后续仍会克隆 `u5-4/cuVSLAM`，但目的只是新增： + +``` +integrations/isaac_ros_3_2_yopo/ +``` + +里面放飞控 IMU 适配、外部 launch、标定配置和 YOPO adapter。不会编译或修改 core17。 + +而且剩余工作不只是一个外参矩阵，还包括时间同步、IMU 坐标转换、噪声参数、运行时话题接入以及重新验收。完成这些后，才能称“生产用途的 cuVSLAM 适配收尾”。 + + + + + + + +当前阶段： + +标定 + +统一坐标系 + + + + + + + + + + + +我们可以写一个录包的launch 文件 可以仿照这个D:\catkin_ws\src\image_bag_recorder目录下的程序来另外写一个我们专门录用程序的（参考的那个是标定四目鱼眼时录制四目相机灰度图像用的） 然后内参的话D435出场就已经标定好了，所以我们只需要录包 飞控IMU和相机的应该录制的话题（灰度还是原画质） + + + + + +ummary: 1 package finished [57.2s] + +========== Overlay ========== + +/workspaces/isaac_ros-dev/install/isaac_ros_visual_slam + +========== Libraries ========== + +-rw-r--r-- 1 admin admin 4.5M Oct 26 2025 install/isaac_ros_visual_slam/lib/libcuvslam.so + +-rw-r--r-- 1 admin admin 5.4M Jul 20 02:38 install/isaac_ros_visual_slam/lib/libvisual_slam_node.so + +========== Missing libraries ========== + +[PASS] No missing dynamic libraries + +[PASS] Patched Visual SLAM build completed + +admin@tegra-ubuntu:/workspaces/isaac_ros-de 这个是CUslam的 + + + + + + + + + + + + + + +
diff --git a/integrations/isaac_ros_3_2_yopo/README_set_up.md b/integrations/isaac_ros_3_2_yopo/README_set_up.md index 8410e8e..82256de 100644 --- a/integrations/isaac_ros_3_2_yopo/README_set_up.md +++ b/integrations/isaac_ros_3_2_yopo/README_set_up.md @@ -2,6 +2,8 @@ 中文日常运行与后续扩展手册：[`STARTUP_RUNBOOK.zh-CN.md`](STARTUP_RUNBOOK.zh-CN.md) +项目任务流与当前进度：[`PROJECT_TASKFLOW.zh-CN.md`](PROJECT_TASKFLOW.zh-CN.md) + 外部飞控 IMU 运行包：[`isaac_ros_yopo_bringup/README.md`](isaac_ros_yopo_bringup/README.md) 本目录提供 YOPO 集成所使用的 Isaac ROS Visual SLAM 包装层树外补丁。本集成不会修改、 @@ -227,8 +229,9 @@ source /workspaces/isaac_ros-dev/install/setup.bash - 所有 IMU 样本均使用预期坐标轴，且标定变换方向已经转换为文档规定的 ROS TF 链； 对于所选的校正后双目输入，Kalibr 结果应被视为 `T_Crect0_I`，而不是原始光学相机 变换； -- 噪声密度与随机游走数值来自选定的飞控 IMU，并通过一个可追溯的 Allan YAML 使用 - Isaac ROS 参数所要求的单位，而不是使用相互独立的 CLI 数值或 D435i/默认数值； +- 噪声密度与随机游走数值来自选定的飞控 IMU，并通过一个带独立项目批准状态和来源 + 验证状态的版本化 YAML 使用 Isaac ROS 参数所要求的单位，而不是使用相互独立的 CLI + 数值或 D435i 默认数值；Allan 来源验证是可选增强，不与项目运行批准混为一项； - Visual SLAM 日志中没有 IMU 注册失败记录； - 使用本补丁后新录制的数据通过跟踪与时间戳检查。 diff --git a/integrations/isaac_ros_3_2_yopo/STARTUP_RUNBOOK.zh-CN.md b/integrations/isaac_ros_3_2_yopo/STARTUP_RUNBOOK.zh-CN.md index fb578f4..8de91d8 100644 --- a/integrations/isaac_ros_3_2_yopo/STARTUP_RUNBOOK.zh-CN.md +++ b/integrations/isaac_ros_3_2_yopo/STARTUP_RUNBOOK.zh-CN.md @@ -2,12 +2,14 @@ 本文档是 Jetson 机载感知与规划链路的长期启动手册。每完成并验收一个阶段，就把该阶段的启动、验证和停止命令追加到这里，使后续使用者可以按顺序直接启动系统。 +项目阶段、复选框、坐标系与 SO3 后续任务统一维护在 [`PROJECT_TASKFLOW.zh-CN.md`](PROJECT_TASKFLOW.zh-CN.md)。 + 当前文档覆盖： - Isaac ROS 开发容器的首次创建、冷启动和重复进入； - Intel RealSense D435i 正常运行节点的独立启动； - NVIDIA Isaac ROS Visual SLAM（cuVSLAM）的独立启动； -- D435i 双红外与 PX4/MAVROS 飞控 IMU 的候选统一启动； +- D435i 双红外与 PX4/MAVROS 飞控 IMU 的 odometry-only 统一启动； - 相机、IMU、cuVSLAM 输出的运行检查； - 正确停止顺序和常见故障； - 飞控 IMU、状态适配器和 YOPO PASSIVE 的后续章节边界。 @@ -23,7 +25,7 @@ | Isaac ROS 3.2 开发容器 | 已验证 | 是 | | D435i + 内置 IMU 正常运行源 | 官方组合链路已验证；拆分命令已按同一源码参数整理 | 是 | | cuVSLAM + D435i 内置 IMU | 官方组合链路已验证；拆分命令需在 Jetson 再次复核 | 是 | -| cuVSLAM + PX4/MAVROS 飞控 IMU | 运行源码已完成；PX4 Allan 参数和 Jetson 联合验收待完成 | 候选命令 | +| cuVSLAM + PX4/MAVROS 飞控 IMU | mapping-on 首次 Jetson 联合验收通过；odometry-only 后续版本待 Jetson A/B，Allan 为可选调优 | 是 | | cuVSLAM 到 `/state/odom` 适配 | 尚未完成联合验收 | 否 | | YOPO PASSIVE | 尚未完成联合验收 | 否 | | YOPO 控制输出 | 禁止启用 | 否 | @@ -52,7 +54,7 @@ 第 3、4 节提供“相机与 cuVSLAM 分终端启动”的候选命令。它们已按 NVIDIA 源码逐项核对，但必须先通过一次 Jetson 冒烟测试，才能提升为日常主路径。 -第 8 节提供飞控 IMU 候选链路。该链路尚未取代上述日常主路径：没有 PX4 Allan 噪声结果时只能做明确标记的接线冒烟，不能据此验收导航精度。 +第 8 节提供飞控 IMU 联合链路。mapping-on 首次真机联合验收已经通过；当前源码后续版本固定为 odometry-only，仍需按第 8.7 节完成 Jetson A/B 后才能替换上述恢复基线。当前 Kalibr 四项噪声权重已获项目运行批准；Allan 只作为独立噪声测量和后续调优手段。 ## 执行位置与终端约定 @@ -574,10 +576,10 @@ ros2 topic echo /visual_slam/status --once 同时查看 `C2` 中是否存在 TF、时间戳、IMU 注册或 cuVSLAM 初始化错误。 -## 8. 飞控 IMU 运行链路（源码已完成，Jetson 待验收） +## 8. 飞控 IMU odometry-only 运行链路 > [!CAUTION] -> 本节是候选链路，不是当前生产主路径。现有 `seeker_imu.yaml` 的四项值是 Kalibr 输入假设，不是 PX4 Allan 标定结果。没有真实 Allan 参数时，只能按 8.6 做接线冒烟，禁止据此验收 VIO 精度或接入飞行控制。 +> mapping-on 首次真机联合验收已经通过。当前后续版本将统一入口固定为 odometry-only，待 Jetson A/B 复验；坐标系、状态适配和系统级安全尚未完成，当前仍禁止接入飞行控制。现有 `seeker_imu.yaml` 四项值是项目批准的 Kalibr 权重，不是 PX4 Allan 输出；Allan 不作为第一版部署的强制前置。 本链路固定为： @@ -619,7 +621,7 @@ isaac_ros_yopo_bringup/config/d435i_243622070369_fcu_imu.yaml 这里有意选择“原厂 rectified CameraInfo + 使用同一相机模型求得的联合外参”。另一组自由估计内参的 Kalibr 结果为左目约 `[324.5671, 325.0486, 322.0116, 184.0997]`、右目约 `[324.9723, 325.4113, 321.3716, 184.1345]`，并估计了非零 radtan 畸变和 `49.9168 mm` 基线；它不能直接与当前 RealSense 已校正图像发布的原厂 `K/D/R/P` 混用。若以后选择该模型，必须同时实现并验收新的图像校正与 CameraInfo 发布链路，再重新生成匹配的联合外参。 -两组联合结果的 IMU/相机旋转接近，但相机原点在 IMU 坐标系中的位置相差约 `6.83 mm`。因此当前 factory-rectified 结果保持候选状态，不能只因为单次优化收敛就标记为 `approved`。 +两组联合结果的 IMU/相机旋转接近，但相机原点在 IMU 坐标系中的位置相差约 `6.83 mm`。该差异来自不同相机模型，不能解释为 factory-rectified 标定失败；当前 factory-rectified 结果已经通过真机联合运行并获项目批准。独立重复标定仍可用于后续量化可重复性。 Kalibr 时移定义为 `t_imu = t_cam + timeshift_cam_imu`，其中 `timeshift_cam_imu=-0.001737986760008108 s`。保持相机时间戳不变时，正确实现就是给原始 IMU 时间戳增加 `1,737,987 ns`，不能再次使用负号。 @@ -752,52 +754,36 @@ ros2 launch isaac_ros_yopo_bringup \ 源码补丁本身的首次应用和 `isaac_ros_visual_slam` 重编译仍按 [`README_set_up.md`](README_set_up.md) 执行。仅编译本 bringup 包不会自动重编译 NVIDIA wrapper。 -### 8.5 `C1`：使用真实 Allan 参数启动候选链路 +### 8.5 `C1`：正常启动 odometry-only 链路 执行位置：容器主终端 `C1`。运行前必须停止所有旧 RealSense 和 Visual SLAM 节点，并确保 `H4` 中 MAVROS 正常连接。 -先把真实 PX4 Allan 结果写成受版本控制的 schema YAML，并设置其容器内绝对路径： - -```bash -export PX4_IMU_NOISE_FILE=/absolute/path/to/px4_imu_allan.yaml -``` - -该文件必须使用 `schema_version: 1`，并包含 `validated: true`、匹配的 `hardware_id`、约 `170 Hz` 采样率、`method.name: allan_deviation`、源文件 SHA-256、四项精确单位和四项数值。launch 从同一个文件读取来源与数值，不接受独立 CLI 数值覆盖。 - -正式启动前执行硬门禁： - ```bash -if [ -z "${PX4_IMU_NOISE_FILE:-}" ] || [ ! -s "$PX4_IMU_NOISE_FILE" ]; then - echo "[STOP] valid PX4 Allan parameters are required" - exit 1 -fi - ros2 launch isaac_ros_yopo_bringup \ - d435i_fcu_imu_cuvslam.launch.py \ - imu_noise_file:="$PX4_IMU_NOISE_FILE" \ - allow_candidate_calibration:=true + d435i_fcu_imu_cuvslam.launch.py ``` -当前版本化联合标定状态仍是候选，因此必须显式设置 `allow_candidate_calibration:=true`。完成真实 Allan 参数和独立重复性验收、把版本化状态改为 `approved` 后，应删除这个开关。该终端会同时保持 RealSense、aligned IMU、标定 TF、cuVSLAM 和运行健康监视节点。不要再单独启动相机节点或官方组合 launch。 +默认标定记录和 IMU noise schema v2 均已获项目批准。noise 文件仍保留 `validated: false`，准确表示当前四项值来自 Kalibr 输入权重而不是独立 Allan 结果；这与 `project_status: approved` 相互独立，因此不需要临时放行参数。 -### 8.6 `C1`：仅做接线冒烟的临时命令 +launch 必须打印 `Operating mode: odometry-only`，并说明 mapping、loop closure、ground constraints、内部可视化和 `map -> odom` TF 均已关闭。该终端会同时保持 RealSense、aligned IMU、标定 TF、cuVSLAM 和运行健康监视节点。不要再单独启动相机节点或官方组合 launch。 -> [!WARNING] -> 下列命令明确使用未通过 Allan 标定的旧输入假设，只能检查节点、话题、TF、时间戳和订阅关系。它不能证明 IMU 权重正确，不能用于 VIO 精度结论，更不能用于飞行。 +### 8.6 `C1`：可选使用独立 Allan 参数 + +需要进行可选 Allan A/B 时，从 `px4_imu_noise_allan.template.yaml` 生成 schema v2 文件。`validated: true` 只表示来源文件和 SHA-256 通过核验，不会自动授予运行批准；完成项目审查后还必须把 `project_status` 从 `candidate` 改为 `approved`。 ```bash -UNVALIDATED_NOISE="$( - ros2 pkg prefix --share isaac_ros_yopo_bringup -)/config/px4_imu_noise_unvalidated.yaml" +export PX4_IMU_NOISE_FILE=/absolute/path/to/px4_imu_allan.yaml -ros2 launch isaac_ros_yopo_bringup \ - d435i_fcu_imu_cuvslam.launch.py \ - imu_noise_file:="$UNVALIDATED_NOISE" \ - allow_candidate_calibration:=true \ - allow_unvalidated_imu_noise:=true +if [ -z "${PX4_IMU_NOISE_FILE:-}" ] || [ ! -s "$PX4_IMU_NOISE_FILE" ]; then + echo "[STOP] approved PX4 Allan YAML does not exist" +else + ros2 launch isaac_ros_yopo_bringup \ + d435i_fcu_imu_cuvslam.launch.py \ + imu_noise_file:="$PX4_IMU_NOISE_FILE" +fi ``` -launch 会打印 `[WARNING] Unvalidated IMU noise was explicitly allowed`。看不到该警告时，不要把当前运行当作本节的临时冒烟。 +旧 schema v1 仅为兼容：`validated: true` 沿用旧版运行批准语义；`validated: false` 视为候选，不能启动。正常运行始终使用 schema v2。 ### 8.7 `C2`：运行验收 @@ -810,7 +796,8 @@ ros2 param get /camera/camera enable_gyro ros2 param get /camera/camera enable_accel ros2 param get /camera/camera unite_imu_method -if ros2 topic list | grep -qx /camera/imu; then +TOPICS="$(ros2 topic list)" +if printf '%s\n' "$TOPICS" | grep -qx /camera/imu; then if timeout 6s ros2 topic echo \ /camera/imu sensor_msgs/msg/Imu \ --once --qos-reliability best_effort \ @@ -852,6 +839,19 @@ ros2 param get /aligned_fcu_imu_relay output_frame_id ros2 param get /visual_slam_node imu_frame ros2 param get /visual_slam_node calibration_frequency +for parameter in \ + enable_localization_n_mapping \ + enable_ground_constraint_in_odometry \ + enable_ground_constraint_in_slam \ + enable_slam_visualization \ + enable_landmarks_view \ + enable_observations_view \ + publish_map_to_odom_tf +do + echo "--- $parameter" + ros2 param get /visual_slam_node "$parameter" +done + timeout 8s ros2 topic echo \ /fcu/imu/data_raw_aligned sensor_msgs/msg/Imu \ --once --qos-reliability best_effort @@ -860,7 +860,7 @@ timeout --signal=INT 5s ros2 run tf2_ros tf2_echo \ camera_infra1_optical_frame fcu_imu || true ``` -RealSense 启动日志必须报告序列号 `243622070369` 和固件 `5.15.1.55`。两路 CameraInfo 必须为 `640x360`，并与版本化 YAML 中的 factory `K/D/R/P` 一致；右 CameraInfo 的消息头允许存在已知的左 frame 复用问题。还必须看到 offset `1737987`、两个 IMU frame 均为 `fcu_imu`、频率参数 `170.0`。TF 平移应接近 `[0.02736293, 0.05285189, -0.06214162] m`，四元数 xyzw 应接近 `[0.50184877, -0.50942523, 0.49064963, 0.49789224]`。 +RealSense 启动日志必须报告序列号 `243622070369` 和固件 `5.15.1.55`。两路 CameraInfo 必须为 `640x360`，并与版本化 YAML 中的 factory `K/D/R/P` 一致；右 CameraInfo 的消息头允许存在已知的左 frame 复用问题。还必须看到 offset `1737987`、两个 IMU frame 均为 `fcu_imu`、频率参数 `170.0`。七个 odometry-only 参数必须全部返回 `False`。TF 平移应接近 `[0.02736293, 0.05285189, -0.06214162] m`，四元数 xyzw 应接近 `[0.50184877, -0.50942523, 0.49064963, 0.49789224]`。 检查 cuVSLAM 只有一个正确 IMU 输入： @@ -886,16 +886,26 @@ fi ```bash timeout --signal=INT 10s ros2 topic hz /camera/infra1/image_rect_raw || true timeout --signal=INT 10s ros2 topic hz /camera/infra2/image_rect_raw || true +timeout --signal=INT 10s ros2 topic hz /camera/infra1/camera_info || true +timeout --signal=INT 10s ros2 topic hz /camera/infra2/camera_info || true timeout --signal=INT 10s ros2 topic hz /fcu/imu/data_raw_aligned || true timeout --signal=INT 10s ros2 topic hz /visual_slam/tracking/odometry || true -timeout 8s ros2 topic echo /diagnostics \ - diagnostic_msgs/msg/DiagnosticArray --once || true +timeout 8s ros2 topic echo \ + /diagnostics diagnostic_msgs/msg/DiagnosticArray \ + --once --filter \ + "m.status[0].name == '/aligned_fcu_imu_relay: aligned FCU IMU'" + +timeout 8s ros2 topic echo \ + /diagnostics diagnostic_msgs/msg/DiagnosticArray \ + --once --filter \ + "m.status[0].name == '/d435i_cuvslam_runtime_health_monitor: calibrated runtime'" + timeout 8s ros2 topic echo /visual_slam/status --once || true timeout 8s ros2 topic echo /visual_slam/tracking/odometry --once || true ``` -参考值为左右图像约 `89.8 Hz`、aligned IMU 约 `170 Hz`、里程计约 `89.8 Hz`。aligned relay diagnostics 必须最终为 `OK`，`last_clock_residual_ms` 的绝对值必须小于 `250`，累计 `zero_stamp`、`invalid_stamp`、`duplicate`、`nonmonotonic`、`frame_mismatch`、`clock_domain_mismatch`、`aligned_out_of_range`、`nonfinite_measurement` 都应为 `0`；单次 IMU 间隔门限约为 `29.4 ms`，持续频率或间隔异常仍会在三个诊断周期后退出。运行健康监视 diagnostics 也必须为 `OK`，odometry 必须使用 `odom -> camera_link`、非零严格递增时间戳和有限数值，`vo_state` 应为 `1`。持续 duplicate、跨时间域、非有限测量、异常频率、CameraInfo 不匹配、D435i IMU 实际出数或里程计中断时，相应守护节点必须非零退出并触发整套 launch 关闭。 +参考值为左右图像和 CameraInfo 约 `89.8 Hz`、aligned IMU 约 `170 Hz`、里程计约 `89.8 Hz`。两个 diagnostics 的 `level` 必须为 `0`；ROS 2 YAML 可能把该字节显示成 `"\0"`。aligned relay 的 `last_clock_residual_ms` 绝对值必须小于 `250`，累计 `zero_stamp`、`invalid_stamp`、`duplicate`、`nonmonotonic`、`frame_mismatch`、`clock_domain_mismatch`、`aligned_out_of_range`、`nonfinite_measurement` 都应为 `0`；单次 IMU 间隔门限约为 `29.4 ms`，持续频率或间隔异常仍会在三个诊断周期后退出。运行健康监视 diagnostics 也必须为 `OK`，odometry 必须使用 `odom -> camera_link`、非零严格递增时间戳和有限数值，`vo_state` 应为 `1`。持续 duplicate、跨时间域、非有限测量、异常频率、CameraInfo 不匹配、D435i IMU 实际出数或里程计中断时，相应守护节点必须非零退出并触发整套 launch 关闭。 ### 8.8 停止顺序和验收边界 @@ -905,8 +915,8 @@ timeout 8s ros2 topic echo /visual_slam/tracking/odometry --once || true 源码与单元测试完成不等于真机生产验收。正式替换第 2 节日常主路径前，仍需： -- 用 PX4 原始静止数据完成 Allan 标定并固化四项噪声参数及来源； -- 在相同 factory-rectified 模型下做一次独立重复联合标定，解释当前约 `6.83 mm` 的新旧相机原点差； +- 可选：用 PX4 原始静止数据完成 Allan 标定并固化四项噪声参数及来源，用于独立噪声测量与调优； +- 可选：在相同 factory-rectified 模型下做一次独立重复联合标定，量化可重复性；当前约 `6.83 mm` 是不同相机模型结果的比较，不等同于标定失败； - 在 Jetson 验证真实 ROS `Imu` 深拷贝、BEST_EFFORT QoS、拒绝计数和 output-stale diagnostics； - 完成时间戳、轨迹质量、失跟恢复、资源占用和长时间稳定性测试。 @@ -967,3 +977,5 @@ timeout 8s ros2 topic echo /visual_slam/tracking/odometry --once || true | --- | --- | | 2026-07-21 | 加入 D435i factory-rectified + PX4/MAVROS 外部 IMU + cuVSLAM 候选统一启动、噪声门禁、验收与停止流程。 | | 2026-07-21 | 创建系统启动手册；加入容器生命周期、正常 D435i、cuVSLAM、验证、停止和后续 YOPO 章节边界。 | +| 2026-07-21 | 首次 Jetson 联合运行验收通过：双 IR/CameraInfo、aligned FCU IMU、标定 TF、唯一 IMU 订阅、两项 diagnostics、`vo_state=1` 与 `odom -> camera_link` 均满足合同，并连续运行约 17 分 48 秒；修正 `/camera/imu` 探测中的 `grep -q` 管道噪声。 | +| 2026-07-21 | 后续源码改为无临时放行参数的正常启动：项目批准与 Allan 来源状态分离，默认使用已批准 Kalibr 权重；统一 launch 固定 odometry-only，并关闭 cuVSLAM `map -> odom` TF；Jetson A/B 待完成。 | diff --git a/integrations/isaac_ros_3_2_yopo/isaac_ros_yopo_bringup/README.md b/integrations/isaac_ros_3_2_yopo/isaac_ros_yopo_bringup/README.md index bf340bb..2b18778 100644 --- a/integrations/isaac_ros_3_2_yopo/isaac_ros_yopo_bringup/README.md +++ b/integrations/isaac_ros_3_2_yopo/isaac_ros_yopo_bringup/README.md @@ -1,6 +1,6 @@ # D435i + PX4 IMU + Isaac ROS cuVSLAM 启动包 -本 ROS 2 Humble 软件包把已完成的 factory-rectified 双目/飞控 IMU 标定结果接入 Isaac ROS Visual SLAM 3.2。当前标定被版本化为运行候选：内参、外参和时间偏移已经固定，但 PX4 Allan 噪声标定与独立重复性验收尚未完成。 +本 ROS 2 Humble 软件包把已完成并获项目运行批准的 factory-rectified 双目/飞控 IMU 标定结果接入 Isaac ROS Visual SLAM 3.2。内参、外参、时间偏移和第一版 IMU 噪声权重均已版本化；PX4 Allan 噪声测量与独立重复标定保留为可选增强，不是启动前置。 完整的宿主机、容器、MAVROS、构建、启动、检查和停止顺序见父目录的 [`STARTUP_RUNBOOK.zh-CN.md`](../STARTUP_RUNBOOK.zh-CN.md) 第 8 节。 @@ -30,6 +30,8 @@ Isaac ROS 容器 统一 launch 会启动一份且仅一份 RealSense 驱动、时间对齐节点、标定静态 TF、官方 cuVSLAM component 和运行健康监视节点。D435i 的 gyro/accel 被明确关闭，cuVSLAM 的 IMU 订阅只映射到 `/fcu/imu/data_raw_aligned`。 +生产入口固定为 odometry-only：`enable_localization_n_mapping=false`，两类 ground constraint、三类内部可视化以及 cuVSLAM 的 `map -> odom` TF 发布均关闭。该入口不提供命令行开关重新启用这些功能；需要建图或调试可视化时应使用单独的调试 launch。 + relay 在 15 秒内收不到首条可发布样本，或运行中 raw/aligned 流持续中断超过 2 秒时会以错误退出；统一 launch 随即停止相机、TF 和 cuVSLAM，避免留下表面存活但不再融合 IMU 的进程。 频率或最大时间间隔单次越界先报告 WARN；连续 3 个诊断周期仍不健康时升级为 ERROR 并退出。最大间隔门限为实测周期的 5 倍，约 `29.4 ms`，为已录制数据的 `16.592 ms` 峰值保留 Jetson 负载余量。`/clock` 或 `use_sim_time` 不会替代该 system-time 新鲜度门禁。 @@ -51,22 +53,27 @@ relay 在 15 秒内收不到首条可发布样本，或运行中 raw/aligned 流 ## 标定状态门禁 -当前 YAML 状态为 `runtime_candidate_pending_allan_and_independent_repeatability`。launch 默认只接受 `approved`；现阶段真机验证必须明确传入 `allow_candidate_calibration:=true`。`rejected` 状态永远不能被命令行覆盖。 - -这个开关只承认“操作者知道当前仍是候选标定”，不会跳过 CameraInfo、TF、时间戳、IMU 数据质量或运行新鲜度检查，也不会把未验证的 IMU 噪声参数变成已验证参数。 +当前 YAML 状态为 `approved`。launch 只接受获项目批准的标定记录，`candidate` 或 `rejected` 不能由命令行覆盖。项目批准来自本机首次联合验收；它不会跳过 CameraInfo、TF、时间戳、IMU 数据质量或运行新鲜度检查。 ## IMU 噪声门禁 -launch 不接受四个彼此独立的 CLI 噪声数值，而是要求一个无默认值的 `imu_noise_file`。该 YAML 必须同时记录： +launch 不接受四个彼此独立的 CLI 噪声数值，而是从一个版本化 `imu_noise_file` 读取完整记录。默认文件为 `config/px4_imu_noise_unvalidated.yaml`；文件名中的 `unvalidated` 仅表示未声称 Allan 来源验证，不表示未获项目运行批准。 + +schema v2 将两个概念明确分开： + +- `project_status`：是否允许用于本项目运行； +- `validated`：是否具有通过文件和 SHA-256 核验的独立 Allan 来源。 + +当前默认记录是 `project_status: approved`、`validated: false`，方法仍诚实记录为 `kalibr_input_assumption`。因此它可用于第一版 cuVSLAM，但不冒充 Allan 结果。schema v1 仅为旧文件兼容：旧 `validated: true` 视为已批准，旧 `validated: false` 视为候选且不能启动。 + +该 YAML 还必须记录： -- `validated` 状态； - PX4 IMU 硬件/传输 ID； -- Allan 数据采样率； -- 方法名 `allan_deviation`； -- 源数据文件名与 SHA-256； +- 数据采样率； +- 方法名及来源文件； - 四项参数及精确单位。 -填写入口为 [`config/px4_imu_noise_allan.template.yaml`](config/px4_imu_noise_allan.template.yaml)。模板中的所有 `REPLACE_WITH_...` 都必须替换，不能把模板本身传给 launch。 +可选 Allan 结果的填写入口为 [`config/px4_imu_noise_allan.template.yaml`](config/px4_imu_noise_allan.template.yaml)。模板默认 `project_status: candidate`；完成来源核验和项目审查后才能改为 `approved`。所有 `REPLACE_WITH_...` 都必须替换，不能把模板本身传给 launch。 四项单位为： @@ -75,7 +82,13 @@ launch 不接受四个彼此独立的 CLI 噪声数值，而是要求一个无 - `accel_noise_density`：`m/(s^2*sqrt(Hz))`； - `accel_random_walk`：`m/(s^3*sqrt(Hz))`。 -现有 `seeker_imu.yaml` 中的 `0.09 / 0.05 / 0.06 / 0.001` 是 Kalibr 联合标定的输入假设，不是 Allan 标定输出。D435i 官方默认四元组也不属于 PX4 IMU。仓库中的 `config/px4_imu_noise_unvalidated.yaml` 明确记录了旧值并设置 `validated: false`；launch 默认拒绝它，`allow_unvalidated_imu_noise:=true` 只用于验证接线，不能作为导航验收结果。真实 Allan YAML 引用的源数据文件必须实际存在，且记录的 SHA-256 必须与该文件字节完全一致。 +现有 `seeker_imu.yaml` 中的 `0.09 / 0.05 / 0.06 / 0.001` 是 Kalibr 联合标定使用并经本次运行验收接受的权重，不是 Allan 标定输出。D435i 官方默认四元组也不属于 PX4 IMU。真实 Allan YAML 引用的源数据文件必须实际存在，且记录的 SHA-256 必须与该文件字节完全一致。 + +正常启动不需要临时放行参数： + +```bash +ros2 launch isaac_ros_yopo_bringup d435i_fcu_imu_cuvslam.launch.py +``` ## 本地逻辑测试 diff --git a/integrations/isaac_ros_3_2_yopo/isaac_ros_yopo_bringup/config/d435i_243622070369_fcu_imu.yaml b/integrations/isaac_ros_3_2_yopo/isaac_ros_yopo_bringup/config/d435i_243622070369_fcu_imu.yaml index dd1022c..349f047 100644 --- a/integrations/isaac_ros_3_2_yopo/isaac_ros_yopo_bringup/config/d435i_243622070369_fcu_imu.yaml +++ b/integrations/isaac_ros_3_2_yopo/isaac_ros_yopo_bringup/config/d435i_243622070369_fcu_imu.yaml @@ -1,6 +1,6 @@ schema_version: 1 calibration_id: d435i_243622070369_factory_rectified_px4_imu_20260720 -status: runtime_candidate_pending_allan_and_independent_repeatability +status: approved provenance: camera_serial: "243622070369" @@ -64,13 +64,6 @@ imu: kalibr_equation: t_imu = t_cam + timeshift_cam_imu runtime_equation: t_aligned = t_imu_raw + imu_to_camera_offset_ns imu_to_camera_offset_ns: 1737987 - noise: - status: required_external_allan_calibration - gyroscope_noise_density: null - gyroscope_random_walk: null - accelerometer_noise_density: null - accelerometer_random_walk: null - extrinsics: convention: p_camera_infra1_optical = T_camera_infra1_optical_fcu_imu * p_fcu_imu parent_frame: camera_infra1_optical_frame diff --git a/integrations/isaac_ros_3_2_yopo/isaac_ros_yopo_bringup/config/px4_imu_noise_allan.template.yaml b/integrations/isaac_ros_3_2_yopo/isaac_ros_yopo_bringup/config/px4_imu_noise_allan.template.yaml index 4086ed0..efef71d 100644 --- a/integrations/isaac_ros_3_2_yopo/isaac_ros_yopo_bringup/config/px4_imu_noise_allan.template.yaml +++ b/integrations/isaac_ros_3_2_yopo/isaac_ros_yopo_bringup/config/px4_imu_noise_allan.template.yaml @@ -1,5 +1,7 @@ -schema_version: 1 +schema_version: 2 calibration_id: REPLACE_WITH_UNIQUE_ALLAN_CALIBRATION_ID +# Allan provenance does not automatically grant runtime approval. +project_status: candidate validated: true sensor: diff --git a/integrations/isaac_ros_3_2_yopo/isaac_ros_yopo_bringup/config/px4_imu_noise_unvalidated.yaml b/integrations/isaac_ros_3_2_yopo/isaac_ros_yopo_bringup/config/px4_imu_noise_unvalidated.yaml index 7cb41af..62776da 100644 --- a/integrations/isaac_ros_3_2_yopo/isaac_ros_yopo_bringup/config/px4_imu_noise_unvalidated.yaml +++ b/integrations/isaac_ros_3_2_yopo/isaac_ros_yopo_bringup/config/px4_imu_noise_unvalidated.yaml @@ -1,5 +1,6 @@ -schema_version: 1 -calibration_id: seeker_imu_kalibr_input_assumption_unvalidated +schema_version: 2 +calibration_id: seeker_imu_kalibr_input_assumption_project_approved_20260721 +project_status: approved validated: false sensor: diff --git a/integrations/isaac_ros_3_2_yopo/isaac_ros_yopo_bringup/isaac_ros_yopo_bringup/calibration.py b/integrations/isaac_ros_3_2_yopo/isaac_ros_yopo_bringup/isaac_ros_yopo_bringup/calibration.py index 5109b79..ba40699 100644 --- a/integrations/isaac_ros_3_2_yopo/isaac_ros_yopo_bringup/isaac_ros_yopo_bringup/calibration.py +++ b/integrations/isaac_ros_3_2_yopo/isaac_ros_yopo_bringup/isaac_ros_yopo_bringup/calibration.py @@ -22,6 +22,14 @@ CANDIDATE_CALIBRATION_STATUS, REJECTED_CALIBRATION_STATUS, } +APPROVED_PROJECT_STATUS = "approved" +CANDIDATE_PROJECT_STATUS = "candidate" +REJECTED_PROJECT_STATUS = "rejected" +SUPPORTED_PROJECT_STATUSES = { + APPROVED_PROJECT_STATUS, + CANDIDATE_PROJECT_STATUS, + REJECTED_PROJECT_STATUS, +} class UniqueKeySafeLoader(yaml.SafeLoader): @@ -99,7 +107,9 @@ def right_camera_frame(self) -> str: @dataclass(frozen=True) class ImuNoiseCalibration: + schema_version: int calibration_id: str + project_status: str validated: bool sensor_hardware_id: str sample_rate_hz: float @@ -446,25 +456,28 @@ def load_calibration(path) -> RuntimeCalibration: def assert_runtime_calibration_allowed( calibration: RuntimeCalibration, - allow_candidate: bool, ) -> None: - """Reject unapproved calibration unless the one known candidate is explicit.""" - if not isinstance(allow_candidate, bool): - raise ValueError("allow_candidate must be a boolean") + """Reject a calibration that has not received project runtime approval.""" if calibration.status == APPROVED_CALIBRATION_STATUS: return if calibration.status == CANDIDATE_CALIBRATION_STATUS: - if allow_candidate: - return - raise ValueError( - "calibration is a runtime candidate pending Allan calibration and " - "independent repeatability; explicitly set allow_candidate_calibration:=true" - ) + raise ValueError("calibration is a runtime candidate and is not project-approved") if calibration.status == REJECTED_CALIBRATION_STATUS: - raise ValueError("calibration status is rejected and cannot be overridden") + raise ValueError("calibration status is rejected") raise ValueError(f"unsupported calibration status: {calibration.status}") +def assert_runtime_imu_noise_allowed(noise: ImuNoiseCalibration) -> None: + """Require project approval independently from Allan provenance validation.""" + if noise.project_status == APPROVED_PROJECT_STATUS: + return + if noise.project_status == CANDIDATE_PROJECT_STATUS: + raise ValueError("IMU noise model is a candidate and is not project-approved") + if noise.project_status == REJECTED_PROJECT_STATUS: + raise ValueError("IMU noise model is rejected") + raise ValueError(f"unsupported IMU noise project_status: {noise.project_status}") + + def load_imu_noise( path, expected_hardware_id: str, @@ -476,13 +489,27 @@ def load_imu_noise( yaml.load(stream, Loader=UniqueKeySafeLoader), "IMU noise calibration", ) - if root.get("schema_version") != 1: + schema_version = root.get("schema_version") + if schema_version not in (1, 2): raise ValueError("unsupported IMU noise schema_version") calibration_id = _nonempty_string(root.get("calibration_id"), "calibration_id") validated = root.get("validated") if not isinstance(validated, bool): raise ValueError("validated must be a boolean") + if schema_version == 1: + project_status = ( + APPROVED_PROJECT_STATUS if validated else CANDIDATE_PROJECT_STATUS + ) + else: + project_status = _nonempty_string( + root.get("project_status"), + "project_status", + ) + if project_status not in SUPPORTED_PROJECT_STATUSES: + raise ValueError( + f"unsupported IMU noise project_status: {project_status}" + ) sensor = _mapping(root.get("sensor"), "sensor") hardware_id = _nonempty_string(sensor.get("hardware_id"), "sensor.hardware_id") @@ -562,7 +589,9 @@ def load_imu_noise( raise ValueError("validated IMU noise cannot reuse a known default/input tuple") return ImuNoiseCalibration( + schema_version=schema_version, calibration_id=calibration_id, + project_status=project_status, validated=validated, sensor_hardware_id=hardware_id, sample_rate_hz=sample_rate_hz, diff --git a/integrations/isaac_ros_3_2_yopo/isaac_ros_yopo_bringup/launch/d435i_fcu_imu_cuvslam.launch.py b/integrations/isaac_ros_3_2_yopo/isaac_ros_yopo_bringup/launch/d435i_fcu_imu_cuvslam.launch.py index e7497b8..52afb27 100644 --- a/integrations/isaac_ros_3_2_yopo/isaac_ros_yopo_bringup/launch/d435i_fcu_imu_cuvslam.launch.py +++ b/integrations/isaac_ros_3_2_yopo/isaac_ros_yopo_bringup/launch/d435i_fcu_imu_cuvslam.launch.py @@ -21,6 +21,7 @@ from isaac_ros_yopo_bringup.calibration import ( assert_runtime_calibration_allowed, + assert_runtime_imu_noise_allowed, load_calibration, load_imu_noise, ) @@ -31,15 +32,6 @@ VISUAL_SLAM_PATCH_MARKER = b"ISAAC_ROS_YOPO_IMU_TIMESTAMP_PATCH_V1" -def _parse_bool(value: str, name: str) -> bool: - normalized = value.strip().lower() - if normalized in {"1", "true", "yes", "on"}: - return True - if normalized in {"0", "false", "no", "off"}: - return False - raise RuntimeError(f"{name} must be true or false, got {value!r}") - - def _shutdown_if_process_exits(action, label: str) -> RegisterEventHandler: return RegisterEventHandler( OnProcessExit( @@ -56,15 +48,8 @@ def _shutdown_if_process_exits(action, label: str) -> RegisterEventHandler: def _build_runtime_actions(context): calibration_path = LaunchConfiguration("calibration_file").perform(context) calibration = load_calibration(calibration_path) - allow_candidate_calibration = _parse_bool( - LaunchConfiguration("allow_candidate_calibration").perform(context), - "allow_candidate_calibration", - ) try: - assert_runtime_calibration_allowed( - calibration, - allow_candidate_calibration, - ) + assert_runtime_calibration_allowed(calibration) except ValueError as error: raise RuntimeError(str(error)) from error @@ -92,26 +77,16 @@ def _build_runtime_actions(context): "IMU timestamp patch marker; rebuild isaac_ros_visual_slam" ) - enable_visualization = _parse_bool( - LaunchConfiguration("enable_visualization").perform(context), - "enable_visualization", - ) - allow_unvalidated_noise = _parse_bool( - LaunchConfiguration("allow_unvalidated_imu_noise").perform(context), - "allow_unvalidated_imu_noise", - ) noise_file = LaunchConfiguration("imu_noise_file").perform(context) noise = load_imu_noise( noise_file, calibration.imu_hardware_id, calibration.imu_rate_hz, ) - if not noise.validated and not allow_unvalidated_noise: - raise RuntimeError( - "the supplied IMU noise file is not validated; provide a traceable PX4 " - "Allan result, or explicitly set " - "allow_unvalidated_imu_noise:=true for a non-production plumbing test" - ) + try: + assert_runtime_imu_noise_allowed(noise) + except ValueError as error: + raise RuntimeError(str(error)) from error camera_node = Node( package="realsense2_camera", @@ -198,10 +173,13 @@ def _build_runtime_actions(context): calibration.left_camera_frame, calibration.right_camera_frame, ], - "enable_localization_n_mapping": True, - "enable_slam_visualization": enable_visualization, - "enable_landmarks_view": enable_visualization, - "enable_observations_view": enable_visualization, + "enable_ground_constraint_in_odometry": False, + "enable_ground_constraint_in_slam": False, + "enable_localization_n_mapping": False, + "enable_slam_visualization": False, + "enable_landmarks_view": False, + "enable_observations_view": False, + "publish_map_to_odom_tf": False, }], remappings=[ ("visual_slam/image_0", "/camera/infra1/image_rect_raw"), @@ -301,8 +279,16 @@ def on_visual_slam_initialization_timeout(_context): ), LogInfo( msg=( - f"IMU noise: {noise.calibration_id}; method={noise.method}; " - f"source={noise.source_artifact}; validated={noise.validated}" + f"IMU noise: {noise.calibration_id}; " + f"project_status={noise.project_status}; method={noise.method}; " + f"source={noise.source_artifact}; " + f"allan_validated={noise.validated}" + ) + ), + LogInfo( + msg=( + "Operating mode: odometry-only; mapping, loop closure, ground " + "constraints, internal visualization, and map->odom TF are disabled" ) ), ] @@ -310,8 +296,8 @@ def on_visual_slam_initialization_timeout(_context): messages.append( LogInfo( msg=( - "[WARNING] Unvalidated IMU noise was explicitly allowed. " - "This run is for plumbing tests only, not navigation acceptance." + "IMU noise provenance: project-approved Kalibr input weights; " + "independent Allan validation is optional and is not claimed" ) ) ) @@ -347,6 +333,13 @@ def generate_launch_description() -> LaunchDescription: "d435i_243622070369_fcu_imu.yaml", ) ) + default_imu_noise = str( + os.path.join( + get_package_share_directory(PACKAGE_NAME), + "config", + "px4_imu_noise_unvalidated.yaml", + ) + ) return LaunchDescription([ DeclareLaunchArgument( "calibration_file", @@ -355,25 +348,11 @@ def generate_launch_description() -> LaunchDescription: ), DeclareLaunchArgument( "imu_noise_file", - description="Required schema-validated PX4 IMU Allan YAML file.", - ), - DeclareLaunchArgument( - "allow_candidate_calibration", - default_value="false", + default_value=default_imu_noise, description=( - "Allow the current calibration candidate pending Allan and " - "independent repeatability checks." + "Version-controlled PX4 IMU noise model with independent project " + "approval and Allan provenance status." ), ), - DeclareLaunchArgument( - "allow_unvalidated_imu_noise", - default_value="false", - description="Allow known placeholder noise only for a non-production smoke test.", - ), - DeclareLaunchArgument( - "enable_visualization", - default_value="false", - description="Enable cuVSLAM observation/landmark visualization publishers.", - ), OpaqueFunction(function=_build_runtime_actions), ]) diff --git a/integrations/isaac_ros_3_2_yopo/isaac_ros_yopo_bringup/package.xml b/integrations/isaac_ros_3_2_yopo/isaac_ros_yopo_bringup/package.xml index 00d2b1c..37c26c2 100644 --- a/integrations/isaac_ros_3_2_yopo/isaac_ros_yopo_bringup/package.xml +++ b/integrations/isaac_ros_3_2_yopo/isaac_ros_yopo_bringup/package.xml @@ -2,7 +2,7 @@ isaac_ros_yopo_bringup - 0.1.0 + 0.2.0 D435i rectified stereo and time-aligned PX4 IMU bringup for Isaac ROS Visual SLAM 3.2. u5-4 Apache-2.0 diff --git a/integrations/isaac_ros_3_2_yopo/isaac_ros_yopo_bringup/setup.py b/integrations/isaac_ros_3_2_yopo/isaac_ros_yopo_bringup/setup.py index efce199..5397655 100644 --- a/integrations/isaac_ros_3_2_yopo/isaac_ros_yopo_bringup/setup.py +++ b/integrations/isaac_ros_3_2_yopo/isaac_ros_yopo_bringup/setup.py @@ -9,7 +9,7 @@ setup( name=PACKAGE_NAME, - version="0.1.0", + version="0.2.0", packages=find_packages(exclude=("test",)), data_files=[ ( diff --git a/integrations/isaac_ros_3_2_yopo/isaac_ros_yopo_bringup/test/test_calibration.py b/integrations/isaac_ros_3_2_yopo/isaac_ros_yopo_bringup/test/test_calibration.py index b63016c..ca1086a 100644 --- a/integrations/isaac_ros_3_2_yopo/isaac_ros_yopo_bringup/test/test_calibration.py +++ b/integrations/isaac_ros_3_2_yopo/isaac_ros_yopo_bringup/test/test_calibration.py @@ -8,6 +8,7 @@ from isaac_ros_yopo_bringup.calibration import ( assert_runtime_calibration_allowed, + assert_runtime_imu_noise_allowed, load_calibration, load_imu_noise, ) @@ -101,19 +102,25 @@ def test_factory_camera_info_is_versioned_exactly(self): self.assertEqual(-16.174942016601562, camera.right_p[3]) self.assertEqual(camera.left_frame, camera.right_recorded_frame_bug) - def test_candidate_requires_an_explicit_runtime_override(self): - with self.assertRaisesRegex(ValueError, "runtime candidate"): - assert_runtime_calibration_allowed(self.calibration, False) - assert_runtime_calibration_allowed(self.calibration, True) + def test_bundled_calibration_is_project_approved(self): + self.assertEqual("approved", self.calibration.status) + assert_runtime_calibration_allowed(self.calibration) - def test_approved_status_is_allowed_without_override(self): + def test_candidate_is_not_runtime_approved(self): + calibration = self._load_modified( + status="runtime_candidate_pending_allan_and_independent_repeatability" + ) + with self.assertRaisesRegex(ValueError, "not project-approved"): + assert_runtime_calibration_allowed(calibration) + + def test_approved_status_is_allowed(self): calibration = self._load_modified(status="approved") - assert_runtime_calibration_allowed(calibration, False) + assert_runtime_calibration_allowed(calibration) def test_rejected_status_cannot_be_overridden(self): calibration = self._load_modified(status="rejected") - with self.assertRaisesRegex(ValueError, "cannot be overridden"): - assert_runtime_calibration_allowed(calibration, True) + with self.assertRaisesRegex(ValueError, "rejected"): + assert_runtime_calibration_allowed(calibration) def test_rejects_unknown_calibration_status(self): with self.assertRaisesRegex(ValueError, "unsupported calibration status"): @@ -196,18 +203,70 @@ def _temporary_yaml(data): class ImuNoiseCalibrationTest(unittest.TestCase): - def test_loads_bundled_values_only_as_unvalidated(self): + def test_loads_bundled_values_as_project_approved_non_allan(self): noise = load_imu_noise( UNVALIDATED_NOISE_PATH, "px4-highres-imu-105-ttyTHS2", 170.0, ) + self.assertEqual(2, noise.schema_version) + self.assertEqual("approved", noise.project_status) self.assertFalse(noise.validated) self.assertEqual("kalibr_input_assumption", noise.method) self.assertEqual(0.06, noise.gyroscope_noise_density) self.assertEqual(0.001, noise.gyroscope_random_walk) self.assertEqual(0.09, noise.accelerometer_noise_density) self.assertEqual(0.05, noise.accelerometer_random_walk) + assert_runtime_imu_noise_allowed(noise) + + def test_schema_one_unvalidated_noise_remains_a_legacy_candidate(self): + with open(UNVALIDATED_NOISE_PATH, "r", encoding="utf-8") as stream: + data = yaml.safe_load(stream) + data["schema_version"] = 1 + data.pop("project_status") + path = self._temporary_yaml(data) + try: + noise = load_imu_noise( + path, + "px4-highres-imu-105-ttyTHS2", + 170.0, + ) + self.assertEqual("candidate", noise.project_status) + with self.assertRaisesRegex(ValueError, "not project-approved"): + assert_runtime_imu_noise_allowed(noise) + finally: + os.unlink(path) + + def test_rejected_noise_model_is_not_runtime_allowed(self): + with open(UNVALIDATED_NOISE_PATH, "r", encoding="utf-8") as stream: + data = yaml.safe_load(stream) + data["project_status"] = "rejected" + path = self._temporary_yaml(data) + try: + noise = load_imu_noise( + path, + "px4-highres-imu-105-ttyTHS2", + 170.0, + ) + with self.assertRaisesRegex(ValueError, "rejected"): + assert_runtime_imu_noise_allowed(noise) + finally: + os.unlink(path) + + def test_rejects_unknown_noise_project_status(self): + with open(UNVALIDATED_NOISE_PATH, "r", encoding="utf-8") as stream: + data = yaml.safe_load(stream) + data["project_status"] = "looks_good" + path = self._temporary_yaml(data) + try: + with self.assertRaisesRegex(ValueError, "unsupported IMU noise"): + load_imu_noise( + path, + "px4-highres-imu-105-ttyTHS2", + 170.0, + ) + finally: + os.unlink(path) def test_rejects_noise_for_another_hardware_id(self): with self.assertRaises(ValueError): @@ -245,6 +304,41 @@ def test_loads_traceable_validated_allan_result(self): ) self.assertTrue(noise.validated) self.assertEqual("allan_deviation", noise.method) + assert_runtime_imu_noise_allowed(noise) + + def test_allan_validation_does_not_approve_a_candidate(self): + with tempfile.TemporaryDirectory() as directory: + path = self._validated_noise_yaml(directory) + with open(path, "r", encoding="utf-8") as stream: + data = yaml.safe_load(stream) + data["project_status"] = "candidate" + with open(path, "w", encoding="utf-8") as stream: + yaml.safe_dump(data, stream, sort_keys=False) + noise = load_imu_noise( + path, + "px4-highres-imu-105-ttyTHS2", + 170.0, + ) + self.assertTrue(noise.validated) + with self.assertRaisesRegex(ValueError, "not project-approved"): + assert_runtime_imu_noise_allowed(noise) + + def test_schema_one_validated_allan_preserves_legacy_runtime_approval(self): + with tempfile.TemporaryDirectory() as directory: + path = self._validated_noise_yaml(directory) + with open(path, "r", encoding="utf-8") as stream: + data = yaml.safe_load(stream) + data["schema_version"] = 1 + data.pop("project_status") + with open(path, "w", encoding="utf-8") as stream: + yaml.safe_dump(data, stream, sort_keys=False) + noise = load_imu_noise( + path, + "px4-highres-imu-105-ttyTHS2", + 170.0, + ) + self.assertEqual("approved", noise.project_status) + assert_runtime_imu_noise_allowed(noise) def test_rejects_validated_allan_hash_mismatch(self): with tempfile.TemporaryDirectory() as directory: @@ -290,6 +384,17 @@ def _validated_noise_yaml(directory, preserve_parameters=False): yaml.safe_dump(data, stream, sort_keys=False) return path + @staticmethod + def _temporary_yaml(data): + with tempfile.NamedTemporaryFile( + mode="w", + suffix=".yaml", + encoding="utf-8", + delete=False, + ) as stream: + yaml.safe_dump(data, stream, sort_keys=False) + return stream.name + if __name__ == "__main__": unittest.main() diff --git a/integrations/isaac_ros_3_2_yopo/isaac_ros_yopo_bringup/test/test_launch_source.py b/integrations/isaac_ros_3_2_yopo/isaac_ros_yopo_bringup/test/test_launch_source.py index 946bdca..206094e 100644 --- a/integrations/isaac_ros_3_2_yopo/isaac_ros_yopo_bringup/test/test_launch_source.py +++ b/integrations/isaac_ros_3_2_yopo/isaac_ros_yopo_bringup/test/test_launch_source.py @@ -43,7 +43,7 @@ def test_visual_slam_consumes_only_aligned_imu(self): self.source, ) - def test_noise_file_is_required_and_cli_values_are_not_accepted(self): + def test_noise_file_has_a_versioned_default_and_cli_values_are_not_accepted(self): required = {"imu_noise_file"} declarations = {} for node in ast.walk(self.tree): @@ -59,7 +59,8 @@ def test_noise_file_is_required_and_cli_values_are_not_accepted(self): } self.assertTrue(required.issubset(declarations)) for name in required: - self.assertNotIn("default_value", declarations[name]) + self.assertIn("default_value", declarations[name]) + self.assertIn('"px4_imu_noise_unvalidated.yaml"', self.source) for forbidden in ( 'DeclareLaunchArgument(\n "gyro_noise_density"', 'DeclareLaunchArgument(\n "gyro_random_walk"', @@ -81,10 +82,25 @@ def test_requires_successful_tracker_initialization(self): self.assertIn("initialization_timeout = TimerAction", self.source) self.assertIn("Patched cuVSLAM tracker was constructed", self.source) - def test_candidate_calibration_requires_explicit_override(self): - self.assertIn('"allow_candidate_calibration"', self.source) + def test_runtime_approval_is_versioned_without_cli_overrides(self): self.assertIn("assert_runtime_calibration_allowed", self.source) - self.assertIn("allow_candidate_calibration", self.source) + self.assertIn("assert_runtime_imu_noise_allowed", self.source) + self.assertNotIn('"allow_candidate_calibration"', self.source) + self.assertNotIn('"allow_unvalidated_imu_noise"', self.source) + + def test_production_launch_is_fixed_to_odometry_only(self): + for parameter in ( + "enable_ground_constraint_in_odometry", + "enable_ground_constraint_in_slam", + "enable_localization_n_mapping", + "enable_slam_visualization", + "enable_landmarks_view", + "enable_observations_view", + "publish_map_to_odom_tf", + ): + self.assertIn(f'"{parameter}": False', self.source) + self.assertNotIn('LaunchConfiguration("enable_visualization")', self.source) + self.assertNotIn('DeclareLaunchArgument(\n "enable_visualization"', self.source) def test_runtime_monitor_is_required(self): self.assertIn('executable="runtime_health_monitor"', self.source) From 25846068b8a71860444cf3befdafb599a14e51ee Mon Sep 17 00:00:00 2001 From: u5-4 Date: Thu, 23 Jul 2026 17:01:23 +0800 Subject: [PATCH 4/4] Enable native D435i depth for YOPO --- .../isaac_ros_3_2_yopo/isaac_ros_yopo_bringup/README.md | 5 ++++- .../launch/d435i_fcu_imu_cuvslam.launch.py | 2 +- .../isaac_ros_yopo_bringup/test/test_launch_source.py | 5 +++++ 3 files changed, 10 insertions(+), 2 deletions(-) diff --git a/integrations/isaac_ros_3_2_yopo/isaac_ros_yopo_bringup/README.md b/integrations/isaac_ros_3_2_yopo/isaac_ros_yopo_bringup/README.md index 2b18778..446d2c3 100644 --- a/integrations/isaac_ros_3_2_yopo/isaac_ros_yopo_bringup/README.md +++ b/integrations/isaac_ros_3_2_yopo/isaac_ros_yopo_bringup/README.md @@ -24,11 +24,14 @@ Isaac ROS 容器 D435i IR1/IR2 + CameraInfo + factory TF -> Isaac ROS cuVSLAM + D435i native Depth + -> ROS 2 DDS planner input + camera_infra1_optical_frame -> fcu_imu -> 已标定静态 TF ``` -统一 launch 会启动一份且仅一份 RealSense 驱动、时间对齐节点、标定静态 TF、官方 cuVSLAM component 和运行健康监视节点。D435i 的 gyro/accel 被明确关闭，cuVSLAM 的 IMU 订阅只映射到 `/fcu/imu/data_raw_aligned`。 +统一 launch 会启动一份且仅一份 RealSense 驱动、时间对齐节点、标定静态 TF、官方 cuVSLAM component 和运行健康监视节点。D435i 的 gyro/accel 被明确关闭，cuVSLAM 的 IMU 订阅只映射到 `/fcu/imu/data_raw_aligned`。原生 Depth 与红外双目共用同一 `640x360@90 Hz` profile，emitter 固定关闭；Depth 只作为规划输入，不改变 cuVSLAM 的 IR1/IR2 图像输入。 生产入口固定为 odometry-only：`enable_localization_n_mapping=false`，两类 ground constraint、三类内部可视化以及 cuVSLAM 的 `map -> odom` TF 发布均关闭。该入口不提供命令行开关重新启用这些功能；需要建图或调试可视化时应使用单独的调试 launch。 diff --git a/integrations/isaac_ros_3_2_yopo/isaac_ros_yopo_bringup/launch/d435i_fcu_imu_cuvslam.launch.py b/integrations/isaac_ros_3_2_yopo/isaac_ros_yopo_bringup/launch/d435i_fcu_imu_cuvslam.launch.py index 52afb27..e76252f 100644 --- a/integrations/isaac_ros_3_2_yopo/isaac_ros_yopo_bringup/launch/d435i_fcu_imu_cuvslam.launch.py +++ b/integrations/isaac_ros_3_2_yopo/isaac_ros_yopo_bringup/launch/d435i_fcu_imu_cuvslam.launch.py @@ -99,7 +99,7 @@ def _build_runtime_actions(context): "enable_infra1": True, "enable_infra2": True, "enable_color": False, - "enable_depth": False, + "enable_depth": True, "depth_module.emitter_enabled": 0, "depth_module.profile": "640x360x90", "enable_gyro": False, diff --git a/integrations/isaac_ros_3_2_yopo/isaac_ros_yopo_bringup/test/test_launch_source.py b/integrations/isaac_ros_3_2_yopo/isaac_ros_yopo_bringup/test/test_launch_source.py index 206094e..7e1b672 100644 --- a/integrations/isaac_ros_3_2_yopo/isaac_ros_yopo_bringup/test/test_launch_source.py +++ b/integrations/isaac_ros_3_2_yopo/isaac_ros_yopo_bringup/test/test_launch_source.py @@ -20,6 +20,11 @@ def test_disables_d435i_imu(self): self.assertIn('"unite_imu_method": 0', self.source) self.assertNotIn('"/camera/imu"', self.source) + def test_enables_native_depth_without_emitter(self): + self.assertIn('"enable_depth": True', self.source) + self.assertIn('"depth_module.emitter_enabled": 0', self.source) + self.assertIn('"depth_module.profile": "640x360x90"', self.source) + def test_uses_official_visual_slam_component(self): self.assertIn( 'plugin="nvidia::isaac_ros::visual_slam::VisualSlamNode"',