diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 588666e..e39e9ac 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -163,3 +163,139 @@ 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: Diagnostics on failure + if: failure() + run: ./tests/dump_ota_diagnostics.sh ota_diagnostics.log + + - name: Upload diagnostics + if: failure() + uses: actions/upload-artifact@v4 + with: + name: ota-diagnostics-smoke + path: ota_diagnostics.log + + - name: Teardown + if: always() + 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 + # 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: Diagnostics on failure + if: failure() + 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() + 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..1c71475 --- /dev/null +++ b/demos/ota_nav2_sensor_fix/Dockerfile.gateway @@ -0,0 +1,250 @@ +# 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 \ + --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/* + + +# 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 +# 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 + +# 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 +# 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..e6480c8 --- /dev/null +++ b/demos/ota_nav2_sensor_fix/README.md @@ -0,0 +1,271 @@ +# 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 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 + `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 # 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 +./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` 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`): + +- `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 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 +# 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 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. + +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 +- 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, +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..4cb65cc --- /dev/null +++ b/demos/ota_nav2_sensor_fix/docker-compose.yml @@ -0,0 +1,51 @@ +# 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" + # 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: + - 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..5c258fa --- /dev/null +++ b/demos/ota_nav2_sensor_fix/manifest.yaml @@ -0,0 +1,282 @@ +# 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 + operator health checks (lidar / localization / drivetrain / costmap) for root-cause confirmation" + hosted_by: + - medkit-gateway + - medkit-fault-manager + - health-check + + - 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..145da10 --- /dev/null +++ b/demos/ota_nav2_sensor_fix/ota_update_plugin/include/ota_update_plugin/ota_update_plugin.hpp @@ -0,0 +1,129 @@ +// 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. 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 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_; + std::string fragments_dir_; + + ros2_medkit_gateway::PluginContext * context_{nullptr}; + + 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/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..321f31a --- /dev/null +++ b/demos/ota_nav2_sensor_fix/ota_update_plugin/src/ota_update_plugin.cpp @@ -0,0 +1,619 @@ +// 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 +#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 { + +/// 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); + + 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); + 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"}); + } + 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_[id] = 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 (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"}); + } + 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::Internal, "extract failed: " + src.error()}); + } + 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); + auto kr = process_runner_->kill_by_executable(kill_target); + if (!kr) { + return tl::make_unexpected(UpdateBackendErrorInfo{UpdateBackendError::Internal, "kill failed: " + kr.error()}); + } + 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); + 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()}); + } + { + 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 + // 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 (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); + 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); + 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()}); + } + 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 + // 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) { + 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()) { + std::error_code ec; + fs::remove_all(install_dir_ + "/" + target_package, ec); + } + // 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()); + } + 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*/) { + // 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 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. 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: " + 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: " + yaml_quote(description) + "\n"; + out += " ros_binding: { node_name: " + yaml_quote(node_name) + ", namespace: / }\n"; + return out; +} + +} // namespace detail + +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). 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"); + } + // 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; + + 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_ + "/" + 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 + // 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 << detail::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 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_ + "/" + 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) { + return tl::make_unexpected("remove fragment failed: " + ec.message()); + } + 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()); +} + +} // 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..cdebdaa --- /dev/null +++ b/demos/ota_nav2_sensor_fix/ota_update_plugin/src/process_runner.cpp @@ -0,0 +1,321 @@ +// 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; +} + +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 + // 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..cb124d0 --- /dev/null +++ b/demos/ota_nav2_sensor_fix/ota_update_plugin/src/process_runner.hpp @@ -0,0 +1,58 @@ +// 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//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 + /// 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); + + /// 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); +}; + +} // 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..912d6a4 --- /dev/null +++ b/demos/ota_nav2_sensor_fix/ota_update_plugin/test/test_plugin_smoke.cpp @@ -0,0 +1,575 @@ +// 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 +#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; // binary-safe: writes embedded NULs of real gzip + return out; + } +}; + +/// 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 { + 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; + 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 { + 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, + std::mutex & mu) { + 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) { + ota_update_plugin::OtaUpdatePlugin plugin; + EXPECT_EQ(plugin.name(), "ota_update_plugin"); +} + +// 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 +// 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) { + 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/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. + 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)); + + // 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); + 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/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 + // 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); + 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); +} 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..2b73496 --- /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(health_check_msgs REQUIRED) + +add_executable(health_check_node src/health_check_node.cpp) +ament_target_dependencies(health_check_node + rclcpp + geometry_msgs + nav_msgs + sensor_msgs + health_check_msgs +) + +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..3fc58c7 --- /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 operation (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 + 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 new file mode 100644 index 0000000..1cbaa74 --- /dev/null +++ b/demos/ota_nav2_sensor_fix/ros2_packages/health_check/src/health_check_node.cpp @@ -0,0 +1,395 @@ +// 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 +// 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)? +// +// 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 +// operation answers "broken" before the fix and "healthy" after it. + +#include +#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 check_lidar_health). +constexpr float kStuckBandM = 0.05f; +constexpr float kStuckMaxRangeM = 1.5f; +constexpr size_t kStuckMinRunRays = 30; + +// Freshness windows. +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. +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 { + 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; + }); + + 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); + }); + } + + // 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: + // 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). + CheckOutcome check_lidar_health() const { + if (!scan_received_ || !last_scan_) { + return {false, "no /scan received"}; + } + + 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."; + return {false, oss.str()}; + } + + 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."; + return {true, oss.str()}; + } + + CheckOutcome check_localization_health() const { + // 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 (xy_var <= kLocalizationMaxVar) { + std::ostringstream oss; + 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 absent - never localized"}; + } + + 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() + : 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) { + return {true, "Drivetrain healthy: odometry + joint states streaming, diff-drive active."}; + } + + 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."; + return {false, 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 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) { + return {false, "no /local_costmap/costmap received"}; + } + + 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."; + return {true, oss.str()}; + } + + return {true, "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 run_health_checks_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/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 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..ded0e90 --- /dev/null +++ b/demos/ota_nav2_sensor_fix/ros2_packages/ota_nav2_sensor_fix_demo/CMakeLists.txt @@ -0,0 +1,31 @@ +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 +) + +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/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..ad5e816 --- /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: 4.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..fcd3435 --- /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,47 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 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..ddd937b --- /dev/null +++ b/demos/ota_nav2_sensor_fix/ros2_packages/ota_nav2_sensor_fix_demo/launch/demo.launch.py @@ -0,0 +1,498 @@ +# 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 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 +# --------------- +# 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') + 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' + ) + 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': '', + }], + ) + + # /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. + 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, + # 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. + # /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, + # /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', '/local_costmap/published_footprint', + ], + '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. 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', + 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, + latched_relay, + 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 0000000..6fe698e Binary files /dev/null and b/demos/ota_nav2_sensor_fix/ros2_packages/ota_nav2_sensor_fix_demo/maps/warehouse.pgm differ 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..cd312e0 --- /dev/null +++ b/demos/ota_nav2_sensor_fix/ros2_packages/ota_nav2_sensor_fix_demo/package.xml @@ -0,0 +1,57 @@ + + + + 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 + + rclpy + std_msgs + tf2_msgs + 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/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..eae64f6 --- /dev/null +++ b/demos/ota_nav2_sensor_fix/ros2_packages/ota_nav2_sensor_fix_demo/scripts/latched_relay.py @@ -0,0 +1,102 @@ +#!/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') + # 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, + 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() 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..7d13f8b --- /dev/null +++ b/demos/ota_nav2_sensor_fix/run-demo.sh @@ -0,0 +1,208 @@ +#!/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 + +# 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 "" +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 # send 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..db6a724 --- /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="", + help="Semantic version of the artifact (required for update/install; omit 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..2287de5 --- /dev/null +++ b/demos/ota_nav2_sensor_fix/scripts/test_pack_artifact.py @@ -0,0 +1,338 @@ +"""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_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", + 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..71df8bf --- /dev/null +++ b/demos/ota_nav2_sensor_fix/send-goal.sh @@ -0,0 +1,71 @@ +#!/bin/bash +# 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}" + +# 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. +# 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 time + +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 + +# 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() +PYEOF +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..0b71958 --- /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": true, + "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/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" 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..e42700a --- /dev/null +++ b/tests/smoke_test_demo_narrative.sh @@ -0,0 +1,415 @@ +#!/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 + +# The robot resumes from the tight spot it stopped in after the broken-phase +# recovery, so nav2's first replanning can log one transient "Failed to make +# progress" that the fault_manager latches - a replanning artifact, not the +# fixed sensor. Give it time to get moving, clear that one-off controller fault, +# then assert none REappears. A real sensor regression keeps the controller +# failing (and the ACTION check above already catches the abort), so a bad +# fixed_lidar would put the fault straight back after this clear. +sleep 15 +curl -fsS -X DELETE "${API_BASE}/${CONTROLLER_ENTITY}/faults" -o /dev/null 2>/dev/null || true + +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)" \ + "controller kept failing after the fix - 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..85c5fce --- /dev/null +++ b/tests/smoke_test_ota.sh @@ -0,0 +1,279 @@ +#!/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. 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" + 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" + 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 --- + +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 + +# 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" + +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"