From 7a85e20fc6e1081732896301a8757bd56d8e64a4 Mon Sep 17 00:00:00 2001 From: Bartosz Burda Date: Sat, 4 Jul 2026 16:39:23 +0200 Subject: [PATCH 01/19] feat(ota_demo): OTA-over-SOVD demo - RB-Theron warehouse, bad lidar update, publish-and-apply a hotfix Dev-grade OTA demo on the ros2_medkit gateway's ota_update_plugin and the SOVD /updates resource. An RB-Theron AMR runs Nav2 in an AWS small-warehouse world. A regressing lidar update (broken_lidar_3_0_0) is auto-applied at boot; a few metres into a mission the scan sensor develops a stuck sector, Nav2 can no longer make progress, and navigate_to_pose aborts. Generic log/action-status bridges surface that as SOVD faults on bt-navigator and controller-server with a freeze-frame and an MCAP capture - the sensor node never reports itself. The operator diagnoses over SOVD: downloads the MCAP, runs a suite of health-check operations (lidar/localization/drivetrain/costmap) to confirm the lidar is the cause, then publishes the forward hotfix (fixed_lidar_3_0_1) as a hand-provided update descriptor via POST /updates and applies it (prepare + execute), and clears the latched faults. A custom nav_to_pose behaviour tree keeps the abort prompt. Foxglove panels + curl scripts drive the loop; smoke tests cover the plugin, the SOVD envelope, the publish-then-apply flow, and the single-publisher /scan remap. Runs on CycloneDDS (FastDDS segfaults amcl/controller_server on Jazzy). --- .github/workflows/ci.yml | 98 ++++ README.md | 36 +- demos/ota_nav2_sensor_fix/Dockerfile.gateway | 216 +++++++++ demos/ota_nav2_sensor_fix/README.md | 256 ++++++++++ .../THIRD_PARTY_NOTICES.md | 16 + demos/ota_nav2_sensor_fix/apply-fix.sh | 42 ++ .../ota_nav2_sensor_fix/artifacts/.gitignore | 2 + demos/ota_nav2_sensor_fix/check-demo.sh | 88 ++++ demos/ota_nav2_sensor_fix/clear-fault.sh | 24 + demos/ota_nav2_sensor_fix/docker-compose.yml | 44 ++ demos/ota_nav2_sensor_fix/entrypoint.sh | 39 ++ demos/ota_nav2_sensor_fix/gateway_config.yaml | 62 +++ demos/ota_nav2_sensor_fix/manifest.yaml | 281 +++++++++++ .../ota_update_plugin/CMakeLists.txt | 86 ++++ .../ota_update_plugin/ota_update_plugin.hpp | 98 ++++ .../ota_update_plugin/package.xml | 20 + .../ota_update_plugin/src/catalog_client.cpp | 171 +++++++ .../ota_update_plugin/src/catalog_client.hpp | 61 +++ .../src/operation_dispatcher.cpp | 48 ++ .../src/operation_dispatcher.hpp | 33 ++ .../src/ota_update_plugin.cpp | 427 +++++++++++++++++ .../ota_update_plugin/src/plugin_exports.cpp | 33 ++ .../ota_update_plugin/src/process_runner.cpp | 292 +++++++++++ .../ota_update_plugin/src/process_runner.hpp | 48 ++ .../test/test_catalog_client.cpp | 59 +++ .../test/test_operation_dispatcher.cpp | 60 +++ .../test/test_plugin_smoke.cpp | 237 +++++++++ .../ota_update_server/.gitignore | 3 + .../ota_update_server/Dockerfile | 99 ++++ .../ota_update_server/__init__.py | 3 + .../ota_update_server/main.py | 43 ++ .../ota_update_server/pyproject.toml | 22 + .../ota_update_server/pyrightconfig.json | 6 + .../ota_update_server/tests/__init__.py | 0 .../ota_update_server/tests/test_main.py | 55 +++ demos/ota_nav2_sensor_fix/publish-fix.sh | 37 ++ .../ros2_packages/broken_lidar/CMakeLists.txt | 22 + .../ros2_packages/broken_lidar/package.xml | 18 + .../broken_lidar/src/broken_lidar_node.cpp | 123 +++++ .../ros2_packages/fixed_lidar/CMakeLists.txt | 21 + .../ros2_packages/fixed_lidar/package.xml | 17 + .../fixed_lidar/src/fixed_lidar_node.cpp | 41 ++ .../ros2_packages/health_check/CMakeLists.txt | 30 ++ .../ros2_packages/health_check/package.xml | 20 + .../health_check/src/health_check_node.cpp | 374 +++++++++++++++ .../ota_nav2_sensor_fix_demo/CMakeLists.txt | 26 + .../config/controllers.yaml | 26 + .../config/nav2_params.yaml | 395 +++++++++++++++ .../config/navigate_to_pose_fast_abort.xml | 50 ++ .../config/ros_gz_bridge.yaml | 19 + .../config/warehouse_map.yaml | 7 + .../launch/demo.launch.py | 452 ++++++++++++++++++ .../maps/warehouse.pgm | Bin 0 -> 161855 bytes .../aws_small_warehouse/worlds/warehouse.sdf | 210 ++++++++ .../models/person_human/model.config | 13 + .../models/person_human/model.sdf | 74 +++ .../models/person_obstacle/model.config | 11 + .../models/person_obstacle/model.sdf | 34 ++ .../ota_nav2_sensor_fix_demo/package.xml | 51 ++ .../urdf/warehouse_rbtheron.urdf.xacro | 118 +++++ demos/ota_nav2_sensor_fix/ros2_ws/.gitignore | 4 + demos/ota_nav2_sensor_fix/run-demo.sh | 160 +++++++ demos/ota_nav2_sensor_fix/scripts/.gitignore | 4 + .../scripts/build_artifacts.sh | 88 ++++ demos/ota_nav2_sensor_fix/scripts/conftest.py | 7 + .../scripts/e2e_webui_smoke.mjs | 124 +++++ .../scripts/pack_artifact.py | 238 +++++++++ .../scripts/pyrightconfig.json | 5 + .../scripts/test_pack_artifact.py | 320 +++++++++++++ demos/ota_nav2_sensor_fix/send-goal.sh | 11 + demos/ota_nav2_sensor_fix/stop-demo.sh | 40 ++ .../ota_nav2_sensor_fix/trigger-bad-update.sh | 37 ++ demos/ota_nav2_sensor_fix/updates/README.md | 30 ++ .../updates/fixed_lidar_3_0_1.json | 15 + tests/smoke_lib.sh | 32 +- tests/smoke_test_demo_narrative.sh | 405 ++++++++++++++++ tests/smoke_test_ota.sh | 249 ++++++++++ 77 files changed, 7060 insertions(+), 6 deletions(-) create mode 100644 demos/ota_nav2_sensor_fix/Dockerfile.gateway create mode 100644 demos/ota_nav2_sensor_fix/README.md create mode 100644 demos/ota_nav2_sensor_fix/THIRD_PARTY_NOTICES.md create mode 100755 demos/ota_nav2_sensor_fix/apply-fix.sh create mode 100644 demos/ota_nav2_sensor_fix/artifacts/.gitignore create mode 100755 demos/ota_nav2_sensor_fix/check-demo.sh create mode 100755 demos/ota_nav2_sensor_fix/clear-fault.sh create mode 100644 demos/ota_nav2_sensor_fix/docker-compose.yml create mode 100755 demos/ota_nav2_sensor_fix/entrypoint.sh create mode 100644 demos/ota_nav2_sensor_fix/gateway_config.yaml create mode 100644 demos/ota_nav2_sensor_fix/manifest.yaml create mode 100644 demos/ota_nav2_sensor_fix/ota_update_plugin/CMakeLists.txt create mode 100644 demos/ota_nav2_sensor_fix/ota_update_plugin/include/ota_update_plugin/ota_update_plugin.hpp create mode 100644 demos/ota_nav2_sensor_fix/ota_update_plugin/package.xml create mode 100644 demos/ota_nav2_sensor_fix/ota_update_plugin/src/catalog_client.cpp create mode 100644 demos/ota_nav2_sensor_fix/ota_update_plugin/src/catalog_client.hpp create mode 100644 demos/ota_nav2_sensor_fix/ota_update_plugin/src/operation_dispatcher.cpp create mode 100644 demos/ota_nav2_sensor_fix/ota_update_plugin/src/operation_dispatcher.hpp create mode 100644 demos/ota_nav2_sensor_fix/ota_update_plugin/src/ota_update_plugin.cpp create mode 100644 demos/ota_nav2_sensor_fix/ota_update_plugin/src/plugin_exports.cpp create mode 100644 demos/ota_nav2_sensor_fix/ota_update_plugin/src/process_runner.cpp create mode 100644 demos/ota_nav2_sensor_fix/ota_update_plugin/src/process_runner.hpp create mode 100644 demos/ota_nav2_sensor_fix/ota_update_plugin/test/test_catalog_client.cpp create mode 100644 demos/ota_nav2_sensor_fix/ota_update_plugin/test/test_operation_dispatcher.cpp create mode 100644 demos/ota_nav2_sensor_fix/ota_update_plugin/test/test_plugin_smoke.cpp create mode 100644 demos/ota_nav2_sensor_fix/ota_update_server/.gitignore create mode 100644 demos/ota_nav2_sensor_fix/ota_update_server/Dockerfile create mode 100644 demos/ota_nav2_sensor_fix/ota_update_server/ota_update_server/__init__.py create mode 100644 demos/ota_nav2_sensor_fix/ota_update_server/ota_update_server/main.py create mode 100644 demos/ota_nav2_sensor_fix/ota_update_server/pyproject.toml create mode 100644 demos/ota_nav2_sensor_fix/ota_update_server/pyrightconfig.json create mode 100644 demos/ota_nav2_sensor_fix/ota_update_server/tests/__init__.py create mode 100644 demos/ota_nav2_sensor_fix/ota_update_server/tests/test_main.py create mode 100755 demos/ota_nav2_sensor_fix/publish-fix.sh create mode 100644 demos/ota_nav2_sensor_fix/ros2_packages/broken_lidar/CMakeLists.txt create mode 100644 demos/ota_nav2_sensor_fix/ros2_packages/broken_lidar/package.xml create mode 100644 demos/ota_nav2_sensor_fix/ros2_packages/broken_lidar/src/broken_lidar_node.cpp create mode 100644 demos/ota_nav2_sensor_fix/ros2_packages/fixed_lidar/CMakeLists.txt create mode 100644 demos/ota_nav2_sensor_fix/ros2_packages/fixed_lidar/package.xml create mode 100644 demos/ota_nav2_sensor_fix/ros2_packages/fixed_lidar/src/fixed_lidar_node.cpp create mode 100644 demos/ota_nav2_sensor_fix/ros2_packages/health_check/CMakeLists.txt create mode 100644 demos/ota_nav2_sensor_fix/ros2_packages/health_check/package.xml create mode 100644 demos/ota_nav2_sensor_fix/ros2_packages/health_check/src/health_check_node.cpp create mode 100644 demos/ota_nav2_sensor_fix/ros2_packages/ota_nav2_sensor_fix_demo/CMakeLists.txt create mode 100644 demos/ota_nav2_sensor_fix/ros2_packages/ota_nav2_sensor_fix_demo/config/controllers.yaml create mode 100644 demos/ota_nav2_sensor_fix/ros2_packages/ota_nav2_sensor_fix_demo/config/nav2_params.yaml create mode 100644 demos/ota_nav2_sensor_fix/ros2_packages/ota_nav2_sensor_fix_demo/config/navigate_to_pose_fast_abort.xml create mode 100644 demos/ota_nav2_sensor_fix/ros2_packages/ota_nav2_sensor_fix_demo/config/ros_gz_bridge.yaml create mode 100644 demos/ota_nav2_sensor_fix/ros2_packages/ota_nav2_sensor_fix_demo/config/warehouse_map.yaml create mode 100644 demos/ota_nav2_sensor_fix/ros2_packages/ota_nav2_sensor_fix_demo/launch/demo.launch.py create mode 100644 demos/ota_nav2_sensor_fix/ros2_packages/ota_nav2_sensor_fix_demo/maps/warehouse.pgm create mode 100644 demos/ota_nav2_sensor_fix/ros2_packages/ota_nav2_sensor_fix_demo/models/aws_small_warehouse/worlds/warehouse.sdf create mode 100644 demos/ota_nav2_sensor_fix/ros2_packages/ota_nav2_sensor_fix_demo/models/person_human/model.config create mode 100644 demos/ota_nav2_sensor_fix/ros2_packages/ota_nav2_sensor_fix_demo/models/person_human/model.sdf create mode 100644 demos/ota_nav2_sensor_fix/ros2_packages/ota_nav2_sensor_fix_demo/models/person_obstacle/model.config create mode 100644 demos/ota_nav2_sensor_fix/ros2_packages/ota_nav2_sensor_fix_demo/models/person_obstacle/model.sdf create mode 100644 demos/ota_nav2_sensor_fix/ros2_packages/ota_nav2_sensor_fix_demo/package.xml create mode 100644 demos/ota_nav2_sensor_fix/ros2_packages/ota_nav2_sensor_fix_demo/urdf/warehouse_rbtheron.urdf.xacro create mode 100644 demos/ota_nav2_sensor_fix/ros2_ws/.gitignore create mode 100755 demos/ota_nav2_sensor_fix/run-demo.sh create mode 100644 demos/ota_nav2_sensor_fix/scripts/.gitignore create mode 100755 demos/ota_nav2_sensor_fix/scripts/build_artifacts.sh create mode 100644 demos/ota_nav2_sensor_fix/scripts/conftest.py create mode 100644 demos/ota_nav2_sensor_fix/scripts/e2e_webui_smoke.mjs create mode 100644 demos/ota_nav2_sensor_fix/scripts/pack_artifact.py create mode 100644 demos/ota_nav2_sensor_fix/scripts/pyrightconfig.json create mode 100644 demos/ota_nav2_sensor_fix/scripts/test_pack_artifact.py create mode 100755 demos/ota_nav2_sensor_fix/send-goal.sh create mode 100755 demos/ota_nav2_sensor_fix/stop-demo.sh create mode 100755 demos/ota_nav2_sensor_fix/trigger-bad-update.sh create mode 100644 demos/ota_nav2_sensor_fix/updates/README.md create mode 100644 demos/ota_nav2_sensor_fix/updates/fixed_lidar_3_0_1.json create mode 100755 tests/smoke_test_demo_narrative.sh create mode 100755 tests/smoke_test_ota.sh diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 588666e..4146321 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -163,3 +163,101 @@ jobs: if: always() working-directory: demos/multi_ecu_aggregation run: docker compose --profile ci down + + build-and-test-ota: + needs: lint + runs-on: ubuntu-24.04 + steps: + - name: Show triggering source + if: github.event_name == 'repository_dispatch' + run: | + SHA="${{ github.event.client_payload.sha }}" + RUN_URL="${{ github.event.client_payload.run_url }}" + echo "## Triggered by ros2_medkit" >> "$GITHUB_STEP_SUMMARY" + echo "- Commit: \`${SHA:-unknown}\`" >> "$GITHUB_STEP_SUMMARY" + if [ -n "$RUN_URL" ]; then + echo "- Run: [View triggering run]($RUN_URL)" >> "$GITHUB_STEP_SUMMARY" + else + echo "- Run: (URL not provided)" >> "$GITHUB_STEP_SUMMARY" + fi + + - name: Checkout repository + uses: actions/checkout@v4 + + - name: Build and start OTA demo + working-directory: demos/ota_nav2_sensor_fix + run: docker compose up -d --build + + - name: Run smoke tests + run: ./tests/smoke_test_ota.sh + + - name: Show gateway logs on failure + if: failure() + working-directory: demos/ota_nav2_sensor_fix + run: docker compose logs gateway --tail=200 + + - name: Show update server logs on failure + if: failure() + working-directory: demos/ota_nav2_sensor_fix + run: docker compose logs ota_update_server --tail=200 + + - name: Teardown + if: always() + working-directory: demos/ota_nav2_sensor_fix + run: docker compose down + + # Separate job from build-and-test-ota: this one drives the full + # latch/publish/apply/clear narrative through the operator scripts. The + # entrypoint auto-applies broken_lidar_3_0_0 at boot; send-goal.sh then + # drives the robot into the phantom sector so Nav2 cannot make progress + # and navigate_to_pose aborts, which the log + action-status bridges + # surface as ACTION_NAVIGATE_TO_POSE_ABORTED on bt-navigator (latched, + # with a freeze-frame + MCAP capture). publish-fix.sh registers the + # forward hotfix fixed_lidar_3_0_1 (not in the boot catalog), then + # apply-fix.sh swaps scan_sensor_node to it, but the fault stays + # latched (no self-heal) until clear-fault.sh explicitly clears it, and + # only then does a fresh send-goal.sh resume clean. Catches regressions + # in that loop (phantom not stalling Nav2, the bridges not promoting the + # failure, fault_manager latching/capture, the fault clearing itself on + # apply). Slower than the API-only smoke job because it has + # to wait for nav2 lifecycle to settle and for /cmd_vel to actually + # fire, so it's split out and can fail in isolation without blocking + # the quick OTA-endpoint check. + ota-demo-narrative: + needs: lint + runs-on: ubuntu-24.04 + steps: + - name: Show triggering source + if: github.event_name == 'repository_dispatch' + run: | + SHA="${{ github.event.client_payload.sha }}" + RUN_URL="${{ github.event.client_payload.run_url }}" + echo "## Triggered by ros2_medkit" >> "$GITHUB_STEP_SUMMARY" + echo "- Commit: \`${SHA:-unknown}\`" >> "$GITHUB_STEP_SUMMARY" + if [ -n "$RUN_URL" ]; then + echo "- Run: [View triggering run]($RUN_URL)" >> "$GITHUB_STEP_SUMMARY" + fi + + - name: Checkout repository + uses: actions/checkout@v4 + + - name: Build and start OTA demo + working-directory: demos/ota_nav2_sensor_fix + # docker compose up --build runs the multi-stage build for + # ota_update_server which produces the catalog + tarballs + # internally - no separate "build artifacts on host" step + # needed (and the host wouldn't have ros2_medkit_msgs anyway). + run: docker compose up -d --build + + - name: Run demo narrative smoke + run: ./tests/smoke_test_demo_narrative.sh + + - name: Show gateway logs on failure + if: failure() + working-directory: demos/ota_nav2_sensor_fix + run: docker compose logs gateway --tail=300 + + - name: Teardown + if: always() + working-directory: demos/ota_nav2_sensor_fix + run: docker compose down diff --git a/README.md b/README.md index c21c948..72c0ea6 100644 --- a/README.md +++ b/README.md @@ -49,6 +49,7 @@ All demos support: | [TurtleBot3 Integration](demos/turtlebot3_integration/) | Full ros2_medkit integration with TurtleBot3 and Nav2 | SOVD-compliant API, manifest-based discovery, fault management | ✅ Ready | | [MoveIt Pick-and-Place](demos/moveit_pick_place/) | Panda 7-DOF arm with MoveIt 2 manipulation and ros2_medkit | Planning fault detection, controller monitoring, joint limits | ✅ Ready | | [Multi-ECU Aggregation](demos/multi_ecu_aggregation/) | Multi-ECU peer aggregation with 3 ECUs (perception, planning, actuation), mDNS discovery, cross-ECU functions | Peer aggregation, mDNS discovery, cross-ECU functions | ✅ Ready | +| [OTA over SOVD - nav2 sensor fix](demos/ota_nav2_sensor_fix/) | Dev-grade OTA plugin showing the SOVD `/updates` lifecycle - a bad lidar update breaks Nav2, publish + apply a forward hotfix over SOVD | SOVD-spec register + update, native binary swap, fork+exec process management, Foxglove panel + curl scripts | ✅ Ready | ### Quick Start @@ -150,6 +151,37 @@ cd demos/multi_ecu_aggregation - Unified SOVD-compliant REST API spanning all ECUs - Web UI for browsing aggregated entity hierarchy +#### OTA over SOVD Demo (Dev-grade Update / Publish-a-Hotfix) + +End-to-end demo of the SOVD `/updates` resource: a regressing sensor update +(`broken_lidar_3_0_0`) is auto-applied at boot and breaks perception. Nav2 +cannot make progress and `navigate_to_pose` aborts; two generic bridges surface +that failure as SOVD faults on `bt-navigator` and `controller-server` (the +sensor node never reports itself). The operator downloads the captured MCAP, +sees the phantom, publishes and applies the forward hotfix +(`fixed_lidar_3_0_1`, not a rollback), and clears the latched fault - all +without SSH, all spec-compliant. + +```bash +cd demos/ota_nav2_sensor_fix +./run-demo.sh # build artifacts + bring up gateway/plugin/update server +./check-demo.sh # show registered updates + per-id status + live process state +./send-goal.sh # drive into the phantom sector; Nav2 stalls, navigate_to_pose aborts +./publish-fix.sh # register fixed_lidar_3_0_1 (SOVD POST /updates) - not in the boot catalog +./apply-fix.sh # broken_lidar_3_0_0 -> fixed_lidar_3_0_1 (the headline: apply the published hotfix) +./clear-fault.sh # operator clear of the latched bt-navigator/controller-server faults +./stop-demo.sh +``` + +**Features:** + +- Dev-grade `ota_update_plugin` C++ gateway plugin (UpdateProvider + GatewayPlugin) +- SOVD ISO 17978-3 compliant `/updates` resource: kind derived from + `updated_components` / `added_components` / `removed_components` metadata +- Native binary swap + `fork+exec` process management (no containers, no signing) +- Foxglove Studio panel mirrors the same SOVD client patterns as the web UI +- Pairs with the [`ros2_medkit_foxglove_extension`](https://github.com/selfpatch/ros2_medkit_foxglove_extension) Updates panel + ## Getting Started ### Prerequisites @@ -209,9 +241,11 @@ Each demo has automated smoke tests that verify the gateway starts and the REST ./tests/smoke_test.sh # Sensor diagnostics (full API coverage + fault injection + beacons) ./tests/smoke_test_turtlebot3.sh # TurtleBot3 (discovery, data, operations, scripts, triggers, logs) ./tests/smoke_test_moveit.sh # MoveIt pick-and-place (discovery, data, operations, scripts, triggers, logs) +./tests/smoke_test_multi_ecu.sh # Multi-ECU aggregation (per-ECU discovery + aggregated view) +./tests/smoke_test_ota.sh # OTA over SOVD (catalog, /updates spec shape, prepare/execute, process swap) ``` -CI runs all 4 demos in parallel - each job builds the Docker image, starts the container, and runs the smoke tests against it. See [CI workflow](.github/workflows/ci.yml). +CI runs all demos in parallel - each job builds the Docker image, starts the container, and runs the smoke tests against it. See [CI workflow](.github/workflows/ci.yml). ## Related Projects diff --git a/demos/ota_nav2_sensor_fix/Dockerfile.gateway b/demos/ota_nav2_sensor_fix/Dockerfile.gateway new file mode 100644 index 0000000..ad0012f --- /dev/null +++ b/demos/ota_nav2_sensor_fix/Dockerfile.gateway @@ -0,0 +1,216 @@ +# Copyright 2026 bburda +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Builds the ros2_medkit gateway, the ota_update_plugin, and the demo ROS 2 +# packages (including the RB-Theron AMR driving in the AWS warehouse under +# Nav2) into a single ROS 2 Jazzy image. Plugin loads at gateway startup via +# /etc/ros2_medkit/gateway_config.yaml. scan_sensor_node (fixed_lidar at +# boot, broken_lidar once the entrypoint auto-applies the OTA regression) +# owns /scan; the publish-then-apply hotfix flow swaps the process back at +# runtime via the plugin. +# +# The gateway clone below is the FULL ros2_medkit workspace with no +# --packages-select/--packages-up-to filter, so ros2_medkit_log_bridge and +# ros2_medkit_action_status_bridge - the generic bridges demo.launch.py uses +# to turn Nav2's own failure into SOVD faults - build and install +# automatically alongside the gateway; no separate build step is needed for +# them here. + +FROM ros:jazzy AS builder + +ARG GATEWAY_REPO=https://github.com/selfpatch/ros2_medkit.git +ARG GATEWAY_REF=main + +RUN apt-get update && apt-get install -y --no-install-recommends \ + git \ + python3-colcon-common-extensions \ + python3-rosdep \ + build-essential \ + cmake \ + curl \ + ca-certificates \ + && rm -rf /var/lib/apt/lists/* + +RUN rosdep init || true +RUN rosdep update --rosdistro=jazzy + +WORKDIR /ws/src +RUN git clone --depth=1 --branch ${GATEWAY_REF} ${GATEWAY_REPO} ros2_medkit + +# Fetch the Robotnik RB-Theron description + sensors at build time (pinned commits), +# instead of vendoring their mesh-heavy trees in the repo (see THIRD_PARTY_NOTICES.md). +# Both are BSD-3-Clause; each clone's own LICENSE travels with it. The unused base +# meshes (128 MB for 9 other robots) are pruned so only the RB-Theron geometry the +# demo renders ships in the image. The RB-Theron xacro pulls only the SICK +# picoScan120, VectorNav IMU and RealSense D435 sensor macros, so robotnik_sensors is +# kept whole (its mesh bulk IS those three sensors). +ARG ROBOTNIK_DESC_REF=751059edd6af3a9c083018cfaee59e4496d46580 +ARG ROBOTNIK_SENS_REF=e5186c343910b86a924201edb256f79eb0f73295 +RUN git clone --filter=blob:none https://github.com/RobotnikAutomation/robotnik_description.git && \ + git -C robotnik_description checkout --quiet "${ROBOTNIK_DESC_REF}" && \ + rm -rf robotnik_description/.git && \ + find robotnik_description/meshes/bases -mindepth 1 -maxdepth 1 -type d ! -name rbtheron -exec rm -rf {} + && \ + git clone --filter=blob:none https://github.com/RobotnikAutomation/robotnik_sensors.git && \ + git -C robotnik_sensors checkout --quiet "${ROBOTNIK_SENS_REF}" && \ + rm -rf robotnik_sensors/.git + +# Copy demo packages (broken_lidar, fixed_lidar, ota_nav2_sensor_fix_demo) +# and the OTA plugin from the build context. +# ota_nav2_sensor_fix_demo/models/aws_small_warehouse/worlds/warehouse.sdf +# is ours (a gz-port that references the AWS models below via model:// URIs); the AWS +# meshes themselves are fetched next, not vendored. +COPY ros2_packages /tmp/ros2_packages +RUN cp -r /tmp/ros2_packages/. /ws/src/ && rm -rf /tmp/ros2_packages +COPY ota_update_plugin /ws/src/ota_update_plugin + +# Fetch the AWS RoboMaker small_warehouse models at build time (pinned commit, +# MIT-0; see THIRD_PARTY_NOTICES.md) into the demo package's model tree, instead of +# vendoring their COLLADA meshes in the repo. The meshes are upstream-verbatim; our gz +# port is worlds/warehouse.sdf (committed) - upstream ships file://models/ paths, gz +# resolves model:// against GZ_SIM_RESOURCE_PATH, so we rewrite the scheme. A handful +# of upstream models also carry inertia tensors that violate the triangle inequality +# (ixx+iyy < izz etc.), which libsdformat 14.x rejects fatally ("A link named link has +# invalid inertia." -> "Failed to load a world."). We rewrite only the offending +# tensors to a valid uniform diagonal so the world loads; these are static fixtures +# (floor, roof, shelves), so the exact inertia is immaterial to the demo. +ARG AWS_WAREHOUSE_REF=ee0af733315e78432408c3cd98d378ecee5f767c +# The inertia-fix script: rewrite only the inertia tensors that fail libsdformat 14.x +# validation (non-positive principal moment, or triangle-inequality violation) to a +# valid uniform diagonal. Written to a file so the multi-command RUN stays a clean +# &&-chain (no heredoc mixed with line continuations). +RUN printf '%s\n' \ + 'import re, sys' \ + 'VALID = "100000100001000"' \ + 'def p(blk, k):' \ + ' m = re.search(r"<%s>([-\d.eE]+)" % (k, k), blk)' \ + ' return float(m.group(1)) if m else None' \ + 'def bad(blk):' \ + ' ixx, iyy, izz = (p(blk, k) for k in ("ixx", "iyy", "izz"))' \ + ' if None in (ixx, iyy, izz):' \ + ' return False' \ + ' if min(ixx, iyy, izz) <= 0:' \ + ' return True' \ + ' return ixx + iyy < izz or iyy + izz < ixx or ixx + izz < iyy' \ + 'for f in sys.argv[1:]:' \ + ' t = open(f).read()' \ + ' new = re.sub(r".*?", lambda m: VALID if bad(m.group(0)) else m.group(0), t, flags=re.S)' \ + ' if new != t:' \ + ' open(f, "w").write(new)' \ + ' print("fixed inertia in", f)' \ + > /tmp/fix_inertia.py +RUN cd /ws/src && \ + git clone --filter=blob:none https://github.com/aws-robotics/aws-robomaker-small-warehouse-world.git aws_up && \ + git -C aws_up checkout --quiet "${AWS_WAREHOUSE_REF}" && \ + cp -r aws_up/models ota_nav2_sensor_fix_demo/models/aws_small_warehouse/models && \ + sed -i 's#file://models/#model://#g' ota_nav2_sensor_fix_demo/models/aws_small_warehouse/models/*/model.sdf && \ + python3 /tmp/fix_inertia.py ota_nav2_sensor_fix_demo/models/aws_small_warehouse/models/*/model.sdf && \ + rm -rf aws_up + +WORKDIR /ws +# rosdep needs the apt cache populated to install gateway dependencies +# (nlohmann-json3-dev, libcpp-httplib-dev, etc.), plus xacro / ros-gz / robot_state_publisher +# / gz_ros2_control / ros2_control / ros2_controllers / diff_drive_controller / +# joint_state_broadcaster for the RB-Theron chain - resolved automatically from the +# ota_nav2_sensor_fix_demo + robotnik_description + robotnik_sensors package.xml +# exec_depends above, not a hardcoded apt list. robotnik_description also declares +# exec_depend on ur_description / ur_simulation_gz (used by OTHER Robotnik robots we +# don't build); those two rosdep keys don't resolve on Jazzy, so skip them - same fix +# the upstream warehouse recipe applies. --dependency-types + -DBUILD_TESTING=OFF skip +# robotnik's test-only deps (liburdfdom-tools, launch_testing_*) we don't need either. +RUN apt-get update +RUN . /opt/ros/jazzy/setup.sh && \ + rosdep install --from-paths src --ignore-src -r -y --rosdistro=jazzy \ + --dependency-types exec --dependency-types build --dependency-types buildtool \ + --skip-keys ur_description \ + --skip-keys ur_simulation_gz && \ + colcon build \ + --cmake-args -DCMAKE_BUILD_TYPE=Release -DBUILD_TESTING=OFF && \ + rm -rf /var/lib/apt/lists/* + + +FROM ros:jazzy + +# Runtime dependencies. Beyond the gateway/plugin bare minimum we also pull in +# Nav2, gz-sim, and the ros2_control / gz_ros2_control chain so the container +# can self-host the visual demo (RB-Theron AMR + headless Gazebo + Nav2) - no +# external sim required, the OTA story becomes "Foxglove sees a stuck robot, +# run an update, robot unsticks". The RB-Theron description + sensors are +# cloned from Robotnik in the builder stage; the AWS warehouse world is baked +# there too, so no turtlebot3-* runtime packages are needed here. +RUN apt-get update && apt-get install -y --no-install-recommends \ + ros-jazzy-rclcpp \ + ros-jazzy-rclcpp-lifecycle \ + ros-jazzy-sensor-msgs \ + ros-jazzy-visualization-msgs \ + ros-jazzy-launch-ros \ + ros-jazzy-test-msgs \ + ros-jazzy-foxglove-bridge \ + ros-jazzy-nav2-bringup \ + ros-jazzy-nav2-bt-navigator \ + ros-jazzy-nav2-controller \ + ros-jazzy-nav2-planner \ + ros-jazzy-nav2-behaviors \ + ros-jazzy-nav2-costmap-2d \ + ros-jazzy-nav2-lifecycle-manager \ + ros-jazzy-nav2-map-server \ + ros-jazzy-nav2-amcl \ + ros-jazzy-ros-gz-sim \ + ros-jazzy-ros-gz-bridge \ + ros-jazzy-rmw-cyclonedds-cpp \ + ros-jazzy-rosbag2-cpp \ + ros-jazzy-rosbag2-storage-default-plugins \ + ros-jazzy-rosbag2-storage-mcap \ + libcpp-httplib-dev \ + libsystemd-dev \ + nlohmann-json3-dev \ + curl \ + procps \ + ros-jazzy-xacro \ + ros-jazzy-robot-state-publisher \ + ros-jazzy-gz-ros2-control \ + ros-jazzy-ros2-control \ + ros-jazzy-ros2-controllers \ + ros-jazzy-controller-manager \ + ros-jazzy-joint-state-broadcaster \ + ros-jazzy-diff-drive-controller \ + && rm -rf /var/lib/apt/lists/* + +COPY --from=builder /ws/install /ws/install +COPY gateway_config.yaml /etc/ros2_medkit/gateway_config.yaml +COPY manifest.yaml /etc/ros2_medkit/manifest.yaml + +# Pre-create the fragments directory so the gateway's manifest manager +# scans an existing (empty) dir at boot rather than logging "missing +# fragments_dir" warnings. Plugin writes / removes yaml files here at +# OTA install / uninstall time. +RUN mkdir -p /etc/ros2_medkit/manifest_fragments +# Rosbag capture storage_path for ros2_medkit_fault_manager's RosbagCapture. +RUN mkdir -p /var/lib/ros2_medkit/rosbags +COPY entrypoint.sh /usr/local/bin/entrypoint.sh +RUN chmod +x /usr/local/bin/entrypoint.sh + +# RMW: jazzy's apt-shipped nav2_msgs fastrtps typesupport pulls +# eprosima::fastcdr::Cdr::serialize(uint32_t), which the bundled +# ros-jazzy-fastcdr 2.2.5 does NOT export - amcl/controller_server segfault +# at startup. Switch to cyclonedds, which doesn't use the broken typesupport. +# +# GZ_SIM_RESOURCE_PATH (RB-Theron + AWS warehouse - actively used by +# demo.launch.py's spawn + world load): the AWS models baked into the demo +# package's own share dir at Docker build time, plus the robotnik_description / +# robotnik_sensors share dirs (colcon-built into /ws/install by the builder +# stage) so gz can resolve both model:// (AWS warehouse fixtures) and +# package:// (RB-Theron meshes) URIs. demo.launch.py's own launch-time +# AppendEnvironmentVariable adds the same warehouse models dir again +# defensively for source-mounted runs. +ENV ROS_DOMAIN_ID=42 \ + GZ_SIM_RESOURCE_PATH=/ws/install/ota_nav2_sensor_fix_demo/share/ota_nav2_sensor_fix_demo/models/aws_small_warehouse/models:/ws/install/robotnik_description/share:/ws/install/robotnik_sensors/share \ + HEADLESS=true \ + RMW_IMPLEMENTATION=rmw_cyclonedds_cpp + +EXPOSE 8080 8765 +ENTRYPOINT ["/usr/local/bin/entrypoint.sh"] diff --git a/demos/ota_nav2_sensor_fix/README.md b/demos/ota_nav2_sensor_fix/README.md new file mode 100644 index 0000000..c6065e4 --- /dev/null +++ b/demos/ota_nav2_sensor_fix/README.md @@ -0,0 +1,256 @@ +# OTA over SOVD - nav2 sensor fix demo + +End-to-end demo: a `ros2_medkit` gateway with a dev-grade OTA plugin that +demonstrates a real update / publish-a-hotfix loop on a ROS 2 node without +SSH-ing into the robot. + +## What this shows + +The headline scene is a diagnostic loop, not just a button-press update: + +1. The robot boots on the known-good lidar (`fixed_lidar` running as + `scan_sensor_node`) - clean `/scan`, no fault. +2. At container startup a routine-looking software update + (`broken_lidar_3_0_0`) is **auto-applied** - the entrypoint swaps + `scan_sensor_node` over to `broken_lidar` before the mission even + starts. This is the root cause the operator will have to find later. +3. An operator sends a nav goal with `./send-goal.sh`. The robot drives, + and the operator's Foxglove view drops (narrative: the operator is away + and the viewer link is lost for a few minutes). Only the operator's view + goes dark - the robot, the on-robot gateway, and the fault manager keep + running and capturing the whole time. +4. While driving, `broken_lidar` overlays a blocking phantom sector onto + the real `/scan_sim` data straight ahead - real obstacles elsewhere in + the scan stay visible. Nav2 genuinely cannot make progress: + `controller_server` logs "Failed to make progress" and + `navigate_to_pose` aborts. Two generic `ros2_medkit` bridges (not a + custom fault in the scan node) turn Nav2's own failure into SOVD + faults: `ros2_medkit_action_status_bridge` reports + `ACTION_NAVIGATE_TO_POSE_ABORTED` on `bt-navigator` (the headline + fault) and `ros2_medkit_log_bridge` reports a + `LOG_CONTROLLER_SERVER_*` fault on `controller-server` (supporting). + `ros2_medkit_fault_manager` confirms both immediately, captures a + freeze-frame + MCAP recording under the `bt-navigator` fault's + `environment_data.snapshots`, and **latches** it - no self-healing. +5. The operator reconnects: the robot is stopped, and the fault is red + (latched). +6. The operator downloads the MCAP capture + (`GET /apps/bt-navigator/bulk-data/rosbags/ACTION_NAVIGATE_TO_POSE_ABORTED`) + and opens it in Foxglove - the phantom blocking the path is visible in + the replayed `/scan`. The Faults Dashboard panel's freeze-frame shows + the same state at confirmation time. + To confirm the root cause rather than guess, the operator runs the + `health-check` app's operations (`lidar_health_check`, + `localization_health_check`, `drivetrain_health_check`, + `costmap_health_check`) from the Operations tab: localization and + drivetrain come back healthy, but `lidar_health_check` reports a stuck + sector and `costmap_health_check` flags an obstacle ahead that is not in + the map - it is the lidar, not something downstream. +7. `GET /api/v1/updates` shows only `broken_lidar_3_0_0` - the suspect + recent change, and the fix the operator needs is not registered yet. +8. The operator publishes the hotfix with `./publish-fix.sh` (SOVD + `POST /api/v1/updates`, registering `fixed_lidar_3_0_1` - a **forward** + fix, version 3.0.1 > the bad 3.0.0, not a rollback to a previous build). + `GET /api/v1/updates` now shows both `broken_lidar_3_0_0` and + `fixed_lidar_3_0_1`. +9. The operator applies the fix with `./apply-fix.sh` (prepare + execute + `fixed_lidar_3_0_1`) - `scan_sensor_node` swaps from `broken_lidar` to + `fixed_lidar` and `/scan` is clean again. The fault stays **latched** - + applying the fix does not clear the DTC. +10. The operator clears the faults with `./clear-fault.sh` (an explicit + `DELETE /apps/bt-navigator/faults/ACTION_NAVIGATE_TO_POSE_ABORTED` plus + a clear-all `DELETE /apps/controller-server/faults` for the + content-hashed `LOG_*` code) - the Faults Dashboard goes green. +11. The operator resumes the mission with `./send-goal.sh` - the robot + reaches the goal. + +The update is SOVD ISO 17978-3 compliant - the kind is derived from +`updated_components` in the update package metadata. + +## Quickstart + +```bash +# Build artifacts + start gateway, plugin, demo nodes, update server. +./run-demo.sh +``` + +The first run pulls `ros:jazzy`, installs the Nav2 + gz-sim runtime, clones +the Robotnik RB-Theron + AWS small-warehouse assets (~3 GB) and builds the +gateway from source - takes ~15-20 minutes on a fresh cache. Subsequent runs +reuse the layer cache. + +In another terminal, drive the demo: + +```bash +./check-demo.sh # at a glance: scan node, applied updates, faults +./send-goal.sh # publish a nav goal (mission start / resume) +./publish-fix.sh # register fixed_lidar_3_0_1 (SOVD POST /updates) - not in the boot catalog +./apply-fix.sh # broken_lidar -> fixed_lidar_3_0_1 (prepare + execute the published fix) +./clear-fault.sh # operator clear of the latched bt-navigator/controller-server faults +./trigger-bad-update.sh # re-arm broken_lidar_3_0_0 (normally auto-applied at boot) +./stop-demo.sh # tear down +``` + +`publish-fix.sh` issues a SOVD `POST /updates` to register the held-back +`fixed_lidar_3_0_1` hotfix. `apply-fix.sh` and `trigger-bad-update.sh` issue +SOVD `PUT /updates/{id}/prepare` then `/execute` and print the resulting +status plus the live process list; `apply-fix.sh` guards on the fix being +registered first and tells you to run `./publish-fix.sh` if it isn't. +`clear-fault.sh` issues a plain SOVD +`DELETE /apps/bt-navigator/faults/ACTION_NAVIGATE_TO_POSE_ABORTED` plus a +clear-all `DELETE /apps/controller-server/faults` (the `LOG_*` code there +is content-hashed, so it is cleared by entity rather than by exact code). +`send-goal.sh` publishes `/goal_pose` once via `ros2 topic pub` inside the +gateway container. + +Port overrides (set as env vars before `./run-demo.sh`): + +- `OTA_GATEWAY_PORT` - gateway HTTP API (default `8080`) +- `OTA_FOXGLOVE_BRIDGE_PORT` - foxglove_bridge WebSocket (default `8765`) + +Tear down: `docker compose down`. + +## Diagnosing the incident over SOVD + +Everything in the loop above is also doable with plain `curl` - the +Foxglove panels (next section) are a convenience layer on top of the same +SOVD REST calls. + +```bash +API=http://localhost:8080/api/v1 + +# 1. Which update is applied to scan_sensor_node right now - the suspect +# recent change. Only the bad update is in the boot catalog; the fix +# is not registered yet. +curl -s "${API}/updates" | jq -r '.items[]' +curl -s "${API}/updates/broken_lidar_3_0_0/status" | jq . + +# 2. Is ACTION_NAVIGATE_TO_POSE_ABORTED confirmed on bt-navigator (the +# headline fault)? Also check controller-server for the supporting +# LOG_* fault. (default filter = PREFAILED + CONFIRMED) +curl -s "${API}/apps/bt-navigator/faults" | jq . +curl -s "${API}/apps/controller-server/faults" | jq . + +# 3. Fault detail - freeze-frame + the MCAP link live under +# environment_data.snapshots. +curl -s "${API}/apps/bt-navigator/faults/ACTION_NAVIGATE_TO_POSE_ABORTED" | jq . + +# 4. Download the MCAP recording and open it in Foxglove. +curl -O -J "${API}/apps/bt-navigator/bulk-data/rosbags/ACTION_NAVIGATE_TO_POSE_ABORTED" + +# 4b. Confirm the root cause with the health-check operations. localization + +# drivetrain come back healthy; lidar_health_check reports a stuck sector. +for op in lidar_health_check localization_health_check drivetrain_health_check costmap_health_check; do + echo "== ${op} ==" + curl -s -X POST -H 'Content-Type: application/json' -d '{}' \ + "${API}/apps/health-check/operations/${op}/executions" | jq '.parameters // .' +done + +# 5. Publish the forward hotfix (or use ./publish-fix.sh). It is not in the +# boot catalog - you register it by POSTing its descriptor, a JSON you +# provide by hand (see updates/README.md for the fields), via SOVD +# POST /updates. +curl -fsS -X POST -H 'Content-Type: application/json' \ + -d @updates/fixed_lidar_3_0_1.json "${API}/updates" +curl -s "${API}/updates" | jq -r '.items[]' + +# 6. Apply the published fix (or use ./apply-fix.sh). +curl -fsS -X PUT -H 'Content-Type: application/json' -d '{}' \ + "${API}/updates/fixed_lidar_3_0_1/prepare" +curl -fsS -X PUT -H 'Content-Type: application/json' -d '{}' \ + "${API}/updates/fixed_lidar_3_0_1/execute" + +# 7. Both faults are still latched after applying the fix - clear them +# explicitly (or use ./clear-fault.sh). The controller-server LOG_* +# code is content-hashed, so clear it with a clear-all on the entity +# instead of a hardcoded code. +curl -X DELETE "${API}/apps/bt-navigator/faults/ACTION_NAVIGATE_TO_POSE_ABORTED" +curl -X DELETE "${API}/apps/controller-server/faults" + +# 8. Resume the mission (or use ./send-goal.sh). +``` + +## Foxglove Studio visualization + +The gateway container bakes in a Robotnik RB-Theron AMR + Nav2 stack running +on top of headless Gazebo in the AWS small-warehouse world. `foxglove_bridge` +runs on port `8765` and exposes the full topic set: `/tf`, `/tf_static`, +`/scan`, `/odom`, `/map`, `/cmd_vel`, `/global_costmap/costmap`, +`/local_costmap/costmap`, etc. - so a Foxglove **3D** panel renders the actual +robot in the warehouse out of the box. + +1. Open Foxglove Studio -> **Open connection** -> **Foxglove WebSocket** -> + `ws://localhost:8765`. The Topics panel should list all of the topics + above. +2. Drop in a **3D** panel. In its settings set **Scene -> Mesh up axis -> + Z**, then reload (Ctrl-R). The mesh geometry carries no up-axis metadata so + Foxglove defaults to Y-up, which renders the robot rotated with its parts + scattered; Z-up puts it upright and assembled. You should then see the + RB-Theron sitting in the AWS small-warehouse world. + Shortly after boot, the auto-applied `broken_lidar_3_0_0` update swaps + `scan_sensor_node` over to `broken_lidar` - a forward sector of `/scan` + starts reporting a phantom close return (a stuck lidar sector). Nav2 cannot + get past the phantom, so the robot stalls and `navigate_to_pose` aborts - + the failure the demo's narrative pivots on. +3. Install the [`ros2_medkit_foxglove_extension`](https://github.com/selfpatch/ros2_medkit_foxglove_extension) + (`npm run local-install` in that repo, or drag-and-drop the `.foxe` + onto Foxglove). It ships three panels: Entity Browser, Faults Dashboard, + and **ros2_medkit Updates**. +4. Add the **Faults Dashboard** panel. Once the robot stalls at the + phantom (see "Driving the robot" below), `ACTION_NAVIGATE_TO_POSE_ABORTED` + shows up CONFIRMED on `bt-navigator` (the headline fault) alongside a + `LOG_CONTROLLER_SERVER_*` fault on `controller-server` (supporting), and + both stay latched. Expand the `bt-navigator` fault to see the + freeze-frame snapshot (`/scan`, `/cmd_vel`, `/local_costmap/costmap`) + captured at confirmation, plus the downloadable MCAP recording. +5. Add the **ros2_medkit Updates** panel and set its `baseUrl` to + `http://localhost:8080/api/v1` (or the port you picked via + `OTA_GATEWAY_PORT`). `broken_lidar_3_0_0` shows as the update applied + to `scan_sensor_node` - the fix is not registered yet. Run + `./publish-fix.sh` in a terminal to register `fixed_lidar_3_0_1` + (SOVD `POST /updates`); it then appears in the panel. Click **Prepare** + then **Execute** for `fixed_lidar_3_0_1` to apply it - the 3D panel + should show the phantom return disappearing as `broken_lidar` is killed + and `fixed_lidar` starts. The Faults Dashboard entry stays red until you + also clear it. + +### Driving the robot to make the narrative reproducible + +The demo doesn't auto-publish a navigation goal - that keeps it +deterministic for CI. Use `./send-goal.sh` to drive the loop yourself: + +```bash +./send-goal.sh # defaults to (1.5, 1.0); pass x y to override +``` + +`send-goal.sh` `docker exec`s into the gateway container (sourcing the ROS +overlay itself, since `docker exec` skips the image entrypoint) and +publishes `/goal_pose` once via `ros2 topic pub`. Foxglove's **3D** panel +also has a built-in "Publish" tool - select pose mode, click a point ahead +of the robot, and Foxglove publishes `/goal_pose` for you. + +While `broken_lidar_3_0_0` is applied (the boot default), driving toward +the phantom blocks the path and stalls Nav2 - `navigate_to_pose` aborts +and confirms `ACTION_NAVIGATE_TO_POSE_ABORTED` on `bt-navigator` (plus a +supporting `LOG_*` fault on `controller-server`). Watch the Faults +Dashboard panel or poll `GET /apps/bt-navigator/faults` and +`GET /apps/controller-server/faults`. After `./publish-fix.sh`, +`./apply-fix.sh`, and `./clear-fault.sh`, send the goal again and the robot +reaches it with a clean `/scan`. + +## Disclosures + +This is **dev-grade** OTA. Deliberately missing for production: + +- No artifact signing or signature verification +- No atomic swap (in-place overwrite) +- No A/B partition rollout +- No fleet-wide staged rollout +- No persistent update state across gateway restarts +- No automated health-gated rollback policy +- No audit log + +Perfect for: prototypes, lab robots, internal demos, dev environments. + +For production-grade OTA (rollout safety, signing, A/B partitions, +fleet-aware staging), reach out. diff --git a/demos/ota_nav2_sensor_fix/THIRD_PARTY_NOTICES.md b/demos/ota_nav2_sensor_fix/THIRD_PARTY_NOTICES.md new file mode 100644 index 0000000..9dcfb59 --- /dev/null +++ b/demos/ota_nav2_sensor_fix/THIRD_PARTY_NOTICES.md @@ -0,0 +1,16 @@ +# Third-party notices + +These assets are **fetched at Docker build time** from their upstream repositories at +the pinned commits below (see `Dockerfile.gateway`), not vendored in this repo - only +our RB-Theron wrapper (`ros2_packages/ota_nav2_sensor_fix_demo/urdf/warehouse_rbtheron.urdf.xacro`), +our gz-port world (`ros2_packages/ota_nav2_sensor_fix_demo/models/aws_small_warehouse/worlds/warehouse.sdf`), +and the build-time `file://models/` -> `model://` rewrite are ours. The pinned commits +are the source of truth; the Dockerfile passes them as the `ROBOTNIK_DESC_REF` / +`ROBOTNIK_SENS_REF` / `AWS_WAREHOUSE_REF` build args. Each upstream's license travels +with its clone (a `LICENSE` file at the repo root). + +| Component | Source | License | Commit | +| --- | --- | --- | --- | +| Robotnik RB-Theron description | https://github.com/RobotnikAutomation/robotnik_description (jazzy-devel) | BSD-3-Clause | 751059edd6af3a9c083018cfaee59e4496d46580 | +| Robotnik sensors | https://github.com/RobotnikAutomation/robotnik_sensors (jazzy-devel) | BSD-3-Clause | e5186c343910b86a924201edb256f79eb0f73295 | +| AWS small warehouse world | https://github.com/aws-robotics/aws-robomaker-small-warehouse-world (ros2) | MIT-0 | ee0af733315e78432408c3cd98d378ecee5f767c | diff --git a/demos/ota_nav2_sensor_fix/apply-fix.sh b/demos/ota_nav2_sensor_fix/apply-fix.sh new file mode 100755 index 0000000..97e64b5 --- /dev/null +++ b/demos/ota_nav2_sensor_fix/apply-fix.sh @@ -0,0 +1,42 @@ +#!/bin/bash +# Apply the published remediation update (fixed_lidar_3_0_1) via SOVD prepare/execute. +# Uses spec endpoints PUT /updates/{id}/prepare then PUT /updates/{id}/execute. + +set -eu + +GATEWAY_URL="${OTA_GATEWAY_URL:-http://localhost:${OTA_GATEWAY_PORT:-8080}}" +API="${GATEWAY_URL}/api/v1" +ID="fixed_lidar_3_0_1" + +if ! curl -fsS "${API}/health" >/dev/null 2>&1; then + echo "Gateway not reachable at ${GATEWAY_URL}. Start it with: ./run-demo.sh" + exit 1 +fi + +# The fix is not in the boot catalog - it has to be published first. +if ! curl -fsS "${API}/updates" 2>/dev/null | grep -q "${ID}"; then + echo "${ID} is not registered yet. Publish it first with: ./publish-fix.sh" + exit 1 +fi + +echo "Update: ${ID}" +echo " PUT /updates/${ID}/prepare" +curl -fsS -X PUT -H 'Content-Type: application/json' -d '{}' \ + "${API}/updates/${ID}/prepare" >/dev/null +sleep 3 + +echo " PUT /updates/${ID}/execute" +curl -fsS -X PUT -H 'Content-Type: application/json' -d '{}' \ + "${API}/updates/${ID}/execute" >/dev/null +sleep 5 + +echo "" +echo "Status after execute:" +curl -fsS "${API}/updates/${ID}/status" | (jq . 2>/dev/null || cat) + +if docker ps --format '{{.Names}}' | grep -q '^ota_demo_gateway$'; then + echo "" + echo "Live processes:" + docker exec ota_demo_gateway pgrep -af 'broken_lidar_node|fixed_lidar_node' \ + 2>/dev/null | grep -v 'pgrep' | sed 's/^/ /' || true +fi diff --git a/demos/ota_nav2_sensor_fix/artifacts/.gitignore b/demos/ota_nav2_sensor_fix/artifacts/.gitignore new file mode 100644 index 0000000..ae78201 --- /dev/null +++ b/demos/ota_nav2_sensor_fix/artifacts/.gitignore @@ -0,0 +1,2 @@ +*.tar.gz +catalog.json diff --git a/demos/ota_nav2_sensor_fix/check-demo.sh b/demos/ota_nav2_sensor_fix/check-demo.sh new file mode 100755 index 0000000..0cafd81 --- /dev/null +++ b/demos/ota_nav2_sensor_fix/check-demo.sh @@ -0,0 +1,88 @@ +#!/bin/bash +# Show the live state of the OTA demo at a glance: which lidar build +# scan_sensor_node is running, the applied updates + their statuses, and +# the current Nav2 faults on bt-navigator + controller-server (so a latched +# ACTION_NAVIGATE_TO_POSE_ABORTED / LOG_* fault and the bad update that +# caused it are both visible in one shot). + +set -eu + +GATEWAY_URL="${OTA_GATEWAY_URL:-http://localhost:${OTA_GATEWAY_PORT:-8080}}" +API="${GATEWAY_URL}/api/v1" + +if ! command -v curl >/dev/null 2>&1; then + echo "curl is required" + exit 1 +fi + +if ! curl -fsS "${API}/health" >/dev/null 2>&1; then + echo "Gateway not reachable at ${GATEWAY_URL}. Start it with: ./run-demo.sh" + exit 1 +fi + +JQ_AVAILABLE="false" +if command -v jq >/dev/null 2>&1; then + JQ_AVAILABLE="true" +fi + +GATEWAY_RUNNING="false" +if docker ps --format '{{.Names}}' | grep -q '^ota_demo_gateway$'; then + GATEWAY_RUNNING="true" +fi + +echo "Gateway: ${GATEWAY_URL}" +echo "Health: $(curl -fsS "${API}/health" | head -c 200)" +echo "" + +echo "Scan sensor (scan_sensor_node):" +if [[ "$GATEWAY_RUNNING" == "true" ]]; then + SCAN_PROC=$(docker exec ota_demo_gateway pgrep -af 'broken_lidar_node|fixed_lidar_node' \ + 2>/dev/null | grep -v 'pgrep' || true) + if echo "$SCAN_PROC" | grep -q 'broken_lidar_node'; then + echo " broken_lidar_node running - REGRESSED build applied (root cause)" + elif echo "$SCAN_PROC" | grep -q 'fixed_lidar_node'; then + echo " fixed_lidar_node running - known-good build" + else + echo " (no scan sensor process found)" + fi +else + echo " ota_demo_gateway container not running" +fi +echo "" + +echo "Applied updates (GET /updates, GET /updates/{id}/status):" +if [[ "$JQ_AVAILABLE" == "true" ]]; then + for id in $(curl -fsS "${API}/updates" | jq -r '.items[]'); do + status=$(curl -fsS "${API}/updates/${id}/status" 2>/dev/null || echo '{"status":""}') + echo " ${id}: $(echo "$status" | jq -c '{status, progress}')" + done +else + curl -fsS "${API}/updates" +fi +echo "" + +echo "Current Nav2 faults (GET /apps/bt-navigator/faults, /apps/controller-server/faults):" +for entity in "apps/bt-navigator" "apps/controller-server"; do + echo " ${entity}:" + FAULTS_JSON=$(curl -fsS "${API}/${entity}/faults" 2>/dev/null || echo '{"items":[]}') + if [[ "$JQ_AVAILABLE" == "true" ]]; then + FAULT_COUNT=$(echo "$FAULTS_JSON" | jq '.items | length') + if [[ "$FAULT_COUNT" -eq 0 ]]; then + echo " (none - clean)" + else + echo "$FAULTS_JSON" | jq -r '.items[] | " \(.fault_code): \(.status)"' + fi + else + echo " $FAULTS_JSON" + fi +done +echo "" + +echo "Plugin-managed processes inside gateway container:" +if [[ "$GATEWAY_RUNNING" == "true" ]]; then + docker exec ota_demo_gateway pgrep -af \ + 'broken_lidar_node|fixed_lidar_node' \ + 2>/dev/null | grep -v 'pgrep' | sed 's/^/ /' || echo " (none)" +else + echo " ota_demo_gateway container not running" +fi diff --git a/demos/ota_nav2_sensor_fix/clear-fault.sh b/demos/ota_nav2_sensor_fix/clear-fault.sh new file mode 100755 index 0000000..ae1ba07 --- /dev/null +++ b/demos/ota_nav2_sensor_fix/clear-fault.sh @@ -0,0 +1,24 @@ +#!/bin/bash +# Operator clear of the latched Nav2 faults after the fix is applied. +# bt-navigator's ACTION_NAVIGATE_TO_POSE_ABORTED is a stable, bridge-generated +# code, so it is cleared by code. controller-server's LOG_CONTROLLER_SERVER_* +# code is content-derived (the hash can change between runs), so it is +# cleared with a clear-all on the entity instead of a hardcoded code. Both +# faults are latched (no self-heal); this is the deliberate operator +# acknowledge step. +set -eu +API="${OTA_GATEWAY_URL:-http://localhost:${OTA_GATEWAY_PORT:-8080}}/api/v1" +NAV_ENTITY="apps/bt-navigator" +NAV_CODE="ACTION_NAVIGATE_TO_POSE_ABORTED" +CONTROLLER_ENTITY="apps/controller-server" + +echo "DELETE /${NAV_ENTITY}/faults/${NAV_CODE}" +curl -fsS -X DELETE "${API}/${NAV_ENTITY}/faults/${NAV_CODE}" -o /dev/null -w ' HTTP %{http_code}\n' + +echo "DELETE /${CONTROLLER_ENTITY}/faults (clear-all - the LOG_* code is content-hashed)" +curl -fsS -X DELETE "${API}/${CONTROLLER_ENTITY}/faults" -o /dev/null -w ' HTTP %{http_code}\n' + +echo "Remaining faults on ${NAV_ENTITY}:" +curl -fsS "${API}/${NAV_ENTITY}/faults" | (jq -r '.items[].fault_code' 2>/dev/null || cat) | sed 's/^/ /' +echo "Remaining faults on ${CONTROLLER_ENTITY}:" +curl -fsS "${API}/${CONTROLLER_ENTITY}/faults" | (jq -r '.items[].fault_code' 2>/dev/null || cat) | sed 's/^/ /' diff --git a/demos/ota_nav2_sensor_fix/docker-compose.yml b/demos/ota_nav2_sensor_fix/docker-compose.yml new file mode 100644 index 0000000..a3d3aa2 --- /dev/null +++ b/demos/ota_nav2_sensor_fix/docker-compose.yml @@ -0,0 +1,44 @@ +# Copyright 2026 bburda +# Apache 2.0 +# +# Two-service stack: the gateway (with ota_update_plugin baked in plus the +# nav2 + RB-Theron + headless Gazebo + foxglove_bridge orchestration) and the +# FastAPI artifact server. The gateway image is hefty (~5GB) because it bakes +# the simulator in - the trade-off is `docker compose up` produces a robot +# Foxglove can render, no external sim setup required. + +services: + gateway: + image: selfpatch/ota_demo_gateway:dev + build: + context: . + dockerfile: Dockerfile.gateway + container_name: ota_demo_gateway + networks: [otanet] + ports: + - "${OTA_GATEWAY_PORT:-8080}:8080" + - "${OTA_FOXGLOVE_BRIDGE_PORT:-8765}:8765" + environment: + ROS_DOMAIN_ID: 42 + HEADLESS: "true" + # Gazebo / DDS appreciate generous shared memory; without this + # /dev/shm fills and gz-sim tends to wedge on shutdown. + shm_size: "2gb" + tty: true + stdin_open: true + depends_on: + - ota_update_server + + ota_update_server: + image: selfpatch/ota_update_server:dev + build: + context: . + dockerfile: ota_update_server/Dockerfile + container_name: ota_demo_update_server + networks: [otanet] + ports: + - "9000:9000" + +networks: + otanet: + driver: bridge diff --git a/demos/ota_nav2_sensor_fix/entrypoint.sh b/demos/ota_nav2_sensor_fix/entrypoint.sh new file mode 100755 index 0000000..f60a738 --- /dev/null +++ b/demos/ota_nav2_sensor_fix/entrypoint.sh @@ -0,0 +1,39 @@ +#!/usr/bin/env bash +# Copyright 2026 bburda +# Apache 2.0 +# +# Container entrypoint: hands off to the ota_nav2_sensor_fix_demo launch file +# which orchestrates everything (RB-Theron AMR + Nav2 + headless Gazebo + +# foxglove_bridge + fault_manager + gateway w/ ota_update_plugin). Once the +# gateway is up, this script auto-applies broken_lidar_3_0_0 so the mission +# starts on the regressed lidar the operator has to diagnose. + +set -e + +# shellcheck disable=SC1091 +source /opt/ros/jazzy/setup.bash +# shellcheck disable=SC1091 +source /ws/install/setup.bash + +# Default to headless; an operator on a workstation can flip via env var. +HEADLESS_ARG="${HEADLESS:-true}" + +# Simulate a routine software update that regressed the lidar: once the +# gateway is healthy and the plugin has registered the catalog, apply +# broken_lidar_3_0_0 so scan_sensor_node is running the bad build before +# the mission starts. The operator later finds it in /updates, publishes +# the forward hotfix (fixed_lidar_3_0_1) with publish-fix.sh, and applies +# it with apply-fix.sh. +( + API="http://localhost:8080/api/v1" + for _ in $(seq 1 60); do + if curl -fsS "${API}/updates" 2>/dev/null | grep -q 'broken_lidar_3_0_0'; then break; fi + sleep 2 + done + curl -fsS -X PUT -H 'Content-Type: application/json' -d '{}' "${API}/updates/broken_lidar_3_0_0/prepare" >/dev/null 2>&1 || true + sleep 3 + curl -fsS -X PUT -H 'Content-Type: application/json' -d '{}' "${API}/updates/broken_lidar_3_0_0/execute" >/dev/null 2>&1 || true +) & + +exec ros2 launch ota_nav2_sensor_fix_demo demo.launch.py \ + "headless:=${HEADLESS_ARG}" diff --git a/demos/ota_nav2_sensor_fix/gateway_config.yaml b/demos/ota_nav2_sensor_fix/gateway_config.yaml new file mode 100644 index 0000000..bb85917 --- /dev/null +++ b/demos/ota_nav2_sensor_fix/gateway_config.yaml @@ -0,0 +1,62 @@ +# Copyright 2026 bburda +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# +# Gateway configuration for the OTA nav2 sensor-fix demo. +# Enables /updates endpoints and loads ota_update_plugin which polls the +# update server's /catalog at boot and exposes Update / Install / Uninstall +# operations over the SOVD HTTP API. + +ros2_medkit_gateway: + ros__parameters: + server: + host: "0.0.0.0" + port: 8080 + + refresh_interval_ms: 2000 + + # CORS so an external Foxglove panel or browser can hit the API. + cors: + allowed_origins: ["*"] + allowed_methods: ["GET", "PUT", "POST", "DELETE", "OPTIONS"] + allowed_headers: ["Content-Type", "Accept"] + allow_credentials: false + max_age_seconds: 86400 + + discovery: + # Hybrid: manifest defines areas/components/apps/functions, runtime + # fills in topics/services/params, and OTA-deployed apps land via + # manifest fragments dropped in fragments_dir below. + mode: "hybrid" + manifest_path: "/etc/ros2_medkit/manifest.yaml" + manifest_strict_validation: false + manifest: + # ota_update_plugin writes one fragment per Install operation + # here and calls notify_entities_changed; the gateway re-merges + # the base manifest + every yaml in this dir on each reload. + # Path is shared with the plugin via plugins.ota_update_plugin + # .fragments_dir below - keep them in lockstep. + fragments_dir: "/etc/ros2_medkit/manifest_fragments" + runtime: + # Manifest already defines the rbtheron Component; suppress the + # host-derived default Component so the entity tree shows a single + # component instead of rbtheron + the host. + default_component: + enabled: false + # Manifest defines functions; the auto-gen-from-namespaces path + # produces single-host noise because the nav2 stack lives at /. + create_functions_from_namespaces: false + + # Enable /updates endpoints; provider supplied by ota_update_plugin below. + updates: + enabled: true + + plugins: ["ota_update_plugin"] + plugins.ota_update_plugin.path: "/ws/install/ota_update_plugin/lib/ota_update_plugin/ota_update_plugin.so" + plugins.ota_update_plugin.catalog_url: "http://ota_update_server:9000" + plugins.ota_update_plugin.staging_dir: "/tmp/ota_staging" + plugins.ota_update_plugin.install_dir: "/ws/install" + # Same path the gateway has under discovery.manifest.fragments_dir. + # Plugin drops one yaml per Install and removes it on Uninstall. + plugins.ota_update_plugin.fragments_dir: "/etc/ros2_medkit/manifest_fragments" diff --git a/demos/ota_nav2_sensor_fix/manifest.yaml b/demos/ota_nav2_sensor_fix/manifest.yaml new file mode 100644 index 0000000..a04bb38 --- /dev/null +++ b/demos/ota_nav2_sensor_fix/manifest.yaml @@ -0,0 +1,281 @@ +# Copyright 2026 bburda. Apache-2.0. +# +# SOVD manifest for the OTA over SOVD nav2 sensor-fix demo. +# +# Hybrid discovery: the manifest defines components + apps + functions, +# and runtime discovery fills in topics/services/params for those +# entities. +# +# Hierarchy used for this demo: +# - One Component: the robot itself. SOVD lets a manifest declare +# multiple components (e.g. one per ECU on a vehicle), but here +# everything runs on a single host so a flat single-component model +# is honest and avoids fake "lidar-sensor / nav2-motion / ..." +# subdivisions that don't correspond to anything you can actually +# swap independently in this demo. +# - Apps are the ROS 2 nodes - one entry per node we want the operator +# to see. They all live on the single robot component. +# - Functions are capability groupings - "Autonomous Navigation", +# "Perception" etc. - that pull apps together by what they deliver, +# orthogonally to where they run. +# +# Areas are intentionally left out. SOVD allows them, but they only add +# value when there's a meaningful zone partition (powertrain / body / +# chassis on a vehicle ECU mesh, or multi-robot tenancy). For a single +# robot they just regroup the same component a second time. + +manifest_version: "1.0" + +metadata: + name: "ota-nav2-sensor-fix" + description: "OTA over SOVD demo - RB-Theron AMR + Nav2 + AWS warehouse (headless Gazebo) + ros2_medkit gateway" + version: "0.1.0" + +config: + # Nav2 spins up internal helper nodes we deliberately don't manifest + # (per-action _rclcpp_node clients, transform_listener_impl_* per TF + # listener) - tolerate them instead of failing discovery. + unmanifested_nodes: warn + # Pull in topics, services, params from the running graph for entities + # the manifest declares. + inherit_runtime_resources: true + +# ============================================================================= +# COMPONENTS - one per OTA boundary. For this demo there's a single host +# so a single component owns every app. +# ============================================================================= +components: + - id: rbtheron + name: "RB-Theron AMR" + type: "platform" + description: "Robotnik RB-Theron AMR running headless Gazebo (AWS small-warehouse world), Nav2, the ros2_medkit gateway, and the OTA-managed sensor stack." + +# ============================================================================= +# APPS - one per ROS 2 node we care about (skip nav2-internal _rclcpp_node +# helpers and transform_listener_impl_* - they're plumbing, not user-facing) +# ============================================================================= +apps: + # ── LiDAR / perception ──────────────────────────────────────────── + - id: scan-sensor-node + name: "Scan Sensor" + category: "sensor" + is_located_on: rbtheron + description: "LaserScan publisher (broken_lidar pre-OTA, fixed_lidar post-OTA)" + ros_binding: { node_name: scan_sensor_node, namespace: / } + + - id: ros-gz-bridge + name: "ROS-Gazebo Bridge" + category: "simulation" + is_located_on: rbtheron + description: "Bridges /scan_sim and /clock from gz-sim" + ros_binding: { node_name: ros_gz_bridge, namespace: / } + + # ── Robot platform ──────────────────────────────────────────────── + - id: robot-state-publisher + name: "Robot State Publisher" + category: "platform" + is_located_on: rbtheron + description: "Publishes the robot URDF TF tree" + ros_binding: { node_name: robot_state_publisher, namespace: / } + + # ── Nav2 motion ─────────────────────────────────────────────────── + - id: bt-navigator + name: "BT Navigator" + category: "navigation" + is_located_on: rbtheron + description: "Behavior Tree navigator - hosts navigate_to_pose action" + ros_binding: { node_name: bt_navigator, namespace: / } + + - id: planner-server + name: "Planner Server" + category: "navigation" + is_located_on: rbtheron + description: "Global path planner" + ros_binding: { node_name: planner_server, namespace: / } + + - id: controller-server + name: "Controller Server" + category: "navigation" + is_located_on: rbtheron + description: "Local path follower" + ros_binding: { node_name: controller_server, namespace: / } + + - id: smoother-server + name: "Smoother Server" + category: "navigation" + is_located_on: rbtheron + description: "Path smoothing" + ros_binding: { node_name: smoother_server, namespace: / } + + - id: route-server + name: "Route Server" + category: "navigation" + is_located_on: rbtheron + description: "Route planning" + ros_binding: { node_name: route_server, namespace: / } + + - id: behavior-server + name: "Behavior Server" + category: "navigation" + is_located_on: rbtheron + description: "Recovery behaviors" + ros_binding: { node_name: behavior_server, namespace: / } + + - id: waypoint-follower + name: "Waypoint Follower" + category: "navigation" + is_located_on: rbtheron + description: "Sequenced waypoint navigation" + ros_binding: { node_name: waypoint_follower, namespace: / } + + - id: velocity-smoother + name: "Velocity Smoother" + category: "navigation" + is_located_on: rbtheron + description: "/cmd_vel smoothing" + ros_binding: { node_name: velocity_smoother, namespace: / } + + - id: collision-monitor + name: "Collision Monitor" + category: "navigation" + is_located_on: rbtheron + description: "Emergency stop on imminent collision" + ros_binding: { node_name: collision_monitor, namespace: / } + + - id: docking-server + name: "Docking Server" + category: "navigation" + is_located_on: rbtheron + description: "Approach + dock action" + ros_binding: { node_name: docking_server, namespace: / } + + - id: global-costmap + name: "Global Costmap" + category: "navigation" + is_located_on: rbtheron + description: "Static + obstacle costmap for planning" + ros_binding: { node_name: global_costmap, namespace: /global_costmap } + + - id: local-costmap + name: "Local Costmap" + category: "navigation" + is_located_on: rbtheron + description: "Local rolling costmap for control" + ros_binding: { node_name: local_costmap, namespace: /local_costmap } + + - id: lifecycle-manager-navigation + name: "Lifecycle Manager (Navigation)" + category: "navigation" + is_located_on: rbtheron + description: "Nav2 motion lifecycle orchestration" + ros_binding: { node_name: lifecycle_manager_navigation, namespace: / } + + # ── Nav2 localization ───────────────────────────────────────────── + - id: amcl + name: "AMCL" + category: "localization" + is_located_on: rbtheron + description: "Adaptive Monte Carlo Localization" + ros_binding: { node_name: amcl, namespace: / } + + - id: map-server + name: "Map Server" + category: "localization" + is_located_on: rbtheron + description: "Static map publisher" + ros_binding: { node_name: map_server, namespace: / } + + - id: lifecycle-manager-localization + name: "Lifecycle Manager (Localization)" + category: "localization" + is_located_on: rbtheron + description: "Localization lifecycle orchestration" + ros_binding: { node_name: lifecycle_manager_localization, namespace: / } + + # ── Diagnostics ─────────────────────────────────────────────────── + - id: medkit-gateway + name: "ros2_medkit Gateway" + category: "gateway" + is_located_on: rbtheron + description: "SOVD REST gateway, hosts the OTA plugin" + ros_binding: { node_name: ros2_medkit_gateway, namespace: / } + + - id: medkit-fault-manager + name: "Fault Manager" + category: "diagnostics" + is_located_on: rbtheron + description: "Fault aggregation + storage" + ros_binding: { node_name: fault_manager, namespace: / } + + # ── Visualization ───────────────────────────────────────────────── + - id: foxglove-bridge + name: "Foxglove Bridge" + category: "visualization" + is_located_on: rbtheron + description: "WebSocket bridge on :8765" + ros_binding: { node_name: foxglove_bridge, namespace: / } + + # ── Diagnostics (operator-invoked) ──────────────────────────────── + - id: health-check + name: "Health Check" + category: "diagnostics" + is_located_on: rbtheron + description: "Operator-invoked differential-diagnosis operations (lidar/localization/drivetrain/costmap) for root-cause confirmation" + ros_binding: { node_name: health_check, namespace: / } + +# ============================================================================= +# FUNCTIONS - what the user actually selects in the tree to ask +# "is this capability working?" +# ============================================================================= +functions: + - id: autonomous-navigation + name: "Autonomous Navigation" + category: "mobility" + description: "Plan + drive to a goal pose - the headline OTA demo capability" + hosted_by: + - bt-navigator + - planner-server + - controller-server + - smoother-server + - route-server + - behavior-server + - waypoint-follower + - velocity-smoother + - collision-monitor + - docking-server + - global-costmap + - local-costmap + - lifecycle-manager-navigation + + - id: localization + name: "Localization" + category: "mobility" + description: "Where is the robot in the map - AMCL + map server" + hosted_by: + - amcl + - map-server + - lifecycle-manager-localization + + - id: perception + name: "Perception" + category: "sensing" + description: "LaserScan stream feeding nav2 - the OTA target" + hosted_by: + - scan-sensor-node + - ros-gz-bridge + + - id: fleet-diagnostics + name: "Fleet Diagnostics" + category: "diagnostics" + description: "SOVD REST surface + fault aggregation - this panel" + hosted_by: + - medkit-gateway + - medkit-fault-manager + + - id: live-telemetry + name: "Live Telemetry" + category: "observability" + description: "Foxglove bridge + URDF publisher feeding the 3D panel" + hosted_by: + - foxglove-bridge + - robot-state-publisher diff --git a/demos/ota_nav2_sensor_fix/ota_update_plugin/CMakeLists.txt b/demos/ota_nav2_sensor_fix/ota_update_plugin/CMakeLists.txt new file mode 100644 index 0000000..050ca09 --- /dev/null +++ b/demos/ota_nav2_sensor_fix/ota_update_plugin/CMakeLists.txt @@ -0,0 +1,86 @@ +# Copyright 2026 bburda +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +cmake_minimum_required(VERSION 3.16) +project(ota_update_plugin CXX) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic -Wshadow -Wconversion) +endif() + +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_POSITION_INDEPENDENT_CODE ON) + +find_package(ament_cmake REQUIRED) +find_package(ros2_medkit_cmake REQUIRED) +include(ROS2MedkitCompat) +find_package(ros2_medkit_gateway REQUIRED) +find_package(nlohmann_json REQUIRED) + +# CatalogClient uses cpp-httplib for HTTP. Use gateway's vendored copy as fallback. +set(_gw_vendored "${ros2_medkit_gateway_DIR}/../vendored/cpp_httplib") +medkit_find_cpp_httplib(VENDORED_DIR "${_gw_vendored}") +unset(_gw_vendored) + +# Static core library: plugin + tests both link against this. +add_library(ota_update_plugin_core STATIC + src/ota_update_plugin.cpp + src/catalog_client.cpp + src/operation_dispatcher.cpp + src/process_runner.cpp +) +target_include_directories(ota_update_plugin_core + PUBLIC + $ + $ +) +ament_target_dependencies(ota_update_plugin_core ros2_medkit_gateway) +target_link_libraries(ota_update_plugin_core + PUBLIC + nlohmann_json::nlohmann_json + cpp_httplib_target +) +set_target_properties(ota_update_plugin_core PROPERTIES POSITION_INDEPENDENT_CODE ON) + +# MODULE target: loaded via dlopen at runtime by PluginManager. +# Symbols from gateway_lib are resolved from the host process at runtime. +add_library(ota_update_plugin MODULE src/plugin_exports.cpp) +target_link_libraries(ota_update_plugin PRIVATE ota_update_plugin_core) +set_target_properties(ota_update_plugin PROPERTIES + PREFIX "" + OUTPUT_NAME "ota_update_plugin" +) +# Allow unresolved symbols - they resolve from the host process at runtime +target_link_options(ota_update_plugin PRIVATE + -Wl,--unresolved-symbols=ignore-all +) + +install(TARGETS ota_update_plugin + LIBRARY DESTINATION lib/${PROJECT_NAME} +) +install(DIRECTORY include/ DESTINATION include) + +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + ament_add_gtest(test_ota_update_plugin + test/test_operation_dispatcher.cpp + test/test_catalog_client.cpp + test/test_plugin_smoke.cpp + ) + target_link_libraries(test_ota_update_plugin ota_update_plugin_core) + target_include_directories(test_ota_update_plugin PRIVATE src) +endif() + +ament_package() diff --git a/demos/ota_nav2_sensor_fix/ota_update_plugin/include/ota_update_plugin/ota_update_plugin.hpp b/demos/ota_nav2_sensor_fix/ota_update_plugin/include/ota_update_plugin/ota_update_plugin.hpp new file mode 100644 index 0000000..55952bd --- /dev/null +++ b/demos/ota_nav2_sensor_fix/ota_update_plugin/include/ota_update_plugin/ota_update_plugin.hpp @@ -0,0 +1,98 @@ +// Copyright 2026 bburda +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +namespace ota_update_plugin { + +class CatalogClient; +class ProcessRunner; + +/// OTA update plugin: implements both GatewayPlugin and UpdateProvider. +/// Polls a FastAPI catalog at boot and supports update / install / uninstall +/// operations derived from SOVD ISO 17978-3 metadata. +class OtaUpdatePlugin : public ros2_medkit_gateway::GatewayPlugin, public ros2_medkit_gateway::UpdateProvider { + public: + OtaUpdatePlugin(); + ~OtaUpdatePlugin() override; + + OtaUpdatePlugin(const OtaUpdatePlugin &) = delete; + OtaUpdatePlugin & operator=(const OtaUpdatePlugin &) = delete; + OtaUpdatePlugin(OtaUpdatePlugin &&) = delete; + OtaUpdatePlugin & operator=(OtaUpdatePlugin &&) = delete; + + // GatewayPlugin + std::string name() const override { + return "ota_update_plugin"; + } + void configure(const nlohmann::json & config) override; + void set_context(ros2_medkit_gateway::PluginContext & context) override; + + // UpdateProvider + tl::expected, ros2_medkit_gateway::UpdateBackendErrorInfo> list_updates( + const ros2_medkit_gateway::UpdateFilter & filter) override; + tl::expected get_update( + const std::string & id) override; + tl::expected register_update( + const nlohmann::json & metadata) override; + tl::expected delete_update(const std::string & id) override; + tl::expected prepare( + const std::string & id, ros2_medkit_gateway::UpdateProgressReporter & reporter) override; + tl::expected execute( + const std::string & id, ros2_medkit_gateway::UpdateProgressReporter & reporter) override; + tl::expected supports_automated(const std::string & id) override; + + // Test seams + void set_catalog_client_for_test(std::unique_ptr client); + void set_process_runner_for_test(std::unique_ptr runner); + void poll_and_register_catalog(); + + private: + // Manifest-fragment helpers. Plugins that deploy new nodes at runtime + // are expected to drop a fragment yaml in `fragments_dir_` and then + // notify the gateway so its ManifestManager re-merges. Without this + // the new app shows up as an "Orphan node (not in manifest)" warn + // log and never attaches to the manifest entity tree. + tl::expected write_install_fragment(const std::string & update_id, + const nlohmann::json & metadata); + tl::expected remove_install_fragment(const std::string & update_id); + void notify_manifest_changed(); + + std::string catalog_url_; + std::string staging_dir_; + std::string install_dir_; + std::string fragments_dir_; + + ros2_medkit_gateway::PluginContext * context_{nullptr}; + + std::mutex mu_; + std::map registry_; + std::map staged_artifacts_; + + std::unique_ptr catalog_client_; + std::unique_ptr process_runner_; +}; + +} // namespace ota_update_plugin diff --git a/demos/ota_nav2_sensor_fix/ota_update_plugin/package.xml b/demos/ota_nav2_sensor_fix/ota_update_plugin/package.xml new file mode 100644 index 0000000..200c826 --- /dev/null +++ b/demos/ota_nav2_sensor_fix/ota_update_plugin/package.xml @@ -0,0 +1,20 @@ + + + ota_update_plugin + 0.1.0 + Dev-grade OTA plugin for ros2_medkit gateway: update / install / uninstall via simple HTTP catalog. + bburda + Apache-2.0 + + ament_cmake + + ros2_medkit_cmake + ros2_medkit_gateway + nlohmann-json-dev + + ament_cmake_gtest + + + ament_cmake + + diff --git a/demos/ota_nav2_sensor_fix/ota_update_plugin/src/catalog_client.cpp b/demos/ota_nav2_sensor_fix/ota_update_plugin/src/catalog_client.cpp new file mode 100644 index 0000000..1849025 --- /dev/null +++ b/demos/ota_nav2_sensor_fix/ota_update_plugin/src/catalog_client.cpp @@ -0,0 +1,171 @@ +// Copyright 2026 bburda +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "catalog_client.hpp" + +#include +#include +#include + +#include + +namespace ota_update_plugin { + +namespace { + +bool starts_with(const std::string & s, const std::string & prefix) { + return s.size() >= prefix.size() && s.compare(0, prefix.size(), prefix) == 0; +} + +} // namespace + +ParsedUrl parse_url(const std::string & url) { + ParsedUrl out{}; + std::string rest; + if (starts_with(url, "https://")) { + out.tls = true; + out.port = 443; + rest = url.substr(8); + } else if (starts_with(url, "http://")) { + out.tls = false; + out.port = 80; + rest = url.substr(7); + } else { + throw std::invalid_argument("unsupported URL scheme: " + url); + } + + // Split host[:port] from path. + const auto slash = rest.find('/'); + std::string authority; + if (slash == std::string::npos) { + authority = rest; + out.path = "/"; + } else { + authority = rest.substr(0, slash); + out.path = rest.substr(slash); + } + + // Split host from port if present. + const auto colon = authority.find(':'); + if (colon == std::string::npos) { + out.host = authority; + } else { + out.host = authority.substr(0, colon); + try { + out.port = std::stoi(authority.substr(colon + 1)); + } catch (const std::exception & e) { + throw std::invalid_argument(std::string("invalid port in URL: ") + url + " (" + e.what() + ")"); + } + } + + if (out.host.empty()) { + throw std::invalid_argument("missing host in URL: " + url); + } + return out; +} + +CatalogClient::CatalogClient(std::string base_url) : base_url_(std::move(base_url)) { +} + +tl::expected CatalogClient::fetch_catalog() { + ParsedUrl parsed; + try { + parsed = parse_url(base_url_); + } catch (const std::exception & e) { + return tl::make_unexpected(std::string("invalid catalog url: ") + e.what()); + } + + if (parsed.tls) { + return tl::make_unexpected("https not supported by demo CatalogClient"); + } + + // Strip trailing slash from base path, then append /catalog. + std::string base_path = parsed.path; + if (!base_path.empty() && base_path.back() == '/') { + base_path.pop_back(); + } + const std::string target = base_path + "/catalog"; + + httplib::Client cli(parsed.host, parsed.port); + cli.set_connection_timeout(5, 0); + cli.set_read_timeout(5, 0); + + auto res = cli.Get(target.c_str()); + if (!res) { + return tl::make_unexpected("catalog GET failed: " + httplib::to_string(res.error())); + } + if (res->status < 200 || res->status >= 300) { + return tl::make_unexpected("catalog GET returned status " + std::to_string(res->status)); + } + try { + return nlohmann::json::parse(res->body); + } catch (const std::exception & e) { + return tl::make_unexpected(std::string("catalog json parse failed: ") + e.what()); + } +} + +tl::expected CatalogClient::download_artifact(const std::string & url_or_path, + const std::string & out_path) { + // If url_or_path is an absolute URL, parse it directly. Otherwise treat as a + // path relative to base_url_. + std::string full_url; + if (starts_with(url_or_path, "http://") || starts_with(url_or_path, "https://")) { + full_url = url_or_path; + } else { + std::string base = base_url_; + // Strip trailing slash on base, leading slash on relative path. + while (!base.empty() && base.back() == '/') { + base.pop_back(); + } + std::string rel = url_or_path; + if (rel.empty() || rel.front() != '/') { + rel = "/" + rel; + } + full_url = base + rel; + } + + ParsedUrl parsed; + try { + parsed = parse_url(full_url); + } catch (const std::exception & e) { + return tl::make_unexpected(std::string("invalid artifact url: ") + e.what()); + } + if (parsed.tls) { + return tl::make_unexpected("https not supported by demo CatalogClient"); + } + + httplib::Client cli(parsed.host, parsed.port); + cli.set_connection_timeout(5, 0); + cli.set_read_timeout(30, 0); + + auto res = cli.Get(parsed.path.c_str()); + if (!res) { + return tl::make_unexpected("artifact GET failed: " + httplib::to_string(res.error())); + } + if (res->status < 200 || res->status >= 300) { + return tl::make_unexpected("artifact GET returned status " + std::to_string(res->status)); + } + + std::ofstream o(out_path, std::ios::binary); + if (!o) { + return tl::make_unexpected("cannot open output file: " + out_path); + } + o.write(res->body.data(), static_cast(res->body.size())); + if (!o) { + return tl::make_unexpected("write to output file failed: " + out_path); + } + return out_path; +} + +} // namespace ota_update_plugin diff --git a/demos/ota_nav2_sensor_fix/ota_update_plugin/src/catalog_client.hpp b/demos/ota_nav2_sensor_fix/ota_update_plugin/src/catalog_client.hpp new file mode 100644 index 0000000..0b8a9b2 --- /dev/null +++ b/demos/ota_nav2_sensor_fix/ota_update_plugin/src/catalog_client.hpp @@ -0,0 +1,61 @@ +// Copyright 2026 bburda +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include + +#include +#include + +namespace ota_update_plugin { + +/// Decomposed URL components used by the HTTP client. +struct ParsedUrl { + std::string host; + int port; + bool tls; + std::string path; +}; + +/// Parse an http:// or https:// URL into components. +/// Throws std::invalid_argument for unsupported schemes. +ParsedUrl parse_url(const std::string & url); + +/// HTTP client that fetches the FastAPI catalog and downloads artifacts. +/// Virtual methods so tests can substitute a fake without touching real HTTP. +class CatalogClient { + public: + explicit CatalogClient(std::string base_url); + virtual ~CatalogClient() = default; + + CatalogClient(const CatalogClient &) = delete; + CatalogClient & operator=(const CatalogClient &) = delete; + CatalogClient(CatalogClient &&) = delete; + CatalogClient & operator=(CatalogClient &&) = delete; + + /// GET {base_url}/catalog and parse JSON. Returns the JSON array on success. + virtual tl::expected fetch_catalog(); + + /// Download an artifact. `url_or_path` may be either an absolute URL or a + /// path (interpreted relative to `base_url`). Body is written to `out_path`. + /// Returns the absolute output path on success. + virtual tl::expected download_artifact(const std::string & url_or_path, + const std::string & out_path); + + protected: + std::string base_url_; +}; + +} // namespace ota_update_plugin diff --git a/demos/ota_nav2_sensor_fix/ota_update_plugin/src/operation_dispatcher.cpp b/demos/ota_nav2_sensor_fix/ota_update_plugin/src/operation_dispatcher.cpp new file mode 100644 index 0000000..b28b2af --- /dev/null +++ b/demos/ota_nav2_sensor_fix/ota_update_plugin/src/operation_dispatcher.cpp @@ -0,0 +1,48 @@ +// Copyright 2026 bburda +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "operation_dispatcher.hpp" + +namespace ota_update_plugin { + +namespace { + +bool non_empty_array(const nlohmann::json & j, const char * key) { + if (!j.contains(key)) { + return false; + } + const auto & v = j.at(key); + return v.is_array() && !v.empty(); +} + +} // namespace + +OperationKind OperationDispatcher::classify(const nlohmann::json & metadata) { + const bool has_updated = non_empty_array(metadata, "updated_components"); + const bool has_added = non_empty_array(metadata, "added_components"); + const bool has_removed = non_empty_array(metadata, "removed_components"); + const int populated = static_cast(has_updated) + static_cast(has_added) + static_cast(has_removed); + if (populated != 1) { + return OperationKind::Unknown; + } + if (has_updated) { + return OperationKind::Update; + } + if (has_added) { + return OperationKind::Install; + } + return OperationKind::Uninstall; +} + +} // namespace ota_update_plugin diff --git a/demos/ota_nav2_sensor_fix/ota_update_plugin/src/operation_dispatcher.hpp b/demos/ota_nav2_sensor_fix/ota_update_plugin/src/operation_dispatcher.hpp new file mode 100644 index 0000000..3398c2e --- /dev/null +++ b/demos/ota_nav2_sensor_fix/ota_update_plugin/src/operation_dispatcher.hpp @@ -0,0 +1,33 @@ +// Copyright 2026 bburda +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include + +namespace ota_update_plugin { + +/// Operation kind classified from SOVD update metadata. +enum class OperationKind { Update, Install, Uninstall, Unknown }; + +/// Maps SOVD update metadata to a concrete operation kind based on which of +/// updated_components / added_components / removed_components is populated. +class OperationDispatcher { + public: + /// Classify an update's operation kind. Returns Unknown if zero or more + /// than one of the three component arrays is non-empty. + static OperationKind classify(const nlohmann::json & metadata); +}; + +} // namespace ota_update_plugin diff --git a/demos/ota_nav2_sensor_fix/ota_update_plugin/src/ota_update_plugin.cpp b/demos/ota_nav2_sensor_fix/ota_update_plugin/src/ota_update_plugin.cpp new file mode 100644 index 0000000..3cf8179 --- /dev/null +++ b/demos/ota_nav2_sensor_fix/ota_update_plugin/src/ota_update_plugin.cpp @@ -0,0 +1,427 @@ +// Copyright 2026 bburda +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "ota_update_plugin/ota_update_plugin.hpp" + +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include "catalog_client.hpp" +#include "operation_dispatcher.hpp" +#include "process_runner.hpp" + +namespace ota_update_plugin { + +namespace fs = std::filesystem; +using ros2_medkit_gateway::UpdateBackendError; +using ros2_medkit_gateway::UpdateBackendErrorInfo; + +namespace { + +/// Extract a packed tarball into a staging directory, then atomically replace +/// `${install_dir}/${target_package}` with the freshly extracted contents. +/// The artifacts are produced by pack_artifact.py and contain a single +/// top-level directory named after the target package. +tl::expected extract_and_swap(const std::string & staged_tarball, const std::string & install_dir, + const std::string & target_package) { + if (target_package.empty()) { + return tl::make_unexpected("target_package is empty"); + } + const std::string staging_extracted = staged_tarball + ".extracted"; + std::error_code ec; + fs::remove_all(staging_extracted, ec); + fs::create_directories(staging_extracted, ec); + + const std::string cmd = "tar -xzf " + staged_tarball + " -C " + staging_extracted; + if (std::system(cmd.c_str()) != 0) { + return tl::make_unexpected("tar extraction failed: " + cmd); + } + + const std::string source = staging_extracted + "/" + target_package; + if (!fs::exists(source)) { + return tl::make_unexpected("artifact missing top-level directory '" + target_package + "' after extraction"); + } + + fs::create_directories(install_dir, ec); + const std::string target = install_dir + "/" + target_package; + fs::remove_all(target, ec); + fs::copy(source, target, fs::copy_options::recursive | fs::copy_options::overwrite_existing, ec); + if (ec) { + return tl::make_unexpected("copy failed: " + ec.message()); + } + return {}; +} + +} // namespace + +OtaUpdatePlugin::OtaUpdatePlugin() : process_runner_(std::make_unique()) { +} + +OtaUpdatePlugin::~OtaUpdatePlugin() = default; + +void OtaUpdatePlugin::configure(const nlohmann::json & config) { + catalog_url_ = config.value("catalog_url", "http://ota_update_server:9000"); + staging_dir_ = config.value("staging_dir", "/tmp/ota_staging"); + install_dir_ = config.value("install_dir", "/ws/install"); + // Where this plugin drops manifest fragments for OTA-installed apps. + // Must equal the path the gateway has configured under + // discovery.manifest.fragments_dir, otherwise the gateway won't pick + // them up on reload. Empty disables fragment writes (legacy behavior: + // installed nodes appear as orphans in the entity tree). + fragments_dir_ = config.value("fragments_dir", ""); + if (!catalog_client_) { + catalog_client_ = std::make_unique(catalog_url_); + } +} + +void OtaUpdatePlugin::set_context(ros2_medkit_gateway::PluginContext & context) { + // Hold on to the context so post-execute we can ask the gateway to + // re-merge manifest fragments and rerun discovery via + // notify_entities_changed. + context_ = &context; + poll_and_register_catalog(); +} + +void OtaUpdatePlugin::poll_and_register_catalog() { + auto fetched = catalog_client_->fetch_catalog(); + if (!fetched) { + std::fprintf(stderr, "[ota_update_plugin] catalog fetch failed: %s\n", fetched.error().c_str()); + return; + } + if (!fetched->is_array()) { + std::fprintf(stderr, "[ota_update_plugin] catalog payload is not an array\n"); + return; + } + for (const auto & entry : *fetched) { + auto rc = register_update(entry); + if (!rc) { + const std::string id = entry.value("id", "?"); + std::fprintf(stderr, "[ota_update_plugin] register %s failed: %s\n", id.c_str(), rc.error().message.c_str()); + } + } +} + +void OtaUpdatePlugin::set_catalog_client_for_test(std::unique_ptr client) { + catalog_client_ = std::move(client); +} + +void OtaUpdatePlugin::set_process_runner_for_test(std::unique_ptr runner) { + process_runner_ = std::move(runner); +} + +tl::expected, UpdateBackendErrorInfo> OtaUpdatePlugin::list_updates( + const ros2_medkit_gateway::UpdateFilter & /*filter*/) { + std::lock_guard lk(mu_); + std::vector ids; + ids.reserve(registry_.size()); + for (const auto & kv : registry_) { + ids.push_back(kv.first); + } + return ids; +} + +tl::expected OtaUpdatePlugin::get_update( + const std::string & id) { + std::lock_guard lk(mu_); + auto it = registry_.find(id); + if (it == registry_.end()) { + return tl::make_unexpected(UpdateBackendErrorInfo{UpdateBackendError::NotFound, "update not registered"}); + } + return ros2_medkit_gateway::dto::UpdateDetail{it->second}; +} + +tl::expected OtaUpdatePlugin::register_update(const nlohmann::json & metadata) { + if (!metadata.contains("id") || !metadata["id"].is_string()) { + return tl::make_unexpected(UpdateBackendErrorInfo{UpdateBackendError::InvalidInput, "metadata missing id"}); + } + std::lock_guard lk(mu_); + registry_[metadata["id"].get()] = metadata; + return {}; +} + +tl::expected OtaUpdatePlugin::delete_update(const std::string & id) { + std::lock_guard lk(mu_); + registry_.erase(id); + staged_artifacts_.erase(id); + return {}; +} + +tl::expected OtaUpdatePlugin::prepare( + const std::string & id, ros2_medkit_gateway::UpdateProgressReporter & reporter) { + nlohmann::json metadata; + { + std::lock_guard lk(mu_); + auto it = registry_.find(id); + if (it == registry_.end()) { + return tl::make_unexpected(UpdateBackendErrorInfo{UpdateBackendError::NotFound, "no such update"}); + } + metadata = it->second; + } + + const auto kind = OperationDispatcher::classify(metadata); + if (kind == OperationKind::Unknown) { + return tl::make_unexpected(UpdateBackendErrorInfo{ + UpdateBackendError::InvalidInput, + "update package must populate exactly one of " + "updated_components / added_components / removed_components"}); + } + + if (kind == OperationKind::Uninstall) { + reporter.set_progress(100); + return {}; + } + + if (!metadata.contains("x_medkit_artifact_url") || !metadata["x_medkit_artifact_url"].is_string()) { + return tl::make_unexpected( + UpdateBackendErrorInfo{UpdateBackendError::InvalidInput, "missing x_medkit_artifact_url"}); + } + + std::error_code ec; + fs::create_directories(staging_dir_, ec); + const std::string url = metadata["x_medkit_artifact_url"].get(); + const std::string staged_path = staging_dir_ + "/" + id + ".tar.gz"; + + reporter.set_progress(10); + auto dl = catalog_client_->download_artifact(url, staged_path); + if (!dl) { + return tl::make_unexpected(UpdateBackendErrorInfo{UpdateBackendError::Internal, "download failed: " + dl.error()}); + } + reporter.set_progress(80); + + { + std::lock_guard lk(mu_); + staged_artifacts_[id] = *dl; + } + reporter.set_progress(100); + return {}; +} + +tl::expected OtaUpdatePlugin::execute( + const std::string & id, ros2_medkit_gateway::UpdateProgressReporter & reporter) { + nlohmann::json metadata; + std::string staged; + { + std::lock_guard lk(mu_); + auto it = registry_.find(id); + if (it == registry_.end()) { + return tl::make_unexpected(UpdateBackendErrorInfo{UpdateBackendError::NotFound, "no such update"}); + } + metadata = it->second; + auto sit = staged_artifacts_.find(id); + staged = (sit != staged_artifacts_.end()) ? sit->second : ""; + } + + const auto kind = OperationDispatcher::classify(metadata); + const std::string target_package = metadata.value("x_medkit_target_package", ""); + const std::string executable = metadata.value("x_medkit_executable", ""); + + if (kind == OperationKind::Update) { + if (staged.empty()) { + return tl::make_unexpected(UpdateBackendErrorInfo{UpdateBackendError::InvalidInput, "call prepare() first"}); + } + if (executable.empty()) { + return tl::make_unexpected( + UpdateBackendErrorInfo{UpdateBackendError::InvalidInput, "missing x_medkit_executable"}); + } + // For an update across packages (e.g. broken_lidar -> fixed_lidar) the + // OLD process binary lives in a different package than the NEW one we + // are about to spawn, so its basename differs from `executable`. Honor + // x_medkit_replaces_executable when present, fall back to executable. + const std::string kill_target = metadata.value("x_medkit_replaces_executable", executable); + reporter.set_progress(20); + auto kr = process_runner_->kill_by_executable(kill_target); + if (!kr) { + return tl::make_unexpected(UpdateBackendErrorInfo{UpdateBackendError::Internal, "kill failed: " + kr.error()}); + } + reporter.set_progress(40); + if (auto sw = extract_and_swap(staged, install_dir_, target_package); !sw) { + return tl::make_unexpected(UpdateBackendErrorInfo{UpdateBackendError::Internal, "swap failed: " + sw.error()}); + } + reporter.set_progress(70); + const std::string bin = install_dir_ + "/" + target_package + "/lib/" + target_package + "/" + executable; + auto sp = process_runner_->spawn(bin); + if (!sp) { + return tl::make_unexpected(UpdateBackendErrorInfo{UpdateBackendError::Internal, "spawn failed: " + sp.error()}); + } + // Update flow: same app id (the binary swapped in is bound to the + // same scan_sensor_node entity as the binary it replaced) - no + // manifest fragment to write, but the gateway still needs to + // rerun discovery so the new pid / process metadata replaces the + // stale entries in the entity cache. + notify_manifest_changed(); + reporter.set_progress(100); + return {}; + } + + if (kind == OperationKind::Install) { + if (staged.empty()) { + return tl::make_unexpected(UpdateBackendErrorInfo{UpdateBackendError::InvalidInput, "call prepare() first"}); + } + if (executable.empty()) { + return tl::make_unexpected( + UpdateBackendErrorInfo{UpdateBackendError::InvalidInput, "missing x_medkit_executable"}); + } + reporter.set_progress(30); + if (auto sw = extract_and_swap(staged, install_dir_, target_package); !sw) { + return tl::make_unexpected(UpdateBackendErrorInfo{UpdateBackendError::Internal, "swap failed: " + sw.error()}); + } + reporter.set_progress(70); + const std::string bin = install_dir_ + "/" + target_package + "/lib/" + target_package + "/" + executable; + auto sp = process_runner_->spawn(bin); + if (!sp) { + return tl::make_unexpected(UpdateBackendErrorInfo{UpdateBackendError::Internal, "spawn failed: " + sp.error()}); + } + // Install flow: NEW app entity. Write a manifest fragment so the + // gateway picks the new app up under the manifest tree (otherwise + // it stays as an "Orphan node (not in manifest)" warn log and + // never appears under the rbtheron component / Functions + // listing). Notify even when fragment write fails - the spawn + // already happened and discovery should still see the new node. + if (auto fr = write_install_fragment(id, metadata); !fr) { + std::fprintf(stderr, "[ota_update_plugin] fragment write failed for %s: %s\n", id.c_str(), + fr.error().c_str()); + } + notify_manifest_changed(); + reporter.set_progress(100); + return {}; + } + + if (kind == OperationKind::Uninstall) { + reporter.set_progress(30); + if (!target_package.empty()) { + // Best-effort kill: legacy nodes may use the package name as their executable basename. + // Failures are tolerated since the process may already be gone. + auto kr = process_runner_->kill_by_executable(target_package); + (void)kr; + reporter.set_progress(70); + std::error_code ec; + fs::remove_all(install_dir_ + "/" + target_package, ec); + } + // Uninstall: drop any fragment we wrote at install time and rerun + // discovery so the entity tree no longer lists the now-dead app. + // Entities defined in the base manifest stay - fragments only + // ADD, they can't remove base-manifest declarations - those + // entries just go offline. + if (auto fr = remove_install_fragment(id); !fr) { + std::fprintf(stderr, "[ota_update_plugin] fragment remove failed for %s: %s\n", id.c_str(), + fr.error().c_str()); + } + notify_manifest_changed(); + reporter.set_progress(100); + return {}; + } + + return tl::make_unexpected(UpdateBackendErrorInfo{UpdateBackendError::Internal, "unknown operation kind"}); +} + +tl::expected OtaUpdatePlugin::supports_automated(const std::string & /*id*/) { + return false; +} + +namespace { + +// Build the YAML body for a single OTA-installed app. We hand-emit the +// minimal subset the gateway's manifest parser accepts (no quoting +// edge cases in our generated values, so a yaml-cpp roundtrip would be +// overkill). The base manifest defines the `rbtheron` component; +// fragments only ever add apps onto it. +std::string render_install_fragment(const std::string & app_id, const std::string & node_name, + const std::string & description) { + std::string out; + out += "manifest_version: \"1.0\"\n"; + out += "apps:\n"; + out += " - id: " + app_id + "\n"; + out += " name: \"" + app_id + "\"\n"; + out += " category: \"ota-installed\"\n"; + out += " is_located_on: rbtheron\n"; + out += " description: \"" + description + "\"\n"; + out += " ros_binding: { node_name: " + node_name + ", namespace: / }\n"; + return out; +} + +} // namespace + +tl::expected OtaUpdatePlugin::write_install_fragment(const std::string & update_id, + const nlohmann::json & metadata) { + if (fragments_dir_.empty()) return {}; + + const std::string node_name = metadata.value("x_medkit_executable", ""); + // SOVD ISO 17978-3 reports the target entity via `added_components` + // (it's an array; for an OTA install we always have exactly one). + std::string app_id; + if (metadata.contains("added_components") && metadata["added_components"].is_array() + && !metadata["added_components"].empty()) { + const auto & first = metadata["added_components"][0]; + if (!first.is_string()) { + return tl::make_unexpected("added_components[0] is not a string"); + } + app_id = first.get(); + } + if (node_name.empty() || app_id.empty()) { + return tl::make_unexpected( + "metadata missing x_medkit_executable / added_components for fragment"); + } + const std::string description = "OTA-installed via " + update_id; + + std::error_code ec; + fs::create_directories(fragments_dir_, ec); + if (ec) { + return tl::make_unexpected("create fragments_dir failed: " + ec.message()); + } + + const std::string final_path = fragments_dir_ + "/" + update_id + ".yaml"; + const std::string tmp_path = fragments_dir_ + "/.tmp-" + update_id + ".yaml"; + // Atomic publish per ManifestManager's fragment contract: write to + // tmp, fsync, rename. The gateway's fragment scanner runs on the + // notify_entities_changed thread - a half-written file would fail + // the manifest reload and roll back the entire merge. + { + std::ofstream f(tmp_path, std::ios::binary | std::ios::trunc); + if (!f) return tl::make_unexpected("open tmp fragment failed: " + tmp_path); + f << render_install_fragment(app_id, node_name, description); + f.flush(); + if (!f) return tl::make_unexpected("write tmp fragment failed: " + tmp_path); + } + if (std::rename(tmp_path.c_str(), final_path.c_str()) != 0) { + return tl::make_unexpected("rename fragment failed: " + std::string(std::strerror(errno))); + } + return {}; +} + +tl::expected OtaUpdatePlugin::remove_install_fragment(const std::string & update_id) { + if (fragments_dir_.empty()) return {}; + std::error_code ec; + fs::remove(fragments_dir_ + "/" + update_id + ".yaml", ec); + // Missing-file is fine (uninstall of an entity that lived in the + // base manifest, never had a fragment); other errors are reported. + if (ec && ec != std::errc::no_such_file_or_directory) { + return tl::make_unexpected("remove fragment failed: " + ec.message()); + } + return {}; +} + +void OtaUpdatePlugin::notify_manifest_changed() { + if (!context_) return; + context_->notify_entities_changed(ros2_medkit_gateway::EntityChangeScope::full_refresh()); +} + +} // namespace ota_update_plugin diff --git a/demos/ota_nav2_sensor_fix/ota_update_plugin/src/plugin_exports.cpp b/demos/ota_nav2_sensor_fix/ota_update_plugin/src/plugin_exports.cpp new file mode 100644 index 0000000..d71b05d --- /dev/null +++ b/demos/ota_nav2_sensor_fix/ota_update_plugin/src/plugin_exports.cpp @@ -0,0 +1,33 @@ +// Copyright 2026 bburda +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "ros2_medkit_gateway/core/plugins/plugin_types.hpp" + +#include "ota_update_plugin/ota_update_plugin.hpp" + +extern "C" GATEWAY_PLUGIN_EXPORT int plugin_api_version() { + return ros2_medkit_gateway::PLUGIN_API_VERSION; +} + +extern "C" GATEWAY_PLUGIN_EXPORT ros2_medkit_gateway::GatewayPlugin * create_plugin() { + return new ota_update_plugin::OtaUpdatePlugin(); +} + +// Explicit cross-cast so the gateway's plugin_loader can resolve the +// UpdateProvider interface without relying on dynamic_cast across the +// dlopen boundary (which is fragile when typeinfo isn't shared). +extern "C" GATEWAY_PLUGIN_EXPORT ros2_medkit_gateway::UpdateProvider * +get_update_provider(ros2_medkit_gateway::GatewayPlugin * plugin) { + return dynamic_cast(plugin); +} diff --git a/demos/ota_nav2_sensor_fix/ota_update_plugin/src/process_runner.cpp b/demos/ota_nav2_sensor_fix/ota_update_plugin/src/process_runner.cpp new file mode 100644 index 0000000..44118f0 --- /dev/null +++ b/demos/ota_nav2_sensor_fix/ota_update_plugin/src/process_runner.cpp @@ -0,0 +1,292 @@ +// Copyright 2026 bburda +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "process_runner.hpp" + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace ota_update_plugin { + +namespace { + +// /proc//comm is truncated to 15 characters by the kernel, which causes +// false negatives for any executable whose basename is longer (e.g. +// "broken_lidar_node" -> "broken_lidar_no"). Read /proc//cmdline +// instead - its first NUL-separated arg holds the full path / argv[0]. +std::string proc_cmdline_arg0(int pid) { + std::ifstream f("/proc/" + std::to_string(pid) + "/cmdline", std::ios::binary); + if (!f) { + return {}; + } + std::string buf((std::istreambuf_iterator(f)), std::istreambuf_iterator()); + if (buf.empty()) { + return {}; + } + // argv[0] runs to the first NUL. + const auto nul = buf.find('\0'); + std::string arg0 = (nul == std::string::npos) ? buf : buf.substr(0, nul); + // Take the basename so callers pass executable_basename without a path. + const auto slash = arg0.rfind('/'); + return (slash == std::string::npos) ? arg0 : arg0.substr(slash + 1); +} + +bool is_pid_dir(const char * name) { + for (const char * p = name; *p; ++p) { + if (*p < '0' || *p > '9') { + return false; + } + } + return *name != '\0'; +} + +// Read exactly `len` bytes from `fd`, retrying on EINTR and short reads. +// Returns the number of bytes actually read: `len` on a full read, a smaller +// count (including 0) if EOF is hit first, or -1 on a read() error other than +// EINTR. Used by the parent side of spawn()'s two status pipes, where a +// short/zero read from `pid_fds` (the *first* pipe) is itself an error +// condition (the intermediate child exited without reporting), while a +// short/zero read from `err_fds` (the *second* pipe) is the success signal +// (EOF via O_CLOEXEC). +ssize_t read_exact(int fd, void * buf, size_t len) { + size_t total = 0; + auto * p = static_cast(buf); + while (total < len) { + const ssize_t n = ::read(fd, p + total, len - total); + if (n < 0) { + if (errno == EINTR) { + continue; + } + return -1; + } + if (n == 0) { + break; // EOF + } + total += static_cast(n); + } + return static_cast(total); +} + +// Best-effort write of `len` bytes to `fd`, retrying on EINTR and short +// writes. Called from a child right before `_exit()`, so there is nothing +// actionable to do on failure - the parent's read side already treats a +// missing/short read as an error, so a failed write here cannot manifest as +// a silent false success. +void write_best_effort(int fd, const void * buf, size_t len) { + const auto * p = static_cast(buf); + size_t total = 0; + while (total < len) { + const ssize_t n = ::write(fd, p + total, len - total); + if (n < 0) { + if (errno == EINTR) { + continue; + } + return; + } + if (n == 0) { + return; + } + total += static_cast(n); + } +} + +} // namespace + +std::vector ProcessRunner::pgrep(const std::string & executable_basename) { + std::vector out; + DIR * d = opendir("/proc"); + if (d == nullptr) { + return out; + } + while (auto * ent = readdir(d)) { + if (!is_pid_dir(ent->d_name)) { + continue; + } + const int pid = std::atoi(ent->d_name); + if (pid <= 0) { + continue; + } + if (proc_cmdline_arg0(pid) == executable_basename) { + out.push_back(pid); + } + } + closedir(d); + return out; +} + +tl::expected ProcessRunner::kill_by_executable(const std::string & executable_basename, + int timeout_ms) { + const auto pids = pgrep(executable_basename); + int signalled = 0; + for (int pid : pids) { + if (::kill(pid, SIGTERM) == 0) { + ++signalled; + } + } + if (signalled == 0) { + return 0; + } + + // Poll for exit. + const auto deadline = std::chrono::steady_clock::now() + std::chrono::milliseconds(timeout_ms); + while (std::chrono::steady_clock::now() < deadline) { + bool any_alive = false; + for (int pid : pids) { + if (::kill(pid, 0) == 0) { + any_alive = true; + break; + } + } + if (!any_alive) { + return signalled; + } + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + } + // Force-kill stragglers. + for (int pid : pids) { + if (::kill(pid, 0) == 0) { + ::kill(pid, SIGKILL); + } + } + return signalled; +} + +tl::expected ProcessRunner::spawn(const std::string & executable_path) { + // Double-fork so the grandchild is reparented to init and never becomes a + // zombie in the gateway process. The intermediate child exits immediately + // and is reaped here. + // + // Two separate status pipes carry the outcome back to the parent, each + // with exactly one writer, so fixed-offset reads can never be misordered: + // - pid_fds: the intermediate child writes the grandchild's pid, then + // exits. The grandchild never writes to this pipe. + // - err_fds: the grandchild writes its errno here ONLY if execl fails, + // then exits 127. On successful exec, its O_CLOEXEC copy of the write + // end closes automatically, so the parent's read sees EOF. The + // intermediate child never writes to this pipe. + // (A single shared pipe for both messages was tried before and reverted: + // both writes are the same size (4 bytes), so nothing guarantees the pid + // arrives before the errno on a very fast exec failure - the parent's two + // fixed-offset reads could then swap them, turning a real errno into a + // bogus strerror() of a pid.) + int pid_fds[2]; + if (::pipe2(pid_fds, O_CLOEXEC) != 0) { + return tl::make_unexpected(std::string("pipe2 failed: ") + std::strerror(errno)); + } + int err_fds[2]; + if (::pipe2(err_fds, O_CLOEXEC) != 0) { + const int err = errno; + ::close(pid_fds[0]); + ::close(pid_fds[1]); + return tl::make_unexpected(std::string("pipe2 failed: ") + std::strerror(err)); + } + + pid_t pid = fork(); + if (pid < 0) { + const int err = errno; + ::close(pid_fds[0]); + ::close(pid_fds[1]); + ::close(err_fds[0]); + ::close(err_fds[1]); + return tl::make_unexpected(std::string("fork failed: ") + std::strerror(err)); + } + if (pid == 0) { + // Intermediate child: only ever writes to pid_fds. Close the read ends + // of both pipes now - it needs neither. err_fds[1] must stay open + // across the second fork() so the grandchild inherits it (that's the + // only way the grandchild gets a copy to write its errno to); the + // intermediate itself never writes to err_fds and its own copy closes + // automatically when it `_exit`s below, without a race (the parent + // `waitpid`s the intermediate before reading either pipe). + ::close(pid_fds[0]); + ::close(err_fds[0]); + pid_t grandchild = fork(); + if (grandchild < 0) { + _exit(126); + } + if (grandchild == 0) { + // Grandchild: only ever writes to err_fds (and only on exec failure), + // so drop pid_fds's write end - it must never write the pid pipe. + ::close(pid_fds[1]); + setsid(); + // Forward use_sim_time so the spawned node aligns with the + // gateway's clock domain. Without this, a node started by OTA + // (post-update fixed_lidar) runs on wall time while the rest of + // the stack runs on /clock from gz-sim - its /scan / /diagnostics + // timestamps fall outside nav2's TF buffer and the costmap drops + // every message: "the timestamp on the message is earlier than + // all the data in the transform cache". Robot stops responding. + // + // Note: this is the minimum viable param plumbing. A full + // production plugin should plumb arbitrary parameters from + // the catalog entry through to execve. + execl(executable_path.c_str(), + executable_path.c_str(), + "--ros-args", + "-p", "use_sim_time:=true", + static_cast(nullptr)); + const int err = errno; + std::fprintf(stderr, "execl %s failed: %s\n", executable_path.c_str(), std::strerror(err)); + write_best_effort(err_fds[1], &err, sizeof(err)); + _exit(127); + } + write_best_effort(pid_fds[1], &grandchild, sizeof(grandchild)); + _exit(0); + } + + // Parent: drop both write ends immediately - if either stayed open here, + // its pipe could never signal EOF (the read below would block forever + // even after both children released their copies). + ::close(pid_fds[1]); + ::close(err_fds[1]); + int status = 0; + ::waitpid(pid, &status, 0); + + pid_t grandchild_pid = -1; + const ssize_t pid_bytes = read_exact(pid_fds[0], &grandchild_pid, sizeof(grandchild_pid)); + ::close(pid_fds[0]); + if (pid_bytes != static_cast(sizeof(grandchild_pid))) { + ::close(err_fds[0]); + return tl::make_unexpected( + std::string("spawn failed: intermediate child exited without reporting a pid " + "(second fork likely failed)")); + } + + int exec_errno = 0; + const ssize_t err_bytes = read_exact(err_fds[0], &exec_errno, sizeof(exec_errno)); + ::close(err_fds[0]); + if (err_bytes == 0) { + // EOF: the grandchild's copy of the write end closed (via O_CLOEXEC) + // without reporting an errno - exec succeeded. + return static_cast(grandchild_pid); + } + if (err_bytes == static_cast(sizeof(exec_errno))) { + return tl::make_unexpected(std::string("execl failed: ") + std::strerror(exec_errno)); + } + return tl::make_unexpected(std::string("spawn failed: unexpected status pipe read")); +} + +} // namespace ota_update_plugin diff --git a/demos/ota_nav2_sensor_fix/ota_update_plugin/src/process_runner.hpp b/demos/ota_nav2_sensor_fix/ota_update_plugin/src/process_runner.hpp new file mode 100644 index 0000000..70e88cb --- /dev/null +++ b/demos/ota_nav2_sensor_fix/ota_update_plugin/src/process_runner.hpp @@ -0,0 +1,48 @@ +// Copyright 2026 bburda +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include +#include + +#include + +namespace ota_update_plugin { + +/// Process management helper for OTA operations: locate, terminate, and spawn +/// demo nodes by executable basename. Pure-virtual for test substitution. +class ProcessRunner { + public: + ProcessRunner() = default; + virtual ~ProcessRunner() = default; + + ProcessRunner(const ProcessRunner &) = delete; + ProcessRunner & operator=(const ProcessRunner &) = delete; + ProcessRunner(ProcessRunner &&) = delete; + ProcessRunner & operator=(ProcessRunner &&) = delete; + + /// Find PIDs of processes whose /proc//comm matches the given basename. + virtual std::vector pgrep(const std::string & executable_basename); + + /// Send SIGTERM to all matching PIDs, wait up to `timeout_ms` for exit, then + /// SIGKILL any stragglers. Returns the number of processes that were signalled. + virtual tl::expected kill_by_executable(const std::string & executable_basename, + int timeout_ms = 2000); + + /// fork+exec the executable at `executable_path`. Returns child PID or error. + virtual tl::expected spawn(const std::string & executable_path); +}; + +} // namespace ota_update_plugin diff --git a/demos/ota_nav2_sensor_fix/ota_update_plugin/test/test_catalog_client.cpp b/demos/ota_nav2_sensor_fix/ota_update_plugin/test/test_catalog_client.cpp new file mode 100644 index 0000000..2a671e3 --- /dev/null +++ b/demos/ota_nav2_sensor_fix/ota_update_plugin/test/test_catalog_client.cpp @@ -0,0 +1,59 @@ +// Copyright 2026 bburda +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include + +#include "catalog_client.hpp" + +using ota_update_plugin::parse_url; + +TEST(ParseUrl, HostAndPort) { + auto p = parse_url("http://server:9000"); + EXPECT_EQ(p.host, "server"); + EXPECT_EQ(p.port, 9000); + EXPECT_FALSE(p.tls); + EXPECT_EQ(p.path, "/"); +} + +TEST(ParseUrl, PathSplit) { + auto p = parse_url("http://server:9000/catalog"); + EXPECT_EQ(p.host, "server"); + EXPECT_EQ(p.port, 9000); + EXPECT_EQ(p.path, "/catalog"); +} + +TEST(ParseUrl, DefaultsHttpPort) { + auto p = parse_url("http://server/catalog"); + EXPECT_EQ(p.host, "server"); + EXPECT_EQ(p.port, 80); + EXPECT_FALSE(p.tls); + EXPECT_EQ(p.path, "/catalog"); +} + +TEST(ParseUrl, HttpsTls) { + auto p = parse_url("https://server/catalog"); + EXPECT_TRUE(p.tls); + EXPECT_EQ(p.port, 443); + EXPECT_EQ(p.path, "/catalog"); +} + +TEST(ParseUrl, RejectsInvalidScheme) { + EXPECT_THROW(parse_url("ftp://server/foo"), std::invalid_argument); +} + +TEST(ParseUrl, RejectsMissingHost) { + EXPECT_THROW(parse_url("http://:9000/foo"), std::invalid_argument); +} diff --git a/demos/ota_nav2_sensor_fix/ota_update_plugin/test/test_operation_dispatcher.cpp b/demos/ota_nav2_sensor_fix/ota_update_plugin/test/test_operation_dispatcher.cpp new file mode 100644 index 0000000..fd22109 --- /dev/null +++ b/demos/ota_nav2_sensor_fix/ota_update_plugin/test/test_operation_dispatcher.cpp @@ -0,0 +1,60 @@ +// Copyright 2026 bburda +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include "operation_dispatcher.hpp" + +using ota_update_plugin::OperationDispatcher; +using ota_update_plugin::OperationKind; + +TEST(OperationDispatcher, UpdateFromUpdatedComponents) { + nlohmann::json m = {{"id", "x"}, {"updated_components", {"scan_sensor_node"}}}; + EXPECT_EQ(OperationDispatcher::classify(m), OperationKind::Update); +} + +TEST(OperationDispatcher, InstallFromAddedComponents) { + nlohmann::json m = {{"id", "x"}, {"added_components", {"demo_addon"}}}; + EXPECT_EQ(OperationDispatcher::classify(m), OperationKind::Install); +} + +TEST(OperationDispatcher, UninstallFromRemovedComponents) { + nlohmann::json m = {{"id", "x"}, {"removed_components", {"deprecated_pkg"}}}; + EXPECT_EQ(OperationDispatcher::classify(m), OperationKind::Uninstall); +} + +TEST(OperationDispatcher, UnknownWhenAllEmpty) { + nlohmann::json m = {{"id", "x"}}; + EXPECT_EQ(OperationDispatcher::classify(m), OperationKind::Unknown); +} + +TEST(OperationDispatcher, UnknownWhenMixed) { + nlohmann::json m = { + {"id", "x"}, + {"added_components", {"a"}}, + {"removed_components", {"b"}}, + }; + EXPECT_EQ(OperationDispatcher::classify(m), OperationKind::Unknown); +} + +TEST(OperationDispatcher, UnknownWhenComponentsAreEmptyArray) { + nlohmann::json m = {{"id", "x"}, {"updated_components", nlohmann::json::array()}}; + EXPECT_EQ(OperationDispatcher::classify(m), OperationKind::Unknown); +} + +TEST(OperationDispatcher, UnknownWhenComponentsIsNotArray) { + nlohmann::json m = {{"id", "x"}, {"updated_components", "scan_sensor_node"}}; + EXPECT_EQ(OperationDispatcher::classify(m), OperationKind::Unknown); +} diff --git a/demos/ota_nav2_sensor_fix/ota_update_plugin/test/test_plugin_smoke.cpp b/demos/ota_nav2_sensor_fix/ota_update_plugin/test/test_plugin_smoke.cpp new file mode 100644 index 0000000..ccd3cac --- /dev/null +++ b/demos/ota_nav2_sensor_fix/ota_update_plugin/test/test_plugin_smoke.cpp @@ -0,0 +1,237 @@ +// Copyright 2026 bburda +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include +#include + +#include + +#include + +#include "catalog_client.hpp" +#include "ota_update_plugin/ota_update_plugin.hpp" +#include "process_runner.hpp" + +namespace { + +class FakeCatalogClient : public ota_update_plugin::CatalogClient { + public: + using CatalogClient::CatalogClient; + + nlohmann::json catalog_payload = nlohmann::json::array(); + std::string artifact_to_return = "TARDATA"; + std::string requested_url; + + tl::expected fetch_catalog() override { + return catalog_payload; + } + + tl::expected download_artifact(const std::string & url, const std::string & out) override { + requested_url = url; + std::ofstream o(out, std::ios::binary); + o << artifact_to_return; + return out; + } +}; + +/// ProcessRunner stub: records the basename passed to kill_by_executable so a +/// test can verify the plugin honors x_medkit_replaces_executable. Returns 0 +/// signalled processes (no-op kill) and an error from spawn so execute() halts +/// before touching the (nonexistent) install dir. +class RecordingProcessRunner : public ota_update_plugin::ProcessRunner { + public: + std::string last_kill_target; + + std::vector pgrep(const std::string & /*executable_basename*/) override { + return {}; + } + + tl::expected kill_by_executable(const std::string & executable_basename, + int /*timeout_ms*/ = 2000) override { + last_kill_target = executable_basename; + return 0; + } + + tl::expected spawn(const std::string & /*executable_path*/) override { + return tl::make_unexpected(std::string("stub: spawn intentionally not implemented")); + } +}; + +ros2_medkit_gateway::UpdateProgressReporter make_reporter(ros2_medkit_gateway::UpdateStatusInfo & info, + std::mutex & mu) { + return ros2_medkit_gateway::UpdateProgressReporter(info, mu); +} + +} // namespace + +TEST(OtaUpdatePluginSmoke, NameAndConstructible) { + ota_update_plugin::OtaUpdatePlugin plugin; + EXPECT_EQ(plugin.name(), "ota_update_plugin"); +} + +// Exercises the real ProcessRunner (not the RecordingProcessRunner stub) +// through the actual double-fork + status pipe in process_runner.cpp. +// Before the pipe2/errno fix, spawn() unconditionally returned the +// (already-reaped) intermediate child's pid regardless of whether execl +// succeeded, so this exact case - a path that cannot exist - reported +// success with a pid that was never running. +TEST(ProcessRunnerSpawn, NonexistentExecutableReturnsError) { + ota_update_plugin::ProcessRunner runner; + auto rc = runner.spawn("/nonexistent/path/does-not-exist-ota-demo"); + ASSERT_FALSE(rc); + EXPECT_NE(rc.error().find("execl failed"), std::string::npos) << "unexpected error message: " << rc.error(); +} + +TEST(OtaUpdatePluginSmoke, RegisterListGet) { + ota_update_plugin::OtaUpdatePlugin plugin; + nlohmann::json md = {{"id", "u1"}, {"updated_components", {"x"}}}; + ASSERT_TRUE(plugin.register_update(md)); + auto ids = plugin.list_updates({}); + ASSERT_TRUE(ids); + ASSERT_EQ(ids->size(), 1u); + EXPECT_EQ((*ids)[0], "u1"); + auto got = plugin.get_update("u1"); + ASSERT_TRUE(got); + EXPECT_EQ((*got).content["id"], "u1"); +} + +TEST(OtaUpdatePluginSmoke, RegisterRequiresId) { + ota_update_plugin::OtaUpdatePlugin plugin; + auto rc = plugin.register_update(nlohmann::json::object()); + EXPECT_FALSE(rc); + EXPECT_EQ(rc.error().code, ros2_medkit_gateway::UpdateBackendError::InvalidInput); +} + +TEST(OtaUpdatePluginSmoke, GetUpdateReturnsNotFoundForUnknownId) { + ota_update_plugin::OtaUpdatePlugin plugin; + auto got = plugin.get_update("does-not-exist"); + ASSERT_FALSE(got); + EXPECT_EQ(got.error().code, ros2_medkit_gateway::UpdateBackendError::NotFound); +} + +TEST(OtaUpdatePluginSmoke, DeleteRemovesEntry) { + ota_update_plugin::OtaUpdatePlugin plugin; + ASSERT_TRUE(plugin.register_update({{"id", "to-delete"}, {"updated_components", {"x"}}})); + ASSERT_TRUE(plugin.delete_update("to-delete")); + auto got = plugin.get_update("to-delete"); + EXPECT_FALSE(got); +} + +TEST(OtaUpdatePluginSmoke, BootPollPopulates) { + ota_update_plugin::OtaUpdatePlugin plugin; + plugin.configure(nlohmann::json::object()); + auto fake = std::make_unique("http://x"); + fake->catalog_payload = nlohmann::json::array({ + {{"id", "a"}, + {"updated_components", {"scan"}}, + {"x_medkit_artifact_url", "/artifacts/a.tgz"}, + {"x_medkit_target_package", "a"}}, + }); + plugin.set_catalog_client_for_test(std::move(fake)); + plugin.poll_and_register_catalog(); + + auto ids = plugin.list_updates({}); + ASSERT_TRUE(ids); + ASSERT_EQ(ids->size(), 1u); + EXPECT_EQ((*ids)[0], "a"); +} + +TEST(OtaUpdatePluginSmoke, PrepareRejectsUnknownOperationKind) { + ota_update_plugin::OtaUpdatePlugin plugin; + ASSERT_TRUE(plugin.register_update({{"id", "bad"}})); + ros2_medkit_gateway::UpdateStatusInfo info; + std::mutex mu; + auto reporter = make_reporter(info, mu); + auto rc = plugin.prepare("bad", reporter); + ASSERT_FALSE(rc); + EXPECT_EQ(rc.error().code, ros2_medkit_gateway::UpdateBackendError::InvalidInput); +} + +TEST(OtaUpdatePluginSmoke, PrepareUninstallSkipsDownload) { + ota_update_plugin::OtaUpdatePlugin plugin; + plugin.configure(nlohmann::json::object()); + // No download should happen for uninstall, but provide a fake just in case. + auto fake = std::make_unique("http://x"); + plugin.set_catalog_client_for_test(std::move(fake)); + ASSERT_TRUE(plugin.register_update({{"id", "rm"}, {"removed_components", {"legacy"}}})); + + ros2_medkit_gateway::UpdateStatusInfo info; + std::mutex mu; + auto reporter = make_reporter(info, mu); + auto rc = plugin.prepare("rm", reporter); + EXPECT_TRUE(rc); + EXPECT_EQ(info.progress.value_or(-1), 100); +} + +TEST(OtaUpdatePluginSmoke, ExecuteUpdateUsesReplacesExecutableForKill) { + ota_update_plugin::OtaUpdatePlugin plugin; + plugin.configure({{"staging_dir", ::testing::TempDir() + "/replaces_test"}}); + plugin.set_catalog_client_for_test(std::make_unique("http://x")); + auto runner = std::make_unique(); + RecordingProcessRunner * runner_raw = runner.get(); + plugin.set_process_runner_for_test(std::move(runner)); + + // Update entry with separate old + new executable basenames. + ASSERT_TRUE(plugin.register_update({ + {"id", "u_replaces"}, + {"updated_components", {"scan_sensor_node"}}, + {"x_medkit_artifact_url", "/artifacts/fixed.tgz"}, + {"x_medkit_target_package", "fixed_lidar"}, + {"x_medkit_executable", "fixed_lidar_node"}, + {"x_medkit_replaces_executable", "broken_lidar_node"}, + })); + + ros2_medkit_gateway::UpdateStatusInfo info; + std::mutex mu; + auto reporter = make_reporter(info, mu); + ASSERT_TRUE(plugin.prepare("u_replaces", reporter)); + + // execute() will fail at extract_and_swap (the staged tarball is not a real + // gzipped archive) but the kill step runs first - that is what we are + // checking here. + auto rc = plugin.execute("u_replaces", reporter); + (void)rc; + EXPECT_EQ(runner_raw->last_kill_target, "broken_lidar_node"); +} + +TEST(OtaUpdatePluginSmoke, ExecuteUpdateFallsBackToExecutableWhenReplacesMissing) { + ota_update_plugin::OtaUpdatePlugin plugin; + plugin.configure({{"staging_dir", ::testing::TempDir() + "/replaces_fallback"}}); + plugin.set_catalog_client_for_test(std::make_unique("http://x")); + auto runner = std::make_unique(); + RecordingProcessRunner * runner_raw = runner.get(); + plugin.set_process_runner_for_test(std::move(runner)); + + // Update entry without x_medkit_replaces_executable - kill should target + // the same name as x_medkit_executable. + ASSERT_TRUE(plugin.register_update({ + {"id", "u_no_replaces"}, + {"updated_components", {"scan_sensor_node"}}, + {"x_medkit_artifact_url", "/artifacts/scan.tgz"}, + {"x_medkit_target_package", "scan_pkg"}, + {"x_medkit_executable", "scan_node"}, + })); + + ros2_medkit_gateway::UpdateStatusInfo info; + std::mutex mu; + auto reporter = make_reporter(info, mu); + ASSERT_TRUE(plugin.prepare("u_no_replaces", reporter)); + + auto rc = plugin.execute("u_no_replaces", reporter); + (void)rc; + EXPECT_EQ(runner_raw->last_kill_target, "scan_node"); +} diff --git a/demos/ota_nav2_sensor_fix/ota_update_server/.gitignore b/demos/ota_nav2_sensor_fix/ota_update_server/.gitignore new file mode 100644 index 0000000..d0ad410 --- /dev/null +++ b/demos/ota_nav2_sensor_fix/ota_update_server/.gitignore @@ -0,0 +1,3 @@ +.venv/ +*.egg-info/ +__pycache__/ diff --git a/demos/ota_nav2_sensor_fix/ota_update_server/Dockerfile b/demos/ota_nav2_sensor_fix/ota_update_server/Dockerfile new file mode 100644 index 0000000..7d60d71 --- /dev/null +++ b/demos/ota_nav2_sensor_fix/ota_update_server/Dockerfile @@ -0,0 +1,99 @@ +# Copyright 2026 bburda +# Apache 2.0 +# +# Multi-stage build: stage 1 produces the SOVD catalog + tarballs from +# the demo's ros2_packages/ (broken_lidar / fixed_lidar are pure rclcpp + +# sensor_msgs republishers - Nav2's own log + action-status bridges turn +# its failure into SOVD faults, not a ReportFault call from these nodes); +# stage 2 ships a slim FastAPI server +# that serves those artefacts. This keeps `docker compose build` +# self-contained and reproducible on CI - no separate "build artifacts on +# host" step required. +# +# Build context is the demo root (so we can pull in ros2_packages/ + +# scripts/ + ota_update_server/). docker-compose.yml wires that up. + +# ============================================================================= +# Stage 1: build artefacts + catalog inside ros:jazzy +# ============================================================================= +FROM ros:jazzy AS artefact_builder + + +RUN apt-get update && apt-get install -y --no-install-recommends \ + git \ + python3-colcon-common-extensions \ + python3-catkin-pkg \ + python3-venv \ + python3-pip \ + build-essential \ + cmake \ + ros-jazzy-rclcpp \ + ros-jazzy-sensor-msgs \ + ros-jazzy-geometry-msgs \ + ros-jazzy-visualization-msgs \ + && rm -rf /var/lib/apt/lists/* + +# broken_lidar / fixed_lidar are pure /scan_sim republishers (Nav2's own +# failure is surfaced via the log/action-status bridges), so neither +# depends on ros2_medkit_msgs - no gateway clone here. +# +# Bring in the demo's source packages + pack_artifact.py. +WORKDIR /demo +COPY ros2_packages /demo/ros2_packages +COPY scripts /demo/scripts + +# Build broken_lidar + fixed_lidar. Tarballs + catalog.json land in +# /demo/artifacts. +RUN mkdir -p /demo/ros2_ws/src && \ + for pkg in broken_lidar fixed_lidar; do \ + ln -sfn /demo/ros2_packages/$pkg /demo/ros2_ws/src/$pkg; \ + done && \ + . /opt/ros/jazzy/setup.sh && \ + cd /demo/ros2_ws && \ + colcon build --packages-select broken_lidar fixed_lidar + +# pack_artifact.py uses pure stdlib (json/tarfile/argparse) so we don't +# need the .venv/pytest dance build_artifacts.sh does locally. +# +# The BOOT catalog (catalog.json, served at GET /catalog) holds only the bad +# update broken_lidar_3_0_0 - that is what was pushed and auto-applied. The +# remediation build fixed_lidar_3_0_1 is packed into catalog_pending.json +# instead (its tarball still ships), so it is NOT in the boot catalog. The +# operator publishes it at diagnose time with publish-fix.sh (SOVD +# POST /updates), the way a real hotfix is released in response to an incident. +RUN mkdir -p /demo/artifacts && \ + rm -f /demo/artifacts/catalog.json && \ + PACK="python3 /demo/scripts/pack_artifact.py" && \ + $PACK --package broken_lidar --version 3.0.0 \ + --kind update --target-component scan_sensor_node \ + --executable broken_lidar_node \ + --replaces-executable fixed_lidar_node \ + --notes "Perception: /scan noise-filter tuning" \ + --skip-build --workspace /demo/ros2_ws \ + --out-dir /demo/artifacts --catalog /demo/artifacts/catalog.json && \ + $PACK --package fixed_lidar --version 3.0.1 \ + --kind update --target-component scan_sensor_node \ + --executable fixed_lidar_node \ + --replaces-executable broken_lidar_node \ + --notes "Fix regressed /scan noise filter (3.0.0 hotfix)" \ + --skip-build --workspace /demo/ros2_ws \ + --out-dir /demo/artifacts --catalog /demo/artifacts/catalog_pending.json + +# ============================================================================= +# Stage 2: slim runtime image +# ============================================================================= +FROM python:3.11-slim + +WORKDIR /app +COPY ota_update_server/pyproject.toml ./ +COPY ota_update_server/ota_update_server ./ota_update_server +RUN pip install --no-cache-dir . + +COPY --from=artefact_builder /demo/artifacts /artifacts + +ENV OTA_ARTIFACTS_DIR=/artifacts \ + OTA_HOST=0.0.0.0 \ + OTA_PORT=9000 +EXPOSE 9000 + +CMD ["ota-update-server"] diff --git a/demos/ota_nav2_sensor_fix/ota_update_server/ota_update_server/__init__.py b/demos/ota_nav2_sensor_fix/ota_update_server/ota_update_server/__init__.py new file mode 100644 index 0000000..813f14c --- /dev/null +++ b/demos/ota_nav2_sensor_fix/ota_update_server/ota_update_server/__init__.py @@ -0,0 +1,3 @@ +from .main import create_app + +__all__ = ["create_app"] diff --git a/demos/ota_nav2_sensor_fix/ota_update_server/ota_update_server/main.py b/demos/ota_nav2_sensor_fix/ota_update_server/ota_update_server/main.py new file mode 100644 index 0000000..159580c --- /dev/null +++ b/demos/ota_nav2_sensor_fix/ota_update_server/ota_update_server/main.py @@ -0,0 +1,43 @@ +"""Minimal FastAPI artifact host for the OTA demo.""" +from __future__ import annotations + +import json +import os +from pathlib import Path + +from fastapi import FastAPI, HTTPException +from fastapi.responses import FileResponse + + +def create_app(artifacts_dir: Path) -> FastAPI: + app = FastAPI(title="OTA Update Server") + artifacts_dir = Path(artifacts_dir) + + @app.get("/catalog") + def catalog() -> list[dict]: + catalog_file = artifacts_dir / "catalog.json" + if not catalog_file.exists(): + return [] + return json.loads(catalog_file.read_text()) + + @app.get("/artifacts/{filename}", response_class=FileResponse) + def artifact(filename: str) -> FileResponse: + if "/" in filename or ".." in filename: + raise HTTPException(status_code=400, detail="invalid filename") + path = artifacts_dir / filename + if not path.exists(): + raise HTTPException(status_code=404, detail="not found") + return FileResponse(path, media_type="application/gzip", filename=filename) + + return app + + +def run() -> None: + import uvicorn + artifacts_dir = Path(os.environ.get("OTA_ARTIFACTS_DIR", "/artifacts")) + host = os.environ.get("OTA_HOST", "0.0.0.0") + port = int(os.environ.get("OTA_PORT", "9000")) + uvicorn.run(create_app(artifacts_dir), host=host, port=port) + + +__all__ = ["create_app", "run"] diff --git a/demos/ota_nav2_sensor_fix/ota_update_server/pyproject.toml b/demos/ota_nav2_sensor_fix/ota_update_server/pyproject.toml new file mode 100644 index 0000000..3479fa6 --- /dev/null +++ b/demos/ota_nav2_sensor_fix/ota_update_server/pyproject.toml @@ -0,0 +1,22 @@ +[project] +name = "ota_update_server" +version = "0.1.0" +description = "Minimal FastAPI artifact host for OTA demo" +requires-python = ">=3.11" +dependencies = [ + "fastapi>=0.110", + "uvicorn[standard]>=0.29", +] + +[project.optional-dependencies] +dev = [ + "pytest>=8", + "httpx>=0.27", +] + +[project.scripts] +ota-update-server = "ota_update_server.main:run" + +[build-system] +requires = ["hatchling"] +build-backend = "hatchling.build" diff --git a/demos/ota_nav2_sensor_fix/ota_update_server/pyrightconfig.json b/demos/ota_nav2_sensor_fix/ota_update_server/pyrightconfig.json new file mode 100644 index 0000000..49f6c16 --- /dev/null +++ b/demos/ota_nav2_sensor_fix/ota_update_server/pyrightconfig.json @@ -0,0 +1,6 @@ +{ + "include": ["ota_update_server", "tests"], + "venvPath": ".", + "venv": ".venv", + "reportUnusedFunction": "none" +} diff --git a/demos/ota_nav2_sensor_fix/ota_update_server/tests/__init__.py b/demos/ota_nav2_sensor_fix/ota_update_server/tests/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/demos/ota_nav2_sensor_fix/ota_update_server/tests/test_main.py b/demos/ota_nav2_sensor_fix/ota_update_server/tests/test_main.py new file mode 100644 index 0000000..33be959 --- /dev/null +++ b/demos/ota_nav2_sensor_fix/ota_update_server/tests/test_main.py @@ -0,0 +1,55 @@ +"""Tests for the FastAPI update server.""" +from __future__ import annotations + +import json +from pathlib import Path + +import pytest +from fastapi.testclient import TestClient + +from ota_update_server import create_app + + +@pytest.fixture +def artifacts_dir(tmp_path) -> Path: + return tmp_path + + +@pytest.fixture +def client(artifacts_dir): + return TestClient(create_app(artifacts_dir)) + + +def test_catalog_empty_when_missing(client): + resp = client.get("/catalog") + assert resp.status_code == 200 + assert resp.json() == [] + + +def test_catalog_returns_file_contents(client, artifacts_dir): + payload = [ + {"id": "fixed_lidar_3_0_1", "updated_components": ["scan_sensor_node"]}, + {"id": "demo_addon_install", "added_components": ["demo_addon"]}, + ] + (artifacts_dir / "catalog.json").write_text(json.dumps(payload)) + resp = client.get("/catalog") + assert resp.status_code == 200 + assert resp.json() == payload + + +def test_artifact_returns_file(client, artifacts_dir): + (artifacts_dir / "fixed_lidar-2.1.0.tar.gz").write_bytes(b"BIN") + resp = client.get("/artifacts/fixed_lidar-2.1.0.tar.gz") + assert resp.status_code == 200 + assert resp.content == b"BIN" + + +def test_artifact_404_when_missing(client): + resp = client.get("/artifacts/missing.tar.gz") + assert resp.status_code == 404 + + +def test_artifact_rejects_path_traversal(client, artifacts_dir): + (artifacts_dir.parent / "secret.txt").write_text("hush") + resp = client.get("/artifacts/..%2Fsecret.txt") + assert resp.status_code in (400, 404) diff --git a/demos/ota_nav2_sensor_fix/publish-fix.sh b/demos/ota_nav2_sensor_fix/publish-fix.sh new file mode 100755 index 0000000..f841391 --- /dev/null +++ b/demos/ota_nav2_sensor_fix/publish-fix.sh @@ -0,0 +1,37 @@ +#!/bin/bash +# Register the remediation update by hand via SOVD POST /updates. +# +# The boot catalog holds only the bad update broken_lidar_3_0_0. The fix is a +# NEW update you publish yourself: its descriptor is a plain JSON file you +# provide - updates/fixed_lidar_3_0_1.json (the id, target component, executable, +# the artifact to fetch, etc.). This just POSTs that file. To do it fully by +# hand, or to register a different update, edit the JSON and POST it directly: +# +# curl -X POST http://localhost:8080/api/v1/updates \ +# -H 'Content-Type: application/json' \ +# -d @updates/fixed_lidar_3_0_1.json +# +# The artifact named in x_medkit_artifact_url must exist on the update server +# (the demo ships fixed_lidar-3.0.1.tar.gz there); apply-fix.sh then fetches it. + +set -eu + +GATEWAY_URL="${OTA_GATEWAY_URL:-http://localhost:${OTA_GATEWAY_PORT:-8080}}" +API="${GATEWAY_URL}/api/v1" +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +META="${SCRIPT_DIR}/updates/fixed_lidar_3_0_1.json" + +if ! curl -fsS "${API}/health" >/dev/null 2>&1; then + echo "Gateway not reachable at ${GATEWAY_URL}. Start it with: ./run-demo.sh" + exit 1 +fi + +echo "Registering the update described in:" +echo " ${META}" +echo " -> POST ${API}/updates" +curl -fsS -X POST -H 'Content-Type: application/json' \ + -d @"${META}" "${API}/updates" >/dev/null + +echo "" +echo "/updates now offers:" +curl -fsS "${API}/updates" | (jq -r '.items[]' 2>/dev/null || cat) diff --git a/demos/ota_nav2_sensor_fix/ros2_packages/broken_lidar/CMakeLists.txt b/demos/ota_nav2_sensor_fix/ros2_packages/broken_lidar/CMakeLists.txt new file mode 100644 index 0000000..b431e4d --- /dev/null +++ b/demos/ota_nav2_sensor_fix/ros2_packages/broken_lidar/CMakeLists.txt @@ -0,0 +1,22 @@ +cmake_minimum_required(VERSION 3.16) +project(broken_lidar) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(nav_msgs REQUIRED) + +add_executable(broken_lidar_node src/broken_lidar_node.cpp) +ament_target_dependencies(broken_lidar_node rclcpp sensor_msgs nav_msgs) + +install(TARGETS broken_lidar_node DESTINATION lib/${PROJECT_NAME}) + +ament_package() diff --git a/demos/ota_nav2_sensor_fix/ros2_packages/broken_lidar/package.xml b/demos/ota_nav2_sensor_fix/ros2_packages/broken_lidar/package.xml new file mode 100644 index 0000000..53b6259 --- /dev/null +++ b/demos/ota_nav2_sensor_fix/ros2_packages/broken_lidar/package.xml @@ -0,0 +1,18 @@ + + + broken_lidar + 1.0.0 + Broken lidar node that publishes /scan with a phantom obstacle (demo target of OTA update). + bburda + Apache-2.0 + + ament_cmake + + rclcpp + sensor_msgs + nav_msgs + + + ament_cmake + + diff --git a/demos/ota_nav2_sensor_fix/ros2_packages/broken_lidar/src/broken_lidar_node.cpp b/demos/ota_nav2_sensor_fix/ros2_packages/broken_lidar/src/broken_lidar_node.cpp new file mode 100644 index 0000000..c6b5c1f --- /dev/null +++ b/demos/ota_nav2_sensor_fix/ros2_packages/broken_lidar/src/broken_lidar_node.cpp @@ -0,0 +1,123 @@ +// Copyright 2026 bburda. Apache-2.0. +// +// Post-OTA (regressed) scan publisher: republishes the REAL gz front-laser +// scan (/scan_sim) onto /scan. It starts CLEAN (a straight passthrough) and +// only overlays a blocking phantom sector once the robot has driven a short +// distance - so the demo shows the AMR set off, drive up the aisle, and then +// lose a lidar sector mid-mission, rather than a robot that is dead on arrival. +// The stuck sector is a contiguous band of rays forced to a constant close +// range regardless of the real geometry; every real obstacle elsewhere in the +// scan stays intact, so Nav2's costmap sees a genuine blocking phantom without +// the robot driving blind through the rest of the warehouse. +// +// Because the phantom is sensor-fixed (it moves + rotates with the robot) it +// sits permanently in front of the AMR once active, so the local planner cannot +// steer around it or rotate away - every forward trajectory runs into it, Nav2 +// makes no progress, and navigate_to_pose aborts. Three ROS parameters, so the +// behaviour can be tuned without touching code: +// phantom_range_m - the constant range the stuck rays report (m). +// Close enough that DWB's forward rollouts hit it. +// phantom_half_band_rays - half-width of the stuck sector in rays either +// side of straight-ahead. +// phantom_onset_distance_m - the robot drives this far (from where this node +// started, tracked on /odom) with a clean scan +// before the sector goes stuck. 0 = phantom from +// the first scan. +// +// Straight-ahead index = round((0 - angle_min) / angle_increment). For the +// RB-Theron front_laser (-2.3..2.3 rad, 810 samples) this is ~405. +// +// Pure republisher - no fault reporting. Nav2's own failure (logged errors, an +// aborted navigate_to_pose) is what turns this into a SOVD fault, via the +// log/action-status bridges - not this node naming its own bug. The onset delay +// only choreographs WHEN the sensor degrades; the fault itself is still Nav2's. + +#include +#include +#include +#include + +#include +#include +#include + +class BrokenLidarNode : public rclcpp::Node { + public: + BrokenLidarNode() : Node("scan_sensor_node") { + // phantom_range_m 0.22 -> the range is measured at the laser, ~0.268 m ahead + // of base_footprint, so 0.22 m places the sector ~0.49 m from centre, just + // outside the 0.45 m footprint: the goal is accepted, but once active the + // robot has no room to creep and stalls. phantom_half_band_rays 184 -> a + // +/-60 deg sector, wide enough that the sector rotating with the robot + // leaves no clear forward heading. + phantom_range_ = static_cast(declare_parameter("phantom_range_m", 0.22)); + phantom_half_band_ = declare_parameter("phantom_half_band_rays", 184); + onset_distance_ = declare_parameter("phantom_onset_distance_m", 1.5); + if (onset_distance_ <= 0.0) { + phantom_active_.store(true); + } + + pub_ = create_publisher("scan", 10); + sub_ = create_subscription( + "scan_sim", 10, + [this](sensor_msgs::msg::LaserScan::UniquePtr msg) { + if (phantom_active_.load()) { + overlay_phantom(*msg); + } + pub_->publish(std::move(msg)); + }); + odom_sub_ = create_subscription( + "odom", 10, + [this](const nav_msgs::msg::Odometry & msg) { track_distance(msg); }); + } + + private: + void track_distance(const nav_msgs::msg::Odometry & msg) { + if (phantom_active_.load()) return; + const double x = msg.pose.pose.position.x; + const double y = msg.pose.pose.position.y; + if (!have_origin_) { + origin_x_ = x; + origin_y_ = y; + have_origin_ = true; + return; + } + if (std::hypot(x - origin_x_, y - origin_y_) >= onset_distance_) { + phantom_active_.store(true); + RCLCPP_INFO(get_logger(), "Lidar sector went stuck after ~%.1f m driven.", onset_distance_); + } + } + + void overlay_phantom(sensor_msgs::msg::LaserScan & msg) const { + if (msg.ranges.empty() || msg.angle_increment == 0.0f) return; + + const auto n = static_cast(msg.ranges.size()); + const double angle_min = static_cast(msg.angle_min); + const double angle_increment = static_cast(msg.angle_increment); + const int idx0 = static_cast(std::lround((0.0 - angle_min) / angle_increment)); + + const int lo = std::max(0, idx0 - phantom_half_band_); + const int hi = std::min(n - 1, idx0 + phantom_half_band_); + for (int i = lo; i <= hi; ++i) { + msg.ranges[static_cast(i)] = phantom_range_; + } + } + + float phantom_range_{0.22f}; + int phantom_half_band_{184}; + double onset_distance_{1.5}; + bool have_origin_{false}; + double origin_x_{0.0}; + double origin_y_{0.0}; + std::atomic phantom_active_{false}; + rclcpp::Publisher::SharedPtr pub_; + rclcpp::Subscription::SharedPtr sub_; + rclcpp::Subscription::SharedPtr odom_sub_; +}; + +int main(int argc, char ** argv) { + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/demos/ota_nav2_sensor_fix/ros2_packages/fixed_lidar/CMakeLists.txt b/demos/ota_nav2_sensor_fix/ros2_packages/fixed_lidar/CMakeLists.txt new file mode 100644 index 0000000..d08580f --- /dev/null +++ b/demos/ota_nav2_sensor_fix/ros2_packages/fixed_lidar/CMakeLists.txt @@ -0,0 +1,21 @@ +cmake_minimum_required(VERSION 3.16) +project(fixed_lidar) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(sensor_msgs REQUIRED) + +add_executable(fixed_lidar_node src/fixed_lidar_node.cpp) +ament_target_dependencies(fixed_lidar_node rclcpp sensor_msgs) + +install(TARGETS fixed_lidar_node DESTINATION lib/${PROJECT_NAME}) + +ament_package() diff --git a/demos/ota_nav2_sensor_fix/ros2_packages/fixed_lidar/package.xml b/demos/ota_nav2_sensor_fix/ros2_packages/fixed_lidar/package.xml new file mode 100644 index 0000000..d0315d7 --- /dev/null +++ b/demos/ota_nav2_sensor_fix/ros2_packages/fixed_lidar/package.xml @@ -0,0 +1,17 @@ + + + fixed_lidar + 2.1.0 + Fixed lidar node that publishes clean /scan. + bburda + Apache-2.0 + + ament_cmake + + rclcpp + sensor_msgs + + + ament_cmake + + diff --git a/demos/ota_nav2_sensor_fix/ros2_packages/fixed_lidar/src/fixed_lidar_node.cpp b/demos/ota_nav2_sensor_fix/ros2_packages/fixed_lidar/src/fixed_lidar_node.cpp new file mode 100644 index 0000000..5f81938 --- /dev/null +++ b/demos/ota_nav2_sensor_fix/ros2_packages/fixed_lidar/src/fixed_lidar_node.cpp @@ -0,0 +1,41 @@ +// Copyright 2026 bburda. Apache-2.0. +// +// Post-OTA scan publisher: clean passthrough of the real gz front-laser +// scan (/scan_sim) onto /scan - no phantom, no fabricated data. The +// message is republished unchanged; frame, angles, ranges and increment +// all come from the incoming scan. +// +// Pure republisher - no fault reporting of any kind. Any fault raised +// while broken_lidar was applied comes from Nav2's own failure (via the +// log/action-status bridges), not from this node, and is a latched +// stored DTC: after the OTA swap the phantom disappears but the stored +// fault remains active on the Faults Dashboard until an operator clears +// it explicitly. + +#include + +#include +#include + +class FixedLidarNode : public rclcpp::Node { + public: + FixedLidarNode() : Node("scan_sensor_node") { + pub_ = create_publisher("scan", 10); + sub_ = create_subscription( + "scan_sim", 10, + [this](sensor_msgs::msg::LaserScan::UniquePtr msg) { + pub_->publish(std::move(msg)); + }); + } + + private: + rclcpp::Publisher::SharedPtr pub_; + rclcpp::Subscription::SharedPtr sub_; +}; + +int main(int argc, char ** argv) { + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/demos/ota_nav2_sensor_fix/ros2_packages/health_check/CMakeLists.txt b/demos/ota_nav2_sensor_fix/ros2_packages/health_check/CMakeLists.txt new file mode 100644 index 0000000..fd56ae4 --- /dev/null +++ b/demos/ota_nav2_sensor_fix/ros2_packages/health_check/CMakeLists.txt @@ -0,0 +1,30 @@ +cmake_minimum_required(VERSION 3.16) +project(health_check) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(std_srvs REQUIRED) + +add_executable(health_check_node src/health_check_node.cpp) +ament_target_dependencies(health_check_node + rclcpp + geometry_msgs + nav_msgs + sensor_msgs + std_srvs +) + +install(TARGETS health_check_node DESTINATION lib/${PROJECT_NAME}) + +ament_package() diff --git a/demos/ota_nav2_sensor_fix/ros2_packages/health_check/package.xml b/demos/ota_nav2_sensor_fix/ros2_packages/health_check/package.xml new file mode 100644 index 0000000..e0b426a --- /dev/null +++ b/demos/ota_nav2_sensor_fix/ros2_packages/health_check/package.xml @@ -0,0 +1,20 @@ + + + health_check + 1.0.0 + Operator-invoked SOVD health-check operations (lidar, localization, drivetrain, costmap) for differential diagnosis of the OTA nav2 sensor-fix demo. + bburda + Apache-2.0 + + ament_cmake + + rclcpp + geometry_msgs + nav_msgs + sensor_msgs + std_srvs + + + ament_cmake + + diff --git a/demos/ota_nav2_sensor_fix/ros2_packages/health_check/src/health_check_node.cpp b/demos/ota_nav2_sensor_fix/ros2_packages/health_check/src/health_check_node.cpp new file mode 100644 index 0000000..16e1f38 --- /dev/null +++ b/demos/ota_nav2_sensor_fix/ros2_packages/health_check/src/health_check_node.cpp @@ -0,0 +1,374 @@ +// Copyright 2026 bburda. Apache-2.0. +// +// Operator-invoked differential-diagnosis node. It does NOT raise faults - +// the log/action-status bridges already turn Nav2's own stall into SOVD +// faults once broken_lidar's phantom sector is applied. This node answers +// four on-demand checks (std_srvs/Trigger operations) so an operator +// working a "navigate_to_pose aborted" fault can narrow the root cause down +// to a specific subsystem instead of guessing: +// ~/lidar_health_check - is /scan reporting a stuck close sector? +// ~/localization_health_check - is AMCL still localizing? +// ~/drivetrain_health_check - is odometry/joint-state telemetry live? +// ~/costmap_health_check - does the local costmap show a live, +// sensor-only obstacle right in front (no +// matching static-map feature)? +// +// Each handler only inspects the latest cached message per topic - no +// history, no state machine, no interaction with the OTA/fault-manager +// flow. Subscribing independently of scan_sensor_node means this node +// survives the broken_lidar <-> fixed_lidar swap untouched, so the same +// four operations answer "broken" before the fix and "healthy" after it. + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include +#include +#include +#include + +namespace { +// Lidar stuck-sector heuristic (see class comment above handle_lidar_health_check). +constexpr float kStuckBandM = 0.05f; +constexpr float kStuckMaxRangeM = 1.5f; +constexpr size_t kStuckMinRunRays = 30; + +// Freshness windows. +constexpr double kLocalizationFreshSec = 10.0; +constexpr double kDrivetrainFreshSec = 5.0; + +// Costmap "directly ahead" window, robot/grid-frame relative. +constexpr double kCostmapAheadMinM = 0.3; +constexpr double kCostmapAheadMaxM = 0.8; +constexpr double kCostmapLateralM = 0.3; +constexpr int8_t kLethalCellValue = 99; +} // namespace + +class HealthCheckNode : public rclcpp::Node { + public: + HealthCheckNode() : Node("health_check") { + scan_sub_ = create_subscription( + "/scan", 10, + [this](sensor_msgs::msg::LaserScan::SharedPtr msg) { + last_scan_ = std::move(msg); + last_scan_time_ = this->now(); + scan_received_ = true; + }); + + amcl_pose_sub_ = create_subscription( + "/amcl_pose", 10, + [this](geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg) { + last_amcl_pose_ = std::move(msg); + last_amcl_pose_time_ = this->now(); + amcl_pose_received_ = true; + }); + + odom_sub_ = create_subscription( + "/odom", 10, + [this](nav_msgs::msg::Odometry::SharedPtr msg) { + last_odom_ = std::move(msg); + last_odom_time_ = this->now(); + odom_received_ = true; + }); + + joint_state_sub_ = create_subscription( + "/joint_states", 10, + [this](sensor_msgs::msg::JointState::SharedPtr msg) { + last_joint_state_ = std::move(msg); + last_joint_state_time_ = this->now(); + joint_state_received_ = true; + }); + + costmap_sub_ = create_subscription( + "/local_costmap/costmap", 10, + [this](nav_msgs::msg::OccupancyGrid::SharedPtr msg) { + last_costmap_ = std::move(msg); + last_costmap_time_ = this->now(); + costmap_received_ = true; + }); + + lidar_health_check_srv_ = create_service( + "~/lidar_health_check", + [this](const std::shared_ptr /*request*/, + std::shared_ptr response) { + handle_lidar_health_check(response); + }); + + localization_health_check_srv_ = create_service( + "~/localization_health_check", + [this](const std::shared_ptr /*request*/, + std::shared_ptr response) { + handle_localization_health_check(response); + }); + + drivetrain_health_check_srv_ = create_service( + "~/drivetrain_health_check", + [this](const std::shared_ptr /*request*/, + std::shared_ptr response) { + handle_drivetrain_health_check(response); + }); + + costmap_health_check_srv_ = create_service( + "~/costmap_health_check", + [this](const std::shared_ptr /*request*/, + std::shared_ptr response) { + handle_costmap_health_check(response); + }); + } + + // Subscription callbacks capture `this`; drop them before the rest of the + // object is torn down so none can fire against a partially-destroyed node. + ~HealthCheckNode() override { + scan_sub_.reset(); + amcl_pose_sub_.reset(); + odom_sub_.reset(); + joint_state_sub_.reset(); + costmap_sub_.reset(); + } + + HealthCheckNode(const HealthCheckNode &) = delete; + HealthCheckNode & operator=(const HealthCheckNode &) = delete; + HealthCheckNode(HealthCheckNode &&) = delete; + HealthCheckNode & operator=(HealthCheckNode &&) = delete; + + private: + // Scans for the longest run of consecutive FINITE ranges that are all + // within kStuckBandM of one another and below kStuckMaxRangeM - a + // contiguous band of near-equal close returns is the signature of the + // broken_lidar phantom sector (a real environment practically never + // presents 30+ consecutive rays all within 5 cm of each other). + void handle_lidar_health_check(std::shared_ptr response) const { + if (!scan_received_ || !last_scan_) { + response->success = false; + response->message = "no /scan received"; + return; + } + + const auto & ranges = last_scan_->ranges; + const size_t n = ranges.size(); + + size_t finite_count = 0; + float finite_min = std::numeric_limits::infinity(); + float finite_max = -std::numeric_limits::infinity(); + for (const float v : ranges) { + if (!std::isfinite(v)) continue; + ++finite_count; + finite_min = std::min(finite_min, v); + finite_max = std::max(finite_max, v); + } + + size_t best_start = 0; + size_t best_len = 0; + float best_value = 0.0f; + size_t i = 0; + while (i < n) { + if (!std::isfinite(ranges[i]) || ranges[i] >= kStuckMaxRangeM) { + ++i; + continue; + } + const size_t start = i; + float run_min = ranges[i]; + float run_max = ranges[i]; + size_t j = i + 1; + while (j < n && std::isfinite(ranges[j]) && ranges[j] < kStuckMaxRangeM) { + const float new_min = std::min(run_min, ranges[j]); + const float new_max = std::max(run_max, ranges[j]); + if (new_max - new_min > kStuckBandM) break; + run_min = new_min; + run_max = new_max; + ++j; + } + const size_t len = j - start; + if (len > best_len) { + best_len = len; + best_start = start; + best_value = (run_min + run_max) / 2.0f; + } + i = j; + } + + if (best_len >= kStuckMinRunRays) { + const double angle_min = static_cast(last_scan_->angle_min); + const double angle_increment = static_cast(last_scan_->angle_increment); + const double bearing_rad = + angle_min + (static_cast(best_start) + static_cast(best_len) / 2.0) * angle_increment; + const double bearing_deg = bearing_rad * 180.0 / M_PI; + + std::ostringstream oss; + oss << "Stuck lidar sector: " << best_len << " rays pinned near " << std::fixed << std::setprecision(2) + << best_value << " m around bearing " << std::setprecision(1) << bearing_deg + << " deg - the lidar is reporting a phantom obstacle."; + response->success = false; + response->message = oss.str(); + return; + } + + std::ostringstream oss; + oss << "Lidar nominal: " << finite_count << " finite returns"; + if (finite_count > 0) { + oss << ", ranges " << std::fixed << std::setprecision(2) << finite_min << "-" << finite_max << " m"; + } + oss << ", no stuck sector."; + response->success = true; + response->message = oss.str(); + } + + void handle_localization_health_check(std::shared_ptr response) const { + if (amcl_pose_received_) { + const double age_s = (this->now() - last_amcl_pose_time_).seconds(); + if (age_s <= kLocalizationFreshSec) { + std::ostringstream oss; + oss << "Localization healthy: AMCL pose fresh (" << std::fixed << std::setprecision(1) << age_s + << "s old), covariance nominal."; + response->success = true; + response->message = oss.str(); + return; + } + } + response->success = false; + response->message = "AMCL pose stale/absent"; + } + + void handle_drivetrain_health_check(std::shared_ptr response) const { + const double odom_age = + odom_received_ ? (this->now() - last_odom_time_).seconds() : std::numeric_limits::infinity(); + const double joint_age = joint_state_received_ ? (this->now() - last_joint_state_time_).seconds() + : std::numeric_limits::infinity(); + const bool odom_fresh = odom_received_ && odom_age <= kDrivetrainFreshSec; + const bool joint_fresh = joint_state_received_ && joint_age <= kDrivetrainFreshSec; + + if (odom_fresh && joint_fresh) { + response->success = true; + response->message = "Drivetrain healthy: odometry + joint states streaming, diff-drive active."; + return; + } + + std::vector stale; + if (!odom_fresh) stale.emplace_back("odometry"); + if (!joint_fresh) stale.emplace_back("joint states"); + + std::ostringstream oss; + oss << "Drivetrain unhealthy: "; + for (size_t k = 0; k < stale.size(); ++k) { + if (k > 0) oss << " and "; + oss << stale[k]; + } + oss << " stale/absent."; + response->success = false; + response->message = oss.str(); + } + + // Counts lethal cells (>= kLethalCellValue) in a small window directly + // ahead of the grid centre (~0.3-0.8 m ahead along the grid's own +x, + // +/-0.3 m lateral). For a rolling local costmap the grid centre tracks + // the robot, so this window is "in front of the robot" without needing a + // TF lookup. A lethal hit here with no corresponding static-map feature + // corroborates a live-sensor phantom rather than a real obstacle - this + // check only ever reports success=true, it is an observation, not a fault. + void handle_costmap_health_check(std::shared_ptr response) const { + if (!costmap_received_ || !last_costmap_ || last_costmap_->data.empty() || last_costmap_->info.width == 0 || + last_costmap_->info.height == 0 || last_costmap_->info.resolution <= 0.0f) { + response->success = false; + response->message = "no /local_costmap/costmap received"; + return; + } + + const auto & info = last_costmap_->info; + const double res = static_cast(info.resolution); + const double origin_x = info.origin.position.x; + const double origin_y = info.origin.position.y; + const int width = static_cast(info.width); + const int height = static_cast(info.height); + + const double centre_x = origin_x + (static_cast(width) * res) / 2.0; + const double centre_y = origin_y + (static_cast(height) * res) / 2.0; + + const auto to_col = [&](double x) { return static_cast(std::floor((x - origin_x) / res)); }; + const auto to_row = [&](double y) { return static_cast(std::floor((y - origin_y) / res)); }; + + const int col_min = std::clamp(to_col(centre_x + kCostmapAheadMinM), 0, width - 1); + const int col_max = std::clamp(to_col(centre_x + kCostmapAheadMaxM), 0, width - 1); + const int row_min = std::clamp(to_row(centre_y - kCostmapLateralM), 0, height - 1); + const int row_max = std::clamp(to_row(centre_y + kCostmapLateralM), 0, height - 1); + + int lethal_count = 0; + double ahead_sum_m = 0.0; + for (int row = row_min; row <= row_max; ++row) { + for (int col = col_min; col <= col_max; ++col) { + const size_t idx = static_cast(row) * static_cast(width) + static_cast(col); + if (idx >= last_costmap_->data.size()) continue; + if (last_costmap_->data[idx] >= kLethalCellValue) { + ++lethal_count; + const double cell_x = origin_x + (static_cast(col) + 0.5) * res; + ahead_sum_m += cell_x - centre_x; + } + } + } + + if (lethal_count > 0) { + const double ahead_avg_m = ahead_sum_m / static_cast(lethal_count); + std::ostringstream oss; + oss << "Local costmap: " << lethal_count << " lethal cells ~" << std::fixed << std::setprecision(2) + << ahead_avg_m + << " m ahead with no matching static-map feature - consistent with a live sensor (phantom), " + "not the environment."; + response->success = true; + response->message = oss.str(); + return; + } + + response->success = true; + response->message = "Local costmap clear ahead."; + } + + // Cached latest messages + reception time (node clock, so this follows + // use_sim_time like the rest of the demo) per subscribed topic. + sensor_msgs::msg::LaserScan::SharedPtr last_scan_; + rclcpp::Time last_scan_time_; + bool scan_received_{false}; + + geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr last_amcl_pose_; + rclcpp::Time last_amcl_pose_time_; + bool amcl_pose_received_{false}; + + nav_msgs::msg::Odometry::SharedPtr last_odom_; + rclcpp::Time last_odom_time_; + bool odom_received_{false}; + + sensor_msgs::msg::JointState::SharedPtr last_joint_state_; + rclcpp::Time last_joint_state_time_; + bool joint_state_received_{false}; + + nav_msgs::msg::OccupancyGrid::SharedPtr last_costmap_; + rclcpp::Time last_costmap_time_; + bool costmap_received_{false}; + + rclcpp::Subscription::SharedPtr scan_sub_; + rclcpp::Subscription::SharedPtr amcl_pose_sub_; + rclcpp::Subscription::SharedPtr odom_sub_; + rclcpp::Subscription::SharedPtr joint_state_sub_; + rclcpp::Subscription::SharedPtr costmap_sub_; + + rclcpp::Service::SharedPtr lidar_health_check_srv_; + rclcpp::Service::SharedPtr localization_health_check_srv_; + rclcpp::Service::SharedPtr drivetrain_health_check_srv_; + rclcpp::Service::SharedPtr costmap_health_check_srv_; +}; + +int main(int argc, char ** argv) { + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/demos/ota_nav2_sensor_fix/ros2_packages/ota_nav2_sensor_fix_demo/CMakeLists.txt b/demos/ota_nav2_sensor_fix/ros2_packages/ota_nav2_sensor_fix_demo/CMakeLists.txt new file mode 100644 index 0000000..daf67aa --- /dev/null +++ b/demos/ota_nav2_sensor_fix/ros2_packages/ota_nav2_sensor_fix_demo/CMakeLists.txt @@ -0,0 +1,26 @@ +cmake_minimum_required(VERSION 3.16) +project(ota_nav2_sensor_fix_demo) + +find_package(ament_cmake REQUIRED) + +install(DIRECTORY launch/ + DESTINATION share/${PROJECT_NAME}/launch +) + +install(DIRECTORY config/ + DESTINATION share/${PROJECT_NAME}/config +) + +install(DIRECTORY urdf/ + DESTINATION share/${PROJECT_NAME}/urdf +) + +install(DIRECTORY models/ + DESTINATION share/${PROJECT_NAME}/models +) + +install(DIRECTORY maps/ + DESTINATION share/${PROJECT_NAME}/maps +) + +ament_package() diff --git a/demos/ota_nav2_sensor_fix/ros2_packages/ota_nav2_sensor_fix_demo/config/controllers.yaml b/demos/ota_nav2_sensor_fix/ros2_packages/ota_nav2_sensor_fix_demo/config/controllers.yaml new file mode 100644 index 0000000..43f5a69 --- /dev/null +++ b/demos/ota_nav2_sensor_fix/ros2_packages/ota_nav2_sensor_fix_demo/config/controllers.yaml @@ -0,0 +1,26 @@ +controller_manager: + ros__parameters: + update_rate: 50 + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + diff_drive_controller: + type: diff_drive_controller/DiffDriveController + +diff_drive_controller: + ros__parameters: + left_wheel_names: ["left_wheel_joint"] + right_wheel_names: ["right_wheel_joint"] + wheel_separation: 0.5032 # metres; 2 * wheel_offset_y (0.2516) from robotnik_description/urdf/bases/rbtheron/rbtheron_base.xacro:20 + wheel_radius: 0.0762 # metres; wheel_radius property from robotnik_description/urdf/wheels/rubber_wheel/rubber_wheel_150.urdf.xacro:13 + base_frame_id: base_footprint + odom_frame_id: odom + enable_odom_tf: true + publish_rate: 50.0 + # Terminal fail-safe: if /cmd_vel stops arriving (nav2 stalls, the goal is + # cancelled, or the controller crashes) the wheels halt after this timeout + # rather than repeating the last command forever. + cmd_vel_timeout: 0.5 + # Jazzy diff_drive_controller subscribes ~/cmd_vel as geometry_msgs/TwistStamped + # unconditionally; no use_stamped_vel parameter exists on Jazzy. Our + # nav2_params.yaml already publishes TwistStamped end to end + # (enable_stamped_cmd_vel: True), so no adapter/shim is needed here. diff --git a/demos/ota_nav2_sensor_fix/ros2_packages/ota_nav2_sensor_fix_demo/config/nav2_params.yaml b/demos/ota_nav2_sensor_fix/ros2_packages/ota_nav2_sensor_fix_demo/config/nav2_params.yaml new file mode 100644 index 0000000..ae62a66 --- /dev/null +++ b/demos/ota_nav2_sensor_fix/ros2_packages/ota_nav2_sensor_fix_demo/config/nav2_params.yaml @@ -0,0 +1,395 @@ +# Nav2 parameters for the RB-Theron AMR + ros2_medkit demo, driving in the AWS +# small-warehouse world on plain /cmd_vel. +# +# Robot: Robotnik RB-Theron - a ~0.7 x 0.5 m differential-drive AMR (circumscribed +# radius ~0.43 m, rounded up to 0.45 m below). Frames: diff_drive_controller +# (config/controllers.yaml) publishes odom->base_footprint; robot_state_publisher +# publishes base_footprint->base_link (+ the rest of the URDF); amcl owns +# map->odom. So base_footprint is the robot base frame across the stack. + +# Lifecycle manager configuration - exclude docking_server which we don't use +lifecycle_manager_navigation: + ros__parameters: + use_sim_time: True + autostart: True + node_names: + - controller_server + - smoother_server + - planner_server + - behavior_server + - bt_navigator + - waypoint_follower + - velocity_smoother + - collision_monitor + # Note: docking_server and route_server removed - not configured for this demo + +amcl: + ros__parameters: + use_sim_time: True + alpha1: 0.2 + alpha2: 0.2 + alpha3: 0.2 + alpha4: 0.2 + alpha5: 0.2 + base_frame_id: "base_footprint" + beam_skip_distance: 0.5 + beam_skip_error_threshold: 0.9 + beam_skip_threshold: 0.3 + do_beamskip: false + global_frame_id: "map" + lambda_short: 0.1 + laser_likelihood_max_dist: 2.0 + laser_max_range: 100.0 + laser_min_range: -1.0 + laser_model_type: "likelihood_field" + max_beams: 60 + max_particles: 2000 + min_particles: 500 + odom_frame_id: "odom" + pf_err: 0.05 + pf_z: 0.99 + recovery_alpha_fast: 0.0 + recovery_alpha_slow: 0.0 + resample_interval: 1 + robot_model_type: "nav2_amcl::DifferentialMotionModel" + save_pose_rate: 0.5 + sigma_hit: 0.2 + tf_broadcast: true + # Broadcast map -> odom on every laser update, not only after the + # robot has driven update_min_d / update_min_a. Demo flow has the + # robot sitting idle while the operator inspects the dashboard; + # without continuous broadcasting Foxglove can't even pick `map` + # in the Display Frame dropdown until the operator publishes a + # goal and the robot moves. + update_min_d: 0.0 + update_min_a: 0.0 + transform_tolerance: 1.0 + z_hit: 0.5 + z_max: 0.05 + z_rand: 0.5 + z_short: 0.05 + scan_topic: scan + # Must match the spawn pose set by demo.launch.py (x_pose=1.8, y_pose=-4.2, + # yaw_pose=1.5708) - a verified-clear north-south aisle in the warehouse map + # (x=1.8) so the AMR starts in open floor space and the particle filter + # converges immediately. Otherwise AMCL's particle filter searches around + # origin, never sees the robot, and never publishes map -> odom - which + # leaves Foxglove unable to set Display Frame to "map" and floods rosout + # with "Invalid frame ID 'map'" errors. + set_initial_pose: true + initial_pose: + x: 1.8 + y: -4.2 + z: 0.0 + yaw: 1.5708 + +bt_navigator: + ros__parameters: + use_sim_time: True + global_frame: map + robot_base_frame: base_footprint + odom_topic: /odom + bt_loop_duration: 10 + default_server_timeout: 20 + wait_for_service_timeout: 1000 + action_server_result_timeout: 900.0 + # Custom nav_to_pose BT: nav2's default retries the whole navigation 6 times + # (RecoveryNode number_of_retries="6") before navigate_to_pose aborts, so + # against the phantom the robot does a long recovery dance and the goal takes + # a minute-plus to abort - the operator just sees the controller "Failed to + # make progress" pile up. This copy drops the top-level retries to 1 so the + # goal aborts promptly (one recovery attempt, then abort), which is what + # raises the headline ACTION_NAVIGATE_TO_POSE_ABORTED fault on bt-navigator. + # Absolute install path (the demo image layout is fixed). + default_nav_to_pose_bt_xml_filename: "/ws/install/ota_nav2_sensor_fix_demo/share/ota_nav2_sensor_fix_demo/config/navigate_to_pose_fast_abort.xml" + navigators: ["navigate_to_pose", "navigate_through_poses"] + navigate_to_pose: + plugin: "nav2_bt_navigator::NavigateToPoseNavigator" + navigate_through_poses: + plugin: "nav2_bt_navigator::NavigateThroughPosesNavigator" + # Note: plugin_lib_names is no longer needed in Jazzy - plugins are auto-loaded + +controller_server: + ros__parameters: + use_sim_time: True + enable_stamped_cmd_vel: True + controller_frequency: 20.0 + min_x_velocity_threshold: 0.001 + min_y_velocity_threshold: 0.5 + min_theta_velocity_threshold: 0.001 + failure_tolerance: 0.3 + progress_checker_plugins: ["progress_checker"] + goal_checker_plugins: ["general_goal_checker"] + controller_plugins: ["FollowPath"] + odom_topic: "odom" + + progress_checker: + plugin: "nav2_controller::SimpleProgressChecker" + required_movement_radius: 0.5 + movement_time_allowance: 10.0 + + general_goal_checker: + stateful: True + plugin: "nav2_controller::SimpleGoalChecker" + xy_goal_tolerance: 0.25 + yaw_goal_tolerance: 0.25 + + # RB-Theron demo-safe top speed 0.5 m/s (the platform can do ~1.0). Angular + # and accel limits kept moderate for a heavier base than TurtleBot3. + FollowPath: + plugin: "dwb_core::DWBLocalPlanner" + debug_trajectory_details: True + min_vel_x: 0.0 + min_vel_y: 0.0 + max_vel_x: 0.5 + max_vel_y: 0.0 + max_vel_theta: 1.0 + min_speed_xy: 0.0 + max_speed_xy: 0.5 + min_speed_theta: 0.0 + acc_lim_x: 2.0 + acc_lim_y: 0.0 + acc_lim_theta: 3.2 + decel_lim_x: -2.0 + decel_lim_y: 0.0 + decel_lim_theta: -3.2 + vx_samples: 20 + vy_samples: 5 + vtheta_samples: 20 + sim_time: 1.7 + linear_granularity: 0.05 + angular_granularity: 0.025 + transform_tolerance: 0.2 + xy_goal_tolerance: 0.25 + trans_stopped_velocity: 0.25 + short_circuit_trajectory_evaluation: True + stateful: True + critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"] + BaseObstacle.scale: 0.02 + PathAlign.scale: 32.0 + PathAlign.forward_point_distance: 0.1 + GoalAlign.scale: 24.0 + GoalAlign.forward_point_distance: 0.1 + PathDist.scale: 32.0 + GoalDist.scale: 24.0 + RotateToGoal.scale: 32.0 + RotateToGoal.slowing_factor: 5.0 + RotateToGoal.lookahead_time: -1.0 + +local_costmap: + local_costmap: + ros__parameters: + update_frequency: 5.0 + publish_frequency: 2.0 + global_frame: odom + robot_base_frame: base_footprint + use_sim_time: True + rolling_window: true + width: 4 + height: 4 + resolution: 0.05 + # RB-Theron circumscribed radius. The base is ~0.7 x 0.5 m so the true + # circumscribed radius is ~0.43 m; round up to 0.45 m for a safety margin. + robot_radius: 0.45 + plugins: ["voxel_layer", "inflation_layer"] + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.50 + voxel_layer: + plugin: "nav2_costmap_2d::VoxelLayer" + enabled: True + publish_voxel_map: True + origin_z: 0.0 + z_resolution: 0.05 + z_voxels: 16 + max_obstacle_height: 2.0 + mark_threshold: 0 + observation_sources: scan + scan: + topic: /scan + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + map_subscribe_transient_local: True + always_send_full_costmap: True + +global_costmap: + global_costmap: + ros__parameters: + update_frequency: 1.0 + publish_frequency: 1.0 + global_frame: map + robot_base_frame: base_footprint + use_sim_time: True + # RB-Theron circumscribed radius (see local_costmap note). + robot_radius: 0.45 + resolution: 0.05 + track_unknown_space: true + plugins: ["static_layer", "obstacle_layer", "inflation_layer"] + obstacle_layer: + plugin: "nav2_costmap_2d::ObstacleLayer" + enabled: True + observation_sources: scan + scan: + topic: /scan + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + map_subscribe_transient_local: True + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.50 + always_send_full_costmap: True + +planner_server: + ros__parameters: + expected_planner_frequency: 20.0 + use_sim_time: True + planner_plugins: ["GridBased"] + GridBased: + plugin: "nav2_navfn_planner::NavfnPlanner" + tolerance: 0.5 + use_astar: false + allow_unknown: true + +smoother_server: + ros__parameters: + use_sim_time: True + smoother_plugins: ["simple_smoother"] + simple_smoother: + plugin: "nav2_smoother::SimpleSmoother" + tolerance: 1.0e-10 + max_its: 1000 + do_refinement: True + +behavior_server: + ros__parameters: + enable_stamped_cmd_vel: True + local_costmap_topic: local_costmap/costmap_raw + global_costmap_topic: global_costmap/costmap_raw + local_footprint_topic: local_costmap/published_footprint + global_footprint_topic: global_costmap/published_footprint + cycle_frequency: 10.0 + behavior_plugins: ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"] + spin: + plugin: "nav2_behaviors::Spin" + backup: + plugin: "nav2_behaviors::BackUp" + drive_on_heading: + plugin: "nav2_behaviors::DriveOnHeading" + wait: + plugin: "nav2_behaviors::Wait" + assisted_teleop: + plugin: "nav2_behaviors::AssistedTeleop" + local_frame: odom + global_frame: map + robot_base_frame: base_footprint + transform_tolerance: 0.1 + use_sim_time: True + simulate_ahead_time: 2.0 + max_rotational_vel: 1.0 + min_rotational_vel: 0.4 + rotational_acc_lim: 3.2 + +waypoint_follower: + ros__parameters: + use_sim_time: True + loop_rate: 20 + stop_on_failure: false + action_server_result_timeout: 900.0 + waypoint_task_executor_plugin: "wait_at_waypoint" + wait_at_waypoint: + plugin: "nav2_waypoint_follower::WaitAtWaypoint" + enabled: True + waypoint_pause_duration: 200 + +velocity_smoother: + ros__parameters: + use_sim_time: True + enable_stamped_cmd_vel: True + smoothing_frequency: 20.0 + scale_velocities: False + feedback: "OPEN_LOOP" + # RB-Theron demo-safe limits: max linear 0.5 m/s (platform can do ~1.0), + # angular 1.0 rad/s; accel/decel matched to the controller_server FollowPath + # limits above. + max_velocity: [0.5, 0.0, 1.0] + min_velocity: [-0.5, 0.0, -1.0] + max_accel: [2.0, 0.0, 3.2] + max_decel: [-2.0, 0.0, -3.2] + odom_topic: "odom" + odom_duration: 0.1 + deadband_velocity: [0.0, 0.0, 0.0] + velocity_timeout: 1.0 + +collision_monitor: + ros__parameters: + use_sim_time: True + enable_stamped_cmd_vel: True + base_frame_id: "base_footprint" + odom_frame_id: "odom" + cmd_vel_in_topic: "cmd_vel_smoothed" + cmd_vel_out_topic: "cmd_vel" + state_topic: "collision_monitor_state" + transform_tolerance: 0.2 + source_timeout: 1.0 + base_shift_correction: True + stop_pub_timeout: 2.0 + polygons: ["PassthroughNoOp"] + # action_type "none": the polygon is evaluated but never brakes, so + # collision_monitor republishes cmd_vel_smoothed -> cmd_vel unchanged. + # Obstacle avoidance stays with the costmap + DWB (and the Phase 2 phantom + # blocks there). The functional FootprintApproach throttled the clear aisle + # to zero: it projects the full 0.45 m footprint 1.2 s ahead and false-fired + # "approach" against the aisle-end shelving, so cmd_vel never reached the + # wheels (cmd_vel_smoothed = 0.5 m/s in, /cmd_vel empty out). This demo is + # about the sensor fault, not reactive collision braking. + PassthroughNoOp: + type: "polygon" + action_type: "none" + points: "[[0.1, 0.1], [0.1, -0.1], [-0.1, -0.1], [-0.1, 0.1]]" + min_points: 4 + visualize: False + enabled: True + observation_sources: ["scan"] + scan: + type: "scan" + topic: "scan" + min_height: 0.15 + max_height: 2.0 + enabled: True + +# Docking server - minimal config to satisfy lifecycle manager +# We don't actually use docking in this demo +docking_server: + ros__parameters: + use_sim_time: True + enable_stamped_cmd_vel: True + dock_plugins: ["simple_charging_dock"] + simple_charging_dock: + plugin: "opennav_docking::SimpleChargingDock" + use_external_detection_pose: false + docking_threshold: 0.02 + staging_x_offset: -0.5 + staging_yaw_offset: 0.0 + +# Route server - minimal config +route_server: + ros__parameters: + use_sim_time: True diff --git a/demos/ota_nav2_sensor_fix/ros2_packages/ota_nav2_sensor_fix_demo/config/navigate_to_pose_fast_abort.xml b/demos/ota_nav2_sensor_fix/ros2_packages/ota_nav2_sensor_fix_demo/config/navigate_to_pose_fast_abort.xml new file mode 100644 index 0000000..807987b --- /dev/null +++ b/demos/ota_nav2_sensor_fix/ros2_packages/ota_nav2_sensor_fix_demo/config/navigate_to_pose_fast_abort.xml @@ -0,0 +1,50 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/demos/ota_nav2_sensor_fix/ros2_packages/ota_nav2_sensor_fix_demo/config/ros_gz_bridge.yaml b/demos/ota_nav2_sensor_fix/ros2_packages/ota_nav2_sensor_fix_demo/config/ros_gz_bridge.yaml new file mode 100644 index 0000000..d73bb06 --- /dev/null +++ b/demos/ota_nav2_sensor_fix/ros2_packages/ota_nav2_sensor_fix_demo/config/ros_gz_bridge.yaml @@ -0,0 +1,19 @@ +# gz <-> ROS2. /tf and /odom are NOT bridged: robot_state_publisher (static+joints), +# diff_drive_controller (odom + odom->base TF), and amcl (map->odom) own the TF tree. +# Bridging gz Pose_V to /tf loses frame_ids and conflicts; do not add it. +- ros_topic_name: "/clock" + gz_topic_name: "/clock" + ros_type_name: "rosgraph_msgs/msg/Clock" + gz_type_name: "gz.msgs.Clock" + direction: GZ_TO_ROS +# The real gz front-laser lands on /scan_sim, not /scan directly: +# scan_sensor_node (broken_lidar/fixed_lidar) subscribes /scan_sim and +# republishes onto /scan (clean passthrough for fixed_lidar, real scan + +# a blocking phantom overlay for broken_lidar), same shadow trick as the +# Earlier SetRemap /scan->/scan_sim. scan_sensor_node is the sole +# publisher on /scan that nav2 + foxglove see. +- ros_topic_name: "/scan_sim" + gz_topic_name: "/robot/front_laser/scan" + ros_type_name: "sensor_msgs/msg/LaserScan" + gz_type_name: "gz.msgs.LaserScan" + direction: GZ_TO_ROS diff --git a/demos/ota_nav2_sensor_fix/ros2_packages/ota_nav2_sensor_fix_demo/config/warehouse_map.yaml b/demos/ota_nav2_sensor_fix/ros2_packages/ota_nav2_sensor_fix_demo/config/warehouse_map.yaml new file mode 100644 index 0000000..e44c436 --- /dev/null +++ b/demos/ota_nav2_sensor_fix/ros2_packages/ota_nav2_sensor_fix_demo/config/warehouse_map.yaml @@ -0,0 +1,7 @@ +image: ../maps/warehouse.pgm +mode: trinary +resolution: 0.050 +origin: [-6.936, -18.504, 0] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.196 diff --git a/demos/ota_nav2_sensor_fix/ros2_packages/ota_nav2_sensor_fix_demo/launch/demo.launch.py b/demos/ota_nav2_sensor_fix/ros2_packages/ota_nav2_sensor_fix_demo/launch/demo.launch.py new file mode 100644 index 0000000..9839896 --- /dev/null +++ b/demos/ota_nav2_sensor_fix/ros2_packages/ota_nav2_sensor_fix_demo/launch/demo.launch.py @@ -0,0 +1,452 @@ +# Copyright 2026 bburda +# Apache 2.0 +# +# Single launch entry point for the OTA over SOVD nav2 sensor-fix demo. +# +# Brings up, in one container: +# - headless Gazebo (gz-sim) with the AWS small-warehouse world +# - the Robotnik RB-Theron AMR spawned in that world, driven through +# gz_ros2_control + a stock diff_drive_controller +# - the full Nav2 stack (bringup_launch.py) with the warehouse map +# - foxglove_bridge on :8765 so Foxglove Studio can render /tf, /scan, /map etc. +# - ros2_medkit fault_manager (the gateway's /faults endpoint depends on it) +# - the gateway with our ota_update_plugin loaded via gateway_config.yaml +# - ros2_medkit_log_bridge + ros2_medkit_action_status_bridge, started +# 15s after boot, turning Nav2's OWN downstream failure into SOVD +# faults (see the "Fault surfacing" comment below) +# - health_check, exposing 4 operator-invoked Trigger operations +# (lidar/localization/drivetrain/costmap) for differential diagnosis; +# independent of scan_sensor_node so it survives the OTA swap +# +# /scan ownership +# --------------- +# The gz front-laser (robot/front_laser/scan) is bridged to /scan_sim, not +# /scan directly - see config/ros_gz_bridge.yaml. scan_sensor_node +# (fixed_lidar at boot, later broken_lidar once the auto-applied OTA +# regression lands) subscribes /scan_sim and republishes onto /scan: a +# clean passthrough for fixed_lidar, the real scan with a narrow blocking +# phantom wedge overlaid for broken_lidar. scan_sensor_node is the sole +# publisher on /scan that nav2 + foxglove see, whichever lidar build is +# currently applied. + +import os + +from ament_index_python.packages import ( + get_package_prefix, + get_package_share_directory, + PackageNotFoundError, +) +from launch import LaunchDescription +from launch.actions import ( + AppendEnvironmentVariable, + DeclareLaunchArgument, + ExecuteProcess, + IncludeLaunchDescription, + TimerAction, +) +from launch.conditions import IfCondition +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import Command, LaunchConfiguration +from launch_ros.actions import Node +from launch_ros.parameter_descriptions import ParameterValue + + +def _resolve_plugin_path(package_name, lib_name): + """Resolve a gateway plugin .so path inside the colcon install tree.""" + try: + prefix = get_package_prefix(package_name) + except PackageNotFoundError: + return '' + candidates = [ + os.path.join(prefix, 'lib', package_name, f'lib{lib_name}.so'), + os.path.join(prefix, 'lib', package_name, f'{lib_name}.so'), + ] + for path in candidates: + if os.path.isfile(path): + return path + return '' + + +def generate_launch_description(): + ros_gz_sim_dir = get_package_share_directory('ros_gz_sim') + nav2_bringup_dir = get_package_share_directory('nav2_bringup') + demo_pkg_dir = get_package_share_directory('ota_nav2_sensor_fix_demo') + + xacro_file = os.path.join(demo_pkg_dir, 'urdf', 'warehouse_rbtheron.urdf.xacro') + bridge_config = os.path.join(demo_pkg_dir, 'config', 'ros_gz_bridge.yaml') + world_file = os.path.join( + demo_pkg_dir, 'models', 'aws_small_warehouse', 'worlds', 'warehouse.sdf' + ) + nav2_params_file = os.path.join(demo_pkg_dir, 'config', 'nav2_params.yaml') + map_file = os.path.join(demo_pkg_dir, 'config', 'warehouse_map.yaml') + + # OTA plugin shipped via the gateway image's /etc/ros2_medkit/gateway_config.yaml. + # The plugin itself loads when gateway_node parses that params file - we just + # point the gateway at it via --ros-args --params-file below. + gateway_config_file = os.environ.get( + 'OTA_DEMO_GATEWAY_CONFIG', + '/etc/ros2_medkit/gateway_config.yaml', + ) + + use_sim_time = LaunchConfiguration('use_sim_time', default='True') + headless = LaunchConfiguration('headless', default='True') + + # Spawn pose: a verified-clear north-south aisle in the warehouse map at + # x=1.8 (checked against the committed map at the RB-Theron footprint + # radius), facing +y (yaw pi/2) so a goal further up the aisle is a + # straight drive. amcl.initial_pose in nav2_params.yaml MUST match this + # pose - the map frame and the gz world frame are identity for this world. + x_pose = LaunchConfiguration('x_pose', default='1.8') + y_pose = LaunchConfiguration('y_pose', default='-4.2') + yaw_pose = LaunchConfiguration('yaw_pose', default='1.5708') + + # robot_description: xacro-process our wrapper with bare joints (prefix:=''). + robot_description = ParameterValue( + Command(['xacro ', xacro_file, ' prefix:=', "''"]), + value_type=str, + ) + + # The world's models live alongside the warehouse models dir (also baked + # into the image's GZ_SIM_RESOURCE_PATH by Dockerfile.gateway); append it + # defensively for source-mounted runs. + set_gz_model_path = AppendEnvironmentVariable( + 'GZ_SIM_RESOURCE_PATH', + os.path.join(demo_pkg_dir, 'models', 'aws_small_warehouse', 'models'), + ) + + # HEADLESS gz: warehouse world, server only. gz_sim.launch.py wants ONE + # space-separated gz_args string; a list is concatenated without + # separators so gz never sees --headless-rendering as its own flag. + # --headless-rendering renders the gpu_lidar offscreen (EGL + Mesa) so + # /scan publishes with no display attached. + gz_headless = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(ros_gz_sim_dir, 'launch', 'gz_sim.launch.py'), + ), + launch_arguments={ + 'gz_args': '-r -s -v2 --headless-rendering ' + world_file, + 'on_exit_shutdown': 'true', + }.items(), + condition=IfCondition(headless), + ) + + # robot_state_publisher: publishes the URDF static + joint TF. frame_prefix='' + # (plain empty string) keeps frames slash-free so the + # map->odom->base_footprint->base_link->{...} TF chain stays connected for + # Foxglove (slash-prefixed frame names render broken there). + robot_state_publisher = Node( + package='robot_state_publisher', + executable='robot_state_publisher', + name='robot_state_publisher', + output='screen', + parameters=[{ + 'use_sim_time': use_sim_time, + 'robot_description': robot_description, + 'frame_prefix': '', + }], + ) + + # Spawn the RB-Theron from /robot_description at the pinned aisle pose. + # -z 0.15 lifts it slightly so the base mesh does not clip the warehouse + # floor on settle. + spawn_robot = Node( + package='ros_gz_sim', + executable='create', + name='spawn_robot', + output='screen', + arguments=[ + '-topic', 'robot_description', + '-name', 'rbtheron', + '-x', x_pose, + '-y', y_pose, + '-Y', yaw_pose, + '-z', '0.15', + ], + ) + + # gz <-> ROS2 bridge: /clock (GZ_TO_ROS) + the front laser scan + # robot/front_laser/scan -> /scan_sim (scan_sensor_node owns /scan from + # there) - see config/ros_gz_bridge.yaml. + ros_gz_bridge = Node( + package='ros_gz_bridge', + executable='parameter_bridge', + name='ros_gz_bridge', + output='screen', + parameters=[{ + 'use_sim_time': use_sim_time, + 'config_file': bridge_config, + }], + ) + + # ros2_control spawners. The controller_manager runs INSIDE gz (the + # gz_ros2_control plugin baked into warehouse_rbtheron.urdf.xacro), at root + # (/controller_manager). Spawn the broadcaster first, then the + # diff_drive_controller. Jazzy's diff_drive_controller subscribes + # ~/cmd_vel as TwistStamped unconditionally; nav2_params.yaml already + # publishes TwistStamped end to end (enable_stamped_cmd_vel: True on every + # node in the chain), so no adapter/shim is needed - just remap + # ~/cmd_vel -> /cmd_vel and ~/odom -> /odom. + joint_state_broadcaster_spawner = Node( + package='controller_manager', + executable='spawner', + name='joint_state_broadcaster_spawner', + output='screen', + arguments=[ + 'joint_state_broadcaster', + '--controller-manager', '/controller_manager', + ], + ) + + diff_drive_controller_spawner = Node( + package='controller_manager', + executable='spawner', + name='diff_drive_controller_spawner', + output='screen', + arguments=[ + 'diff_drive_controller', + '--controller-manager', '/controller_manager', + '--controller-ros-args', + '-r /diff_drive_controller/cmd_vel:=/cmd_vel ' + '-r /diff_drive_controller/odom:=/odom', + ], + ) + + # The gz_ros2_control hardware interface only exports the wheel command + # interfaces once gz has stepped a few cycles, so the spawner above can + # load + configure diff_drive_controller but lose the activate + # transition - it stays inactive ("Can't accept new commands, subscriber + # is inactive") and Nav2 then rejects every goal ("Goal rejected by + # server"). Retry the activation until it sticks so the robot is + # drive-ready on a cold boot with no manual + # `ros2 control set_controller_state ... active`. Bounded to ~120s so a + # genuinely broken hardware interface still lets the launch settle. + controller_activator = ExecuteProcess( + name='controller_activator', + output='screen', + cmd=[ + 'bash', + '-c', + # Both spawners race the gz_ros2_control hardware readiness: whichever + # switches first (usually joint_state_broadcaster) can time out its + # activate and die. Without an active joint_state_broadcaster there are + # no /joint_states, so diff_drive_controller publishes no odom TF, the + # `odom` frame never appears and Nav2 fails every goal with "Failed to + # make progress". Retry activating BOTH until they stick. + "for i in $(seq 1 60); do " + " L=$(ros2 control list_controllers 2>/dev/null); " + " if echo \"$L\" | grep joint_state_broadcaster | grep -qw active " + " && echo \"$L\" | grep diff_drive_controller | grep -qw active; then " + " echo 'joint_state_broadcaster + diff_drive_controller active'; exit 0; " + " fi; " + " ros2 control set_controller_state joint_state_broadcaster active >/dev/null 2>&1; " + " ros2 control set_controller_state diff_drive_controller active >/dev/null 2>&1; " + " sleep 2; " + "done; " + "echo 'controller activation gave up after ~120s' >&2", + ], + ) + + # use_composition=False forces nav2 to launch each lifecycle node as its + # own process instead of co-loading them into component_container_isolated. + # The Jazzy apt build of nav2_msgs has an ABI mismatch against the + # fastcdr 2.2.5 currently shipping with ros-jazzy-fastcdr (missing + # eprosima::fastcdr::Cdr::serialize(unsigned int)) which immediately kills + # the container at composition time. Per-node mode dodges that crash. + nav2 = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(nav2_bringup_dir, 'launch', 'bringup_launch.py'), + ), + launch_arguments={ + 'map': map_file, + 'params_file': nav2_params_file, + 'use_sim_time': use_sim_time, + 'autostart': 'True', + 'use_composition': 'False', + }.items(), + ) + + fault_manager = Node( + package='ros2_medkit_fault_manager', + executable='fault_manager_node', + name='fault_manager', + namespace='', + output='screen', + parameters=[{ + 'use_sim_time': use_sim_time, + 'storage_type': 'memory', # clean Faults Dashboard every run + 'healing_enabled': False, # stored DTC: only an operator DELETE clears it + 'confirmation_threshold': -1, # immediate confirm -> capture fires at once + # Freeze-frame snapshots of the relevant topics at fault confirmation. + 'snapshots.enabled': True, + # On-demand (not background cache): subscribe to each topic at + # confirmation and wait up to timeout_sec for a message. The + # background cache intermittently missed /cmd_vel and + # /local_costmap/costmap (1/3 or 0/3 captured); on-demand reliably + # grabs all three for the freeze-frame. + 'snapshots.background_capture': False, + 'snapshots.timeout_sec': 2.0, + 'snapshots.default_topics': ['/scan', '/cmd_vel', '/local_costmap/costmap'], + # Ring-buffered MCAP around the fault (Foxglove-native, downloadable over SOVD). + 'snapshots.rosbag.enabled': True, + 'snapshots.rosbag.format': 'mcap', + 'snapshots.rosbag.lazy_start': False, # keep buffering so the pre-trigger window is captured + 'snapshots.rosbag.duration_sec': 5.0, + 'snapshots.rosbag.duration_after_sec': 2.0, + 'snapshots.rosbag.include_topics': ['/scan', '/cmd_vel', '/tf', '/tf_static', '/local_costmap/costmap'], + 'snapshots.rosbag.storage_path': '/var/lib/ros2_medkit/rosbags', + }], + ) + + foxglove = Node( + package='foxglove_bridge', + executable='foxglove_bridge', + name='foxglove_bridge', + output='screen', + parameters=[ + {'port': 8765}, + {'address': '0.0.0.0'}, + {'use_sim_time': use_sim_time}, + # /tf publisher uses history depth 210 (15 nav2 publishers x ~14 + # transforms). foxglove_bridge defaults max_qos_depth to 25, + # which silently drops most TF samples and breaks Foxglove's + # TF chain reconstruction - costmaps drift off the map, robot + # mesh floats off /scan, /amcl_pose lags. Bump to 1000 to + # accept the full nav2 fan-in. + {'max_qos_depth': 1000}, + ], + ) + + # broken_lidar/fixed_lidar (scan_sensor_node) own /scan by subscribing the + # real gz laser on /scan_sim and republishing it onto /scan (fixed_lidar as + # a clean passthrough, broken_lidar with a blocking phantom overlaid). + # fixed_lidar boots by default; the entrypoint auto-applies broken_lidar_3_0_0 + # shortly after boot as the regressing OTA update. + scan_sensor_node = Node( + package='fixed_lidar', + executable='fixed_lidar_node', + name='scan_sensor_node', + output='screen', + parameters=[{'use_sim_time': use_sim_time}], + ) + + # Operator-invoked differential-diagnosis node (4 Trigger operations: + # lidar/localization/drivetrain/costmap health checks). Independent of + # scan_sensor_node - it subscribes /scan directly rather than + # depending on the broken_lidar/fixed_lidar swap, so it survives the + # OTA update untouched and answers "broken" before the fix, "healthy" + # after it. + health_check_node = Node( + package='health_check', + executable='health_check_node', + name='health_check', + output='screen', + parameters=[{'use_sim_time': use_sim_time}], + ) + + # Plugin overrides + node params come from gateway_config.yaml. The .so + # path is pinned absolutely there (/ws/install/...), so we don't need to + # resolve it via _resolve_plugin_path the way the earlier demo did. + _ = _resolve_plugin_path # kept for parity / future overrides + + gateway = Node( + package='ros2_medkit_gateway', + executable='gateway_node', + name='ros2_medkit_gateway', + output='screen', + parameters=[gateway_config_file], + arguments=['--ros-args', '--log-level', 'info'], + ) + + # Fault surfacing: generic ros2_medkit bridges, not a custom fault in the + # scan nodes. The phantom (broken_lidar) blocks the path so Nav2 genuinely + # fails on its own; these two bridges turn THAT failure into SOVD faults: + # - log_bridge promotes controller_server's own "Failed to make + # progress" /rosout ERROR to a LOG_* fault on controller-server. + # - action_status_bridge promotes navigate_to_pose's GoalStatus + # ABORTED to an ACTION_NAVIGATE_TO_POSE_ABORTED fault on bt-navigator + # (the action server's node). + # Neither bridge is told about the phantom; they only see Nav2's own + # downstream symptoms, so the operator has to investigate and correlate + # the fault back to the recent lidar update - not read it off a + # self-reported code. + log_bridge = Node( + package='ros2_medkit_log_bridge', + executable='log_bridge_node', + name='log_bridge', + output='screen', + parameters=[{ + 'use_sim_time': use_sim_time, + # ERROR only, so startup INFO/WARN chatter never promotes - just + # the controller's "Failed to make progress" once it truly stalls. + 'severity_floor': 40, + 'include_only_nodes': ['controller_server'], + 'code_prefix': 'LOG', + 'exclude_medkit_stack': True, + }], + ) + + action_status_bridge = Node( + package='ros2_medkit_action_status_bridge', + executable='action_status_bridge_node', + name='action_status_bridge', + output='screen', + parameters=[{ + 'use_sim_time': use_sim_time, + # Top-level goal only - not the BT's internal /spin, /follow_path, + # /backup sub-actions. + 'include_only_actions': ['/navigate_to_pose'], + 'aborted_severity': 2, # SEVERITY_ERROR + 'canceled_is_fault': False, + 'code_prefix': 'ACTION', + }], + ) + + # Start the bridges once Nav2's lifecycle bringup + controller activation + # have had time to settle, so they watch the real running stack rather + # than transient startup state. Neither bridge needs this for + # correctness (log_bridge only cares about controller_server logs after + # a goal is sent; action_status_bridge rescans for new actions every + # rescan_period_sec), but it keeps the fault set clean. + bridges_after_nav2 = TimerAction( + period=15.0, + actions=[log_bridge, action_status_bridge], + ) + + return LaunchDescription([ + DeclareLaunchArgument( + 'use_sim_time', default_value='True', + description='Use simulation (Gazebo) clock if true', + ), + DeclareLaunchArgument( + 'headless', default_value='True', + description='Run Gazebo without a GUI - default True for Docker/CI ' + '(the only path wired here)', + ), + DeclareLaunchArgument( + 'x_pose', default_value='1.8', + description='Robot initial X position (warehouse map frame)', + ), + DeclareLaunchArgument( + 'y_pose', default_value='-4.2', + description='Robot initial Y position (warehouse map frame)', + ), + DeclareLaunchArgument( + 'yaw_pose', default_value='1.5708', + description='Robot initial yaw, radians (warehouse map frame)', + ), + set_gz_model_path, + gz_headless, + robot_state_publisher, + spawn_robot, + ros_gz_bridge, + joint_state_broadcaster_spawner, + diff_drive_controller_spawner, + controller_activator, + nav2, + fault_manager, + foxglove, + scan_sensor_node, + gateway, + bridges_after_nav2, + health_check_node, + ]) diff --git a/demos/ota_nav2_sensor_fix/ros2_packages/ota_nav2_sensor_fix_demo/maps/warehouse.pgm b/demos/ota_nav2_sensor_fix/ros2_packages/ota_nav2_sensor_fix_demo/maps/warehouse.pgm new file mode 100644 index 0000000000000000000000000000000000000000..6fe698e2bcfcbfb77f0c8b733cb442f1d68070fc GIT binary patch literal 161855 zcmeI5X_DkPj)m)YpJI+M)@EhX$LNdamC6S1VIe6(a`_taAQ%xM32u0wKuR8&S@qw) z|MlCYeJvI@%hmaj@h;3k28ZuVHL zg8?Zni{1BUKgwE>px5k_{3AQd~A%>`>{s^yO{u zHR?jZAOwqMS_46NZu5iE7I3~2ZuaxTRt*kBKNya0=GisoNhI6Okx*mEHwV5&q2cpBk+Q6j}f z!opAhi=8z1ONgtd2oOd~J(4^g$ORYWQs;|-ArfMdl7)+u_*ZzH-GVtufC$cAJ8D?d zz*&%pN=CQ<2f0|dXnjc}eBR)%y!TupWXNe1F5{zD|4l#yiv$d|kpba``ez)l2&OJh zL@{_UY00)b@qDKXC{%{*<&26i&XgZ->latV67csQ*@k03o zqb2Mr+gr>TLXPg&=ZFnU8Wd9T=-}~U&!R@nyDyRlAl)}YCsKe~1jQ(`fdP}?g6+cq zf^DJH)Di$7;5*^65R^_TakM>z7_r2n!RiLAQbdr%4=6=y_ElI6IvFtt4vUT8(o94D zMJm`hB2^)D@dC4>S_H))liH08NO-ugXdyHVSfxT3BGz)}0zwu>tB}~}M6g(3kd2eE zowVt_NVP>v8ZgRi5f;5nZWrA%e$uvhS@?L-rw-2&3mC^DxmcD>JX|F(m~&TwhbD4a z@(+)6?+^zeG$sg_h2sS~g#-{J1Yxn_T-N-o5?_u*46v=r1;Ph5;1LN_mW&q$1W^!=hK*ryU~BFp1_a3P z8(=hPF7-uU>uetpNTcFANn00;mzho9vq*V6ej$e6QojMUNC?)KH-$KMTIU zG%ku@j1|=6cv&)D5D)l9Y)r`-R}C2Qa6mmA!L{aV9U`FzF_qD}WVkdgDIi4As);8I zFEg6exj?`1+3uxP!-YYD3kyyqD$8O~B*DSKc)l)|4oO#ZE^EHkF&Z2}`gXT&yy(G_ zgbI1B%SB&dcDSthY1)FJZ6q&6zAzVj2--=j{`Lj&f*1WAEHE$pJW8KKj%sBtM*&k@ zJVM5NSuXmEgb{|;cKwPBiu~PeJxZ!fH1etUfE%~?L z`Hbd~OISN#29WDijLYs5Y0-06vB(wQX<8>ldZqp_5-2!o=CowIC>2~8FbS76{}!2Y z!44vrIa=2Iy*G~)Mhn15AxSIlBfw}sEKOvVmWuLDD-p=mxFzz zB=Enj!>1@ga^k}j;bDZVdyXO*e)vKzh!)b>Ibje3lmPA2@8W=@OK}!?)JPj*NUHem z(?5R!3(|)bpIc6&1rg(f=73T6&s<&V(AVS-HmP7qWS8UuEQl1%g$*lPG)1TUUo1@v=M$Z7yu@h}GaGQNr@yXSi<_Mc+dV5Ka-;=I>vYRN^XtBa{ zW8aHLPVo!xBhxNSK8!^czBm`%M4!iUbuJbvuK{y@mdl>I-Odp;an!u`ROx01m`|Rk zC)esc{45j;sjEJZdM;8vzX3J_hw2@(NVwpo>2hh%^Vx{u(q9oTciiWZ=M%LrIA28F z$IsoXzSnK<^czCvc*@Jii-yXvh%Ln@>ao!EpVO~;9Nk6h^6_G7;y&J`?(xpo=K><1 zroxYn*MEO`{I(AxPtc7n*N>Oag-bx;pB_k*UM~&z`3EZUG%79$_u`U35fhG=3+|)b z*$-MT8ZQHZ?l>Im@2q?Smv`#loxAo?il)w&6pI?`^<{=5`{;UDe&VFvpsDj^1H`J= z{Ct%A`a$bOe~e_Vf!E~{2nH}-spqtl7;}l23qFpH@8zh#YJ73ma#u{dhViK5 zC9&Mig?~I4dh>#84epNCy5+)Uwa-=m@@{kfL#6ophPvK0xlAnX4xnj=G@-CBe;C;fj}p%mu^c6VK_*N2MMNm;Z(5%Yo!x{&D%2aOpo#?{V3FeBOS%Hc+kG z_n$94mL0h;Z-{NG$_3q|grD!{Vz=6q4YjDu6lM!LH|y27gp~7zCT4$3+aUXWq@{KH z1zZl5KH6AZa&pC`xa0(KYsJg;x#S-HjP=Lk5JA8u6kBfZ3fwypZ zz>lj9c@>wd>3Y_mhq*kv@WscUN6QmO!8G-6w9qYwV5%@mYc-yf+-4i)a&zn5YrSc}Tf9>)5idJy%;wqhCe=SQt)II=CiNpI9Cu5{ey0)t4O;F{ zRU7~YHDblzl<9YJ-f|Lof{$Q=Zv!DCc|T7_>n40jl-~Su57qS@T9+CNFXGpZwAr%4rLoIng&a1><0a4@!SSE{Xh{dkUJhH5;!S?)cB(`-o1Em z!CvBbw&KiXwq)bJ*XEy+g~>rWOW|z4gQYIg1TM!+#8LTw0!{ADmZ{6P8Xzt9(Xoh` z!fZL7;fq?yWuvdJwm9moCNMGW9NJ$*q`B(nOIS>AV-hW6A>}f5`PLew4O{?2j1!aJ z{vmO51IficA1RCNTkk|DX86V7qVIJcAM(MPff9n93Gy%om(0`}zrVzN1SYn75&{r8 znJ2^dqX-4ihF|Z7K??cXOhO?s_49mtS&o;EixgA6i7_#KI}7(8(&D|&!p471<$W}t zt-lxxB`X&TnJ#iVF0jrRW=l8ZyNisM`1OSb%to%0+L%*P-wk8qy}j2(!1Mqd%4WzC zRpGLIAZ>K#+bG#tGOnk(ndZx?a=t(e#OqHgEj1=e&-P1(K5ja^3vn)7{CVVH z=0(tw1j5|$aKSe=zT6YzqHi$VO~OEoBryoFrTd1LdowKf;}we+C|1>}UKo2QKkoM1 z9RQaGWjun97Ekdd+wEwME4Rr<#`Z}7fVP4eCR=0D@5&+{@Xg*|Z~*J*MoH>sn_xY# z&=(EisiBW)ry|}Du@SH)SwlB(O^x38HMw$;Na?5q7Z8yj1(WH`Y+ob&&{5?QPz-@) zgO z7et9uk@EJR1f%&^hsJcbp_Z?8+&{|3&zrGx!X+_{V?I?o_d&V)PmLWq=Cb^4*Phi$ za1fezKb%t<&-S}D&l}qBb4}yJwvomSCeTt}*m!#1Dx1MyP{sKID*BB)Q!KV1Wumff zKcn}YC+B)WJ$nii%OPqMAGP5#)6al(kQs33xhxNf*q zgzMhp@=a1t`wFpmd+Dk0dE}36r6^|{GPv{*j`@(z+BjQ_-s+7^^{-Q^UcQd zj-aQWyY`rdXLu$v0Z$-GCQVE_@K;=MzL1I_z_$|$B2+mtK-={G_bX^L5z57pkOp3W zfv9zo%1B0B4sIBSS(e!}bZsXT7r}I^QZhG4$5dFKLH=vX6oaxWJN3KHFvshUL~|!mFDk zUI;_{v`>$jwmgWgq0QebZg_L&9pmc?$tWHpumv&CI$PjaZoT31bImVZl9S=0?{(Zj zdchwkj8ssY7oRPZw_|=z`iDo83qK$c1Of&r%M!VXDdVSee%;mM#exO?t6-zEFDT`* z-~G#4{M8m16h)?2Y&SVkcs6(@a<11r?n4&1r%&Z!3x(H)Lb5P*o@KVK`Jnjt$b(5= z+=S;5sfc=VFc`MXQ7!Z28zg6b4>pL<*B6)rLDMECwKh{su=6XZq-PBIrUE_(AQ=^8 zG+3Q&YTH=6h51q2(D7?3e6I^EbQ@gsqcv!$sGW4C09VC>DLgYk{#XwfcC(cJ>eJiu-!J z?zwK!i|1|=C8gPE~G(9A& zgP>B$j^&QkleN>)XZxjtUuc9MBG@j%!ZdIHKmeWEHgX4WUe8Kqi}X*7iODDa@f^P4 zaFGj2hDG71MUBLQen@)l3pD#BWp!l;?s&`Tt-aSN8i*D=y$nEOYU8?6-&4)ArR)AB zMQ9Msk}+Ty3y2y!UsQ!ri7*+oouhEB*D&#qR+I~W(cpe?Gxa(GOA032HQ8z6(_7vz zuxWZx^{ExeWx|5k5xvU6v9Ow=>4aqMPdV7l{mCM^$Tnh-$imcRJm80)EmCAx*jkz> zYz+t|KHFdWPSZTNqXhp?-y({I)1rvq0HH-T>5)r3GOfwBY&@Oep*D1j%L`AJ&;@IC z9bjr{x1n`;QR&Qm;&U3J>{-ns*SBav6Vz<5NS9HFwKtuoo%?>-`USr1Uh!=4miors@ zJp&h7(A+yJ)-;WFpC?9QGFc4q!oZO^7&~4W=Zs;a`L(r!MZ{0&wlBjX@xo+V3JQ9M z6u@ALX0w8+l4(28Ecv*Y-~t7Bs9Xe#hX&Co6tzI5-8K)`waQ{bl)6fZ{-3o;<;V9gK#D=c1)5FgFuSpb-L^5 zMkD*+h{3Gri^-WuDIMl3^!e$I{hC6f02GvQ&9|TQKj$~6e)EM~DpYJn)`NQBXcFwJtp!s6XE@Zx0X=0SSWO()?wxA)2e(y8k8Al zyt2Oc{K)6K$p*t79J({NO0drX4GRN?T68C1^mr%$l>9VrI&${*ECeu*WPG+M8bu_- z+Q=Q(PIcKG`-+CxN2)@vkPTs~Um>`5&Amn!hKh}qOfev2w2Zl>lyL#56R`FK4?~B_ z-iSjxW{`bE7QDcJ@tsPbPk&h8L})skQPedWV2iT~(-4g|?fJcDzp>yOn#sl9Zb`&0 z_wbEYHF21KPj3zZePUrZX;0)Pg#hBEMSpu51?BUk^n|33MXOnvYJkDIZ~nk zi8t3cC<(fc7GObwQY&cHJ`l1=Tcihe%?682&k1Y_>;97L38XtiVFnbtxg!?pgX04Y zRU#(&o@WIS9w$1O-E?~JB^VhtA51#ygV))ObfWF5JHy;;Sja?F;Tz8=s?a+OjpTsL zQ(tylbjLLQ6QUV0=+W`4QP`XVD0^?$+V^KnE;!?i0Pl5W@HXE`|oM9U3^N#t+ZZ^nu5ROp* zi^fAs{j3`-Rs|Sn2S^a%MXvUMLX+9j=;#-?N3QgphK@k)t#LYjJ~ADSh0;(lLpSkY zL?sdjDHdKi79iwD%Z?>KUs6~x`^O97HxfLj_j=tL{FYcR^)mFuSWLw8$|eIAebe)3 zfkj^fKCbk>1@R)L6hfWdW9|n|hL%mNiV0S~YKaq(1$bzXNQs{zu!gqdX}6h;Gi`0# zIy+^T(;c(Hdp2`hA-EXPApP|kEk1*rSywnHQa5;TC~5)1o7BXQUq0DE{ua68dIw9` zJthSDt-54{D(tW{20FXV%6k2yzNLE9z4;)DR71S`T;yV(yudpO zk!$-_6sR+?1+1+Oqu7v99m+~XhCnn1NWvA9v?8ob7(1mI)nL)p2UAQjTW3pjK2gAE z6Qb@@hst(Q=z*y;IMh44YDO%Oiz$#S=UlA#M$%@vru*o=Fz?mP5N1o?|7Mw7cz>Y< z0`(i8^{j+BsJ~szA_gvxaYVt3zV<5}osKlLjYYI&Y0@$oFzUQ9TjLHJ;sp#Kj{-IK z<3*`RQ*(&@*h_6XRIEtOLUY<|em3i+&DA@aTl*e+<$FfTi5Ut`s1fVqQ zUDB!98rU#Kl3@|E7v0RIX)Q@weK-uWq7ILC{+oF|9;M@l4)N{4=19z;Q$9fkRN1{ z2+u2OeY@fpEC9#FYt;fZECZDa_e?p_>Hs0xdINxQh#WQ~nLljOAC~N$f3i&mUHswe zIauCHvA}hesG>!Ptj{YhYz&9YLaTRyTus2RO_&17(8=1mJQjIAB>-JrH+j;lYn_U6 z_jnT(3nk-%iIIDz3QPxkNjr9+nB)?YhZ0aAX^*ZC#+*orx)Y<&WgeNHX~}Fp5?s`E z%T$ImQ!@lW1G|S9?PFawSjKq4s}N>VJj{3}W{>W?pZx@`rMJ!HbG>|=co|qIR5XhW zG_$1Q?Ho2#=wYoUT7^?Kxp*D9gK-2V3^HUx3MR%uiER>8mP%N^P!NEv^ z&;mgO76pV+L-M-j8ZT}GsjWAxZ>`&UI_Paizn|eefhZgTF$U(PG{C?XorF1opN)X#7qXdu^ENx6G!$mn=fjgk5r~@ACabq zCD<4k?a!)TxIr{{Og^&ld87>^7DS^j-}#l8y4L%Ox90M@y+11)cPQKfwwFY-B*Ow; zQN;iV1w6U1gsZ2UU^p+P8L>w-F(D;y5en7^ z`C-z7w{_Wf=F=@ZJYj}B-hSlNY6jxaGTh}9)`vaWTTwO1D9!OQf{WQu=+flmWav6S zP`qzCoFw#Hu_!0U!5l6iV$3M)tiOH^7@=gf5eufuju)6W={_Z!!Rqv691J@Pz%2LI zWH#YK0FNI4z=fIgjk;Lz4Wt=|jctmb!8NaN9k0A4&mkmMnoI-fW(~{#;HyxWAEM=)#Jdh5Cu6906kR{{5_RoAtH)B7cUzwm^2NJ znL^ra#&?p;JbyOOdyz}SqCp^aY$R8Jk~T;QGUY-8D=^%XT-aIF-v4ts*iezmb~+$p zQGyPv@+?8ck-(g9naMgDD9`UouXVCbhQ9ax<$1cVs#l9P$+l+(n7{HDJ8^9Y}`b#hhp${Lcn((sMcMvCY!Y%%>E4C(kjnm7A%RbYc9H zJurcGxw62Ebfm#7=%lzBkwI`XAC`1gzrq`f??%cR)6d+})+z9!=Q?sEIJVi`!z^JN z3gAHk9Tr$n)e8$=h4EH8c-po;$dNo z^m$}_z=5xDjY%urg0yMxRu;kjVIHfhZwMRr(sMCHF~;}m=2q*rbJp(mkJ%9z5fxH4 zETDv<%12EfJuG2>7dEQ1DtD?k==?5Stc(|Rr}MTehO2J#JYf}YAKphCmE^UUYRBg7 zL~iGc?Gxzb7v9;V05GT{X|5JEJ*qV5>1F#Nxj{LWO`1j%!bak*+SoFreISo zni~kcwZTNOI3tsEZx%=tg$*|>vzN1FnBaD0pq+F-Sb)FZwP2uXhoc_~gOgUFq_N2( zW3q-J!opsr8<&mGqrbP*Z3fR(oo<|T?loGt#Tgs2JXMQwvcbhHUD3Lzq)==MZ)CPx zRHsEE=1WtPF?MM77&e(ELv$SrJN;nRqYqAk0WV_2H!97hMelyuD`3Vgqb@TPh{a$D2H1$}UL@$RoE?r-ZSqdLcsAB@ zA(OiCIgaM3i;AAilY#wUrC1RxCH0;K@eejNsRZpwbu{Bbcv0Icv|g0$(!IO7EL!{> z#tnPwqP;830%uFt{ivvKM&=#vloa(xD{6rWRuyLzmz3p=!qCdu=nlAu(*gC!$F%J} z#eN%N?R3t`2a^KL2nI-2AZRhW zcCxA>!#?8gh6F)-;P?@txz|F-a~a6U1_HjEn5*1bO`6u zj#N#}XoY%qlOo!2;T-6}_TWPc2UrUY;vB(Hy;`)06GmEag#i6E3X9Zp=Y?y)F-DL*2);9%1o<3_ob~UY?`(K{C}Lkgi9~3W{TEeWX5McD~^r!AnhMhAmvz zYIGagniZIBkdNd*sdzm)>q=07)zGd*t)V;GPN(~NpPZI1wZ6j|>Ytd@VHD6)g!lkc z0z=rmZRxydEPeY+d=V%DMW6^2fg(@@ia-%40!5$*6oDd81d2crC;~;G2o!-LPy~uV z5hwyhpa>L!B2Wa1KoKYcMW6^2fg(@@ia-%40!5$*6oDd81d2crC;~;G2o!-LPy~uV z5hwyhpa>L!B2Wa1KoKYcMW6^2fg(@@ia-%40!5$*6oDd81d2crC;~;G2o!-LPy~uV z5hwyhpa>L!B2Wa1KoKYcMW6^2fg(@@ia-%40!5$*6oDd81d2crC;~;G2o!-LPy~uV z5hwyhpa>L!B2Wa1KoKYcMW6^2fg(@@ia-%40!5$*6oDd81d2crC;~;G2o!-LPy~uV n5hwyhpa>L!B2Wa1KoKYcMW6^2fg(@@ia-%40!5$*91!?_^9(^b literal 0 HcmV?d00001 diff --git a/demos/ota_nav2_sensor_fix/ros2_packages/ota_nav2_sensor_fix_demo/models/aws_small_warehouse/worlds/warehouse.sdf b/demos/ota_nav2_sensor_fix/ros2_packages/ota_nav2_sensor_fix_demo/models/aws_small_warehouse/worlds/warehouse.sdf new file mode 100644 index 0000000..56c0bf2 --- /dev/null +++ b/demos/ota_nav2_sensor_fix/ros2_packages/ota_nav2_sensor_fix_demo/models/aws_small_warehouse/worlds/warehouse.sdf @@ -0,0 +1,210 @@ + + + + + + + ogre2 + + + 1 1 1 1 + 0.8 0.8 0.8 1 + false + + + + true + 0 0 10 0 0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + -0.5 0.1 -0.9 + + + + + true + + + -0.3 -5.9 1.9 0 0.28 0.97 + + 30 + 1 + scene_cam + + 1.3 + 1600900 + 0.160 + + + + + + + + aws_robomaker_warehouse_GroundB_01_001 + model://aws_robomaker_warehouse_GroundB_01 + 0.0 0.0 -0.090092 0 0 0 + + + + aws_robomaker_warehouse_WallB_01_001 + model://aws_robomaker_warehouse_WallB_01 + 0.0 0.0 0 0 0 0 + + + + + aws_robomaker_warehouse_ShelfF_01_001 + model://aws_robomaker_warehouse_ShelfF_01 + -5.795143 -0.956635 0 0 0 0 + + + + + aws_robomaker_warehouse_ShelfE_01_001 + model://aws_robomaker_warehouse_ShelfE_01 + 4.73156 0.57943 0 0 0 0 + + + + aws_robomaker_warehouse_ShelfE_01_002 + model://aws_robomaker_warehouse_ShelfE_01 + 4.73156 -4.827049 0 0 0 0 + + + + aws_robomaker_warehouse_ShelfE_01_003 + model://aws_robomaker_warehouse_ShelfE_01 + 4.73156 -8.6651 0 0 0 0 + + + + + aws_robomaker_warehouse_ShelfD_01_001 + model://aws_robomaker_warehouse_ShelfD_01 + 4.73156 -1.242668 0 0 0 0 + + + + aws_robomaker_warehouse_ShelfD_01_002 + model://aws_robomaker_warehouse_ShelfD_01 + 4.73156 -3.038551 0 0 0 0 + + + + aws_robomaker_warehouse_ShelfD_01_003 + model://aws_robomaker_warehouse_ShelfD_01 + 4.73156 -6.750542 0 0 0 0 + + + + + aws_robomaker_warehouse_Lamp_01_005 + model://aws_robomaker_warehouse_Lamp_01 + 0 0 -4 0 0 0 + + + + + aws_robomaker_warehouse_Bucket_01_020 + model://aws_robomaker_warehouse_Bucket_01 + 0.433449 9.631706 0 0 0 -1.563161 + + + + aws_robomaker_warehouse_Bucket_01_021 + model://aws_robomaker_warehouse_Bucket_01 + -1.8321 -6.3752 0 0 0 -1.563161 + + + + aws_robomaker_warehouse_Bucket_01_022 + model://aws_robomaker_warehouse_Bucket_01 + 0.433449 8.59 0 0 0 -1.563161 + + + + + aws_robomaker_warehouse_ClutteringA_01_016 + model://aws_robomaker_warehouse_ClutteringA_01 + 5.708138 8.616844 -0.017477 0 0 0 + + + + aws_robomaker_warehouse_ClutteringA_01_017 + model://aws_robomaker_warehouse_ClutteringA_01 + 3.408638 8.616844 -0.017477 0 0 0 + + + + aws_robomaker_warehouse_ClutteringA_01_018 + model://aws_robomaker_warehouse_ClutteringA_01 + -1.491287 5.222435 -0.017477 0 0 -1.583185 + + + + + aws_robomaker_warehouse_ClutteringC_01_027 + model://aws_robomaker_warehouse_ClutteringC_01 + 3.324959 3.822449 -0.012064 0 0 1.563871 + + + + aws_robomaker_warehouse_ClutteringC_01_028 + model://aws_robomaker_warehouse_ClutteringC_01 + 5.54171 3.816475 -0.015663 0 0 -1.583191 + + + + aws_robomaker_warehouse_ClutteringC_01_029 + model://aws_robomaker_warehouse_ClutteringC_01 + 5.384239 6.137154 0 0 0 3.150000 + + + + aws_robomaker_warehouse_ClutteringC_01_030 + model://aws_robomaker_warehouse_ClutteringC_01 + 3.236 6.137154 0 0 0 3.150000 + + + + aws_robomaker_warehouse_ClutteringC_01_031 + model://aws_robomaker_warehouse_ClutteringC_01 + -1.573677 2.301994 -0.015663 0 0 -3.133191 + + + + aws_robomaker_warehouse_ClutteringC_01_032 + model://aws_robomaker_warehouse_ClutteringC_01 + -1.2196 9.407 -0.015663 0 0 1.563871 + + + + + aws_robomaker_warehouse_ClutteringD_01_005 + model://aws_robomaker_warehouse_ClutteringD_01 + -1.634682 -7.811813 -0.319559 0 0 0 + + + + + aws_robomaker_warehouse_TrashCanC_01_002 + model://aws_robomaker_warehouse_TrashCanC_01 + -1.592441 7.715420 0 0 0 0 + + + + + aws_robomaker_warehouse_PalletJackB_01_001 + model://aws_robomaker_warehouse_PalletJackB_01 + -0.276098 -9.481944 0.023266 0 0 0 + + + + diff --git a/demos/ota_nav2_sensor_fix/ros2_packages/ota_nav2_sensor_fix_demo/models/person_human/model.config b/demos/ota_nav2_sensor_fix/ros2_packages/ota_nav2_sensor_fix_demo/models/person_human/model.config new file mode 100644 index 0000000..1bec064 --- /dev/null +++ b/demos/ota_nav2_sensor_fix/ros2_packages/ota_nav2_sensor_fix_demo/models/person_human/model.config @@ -0,0 +1,13 @@ + + + person_human + 1.0 + model.sdf + + Static human-shaped obstacle for the OTA nav2 sensor-fix demo. A single + collision box (0.6 x 0.5 x 1.7 m) trips Nav2's costmap/controller via the + front_laser /scan, while the visual is a recognisable human built from + mesh-free primitives (legs box + torso cylinder + head sphere). No + external mesh dependencies. + + diff --git a/demos/ota_nav2_sensor_fix/ros2_packages/ota_nav2_sensor_fix_demo/models/person_human/model.sdf b/demos/ota_nav2_sensor_fix/ros2_packages/ota_nav2_sensor_fix_demo/models/person_human/model.sdf new file mode 100644 index 0000000..1b92ef4 --- /dev/null +++ b/demos/ota_nav2_sensor_fix/ros2_packages/ota_nav2_sensor_fix_demo/models/person_human/model.sdf @@ -0,0 +1,74 @@ + + + + + + true + + + + 0 0 0.85 0 0 0 + + + 0.6 0.5 1.7 + + + + + + + 0 0 0.45 0 0 0 + + + 0.34 0.24 0.9 + + + + 0.12 0.13 0.28 1 + 0.15 0.16 0.35 1 + 0.05 0.05 0.08 1 + + + + + + 0 0 1.2 0 0 0 + + + 0.2 + 0.6 + + + + 0.55 0.30 0.12 1 + 0.78 0.42 0.16 1 + 0.10 0.06 0.03 1 + + + + + + 0 0 1.6 0 0 0 + + + 0.12 + + + + 0.70 0.56 0.45 1 + 0.90 0.74 0.62 1 + 0.12 0.10 0.08 1 + + + + + diff --git a/demos/ota_nav2_sensor_fix/ros2_packages/ota_nav2_sensor_fix_demo/models/person_obstacle/model.config b/demos/ota_nav2_sensor_fix/ros2_packages/ota_nav2_sensor_fix_demo/models/person_obstacle/model.config new file mode 100644 index 0000000..03297d8 --- /dev/null +++ b/demos/ota_nav2_sensor_fix/ros2_packages/ota_nav2_sensor_fix_demo/models/person_obstacle/model.config @@ -0,0 +1,11 @@ + + + person_obstacle + 1.0 + model.sdf + + Static person-sized obstacle (0.5 x 0.4 x 1.7 m box) for the OTA nav2 + sensor-fix demo. Spawned in front of the RB-Theron to trip Nav2's + costmap/controller. No external mesh dependencies. + + diff --git a/demos/ota_nav2_sensor_fix/ros2_packages/ota_nav2_sensor_fix_demo/models/person_obstacle/model.sdf b/demos/ota_nav2_sensor_fix/ros2_packages/ota_nav2_sensor_fix_demo/models/person_obstacle/model.sdf new file mode 100644 index 0000000..0cf1838 --- /dev/null +++ b/demos/ota_nav2_sensor_fix/ros2_packages/ota_nav2_sensor_fix_demo/models/person_obstacle/model.sdf @@ -0,0 +1,34 @@ + + + + + + true + + 0 0 0.85 0 0 0 + + + + 0.5 0.4 1.7 + + + + + + + 0.5 0.4 1.7 + + + + 0.15 0.15 0.18 1 + 0.15 0.15 0.18 1 + 0.05 0.05 0.05 1 + + + + + diff --git a/demos/ota_nav2_sensor_fix/ros2_packages/ota_nav2_sensor_fix_demo/package.xml b/demos/ota_nav2_sensor_fix/ros2_packages/ota_nav2_sensor_fix_demo/package.xml new file mode 100644 index 0000000..d70da01 --- /dev/null +++ b/demos/ota_nav2_sensor_fix/ros2_packages/ota_nav2_sensor_fix_demo/package.xml @@ -0,0 +1,51 @@ + + + + ota_nav2_sensor_fix_demo + 0.1.0 + Launch + config glue for the OTA over SOVD nav2 sensor-fix demo (RB-Theron AMR + Nav2 + AWS warehouse + broken_lidar/fixed_lidar swap). + bburda + Apache-2.0 + + ament_cmake + + ros2launch + ros_gz_sim + ros_gz_bridge + ros_gz_interfaces + + xacro + robotnik_description + robotnik_sensors + robot_state_publisher + gz_ros2_control + ros2_control + ros2_controllers + diff_drive_controller + joint_state_broadcaster + nav2_bringup + nav2_amcl + nav2_bt_navigator + nav2_controller + nav2_planner + nav2_behaviors + nav2_costmap_2d + nav2_lifecycle_manager + nav2_map_server + foxglove_bridge + ros2_medkit_gateway + ros2_medkit_fault_manager + + ros2_medkit_log_bridge + ros2_medkit_action_status_bridge + broken_lidar + + + ament_cmake + + diff --git a/demos/ota_nav2_sensor_fix/ros2_packages/ota_nav2_sensor_fix_demo/urdf/warehouse_rbtheron.urdf.xacro b/demos/ota_nav2_sensor_fix/ros2_packages/ota_nav2_sensor_fix_demo/urdf/warehouse_rbtheron.urdf.xacro new file mode 100644 index 0000000..0c01d27 --- /dev/null +++ b/demos/ota_nav2_sensor_fix/ros2_packages/ota_nav2_sensor_fix_demo/urdf/warehouse_rbtheron.urdf.xacro @@ -0,0 +1,118 @@ + + + + + + + + + + + + + + + + + robot_description + robot_state_publisher + $(find ota_nav2_sensor_fix_demo)/config/controllers.yaml + + + + + + + gz_ros2_control/GazeboSimSystem + + + + -10 + 10 + + + + + + + + -10 + 10 + + + + + + + + + + + diff --git a/demos/ota_nav2_sensor_fix/ros2_ws/.gitignore b/demos/ota_nav2_sensor_fix/ros2_ws/.gitignore new file mode 100644 index 0000000..fb3674e --- /dev/null +++ b/demos/ota_nav2_sensor_fix/ros2_ws/.gitignore @@ -0,0 +1,4 @@ +build/ +install/ +log/ +src/ diff --git a/demos/ota_nav2_sensor_fix/run-demo.sh b/demos/ota_nav2_sensor_fix/run-demo.sh new file mode 100755 index 0000000..860fdcf --- /dev/null +++ b/demos/ota_nav2_sensor_fix/run-demo.sh @@ -0,0 +1,160 @@ +#!/bin/bash +# OTA over SOVD - nav2 sensor-fix demo runner. +# Brings up the gateway (with the dev-grade ota_update_plugin baked in) and +# the FastAPI artifact server. The gateway image bundles a full TurtleBot3 + +# Nav2 + headless Gazebo stack and runs foxglove_bridge on :8765, so the +# demo is self-contained: broken_lidar publishes /scan with a phantom +# obstacle that nav2 + a Foxglove 3D panel both react to. The OTA flow +# swaps broken_lidar -> fixed_lidar and the phantom disappears. + +set -eu + +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +cd "$SCRIPT_DIR" + +DETACH_MODE="true" +UPDATE_IMAGES="false" +BUILD_ARGS="" +# The ota_update_server image self-builds its catalog + tarballs in-image (no +# host `./artifacts` mount is used), so a host-side artifact rebuild is not +# part of the reproducible path. Default to skipping it; --build-artifacts is +# an opt-in for maintainers who are iterating on scripts/build_artifacts.sh +# and want the host-built output for local inspection. +SKIP_ARTIFACTS="true" + +usage() { + echo "Usage: $0 [OPTIONS]" + echo "" + echo "Options:" + echo " --attached Run in foreground (default: daemon mode)" + echo " --update Pull latest images before running" + echo " --no-cache Build Docker images without cache" + echo " --build-artifacts Rebuild artifacts/catalog.json on the host before starting" + echo " (maintainer opt-in; requires host ROS + ros2_medkit_msgs)" + echo " -h, --help Show this help message" + echo "" + echo "Environment:" + echo " OTA_GATEWAY_PORT Host port for gateway HTTP API (default: 8080)" + echo " OTA_FOXGLOVE_BRIDGE_PORT Host port for foxglove_bridge WebSocket (default: 8765)" + echo "" + echo "Examples:" + echo " $0 # Daemon mode (default)" + echo " $0 --attached # Foreground with logs" + echo " OTA_GATEWAY_PORT=8081 $0 # Use a different host port" +} + +while [[ $# -gt 0 ]]; do + case "$1" in + --attached) DETACH_MODE="false" ;; + --update) UPDATE_IMAGES="true" ;; + --no-cache) BUILD_ARGS="--no-cache" ;; + --build-artifacts) SKIP_ARTIFACTS="false" ;; + -h|--help) usage; exit 0 ;; + *) echo "Unknown option: $1"; usage; exit 1 ;; + esac + shift +done + +GATEWAY_PORT="${OTA_GATEWAY_PORT:-8080}" +GATEWAY_URL="http://localhost:${GATEWAY_PORT}" + +echo "OTA over SOVD - nav2 sensor-fix demo" +echo "====================================" +echo "" + +if ! command -v docker &> /dev/null; then + echo "Error: Docker is not installed" + exit 1 +fi + +if [[ "$SKIP_ARTIFACTS" != "true" ]]; then + if [[ ! -x "$SCRIPT_DIR/scripts/build_artifacts.sh" ]]; then + chmod +x "$SCRIPT_DIR/scripts/build_artifacts.sh" + fi + echo "[1/3] Building OTA artifacts (catalog.json + tarballs)..." + "$SCRIPT_DIR/scripts/build_artifacts.sh" + echo "" +fi + +if docker compose version &> /dev/null; then + COMPOSE_CMD="docker compose" +else + COMPOSE_CMD="docker-compose" +fi + +if [[ "$UPDATE_IMAGES" == "true" ]]; then + echo "Pulling latest images..." + ${COMPOSE_CMD} pull +fi + +echo "[2/3] Building and starting demo..." +echo " (First run pulls ros:jazzy and builds the gateway, ~10 minutes)" +echo "" + +DETACH_FLAG="" +if [[ "$DETACH_MODE" == "true" ]]; then + DETACH_FLAG="-d" +fi + +# shellcheck disable=SC2086 +if ! ${COMPOSE_CMD} build ${BUILD_ARGS}; then + echo "Docker build failed. Stopping any partially created containers..." + ${COMPOSE_CMD} down 2>/dev/null || true + exit 1 +fi + +# shellcheck disable=SC2086 +${COMPOSE_CMD} up ${DETACH_FLAG} + +if [[ "$DETACH_MODE" != "true" ]]; then + exit 0 +fi + +echo "" +echo "[3/3] Waiting for gateway to come up..." +for _ in 1 2 3 4 5 6 7 8 9 10 11 12; do + if curl -fsS "${GATEWAY_URL}/api/v1/health" >/dev/null 2>&1; then + break + fi + sleep 2 +done + +if ! curl -fsS "${GATEWAY_URL}/api/v1/health" >/dev/null 2>&1; then + echo "Gateway did not respond on ${GATEWAY_URL} - check logs with:" + echo " ${COMPOSE_CMD} logs gateway" + exit 1 +fi + +echo "" +echo "Demo is up." +echo "" +echo " Gateway HTTP API: ${GATEWAY_URL}/api/v1/" +echo " Foxglove WebSocket: ws://localhost:${OTA_FOXGLOVE_BRIDGE_PORT:-8765}" +echo " Update server: http://localhost:9000/catalog" +echo "" +echo "Registered updates:" +if command -v jq >/dev/null 2>&1; then + curl -fsS "${GATEWAY_URL}/api/v1/updates" | jq -r '.items[]' | sed 's/^/ /' +else + curl -fsS "${GATEWAY_URL}/api/v1/updates" +fi +echo "" +echo "Drive the demo:" +echo " ./check-demo.sh # show current state" +echo " ./publish-fix.sh # register fixed_lidar_3_0_1 (SOVD POST /updates) - not in the boot catalog" +echo " ./apply-fix.sh # apply the published fix: broken_lidar -> fixed_lidar_3_0_1" +echo " ./trigger-bad-update.sh # re-arm broken_lidar (root cause) for a rerun" +echo " ./clear-fault.sh # operator clear of the latched bt-navigator/controller-server faults" +echo " ./send-goal.sh # publish a nav goal (mission start / resume)" +echo " ./stop-demo.sh # tear down" +echo "" +echo "Connect a UI:" +echo " Web UI (ros2_medkit_web_ui):" +echo " npm install && npm run dev" +echo " open http://localhost:5173 -> Connect -> ${GATEWAY_URL}" +echo "" +echo " Foxglove Studio (recommended for the 3D narrative):" +echo " Open connection -> Foxglove WebSocket -> ws://localhost:${OTA_FOXGLOVE_BRIDGE_PORT:-8765}" +echo " Add a 3D panel: TurtleBot3 in the world, /scan cone shows the phantom" +echo " Install ros2_medkit_foxglove_extension (npm run local-install) for the" +echo " 'ros2_medkit Updates' panel; set baseUrl to ${GATEWAY_URL}/api/v1" diff --git a/demos/ota_nav2_sensor_fix/scripts/.gitignore b/demos/ota_nav2_sensor_fix/scripts/.gitignore new file mode 100644 index 0000000..17cd2fc --- /dev/null +++ b/demos/ota_nav2_sensor_fix/scripts/.gitignore @@ -0,0 +1,4 @@ +.venv/ +__pycache__/ +*.pyc +.pytest_cache/ diff --git a/demos/ota_nav2_sensor_fix/scripts/build_artifacts.sh b/demos/ota_nav2_sensor_fix/scripts/build_artifacts.sh new file mode 100755 index 0000000..20da598 --- /dev/null +++ b/demos/ota_nav2_sensor_fix/scripts/build_artifacts.sh @@ -0,0 +1,88 @@ +#!/usr/bin/env bash +# +# Optional dev-convenience: build artefact tarballs + catalog.json on +# the host so a maintainer can iterate on broken_lidar / fixed_lidar +# without going through `docker compose build` every time. +# +# This script is NOT load-bearing for CI or distribution. The +# reproducible path is `docker compose build ota_update_server`, which +# multi-stage-builds the same artefacts inside ros:jazzy. If you don't +# want to think about ROS env on your host, use compose. +# +# broken_lidar / fixed_lidar are pure rclcpp + sensor_msgs +# (+ visualization_msgs) republishers - Nav2's own log + action-status +# bridges turn its failure into SOVD faults, not a ReportFault call +# from these nodes. +# +# Prerequisites for running locally: +# - /opt/ros/jazzy on the prefix path +# - ros2_medkit_msgs sourced (e.g. via a colcon overlay built from +# a local clone of ros2_medkit; the gateway image embeds this). + +set -eo pipefail +SCRIPT_DIR="$(cd "$(dirname "$0")" && pwd)" +DEMO_DIR="$(dirname "$SCRIPT_DIR")" +WS="$DEMO_DIR/ros2_ws" +ARTIFACTS="$DEMO_DIR/artifacts" + +# shellcheck disable=SC1091 +source /opt/ros/jazzy/setup.bash + +if ! ros2 pkg prefix ros2_medkit_msgs > /dev/null 2>&1; then + echo "ros2_medkit_msgs not found on the prefix path." >&2 + echo "" >&2 + echo "This script still gates on ros2_medkit_msgs being sourced (legacy of" >&2 + echo "when fixed_lidar / broken_lidar called ReportFault directly; they are" >&2 + echo "now pure /scan_sim republishers with no ros2_medkit dependency, but" >&2 + echo "the gate is left in place here). Either:" >&2 + echo " - source an overlay that has it built, or" >&2 + echo " - run 'docker compose build ota_update_server' instead - that" >&2 + echo " path is reproducible and bundles the msgs build internally." >&2 + exit 1 +fi + +set -u + +mkdir -p "$WS/src" +for pkg in broken_lidar fixed_lidar; do + ln -sfn "$DEMO_DIR/ros2_packages/$pkg" "$WS/src/$pkg" +done + +(cd "$WS" && colcon build --packages-select broken_lidar fixed_lidar) + +mkdir -p "$ARTIFACTS" +rm -f "$ARTIFACTS/catalog.json" "$ARTIFACTS/catalog_pending.json" + +PACK=("$SCRIPT_DIR/.venv/bin/python" "$SCRIPT_DIR/pack_artifact.py") + +# The BOOT catalog (catalog.json) holds only the bad update broken_lidar_3_0_0 +# - that is what was pushed and auto-applied. The remediation build +# fixed_lidar_3_0_1 is packed into catalog_pending.json instead (its tarball +# still ships), so it is NOT in the boot catalog - the operator publishes it +# at diagnose time with publish-fix.sh (SOVD POST /updates). This mirrors +# ota_update_server/Dockerfile's in-image build exactly. +env -i PATH=/usr/bin:/bin HOME="$HOME" "${PACK[@]}" \ + --package broken_lidar --version 3.0.0 \ + --kind update --target-component scan_sensor_node \ + --executable broken_lidar_node \ + --replaces-executable fixed_lidar_node \ + --notes "Perception: /scan noise-filter tuning" \ + --skip-build --workspace "$WS" \ + --out-dir "$ARTIFACTS" --catalog "$ARTIFACTS/catalog.json" + +env -i PATH=/usr/bin:/bin HOME="$HOME" "${PACK[@]}" \ + --package fixed_lidar --version 3.0.1 \ + --kind update --target-component scan_sensor_node \ + --executable fixed_lidar_node \ + --replaces-executable broken_lidar_node \ + --notes "Fix regressed /scan noise filter (3.0.0 hotfix)" \ + --skip-build --workspace "$WS" \ + --out-dir "$ARTIFACTS" --catalog "$ARTIFACTS/catalog_pending.json" + +if command -v jq >/dev/null 2>&1; then + echo "Built boot catalog with $(jq length "$ARTIFACTS/catalog.json") entries" + echo "Built pending catalog with $(jq length "$ARTIFACTS/catalog_pending.json") entries" +else + echo "Built boot catalog: $(wc -l < "$ARTIFACTS/catalog.json") lines" + echo "Built pending catalog: $(wc -l < "$ARTIFACTS/catalog_pending.json") lines" +fi diff --git a/demos/ota_nav2_sensor_fix/scripts/conftest.py b/demos/ota_nav2_sensor_fix/scripts/conftest.py new file mode 100644 index 0000000..85d7c2d --- /dev/null +++ b/demos/ota_nav2_sensor_fix/scripts/conftest.py @@ -0,0 +1,7 @@ +"""Pytest fixtures for pack_artifact tests.""" +from __future__ import annotations + +import sys +from pathlib import Path + +sys.path.insert(0, str(Path(__file__).parent)) diff --git a/demos/ota_nav2_sensor_fix/scripts/e2e_webui_smoke.mjs b/demos/ota_nav2_sensor_fix/scripts/e2e_webui_smoke.mjs new file mode 100644 index 0000000..16efcd9 --- /dev/null +++ b/demos/ota_nav2_sensor_fix/scripts/e2e_webui_smoke.mjs @@ -0,0 +1,124 @@ +// E2E smoke driver for the OTA demo. Drives ros2_medkit_web_ui (running +// at WEB_UI_URL) against the live demo gateway (GATEWAY_URL) and asserts +// that both catalog entries register and that the SOVD wire format +// matches what we ship from pack_artifact.py. +// +// The fix (fixed_lidar_3_0_1) is a forward hotfix held out of the boot +// catalog - it ships on the update server's catalog_pending.json and is +// only registered once published (SOVD POST /updates), the way the +// operator would use ./publish-fix.sh. This driver replicates that publish +// step directly (fetching the pending entry from the update server and +// POSTing it to the gateway) before asserting the web UI reflects it. +// +// Why this exists: the Foxglove updates panel mirrors the same SOVD client +// patterns the web UI uses (fetchUpdateIds parses {items: [...]}, +// per-id /status, lazy /detail). Verifying the web UI flow end-to-end +// gives us a canonical reference point for both clients. +// +// Usage: +// docker compose up -d +// cd /path/to/ros2_medkit_web_ui && npm install && npm run dev +// GATEWAY_URL=http://localhost:8080 \ +// WEB_UI_URL=http://localhost:5173 \ +// UPDATE_SERVER_URL=http://localhost:9000 \ +// node /path/to/this/e2e_webui_smoke.mjs +// +// Requires: playwright (`npm install --no-save playwright` in the web UI +// dir), chromium-headless-shell (`npx playwright install +// chromium-headless-shell`), the demo stack from ../docker-compose.yml. + +import { chromium } from "playwright"; + +const WEB_UI_URL = process.env.WEB_UI_URL ?? "http://localhost:5173/"; +const GATEWAY_URL = process.env.GATEWAY_URL ?? "http://localhost:8080"; +const UPDATE_SERVER_URL = process.env.UPDATE_SERVER_URL ?? "http://localhost:9000"; + +const EXPECTED_IDS = [ + "broken_lidar_3_0_0", + "fixed_lidar_3_0_1", +]; + +const EXPECTED_API_PATHS = [ + "/api/v1/updates", + "/api/v1/updates/broken_lidar_3_0_0/status", + "/api/v1/updates/fixed_lidar_3_0_1/status", +]; + +// Publish the held-back hotfix (mirrors ./publish-fix.sh): fetch the pending +// catalog entry from the update server and register it via SOVD POST +// /updates so it shows up in /updates and in the web UI. +async function publishFix() { + const pending = await fetch(`${UPDATE_SERVER_URL}/artifacts/catalog_pending.json`).then( + (r) => r.json(), + ); + const entry = pending[0]; + const resp = await fetch(`${GATEWAY_URL}/api/v1/updates`, { + method: "POST", + headers: { "Content-Type": "application/json" }, + body: JSON.stringify(entry), + }); + if (!resp.ok) { + throw new Error(`publish fixed_lidar_3_0_1 failed: HTTP ${resp.status}`); + } +} + +(async () => { + await publishFix(); + + const browser = await chromium.launch({ + channel: "chromium-headless-shell", + headless: true, + }); + const ctx = await browser.newContext(); + const page = await ctx.newPage(); + + page.on("pageerror", (err) => console.log(`[pageerror] ${err.message}`)); + + const apiCalls = new Set(); + page.on("request", (req) => { + const u = req.url(); + if (u.includes("/api/v1/")) { + apiCalls.add(`${req.method()} ${u}`); + } + }); + + await page.goto(WEB_UI_URL, { waitUntil: "domcontentloaded" }); + + await page.getByRole("button", { name: /connect to server/i }).click(); + await page.waitForTimeout(300); + await page.locator('input[type="text"], input:not([type])').first().fill(GATEWAY_URL); + await page.getByRole("button", { name: /^connect$/i }).last().click(); + await page.waitForTimeout(2000); + + const updatesButton = page.getByRole("button", { name: /updates/i }).first(); + if (await updatesButton.count()) { + await updatesButton.click(); + await page.waitForTimeout(2000); + } + + const bodyText = await page.locator("body").textContent(); + + let failed = 0; + for (const id of EXPECTED_IDS) { + const visible = bodyText?.includes(id) ?? false; + console.log(` id ${id}: ${visible ? "PASS" : "FAIL"}`); + if (!visible) failed++; + } + + for (const path of EXPECTED_API_PATHS) { + const hit = [...apiCalls].some((c) => c.endsWith(path)); + console.log(` api ${path}: ${hit ? "PASS" : "FAIL"}`); + if (!hit) failed++; + } + + await browser.close(); + + if (failed > 0) { + console.error(`\n${failed} assertion(s) failed`); + process.exit(1); + } + console.log("\nDONE: all SOVD flows verified"); +})().catch((err) => { + console.error("FAIL:", err); + process.exit(1); +}); diff --git a/demos/ota_nav2_sensor_fix/scripts/pack_artifact.py b/demos/ota_nav2_sensor_fix/scripts/pack_artifact.py new file mode 100644 index 0000000..fabe8e4 --- /dev/null +++ b/demos/ota_nav2_sensor_fix/scripts/pack_artifact.py @@ -0,0 +1,238 @@ +#!/usr/bin/env python3 +"""Pack a ROS 2 package into an OTA artifact + SOVD-shaped catalog entry.""" +from __future__ import annotations + +import argparse +import json +import subprocess +import sys +import tarfile +from pathlib import Path +from typing import Literal + +Kind = Literal["update", "install", "uninstall"] + + +def build_parser() -> argparse.ArgumentParser: + parser = argparse.ArgumentParser( + description="Pack a ROS 2 package into an OTA artifact + SOVD catalog entry.", + ) + parser.add_argument("--package", required=True, help="ROS 2 package name to pack.") + parser.add_argument( + "--version", + default="0.0.0", + help="Semantic version of the artifact (pass '' for uninstall).", + ) + parser.add_argument( + "--kind", + required=True, + choices=["update", "install", "uninstall"], + help="Catalog entry kind.", + ) + parser.add_argument( + "--target-component", + required=True, + help="SOVD component the entry targets.", + ) + parser.add_argument( + "--executable", + default="", + help="Executable name inside install//lib (required for install).", + ) + parser.add_argument( + "--replaces-executable", + default="", + help=( + "For kind=update: name of the OLD executable to kill before " + "spawning --executable. Defaults to --executable when omitted." + ), + ) + parser.add_argument("--notes", default="", help="Free-text notes for the catalog entry.") + parser.add_argument( + "--duration", + type=int, + default=10, + help="Estimated install duration in seconds.", + ) + parser.add_argument( + "--out-dir", + default="artifacts", + help="Output directory for tarballs.", + ) + parser.add_argument( + "--catalog", + default="artifacts/catalog.json", + help="Path to the SOVD catalog JSON file.", + ) + parser.add_argument( + "--skip-build", + action="store_true", + help="Skip running colcon build; reuse existing install/ tree.", + ) + parser.add_argument( + "--workspace", + default=".", + help="Path to the colcon workspace root.", + ) + return parser + + +def slug(package: str, version: str) -> str: + return f"{package}_{version.replace('.', '_')}" if version else package + + +def build_entry( + *, + package: str, + version: str, + kind: Kind, + target_component: str, + executable: str, + notes: str, + duration: int, + size_bytes: int, + replaces_executable: str = "", +) -> dict: + entry: dict = { + "id": slug(package, version) if kind != "uninstall" else f"{package}_remove", + # SOVD ISO 17978-3 mandates "update_name". Earlier drafts of this + # script wrote "name" - the gateway passes that through to clients + # but spec-compliant consumers (web UI, Foxglove panel) expect + # update_name. + "update_name": f"{package} {version}".strip(), + "automated": False, + "origins": ["remote"], + "notes": notes, + "duration": duration, + } + if version: + # SOVD spec does not define a top-level version field on update + # detail, so we expose it as a vendor extension. + entry["x_medkit_version"] = version + if size_bytes > 0: + entry["size"] = max(1, size_bytes // 1024) + + if kind == "update": + entry["updated_components"] = [target_component] + elif kind == "install": + entry["added_components"] = [target_component] + else: # uninstall + entry["removed_components"] = [target_component] + + if kind != "uninstall": + entry["x_medkit_artifact_url"] = f"/artifacts/{package}-{version}.tar.gz" + entry["x_medkit_target_package"] = package + if executable: + entry["x_medkit_executable"] = executable + else: + entry["x_medkit_target_package"] = package + + if kind == "update" and replaces_executable: + entry["x_medkit_replaces_executable"] = replaces_executable + + return entry + + +def merge_catalog(catalog_path: Path, entry: dict) -> None: + catalog_path = Path(catalog_path) + catalog_path.parent.mkdir(parents=True, exist_ok=True) + if catalog_path.exists(): + data = json.loads(catalog_path.read_text()) + else: + data = [] + data = [e for e in data if e.get("id") != entry["id"]] + data.append(entry) + catalog_path.write_text(json.dumps(data, indent=2) + "\n") + + +def create_tarball( + *, + package: str, + version: str, + install_dir: Path, + out_dir: Path, +) -> Path: + install_dir = Path(install_dir) + if not install_dir.exists(): + raise FileNotFoundError(f"install dir does not exist: {install_dir}") + out_dir = Path(out_dir) + out_dir.mkdir(parents=True, exist_ok=True) + out_path = out_dir / f"{package}-{version}.tar.gz" + with tarfile.open(out_path, "w:gz") as tf: + tf.add(install_dir, arcname=package) + return out_path + + +def colcon_build(workspace: Path, package: str) -> None: + cmd = ["colcon", "build", "--packages-select", package, "--symlink-install"] + completed = subprocess.run(cmd, cwd=workspace, check=False) + if completed.returncode != 0: + raise SystemExit(f"colcon build failed for {package}") + + +def run( + *, + package: str, + version: str, + kind: Kind, + target_component: str, + executable: str, + notes: str, + duration: int, + out_dir: str, + catalog: str, + skip_build: bool, + workspace: str, + replaces_executable: str = "", +) -> int: + if kind == "install" and not executable: + sys.stderr.write("--executable is required for install\n") + raise SystemExit(2) + if kind != "uninstall" and not version: + sys.stderr.write(f"--version is required for kind={kind}\n") + raise SystemExit(2) + + out_dir_p = Path(out_dir) + catalog_p = Path(catalog) + workspace_p = Path(workspace) + + size_bytes = 0 + if kind != "uninstall": + if not skip_build: + colcon_build(workspace_p, package) + install_dir = workspace_p / "install" / package + if not install_dir.exists(): + sys.stderr.write(f"install dir missing: {install_dir}\n") + raise SystemExit(3) + tarball = create_tarball( + package=package, + version=version, + install_dir=install_dir, + out_dir=out_dir_p, + ) + size_bytes = tarball.stat().st_size + + entry = build_entry( + package=package, + version=version, + kind=kind, + target_component=target_component, + executable=executable, + notes=notes, + duration=duration, + size_bytes=size_bytes, + replaces_executable=replaces_executable, + ) + merge_catalog(catalog_p, entry) + print(f"packed {entry['id']}") + return 0 + + +def main(argv: list[str] | None = None) -> int: + parser = build_parser() + args = parser.parse_args(argv) + return run(**vars(args)) + + +if __name__ == "__main__": + sys.exit(main()) diff --git a/demos/ota_nav2_sensor_fix/scripts/pyrightconfig.json b/demos/ota_nav2_sensor_fix/scripts/pyrightconfig.json new file mode 100644 index 0000000..5b3e8d0 --- /dev/null +++ b/demos/ota_nav2_sensor_fix/scripts/pyrightconfig.json @@ -0,0 +1,5 @@ +{ + "extraPaths": ["."], + "venvPath": ".", + "venv": ".venv" +} diff --git a/demos/ota_nav2_sensor_fix/scripts/test_pack_artifact.py b/demos/ota_nav2_sensor_fix/scripts/test_pack_artifact.py new file mode 100644 index 0000000..f37d05b --- /dev/null +++ b/demos/ota_nav2_sensor_fix/scripts/test_pack_artifact.py @@ -0,0 +1,320 @@ +"""Tests for pack_artifact.py.""" +from __future__ import annotations + +import json +import tarfile + +import pytest + +import pack_artifact + + +def test_main_requires_package(): + with pytest.raises(SystemExit): + pack_artifact.main([]) + + +def test_main_parses_basic_args(monkeypatch, tmp_path): + captured = {} + + def fake_run(**kwargs): + captured.update(kwargs) + return 0 + + monkeypatch.setattr(pack_artifact, "run", fake_run) + rc = pack_artifact.main( + [ + "--package", "fixed_lidar", + "--version", "3.0.1", + "--kind", "update", + "--target-component", "scan_sensor_node", + "--executable", "fixed_lidar_node", + "--notes", "noise filter fix", + "--out-dir", str(tmp_path / "artifacts"), + "--catalog", str(tmp_path / "artifacts" / "catalog.json"), + "--skip-build", + ] + ) + assert rc == 0 + assert captured["package"] == "fixed_lidar" + assert captured["version"] == "3.0.1" + assert captured["kind"] == "update" + assert captured["target_component"] == "scan_sensor_node" + assert captured["executable"] == "fixed_lidar_node" + assert captured["notes"] == "noise filter fix" + assert captured["skip_build"] is True + + +def test_build_entry_update_kind(): + entry = pack_artifact.build_entry( + package="fixed_lidar", + version="3.0.1", + kind="update", + target_component="scan_sensor_node", + executable="fixed_lidar_node", + notes="fix noise", + duration=10, + size_bytes=2048, + ) + assert entry["id"] == "fixed_lidar_3_0_1" + assert entry["update_name"] == "fixed_lidar 3.0.1" + assert "name" not in entry, "use update_name (SOVD spec) not name" + assert entry["x_medkit_version"] == "3.0.1" + assert "version" not in entry, "version is not a SOVD field; use x_medkit_version" + assert entry["automated"] is False + assert entry["origins"] == ["remote"] + assert entry["notes"] == "fix noise" + assert entry["size"] == 2 # KB rounded + assert entry["duration"] == 10 + assert entry["updated_components"] == ["scan_sensor_node"] + assert "added_components" not in entry + assert "removed_components" not in entry + assert entry["x_medkit_target_package"] == "fixed_lidar" + assert entry["x_medkit_executable"] == "fixed_lidar_node" + assert entry["x_medkit_artifact_url"] == "/artifacts/fixed_lidar-3.0.1.tar.gz" + assert "x_medkit_replaces_executable" not in entry + + +def test_build_entry_update_kind_with_replaces(): + entry = pack_artifact.build_entry( + package="fixed_lidar", + version="3.0.1", + kind="update", + target_component="scan_sensor_node", + executable="fixed_lidar_node", + replaces_executable="broken_lidar_node", + notes="", + duration=10, + size_bytes=1024, + ) + assert entry["x_medkit_replaces_executable"] == "broken_lidar_node" + + +def test_build_entry_install_kind(): + entry = pack_artifact.build_entry( + package="demo_addon_pkg", + version="1.0.0", + kind="install", + target_component="demo_addon", + executable="demo_addon_node", + notes="extra safety", + duration=15, + size_bytes=4096, + ) + assert entry["added_components"] == ["demo_addon"] + assert "updated_components" not in entry + assert "removed_components" not in entry + + +def test_build_entry_uninstall_kind(): + entry = pack_artifact.build_entry( + package="deprecated_pkg", + version="", + kind="uninstall", + target_component="deprecated_pkg", + executable="", + notes="cleanup", + duration=5, + size_bytes=0, + ) + assert entry["removed_components"] == ["deprecated_pkg"] + assert "added_components" not in entry + assert "updated_components" not in entry + assert "x_medkit_artifact_url" not in entry + assert "x_medkit_executable" not in entry + + +def test_merge_catalog_creates_file(tmp_path): + catalog = tmp_path / "catalog.json" + entry = {"id": "a", "name": "a"} + pack_artifact.merge_catalog(catalog, entry) + data = json.loads(catalog.read_text()) + assert data == [entry] + + +def test_merge_catalog_appends(tmp_path): + catalog = tmp_path / "catalog.json" + catalog.write_text(json.dumps([{"id": "a", "name": "a"}])) + entry = {"id": "b", "name": "b"} + pack_artifact.merge_catalog(catalog, entry) + data = json.loads(catalog.read_text()) + assert [e["id"] for e in data] == ["a", "b"] + + +def test_merge_catalog_replaces_same_id(tmp_path): + catalog = tmp_path / "catalog.json" + catalog.write_text(json.dumps([{"id": "a", "name": "old"}])) + entry = {"id": "a", "name": "new"} + pack_artifact.merge_catalog(catalog, entry) + data = json.loads(catalog.read_text()) + assert data == [entry] + + +def test_create_tarball(tmp_path): + install = tmp_path / "install" / "fixed_lidar" + (install / "lib").mkdir(parents=True) + (install / "lib" / "fixed_lidar_node").write_text("binary") + out_dir = tmp_path / "artifacts" + out_path = pack_artifact.create_tarball( + package="fixed_lidar", + version="3.0.1", + install_dir=install, + out_dir=out_dir, + ) + assert out_path == out_dir / "fixed_lidar-3.0.1.tar.gz" + assert out_path.exists() + with tarfile.open(out_path) as tf: + names = tf.getnames() + assert "fixed_lidar/lib/fixed_lidar_node" in names + + +def test_run_update_kind_e2e(tmp_path): + workspace = tmp_path / "ws" + install = workspace / "install" / "fixed_lidar" / "lib" + install.mkdir(parents=True) + (install / "fixed_lidar_node").write_text("bin") + out_dir = tmp_path / "artifacts" + catalog = out_dir / "catalog.json" + + rc = pack_artifact.run( + package="fixed_lidar", + version="3.0.1", + kind="update", + target_component="scan_sensor_node", + executable="fixed_lidar_node", + notes="fix", + duration=10, + out_dir=str(out_dir), + catalog=str(catalog), + skip_build=True, + workspace=str(workspace), + ) + + assert rc == 0 + assert (out_dir / "fixed_lidar-3.0.1.tar.gz").exists() + data = json.loads(catalog.read_text()) + assert data[0]["id"] == "fixed_lidar_3_0_1" + assert data[0]["updated_components"] == ["scan_sensor_node"] + + +def test_run_uninstall_skips_tarball(tmp_path): + workspace = tmp_path / "ws" + workspace.mkdir() + out_dir = tmp_path / "artifacts" + catalog = out_dir / "catalog.json" + + rc = pack_artifact.run( + package="deprecated_pkg", + version="", + kind="uninstall", + target_component="deprecated_pkg", + executable="", + notes="cleanup", + duration=5, + out_dir=str(out_dir), + catalog=str(catalog), + skip_build=True, + workspace=str(workspace), + ) + + assert rc == 0 + assert not list(out_dir.glob("*.tar.gz")) + data = json.loads(catalog.read_text()) + assert data[0]["removed_components"] == ["deprecated_pkg"] + + +def test_run_install_requires_executable(tmp_path): + with pytest.raises(SystemExit): + pack_artifact.run( + package="demo_addon_pkg", + version="1.0.0", + kind="install", + target_component="demo_addon", + executable="", + notes="", + duration=10, + out_dir=str(tmp_path / "out"), + catalog=str(tmp_path / "out" / "catalog.json"), + skip_build=True, + workspace=str(tmp_path / "ws"), + ) + + +def test_run_update_requires_version(tmp_path): + with pytest.raises(SystemExit): + pack_artifact.run( + package="fixed_lidar", + version="", + kind="update", + target_component="scan_sensor_node", + executable="fixed_lidar_node", + notes="", + duration=10, + out_dir=str(tmp_path / "out"), + catalog=str(tmp_path / "out" / "catalog.json"), + skip_build=True, + workspace=str(tmp_path / "ws"), + ) + + +def test_run_install_kind_e2e(tmp_path): + workspace = tmp_path / "ws" + install = workspace / "install" / "demo_addon_pkg" / "lib" + install.mkdir(parents=True) + (install / "demo_addon_node").write_text("bin") + out_dir = tmp_path / "artifacts" + catalog = out_dir / "catalog.json" + + rc = pack_artifact.run( + package="demo_addon_pkg", + version="1.0.0", + kind="install", + target_component="demo_addon", + executable="demo_addon_node", + notes="extra safety", + duration=15, + out_dir=str(out_dir), + catalog=str(catalog), + skip_build=True, + workspace=str(workspace), + ) + + assert rc == 0 + assert (out_dir / "demo_addon_pkg-1.0.0.tar.gz").exists() + data = json.loads(catalog.read_text()) + assert data[0]["id"] == "demo_addon_pkg_1_0_0" + assert data[0]["added_components"] == ["demo_addon"] + assert data[0]["x_medkit_executable"] == "demo_addon_node" + + +def test_colcon_build_invokes_subprocess(tmp_path, monkeypatch): + captured = {} + + class FakeCompleted: + returncode = 0 + + def fake_run(cmd, cwd, check): + captured["cmd"] = cmd + captured["cwd"] = cwd + captured["check"] = check + return FakeCompleted() + + monkeypatch.setattr(pack_artifact.subprocess, "run", fake_run) + pack_artifact.colcon_build(tmp_path, "broken_lidar") + + assert captured["cmd"] == [ + "colcon", "build", "--packages-select", "broken_lidar", "--symlink-install" + ] + assert captured["cwd"] == tmp_path + assert captured["check"] is False + + +def test_colcon_build_raises_on_nonzero(tmp_path, monkeypatch): + class FakeCompleted: + returncode = 1 + + monkeypatch.setattr( + pack_artifact.subprocess, "run", lambda *_args, **_kwargs: FakeCompleted() + ) + with pytest.raises(SystemExit): + pack_artifact.colcon_build(tmp_path, "broken_lidar") diff --git a/demos/ota_nav2_sensor_fix/send-goal.sh b/demos/ota_nav2_sensor_fix/send-goal.sh new file mode 100755 index 0000000..6dd015e --- /dev/null +++ b/demos/ota_nav2_sensor_fix/send-goal.sh @@ -0,0 +1,11 @@ +#!/bin/bash +# Publish a Nav2 goal into the demo container (ROS_DOMAIN_ID=42). Used to +# start the mission (robot drives into the phantom) and to resume after fix. +set -eu +X="${1:-1.5}"; Y="${2:-1.0}" +GOAL="{header: {frame_id: map}, pose: {position: {x: ${X}, y: ${Y}, z: 0.0}, orientation: {w: 1.0}}}" +# ros2 is not on the container's default PATH and `docker exec` does not run the +# image entrypoint that sources it, so source the ROS overlay inside the exec. +docker exec ota_demo_gateway bash -c \ + "source /opt/ros/jazzy/setup.bash && source /ws/install/setup.bash && ros2 topic pub --once /goal_pose geometry_msgs/PoseStamped '${GOAL}'" +echo "Goal sent: (${X}, ${Y})" diff --git a/demos/ota_nav2_sensor_fix/stop-demo.sh b/demos/ota_nav2_sensor_fix/stop-demo.sh new file mode 100755 index 0000000..1233d63 --- /dev/null +++ b/demos/ota_nav2_sensor_fix/stop-demo.sh @@ -0,0 +1,40 @@ +#!/bin/bash +# Stop the OTA over SOVD - nav2 sensor-fix demo. + +set -eu + +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +cd "$SCRIPT_DIR" + +REMOVE_VOLUMES="" +REMOVE_IMAGES="" + +usage() { + echo "Usage: $0 [OPTIONS]" + echo "" + echo "Options:" + echo " -v, --volumes Remove named volumes" + echo " --images Remove built images" + echo " -h, --help Show this help message" +} + +while [[ $# -gt 0 ]]; do + case "$1" in + -v|--volumes) REMOVE_VOLUMES="-v" ;; + --images) REMOVE_IMAGES="--rmi local" ;; + -h|--help) usage; exit 0 ;; + *) echo "Unknown option: $1"; usage; exit 1 ;; + esac + shift +done + +if docker compose version &> /dev/null; then + COMPOSE_CMD="docker compose" +else + COMPOSE_CMD="docker-compose" +fi + +# shellcheck disable=SC2086 +${COMPOSE_CMD} down ${REMOVE_VOLUMES} ${REMOVE_IMAGES} +echo "" +echo "Demo stopped." diff --git a/demos/ota_nav2_sensor_fix/trigger-bad-update.sh b/demos/ota_nav2_sensor_fix/trigger-bad-update.sh new file mode 100755 index 0000000..696ca02 --- /dev/null +++ b/demos/ota_nav2_sensor_fix/trigger-bad-update.sh @@ -0,0 +1,37 @@ +#!/bin/bash +# Apply the regressing lidar update (root cause); normally auto-applied at boot, +# this re-arms it during a recording. +# Uses spec endpoints PUT /updates/{id}/prepare then PUT /updates/{id}/execute. + +set -eu + +GATEWAY_URL="${OTA_GATEWAY_URL:-http://localhost:${OTA_GATEWAY_PORT:-8080}}" +API="${GATEWAY_URL}/api/v1" +ID="broken_lidar_3_0_0" + +if ! curl -fsS "${API}/health" >/dev/null 2>&1; then + echo "Gateway not reachable at ${GATEWAY_URL}. Start it with: ./run-demo.sh" + exit 1 +fi + +echo "Update: ${ID}" +echo " PUT /updates/${ID}/prepare" +curl -fsS -X PUT -H 'Content-Type: application/json' -d '{}' \ + "${API}/updates/${ID}/prepare" >/dev/null +sleep 3 + +echo " PUT /updates/${ID}/execute" +curl -fsS -X PUT -H 'Content-Type: application/json' -d '{}' \ + "${API}/updates/${ID}/execute" >/dev/null +sleep 5 + +echo "" +echo "Status after execute:" +curl -fsS "${API}/updates/${ID}/status" | (jq . 2>/dev/null || cat) + +if docker ps --format '{{.Names}}' | grep -q '^ota_demo_gateway$'; then + echo "" + echo "Live processes:" + docker exec ota_demo_gateway pgrep -af 'broken_lidar_node|fixed_lidar_node' \ + 2>/dev/null | grep -v 'pgrep' | sed 's/^/ /' || true +fi diff --git a/demos/ota_nav2_sensor_fix/updates/README.md b/demos/ota_nav2_sensor_fix/updates/README.md new file mode 100644 index 0000000..16e6a2d --- /dev/null +++ b/demos/ota_nav2_sensor_fix/updates/README.md @@ -0,0 +1,30 @@ +# Registering an update by hand + +`GET /updates` at boot lists only the bad update (`broken_lidar_3_0_0`). A new +update is not in the catalog until someone publishes it, and you publish it by +POSTing its descriptor - the JSON you see in `fixed_lidar_3_0_1.json`: + +```bash +curl -X POST http://localhost:8080/api/v1/updates \ + -H 'Content-Type: application/json' \ + -d @updates/fixed_lidar_3_0_1.json +``` + +`./publish-fix.sh` is just that one call. Edit the JSON (or write your own) to +register any other update. The fields: + +| Field | What it is | +|-------|------------| +| `id` | The update id used in every later call (`/updates//prepare`, `/execute`). | +| `update_name` | Human-readable name shown in the Updates panel. | +| `notes` | Free-text release note. | +| `x_medkit_version` | The build version (a vendor extension, so it is `x_medkit_*`). | +| `updated_components` | The SOVD component this update changes. `added_components` / `removed_components` instead would make it an install / uninstall. The kind is derived from which of the three you set. | +| `x_medkit_target_package` | The ROS 2 package the artifact installs. | +| `x_medkit_executable` | The binary the swapped-in node runs. | +| `x_medkit_replaces_executable` | The binary it replaces (so the plugin knows which process to kill on execute). | +| `x_medkit_artifact_url` | Where the update server serves the tarball. **This file must exist on the update server** (the demo ships `fixed_lidar-3.0.1.tar.gz`); `apply-fix.sh` fetches it during `prepare`. | +| `automated`, `origins`, `duration`, `size` | Informational metadata surfaced in the panel. | + +After the POST, `GET /updates` includes your new id, and you apply it with +`./apply-fix.sh` (or `PUT /updates//prepare` then `/execute`). diff --git a/demos/ota_nav2_sensor_fix/updates/fixed_lidar_3_0_1.json b/demos/ota_nav2_sensor_fix/updates/fixed_lidar_3_0_1.json new file mode 100644 index 0000000..2f06787 --- /dev/null +++ b/demos/ota_nav2_sensor_fix/updates/fixed_lidar_3_0_1.json @@ -0,0 +1,15 @@ +{ + "id": "fixed_lidar_3_0_1", + "update_name": "fixed_lidar 3.0.1", + "automated": false, + "origins": ["remote"], + "notes": "Fix regressed /scan noise filter (3.0.0 hotfix)", + "duration": 10, + "x_medkit_version": "3.0.1", + "size": 717, + "updated_components": ["scan_sensor_node"], + "x_medkit_artifact_url": "/artifacts/fixed_lidar-3.0.1.tar.gz", + "x_medkit_target_package": "fixed_lidar", + "x_medkit_executable": "fixed_lidar_node", + "x_medkit_replaces_executable": "broken_lidar_node" +} diff --git a/tests/smoke_lib.sh b/tests/smoke_lib.sh index 6b0edc3..b4d3bc3 100755 --- a/tests/smoke_lib.sh +++ b/tests/smoke_lib.sh @@ -348,10 +348,21 @@ assert_triggers_crud() { fi } -# Print test summary (called via EXIT trap - do not call exit here) +# Print test summary and gate the process exit code on FAIL_COUNT (called via +# EXIT trap - the exit here is intentional: it is how the harness fails CI +# when a behavioral fail() was recorded, instead of always exiting 0). SUMMARY_PRINTED=false print_summary() { - # Guard against double-printing when called as both trap and explicit call + # Capture the real exit status FIRST, before anything else (including the + # SUMMARY_PRINTED guard/assignment below) can clobber $?. When this runs + # as an EXIT trap after a crash (e.g. `set -e` tripping on an unhandled + # command failure before any fail() was recorded), $? here is that crash's + # real exit status - it must survive to the final `exit` below, or a + # genuine crash with FAIL_COUNT still 0 would silently report green. + local rc=$? + + # Guard against double-printing / recursive re-entry when called as both + # trap and explicit call. if [ "$SUMMARY_PRINTED" = true ]; then return fi @@ -365,9 +376,20 @@ print_summary() { if [ "$FAIL_COUNT" -gt 0 ]; then echo -e "\n ${RED}Failed tests:${FAILED_TESTS}${NC}" echo -e "${BLUE}================================${NC}" - return + elif [ "$rc" -eq 0 ]; then + echo -e "${BLUE}================================${NC}" + echo -e "\n${GREEN}All smoke tests passed!${NC}" + else + echo -e "${BLUE}================================${NC}" + echo -e "\n${RED}Script exited abnormally (no assertions recorded), exit code ${rc}${NC}" fi - echo -e "${BLUE}================================${NC}" - echo -e "\n${GREEN}All smoke tests passed!${NC}" + # Gate on FAIL_COUNT when behavioral fail()s were recorded; otherwise + # preserve whatever the script's real exit status was (0 on a clean run, + # non-zero if it crashed before recording any fail()). + if [ "$FAIL_COUNT" -gt 0 ]; then + exit "$FAIL_COUNT" + else + exit "$rc" + fi } diff --git a/tests/smoke_test_demo_narrative.sh b/tests/smoke_test_demo_narrative.sh new file mode 100755 index 0000000..b163952 --- /dev/null +++ b/tests/smoke_test_demo_narrative.sh @@ -0,0 +1,405 @@ +#!/bin/bash +# Demo-narrative smoke test for the ota_nav2_sensor_fix demo. +# +# The other smoke test (smoke_test_ota.sh) exercises the SOVD /updates +# endpoints directly; this one drives the demo the way an operator would - +# through the operator scripts (send-goal.sh / publish-fix.sh / apply-fix.sh / +# clear-fault.sh) - and asserts the latch/publish/apply/clear loop end-to-end. +# +# The narrative: the entrypoint auto-applies broken_lidar_3_0_0 before the +# mission starts (a routine fleet update that regressed the lidar). The +# operator sends a goal, the robot drives into the phantom sector, and Nav2 +# genuinely cannot make progress - navigate_to_pose aborts. Two generic +# ros2_medkit bridges (not a custom fault in the scan node) turn that Nav2 +# failure into SOVD faults: ACTION_NAVIGATE_TO_POSE_ABORTED on bt-navigator +# (the headline fault, with the freeze-frame + rosbag snapshot under its +# environment_data.snapshots) and a content-hashed LOG_CONTROLLER_SERVER_* +# on controller-server (supporting, checked by entity since the hash isn't +# stable). The fix (fixed_lidar_3_0_1) is a forward hotfix - not a rollback +# to a previous build - and is held out of the boot catalog: the operator +# publishes it with ./publish-fix.sh (SOVD POST /updates), then applies it +# with ./apply-fix.sh (prepare/execute) - but both faults are latched (no +# self-heal): they stay CONFIRMED until the operator explicitly clears them. +# Only after the deliberate clear does a fresh goal resume clean. +# +# What it asserts, in order (poll-with-timeout, real HTTP/process checks - +# no hollow asserts). It deliberately does NOT assert full nav2 goal +# completion (flaky) - only the reactive fault/update/process behavior: +# 1. Boot: broken_lidar_3_0_0 is applied (entrypoint auto-apply) and +# scan_sensor_node is running broken_lidar_node; fixed_lidar_3_0_1 is +# NOT yet registered (boot catalog holds only the bad update). +# 2. send-goal.sh -> ACTION_NAVIGATE_TO_POSE_ABORTED reaches CONFIRMED on +# bt-navigator, and controller-server picks up a supporting LOG_* fault. +# 3. Fault detail (bt-navigator) has environment_data.snapshots >= 1, and +# the rosbag bulk-data download returns a non-empty MCAP body. +# 4. publish-fix.sh -> fixed_lidar_3_0_1 appears in /updates (SOVD +# POST /updates). +# 5. apply-fix.sh -> scan_sensor_node swaps to fixed_lidar_node, but both +# faults stay latched (the key regression guard vs the old +# self-healing behavior). +# 6. clear-fault.sh -> ACTION_NAVIGATE_TO_POSE_ABORTED is gone from +# bt-navigator and the LOG_* fault is gone from controller-server. +# 7. send-goal.sh again -> neither fault reappears (clean lidar, healthy +# resume). +# +# Usage: ./tests/smoke_test_demo_narrative.sh [GATEWAY_URL] +# GATEWAY_URL defaults to OTA_GATEWAY_URL, or http://localhost:$OTA_GATEWAY_PORT +# (default port 8080) - the same env vars the operator scripts honor. + +GATEWAY_URL="${1:-${OTA_GATEWAY_URL:-http://localhost:${OTA_GATEWAY_PORT:-8080}}}" +# shellcheck disable=SC2034 # used by smoke_lib.sh +API_BASE="${GATEWAY_URL}/api/v1" + +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +# shellcheck source=tests/smoke_lib.sh +source "${SCRIPT_DIR}/smoke_lib.sh" + +trap print_summary EXIT + +DEMO_DIR="$(cd "${SCRIPT_DIR}/../demos/ota_nav2_sensor_fix" && pwd)" +GATEWAY_CONTAINER="${OTA_DEMO_GATEWAY_CONTAINER:-ota_demo_gateway}" + +BOOT_ID="broken_lidar_3_0_0" +FIX_ID="fixed_lidar_3_0_1" + +NAV_ENTITY="apps/bt-navigator" +NAV_CODE="ACTION_NAVIGATE_TO_POSE_ABORTED" +CONTROLLER_ENTITY="apps/controller-server" +# controller-server's LOG_CONTROLLER_SERVER_* code is content-hashed (derived +# from the log message), so it is never matched by exact code - only by +# "does this entity have any fault at all" (see fault_present with code=""). + +# --- Helpers built on top of smoke_lib.sh's api_get/poll_until ------------- + +# Returns 0 if a fault is present (any status) in the default GET +# /{entity}/faults list. The gateway's default filter already excludes +# cleared/healed faults, so "present" here means pending or confirmed. +# If $code is empty, matches any fault on the entity (used for the +# content-hashed controller-server LOG_* code). +fault_present() { + local entity="$1" + local code="$2" + api_get "/${entity}/faults" || return 1 + if [ -z "$code" ]; then + echo "$RESPONSE" | jq -e '.items | length > 0' > /dev/null 2>&1 + else + echo "$RESPONSE" | jq -e --arg c "$code" '.items[] | select(.fault_code == $c)' > /dev/null 2>&1 + fi +} + +# Poll until fault_present() is false, up to $3 seconds. +poll_fault_absent() { + local entity="$1" + local code="$2" + local timeout="${3:-30}" + local elapsed=0 + while [ $elapsed -lt "$timeout" ]; do + if ! fault_present "$entity" "$code"; then + return 0 + fi + sleep 2 + elapsed=$((elapsed + 2)) + done + return 1 +} + +# Assert the fault stays absent for the *entire* $3-second window - proves +# it does not reappear, rather than just "hasn't yet". +assert_fault_stays_absent() { + local entity="$1" + local code="$2" + local window="${3:-30}" + local elapsed=0 + while [ $elapsed -lt "$window" ]; do + if fault_present "$entity" "$code"; then + return 1 + fi + sleep 2 + elapsed=$((elapsed + 2)) + done + return 0 +} + +# Assert the fault stays present for the *entire* $3-second window - proves +# latching (no self-heal), rather than just "hasn't cleared yet". This is +# the regression guard against the old self-healing behavior. +assert_fault_stays_present() { + local entity="$1" + local code="$2" + local window="${3:-60}" + local elapsed=0 + while [ $elapsed -lt "$window" ]; do + if ! fault_present "$entity" "$code"; then + return 1 + fi + sleep 2 + elapsed=$((elapsed + 2)) + done + return 0 +} + +# Poll until `pgrep -af ` succeeds inside the gateway container +# (process present), up to $2 seconds. +poll_process_running() { + local pattern="$1" + local timeout="${2:-20}" + local elapsed=0 + while [ $elapsed -lt "$timeout" ]; do + if docker exec "$GATEWAY_CONTAINER" pgrep -af "$pattern" > /dev/null 2>&1; then + return 0 + fi + sleep 2 + elapsed=$((elapsed + 2)) + done + return 1 +} + +# Poll until `pgrep -af ` fails inside the gateway container +# (process gone), up to $2 seconds. +poll_process_gone() { + local pattern="$1" + local timeout="${2:-20}" + local elapsed=0 + while [ $elapsed -lt "$timeout" ]; do + if ! docker exec "$GATEWAY_CONTAINER" pgrep -af "$pattern" > /dev/null 2>&1; then + return 0 + fi + sleep 2 + elapsed=$((elapsed + 2)) + done + return 1 +} + +# --------------------------------------------------------------------- +# Wait for the gateway to come up +# --------------------------------------------------------------------- +wait_for_gateway 120 + +# --------------------------------------------------------------------- +# Step 1: Boot - entrypoint auto-applies broken_lidar_3_0_0; the fix is not +# registered yet +# --------------------------------------------------------------------- +section "Boot: ${BOOT_ID} auto-applied by the entrypoint; ${FIX_ID} not registered yet" + +echo " Waiting for ${BOOT_ID} to appear in /updates (max 120s)..." +if poll_until "/updates" ".items[] | select(. == \"${BOOT_ID}\")" 120; then + pass "${BOOT_ID} listed in /updates" +else + fail "${BOOT_ID} listed in /updates" "missing after 120s - entrypoint auto-apply did not register the update" + exit 1 +fi + +echo " Waiting for ${BOOT_ID} status to reach 'completed' (max 60s)..." +if poll_until "/updates/${BOOT_ID}/status" '.status == "completed"' 60; then + pass "${BOOT_ID} status is 'completed'" +else + fail "${BOOT_ID} status is 'completed'" "entrypoint auto-apply (prepare+execute) did not complete within 60s" + exit 1 +fi + +echo " Waiting for scan_sensor_node to run broken_lidar_node (max 20s)..." +if poll_process_running "/lib/broken_lidar/broken_lidar_node" 20; then + pass "scan_sensor_node runs broken_lidar_node at boot" +else + fail "scan_sensor_node runs broken_lidar_node at boot" "broken_lidar_node process not found in ${GATEWAY_CONTAINER}" + exit 1 +fi + +if api_get "/updates" && echo "$RESPONSE" | jq -e --arg id "$FIX_ID" '.items[] | select(. == $id)' >/dev/null 2>&1; then + fail "${FIX_ID} is NOT registered at boot" "the forward hotfix leaked into the boot catalog" +else + pass "${FIX_ID} is NOT registered at boot (boot catalog holds only ${BOOT_ID})" +fi + +# --------------------------------------------------------------------- +# Step 2: send-goal.sh -> reactive ACTION_NAVIGATE_TO_POSE_ABORTED +# --------------------------------------------------------------------- +section "Reactive fault: send-goal.sh triggers ACTION_NAVIGATE_TO_POSE_ABORTED" + +# x=1.8, y=2.3 (frame map) drives straight into the phantom sector so nav2 +# reliably stalls - the send-goal.sh script defaults elsewhere are for +# ad-hoc operator use, not this repeatable regression check. +"${DEMO_DIR}/send-goal.sh" 1.8 2.3 + +echo " Waiting for ${NAV_CODE} to reach CONFIRMED on ${NAV_ENTITY} (max 60s)..." +if poll_until "/${NAV_ENTITY}/faults" \ + ".items[] | select(.fault_code == \"${NAV_CODE}\") | select((.status // \"\") | ascii_upcase == \"CONFIRMED\")" \ + 60; then + pass "${NAV_CODE} confirmed on ${NAV_ENTITY} after send-goal.sh" +else + fail "${NAV_CODE} confirmed on ${NAV_ENTITY} after send-goal.sh" \ + "fault never reached CONFIRMED within 60s - either nav2 didn't accept the goal or the action-status bridge is broken" +fi + +echo " Waiting for a supporting LOG_* fault on ${CONTROLLER_ENTITY} (max 60s)..." +if poll_until "/${CONTROLLER_ENTITY}/faults" '.items | length > 0' 60; then + pass "supporting LOG_* fault present on ${CONTROLLER_ENTITY}" +else + fail "supporting LOG_* fault present on ${CONTROLLER_ENTITY}" \ + "no fault appeared within 60s - either nav2 didn't stall or the log bridge is broken" +fi + +# --------------------------------------------------------------------- +# Step 3: fault detail environment data + MCAP rosbag capture +# --------------------------------------------------------------------- +section "Fault detail: environment_data snapshot + MCAP rosbag capture" + +if api_get "/${NAV_ENTITY}/faults/${NAV_CODE}"; then + pass "GET /${NAV_ENTITY}/faults/${NAV_CODE} returns 200" + if echo "$RESPONSE" | jq -e '(.environment_data.snapshots // []) | length >= 1' > /dev/null 2>&1; then + pass "fault detail has >=1 environment_data snapshot" + else + fail "fault detail has >=1 environment_data snapshot" \ + "got $(echo "$RESPONSE" | jq -c '.environment_data.snapshots // []' 2>/dev/null)" + fi +else + fail "GET /${NAV_ENTITY}/faults/${NAV_CODE} returns 200" "unexpected status code" +fi + +# The MCAP rosbag is written ASYNCHRONOUSLY - the ring buffer is flushed on +# confirm, then rosbag.duration_after_sec more seconds are recorded and the bag +# is finalized + registered a few seconds AFTER the fault confirms. So poll for +# the rosbag snapshot to attach and for the bag to be downloadable, rather than +# checking once (which races the write and 404s). +rosbag_snapshot=no +for _ in $(seq 1 20); do + if api_get "/${NAV_ENTITY}/faults/${NAV_CODE}" && \ + echo "$RESPONSE" | jq -e '[.environment_data.snapshots[]?.type] | index("rosbag")' > /dev/null 2>&1; then + rosbag_snapshot=yes + break + fi + sleep 2 +done +if [ "$rosbag_snapshot" = "yes" ]; then + pass "fault detail has a rosbag snapshot (MCAP capture attached)" +else + fail "fault detail has a rosbag snapshot" \ + "no rosbag snapshot after ~40s: $(echo "$RESPONSE" | jq -c '[.environment_data.snapshots[]?.type]' 2>/dev/null)" +fi + +# Binary MCAP body - bypass api_get (it reconstructs $RESPONSE via sed/echo, +# which mangles binary content); write straight to a temp file instead. Poll +# the download until served (same async-capture reason as above). Path is +# GET /apps/bt-navigator/bulk-data/rosbags/ACTION_NAVIGATE_TO_POSE_ABORTED. +rosbag_tmp="$(mktemp)" +rosbag_http=000 +for _ in $(seq 1 20); do + rosbag_http=$(curl -s -o "$rosbag_tmp" -w '%{http_code}' \ + "${API_BASE}/${NAV_ENTITY}/bulk-data/rosbags/${NAV_CODE}" 2>/dev/null) || true + [ "$rosbag_http" = "200" ] && break + sleep 2 +done +rosbag_bytes=$(wc -c < "$rosbag_tmp" 2>/dev/null || echo 0) +rm -f "$rosbag_tmp" + +if [ "$rosbag_http" = "200" ]; then + pass "GET /${NAV_ENTITY}/bulk-data/rosbags/${NAV_CODE} returns 200" +else + fail "GET /${NAV_ENTITY}/bulk-data/rosbags/${NAV_CODE} returns 200" "got HTTP ${rosbag_http}" +fi + +if [ "${rosbag_bytes:-0}" -gt 0 ] 2>/dev/null; then + pass "MCAP rosbag body is non-empty (${rosbag_bytes} bytes)" +else + fail "MCAP rosbag body is non-empty" "body was 0 bytes" +fi + +# --------------------------------------------------------------------- +# Step 4: publish-fix.sh -> fixed_lidar_3_0_1 registers via SOVD POST /updates +# --------------------------------------------------------------------- +section "Publish: publish-fix.sh registers ${FIX_ID} (SOVD POST /updates)" + +if "${DEMO_DIR}/publish-fix.sh" >/dev/null; then + pass "./publish-fix.sh registers ${FIX_ID}" +else + fail "./publish-fix.sh registers ${FIX_ID}" "publish-fix.sh exited non-zero" +fi + +echo " Waiting for ${FIX_ID} to appear in /updates after publish (max 20s)..." +if poll_until "/updates" ".items[] | select(. == \"${FIX_ID}\")" 20; then + pass "/updates contains '${FIX_ID}' after publish" +else + fail "/updates contains '${FIX_ID}' after publish" "id missing after publish-fix.sh" +fi + +# --------------------------------------------------------------------- +# Step 5: apply-fix.sh -> fixed_lidar, both faults stay latched +# --------------------------------------------------------------------- +section "Apply: apply-fix.sh swaps to fixed_lidar, faults stay latched" + +"${DEMO_DIR}/apply-fix.sh" + +echo " Waiting for scan_sensor_node to run fixed_lidar_node (max 20s)..." +if poll_process_running "/lib/fixed_lidar/fixed_lidar_node" 20; then + pass "scan_sensor_node runs fixed_lidar_node after apply" +else + fail "scan_sensor_node runs fixed_lidar_node after apply" "fixed_lidar_node process not found in ${GATEWAY_CONTAINER}" +fi + +echo " Waiting for broken_lidar_node to be gone (max 20s)..." +if poll_process_gone "/lib/broken_lidar/broken_lidar_node" 20; then + pass "broken_lidar_node killed after apply" +else + fail "broken_lidar_node killed after apply" "broken_lidar_node still alive in ${GATEWAY_CONTAINER}" +fi + +echo " Asserting ${NAV_CODE} stays latched on ${NAV_ENTITY} for 60s post-apply (regression guard vs self-heal)..." +if assert_fault_stays_present "$NAV_ENTITY" "$NAV_CODE" 60; then + pass "${NAV_CODE} still present on ${NAV_ENTITY} after apply (latched, not self-healed)" +else + fail "${NAV_CODE} still present on ${NAV_ENTITY} after apply (latched, not self-healed)" \ + "fault disappeared on its own after the fix was applied - self-healing regression" +fi + +echo " Asserting the supporting LOG_* fault stays latched on ${CONTROLLER_ENTITY} for 60s post-apply..." +if assert_fault_stays_present "$CONTROLLER_ENTITY" "" 60; then + pass "LOG_* fault still present on ${CONTROLLER_ENTITY} after apply (latched, not self-healed)" +else + fail "LOG_* fault still present on ${CONTROLLER_ENTITY} after apply (latched, not self-healed)" \ + "fault disappeared on its own after the fix was applied - self-healing regression" +fi + +# --------------------------------------------------------------------- +# Step 6: clear-fault.sh -> operator clear removes both latched faults +# --------------------------------------------------------------------- +section "Operator clear: clear-fault.sh removes the latched faults" + +"${DEMO_DIR}/clear-fault.sh" + +echo " Waiting for ${NAV_CODE} to be gone from /${NAV_ENTITY}/faults (max 30s)..." +if poll_fault_absent "$NAV_ENTITY" "$NAV_CODE" 30; then + pass "${NAV_CODE} gone from ${NAV_ENTITY} after clear-fault.sh" +else + fail "${NAV_CODE} gone from ${NAV_ENTITY} after clear-fault.sh" "fault still listed 30s after the operator clear" +fi + +echo " Waiting for ${CONTROLLER_ENTITY} faults to clear (max 30s)..." +if poll_fault_absent "$CONTROLLER_ENTITY" "" 30; then + pass "LOG_* fault gone from ${CONTROLLER_ENTITY} after clear-fault.sh (clear-all)" +else + fail "LOG_* fault gone from ${CONTROLLER_ENTITY} after clear-fault.sh (clear-all)" \ + "fault still listed 30s after the operator clear-all" +fi + +# --------------------------------------------------------------------- +# Step 7: send-goal.sh again -> healthy resume, no relapse +# --------------------------------------------------------------------- +section "Healthy resume: send-goal.sh on the clean lidar does not reintroduce either fault" + +"${DEMO_DIR}/send-goal.sh" 1.8 2.3 + +echo " Watching for ${NAV_CODE} to stay absent on ${NAV_ENTITY} for 30s (clean lidar, healthy resume)..." +if assert_fault_stays_absent "$NAV_ENTITY" "$NAV_CODE" 30; then + pass "${NAV_CODE} does not reappear on ${NAV_ENTITY} (healthy resume)" +else + fail "${NAV_CODE} does not reappear on ${NAV_ENTITY} (healthy resume)" \ + "fault reappeared on the clean lidar - fixed_lidar or fault_manager regression" +fi + +echo " Watching for ${CONTROLLER_ENTITY} to stay clean for 30s (clean lidar, healthy resume)..." +if assert_fault_stays_absent "$CONTROLLER_ENTITY" "" 30; then + pass "no LOG_* fault reappears on ${CONTROLLER_ENTITY} (healthy resume)" +else + fail "no LOG_* fault reappears on ${CONTROLLER_ENTITY} (healthy resume)" \ + "fault reappeared on the clean lidar - fixed_lidar or fault_manager regression" +fi diff --git a/tests/smoke_test_ota.sh b/tests/smoke_test_ota.sh new file mode 100755 index 0000000..5c5564a --- /dev/null +++ b/tests/smoke_test_ota.sh @@ -0,0 +1,249 @@ +#!/bin/bash +# Smoke tests for the ota_nav2_sensor_fix demo. +# Runs from the host against the gateway on localhost:8080 and asserts: +# - the gateway loads our ota_update_plugin as the UpdateProvider +# - the boot catalog holds ONLY the bad update (broken_lidar_3_0_0) - the +# forward hotfix (fixed_lidar_3_0_1) is NOT present until published +# - ./publish-fix.sh registers fixed_lidar_3_0_1 via SOVD POST /updates, +# after which it appears in GET /updates +# - the update detail uses spec field names (update_name, no `name`/`version`) +# - the publish + apply flow actually swaps broken_lidar_node for +# fixed_lidar_node inside the gateway container +# +# Usage: ./tests/smoke_test_ota.sh [GATEWAY_URL] +# Default GATEWAY_URL: http://localhost:8080 + +GATEWAY_URL="${1:-http://localhost:8080}" +# shellcheck disable=SC2034 # Used by smoke_lib.sh +API_BASE="${GATEWAY_URL}/api/v1" + +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +# shellcheck source=tests/smoke_lib.sh +source "${SCRIPT_DIR}/smoke_lib.sh" + +trap print_summary EXIT + +DEMO_DIR="$(cd "${SCRIPT_DIR}/../demos/ota_nav2_sensor_fix" && pwd)" +GATEWAY_CONTAINER="${OTA_DEMO_GATEWAY_CONTAINER:-ota_demo_gateway}" + +BOOT_ID="broken_lidar_3_0_0" +FIX_ID="fixed_lidar_3_0_1" + +# Confirm a process is or is not running inside the gateway container. +# Usage: assert_process_running +# assert_process_gone +assert_process_running() { + local pattern="$1" + local desc="$2" + if docker exec "$GATEWAY_CONTAINER" pgrep -f "$pattern" >/dev/null 2>&1; then + pass "$desc" + else + fail "$desc" "no process matching '$pattern' in $GATEWAY_CONTAINER" + fi +} + +assert_process_gone() { + local pattern="$1" + local desc="$2" + if ! docker exec "$GATEWAY_CONTAINER" pgrep -f "$pattern" >/dev/null 2>&1; then + pass "$desc" + else + fail "$desc" "process matching '$pattern' still alive in $GATEWAY_CONTAINER" + fi +} + +# --- Wait for gateway startup --- + +wait_for_gateway 90 + +# Plugin's boot poll fetches /catalog and registers entries; wait for it. +echo " Waiting for plugin's boot poll to register catalog (max 30s)..." +if poll_until "/updates" ".items[] | select(. == \"${BOOT_ID}\")" 30; then + echo " Catalog registered" +else + echo " Catalog NOT registered within 30s" + exit 1 +fi + +# --- Tests --- + +section "Health" + +if api_get "/health"; then + pass "GET /health returns 200" +else + fail "GET /health returns 200" "unexpected status code" +fi + +section "UpdateProvider plugin loaded" + +# Capture logs into a variable and grep via here-string. Piping `printf | grep -q` +# still SIGPIPEs printf when grep -q exits early on first match, and with +# `set -o pipefail` the whole pipeline returns 141 - which `if` reads as +# "no match" even when the line was found. Here-strings avoid the pipe entirely. +GATEWAY_LOGS=$(docker logs "$GATEWAY_CONTAINER" 2>&1 || true) + +if grep -q "Update backend provided by plugin" <<<"$GATEWAY_LOGS"; then + pass "gateway log says: 'Update backend provided by plugin'" +else + fail "gateway log says: 'Update backend provided by plugin'" "log line missing" +fi + +if grep -q "Updates enabled but no UpdateProvider plugin loaded" <<<"$GATEWAY_LOGS"; then + fail "no 'no UpdateProvider' warning" "warning was logged" +else + pass "no 'no UpdateProvider' warning" +fi + +section "Boot catalog (GET /updates returns SOVD {items}) - bad update only" + +if api_get "/updates"; then + pass "GET /updates returns 200" +else + fail "GET /updates returns 200" "unexpected status code" +fi + +if echo "$RESPONSE" | jq -e '.items | type == "array"' >/dev/null 2>&1; then + pass "/updates response has items array" +else + fail "/updates response has items array" "envelope mismatch (SOVD spec violation)" +fi + +if echo "$RESPONSE" | jq -e --arg id "$BOOT_ID" '.items[] | select(. == $id)' >/dev/null 2>&1; then + pass "/updates contains '$BOOT_ID'" +else + fail "/updates contains '$BOOT_ID'" "id missing" +fi + +if echo "$RESPONSE" | jq -e --arg id "$FIX_ID" '.items[] | select(. == $id)' >/dev/null 2>&1; then + fail "/updates does NOT contain '$FIX_ID' before publish" "the fix leaked into the boot catalog" +else + pass "/updates does NOT contain '$FIX_ID' before publish" +fi + +section "Publish the fix (./publish-fix.sh -> SOVD POST /updates)" + +if OTA_GATEWAY_URL="$GATEWAY_URL" OTA_GATEWAY_CONTAINER="$GATEWAY_CONTAINER" \ + "${DEMO_DIR}/publish-fix.sh" >/dev/null; then + pass "./publish-fix.sh registers ${FIX_ID}" +else + fail "./publish-fix.sh registers ${FIX_ID}" "publish-fix.sh exited non-zero" +fi + +echo " Waiting for ${FIX_ID} to appear in /updates after publish (max 20s)..." +if poll_until "/updates" ".items[] | select(. == \"${FIX_ID}\")" 20; then + pass "/updates contains '${FIX_ID}' after publish" +else + fail "/updates contains '${FIX_ID}' after publish" "id missing after publish-fix.sh" +fi + +section "Detail field shape (SOVD ISO 17978-3 compliance)" + +# fixed_lidar fix detail: must use spec field names +if api_get "/updates/${FIX_ID}"; then + pass "GET /updates/${FIX_ID} returns 200" + + if echo "$RESPONSE" | jq -e '.update_name' >/dev/null 2>&1; then + pass "detail has update_name (SOVD spec)" + else + fail "detail has update_name (SOVD spec)" "field missing - spec violation" + fi + + if echo "$RESPONSE" | jq -e '.name' >/dev/null 2>&1; then + fail "detail does NOT have 'name'" "found 'name' instead of 'update_name'" + else + pass "detail does NOT have 'name'" + fi + + if echo "$RESPONSE" | jq -e '.version' >/dev/null 2>&1; then + fail "detail does NOT have plain 'version'" "should be x_medkit_version (vendor extension)" + else + pass "detail does NOT have plain 'version'" + fi + + if echo "$RESPONSE" | jq -e '.x_medkit_version == "3.0.1"' >/dev/null 2>&1; then + pass "detail has x_medkit_version = 3.0.1" + else + fail "detail has x_medkit_version = 3.0.1" "field missing or wrong value" + fi + + if echo "$RESPONSE" | jq -e '.updated_components | index("scan_sensor_node")' >/dev/null 2>&1; then + pass "detail has updated_components: ['scan_sensor_node']" + else + fail "detail has updated_components: ['scan_sensor_node']" "kind metadata missing" + fi + + if echo "$RESPONSE" | jq -e '.x_medkit_replaces_executable == "broken_lidar_node"' >/dev/null 2>&1; then + pass "detail has x_medkit_replaces_executable = broken_lidar_node" + else + fail "detail has x_medkit_replaces_executable" "field missing" + fi +fi + +section "Initial process state" + +assert_process_running "/lib/broken_lidar/broken_lidar_node" "broken_lidar_node running before update" + +section "/scan SetRemap regression (only broken_lidar publishes, not gz-bridge)" + +# config/ros_gz_bridge.yaml bridges the real gz front-laser onto /scan_sim, +# not /scan directly - scan_sensor_node (broken_lidar/fixed_lidar) subscribes +# /scan_sim and republishes onto /scan, leaving it the sole publisher there. +# If that remap regresses, both publishers stomp each other and nav2 sees +# garbage. Use ros2 topic info -v inside the container (host runner has no +# ROS install) and assert exactly one publisher whose node name is NOT +# ros_gz_bridge. +# `ros2 topic info -v` depends on the ros2 daemon's graph cache, which is +# unreliable in some container/DDS setups (it reports 0 publishers for a topic +# that clearly has one, and --no-daemon can hang). Use rclpy's +# get_publishers_info_by_topic instead - it does its own fresh discovery and is +# deterministic. Assert exactly one publisher on /scan and that it is +# scan_sensor_node, not the gz bridge (the gz bridge on /scan would mean the +# /scan_sim remap regressed and both stomp /scan). +# Do the whole check in ONE docker exec and return the verdict via the python +# exit code - no temp file, no captured stdout, no write-then-read race (all of +# which proved flaky under docker-out-of-docker). The probe polls for the +# publisher (scan_sensor_node respawns right after an update swap) and exits 0 +# iff /scan has exactly one publisher and it is not the gz bridge. +if docker exec -i "$GATEWAY_CONTAINER" bash -lc \ + 'source /opt/ros/jazzy/setup.bash && python3 -' <<'PYEOF' +import rclpy, time, sys +from rclpy.node import Node +rclpy.init() +n = Node('scan_pub_probe') +info = [] +for _ in range(15): + info = n.get_publishers_info_by_topic('/scan') + if len(info) >= 1: + break + time.sleep(1) +names = ' '.join(i.node_name for i in info) +ok = len(info) == 1 and 'ros_gz_bridge' not in names and 'parameter_bridge' not in names +sys.exit(0 if ok else 1) +PYEOF +then + pass "/scan has exactly 1 publisher (scan_sensor_node, not the gz bridge)" +else + fail "/scan has exactly 1 publisher (scan_sensor_node, not the gz bridge)" \ + "expected one non-gz-bridge publisher on /scan; SetRemap may have regressed" +fi + +section "Apply flow: PUT /updates/${FIX_ID}/prepare + /execute" + +curl -fsS -X PUT -H 'Content-Type: application/json' -d '{}' \ + "${API_BASE}/updates/${FIX_ID}/prepare" >/dev/null +sleep 4 +curl -fsS -X PUT -H 'Content-Type: application/json' -d '{}' \ + "${API_BASE}/updates/${FIX_ID}/execute" >/dev/null +sleep 6 + +if api_get "/updates/${FIX_ID}/status"; then + if echo "$RESPONSE" | jq -e '.status == "completed"' >/dev/null 2>&1; then + pass "${FIX_ID} status is completed" + else + fail "${FIX_ID} status is completed" "got $(echo "$RESPONSE" | jq -c .)" + fi +fi + +assert_process_gone "/lib/broken_lidar/broken_lidar_node" "broken_lidar_node killed after update" +assert_process_running "/lib/fixed_lidar/fixed_lidar_node" "fixed_lidar_node spawned after update" From d543acec9a98997d6d2d68cc72b4e94393cced2a Mon Sep 17 00:00:00 2001 From: Bartosz Burda Date: Sat, 4 Jul 2026 18:05:55 +0200 Subject: [PATCH 02/19] fix(ota_demo): drop the recovery dance so the stall aborts cleanly The default recovery (spin, wait 5s, back-up) made a stuck robot thrash for 20-30s before navigate_to_pose aborted. Strip the recovery RoundRobin down to a costmap clear (no motion), keep the top-level retry so the goal still aborts, and shorten the progress-checker window (movement_time_allowance 10 -> 4s). The robot now drives, stalls on the phantom, and aborts in ~15-19s with no spinning. --- .../ota_nav2_sensor_fix_demo/config/nav2_params.yaml | 2 +- .../config/navigate_to_pose_fast_abort.xml | 5 +---- 2 files changed, 2 insertions(+), 5 deletions(-) diff --git a/demos/ota_nav2_sensor_fix/ros2_packages/ota_nav2_sensor_fix_demo/config/nav2_params.yaml b/demos/ota_nav2_sensor_fix/ros2_packages/ota_nav2_sensor_fix_demo/config/nav2_params.yaml index ae62a66..ad5e816 100644 --- a/demos/ota_nav2_sensor_fix/ros2_packages/ota_nav2_sensor_fix_demo/config/nav2_params.yaml +++ b/demos/ota_nav2_sensor_fix/ros2_packages/ota_nav2_sensor_fix_demo/config/nav2_params.yaml @@ -126,7 +126,7 @@ controller_server: progress_checker: plugin: "nav2_controller::SimpleProgressChecker" required_movement_radius: 0.5 - movement_time_allowance: 10.0 + movement_time_allowance: 4.0 general_goal_checker: stateful: True diff --git a/demos/ota_nav2_sensor_fix/ros2_packages/ota_nav2_sensor_fix_demo/config/navigate_to_pose_fast_abort.xml b/demos/ota_nav2_sensor_fix/ros2_packages/ota_nav2_sensor_fix_demo/config/navigate_to_pose_fast_abort.xml index 807987b..fcd3435 100644 --- a/demos/ota_nav2_sensor_fix/ros2_packages/ota_nav2_sensor_fix_demo/config/navigate_to_pose_fast_abort.xml +++ b/demos/ota_nav2_sensor_fix/ros2_packages/ota_nav2_sensor_fix_demo/config/navigate_to_pose_fast_abort.xml @@ -19,7 +19,7 @@ - + @@ -39,9 +39,6 @@ - - - From 61da2f35371dea75bdd50d32adce4a148324400e Mon Sep 17 00:00:00 2001 From: Bartosz Burda Date: Sat, 4 Jul 2026 18:43:51 +0200 Subject: [PATCH 03/19] feat(ota_demo): expose the health checks as a SOVD diagnostic function Group the health-check app under a new 'Health Checks' function (category diagnostics) so the differential-diagnosis operations (lidar / localization / drivetrain / costmap) show up as a first-class functional grouping in the SOVD entity tree, alongside Fleet Diagnostics. --- demos/ota_nav2_sensor_fix/manifest.yaml | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/demos/ota_nav2_sensor_fix/manifest.yaml b/demos/ota_nav2_sensor_fix/manifest.yaml index a04bb38..8d3d5b9 100644 --- a/demos/ota_nav2_sensor_fix/manifest.yaml +++ b/demos/ota_nav2_sensor_fix/manifest.yaml @@ -272,6 +272,13 @@ functions: - medkit-gateway - medkit-fault-manager + - id: health-checks + name: "Health Checks" + category: "diagnostics" + description: "Operator-invoked differential-diagnosis checks (lidar / localization / drivetrain / costmap) for root-cause confirmation" + hosted_by: + - health-check + - id: live-telemetry name: "Live Telemetry" category: "observability" From d9e81edac2584142454961af5095fe2f560388d2 Mon Sep 17 00:00:00 2001 From: Bartosz Burda Date: Sat, 4 Jul 2026 19:03:50 +0200 Subject: [PATCH 04/19] fix(ota_demo): gate run-demo.sh on real drive-readiness, self-heal the gz race The gz_ros2_control cold start races the controller spawners; when it loses, the robot has no odometry/TF and Nav2 aborts every goal instantly. run-demo.sh now waits for the odom->base_footprint TF to actually flow before reporting the demo up, and restarts the sim (bounded to 3 tries) if it does not - so one run-demo.sh hands off a genuinely drive-ready robot instead of leaving it to chance. --- demos/ota_nav2_sensor_fix/run-demo.sh | 48 +++++++++++++++++++++++++++ 1 file changed, 48 insertions(+) diff --git a/demos/ota_nav2_sensor_fix/run-demo.sh b/demos/ota_nav2_sensor_fix/run-demo.sh index 860fdcf..6d8c5f2 100755 --- a/demos/ota_nav2_sensor_fix/run-demo.sh +++ b/demos/ota_nav2_sensor_fix/run-demo.sh @@ -125,6 +125,54 @@ if ! curl -fsS "${GATEWAY_URL}/api/v1/health" >/dev/null 2>&1; then exit 1 fi +# Drive-readiness gate. The gz_ros2_control hardware interface + the +# joint_state_broadcaster / diff_drive_controller spawners race the sim's +# cold start; when they lose, diff_drive publishes no odometry, the +# odom->base_footprint TF never appears, and Nav2 aborts every goal instantly +# (the robot never moves). That race is more likely when the host CPU is +# loaded. Rather than hand off a demo that cannot drive, wait for real +# odometry; if it does not come up, restart the sim and try again (bounded), +# so `run-demo.sh` only reports "up" once the robot is genuinely drive-ready. +GW_CONTAINER="${OTA_DEMO_GATEWAY_CONTAINER:-ota_demo_gateway}" +drive_ready() { + docker exec "${GW_CONTAINER}" bash -lc \ + 'source /opt/ros/jazzy/setup.bash >/dev/null 2>&1; \ + timeout 6 ros2 run tf2_ros tf2_echo odom base_footprint > /tmp/_navcheck 2>&1; \ + grep -qE "Translation|At time" /tmp/_navcheck' >/dev/null 2>&1 +} + +echo "" +echo "[3b/3] Waiting for the robot to become drive-ready (odometry + TF)..." +DRIVE_READY=false +for boot_try in 1 2 3; do + for _ in $(seq 1 13); do + if drive_ready; then DRIVE_READY=true; break; fi + sleep 10 + done + if [[ "$DRIVE_READY" == "true" ]]; then + echo " Drive-ready: odometry and odom->base_footprint TF are live." + break + fi + if [[ "$boot_try" -lt 3 ]]; then + echo " No odometry after ~130s (gz_ros2_control cold-start race)." + echo " Restarting the sim (attempt $((boot_try + 1))/3)..." + ${COMPOSE_CMD} restart gateway >/dev/null 2>&1 || true + for _ in $(seq 1 40); do + curl -fsS "${GATEWAY_URL}/api/v1/health" >/dev/null 2>&1 && break + sleep 3 + done + fi +done + +if [[ "$DRIVE_READY" != "true" ]]; then + echo "" + echo "WARNING: the robot's odometry/TF did not come up after 3 sim starts." + echo " This is a gz_ros2_control cold-start race, more likely under host CPU load." + echo " Free host CPU (close heavy apps; on WSL, 'wsl --shutdown') and re-run ./run-demo.sh." + echo " Check manually:" + echo " docker exec ${GW_CONTAINER} bash -lc 'source /opt/ros/jazzy/setup.bash && ros2 run tf2_ros tf2_echo odom base_footprint'" +fi + echo "" echo "Demo is up." echo "" From aef46534c6451d2b46074ae5d979bba8ef9e5823 Mon Sep 17 00:00:00 2001 From: Bartosz Burda Date: Sat, 4 Jul 2026 19:45:00 +0200 Subject: [PATCH 05/19] fix(ota_demo): capture the diagnostic topics in the fault MCAP + fold health checks into Fleet Diagnostics The fault_manager defaults snapshots.rosbag.topics to 'entity', which narrows the captured bag to the fault source node's subgraph (bt_navigator: /odom /tf /bond /navigate_to_pose/*) and drops /scan, /cmd_vel and /local_costmap - so the downloaded MCAP had none of the diagnostic topics and Foxglove playback showed nothing. Set topics 'explicit' (write exactly include_topics) + qos_match so the capture records /scan (the phantom), /cmd_vel, /tf and the local costmap. Also add the health-check app to the Fleet Diagnostics function so its operations surface there (dropped the redundant standalone function). --- demos/ota_nav2_sensor_fix/manifest.yaml | 8 +------- .../ota_nav2_sensor_fix_demo/launch/demo.launch.py | 11 +++++++++++ 2 files changed, 12 insertions(+), 7 deletions(-) diff --git a/demos/ota_nav2_sensor_fix/manifest.yaml b/demos/ota_nav2_sensor_fix/manifest.yaml index 8d3d5b9..5c258fa 100644 --- a/demos/ota_nav2_sensor_fix/manifest.yaml +++ b/demos/ota_nav2_sensor_fix/manifest.yaml @@ -267,16 +267,10 @@ functions: - id: fleet-diagnostics name: "Fleet Diagnostics" category: "diagnostics" - description: "SOVD REST surface + fault aggregation - this panel" + description: "SOVD REST surface + fault aggregation + operator health checks (lidar / localization / drivetrain / costmap) for root-cause confirmation" hosted_by: - medkit-gateway - medkit-fault-manager - - - id: health-checks - name: "Health Checks" - category: "diagnostics" - description: "Operator-invoked differential-diagnosis checks (lidar / localization / drivetrain / costmap) for root-cause confirmation" - hosted_by: - health-check - id: live-telemetry diff --git a/demos/ota_nav2_sensor_fix/ros2_packages/ota_nav2_sensor_fix_demo/launch/demo.launch.py b/demos/ota_nav2_sensor_fix/ros2_packages/ota_nav2_sensor_fix_demo/launch/demo.launch.py index 9839896..ebf450c 100644 --- a/demos/ota_nav2_sensor_fix/ros2_packages/ota_nav2_sensor_fix_demo/launch/demo.launch.py +++ b/demos/ota_nav2_sensor_fix/ros2_packages/ota_nav2_sensor_fix_demo/launch/demo.launch.py @@ -292,6 +292,17 @@ def generate_launch_description(): 'snapshots.rosbag.lazy_start': False, # keep buffering so the pre-trigger window is captured 'snapshots.rosbag.duration_sec': 5.0, 'snapshots.rosbag.duration_after_sec': 2.0, + # topics 'explicit' -> write EXACTLY include_topics. The fault_manager + # default is 'entity', which narrows the flush to the fault source + # node's subgraph (bt_navigator: /odom /tf /bond /navigate_to_pose/*) + # and discards /scan, /cmd_vel and /local_costmap from the buffer - + # so the downloaded MCAP had none of the diagnostic topics and + # Foxglove playback showed nothing. qos_match upgrades the capture + # subscription to transient_local so the latched /tf_static (published + # once at startup) is actually recorded; without it the TF tree can't + # be rebuilt on replay and /scan will not render in 3D. + 'snapshots.rosbag.topics': 'explicit', + 'snapshots.rosbag.qos_match': True, 'snapshots.rosbag.include_topics': ['/scan', '/cmd_vel', '/tf', '/tf_static', '/local_costmap/costmap'], 'snapshots.rosbag.storage_path': '/var/lib/ros2_medkit/rosbags', }], From 901d7e95e70d85cdc677adba1ab5f762c9a8db13 Mon Sep 17 00:00:00 2001 From: Bartosz Burda Date: Sat, 4 Jul 2026 21:29:17 +0200 Subject: [PATCH 06/19] fix(ota_demo): mark the hotfix update automated so the UI enables Execute The descriptor shipped automated:false, which the Updates panel surfaces as 'not automated' and uses to disable the Execute button after a completed Prepare. The OTA plugin does apply the update itself on execute (kill + respawn), so it is an automated application - set automated:true. Verified: Prepare alone does NOT auto-swap (broken stays), Execute swaps to fixed - the two-step operator flow is preserved. --- demos/ota_nav2_sensor_fix/updates/fixed_lidar_3_0_1.json | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/demos/ota_nav2_sensor_fix/updates/fixed_lidar_3_0_1.json b/demos/ota_nav2_sensor_fix/updates/fixed_lidar_3_0_1.json index 2f06787..0b71958 100644 --- a/demos/ota_nav2_sensor_fix/updates/fixed_lidar_3_0_1.json +++ b/demos/ota_nav2_sensor_fix/updates/fixed_lidar_3_0_1.json @@ -1,7 +1,7 @@ { "id": "fixed_lidar_3_0_1", "update_name": "fixed_lidar 3.0.1", - "automated": false, + "automated": true, "origins": ["remote"], "notes": "Fix regressed /scan noise filter (3.0.0 hotfix)", "duration": 10, From 9ae82b15a03e50b438f7d5406e482a6fc82f2b7d Mon Sep 17 00:00:00 2001 From: Bartosz Burda Date: Sat, 4 Jul 2026 21:45:02 +0200 Subject: [PATCH 07/19] fix(ota_demo): make the fault MCAP self-contained + persist the fast-abort BT Add a latched_relay node that re-publishes the latched /robot_description (URDF) and /tf_static at 2 Hz so the fault_manager's post-hoc volatile rosbag capture records them - without them a downloaded MCAP had no robot mesh and no static TF to place /scan against, so playback showed only floating obstacles. Add /robot_description to the capture include_topics. Also bake the nav2 default behavior-tree overwrite into Dockerfile.gateway (was only applied at runtime) so a clean build ships the fast-abort tree. --- demos/ota_nav2_sensor_fix/Dockerfile.gateway | 10 ++ .../ota_nav2_sensor_fix_demo/CMakeLists.txt | 5 + .../launch/demo.launch.py | 46 +++++++-- .../ota_nav2_sensor_fix_demo/package.xml | 6 ++ .../scripts/latched_relay.py | 99 +++++++++++++++++++ 5 files changed, 157 insertions(+), 9 deletions(-) create mode 100755 demos/ota_nav2_sensor_fix/ros2_packages/ota_nav2_sensor_fix_demo/scripts/latched_relay.py diff --git a/demos/ota_nav2_sensor_fix/Dockerfile.gateway b/demos/ota_nav2_sensor_fix/Dockerfile.gateway index ad0012f..2a3f398 100644 --- a/demos/ota_nav2_sensor_fix/Dockerfile.gateway +++ b/demos/ota_nav2_sensor_fix/Dockerfile.gateway @@ -184,6 +184,16 @@ COPY --from=builder /ws/install /ws/install COPY gateway_config.yaml /etc/ros2_medkit/gateway_config.yaml COPY manifest.yaml /etc/ros2_medkit/manifest.yaml +# nav2 loads its built-in default nav_to_pose behavior tree (the +# default_nav_to_pose_bt_xml_filename param does not reliably override it under +# this bringup), whose recovery subtree (spin, 5s wait, back-up, 6 retries) +# makes a phantom-stuck robot thrash for 20-30s before navigate_to_pose aborts. +# Overwrite that default with our fast-abort tree (recovery reduced to a costmap +# clear, single retry) so the stall aborts cleanly in ~15s no matter which tree +# nav2 ends up loading. +RUN cp /ws/install/ota_nav2_sensor_fix_demo/share/ota_nav2_sensor_fix_demo/config/navigate_to_pose_fast_abort.xml \ + /opt/ros/jazzy/share/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml + # Pre-create the fragments directory so the gateway's manifest manager # scans an existing (empty) dir at boot rather than logging "missing # fragments_dir" warnings. Plugin writes / removes yaml files here at diff --git a/demos/ota_nav2_sensor_fix/ros2_packages/ota_nav2_sensor_fix_demo/CMakeLists.txt b/demos/ota_nav2_sensor_fix/ros2_packages/ota_nav2_sensor_fix_demo/CMakeLists.txt index daf67aa..ded0e90 100644 --- a/demos/ota_nav2_sensor_fix/ros2_packages/ota_nav2_sensor_fix_demo/CMakeLists.txt +++ b/demos/ota_nav2_sensor_fix/ros2_packages/ota_nav2_sensor_fix_demo/CMakeLists.txt @@ -23,4 +23,9 @@ install(DIRECTORY maps/ DESTINATION share/${PROJECT_NAME}/maps ) +install(DIRECTORY scripts/ + DESTINATION share/${PROJECT_NAME}/scripts + USE_SOURCE_PERMISSIONS +) + ament_package() diff --git a/demos/ota_nav2_sensor_fix/ros2_packages/ota_nav2_sensor_fix_demo/launch/demo.launch.py b/demos/ota_nav2_sensor_fix/ros2_packages/ota_nav2_sensor_fix_demo/launch/demo.launch.py index ebf450c..fbdf359 100644 --- a/demos/ota_nav2_sensor_fix/ros2_packages/ota_nav2_sensor_fix_demo/launch/demo.launch.py +++ b/demos/ota_nav2_sensor_fix/ros2_packages/ota_nav2_sensor_fix_demo/launch/demo.launch.py @@ -14,9 +14,9 @@ # - ros2_medkit_log_bridge + ros2_medkit_action_status_bridge, started # 15s after boot, turning Nav2's OWN downstream failure into SOVD # faults (see the "Fault surfacing" comment below) -# - health_check, exposing 4 operator-invoked Trigger operations -# (lidar/localization/drivetrain/costmap) for differential diagnosis; -# independent of scan_sensor_node so it survives the OTA swap +# - health_check, exposing one run_health_checks operation that runs the +# requested lidar/localization/drivetrain/costmap checks for differential +# diagnosis; independent of scan_sensor_node so it survives the OTA swap # # /scan ownership # --------------- @@ -73,6 +73,7 @@ def generate_launch_description(): demo_pkg_dir = get_package_share_directory('ota_nav2_sensor_fix_demo') xacro_file = os.path.join(demo_pkg_dir, 'urdf', 'warehouse_rbtheron.urdf.xacro') + latched_relay_script = os.path.join(demo_pkg_dir, 'scripts', 'latched_relay.py') bridge_config = os.path.join(demo_pkg_dir, 'config', 'ros_gz_bridge.yaml') world_file = os.path.join( demo_pkg_dir, 'models', 'aws_small_warehouse', 'worlds', 'warehouse.sdf' @@ -146,6 +147,23 @@ def generate_launch_description(): }], ) + # /robot_description and /tf_static are published exactly ONCE at startup + # (robot_state_publisher latches them with TRANSIENT_LOCAL QoS). The + # fault_manager's rosbag capture subscribes with a VOLATILE QoS at fault + # time, so it never receives those latched samples and downloaded MCAPs + # were missing the robot URDF + static TF tree - no robot mesh, no frame + # to place /scan against, on Foxglove playback. latched_relay caches the + # latched sample and re-publishes it on a plain VOLATILE topic ~2 Hz, so + # every ~7s capture window is guaranteed to contain a recent sample. + # A plain script, not a Node action, since it's an unregistered + # executable (installed via CMakeLists.txt scripts/ install, not + # ament_python) - ExecuteProcess just runs it with python3 directly. + latched_relay = ExecuteProcess( + name='latched_relay', + output='screen', + cmd=['python3', latched_relay_script, '--ros-args', '-p', ['use_sim_time:=', use_sim_time]], + ) + # Spawn the RB-Theron from /robot_description at the pinned aisle pose. # -z 0.15 lifts it slightly so the base mesh does not clip the warehouse # floor on settle. @@ -301,9 +319,17 @@ def generate_launch_description(): # subscription to transient_local so the latched /tf_static (published # once at startup) is actually recorded; without it the TF tree can't # be rebuilt on replay and /scan will not render in 3D. + # /robot_description is included too so the downloaded MCAP carries + # the URDF itself (Foxglove's 3D panel reads it to render the robot + # mesh) - latched_relay (below) is what actually makes this and + # /tf_static land in every capture window despite the volatile + # capture subscription, since both are otherwise published once, + # latched, at startup, long before any given fault fires. 'snapshots.rosbag.topics': 'explicit', 'snapshots.rosbag.qos_match': True, - 'snapshots.rosbag.include_topics': ['/scan', '/cmd_vel', '/tf', '/tf_static', '/local_costmap/costmap'], + 'snapshots.rosbag.include_topics': [ + '/scan', '/cmd_vel', '/tf', '/tf_static', '/local_costmap/costmap', '/robot_description', + ], 'snapshots.rosbag.storage_path': '/var/lib/ros2_medkit/rosbags', }], ) @@ -340,11 +366,12 @@ def generate_launch_description(): parameters=[{'use_sim_time': use_sim_time}], ) - # Operator-invoked differential-diagnosis node (4 Trigger operations: - # lidar/localization/drivetrain/costmap health checks). Independent of - # scan_sensor_node - it subscribes /scan directly rather than - # depending on the broken_lidar/fixed_lidar swap, so it survives the - # OTA update untouched and answers "broken" before the fix, "healthy" + # Operator-invoked differential-diagnosis node. One run_health_checks + # operation (health_check_msgs/RunHealthChecks) runs the requested + # lidar/localization/drivetrain/costmap checks and returns a combined + # report. Independent of scan_sensor_node - it subscribes /scan directly + # rather than depending on the broken_lidar/fixed_lidar swap, so it survives + # the OTA update untouched and answers "broken" before the fix, "healthy" # after it. health_check_node = Node( package='health_check', @@ -448,6 +475,7 @@ def generate_launch_description(): set_gz_model_path, gz_headless, robot_state_publisher, + latched_relay, spawn_robot, ros_gz_bridge, joint_state_broadcaster_spawner, diff --git a/demos/ota_nav2_sensor_fix/ros2_packages/ota_nav2_sensor_fix_demo/package.xml b/demos/ota_nav2_sensor_fix/ros2_packages/ota_nav2_sensor_fix_demo/package.xml index d70da01..cd312e0 100644 --- a/demos/ota_nav2_sensor_fix/ros2_packages/ota_nav2_sensor_fix_demo/package.xml +++ b/demos/ota_nav2_sensor_fix/ros2_packages/ota_nav2_sensor_fix_demo/package.xml @@ -21,6 +21,12 @@ robotnik_description robotnik_sensors robot_state_publisher + + rclpy + std_msgs + tf2_msgs gz_ros2_control ros2_control ros2_controllers diff --git a/demos/ota_nav2_sensor_fix/ros2_packages/ota_nav2_sensor_fix_demo/scripts/latched_relay.py b/demos/ota_nav2_sensor_fix/ros2_packages/ota_nav2_sensor_fix_demo/scripts/latched_relay.py new file mode 100755 index 0000000..876ca9e --- /dev/null +++ b/demos/ota_nav2_sensor_fix/ros2_packages/ota_nav2_sensor_fix_demo/scripts/latched_relay.py @@ -0,0 +1,99 @@ +#!/usr/bin/env python3 +# Copyright 2026 bburda. Apache-2.0. +# +# latched_relay: keeps /robot_description (std_msgs/String) and /tf_static +# (tf2_msgs/TFMessage) "fresh" so a VOLATILE, best-effort-at-capture-time +# subscriber can actually receive them. +# +# Both topics are published exactly ONCE at startup - robot_state_publisher +# latches them with TRANSIENT_LOCAL durability. The fault_manager's +# ring-buffered rosbag snapshot subscribes to its include_topics with a +# VOLATILE QoS at fault-confirmation time (it has no way to know ahead of +# time which topics/QoS a given fault snapshot will need), so it never sees +# a late-joining latched sample. Every downloaded MCAP was therefore missing +# the robot URDF and the static TF tree (base_footprint -> base_link -> +# laser etc.), so Foxglove playback had no robot mesh and nowhere to place +# /scan relative to the robot. +# +# This node subscribes both topics ONCE with a TRANSIENT_LOCAL QoS (so it +# receives the one latched sample from robot_state_publisher), caches the +# last message of each, and re-publishes them periodically on a separate, +# VOLATILE QoS. Any volatile subscriber - including the fault_manager's +# rosbag capture - is then guaranteed to see a recent sample inside every +# ~7s capture window, not just the single instant at startup. Re-publishing +# the same static transforms / URDF repeatedly is idempotent for both TF2 +# (static transforms have no expiry) and Foxglove. + +import rclpy +from rclpy.node import Node +from rclpy.qos import DurabilityPolicy, QoSProfile, ReliabilityPolicy +from std_msgs.msg import String +from tf2_msgs.msg import TFMessage + +_REPUBLISH_PERIOD_SEC = 0.5 # ~2 Hz + + +class LatchedRelay(Node): + """Relay /robot_description + /tf_static from TRANSIENT_LOCAL (latched, + published once) onto a periodic VOLATILE republish, so a volatile-QoS + capture always sees a recent sample.""" + + def __init__(self) -> None: + super().__init__('latched_relay') + # Default True: this node only exists to keep the sim-time bag + # capture self-contained, so it always follows the sim clock. + self.declare_parameter('use_sim_time', True) + + latched_qos = QoSProfile( + depth=1, + reliability=ReliabilityPolicy.RELIABLE, + durability=DurabilityPolicy.TRANSIENT_LOCAL, + ) + volatile_qos = QoSProfile( + depth=1, + reliability=ReliabilityPolicy.RELIABLE, + durability=DurabilityPolicy.VOLATILE, + ) + + self._robot_description = None + self._tf_static = None + + self._robot_description_pub = self.create_publisher( + String, '/robot_description', volatile_qos) + self._tf_static_pub = self.create_publisher( + TFMessage, '/tf_static', volatile_qos) + + self.create_subscription( + String, '/robot_description', self._on_robot_description, latched_qos) + self.create_subscription( + TFMessage, '/tf_static', self._on_tf_static, latched_qos) + + self.create_timer(_REPUBLISH_PERIOD_SEC, self._republish) + + def _on_robot_description(self, msg: String) -> None: + self._robot_description = msg + + def _on_tf_static(self, msg: TFMessage) -> None: + self._tf_static = msg + + def _republish(self) -> None: + if self._robot_description is not None: + self._robot_description_pub.publish(self._robot_description) + if self._tf_static is not None: + self._tf_static_pub.publish(self._tf_static) + + +def main() -> None: + rclpy.init() + node = LatchedRelay() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() From bfcf830588cab4985266bd982640b408b2ff674a Mon Sep 17 00:00:00 2001 From: Bartosz Burda Date: Sat, 4 Jul 2026 21:45:15 +0200 Subject: [PATCH 08/19] feat(ota_demo): consolidate the 4 health checks into one run_health_checks operation The 4 std_srvs/Trigger checks each showed a meaningless auto-generated 'structure_needs_at_least_one_member' request field and needed 4 separate clicks. Replace them with a single health_check_msgs/RunHealthChecks service: request bools lidar/localization/drivetrain/costmap (default true) select which checks to run, response returns ok / checks_run / checks_failed and a per-check report. One operator operation, meaningful request fields, richer diagnostic output; the per-check logic and message wording are unchanged. --- demos/ota_nav2_sensor_fix/README.md | 23 ++- .../ros2_packages/health_check/CMakeLists.txt | 4 +- .../ros2_packages/health_check/package.xml | 4 +- .../health_check/src/health_check_node.cpp | 159 ++++++++++-------- .../health_check_msgs/CMakeLists.txt | 27 +++ .../health_check_msgs/package.xml | 21 +++ .../health_check_msgs/srv/RunHealthChecks.srv | 28 +++ 7 files changed, 175 insertions(+), 91 deletions(-) create mode 100644 demos/ota_nav2_sensor_fix/ros2_packages/health_check_msgs/CMakeLists.txt create mode 100644 demos/ota_nav2_sensor_fix/ros2_packages/health_check_msgs/package.xml create mode 100644 demos/ota_nav2_sensor_fix/ros2_packages/health_check_msgs/srv/RunHealthChecks.srv diff --git a/demos/ota_nav2_sensor_fix/README.md b/demos/ota_nav2_sensor_fix/README.md index c6065e4..f176bb7 100644 --- a/demos/ota_nav2_sensor_fix/README.md +++ b/demos/ota_nav2_sensor_fix/README.md @@ -40,12 +40,11 @@ The headline scene is a diagnostic loop, not just a button-press update: the replayed `/scan`. The Faults Dashboard panel's freeze-frame shows the same state at confirmation time. To confirm the root cause rather than guess, the operator runs the - `health-check` app's operations (`lidar_health_check`, - `localization_health_check`, `drivetrain_health_check`, - `costmap_health_check`) from the Operations tab: localization and - drivetrain come back healthy, but `lidar_health_check` reports a stuck - sector and `costmap_health_check` flags an obstacle ahead that is not in - the map - it is the lidar, not something downstream. + `health-check` app's single `run_health_checks` operation from the + Operations tab (all four checks default to enabled): localization and + drivetrain come back healthy, but the report shows lidar failed with a + stuck sector and costmap flags an obstacle ahead that is not in the map - + it is the lidar, not something downstream. 7. `GET /api/v1/updates` shows only `broken_lidar_3_0_0` - the suspect recent change, and the fix the operator needs is not registered yet. 8. The operator publishes the hotfix with `./publish-fix.sh` (SOVD @@ -138,13 +137,11 @@ curl -s "${API}/apps/bt-navigator/faults/ACTION_NAVIGATE_TO_POSE_ABORTED" | jq . # 4. Download the MCAP recording and open it in Foxglove. curl -O -J "${API}/apps/bt-navigator/bulk-data/rosbags/ACTION_NAVIGATE_TO_POSE_ABORTED" -# 4b. Confirm the root cause with the health-check operations. localization + -# drivetrain come back healthy; lidar_health_check reports a stuck sector. -for op in lidar_health_check localization_health_check drivetrain_health_check costmap_health_check; do - echo "== ${op} ==" - curl -s -X POST -H 'Content-Type: application/json' -d '{}' \ - "${API}/apps/health-check/operations/${op}/executions" | jq '.parameters // .' -done +# 4b. Confirm the root cause with the single run_health_checks operation +# (all four checks default to enabled). localization + drivetrain come +# back healthy; the report line for lidar shows a stuck sector. +curl -s -X POST -H 'Content-Type: application/json' -d '{}' \ + "${API}/apps/health-check/operations/run_health_checks/executions" | jq '.parameters // .' # 5. Publish the forward hotfix (or use ./publish-fix.sh). It is not in the # boot catalog - you register it by POSTing its descriptor, a JSON you diff --git a/demos/ota_nav2_sensor_fix/ros2_packages/health_check/CMakeLists.txt b/demos/ota_nav2_sensor_fix/ros2_packages/health_check/CMakeLists.txt index fd56ae4..2b73496 100644 --- a/demos/ota_nav2_sensor_fix/ros2_packages/health_check/CMakeLists.txt +++ b/demos/ota_nav2_sensor_fix/ros2_packages/health_check/CMakeLists.txt @@ -14,7 +14,7 @@ find_package(rclcpp REQUIRED) find_package(geometry_msgs REQUIRED) find_package(nav_msgs REQUIRED) find_package(sensor_msgs REQUIRED) -find_package(std_srvs REQUIRED) +find_package(health_check_msgs REQUIRED) add_executable(health_check_node src/health_check_node.cpp) ament_target_dependencies(health_check_node @@ -22,7 +22,7 @@ ament_target_dependencies(health_check_node geometry_msgs nav_msgs sensor_msgs - std_srvs + health_check_msgs ) install(TARGETS health_check_node DESTINATION lib/${PROJECT_NAME}) diff --git a/demos/ota_nav2_sensor_fix/ros2_packages/health_check/package.xml b/demos/ota_nav2_sensor_fix/ros2_packages/health_check/package.xml index e0b426a..3fc58c7 100644 --- a/demos/ota_nav2_sensor_fix/ros2_packages/health_check/package.xml +++ b/demos/ota_nav2_sensor_fix/ros2_packages/health_check/package.xml @@ -2,7 +2,7 @@ health_check 1.0.0 - Operator-invoked SOVD health-check operations (lidar, localization, drivetrain, costmap) for differential diagnosis of the OTA nav2 sensor-fix demo. + Operator-invoked SOVD health-check operation (lidar, localization, drivetrain, costmap) for differential diagnosis of the OTA nav2 sensor-fix demo. bburda Apache-2.0 @@ -12,7 +12,7 @@ geometry_msgs nav_msgs sensor_msgs - std_srvs + health_check_msgs ament_cmake diff --git a/demos/ota_nav2_sensor_fix/ros2_packages/health_check/src/health_check_node.cpp b/demos/ota_nav2_sensor_fix/ros2_packages/health_check/src/health_check_node.cpp index 16e1f38..f99c008 100644 --- a/demos/ota_nav2_sensor_fix/ros2_packages/health_check/src/health_check_node.cpp +++ b/demos/ota_nav2_sensor_fix/ros2_packages/health_check/src/health_check_node.cpp @@ -3,25 +3,32 @@ // Operator-invoked differential-diagnosis node. It does NOT raise faults - // the log/action-status bridges already turn Nav2's own stall into SOVD // faults once broken_lidar's phantom sector is applied. This node answers -// four on-demand checks (std_srvs/Trigger operations) so an operator -// working a "navigate_to_pose aborted" fault can narrow the root cause down -// to a specific subsystem instead of guessing: -// ~/lidar_health_check - is /scan reporting a stuck close sector? -// ~/localization_health_check - is AMCL still localizing? -// ~/drivetrain_health_check - is odometry/joint-state telemetry live? -// ~/costmap_health_check - does the local costmap show a live, -// sensor-only obstacle right in front (no -// matching static-map feature)? +// a single on-demand operation (health_check_msgs/RunHealthChecks) so an +// operator working a "navigate_to_pose aborted" fault can narrow the root +// cause down to a specific subsystem instead of guessing. The request picks +// which of the four checks to run (each bool defaults to true, so a bare +// call runs everything): +// lidar - is /scan reporting a stuck close sector? +// localization - is AMCL still localizing? +// drivetrain - is odometry/joint-state telemetry live? +// costmap - does the local costmap show a live, sensor-only +// obstacle right in front (no matching static-map +// feature)? // -// Each handler only inspects the latest cached message per topic - no +// The response carries one report line per requested check plus an overall +// ok / checks_run / checks_failed summary - see +// health_check_msgs/srv/RunHealthChecks.srv. +// +// Each check only inspects the latest cached message per topic - no // history, no state machine, no interaction with the OTA/fault-manager // flow. Subscribing independently of scan_sensor_node means this node // survives the broken_lidar <-> fixed_lidar swap untouched, so the same -// four operations answer "broken" before the fix and "healthy" after it. +// operation answers "broken" before the fix and "healthy" after it. #include #include #include +#include #include #include #include @@ -36,10 +43,11 @@ #include #include #include -#include + +#include namespace { -// Lidar stuck-sector heuristic (see class comment above handle_lidar_health_check). +// Lidar stuck-sector heuristic (see class comment above check_lidar_health). constexpr float kStuckBandM = 0.05f; constexpr float kStuckMaxRangeM = 1.5f; constexpr size_t kStuckMinRunRays = 30; @@ -53,6 +61,13 @@ constexpr double kCostmapAheadMinM = 0.3; constexpr double kCostmapAheadMaxM = 0.8; constexpr double kCostmapLateralM = 0.3; constexpr int8_t kLethalCellValue = 99; + +// Result of a single check: whether it passed and a human-readable detail +// message, combined into one ": ok|FAIL - " report line. +struct CheckOutcome { + bool ok; + std::string message; +}; } // namespace class HealthCheckNode : public rclcpp::Node { @@ -98,32 +113,11 @@ class HealthCheckNode : public rclcpp::Node { costmap_received_ = true; }); - lidar_health_check_srv_ = create_service( - "~/lidar_health_check", - [this](const std::shared_ptr /*request*/, - std::shared_ptr response) { - handle_lidar_health_check(response); - }); - - localization_health_check_srv_ = create_service( - "~/localization_health_check", - [this](const std::shared_ptr /*request*/, - std::shared_ptr response) { - handle_localization_health_check(response); - }); - - drivetrain_health_check_srv_ = create_service( - "~/drivetrain_health_check", - [this](const std::shared_ptr /*request*/, - std::shared_ptr response) { - handle_drivetrain_health_check(response); - }); - - costmap_health_check_srv_ = create_service( - "~/costmap_health_check", - [this](const std::shared_ptr /*request*/, - std::shared_ptr response) { - handle_costmap_health_check(response); + run_health_checks_srv_ = create_service( + "~/run_health_checks", + [this](const std::shared_ptr request, + std::shared_ptr response) { + handle_run_health_checks(request, response); }); } @@ -143,16 +137,50 @@ class HealthCheckNode : public rclcpp::Node { HealthCheckNode & operator=(HealthCheckNode &&) = delete; private: + // Runs every check whose request flag is true, builds one report line per + // requested check (": ok|FAIL - "), and rolls the results up + // into ok/checks_run/checks_failed. + void handle_run_health_checks( + const std::shared_ptr request, + std::shared_ptr response) const { + std::vector lines; + uint8_t checks_run = 0; + uint8_t checks_failed = 0; + + const auto record = [&](const char * name, const CheckOutcome & outcome) { + ++checks_run; + if (!outcome.ok) ++checks_failed; + std::ostringstream line; + line << name << ": " << (outcome.ok ? "ok" : "FAIL") << " - " << outcome.message; + lines.push_back(line.str()); + }; + + // Only evaluate (and report on) a check when it was actually requested. + if (request->lidar) record("lidar", check_lidar_health()); + if (request->localization) record("localization", check_localization_health()); + if (request->drivetrain) record("drivetrain", check_drivetrain_health()); + if (request->costmap) record("costmap", check_costmap_health()); + + std::ostringstream report; + for (size_t i = 0; i < lines.size(); ++i) { + if (i > 0) report << "\n"; + report << lines[i]; + } + + response->checks_run = checks_run; + response->checks_failed = checks_failed; + response->report = report.str(); + response->ok = (checks_failed == 0); + } + // Scans for the longest run of consecutive FINITE ranges that are all // within kStuckBandM of one another and below kStuckMaxRangeM - a // contiguous band of near-equal close returns is the signature of the // broken_lidar phantom sector (a real environment practically never // presents 30+ consecutive rays all within 5 cm of each other). - void handle_lidar_health_check(std::shared_ptr response) const { + CheckOutcome check_lidar_health() const { if (!scan_received_ || !last_scan_) { - response->success = false; - response->message = "no /scan received"; - return; + return {false, "no /scan received"}; } const auto & ranges = last_scan_->ranges; @@ -209,9 +237,7 @@ class HealthCheckNode : public rclcpp::Node { oss << "Stuck lidar sector: " << best_len << " rays pinned near " << std::fixed << std::setprecision(2) << best_value << " m around bearing " << std::setprecision(1) << bearing_deg << " deg - the lidar is reporting a phantom obstacle."; - response->success = false; - response->message = oss.str(); - return; + return {false, oss.str()}; } std::ostringstream oss; @@ -220,27 +246,23 @@ class HealthCheckNode : public rclcpp::Node { oss << ", ranges " << std::fixed << std::setprecision(2) << finite_min << "-" << finite_max << " m"; } oss << ", no stuck sector."; - response->success = true; - response->message = oss.str(); + return {true, oss.str()}; } - void handle_localization_health_check(std::shared_ptr response) const { + CheckOutcome check_localization_health() const { if (amcl_pose_received_) { const double age_s = (this->now() - last_amcl_pose_time_).seconds(); if (age_s <= kLocalizationFreshSec) { std::ostringstream oss; oss << "Localization healthy: AMCL pose fresh (" << std::fixed << std::setprecision(1) << age_s << "s old), covariance nominal."; - response->success = true; - response->message = oss.str(); - return; + return {true, oss.str()}; } } - response->success = false; - response->message = "AMCL pose stale/absent"; + return {false, "AMCL pose stale/absent"}; } - void handle_drivetrain_health_check(std::shared_ptr response) const { + CheckOutcome check_drivetrain_health() const { const double odom_age = odom_received_ ? (this->now() - last_odom_time_).seconds() : std::numeric_limits::infinity(); const double joint_age = joint_state_received_ ? (this->now() - last_joint_state_time_).seconds() @@ -249,9 +271,7 @@ class HealthCheckNode : public rclcpp::Node { const bool joint_fresh = joint_state_received_ && joint_age <= kDrivetrainFreshSec; if (odom_fresh && joint_fresh) { - response->success = true; - response->message = "Drivetrain healthy: odometry + joint states streaming, diff-drive active."; - return; + return {true, "Drivetrain healthy: odometry + joint states streaming, diff-drive active."}; } std::vector stale; @@ -265,8 +285,7 @@ class HealthCheckNode : public rclcpp::Node { oss << stale[k]; } oss << " stale/absent."; - response->success = false; - response->message = oss.str(); + return {false, oss.str()}; } // Counts lethal cells (>= kLethalCellValue) in a small window directly @@ -275,13 +294,11 @@ class HealthCheckNode : public rclcpp::Node { // the robot, so this window is "in front of the robot" without needing a // TF lookup. A lethal hit here with no corresponding static-map feature // corroborates a live-sensor phantom rather than a real obstacle - this - // check only ever reports success=true, it is an observation, not a fault. - void handle_costmap_health_check(std::shared_ptr response) const { + // check only ever reports ok=true, it is an observation, not a fault. + CheckOutcome check_costmap_health() const { if (!costmap_received_ || !last_costmap_ || last_costmap_->data.empty() || last_costmap_->info.width == 0 || last_costmap_->info.height == 0 || last_costmap_->info.resolution <= 0.0f) { - response->success = false; - response->message = "no /local_costmap/costmap received"; - return; + return {false, "no /local_costmap/costmap received"}; } const auto & info = last_costmap_->info; @@ -323,13 +340,10 @@ class HealthCheckNode : public rclcpp::Node { << ahead_avg_m << " m ahead with no matching static-map feature - consistent with a live sensor (phantom), " "not the environment."; - response->success = true; - response->message = oss.str(); - return; + return {true, oss.str()}; } - response->success = true; - response->message = "Local costmap clear ahead."; + return {true, "Local costmap clear ahead."}; } // Cached latest messages + reception time (node clock, so this follows @@ -360,10 +374,7 @@ class HealthCheckNode : public rclcpp::Node { rclcpp::Subscription::SharedPtr joint_state_sub_; rclcpp::Subscription::SharedPtr costmap_sub_; - rclcpp::Service::SharedPtr lidar_health_check_srv_; - rclcpp::Service::SharedPtr localization_health_check_srv_; - rclcpp::Service::SharedPtr drivetrain_health_check_srv_; - rclcpp::Service::SharedPtr costmap_health_check_srv_; + rclcpp::Service::SharedPtr run_health_checks_srv_; }; int main(int argc, char ** argv) { diff --git a/demos/ota_nav2_sensor_fix/ros2_packages/health_check_msgs/CMakeLists.txt b/demos/ota_nav2_sensor_fix/ros2_packages/health_check_msgs/CMakeLists.txt new file mode 100644 index 0000000..5b81849 --- /dev/null +++ b/demos/ota_nav2_sensor_fix/ros2_packages/health_check_msgs/CMakeLists.txt @@ -0,0 +1,27 @@ +# Copyright 2026 bburda +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +cmake_minimum_required(VERSION 3.16) +project(health_check_msgs) + +# Note: rosidl-generated code triggers strict warnings we cannot fix, so no +# project-wide -Wall/-Wextra/-Wpedantic here. +find_package(ament_cmake REQUIRED) +find_package(rosidl_default_generators REQUIRED) + +rosidl_generate_interfaces(${PROJECT_NAME} + "srv/RunHealthChecks.srv" +) + +ament_package() diff --git a/demos/ota_nav2_sensor_fix/ros2_packages/health_check_msgs/package.xml b/demos/ota_nav2_sensor_fix/ros2_packages/health_check_msgs/package.xml new file mode 100644 index 0000000..4c42508 --- /dev/null +++ b/demos/ota_nav2_sensor_fix/ros2_packages/health_check_msgs/package.xml @@ -0,0 +1,21 @@ + + + + health_check_msgs + 1.0.0 + Service definition for the health_check node's single differential-diagnosis operation (lidar/localization/drivetrain/costmap) in the OTA nav2 sensor-fix demo. + + bburda + Apache-2.0 + + ament_cmake + rosidl_default_generators + + rosidl_default_runtime + + rosidl_interface_packages + + + ament_cmake + + diff --git a/demos/ota_nav2_sensor_fix/ros2_packages/health_check_msgs/srv/RunHealthChecks.srv b/demos/ota_nav2_sensor_fix/ros2_packages/health_check_msgs/srv/RunHealthChecks.srv new file mode 100644 index 0000000..a248c27 --- /dev/null +++ b/demos/ota_nav2_sensor_fix/ros2_packages/health_check_msgs/srv/RunHealthChecks.srv @@ -0,0 +1,28 @@ +# Copyright 2026 bburda +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# +# RunHealthChecks.srv - Single differential-diagnosis operation for the +# health_check node, replacing four separate empty-request Trigger services +# with one call an operator can parameterize. + +# Which checks to run (all default true so a bare call runs everything) +bool lidar true +bool localization true +bool drivetrain true +bool costmap true +--- +bool ok # true iff every REQUESTED check passed +uint8 checks_run +uint8 checks_failed +string report # one human-readable line per requested check From 71926e695b9fe7b6c05d0c035b9b6908129b2dfc Mon Sep 17 00:00:00 2001 From: Bartosz Burda Date: Mon, 6 Jul 2026 08:49:03 +0200 Subject: [PATCH 09/19] fix(ota_demo): stop latched_relay crashing on a re-declared use_sim_time The relay called declare_parameter('use_sim_time') but rclpy auto-declares that parameter and the launch already sets it via -p, so the node raised ParameterAlreadyDeclaredException and died on startup every boot. Rely on the launch-provided value instead of re-declaring. --- .../ota_nav2_sensor_fix_demo/scripts/latched_relay.py | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/demos/ota_nav2_sensor_fix/ros2_packages/ota_nav2_sensor_fix_demo/scripts/latched_relay.py b/demos/ota_nav2_sensor_fix/ros2_packages/ota_nav2_sensor_fix_demo/scripts/latched_relay.py index 876ca9e..eae64f6 100755 --- a/demos/ota_nav2_sensor_fix/ros2_packages/ota_nav2_sensor_fix_demo/scripts/latched_relay.py +++ b/demos/ota_nav2_sensor_fix/ros2_packages/ota_nav2_sensor_fix_demo/scripts/latched_relay.py @@ -40,9 +40,12 @@ class LatchedRelay(Node): def __init__(self) -> None: super().__init__('latched_relay') - # Default True: this node only exists to keep the sim-time bag - # capture self-contained, so it always follows the sim clock. - self.declare_parameter('use_sim_time', True) + # use_sim_time is auto-declared by every rclpy node and set to True via + # the launch's `--ros-args -p use_sim_time:=True`, so this node follows + # the sim clock. Do NOT re-declare it here - declare_parameter on an + # already-declared parameter raises ParameterAlreadyDeclaredException and + # kills the node on startup (which silently left the fault MCAP without + # /robot_description + /tf_static, so playback showed no robot). latched_qos = QoSProfile( depth=1, From 4d9df53864a3efad23ae438fc8f18250d55d1284 Mon Sep 17 00:00:00 2001 From: Bartosz Burda Date: Mon, 6 Jul 2026 09:00:10 +0200 Subject: [PATCH 10/19] fix(ota_demo): support automated apply + capture the robot footprint in the MCAP supports_automated() returned false, so the UI's combined 'Prepare & execute' reported 'package does not support automatic updates' even though the plugin applies updates itself on execute - return true. Also capture /local_costmap/published_footprint so MCAP playback shows the robot's outline and pose; the URDF mesh cannot render on replay (Foxglove resolves package:// meshes via the live foxglove_bridge asset service, absent in a bag), so the footprint + /tf frames are how you see where the robot is in the recording. --- .../ota_update_plugin/src/ota_update_plugin.cpp | 6 +++++- .../ota_nav2_sensor_fix_demo/launch/demo.launch.py | 9 ++++++++- 2 files changed, 13 insertions(+), 2 deletions(-) diff --git a/demos/ota_nav2_sensor_fix/ota_update_plugin/src/ota_update_plugin.cpp b/demos/ota_nav2_sensor_fix/ota_update_plugin/src/ota_update_plugin.cpp index 3cf8179..1fab6c6 100644 --- a/demos/ota_nav2_sensor_fix/ota_update_plugin/src/ota_update_plugin.cpp +++ b/demos/ota_nav2_sensor_fix/ota_update_plugin/src/ota_update_plugin.cpp @@ -334,7 +334,11 @@ tl::expected OtaUpdatePlugin::execute( } tl::expected OtaUpdatePlugin::supports_automated(const std::string & /*id*/) { - return false; + // This plugin applies updates itself on execute (kill + respawn the target + // binary), so every update it serves supports automated application. The UI's + // combined "Prepare & execute" action gates on this; returning false made it + // report "package does not support automatic updates". + return true; } namespace { diff --git a/demos/ota_nav2_sensor_fix/ros2_packages/ota_nav2_sensor_fix_demo/launch/demo.launch.py b/demos/ota_nav2_sensor_fix/ros2_packages/ota_nav2_sensor_fix_demo/launch/demo.launch.py index fbdf359..ddd937b 100644 --- a/demos/ota_nav2_sensor_fix/ros2_packages/ota_nav2_sensor_fix_demo/launch/demo.launch.py +++ b/demos/ota_nav2_sensor_fix/ros2_packages/ota_nav2_sensor_fix_demo/launch/demo.launch.py @@ -327,8 +327,15 @@ def generate_launch_description(): # latched, at startup, long before any given fault fires. 'snapshots.rosbag.topics': 'explicit', 'snapshots.rosbag.qos_match': True, + # /local_costmap/published_footprint (a polygon at the robot pose) + # draws the robot's outline on MCAP playback using only /tf. The URDF + # mesh itself does NOT render on replay - Foxglove resolves the + # robot's package:// meshes through the live foxglove_bridge asset + # service, which a bag replay has no equivalent for, so the mesh is a + # live-only view. The footprint + /tf frames show WHERE the robot is. 'snapshots.rosbag.include_topics': [ - '/scan', '/cmd_vel', '/tf', '/tf_static', '/local_costmap/costmap', '/robot_description', + '/scan', '/cmd_vel', '/tf', '/tf_static', '/local_costmap/costmap', + '/robot_description', '/local_costmap/published_footprint', ], 'snapshots.rosbag.storage_path': '/var/lib/ros2_medkit/rosbags', }], From 8117b42ba738de7dbc8a16720770f417b956c34c Mon Sep 17 00:00:00 2001 From: Bartosz Burda Date: Mon, 6 Jul 2026 10:35:53 +0200 Subject: [PATCH 11/19] fix(ota_demo): localization health check judges AMCL covariance, not pose freshness AMCL only republishes /amcl_pose on a motion-triggered filter update, so once the robot reaches its goal and sits still the pose goes stale even though AMCL is still localized (it keeps broadcasting map->odom). The freshness check then reported 'AMCL pose stale/absent' after the OTA fix, collapsing the whole post-fix 'all healthy' result on a false positive. Judge the cached pose's xy covariance instead - small = converged, regardless of age; a real divergence still trips the bound. --- .../health_check/src/health_check_node.cpp | 22 ++++++++++++++----- 1 file changed, 16 insertions(+), 6 deletions(-) diff --git a/demos/ota_nav2_sensor_fix/ros2_packages/health_check/src/health_check_node.cpp b/demos/ota_nav2_sensor_fix/ros2_packages/health_check/src/health_check_node.cpp index f99c008..1cbaa74 100644 --- a/demos/ota_nav2_sensor_fix/ros2_packages/health_check/src/health_check_node.cpp +++ b/demos/ota_nav2_sensor_fix/ros2_packages/health_check/src/health_check_node.cpp @@ -53,7 +53,7 @@ constexpr float kStuckMaxRangeM = 1.5f; constexpr size_t kStuckMinRunRays = 30; // Freshness windows. -constexpr double kLocalizationFreshSec = 10.0; +constexpr double kLocalizationMaxVar = 1.0; // AMCL xy covariance (m^2); above this = diverged constexpr double kDrivetrainFreshSec = 5.0; // Costmap "directly ahead" window, robot/grid-frame relative. @@ -250,16 +250,26 @@ class HealthCheckNode : public rclcpp::Node { } CheckOutcome check_localization_health() const { - if (amcl_pose_received_) { + // AMCL only republishes /amcl_pose on a motion-triggered filter update, so a + // stationary robot that is already localized lets it go stale - that is NOT a + // localization loss (AMCL keeps broadcasting map->odom the whole time). Judge + // on the last pose's covariance (small = converged), not its age, so a robot + // sitting still after the OTA fix still reports healthy; a genuine divergence + // still trips the covariance bound. + if (amcl_pose_received_ && last_amcl_pose_) { + const auto & cov = last_amcl_pose_->pose.covariance; + const double xy_var = cov[0] + cov[7]; // var(x) + var(y), m^2 const double age_s = (this->now() - last_amcl_pose_time_).seconds(); - if (age_s <= kLocalizationFreshSec) { + if (xy_var <= kLocalizationMaxVar) { std::ostringstream oss; - oss << "Localization healthy: AMCL pose fresh (" << std::fixed << std::setprecision(1) << age_s - << "s old), covariance nominal."; + oss << "Localization healthy: AMCL converged (xy variance " << std::fixed + << std::setprecision(3) << xy_var << " m^2, last update " + << std::setprecision(1) << age_s << "s ago)."; return {true, oss.str()}; } + return {false, "AMCL localization diverged (high covariance)"}; } - return {false, "AMCL pose stale/absent"}; + return {false, "AMCL pose absent - never localized"}; } CheckOutcome check_drivetrain_health() const { From c5d808b794b2d3065042018b9f93a53482c612d5 Mon Sep 17 00:00:00 2001 From: Bartosz Burda Date: Mon, 6 Jul 2026 10:52:42 +0200 Subject: [PATCH 12/19] perf(ota_demo): build only the packages the demo needs, not the whole medkit workspace The gateway image ran a bare 'colcon build' over the full 15-package ros2_medkit clone - including ros2_medkit_opcua (open62541pp, very slow to compile), integration_tests, and the discovery beacons the demo never loads (it only loads ota_update_plugin). Restrict the build to --packages-up-to the four medkit packages the demo actually runs (gateway, fault_manager, log/action-status bridges) plus the demo's own packages, so their deps come in transitively and everything else is skipped. --- demos/ota_nav2_sensor_fix/Dockerfile.gateway | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/demos/ota_nav2_sensor_fix/Dockerfile.gateway b/demos/ota_nav2_sensor_fix/Dockerfile.gateway index 2a3f398..cc3c5b5 100644 --- a/demos/ota_nav2_sensor_fix/Dockerfile.gateway +++ b/demos/ota_nav2_sensor_fix/Dockerfile.gateway @@ -129,6 +129,11 @@ RUN . /opt/ros/jazzy/setup.sh && \ --skip-keys ur_description \ --skip-keys ur_simulation_gz && \ colcon build \ + --packages-up-to \ + ros2_medkit_gateway ros2_medkit_fault_manager \ + ros2_medkit_log_bridge ros2_medkit_action_status_bridge \ + ota_update_plugin broken_lidar fixed_lidar health_check \ + ota_nav2_sensor_fix_demo \ --cmake-args -DCMAKE_BUILD_TYPE=Release -DBUILD_TESTING=OFF && \ rm -rf /var/lib/apt/lists/* From d48fb87606ab68c4fa3cb2921ddd308f02d01e40 Mon Sep 17 00:00:00 2001 From: Bartosz Burda Date: Tue, 7 Jul 2026 08:32:32 +0200 Subject: [PATCH 13/19] fix(tests/ota): wait for the boot auto-apply before the process check smoke_test_ota.sh asserted broken_lidar_node was the live scan_sensor_node with a one-shot pgrep, but the entrypoint applies broken_lidar_3_0_0 asynchronously after the catalog registers, so the check raced the async apply and flaked. Gate on broken_lidar_3_0_0 reaching 'completed' before the "Initial process state" section, and poll the process assertions instead of checking once (matches the boot gate smoke_test_demo_narrative.sh already uses). --- tests/smoke_test_ota.sh | 56 +++++++++++++++++++++++++++++++---------- 1 file changed, 43 insertions(+), 13 deletions(-) diff --git a/tests/smoke_test_ota.sh b/tests/smoke_test_ota.sh index 5c5564a..85c5fce 100755 --- a/tests/smoke_test_ota.sh +++ b/tests/smoke_test_ota.sh @@ -29,27 +29,42 @@ GATEWAY_CONTAINER="${OTA_DEMO_GATEWAY_CONTAINER:-ota_demo_gateway}" BOOT_ID="broken_lidar_3_0_0" FIX_ID="fixed_lidar_3_0_1" -# Confirm a process is or is not running inside the gateway container. -# Usage: assert_process_running -# assert_process_gone +# Confirm a process is or is not running inside the gateway container. Both +# POLL (pgrep -f retry loop) rather than checking once: the boot auto-apply and +# the fix apply swap scan_sensor_node's executable asynchronously, so a one-shot +# check races the spawn/kill. Usage: +# assert_process_running [timeout_s] +# assert_process_gone [timeout_s] assert_process_running() { local pattern="$1" local desc="$2" - if docker exec "$GATEWAY_CONTAINER" pgrep -f "$pattern" >/dev/null 2>&1; then - pass "$desc" - else - fail "$desc" "no process matching '$pattern' in $GATEWAY_CONTAINER" - fi + local timeout="${3:-25}" + local elapsed=0 + while [ "$elapsed" -lt "$timeout" ]; do + if docker exec "$GATEWAY_CONTAINER" pgrep -f "$pattern" >/dev/null 2>&1; then + pass "$desc" + return + fi + sleep 2 + elapsed=$((elapsed + 2)) + done + fail "$desc" "no process matching '$pattern' in $GATEWAY_CONTAINER after ${timeout}s" } assert_process_gone() { local pattern="$1" local desc="$2" - if ! docker exec "$GATEWAY_CONTAINER" pgrep -f "$pattern" >/dev/null 2>&1; then - pass "$desc" - else - fail "$desc" "process matching '$pattern' still alive in $GATEWAY_CONTAINER" - fi + local timeout="${3:-25}" + local elapsed=0 + while [ "$elapsed" -lt "$timeout" ]; do + if ! docker exec "$GATEWAY_CONTAINER" pgrep -f "$pattern" >/dev/null 2>&1; then + pass "$desc" + return + fi + sleep 2 + elapsed=$((elapsed + 2)) + done + fail "$desc" "process matching '$pattern' still alive in $GATEWAY_CONTAINER after ${timeout}s" } # --- Wait for gateway startup --- @@ -65,6 +80,21 @@ else exit 1 fi +# The entrypoint auto-applies broken_lidar_3_0_0 asynchronously (prepare + +# execute) AFTER the catalog registers. Catalog registration only proves the +# entry is listed, NOT that the regression was applied - so wait for the boot +# update to actually reach 'completed' before the "Initial process state" check +# below asserts broken_lidar_node is the live process. Without this gate that +# check is a one-shot race against the async apply (this mirrors the boot gate +# smoke_test_demo_narrative.sh already has). +echo " Waiting for ${BOOT_ID} boot auto-apply to reach 'completed' (max 90s)..." +if poll_until "/updates/${BOOT_ID}/status" '.status == "completed"' 90; then + echo " Boot regression applied" +else + echo " Boot regression NOT applied within 90s" + exit 1 +fi + # --- Tests --- section "Health" From fbd755bc9c1bba9d0b711c439082e003f3b9b1e6 Mon Sep 17 00:00:00 2001 From: Bartosz Burda Date: Tue, 7 Jul 2026 08:32:32 +0200 Subject: [PATCH 14/19] ci(ota): dump full logs and ROS state on failure instead of a log tail The gateway container multiplexes ~15 nodes into one stdout, so 'docker compose logs gateway --tail=N' on failure showed only discovery chatter and never the nav2 goal handling. Add tests/dump_ota_diagnostics.sh and run it from both OTA jobs on failure: it captures the full compose log plus ROS introspection (lifecycle, /navigate_to_pose server, /scan, /amcl_pose, TF, controllers, faults) and uploads it as an artifact. --- .github/workflows/ci.yml | 25 +++++++++++------ tests/dump_ota_diagnostics.sh | 52 +++++++++++++++++++++++++++++++++++ 2 files changed, 68 insertions(+), 9 deletions(-) create mode 100755 tests/dump_ota_diagnostics.sh diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 4146321..03a9b73 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -191,15 +191,16 @@ jobs: - name: Run smoke tests run: ./tests/smoke_test_ota.sh - - name: Show gateway logs on failure + - name: Diagnostics on failure if: failure() - working-directory: demos/ota_nav2_sensor_fix - run: docker compose logs gateway --tail=200 + run: ./tests/dump_ota_diagnostics.sh ota_diagnostics.log - - name: Show update server logs on failure + - name: Upload diagnostics if: failure() - working-directory: demos/ota_nav2_sensor_fix - run: docker compose logs ota_update_server --tail=200 + uses: actions/upload-artifact@v4 + with: + name: ota-diagnostics-smoke + path: ota_diagnostics.log - name: Teardown if: always() @@ -252,10 +253,16 @@ jobs: - name: Run demo narrative smoke run: ./tests/smoke_test_demo_narrative.sh - - name: Show gateway logs on failure + - name: Diagnostics on failure if: failure() - working-directory: demos/ota_nav2_sensor_fix - run: docker compose logs gateway --tail=300 + run: ./tests/dump_ota_diagnostics.sh ota_diagnostics.log + + - name: Upload diagnostics + if: failure() + uses: actions/upload-artifact@v4 + with: + name: ota-diagnostics-narrative + path: ota_diagnostics.log - name: Teardown if: always() diff --git a/tests/dump_ota_diagnostics.sh b/tests/dump_ota_diagnostics.sh new file mode 100755 index 0000000..927f012 --- /dev/null +++ b/tests/dump_ota_diagnostics.sh @@ -0,0 +1,52 @@ +#!/bin/bash +# On-failure diagnostics for the ota_nav2_sensor_fix CI jobs. +# +# The gateway container multiplexes ~15 nodes (headless gz + the full Nav2 stack +# + the gateway + the log/action bridges) into a single stdout, so +# `docker compose logs gateway --tail=N` shows only the last few seconds of +# discovery chatter and never the nav2 goal handling that actually explains a +# failure. This dumps the FULL compose log plus live ROS introspection +# (lifecycle states, the /navigate_to_pose action server, /scan, /amcl_pose, +# the map->odom TF, controllers, processes, faults) so a red run is debuggable. +# +# Best-effort: every probe is bounded and this script never fails the CI step. +# Usage: ./tests/dump_ota_diagnostics.sh [output_log_path] +set +e + +DEMO_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")/../demos/ota_nav2_sensor_fix" && pwd)" +C="${OTA_DEMO_GATEWAY_CONTAINER:-ota_demo_gateway}" +OUT="${1:-ota_diagnostics.log}" + +# Run a command inside the gateway container with the ROS overlay sourced +# (docker exec does not run the image entrypoint that sources it). +RUN() { docker exec "$C" bash -lc "source /opt/ros/jazzy/setup.bash && source /ws/install/setup.bash && $*" 2>&1; } + +{ + echo "===== full compose logs =====" + ( cd "$DEMO_DIR" && docker compose logs --no-color --timestamps ) + echo + echo "===== ros2 node list =====" + RUN "timeout 15 ros2 node list | sort" + echo "===== nav2 lifecycle (is the stack active?) =====" + for n in map_server amcl planner_server controller_server bt_navigator; do + printf '%s: ' "$n" + RUN "timeout 8 ros2 lifecycle get /$n" + done + echo "===== /navigate_to_pose action server (did the goal have a server?) =====" + RUN "timeout 12 ros2 action info /navigate_to_pose -t" + echo "===== /amcl_pose (is the robot localized?) =====" + RUN "timeout 8 ros2 topic echo --once /amcl_pose" + echo "===== /scan header (is the lidar publishing?) =====" + RUN "timeout 8 ros2 topic echo --once --field header /scan" + echo "===== map->odom TF (localization connected?) =====" + RUN "timeout 8 ros2 run tf2_ros tf2_echo map odom" + echo "===== controllers =====" + RUN "timeout 10 ros2 control list_controllers" + echo "===== live processes =====" + docker exec "$C" ps -eo pid,etimes,args 2>&1 | grep -iE "gz|nav2|controller|amcl|bt_navigator|lidar_node|bridge" | grep -v grep + echo "===== faults (bt-navigator / controller-server) =====" + docker exec "$C" curl -s localhost:8080/api/v1/apps/bt-navigator/faults 2>&1 + echo + docker exec "$C" curl -s localhost:8080/api/v1/apps/controller-server/faults 2>&1 + echo +} | tee "$OUT" From 271a421937fbeebe2adfd91b572df5b5a2b5fa90 Mon Sep 17 00:00:00 2001 From: Bartosz Burda Date: Tue, 7 Jul 2026 09:39:01 +0200 Subject: [PATCH 15/19] fix(ota_demo): send the nav goal via the action, not a one-shot topic publish send-goal.sh published /goal_pose once with 'ros2 topic pub --once' from a transient docker exec node. On a cold CI runner that single volatile publish raced DDS discovery and was dropped before bt_navigator matched the subscription, so nav2 never drove, never aborted, and the reactive-fault scenario raised nothing (ota-demo-narrative timed out waiting for ACTION_NAVIGATE_TO_POSE_ABORTED). Send through the /navigate_to_pose action instead: the client waits for the server and confirms the goal is accepted, guaranteeing delivery, and returns on acceptance so a healthy-resume goal does not block on the drive. Update the README/run-demo help to match. --- demos/ota_nav2_sensor_fix/README.md | 11 +++--- demos/ota_nav2_sensor_fix/run-demo.sh | 2 +- demos/ota_nav2_sensor_fix/send-goal.sh | 54 +++++++++++++++++++++++--- 3 files changed, 56 insertions(+), 11 deletions(-) diff --git a/demos/ota_nav2_sensor_fix/README.md b/demos/ota_nav2_sensor_fix/README.md index f176bb7..aed52e7 100644 --- a/demos/ota_nav2_sensor_fix/README.md +++ b/demos/ota_nav2_sensor_fix/README.md @@ -82,7 +82,7 @@ In another terminal, drive the demo: ```bash ./check-demo.sh # at a glance: scan node, applied updates, faults -./send-goal.sh # publish a nav goal (mission start / resume) +./send-goal.sh # send a nav goal (mission start / resume) ./publish-fix.sh # register fixed_lidar_3_0_1 (SOVD POST /updates) - not in the boot catalog ./apply-fix.sh # broken_lidar -> fixed_lidar_3_0_1 (prepare + execute the published fix) ./clear-fault.sh # operator clear of the latched bt-navigator/controller-server faults @@ -99,8 +99,9 @@ registered first and tells you to run `./publish-fix.sh` if it isn't. `DELETE /apps/bt-navigator/faults/ACTION_NAVIGATE_TO_POSE_ABORTED` plus a clear-all `DELETE /apps/controller-server/faults` (the `LOG_*` code there is content-hashed, so it is cleared by entity rather than by exact code). -`send-goal.sh` publishes `/goal_pose` once via `ros2 topic pub` inside the -gateway container. +`send-goal.sh` sends the goal through the `/navigate_to_pose` action inside the +gateway container (an action client that waits for the server and confirms the +goal is accepted, so a transient publisher never drops it before nav2 sees it). Port overrides (set as env vars before `./run-demo.sh`): @@ -221,8 +222,8 @@ deterministic for CI. Use `./send-goal.sh` to drive the loop yourself: ``` `send-goal.sh` `docker exec`s into the gateway container (sourcing the ROS -overlay itself, since `docker exec` skips the image entrypoint) and -publishes `/goal_pose` once via `ros2 topic pub`. Foxglove's **3D** panel +overlay itself, since `docker exec` skips the image entrypoint) and sends the +goal through the `/navigate_to_pose` action. Foxglove's **3D** panel also has a built-in "Publish" tool - select pose mode, click a point ahead of the robot, and Foxglove publishes `/goal_pose` for you. diff --git a/demos/ota_nav2_sensor_fix/run-demo.sh b/demos/ota_nav2_sensor_fix/run-demo.sh index 6d8c5f2..7d13f8b 100755 --- a/demos/ota_nav2_sensor_fix/run-demo.sh +++ b/demos/ota_nav2_sensor_fix/run-demo.sh @@ -193,7 +193,7 @@ echo " ./publish-fix.sh # register fixed_lidar_3_0_1 (SOVD POST /updat echo " ./apply-fix.sh # apply the published fix: broken_lidar -> fixed_lidar_3_0_1" echo " ./trigger-bad-update.sh # re-arm broken_lidar (root cause) for a rerun" echo " ./clear-fault.sh # operator clear of the latched bt-navigator/controller-server faults" -echo " ./send-goal.sh # publish a nav goal (mission start / resume)" +echo " ./send-goal.sh # send a nav goal (mission start / resume)" echo " ./stop-demo.sh # tear down" echo "" echo "Connect a UI:" diff --git a/demos/ota_nav2_sensor_fix/send-goal.sh b/demos/ota_nav2_sensor_fix/send-goal.sh index 6dd015e..8c878bd 100755 --- a/demos/ota_nav2_sensor_fix/send-goal.sh +++ b/demos/ota_nav2_sensor_fix/send-goal.sh @@ -1,11 +1,55 @@ #!/bin/bash -# Publish a Nav2 goal into the demo container (ROS_DOMAIN_ID=42). Used to -# start the mission (robot drives into the phantom) and to resume after fix. +# Send a Nav2 goal into the demo container (ROS_DOMAIN_ID=42) via the +# NavigateToPose action. Starts the mission (robot drives into the phantom +# sector) and resumes it after the fix. +# +# This goes through the /navigate_to_pose action instead of a one-shot +# `ros2 topic pub --once /goal_pose`: a single volatile publish from a transient +# `docker exec` node races DDS discovery and, on a cold CI runner, is dropped +# before bt_navigator's /goal_pose subscription is matched - so the goal never +# arrives, nav2 never drives, and no fault is ever raised. An action client +# waits for the server and confirms the goal is accepted, guaranteeing delivery. +# It returns as soon as the goal is ACCEPTED (it does not block on the drive +# result), so a healthy-resume goal does not stall the caller. set -eu X="${1:-1.5}"; Y="${2:-1.0}" -GOAL="{header: {frame_id: map}, pose: {position: {x: ${X}, y: ${Y}, z: 0.0}, orientation: {w: 1.0}}}" + # ros2 is not on the container's default PATH and `docker exec` does not run the # image entrypoint that sources it, so source the ROS overlay inside the exec. -docker exec ota_demo_gateway bash -c \ - "source /opt/ros/jazzy/setup.bash && source /ws/install/setup.bash && ros2 topic pub --once /goal_pose geometry_msgs/PoseStamped '${GOAL}'" +# Goal coordinates are passed as env vars (not string-interpolated into the +# heredoc) so the Python body stays a fixed, quote-safe literal. +docker exec -e GOAL_X="$X" -e GOAL_Y="$Y" -i ota_demo_gateway bash -lc \ + 'source /opt/ros/jazzy/setup.bash && source /ws/install/setup.bash && python3 -' <<'PYEOF' +import os +import sys + +import rclpy +from rclpy.action import ActionClient +from nav2_msgs.action import NavigateToPose + +x = float(os.environ["GOAL_X"]) +y = float(os.environ["GOAL_Y"]) + +rclpy.init() +node = rclpy.create_node("send_goal_cli") +client = ActionClient(node, NavigateToPose, "/navigate_to_pose") +if not client.wait_for_server(timeout_sec=30.0): + print("send-goal: /navigate_to_pose action server not available within 30s", file=sys.stderr) + sys.exit(1) + +goal = NavigateToPose.Goal() +goal.pose.header.frame_id = "map" +goal.pose.pose.position.x = x +goal.pose.pose.position.y = y +goal.pose.pose.orientation.w = 1.0 + +send_future = client.send_goal_async(goal) +rclpy.spin_until_future_complete(node, send_future) +handle = send_future.result() +if handle is None or not handle.accepted: + print("send-goal: goal was rejected by /navigate_to_pose", file=sys.stderr) + sys.exit(1) + +rclpy.shutdown() +PYEOF echo "Goal sent: (${X}, ${Y})" From 621ea218f26c3faa9ac4231da3845f625d19d17c Mon Sep 17 00:00:00 2001 From: Bartosz Burda Date: Tue, 7 Jul 2026 09:57:05 +0200 Subject: [PATCH 16/19] fix(ota_demo): retry the nav goal until nav2 is ready to accept it Sending via the action exposed a second cold-start race: bt_navigator is lifecycle-active and its action server is discoverable before it can actually accept a goal (amcl has not published map->odom yet, so it has no pose to plan from), so it rejects the goal. A goal fired right after boot was rejected once and dropped. Retry send_goal_async until the goal is accepted (up to 90s), so the reactive-fault step reliably starts the mission on a cold CI runner. --- demos/ota_nav2_sensor_fix/send-goal.sh | 26 +++++++++++++++++++++----- 1 file changed, 21 insertions(+), 5 deletions(-) diff --git a/demos/ota_nav2_sensor_fix/send-goal.sh b/demos/ota_nav2_sensor_fix/send-goal.sh index 8c878bd..71df8bf 100755 --- a/demos/ota_nav2_sensor_fix/send-goal.sh +++ b/demos/ota_nav2_sensor_fix/send-goal.sh @@ -22,6 +22,7 @@ docker exec -e GOAL_X="$X" -e GOAL_Y="$Y" -i ota_demo_gateway bash -lc \ 'source /opt/ros/jazzy/setup.bash && source /ws/install/setup.bash && python3 -' <<'PYEOF' import os import sys +import time import rclpy from rclpy.action import ActionClient @@ -43,11 +44,26 @@ goal.pose.pose.position.x = x goal.pose.pose.position.y = y goal.pose.pose.orientation.w = 1.0 -send_future = client.send_goal_async(goal) -rclpy.spin_until_future_complete(node, send_future) -handle = send_future.result() -if handle is None or not handle.accepted: - print("send-goal: goal was rejected by /navigate_to_pose", file=sys.stderr) +# bt_navigator can be lifecycle-active yet still reject a goal for a short window +# after startup: amcl has not produced map->odom yet, or the global costmap is +# not populated, so it has no robot pose to plan from. Retry until it accepts, +# so a goal fired right after boot (as the smoke test does) is not lost. +deadline = time.monotonic() + 90.0 +accepted = False +attempt = 0 +while time.monotonic() < deadline: + attempt += 1 + send_future = client.send_goal_async(goal) + rclpy.spin_until_future_complete(node, send_future, timeout_sec=10.0) + handle = send_future.result() + if handle is not None and handle.accepted: + accepted = True + break + print(f"send-goal: goal not accepted yet (attempt {attempt}); nav2 not ready, retrying...", file=sys.stderr) + time.sleep(3.0) + +if not accepted: + print("send-goal: /navigate_to_pose kept rejecting the goal (nav2 never became ready)", file=sys.stderr) sys.exit(1) rclpy.shutdown() From 8959a5792d68872cb62c476c5a8cdc40b18127d1 Mon Sep 17 00:00:00 2001 From: Bartosz Burda Date: Tue, 7 Jul 2026 11:59:21 +0200 Subject: [PATCH 17/19] fix(ota_demo): version default, pgrep doc, no-new-privileges (review) - pack_artifact.py: --version defaults to '' so an omitted version fails loud for update/install instead of silently packing a 0.0.0 artifact. - process_runner: correct the pgrep doc to cmdline argv0 (not comm, which the kernel truncates to 15 chars). - docker-compose: block privilege escalation on the gateway (no-new-privileges); cap_drop / non-root USER are documented as production hardening, not applied here (they risk breaking gz-sim / DDS in this all-in-one demo container). --- demos/ota_nav2_sensor_fix/docker-compose.yml | 7 +++++++ .../ota_update_plugin/src/process_runner.hpp | 4 +++- .../scripts/pack_artifact.py | 4 ++-- .../scripts/test_pack_artifact.py | 18 ++++++++++++++++++ 4 files changed, 30 insertions(+), 3 deletions(-) diff --git a/demos/ota_nav2_sensor_fix/docker-compose.yml b/demos/ota_nav2_sensor_fix/docker-compose.yml index a3d3aa2..4cb65cc 100644 --- a/demos/ota_nav2_sensor_fix/docker-compose.yml +++ b/demos/ota_nav2_sensor_fix/docker-compose.yml @@ -24,6 +24,13 @@ services: # Gazebo / DDS appreciate generous shared memory; without this # /dev/shm fills and gz-sim tends to wedge on shutdown. shm_size: "2gb" + # OTA execute() runs kill/extract/spawn inside this container. This is a + # trusted-local demo (see README "Security model & demo scope"), but block + # privilege escalation as a free, no-UX-cost hardening. cap_drop:[ALL] and a + # non-root USER are documented as production hardening rather than applied + # here - they risk breaking gz-sim / DDS in this all-in-one demo container. + security_opt: + - no-new-privileges:true tty: true stdin_open: true depends_on: diff --git a/demos/ota_nav2_sensor_fix/ota_update_plugin/src/process_runner.hpp b/demos/ota_nav2_sensor_fix/ota_update_plugin/src/process_runner.hpp index 70e88cb..d145f06 100644 --- a/demos/ota_nav2_sensor_fix/ota_update_plugin/src/process_runner.hpp +++ b/demos/ota_nav2_sensor_fix/ota_update_plugin/src/process_runner.hpp @@ -33,7 +33,9 @@ class ProcessRunner { ProcessRunner(ProcessRunner &&) = delete; ProcessRunner & operator=(ProcessRunner &&) = delete; - /// Find PIDs of processes whose /proc//comm matches the given basename. + /// Find PIDs of processes whose /proc//cmdline argv[0] basename matches + /// the given basename. (cmdline, not comm: the kernel truncates comm to 15 + /// chars, which would clip e.g. "broken_lidar_node" to "broken_lidar_no".) virtual std::vector pgrep(const std::string & executable_basename); /// Send SIGTERM to all matching PIDs, wait up to `timeout_ms` for exit, then diff --git a/demos/ota_nav2_sensor_fix/scripts/pack_artifact.py b/demos/ota_nav2_sensor_fix/scripts/pack_artifact.py index fabe8e4..db6a724 100644 --- a/demos/ota_nav2_sensor_fix/scripts/pack_artifact.py +++ b/demos/ota_nav2_sensor_fix/scripts/pack_artifact.py @@ -20,8 +20,8 @@ def build_parser() -> argparse.ArgumentParser: parser.add_argument("--package", required=True, help="ROS 2 package name to pack.") parser.add_argument( "--version", - default="0.0.0", - help="Semantic version of the artifact (pass '' for uninstall).", + default="", + help="Semantic version of the artifact (required for update/install; omit for uninstall).", ) parser.add_argument( "--kind", diff --git a/demos/ota_nav2_sensor_fix/scripts/test_pack_artifact.py b/demos/ota_nav2_sensor_fix/scripts/test_pack_artifact.py index f37d05b..2287de5 100644 --- a/demos/ota_nav2_sensor_fix/scripts/test_pack_artifact.py +++ b/demos/ota_nav2_sensor_fix/scripts/test_pack_artifact.py @@ -45,6 +45,24 @@ def fake_run(**kwargs): assert captured["skip_build"] is True +def test_main_update_without_version_fails_loud(tmp_path): + # --version defaults to '' (not '0.0.0'), so an omitted version for an + # update fails loudly in run() instead of silently packing a 0.0.0 artifact. + with pytest.raises(SystemExit): + pack_artifact.main( + [ + "--package", "fixed_lidar", + "--kind", "update", + "--target-component", "scan_sensor_node", + "--executable", "fixed_lidar_node", + "--skip-build", + "--out-dir", str(tmp_path / "artifacts"), + "--catalog", str(tmp_path / "artifacts" / "catalog.json"), + "--workspace", str(tmp_path / "ws"), + ] + ) + + def test_build_entry_update_kind(): entry = pack_artifact.build_entry( package="fixed_lidar", From 1001546e0a4fd605367eeb78a3f30067e2cc573d Mon Sep 17 00:00:00 2001 From: Bartosz Burda Date: Tue, 7 Jul 2026 13:29:24 +0200 Subject: [PATCH 18/19] fix(ota_demo): harden the OTA update plugin (validate, no shell, stage-then-swap) - Reject catalog ids/components/executables/target packages that are not [A-Za-z0-9._-] before they reach a path or exec (blocks '../' traversal and argument injection). - Extract the artifact with fork+execvp instead of std::system, so a catalog-controlled name can never be interpreted by a shell. - Stage-then-swap: extract and verify the new tree before killing the running node, so a bad tarball no longer leaves the sensor with no lidar. - Track processes the plugin spawned (keyed by component) and also kill the recorded pid on re-apply, guarded by a /proc//cmdline argv0 check against pid reuse - fixes the duplicate scan_sensor_node on a repeated apply. - Uninstall kills the recorded executable (not the package name, which never matched argv0) and removes the install fragment keyed on the component, so an uninstalled app no longer lingers in the entity tree. - Emit install-fragment scalars as quoted YAML. - Rewrite the process-runner test stub into a stateful fake covering the swap, duplicate-spawn, uninstall, traversal-rejection and quoting paths. - README: document the demo's trusted-local security model and what a production deployment would add; correct the swap note. --- demos/ota_nav2_sensor_fix/README.md | 19 +- .../ota_update_plugin/ota_update_plugin.hpp | 35 +- .../src/ota_update_plugin.cpp | 316 +++++++++++--- .../ota_update_plugin/src/process_runner.cpp | 29 ++ .../ota_update_plugin/src/process_runner.hpp | 8 + .../test/test_plugin_smoke.cpp | 388 ++++++++++++++++-- 6 files changed, 703 insertions(+), 92 deletions(-) diff --git a/demos/ota_nav2_sensor_fix/README.md b/demos/ota_nav2_sensor_fix/README.md index aed52e7..e6480c8 100644 --- a/demos/ota_nav2_sensor_fix/README.md +++ b/demos/ota_nav2_sensor_fix/README.md @@ -241,13 +241,30 @@ reaches it with a clean `/scan`. This is **dev-grade** OTA. Deliberately missing for production: - No artifact signing or signature verification -- No atomic swap (in-place overwrite) +- Staged swap: the new build is extracted and verified into a staging dir + before the live install is replaced and the old process is killed, but + no rollback-respawn if the new process fails to spawn - No A/B partition rollout - No fleet-wide staged rollout - No persistent update state across gateway restarts - No automated health-gated rollback policy - No audit log +**Security model & demo scope.** This is a self-contained docker-compose +demo on a trusted local network, driven with `curl` against a catalog +authored locally by `scripts/build_artifacts.sh`. The update API is +unauthenticated by design (bound to `0.0.0.0:8080`, origins `[*]`, updates +enabled) so the demo stays curl-drivable. A production deployment would add: + +- An authenticated, TLS-terminated update API in place of the open, + plaintext one +- A signed catalog and a per-artifact `sha256` verified before extraction, + with artifacts fetched only from the configured origin (not an arbitrary + URL taken from the catalog) +- The gateway running as a non-root user with dropped capabilities + (`cap_drop: [ALL]`); `no-new-privileges` is already set in + `docker-compose.yml` + Perfect for: prototypes, lab robots, internal demos, dev environments. For production-grade OTA (rollout safety, signing, A/B partitions, diff --git a/demos/ota_nav2_sensor_fix/ota_update_plugin/include/ota_update_plugin/ota_update_plugin.hpp b/demos/ota_nav2_sensor_fix/ota_update_plugin/include/ota_update_plugin/ota_update_plugin.hpp index 55952bd..145da10 100644 --- a/demos/ota_nav2_sensor_fix/ota_update_plugin/include/ota_update_plugin/ota_update_plugin.hpp +++ b/demos/ota_nav2_sensor_fix/ota_update_plugin/include/ota_update_plugin/ota_update_plugin.hpp @@ -74,12 +74,26 @@ class OtaUpdatePlugin : public ros2_medkit_gateway::GatewayPlugin, public ros2_m // are expected to drop a fragment yaml in `fragments_dir_` and then // notify the gateway so its ManifestManager re-merges. Without this // the new app shows up as an "Orphan node (not in manifest)" warn - // log and never attaches to the manifest entity tree. + // log and never attaches to the manifest entity tree. The fragment + // filename keys on the target component (added/removed_components[0]), + // shared by install + uninstall, so uninstall removes the same file. tl::expected write_install_fragment(const std::string & update_id, const nlohmann::json & metadata); - tl::expected remove_install_fragment(const std::string & update_id); + tl::expected remove_install_fragment(const nlohmann::json & metadata); void notify_manifest_changed(); + /// A process this plugin itself spawned, tracked so a later re-apply or + /// uninstall can kill exactly that process (by recorded pid, pid-reuse + /// guarded) instead of guessing at a catalog-supplied basename. + struct SpawnedProc { + int pid{-1}; + std::string executable; + }; + + /// If we spawned a process for `component`, terminate it (by recorded pid) + /// and drop the record. No-op if we never spawned one for that component. + void kill_previous_spawn(const std::string & component); + std::string catalog_url_; std::string staging_dir_; std::string install_dir_; @@ -90,9 +104,26 @@ class OtaUpdatePlugin : public ros2_medkit_gateway::GatewayPlugin, public ros2_m std::mutex mu_; std::map registry_; std::map staged_artifacts_; + // Keyed by target component: the process THIS plugin spawned for it. + std::map spawned_; std::unique_ptr catalog_client_; std::unique_ptr process_runner_; }; +namespace detail { + +/// Escape a string for emission as a double-quoted YAML scalar. Backslash and +/// double-quote are backslash-escaped; control characters (newline, CR, tab) +/// are emitted as YAML escapes so a value can never break out of its scalar or +/// inject a sibling key. Exposed for direct testing of the quoting behavior. +std::string yaml_quote(const std::string & value); + +/// Render the manifest-fragment YAML body for one OTA-installed app, emitting +/// app_id / node_name / description as quoted scalars. Exposed for testing. +std::string render_install_fragment(const std::string & app_id, const std::string & node_name, + const std::string & description); + +} // namespace detail + } // namespace ota_update_plugin diff --git a/demos/ota_nav2_sensor_fix/ota_update_plugin/src/ota_update_plugin.cpp b/demos/ota_nav2_sensor_fix/ota_update_plugin/src/ota_update_plugin.cpp index 1fab6c6..321f31a 100644 --- a/demos/ota_nav2_sensor_fix/ota_update_plugin/src/ota_update_plugin.cpp +++ b/demos/ota_nav2_sensor_fix/ota_update_plugin/src/ota_update_plugin.cpp @@ -14,12 +14,18 @@ #include "ota_update_plugin/ota_update_plugin.hpp" +#include +#include +#include + +#include #include #include #include #include #include #include +#include #include #include @@ -37,30 +43,100 @@ using ros2_medkit_gateway::UpdateBackendErrorInfo; namespace { -/// Extract a packed tarball into a staging directory, then atomically replace -/// `${install_dir}/${target_package}` with the freshly extracted contents. -/// The artifacts are produced by pack_artifact.py and contain a single -/// top-level directory named after the target package. -tl::expected extract_and_swap(const std::string & staged_tarball, const std::string & install_dir, - const std::string & target_package) { - if (target_package.empty()) { - return tl::make_unexpected("target_package is empty"); +/// Reject any catalog-supplied name (component / update id / executable / +/// target package) that could escape the staging or install tree, or inject a +/// process/tar argument. Only `[A-Za-z0-9._-]` is allowed; empty, "." and ".." +/// are rejected explicitly (a no-op or a parent-dir escape as a path segment). +/// '/' and whitespace are outside the allow-list, so path separators and +/// argument injection are rejected as a consequence. +bool is_safe_component(const std::string & s) { + if (s.empty() || s == "." || s == "..") { + return false; + } + for (const char c : s) { + const bool ok = (c >= 'A' && c <= 'Z') || (c >= 'a' && c <= 'z') || (c >= '0' && c <= '9') || c == '.' || + c == '_' || c == '-'; + if (!ok) { + return false; + } + } + return true; +} + +/// First string element of a SOVD component array (updated / added / removed), +/// or "" if the key is absent, empty, or the element is not a string. +std::string first_component(const nlohmann::json & metadata, const char * key) { + if (!metadata.contains(key) || !metadata[key].is_array() || metadata[key].empty()) { + return {}; } + const auto & first = metadata[key][0]; + return first.is_string() ? first.get() : std::string{}; +} + +/// Run `tar -xzf -C ` via fork + execvp - no shell, +/// so a catalog-controlled name can never be interpreted as a tar option or a +/// shell metacharacter. Waits for tar and maps a non-zero exit to an error. +tl::expected run_tar_extract(const std::string & staged_tarball, const std::string & dest_dir) { + // execvp wants `char* const[]`. std::string::data() is non-const since C++17 + // and execvp does not modify the buffers, so no copy or const_cast is needed. + std::vector args = {"tar", "-xzf", staged_tarball, "-C", dest_dir}; + std::vector argv; + argv.reserve(args.size() + 1); + for (auto & a : args) { + argv.push_back(a.data()); + } + argv.push_back(nullptr); + + const pid_t pid = fork(); + if (pid < 0) { + return tl::make_unexpected(std::string("fork failed: ") + std::strerror(errno)); + } + if (pid == 0) { + execvp(argv[0], argv.data()); + _exit(127); // reached only if exec failed + } + int status = 0; + if (::waitpid(pid, &status, 0) < 0) { + return tl::make_unexpected(std::string("waitpid failed: ") + std::strerror(errno)); + } + if (!WIFEXITED(status) || WEXITSTATUS(status) != 0) { + return tl::make_unexpected("tar extraction failed for " + staged_tarball); + } + return {}; +} + +/// Stage half of a stage-then-swap: extract the tarball into a scratch dir and +/// verify it carries the expected top-level package directory. Does NOT touch +/// the live install tree - the caller only swaps it into place after this +/// succeeds, so a corrupt tarball can never leave a half-removed binary behind +/// (the update path must not kill the running node before this returns). +/// Returns the path to the extracted package tree on success. +tl::expected extract_to_staging(const std::string & staged_tarball, + const std::string & target_package) { const std::string staging_extracted = staged_tarball + ".extracted"; std::error_code ec; fs::remove_all(staging_extracted, ec); fs::create_directories(staging_extracted, ec); - const std::string cmd = "tar -xzf " + staged_tarball + " -C " + staging_extracted; - if (std::system(cmd.c_str()) != 0) { - return tl::make_unexpected("tar extraction failed: " + cmd); + if (auto ex = run_tar_extract(staged_tarball, staging_extracted); !ex) { + return tl::make_unexpected(ex.error()); } const std::string source = staging_extracted + "/" + target_package; if (!fs::exists(source)) { return tl::make_unexpected("artifact missing top-level directory '" + target_package + "' after extraction"); } + return source; +} +/// Swap half of a stage-then-swap: replace `${install_dir}/${target_package}` +/// with the freshly extracted tree. This is the destructive step and runs only +/// after extract_to_staging() succeeded. Not a true atomic rename (source and +/// target may sit on different filesystems), but the old tree is removed only +/// once a verified new tree exists, so there is no window with neither. +tl::expected swap_into_place(const std::string & source, const std::string & install_dir, + const std::string & target_package) { + std::error_code ec; fs::create_directories(install_dir, ec); const std::string target = install_dir + "/" + target_package; fs::remove_all(target, ec); @@ -153,8 +229,16 @@ tl::expected OtaUpdatePlugin::register_update(cons if (!metadata.contains("id") || !metadata["id"].is_string()) { return tl::make_unexpected(UpdateBackendErrorInfo{UpdateBackendError::InvalidInput, "metadata missing id"}); } + const std::string id = metadata["id"].get(); + // The id becomes a staging path segment in prepare() (staging_dir_/.tar.gz). + // Reject an unsafe id here so it never enters the registry - a bad id can then + // never reach a filesystem or exec path downstream. + if (!is_safe_component(id)) { + return tl::make_unexpected( + UpdateBackendErrorInfo{UpdateBackendError::InvalidInput, "update id contains unsafe characters: " + id}); + } std::lock_guard lk(mu_); - registry_[metadata["id"].get()] = metadata; + registry_[id] = metadata; return {}; } @@ -235,25 +319,50 @@ tl::expected OtaUpdatePlugin::execute( const std::string executable = metadata.value("x_medkit_executable", ""); if (kind == OperationKind::Update) { + if (executable.empty()) { + return tl::make_unexpected( + UpdateBackendErrorInfo{UpdateBackendError::InvalidInput, "missing x_medkit_executable"}); + } + // Validate every catalog-controlled name BEFORE it reaches a path or exec: + // target_package builds the extract/install path and the spawn binary, + // executable is the argv[0] we spawn. + if (!is_safe_component(target_package)) { + return tl::make_unexpected( + UpdateBackendErrorInfo{UpdateBackendError::InvalidInput, "x_medkit_target_package is unsafe"}); + } + if (!is_safe_component(executable)) { + return tl::make_unexpected( + UpdateBackendErrorInfo{UpdateBackendError::InvalidInput, "x_medkit_executable is unsafe"}); + } if (staged.empty()) { return tl::make_unexpected(UpdateBackendErrorInfo{UpdateBackendError::InvalidInput, "call prepare() first"}); } - if (executable.empty()) { + const std::string component = first_component(metadata, "updated_components"); + + // Stage-then-swap: extract + verify the new tree FIRST. A bad tarball + // returns an error here without ever stopping the running node - the kill + // below happens only once we hold a good extract. + reporter.set_progress(20); + auto src = extract_to_staging(staged, target_package); + if (!src) { return tl::make_unexpected( - UpdateBackendErrorInfo{UpdateBackendError::InvalidInput, "missing x_medkit_executable"}); + UpdateBackendErrorInfo{UpdateBackendError::Internal, "extract failed: " + src.error()}); } - // For an update across packages (e.g. broken_lidar -> fixed_lidar) the - // OLD process binary lives in a different package than the NEW one we - // are about to spawn, so its basename differs from `executable`. Honor - // x_medkit_replaces_executable when present, fall back to executable. + reporter.set_progress(40); + + // The kill is ADDITIVE. At boot fixed_lidar_node is spawned by the launch + // file, not this plugin, so the only handle we have on it is its basename + // (x_medkit_replaces_executable, falling back to executable). ALSO kill any + // node WE spawned earlier for this component so a re-apply of the same + // update does not leave a duplicate. const std::string kill_target = metadata.value("x_medkit_replaces_executable", executable); - reporter.set_progress(20); auto kr = process_runner_->kill_by_executable(kill_target); if (!kr) { return tl::make_unexpected(UpdateBackendErrorInfo{UpdateBackendError::Internal, "kill failed: " + kr.error()}); } - reporter.set_progress(40); - if (auto sw = extract_and_swap(staged, install_dir_, target_package); !sw) { + kill_previous_spawn(component); + + if (auto sw = swap_into_place(*src, install_dir_, target_package); !sw) { return tl::make_unexpected(UpdateBackendErrorInfo{UpdateBackendError::Internal, "swap failed: " + sw.error()}); } reporter.set_progress(70); @@ -262,6 +371,10 @@ tl::expected OtaUpdatePlugin::execute( if (!sp) { return tl::make_unexpected(UpdateBackendErrorInfo{UpdateBackendError::Internal, "spawn failed: " + sp.error()}); } + { + std::lock_guard lk(mu_); + spawned_[component] = SpawnedProc{*sp, executable}; + } // Update flow: same app id (the binary swapped in is bound to the // same scan_sensor_node entity as the binary it replaced) - no // manifest fragment to write, but the gateway still needs to @@ -273,15 +386,28 @@ tl::expected OtaUpdatePlugin::execute( } if (kind == OperationKind::Install) { - if (staged.empty()) { - return tl::make_unexpected(UpdateBackendErrorInfo{UpdateBackendError::InvalidInput, "call prepare() first"}); - } if (executable.empty()) { return tl::make_unexpected( UpdateBackendErrorInfo{UpdateBackendError::InvalidInput, "missing x_medkit_executable"}); } + if (!is_safe_component(target_package)) { + return tl::make_unexpected( + UpdateBackendErrorInfo{UpdateBackendError::InvalidInput, "x_medkit_target_package is unsafe"}); + } + if (!is_safe_component(executable)) { + return tl::make_unexpected( + UpdateBackendErrorInfo{UpdateBackendError::InvalidInput, "x_medkit_executable is unsafe"}); + } + if (staged.empty()) { + return tl::make_unexpected(UpdateBackendErrorInfo{UpdateBackendError::InvalidInput, "call prepare() first"}); + } reporter.set_progress(30); - if (auto sw = extract_and_swap(staged, install_dir_, target_package); !sw) { + auto src = extract_to_staging(staged, target_package); + if (!src) { + return tl::make_unexpected( + UpdateBackendErrorInfo{UpdateBackendError::Internal, "extract failed: " + src.error()}); + } + if (auto sw = swap_into_place(*src, install_dir_, target_package); !sw) { return tl::make_unexpected(UpdateBackendErrorInfo{UpdateBackendError::Internal, "swap failed: " + sw.error()}); } reporter.set_progress(70); @@ -290,6 +416,11 @@ tl::expected OtaUpdatePlugin::execute( if (!sp) { return tl::make_unexpected(UpdateBackendErrorInfo{UpdateBackendError::Internal, "spawn failed: " + sp.error()}); } + const std::string component = first_component(metadata, "added_components"); + { + std::lock_guard lk(mu_); + spawned_[component] = SpawnedProc{*sp, executable}; + } // Install flow: NEW app entity. Write a manifest fragment so the // gateway picks the new app up under the manifest tree (otherwise // it stays as an "Orphan node (not in manifest)" warn log and @@ -306,22 +437,27 @@ tl::expected OtaUpdatePlugin::execute( } if (kind == OperationKind::Uninstall) { + if (!target_package.empty() && !is_safe_component(target_package)) { + return tl::make_unexpected( + UpdateBackendErrorInfo{UpdateBackendError::InvalidInput, "x_medkit_target_package is unsafe"}); + } + const std::string component = first_component(metadata, "removed_components"); reporter.set_progress(30); + // Kill the node WE spawned for this component. Its argv[0] is the recorded + // executable basename, NOT target_package - the running node identifies as + // the executable, so killing by the package name missed it and left an + // orphan. No-op if we never spawned it (e.g. a base-manifest node). + kill_previous_spawn(component); + reporter.set_progress(70); if (!target_package.empty()) { - // Best-effort kill: legacy nodes may use the package name as their executable basename. - // Failures are tolerated since the process may already be gone. - auto kr = process_runner_->kill_by_executable(target_package); - (void)kr; - reporter.set_progress(70); std::error_code ec; fs::remove_all(install_dir_ + "/" + target_package, ec); } - // Uninstall: drop any fragment we wrote at install time and rerun - // discovery so the entity tree no longer lists the now-dead app. - // Entities defined in the base manifest stay - fragments only - // ADD, they can't remove base-manifest declarations - those - // entries just go offline. - if (auto fr = remove_install_fragment(id); !fr) { + // Uninstall: drop the fragment we wrote at install time (keyed on the same + // component) and rerun discovery so the entity tree no longer lists the + // now-dead app. Entities defined in the base manifest stay - fragments only + // ADD, they can't remove base-manifest declarations - those go offline. + if (auto fr = remove_install_fragment(metadata); !fr) { std::fprintf(stderr, "[ota_update_plugin] fragment remove failed for %s: %s\n", id.c_str(), fr.error().c_str()); } @@ -341,28 +477,56 @@ tl::expected OtaUpdatePlugin::supports_automated(c return true; } -namespace { +namespace detail { + +std::string yaml_quote(const std::string & value) { + std::string out = "\""; + for (const char c : value) { + switch (c) { + case '\\': + out += "\\\\"; + break; + case '"': + out += "\\\""; + break; + case '\n': + out += "\\n"; + break; + case '\r': + out += "\\r"; + break; + case '\t': + out += "\\t"; + break; + default: + out += c; + break; + } + } + out += "\""; + return out; +} -// Build the YAML body for a single OTA-installed app. We hand-emit the -// minimal subset the gateway's manifest parser accepts (no quoting -// edge cases in our generated values, so a yaml-cpp roundtrip would be -// overkill). The base manifest defines the `rbtheron` component; -// fragments only ever add apps onto it. +// Build the YAML body for a single OTA-installed app. app_id, node_name and +// description are catalog-derived, so they are emitted as double-quoted scalars +// via yaml_quote(): a space, ':' or newline in a value can neither break out of +// its scalar nor inject a sibling key. The base manifest defines the `rbtheron` +// component; fragments only ever add apps onto it. std::string render_install_fragment(const std::string & app_id, const std::string & node_name, const std::string & description) { std::string out; out += "manifest_version: \"1.0\"\n"; out += "apps:\n"; - out += " - id: " + app_id + "\n"; - out += " name: \"" + app_id + "\"\n"; + out += " - id: " + yaml_quote(app_id) + "\n"; + out += " name: " + yaml_quote(app_id) + "\n"; out += " category: \"ota-installed\"\n"; out += " is_located_on: rbtheron\n"; - out += " description: \"" + description + "\"\n"; - out += " ros_binding: { node_name: " + node_name + ", namespace: / }\n"; + out += " description: " + yaml_quote(description) + "\n"; + out += " ros_binding: { node_name: " + yaml_quote(node_name) + ", namespace: / }\n"; return out; } -} // namespace +} // namespace detail tl::expected OtaUpdatePlugin::write_install_fragment(const std::string & update_id, const nlohmann::json & metadata) { @@ -370,19 +534,17 @@ tl::expected OtaUpdatePlugin::write_install_fragment(const st const std::string node_name = metadata.value("x_medkit_executable", ""); // SOVD ISO 17978-3 reports the target entity via `added_components` - // (it's an array; for an OTA install we always have exactly one). - std::string app_id; - if (metadata.contains("added_components") && metadata["added_components"].is_array() - && !metadata["added_components"].empty()) { - const auto & first = metadata["added_components"][0]; - if (!first.is_string()) { - return tl::make_unexpected("added_components[0] is not a string"); - } - app_id = first.get(); - } + // (it's an array; for an OTA install we always have exactly one). The + // component is the fragment's stable key: install writes .yaml + // and uninstall removes the same file. + const std::string app_id = first_component(metadata, "added_components"); if (node_name.empty() || app_id.empty()) { - return tl::make_unexpected( - "metadata missing x_medkit_executable / added_components for fragment"); + return tl::make_unexpected("metadata missing x_medkit_executable / added_components for fragment"); + } + // The component becomes a path segment (the fragment filename), so validate + // it before building the path. + if (!is_safe_component(app_id)) { + return tl::make_unexpected("added_components[0] contains unsafe characters: " + app_id); } const std::string description = "OTA-installed via " + update_id; @@ -392,8 +554,8 @@ tl::expected OtaUpdatePlugin::write_install_fragment(const st return tl::make_unexpected("create fragments_dir failed: " + ec.message()); } - const std::string final_path = fragments_dir_ + "/" + update_id + ".yaml"; - const std::string tmp_path = fragments_dir_ + "/.tmp-" + update_id + ".yaml"; + const std::string final_path = fragments_dir_ + "/" + app_id + ".yaml"; + const std::string tmp_path = fragments_dir_ + "/.tmp-" + app_id + ".yaml"; // Atomic publish per ManifestManager's fragment contract: write to // tmp, fsync, rename. The gateway's fragment scanner runs on the // notify_entities_changed thread - a half-written file would fail @@ -401,7 +563,7 @@ tl::expected OtaUpdatePlugin::write_install_fragment(const st { std::ofstream f(tmp_path, std::ios::binary | std::ios::trunc); if (!f) return tl::make_unexpected("open tmp fragment failed: " + tmp_path); - f << render_install_fragment(app_id, node_name, description); + f << detail::render_install_fragment(app_id, node_name, description); f.flush(); if (!f) return tl::make_unexpected("write tmp fragment failed: " + tmp_path); } @@ -411,10 +573,19 @@ tl::expected OtaUpdatePlugin::write_install_fragment(const st return {}; } -tl::expected OtaUpdatePlugin::remove_install_fragment(const std::string & update_id) { +tl::expected OtaUpdatePlugin::remove_install_fragment(const nlohmann::json & metadata) { if (fragments_dir_.empty()) return {}; + // Same key as write_install_fragment: the target component, reported via + // removed_components for an uninstall. + const std::string component = first_component(metadata, "removed_components"); + if (component.empty()) { + return tl::make_unexpected("metadata missing removed_components for fragment removal"); + } + if (!is_safe_component(component)) { + return tl::make_unexpected("removed_components[0] contains unsafe characters: " + component); + } std::error_code ec; - fs::remove(fragments_dir_ + "/" + update_id + ".yaml", ec); + fs::remove(fragments_dir_ + "/" + component + ".yaml", ec); // Missing-file is fine (uninstall of an entity that lived in the // base manifest, never had a fragment); other errors are reported. if (ec && ec != std::errc::no_such_file_or_directory) { @@ -423,6 +594,23 @@ tl::expected OtaUpdatePlugin::remove_install_fragment(const s return {}; } +void OtaUpdatePlugin::kill_previous_spawn(const std::string & component) { + SpawnedProc prev; + { + std::lock_guard lk(mu_); + auto it = spawned_.find(component); + if (it == spawned_.end()) { + return; + } + prev = it->second; + spawned_.erase(it); + } + // kill_pid is pid-reuse guarded: it only signals if argv[0] still matches the + // executable we recorded, so a recycled pid is never harmed. Dropping the + // record above is correct either way (the process is gone or no longer ours). + process_runner_->kill_pid(prev.pid, prev.executable); +} + void OtaUpdatePlugin::notify_manifest_changed() { if (!context_) return; context_->notify_entities_changed(ros2_medkit_gateway::EntityChangeScope::full_refresh()); diff --git a/demos/ota_nav2_sensor_fix/ota_update_plugin/src/process_runner.cpp b/demos/ota_nav2_sensor_fix/ota_update_plugin/src/process_runner.cpp index 44118f0..cdebdaa 100644 --- a/demos/ota_nav2_sensor_fix/ota_update_plugin/src/process_runner.cpp +++ b/demos/ota_nav2_sensor_fix/ota_update_plugin/src/process_runner.cpp @@ -174,6 +174,35 @@ tl::expected ProcessRunner::kill_by_executable(const std::stri return signalled; } +bool ProcessRunner::kill_pid(int pid, const std::string & expected_basename, int timeout_ms) { + if (pid <= 0) { + return false; + } + // Existence probe. A dead pid (or one we do not own) fails here. + if (::kill(pid, 0) != 0) { + return false; + } + // Pid-reuse guard: refuse to signal a pid the kernel has recycled onto an + // unrelated process. argv[0] basename must still match what we spawned. + if (proc_cmdline_arg0(pid) != expected_basename) { + return false; + } + if (::kill(pid, SIGTERM) != 0) { + return false; + } + const auto deadline = std::chrono::steady_clock::now() + std::chrono::milliseconds(timeout_ms); + while (std::chrono::steady_clock::now() < deadline) { + if (::kill(pid, 0) != 0) { + return true; // exited on its own + } + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + } + if (::kill(pid, 0) == 0) { + ::kill(pid, SIGKILL); + } + return true; +} + tl::expected ProcessRunner::spawn(const std::string & executable_path) { // Double-fork so the grandchild is reparented to init and never becomes a // zombie in the gateway process. The intermediate child exits immediately diff --git a/demos/ota_nav2_sensor_fix/ota_update_plugin/src/process_runner.hpp b/demos/ota_nav2_sensor_fix/ota_update_plugin/src/process_runner.hpp index d145f06..cb124d0 100644 --- a/demos/ota_nav2_sensor_fix/ota_update_plugin/src/process_runner.hpp +++ b/demos/ota_nav2_sensor_fix/ota_update_plugin/src/process_runner.hpp @@ -43,6 +43,14 @@ class ProcessRunner { virtual tl::expected kill_by_executable(const std::string & executable_basename, int timeout_ms = 2000); + /// Terminate a specific pid, guarding against pid reuse: signals only if + /// /proc//cmdline argv[0] basename == `expected_basename`. Sends + /// SIGTERM, waits up to `timeout_ms` for exit, then SIGKILL. Returns true if + /// the pid was ours and was signalled; false if pid <= 0, already gone, or + /// now a different process (so the caller can drop a stale record without + /// signalling an unrelated one). + virtual bool kill_pid(int pid, const std::string & expected_basename, int timeout_ms = 2000); + /// fork+exec the executable at `executable_path`. Returns child PID or error. virtual tl::expected spawn(const std::string & executable_path); }; diff --git a/demos/ota_nav2_sensor_fix/ota_update_plugin/test/test_plugin_smoke.cpp b/demos/ota_nav2_sensor_fix/ota_update_plugin/test/test_plugin_smoke.cpp index ccd3cac..912d6a4 100644 --- a/demos/ota_nav2_sensor_fix/ota_update_plugin/test/test_plugin_smoke.cpp +++ b/demos/ota_nav2_sensor_fix/ota_update_plugin/test/test_plugin_smoke.cpp @@ -12,11 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include +#include #include +#include #include #include #include #include +#include #include @@ -43,32 +47,89 @@ class FakeCatalogClient : public ota_update_plugin::CatalogClient { tl::expected download_artifact(const std::string & url, const std::string & out) override { requested_url = url; std::ofstream o(out, std::ios::binary); - o << artifact_to_return; + o << artifact_to_return; // binary-safe: writes embedded NULs of real gzip return out; } }; -/// ProcessRunner stub: records the basename passed to kill_by_executable so a -/// test can verify the plugin honors x_medkit_replaces_executable. Returns 0 -/// signalled processes (no-op kill) and an error from spawn so execute() halts -/// before touching the (nonexistent) install dir. -class RecordingProcessRunner : public ota_update_plugin::ProcessRunner { +/// Stateful ProcessRunner double modelling a tiny process table so tests can +/// assert exactly which processes are live after a sequence of spawn/kill +/// calls. spawn() hands out monotonically increasing fake pids; the kill +/// methods remove matching entries. No real process is ever created. +class FakeProcessRunner : public ota_update_plugin::ProcessRunner { public: + struct Entry { + int pid; + std::string executable; + }; + std::string last_kill_target; - std::vector pgrep(const std::string & /*executable_basename*/) override { - return {}; + std::vector pgrep(const std::string & executable_basename) override { + std::vector out; + for (const auto & e : table_) { + if (e.executable == executable_basename) { + out.push_back(e.pid); + } + } + return out; } tl::expected kill_by_executable(const std::string & executable_basename, int /*timeout_ms*/ = 2000) override { last_kill_target = executable_basename; - return 0; + int removed = 0; + for (auto it = table_.begin(); it != table_.end();) { + if (it->executable == executable_basename) { + it = table_.erase(it); + ++removed; + } else { + ++it; + } + } + return removed; + } + + bool kill_pid(int pid, const std::string & expected_basename, int /*timeout_ms*/ = 2000) override { + for (auto it = table_.begin(); it != table_.end(); ++it) { + if (it->pid == pid && it->executable == expected_basename) { + table_.erase(it); + return true; + } + } + return false; } - tl::expected spawn(const std::string & /*executable_path*/) override { - return tl::make_unexpected(std::string("stub: spawn intentionally not implemented")); + tl::expected spawn(const std::string & executable_path) override { + const auto slash = executable_path.rfind('/'); + const std::string base = (slash == std::string::npos) ? executable_path : executable_path.substr(slash + 1); + const int pid = next_fake_pid_++; + table_.push_back(Entry{pid, base}); + return pid; } + + int count(const std::string & executable_basename) const { + int n = 0; + for (const auto & e : table_) { + if (e.executable == executable_basename) { + ++n; + } + } + return n; + } + + std::vector live_executables() const { + std::vector out; + out.reserve(table_.size()); + for (const auto & e : table_) { + out.push_back(e.executable); + } + return out; + } + + private: + std::vector table_; + int next_fake_pid_ = 1000; }; ros2_medkit_gateway::UpdateProgressReporter make_reporter(ros2_medkit_gateway::UpdateStatusInfo & info, @@ -76,6 +137,28 @@ ros2_medkit_gateway::UpdateProgressReporter make_reporter(ros2_medkit_gateway::U return ros2_medkit_gateway::UpdateProgressReporter(info, mu); } +// Build a valid .tar.gz containing /lib// (a tiny stub) and +// return its raw bytes, or "" if the system tar is unavailable. The plugin +// under test extracts via fork+execvp; this fixture setup uses the shell tar. +std::string build_artifact_bytes(const std::string & pkg, const std::string & exe) { + namespace fs = std::filesystem; + std::error_code ec; + const std::string work = ::testing::TempDir() + "/ota_fixture_" + pkg + "_" + exe; + fs::remove_all(work, ec); + fs::create_directories(work + "/" + pkg + "/lib/" + pkg, ec); + { + std::ofstream f(work + "/" + pkg + "/lib/" + pkg + "/" + exe, std::ios::binary); + f << "#!/bin/sh\nexit 0\n"; + } + const std::string tgz = work + ".tar.gz"; + const std::string cmd = "tar -czf '" + tgz + "' -C '" + work + "' '" + pkg + "'"; + if (std::system(cmd.c_str()) != 0) { + return {}; + } + std::ifstream in(tgz, std::ios::binary); + return std::string((std::istreambuf_iterator(in)), std::istreambuf_iterator()); +} + } // namespace TEST(OtaUpdatePluginSmoke, NameAndConstructible) { @@ -83,7 +166,7 @@ TEST(OtaUpdatePluginSmoke, NameAndConstructible) { EXPECT_EQ(plugin.name(), "ota_update_plugin"); } -// Exercises the real ProcessRunner (not the RecordingProcessRunner stub) +// Exercises the real ProcessRunner (not the FakeProcessRunner double) // through the actual double-fork + status pipe in process_runner.cpp. // Before the pipe2/errno fix, spawn() unconditionally returned the // (already-reaped) intermediate child's pid regardless of whether execl @@ -178,11 +261,17 @@ TEST(OtaUpdatePluginSmoke, PrepareUninstallSkipsDownload) { } TEST(OtaUpdatePluginSmoke, ExecuteUpdateUsesReplacesExecutableForKill) { + const std::string bytes = build_artifact_bytes("fixed_lidar", "fixed_lidar_node"); + if (bytes.empty()) GTEST_SKIP() << "system tar unavailable to build fixture tarball"; + ota_update_plugin::OtaUpdatePlugin plugin; - plugin.configure({{"staging_dir", ::testing::TempDir() + "/replaces_test"}}); - plugin.set_catalog_client_for_test(std::make_unique("http://x")); - auto runner = std::make_unique(); - RecordingProcessRunner * runner_raw = runner.get(); + plugin.configure({{"staging_dir", ::testing::TempDir() + "/replaces_test/staging"}, + {"install_dir", ::testing::TempDir() + "/replaces_test/install"}}); + auto fake = std::make_unique("http://x"); + fake->artifact_to_return = bytes; + plugin.set_catalog_client_for_test(std::move(fake)); + auto runner = std::make_unique(); + FakeProcessRunner * runner_raw = runner.get(); plugin.set_process_runner_for_test(std::move(runner)); // Update entry with separate old + new executable basenames. @@ -200,20 +289,26 @@ TEST(OtaUpdatePluginSmoke, ExecuteUpdateUsesReplacesExecutableForKill) { auto reporter = make_reporter(info, mu); ASSERT_TRUE(plugin.prepare("u_replaces", reporter)); - // execute() will fail at extract_and_swap (the staged tarball is not a real - // gzipped archive) but the kill step runs first - that is what we are - // checking here. + // With stage-then-swap the extract now succeeds first (real tarball), so the + // additive kill runs and still targets x_medkit_replaces_executable. auto rc = plugin.execute("u_replaces", reporter); - (void)rc; + ASSERT_TRUE(rc) << rc.error().message; EXPECT_EQ(runner_raw->last_kill_target, "broken_lidar_node"); + EXPECT_EQ(runner_raw->count("fixed_lidar_node"), 1); } TEST(OtaUpdatePluginSmoke, ExecuteUpdateFallsBackToExecutableWhenReplacesMissing) { + const std::string bytes = build_artifact_bytes("scan_pkg", "scan_node"); + if (bytes.empty()) GTEST_SKIP() << "system tar unavailable to build fixture tarball"; + ota_update_plugin::OtaUpdatePlugin plugin; - plugin.configure({{"staging_dir", ::testing::TempDir() + "/replaces_fallback"}}); - plugin.set_catalog_client_for_test(std::make_unique("http://x")); - auto runner = std::make_unique(); - RecordingProcessRunner * runner_raw = runner.get(); + plugin.configure({{"staging_dir", ::testing::TempDir() + "/replaces_fallback/staging"}, + {"install_dir", ::testing::TempDir() + "/replaces_fallback/install"}}); + auto fake = std::make_unique("http://x"); + fake->artifact_to_return = bytes; + plugin.set_catalog_client_for_test(std::move(fake)); + auto runner = std::make_unique(); + FakeProcessRunner * runner_raw = runner.get(); plugin.set_process_runner_for_test(std::move(runner)); // Update entry without x_medkit_replaces_executable - kill should target @@ -232,6 +327,249 @@ TEST(OtaUpdatePluginSmoke, ExecuteUpdateFallsBackToExecutableWhenReplacesMissing ASSERT_TRUE(plugin.prepare("u_no_replaces", reporter)); auto rc = plugin.execute("u_no_replaces", reporter); - (void)rc; + ASSERT_TRUE(rc) << rc.error().message; EXPECT_EQ(runner_raw->last_kill_target, "scan_node"); } + +// --- Path-traversal rejection (findings #2/#8/#13) --- + +TEST(OtaUpdatePluginSmoke, ExecuteRejectsTargetPackageTraversal) { + ota_update_plugin::OtaUpdatePlugin plugin; + plugin.configure({{"staging_dir", ::testing::TempDir() + "/trav_pkg/staging"}, + {"install_dir", ::testing::TempDir() + "/trav_pkg/install"}}); + plugin.set_catalog_client_for_test(std::make_unique("http://x")); + auto runner = std::make_unique(); + FakeProcessRunner * runner_raw = runner.get(); + plugin.set_process_runner_for_test(std::move(runner)); + + ASSERT_TRUE(plugin.register_update({ + {"id", "u_trav_pkg"}, + {"updated_components", {"scan_sensor_node"}}, + {"x_medkit_artifact_url", "/artifacts/x.tgz"}, + {"x_medkit_target_package", "../../etc/x"}, + {"x_medkit_executable", "scan_node"}, + })); + + ros2_medkit_gateway::UpdateStatusInfo info; + std::mutex mu; + auto reporter = make_reporter(info, mu); + ASSERT_TRUE(plugin.prepare("u_trav_pkg", reporter)); + auto rc = plugin.execute("u_trav_pkg", reporter); + ASSERT_FALSE(rc); + EXPECT_EQ(rc.error().code, ros2_medkit_gateway::UpdateBackendError::InvalidInput); + // Rejected before any process action or filesystem swap - no kill, no spawn. + EXPECT_TRUE(runner_raw->last_kill_target.empty()); + EXPECT_TRUE(runner_raw->live_executables().empty()); +} + +TEST(OtaUpdatePluginSmoke, ExecuteRejectsExecutableTraversal) { + ota_update_plugin::OtaUpdatePlugin plugin; + plugin.configure({{"staging_dir", ::testing::TempDir() + "/trav_exe/staging"}, + {"install_dir", ::testing::TempDir() + "/trav_exe/install"}}); + plugin.set_catalog_client_for_test(std::make_unique("http://x")); + auto runner = std::make_unique(); + FakeProcessRunner * runner_raw = runner.get(); + plugin.set_process_runner_for_test(std::move(runner)); + + ASSERT_TRUE(plugin.register_update({ + {"id", "u_trav_exe"}, + {"updated_components", {"scan_sensor_node"}}, + {"x_medkit_artifact_url", "/artifacts/x.tgz"}, + {"x_medkit_target_package", "fixed_lidar"}, + {"x_medkit_executable", "../../bin/sh"}, + })); + + ros2_medkit_gateway::UpdateStatusInfo info; + std::mutex mu; + auto reporter = make_reporter(info, mu); + ASSERT_TRUE(plugin.prepare("u_trav_exe", reporter)); + auto rc = plugin.execute("u_trav_exe", reporter); + ASSERT_FALSE(rc); + EXPECT_EQ(rc.error().code, ros2_medkit_gateway::UpdateBackendError::InvalidInput); + EXPECT_TRUE(runner_raw->last_kill_target.empty()); + EXPECT_TRUE(runner_raw->live_executables().empty()); +} + +// --- Stage-then-swap: don't kill before a verified extract (finding #7) --- + +TEST(OtaUpdatePluginSmoke, ExtractFailureDoesNotKill) { + ota_update_plugin::OtaUpdatePlugin plugin; + plugin.configure({{"staging_dir", ::testing::TempDir() + "/extfail/staging"}, + {"install_dir", ::testing::TempDir() + "/extfail/install"}}); + auto fake = std::make_unique("http://x"); + fake->artifact_to_return = "this is not a gzip archive"; // extraction will fail + plugin.set_catalog_client_for_test(std::move(fake)); + auto runner = std::make_unique(); + FakeProcessRunner * runner_raw = runner.get(); + // Pre-existing lidar node (as if spawned by the launch file, not the plugin). + ASSERT_TRUE(runner_raw->spawn("/opt/ros/lib/broken_lidar/broken_lidar_node")); + plugin.set_process_runner_for_test(std::move(runner)); + + ASSERT_TRUE(plugin.register_update({ + {"id", "u_extfail"}, + {"updated_components", {"scan_sensor_node"}}, + {"x_medkit_artifact_url", "/artifacts/broken.tgz"}, + {"x_medkit_target_package", "fixed_lidar"}, + {"x_medkit_executable", "fixed_lidar_node"}, + {"x_medkit_replaces_executable", "broken_lidar_node"}, + })); + + ros2_medkit_gateway::UpdateStatusInfo info; + std::mutex mu; + auto reporter = make_reporter(info, mu); + ASSERT_TRUE(plugin.prepare("u_extfail", reporter)); + auto rc = plugin.execute("u_extfail", reporter); + ASSERT_FALSE(rc); // extract failed + // The running lidar was NOT killed - extract happens before any kill. + EXPECT_TRUE(runner_raw->last_kill_target.empty()); + EXPECT_EQ(runner_raw->count("broken_lidar_node"), 1); +} + +// --- Track what we spawned; additive re-apply dedup (findings #9/#10) --- + +TEST(OtaUpdatePluginSmoke, CrossPackageReexecuteDoesNotDuplicate) { + const std::string bytes = build_artifact_bytes("fixed_lidar", "fixed_lidar_node"); + if (bytes.empty()) GTEST_SKIP() << "system tar unavailable to build fixture tarball"; + + ota_update_plugin::OtaUpdatePlugin plugin; + plugin.configure({{"staging_dir", ::testing::TempDir() + "/reexec/staging"}, + {"install_dir", ::testing::TempDir() + "/reexec/install"}}); + auto fake = std::make_unique("http://x"); + fake->artifact_to_return = bytes; + plugin.set_catalog_client_for_test(std::move(fake)); + auto runner = std::make_unique(); + FakeProcessRunner * runner_raw = runner.get(); + plugin.set_process_runner_for_test(std::move(runner)); + + ASSERT_TRUE(plugin.register_update({ + {"id", "u_reexec"}, + {"updated_components", {"scan_sensor_node"}}, + {"x_medkit_artifact_url", "/artifacts/fixed.tgz"}, + {"x_medkit_target_package", "fixed_lidar"}, + {"x_medkit_executable", "fixed_lidar_node"}, + {"x_medkit_replaces_executable", "broken_lidar_node"}, + })); + + ros2_medkit_gateway::UpdateStatusInfo info; + std::mutex mu; + auto reporter = make_reporter(info, mu); + ASSERT_TRUE(plugin.prepare("u_reexec", reporter)); + ASSERT_TRUE(plugin.execute("u_reexec", reporter)); + EXPECT_EQ(runner_raw->count("fixed_lidar_node"), 1); + // Re-apply the same update: the previously plugin-spawned node must be killed + // by recorded pid so we do not accumulate duplicates. + ASSERT_TRUE(plugin.execute("u_reexec", reporter)); + EXPECT_EQ(runner_raw->count("fixed_lidar_node"), 1); +} + +// --- Uninstall kill + fragment (findings #11/#12) --- + +TEST(OtaUpdatePluginSmoke, UninstallKillsRecordedExecutableNotPackage) { + const std::string bytes = build_artifact_bytes("newpkg", "new_node"); + if (bytes.empty()) GTEST_SKIP() << "system tar unavailable to build fixture tarball"; + + ota_update_plugin::OtaUpdatePlugin plugin; + plugin.configure({{"staging_dir", ::testing::TempDir() + "/uninstall_kill/staging"}, + {"install_dir", ::testing::TempDir() + "/uninstall_kill/install"}}); + auto fake = std::make_unique("http://x"); + fake->artifact_to_return = bytes; + plugin.set_catalog_client_for_test(std::move(fake)); + auto runner = std::make_unique(); + FakeProcessRunner * runner_raw = runner.get(); + plugin.set_process_runner_for_test(std::move(runner)); + + ros2_medkit_gateway::UpdateStatusInfo info; + std::mutex mu; + auto reporter = make_reporter(info, mu); + + // Install records the spawned executable under component "newapp". + ASSERT_TRUE(plugin.register_update({ + {"id", "i_new"}, + {"added_components", {"newapp"}}, + {"x_medkit_artifact_url", "/artifacts/new.tgz"}, + {"x_medkit_target_package", "newpkg"}, + {"x_medkit_executable", "new_node"}, + })); + ASSERT_TRUE(plugin.prepare("i_new", reporter)); + ASSERT_TRUE(plugin.execute("i_new", reporter)); + ASSERT_EQ(runner_raw->count("new_node"), 1); + + // Uninstall the same component: the recorded executable is killed, NOT the + // package basename. + ASSERT_TRUE(plugin.register_update({ + {"id", "rm_new"}, + {"removed_components", {"newapp"}}, + {"x_medkit_target_package", "newpkg"}, + })); + ASSERT_TRUE(plugin.prepare("rm_new", reporter)); + ASSERT_TRUE(plugin.execute("rm_new", reporter)); + EXPECT_EQ(runner_raw->count("new_node"), 0); + // Uninstall targets the recorded pid via kill_pid, never kill_by_executable + // on the package name. + EXPECT_NE(runner_raw->last_kill_target, "newpkg"); +} + +TEST(OtaUpdatePluginSmoke, UninstallRemovesFragmentByComponent) { + const std::string bytes = build_artifact_bytes("fragpkg", "frag_node"); + if (bytes.empty()) GTEST_SKIP() << "system tar unavailable to build fixture tarball"; + + namespace fs = std::filesystem; + const std::string frag_dir = ::testing::TempDir() + "/frag_component/fragments"; + std::error_code ec; + fs::remove_all(frag_dir, ec); + + ota_update_plugin::OtaUpdatePlugin plugin; + plugin.configure({{"staging_dir", ::testing::TempDir() + "/frag_component/staging"}, + {"install_dir", ::testing::TempDir() + "/frag_component/install"}, + {"fragments_dir", frag_dir}}); + auto fake = std::make_unique("http://x"); + fake->artifact_to_return = bytes; + plugin.set_catalog_client_for_test(std::move(fake)); + plugin.set_process_runner_for_test(std::make_unique()); + + ros2_medkit_gateway::UpdateStatusInfo info; + std::mutex mu; + auto reporter = make_reporter(info, mu); + + // Install writes .yaml keyed on added_components[0]. + ASSERT_TRUE(plugin.register_update({ + {"id", "i_frag"}, + {"added_components", {"frag_app"}}, + {"x_medkit_artifact_url", "/artifacts/frag.tgz"}, + {"x_medkit_target_package", "fragpkg"}, + {"x_medkit_executable", "frag_node"}, + })); + ASSERT_TRUE(plugin.prepare("i_frag", reporter)); + ASSERT_TRUE(plugin.execute("i_frag", reporter)); + EXPECT_TRUE(fs::exists(frag_dir + "/frag_app.yaml")); + + // Uninstall removes the SAME file, keyed on removed_components[0]. + ASSERT_TRUE(plugin.register_update({ + {"id", "rm_frag"}, + {"removed_components", {"frag_app"}}, + {"x_medkit_target_package", "fragpkg"}, + })); + ASSERT_TRUE(plugin.prepare("rm_frag", reporter)); + ASSERT_TRUE(plugin.execute("rm_frag", reporter)); + EXPECT_FALSE(fs::exists(frag_dir + "/frag_app.yaml")); + + int remaining = 0; + for (const auto & entry : fs::directory_iterator(frag_dir, ec)) { + (void)entry; + ++remaining; + } + EXPECT_EQ(remaining, 0); +} + +// --- Fragment YAML quoting (finding #13-yaml) --- + +TEST(OtaFragmentRender, RenderFragmentQuotesScalars) { + const std::string body = ota_update_plugin::detail::render_install_fragment( + "scan node: 1", "lidar node", "OTA-installed via u\ninjected_top: true"); + // Values with spaces/colons are emitted as double-quoted scalars. + EXPECT_NE(body.find("\"scan node: 1\""), std::string::npos); + EXPECT_NE(body.find("\"lidar node\""), std::string::npos); + // A newline in a value is escaped, so it cannot open a new top-level YAML key. + EXPECT_EQ(body.find("\ninjected_top:"), std::string::npos); + EXPECT_NE(body.find("\\ninjected_top: true"), std::string::npos); +} From 195d62cddc72b72990ade200f708a6ddd8e7b873 Mon Sep 17 00:00:00 2001 From: Bartosz Burda Date: Tue, 7 Jul 2026 13:29:24 +0200 Subject: [PATCH 19/19] ci(ota_demo): build and run the plugin gtest + packer/server pytest in CI The plugin image built with -DBUILD_TESTING=OFF and no job ran colcon test or pytest, so the plugin gtest, the ota_update_server tests and the pack_artifact tests never executed anywhere. Add a Dockerfile 'ota-plugin-test' stage that rebuilds the plugin with BUILD_TESTING=ON and runs its gtest, and a 'plugin-tests' CI job that builds that stage and runs both pytest suites. --- .github/workflows/ci.yml | 31 ++++++++++++++++++++ demos/ota_nav2_sensor_fix/Dockerfile.gateway | 19 ++++++++++++ 2 files changed, 50 insertions(+) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 03a9b73..e39e9ac 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -207,6 +207,37 @@ jobs: working-directory: demos/ota_nav2_sensor_fix run: docker compose down + # Actually runs the OTA demo's own test suites (they never run in the demo + # up/smoke jobs). The C++ gtest is built and executed inside the plugin test + # stage of Dockerfile.gateway (a failing test fails the docker build); the two + # pytest suites run natively. + plugin-tests: + needs: lint + runs-on: ubuntu-24.04 + steps: + - name: Checkout repository + uses: actions/checkout@v4 + + - name: Run ota_update_plugin C++ gtest (in image build) + working-directory: demos/ota_nav2_sensor_fix + run: docker build --target ota-plugin-test -f Dockerfile.gateway . + + - name: Set up Python + uses: actions/setup-python@v5 + with: + python-version: '3.12' + + - name: Install Python test dependencies + run: pip install pytest fastapi 'uvicorn[standard]' httpx + + - name: Run ota_update_server pytest suite + working-directory: demos/ota_nav2_sensor_fix/ota_update_server + run: pytest tests + + - name: Run pack_artifact pytest suite + working-directory: demos/ota_nav2_sensor_fix/scripts + run: pytest test_pack_artifact.py + # Separate job from build-and-test-ota: this one drives the full # latch/publish/apply/clear narrative through the operator scripts. The # entrypoint auto-applies broken_lidar_3_0_0 at boot; send-goal.sh then diff --git a/demos/ota_nav2_sensor_fix/Dockerfile.gateway b/demos/ota_nav2_sensor_fix/Dockerfile.gateway index cc3c5b5..1c71475 100644 --- a/demos/ota_nav2_sensor_fix/Dockerfile.gateway +++ b/demos/ota_nav2_sensor_fix/Dockerfile.gateway @@ -138,6 +138,25 @@ RUN . /opt/ros/jazzy/setup.sh && \ rm -rf /var/lib/apt/lists/* +# Test stage: rebuild ONLY the OTA plugin with tests enabled and run its gtest +# suite (ament_add_gtest -> test_ota_update_plugin). Because it is FROM builder, +# the heavy gateway + demo build above is reused as-is; only the plugin +# recompiles with BUILD_TESTING=ON. A failing gtest makes colcon test-result +# return non-zero, which fails this image build - CI drives it via +# `docker build --target ota-plugin-test`. The runtime stage below is a +# separate FROM and is unaffected; a full build/compose build (which targets +# the final stage) never runs this stage. +FROM builder AS ota-plugin-test +WORKDIR /ws +RUN . /opt/ros/jazzy/setup.sh && \ + . /ws/install/setup.sh && \ + colcon build --packages-select ota_update_plugin \ + --cmake-args -DCMAKE_BUILD_TYPE=Release -DBUILD_TESTING=ON && \ + colcon test --packages-select ota_update_plugin \ + --event-handlers console_direct+ && \ + colcon test-result --verbose + + FROM ros:jazzy # Runtime dependencies. Beyond the gateway/plugin bare minimum we also pull in