diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 588666e..0432951 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -163,3 +163,75 @@ 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 artifacts (catalog + tarballs) inside ros:jazzy + working-directory: demos/ota_nav2_sensor_fix + run: | + docker run --rm \ + -v "$PWD":/work \ + -w /work \ + ros:jazzy \ + bash -c ' + set -eu + apt-get update + apt-get install -y --no-install-recommends \ + python3-colcon-common-extensions \ + python3-catkin-pkg \ + python3-venv \ + python3-pip \ + build-essential \ + cmake \ + ros-jazzy-rclcpp \ + ros-jazzy-sensor-msgs \ + ros-jazzy-visualization-msgs + cd scripts + python3 -m venv .venv + .venv/bin/pip install --upgrade pip + .venv/bin/pip install pytest + cd .. + ./scripts/build_artifacts.sh + ' + # Restore ownership of files the container created as root. + sudo chown -R "$USER:$USER" . + + - name: Build and start OTA demo + working-directory: demos/ota_nav2_sensor_fix + run: docker compose up -d --build + + - name: Run smoke tests + run: ./tests/smoke_test_ota.sh + + - name: Show gateway logs on failure + if: failure() + working-directory: demos/ota_nav2_sensor_fix + run: docker compose logs gateway --tail=200 + + - name: Show update server logs on failure + if: failure() + working-directory: demos/ota_nav2_sensor_fix + run: docker compose logs ota_update_server --tail=200 + + - name: Teardown + if: always() + working-directory: demos/ota_nav2_sensor_fix + run: docker compose down diff --git a/README.md b/README.md index c21c948..164af8b 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 - update a broken lidar node, install a new safety classifier, uninstall a deprecated package | SOVD-spec update / install / uninstall, native binary swap, fork+exec process management, Foxglove panel + curl scripts | ✅ Ready | ### Quick Start @@ -150,6 +151,32 @@ 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 / Install / Uninstall) + +End-to-end demo of the SOVD `/updates` resource: a broken lidar node is +swapped with a fixed version over HTTP, an extra safety classifier is +installed from scratch, and a deprecated package is uninstalled - 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 +./trigger-update.sh # broken_lidar -> fixed_lidar (the headline) +./trigger-install.sh # install obstacle_classifier_v2 +./trigger-uninstall.sh # remove broken_lidar_legacy +./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 +236,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..2375db4 --- /dev/null +++ b/demos/ota_nav2_sensor_fix/Dockerfile.gateway @@ -0,0 +1,108 @@ +# 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 four demo +# ROS 2 packages into a single ROS 2 Jazzy image. Plugin loads at gateway +# startup via /etc/ros2_medkit/gateway_config.yaml and the entrypoint also +# launches the broken_lidar demo nodes that get swapped/uninstalled at +# runtime by the plugin. + +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 + +# Copy demo packages (broken_lidar, fixed_lidar, broken_lidar_legacy, +# obstacle_classifier_v2) and the OTA plugin from the build context. +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 + +WORKDIR /ws +# rosdep needs the apt cache populated to install gateway dependencies +# (nlohmann-json3-dev, libcpp-httplib-dev, etc.). +RUN apt-get update +RUN . /opt/ros/jazzy/setup.sh && \ + rosdep install --from-paths src --ignore-src -r -y --rosdistro=jazzy && \ + colcon build \ + --cmake-args -DCMAKE_BUILD_TYPE=Release && \ + rm -rf /var/lib/apt/lists/* + + +FROM ros:jazzy + +# Runtime dependencies. Beyond the gateway/plugin bare minimum we also pull in +# TurtleBot3, Nav2, and gz-sim so the container can self-host the visual demo +# (TB3 + headless Gazebo + Nav2) - no external sim required, the OTA story +# becomes "Foxglove sees a stuck robot, run an update, robot unsticks". +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-turtlebot3-gazebo \ + ros-jazzy-turtlebot3-msgs \ + ros-jazzy-turtlebot3-description \ + ros-jazzy-turtlebot3-navigation2 \ + 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 \ + libcpp-httplib-dev \ + libsystemd-dev \ + nlohmann-json3-dev \ + curl \ + procps \ + && rm -rf /var/lib/apt/lists/* + +COPY --from=builder /ws/install /ws/install +COPY gateway_config.yaml /etc/ros2_medkit/gateway_config.yaml +COPY entrypoint.sh /usr/local/bin/entrypoint.sh +RUN chmod +x /usr/local/bin/entrypoint.sh + +# Default TB3 model + gz-sim resource path so spawn_turtlebot3 + gz can find +# the burger URDF/world models without the launch file having to set them. +# 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. +ENV ROS_DOMAIN_ID=42 \ + TURTLEBOT3_MODEL=burger \ + GAZEBO_MODEL_PATH=/opt/ros/jazzy/share/turtlebot3_gazebo/models \ + 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..2686a56 --- /dev/null +++ b/demos/ota_nav2_sensor_fix/README.md @@ -0,0 +1,109 @@ +# OTA over SOVD - nav2 sensor fix demo + +End-to-end demo: a `ros2_medkit` gateway with a dev-grade OTA plugin that +demonstrates the full Update / Install / Uninstall lifecycle on ROS 2 nodes +without SSH-ing into the robot. + +## What this shows + +Three things you can do to a ROS 2 robot over the air: + +1. **Update** - swap a running sensor node with a fixed version (the + `broken_lidar` -> `fixed_lidar` flip). +2. **Install** - pull and start a new ROS 2 package + (`obstacle_classifier_v2`). +3. **Uninstall** - stop and remove a deprecated package + (`broken_lidar_legacy`). + +All three operations are SOVD ISO 17978-3 compliant - the kind is derived +from `updated_components` / `added_components` / `removed_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 TurtleBot3 + Nav2 + gz-sim +runtime (~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 # show registered updates + live process state +./trigger-update.sh # broken_lidar -> fixed_lidar (the headline scene) +./trigger-install.sh # install obstacle_classifier_v2 from scratch +./trigger-uninstall.sh # remove broken_lidar_legacy +./stop-demo.sh # tear down +``` + +Each trigger script issues SOVD `PUT /updates/{id}/prepare` then `/execute` +and prints the resulting status plus the live process list. + +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`. + +## Foxglove Studio visualization + +The gateway container bakes in a TurtleBot3 burger + Nav2 stack running on +top of headless Gazebo. `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 world 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. You should see the TB3 burger sitting in the + default `turtlebot3_world.world` map, with the laser scan cone visible. + Before the OTA update, ray index 180 reports a phantom 1 m return - the + "obstacle" 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 **ros2_medkit Updates** panel and set its `baseUrl` to + `http://localhost:8080/api/v1` (or the port you picked via + `OTA_GATEWAY_PORT`). Click **Prepare** then **Execute** for + `fixed_lidar_2_1_0`. The 3D panel should show the phantom return + disappearing as `broken_lidar` is killed and `fixed_lidar` starts. + +### Driving the robot to make the narrative reproducible + +The demo doesn't auto-publish a navigation goal - that keeps it deterministic +for CI. To trigger the "robot stuck on phantom obstacle" beat manually: + +```bash +# From the host (or any container on the same ROS_DOMAIN_ID=42): +ros2 topic pub --once /goal_pose geometry_msgs/PoseStamped \ + '{header: {frame_id: map}, pose: {position: {x: 1.5, y: 0.0, z: 0.0}, orientation: {w: 1.0}}}' +``` + +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. Before the update, Nav2 refuses to drive through the phantom return; +after `trigger-update.sh`, the robot completes the goal. + +## Disclosures + +This is **dev-grade** OTA. Deliberately missing for production: + +- No artifact signing or signature verification +- No atomic swap (in-place overwrite) +- No A/B partition rollout +- No fleet-wide staged rollout +- No persistent update state across gateway restarts +- No automated health-gated rollback policy +- No audit log + +Perfect for: prototypes, lab robots, internal demos, dev environments. + +For production-grade OTA (rollout safety, signing, A/B partitions, +fleet-aware staging), reach out. diff --git a/demos/ota_nav2_sensor_fix/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..40e49fa --- /dev/null +++ b/demos/ota_nav2_sensor_fix/check-demo.sh @@ -0,0 +1,56 @@ +#!/bin/bash +# Show the live state of the OTA demo: registered updates, per-update +# status, and the demo node processes the plugin manages inside the +# gateway container. + +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 + +echo "Gateway: ${GATEWAY_URL}" +echo "Health: $(curl -fsS "${API}/health" | head -c 200)" +echo "" + +echo "Registered updates (GET /updates):" +if [[ "$JQ_AVAILABLE" == "true" ]]; then + curl -fsS "${API}/updates" | jq -r '.items[]' | sed 's/^/ /' +else + curl -fsS "${API}/updates" +fi +echo "" + +echo "Per-update status (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 + echo " (install jq for detail)" +fi +echo "" + +echo "Plugin-managed processes inside gateway container:" +if docker ps --format '{{.Names}}' | grep -q '^ota_demo_gateway$'; then + docker exec ota_demo_gateway pgrep -af \ + 'broken_lidar_node|fixed_lidar_node|broken_lidar_legacy|obstacle_classifier' \ + 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/docker-compose.yml b/demos/ota_nav2_sensor_fix/docker-compose.yml new file mode 100644 index 0000000..f3d9cf3 --- /dev/null +++ b/demos/ota_nav2_sensor_fix/docker-compose.yml @@ -0,0 +1,45 @@ +# Copyright 2026 bburda +# Apache 2.0 +# +# Two-service stack: the gateway (with ota_update_plugin baked in plus the +# nav2 + TB3 + 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" + TURTLEBOT3_MODEL: burger + # Gazebo / DDS appreciate generous shared memory; without this + # /dev/shm fills and gz-sim tends to wedge on shutdown. + shm_size: "2gb" + tty: true + stdin_open: true + depends_on: + - ota_update_server + + ota_update_server: + image: selfpatch/ota_update_server:dev + build: + context: . + dockerfile: ota_update_server/Dockerfile + container_name: ota_demo_update_server + networks: [otanet] + ports: + - "9000:9000" + +networks: + otanet: + driver: bridge diff --git a/demos/ota_nav2_sensor_fix/entrypoint.sh b/demos/ota_nav2_sensor_fix/entrypoint.sh new file mode 100755 index 0000000..916c5aa --- /dev/null +++ b/demos/ota_nav2_sensor_fix/entrypoint.sh @@ -0,0 +1,20 @@ +#!/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 (TB3 + Nav2 + headless Gazebo + foxglove_bridge +# + fault_manager + broken_lidar/legacy + gateway w/ ota_update_plugin). + +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}" + +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..3cbda32 --- /dev/null +++ b/demos/ota_nav2_sensor_fix/gateway_config.yaml @@ -0,0 +1,38 @@ +# 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: + mode: "runtime_only" + + # 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" 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..7e67c0b --- /dev/null +++ b/demos/ota_nav2_sensor_fix/ota_update_plugin/include/ota_update_plugin/ota_update_plugin.hpp @@ -0,0 +1,88 @@ +// 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 +// UpdateProvider lives at providers/ in newer gateway revisions and updates/ in older ones. +#if __has_include() +#include +#else +#include +#endif + +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: + std::string catalog_url_; + std::string staging_dir_; + std::string install_dir_; + + std::mutex mu_; + std::map registry_; + std::map staged_artifacts_; + + std::unique_ptr catalog_client_; + std::unique_ptr process_runner_; +}; + +} // namespace ota_update_plugin diff --git a/demos/ota_nav2_sensor_fix/ota_update_plugin/package.xml b/demos/ota_nav2_sensor_fix/ota_update_plugin/package.xml new file mode 100644 index 0000000..200c826 --- /dev/null +++ b/demos/ota_nav2_sensor_fix/ota_update_plugin/package.xml @@ -0,0 +1,20 @@ + + + ota_update_plugin + 0.1.0 + Dev-grade OTA plugin for ros2_medkit gateway: update / install / uninstall via simple HTTP catalog. + bburda + Apache-2.0 + + ament_cmake + + ros2_medkit_cmake + ros2_medkit_gateway + nlohmann-json-dev + + ament_cmake_gtest + + + ament_cmake + + diff --git a/demos/ota_nav2_sensor_fix/ota_update_plugin/src/catalog_client.cpp b/demos/ota_nav2_sensor_fix/ota_update_plugin/src/catalog_client.cpp new file mode 100644 index 0000000..1849025 --- /dev/null +++ b/demos/ota_nav2_sensor_fix/ota_update_plugin/src/catalog_client.cpp @@ -0,0 +1,171 @@ +// Copyright 2026 bburda +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "catalog_client.hpp" + +#include +#include +#include + +#include + +namespace ota_update_plugin { + +namespace { + +bool starts_with(const std::string & s, const std::string & prefix) { + return s.size() >= prefix.size() && s.compare(0, prefix.size(), prefix) == 0; +} + +} // namespace + +ParsedUrl parse_url(const std::string & url) { + ParsedUrl out{}; + std::string rest; + if (starts_with(url, "https://")) { + out.tls = true; + out.port = 443; + rest = url.substr(8); + } else if (starts_with(url, "http://")) { + out.tls = false; + out.port = 80; + rest = url.substr(7); + } else { + throw std::invalid_argument("unsupported URL scheme: " + url); + } + + // Split host[:port] from path. + const auto slash = rest.find('/'); + std::string authority; + if (slash == std::string::npos) { + authority = rest; + out.path = "/"; + } else { + authority = rest.substr(0, slash); + out.path = rest.substr(slash); + } + + // Split host from port if present. + const auto colon = authority.find(':'); + if (colon == std::string::npos) { + out.host = authority; + } else { + out.host = authority.substr(0, colon); + try { + out.port = std::stoi(authority.substr(colon + 1)); + } catch (const std::exception & e) { + throw std::invalid_argument(std::string("invalid port in URL: ") + url + " (" + e.what() + ")"); + } + } + + if (out.host.empty()) { + throw std::invalid_argument("missing host in URL: " + url); + } + return out; +} + +CatalogClient::CatalogClient(std::string base_url) : base_url_(std::move(base_url)) { +} + +tl::expected CatalogClient::fetch_catalog() { + ParsedUrl parsed; + try { + parsed = parse_url(base_url_); + } catch (const std::exception & e) { + return tl::make_unexpected(std::string("invalid catalog url: ") + e.what()); + } + + if (parsed.tls) { + return tl::make_unexpected("https not supported by demo CatalogClient"); + } + + // Strip trailing slash from base path, then append /catalog. + std::string base_path = parsed.path; + if (!base_path.empty() && base_path.back() == '/') { + base_path.pop_back(); + } + const std::string target = base_path + "/catalog"; + + httplib::Client cli(parsed.host, parsed.port); + cli.set_connection_timeout(5, 0); + cli.set_read_timeout(5, 0); + + auto res = cli.Get(target.c_str()); + if (!res) { + return tl::make_unexpected("catalog GET failed: " + httplib::to_string(res.error())); + } + if (res->status < 200 || res->status >= 300) { + return tl::make_unexpected("catalog GET returned status " + std::to_string(res->status)); + } + try { + return nlohmann::json::parse(res->body); + } catch (const std::exception & e) { + return tl::make_unexpected(std::string("catalog json parse failed: ") + e.what()); + } +} + +tl::expected CatalogClient::download_artifact(const std::string & url_or_path, + const std::string & out_path) { + // If url_or_path is an absolute URL, parse it directly. Otherwise treat as a + // path relative to base_url_. + std::string full_url; + if (starts_with(url_or_path, "http://") || starts_with(url_or_path, "https://")) { + full_url = url_or_path; + } else { + std::string base = base_url_; + // Strip trailing slash on base, leading slash on relative path. + while (!base.empty() && base.back() == '/') { + base.pop_back(); + } + std::string rel = url_or_path; + if (rel.empty() || rel.front() != '/') { + rel = "/" + rel; + } + full_url = base + rel; + } + + ParsedUrl parsed; + try { + parsed = parse_url(full_url); + } catch (const std::exception & e) { + return tl::make_unexpected(std::string("invalid artifact url: ") + e.what()); + } + if (parsed.tls) { + return tl::make_unexpected("https not supported by demo CatalogClient"); + } + + httplib::Client cli(parsed.host, parsed.port); + cli.set_connection_timeout(5, 0); + cli.set_read_timeout(30, 0); + + auto res = cli.Get(parsed.path.c_str()); + if (!res) { + return tl::make_unexpected("artifact GET failed: " + httplib::to_string(res.error())); + } + if (res->status < 200 || res->status >= 300) { + return tl::make_unexpected("artifact GET returned status " + std::to_string(res->status)); + } + + std::ofstream o(out_path, std::ios::binary); + if (!o) { + return tl::make_unexpected("cannot open output file: " + out_path); + } + o.write(res->body.data(), static_cast(res->body.size())); + if (!o) { + return tl::make_unexpected("write to output file failed: " + out_path); + } + return out_path; +} + +} // namespace ota_update_plugin diff --git a/demos/ota_nav2_sensor_fix/ota_update_plugin/src/catalog_client.hpp b/demos/ota_nav2_sensor_fix/ota_update_plugin/src/catalog_client.hpp new file mode 100644 index 0000000..0b8a9b2 --- /dev/null +++ b/demos/ota_nav2_sensor_fix/ota_update_plugin/src/catalog_client.hpp @@ -0,0 +1,61 @@ +// Copyright 2026 bburda +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include + +#include +#include + +namespace ota_update_plugin { + +/// Decomposed URL components used by the HTTP client. +struct ParsedUrl { + std::string host; + int port; + bool tls; + std::string path; +}; + +/// Parse an http:// or https:// URL into components. +/// Throws std::invalid_argument for unsupported schemes. +ParsedUrl parse_url(const std::string & url); + +/// HTTP client that fetches the FastAPI catalog and downloads artifacts. +/// Virtual methods so tests can substitute a fake without touching real HTTP. +class CatalogClient { + public: + explicit CatalogClient(std::string base_url); + virtual ~CatalogClient() = default; + + CatalogClient(const CatalogClient &) = delete; + CatalogClient & operator=(const CatalogClient &) = delete; + CatalogClient(CatalogClient &&) = delete; + CatalogClient & operator=(CatalogClient &&) = delete; + + /// GET {base_url}/catalog and parse JSON. Returns the JSON array on success. + virtual tl::expected fetch_catalog(); + + /// Download an artifact. `url_or_path` may be either an absolute URL or a + /// path (interpreted relative to `base_url`). Body is written to `out_path`. + /// Returns the absolute output path on success. + virtual tl::expected download_artifact(const std::string & url_or_path, + const std::string & out_path); + + protected: + std::string base_url_; +}; + +} // namespace ota_update_plugin diff --git a/demos/ota_nav2_sensor_fix/ota_update_plugin/src/operation_dispatcher.cpp b/demos/ota_nav2_sensor_fix/ota_update_plugin/src/operation_dispatcher.cpp new file mode 100644 index 0000000..b28b2af --- /dev/null +++ b/demos/ota_nav2_sensor_fix/ota_update_plugin/src/operation_dispatcher.cpp @@ -0,0 +1,48 @@ +// Copyright 2026 bburda +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "operation_dispatcher.hpp" + +namespace ota_update_plugin { + +namespace { + +bool non_empty_array(const nlohmann::json & j, const char * key) { + if (!j.contains(key)) { + return false; + } + const auto & v = j.at(key); + return v.is_array() && !v.empty(); +} + +} // namespace + +OperationKind OperationDispatcher::classify(const nlohmann::json & metadata) { + const bool has_updated = non_empty_array(metadata, "updated_components"); + const bool has_added = non_empty_array(metadata, "added_components"); + const bool has_removed = non_empty_array(metadata, "removed_components"); + const int populated = static_cast(has_updated) + static_cast(has_added) + static_cast(has_removed); + if (populated != 1) { + return OperationKind::Unknown; + } + if (has_updated) { + return OperationKind::Update; + } + if (has_added) { + return OperationKind::Install; + } + return OperationKind::Uninstall; +} + +} // namespace ota_update_plugin diff --git a/demos/ota_nav2_sensor_fix/ota_update_plugin/src/operation_dispatcher.hpp b/demos/ota_nav2_sensor_fix/ota_update_plugin/src/operation_dispatcher.hpp new file mode 100644 index 0000000..3398c2e --- /dev/null +++ b/demos/ota_nav2_sensor_fix/ota_update_plugin/src/operation_dispatcher.hpp @@ -0,0 +1,33 @@ +// Copyright 2026 bburda +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include + +namespace ota_update_plugin { + +/// Operation kind classified from SOVD update metadata. +enum class OperationKind { Update, Install, Uninstall, Unknown }; + +/// Maps SOVD update metadata to a concrete operation kind based on which of +/// updated_components / added_components / removed_components is populated. +class OperationDispatcher { + public: + /// Classify an update's operation kind. Returns Unknown if zero or more + /// than one of the three component arrays is non-empty. + static OperationKind classify(const nlohmann::json & metadata); +}; + +} // namespace ota_update_plugin diff --git a/demos/ota_nav2_sensor_fix/ota_update_plugin/src/ota_update_plugin.cpp b/demos/ota_nav2_sensor_fix/ota_update_plugin/src/ota_update_plugin.cpp new file mode 100644 index 0000000..f081a10 --- /dev/null +++ b/demos/ota_nav2_sensor_fix/ota_update_plugin/src/ota_update_plugin.cpp @@ -0,0 +1,296 @@ +// 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 "catalog_client.hpp" +#include "operation_dispatcher.hpp" +#include "process_runner.hpp" + +namespace ota_update_plugin { + +namespace fs = std::filesystem; +using ros2_medkit_gateway::UpdateBackendError; +using ros2_medkit_gateway::UpdateBackendErrorInfo; + +namespace { + +/// Extract a packed tarball into a staging directory, then atomically replace +/// `${install_dir}/${target_package}` with the freshly extracted contents. +/// The artifacts are produced by pack_artifact.py and contain a single +/// top-level directory named after the target package. +tl::expected extract_and_swap(const std::string & staged_tarball, const std::string & install_dir, + const std::string & target_package) { + if (target_package.empty()) { + return tl::make_unexpected("target_package is empty"); + } + const std::string staging_extracted = staged_tarball + ".extracted"; + std::error_code ec; + fs::remove_all(staging_extracted, ec); + fs::create_directories(staging_extracted, ec); + + const std::string cmd = "tar -xzf " + staged_tarball + " -C " + staging_extracted; + if (std::system(cmd.c_str()) != 0) { + return tl::make_unexpected("tar extraction failed: " + cmd); + } + + const std::string source = staging_extracted + "/" + target_package; + if (!fs::exists(source)) { + return tl::make_unexpected("artifact missing top-level directory '" + target_package + "' after extraction"); + } + + fs::create_directories(install_dir, ec); + const std::string target = install_dir + "/" + target_package; + fs::remove_all(target, ec); + fs::copy(source, target, fs::copy_options::recursive | fs::copy_options::overwrite_existing, ec); + if (ec) { + return tl::make_unexpected("copy failed: " + ec.message()); + } + return {}; +} + +} // namespace + +OtaUpdatePlugin::OtaUpdatePlugin() : process_runner_(std::make_unique()) { +} + +OtaUpdatePlugin::~OtaUpdatePlugin() = default; + +void OtaUpdatePlugin::configure(const nlohmann::json & config) { + catalog_url_ = config.value("catalog_url", "http://ota_update_server:9000"); + staging_dir_ = config.value("staging_dir", "/tmp/ota_staging"); + install_dir_ = config.value("install_dir", "/ws/install"); + if (!catalog_client_) { + catalog_client_ = std::make_unique(catalog_url_); + } +} + +void OtaUpdatePlugin::set_context(ros2_medkit_gateway::PluginContext & /*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 it->second; +} + +tl::expected OtaUpdatePlugin::register_update(const nlohmann::json & metadata) { + if (!metadata.contains("id") || !metadata["id"].is_string()) { + return tl::make_unexpected(UpdateBackendErrorInfo{UpdateBackendError::InvalidInput, "metadata missing id"}); + } + std::lock_guard lk(mu_); + registry_[metadata["id"].get()] = metadata; + return {}; +} + +tl::expected OtaUpdatePlugin::delete_update(const std::string & id) { + std::lock_guard lk(mu_); + registry_.erase(id); + staged_artifacts_.erase(id); + return {}; +} + +tl::expected OtaUpdatePlugin::prepare( + const std::string & id, ros2_medkit_gateway::UpdateProgressReporter & reporter) { + nlohmann::json metadata; + { + std::lock_guard lk(mu_); + auto it = registry_.find(id); + if (it == registry_.end()) { + return tl::make_unexpected(UpdateBackendErrorInfo{UpdateBackendError::NotFound, "no such update"}); + } + metadata = it->second; + } + + const auto kind = OperationDispatcher::classify(metadata); + if (kind == OperationKind::Unknown) { + return tl::make_unexpected(UpdateBackendErrorInfo{ + UpdateBackendError::InvalidInput, + "update package must populate exactly one of " + "updated_components / added_components / removed_components"}); + } + + if (kind == OperationKind::Uninstall) { + reporter.set_progress(100); + return {}; + } + + if (!metadata.contains("x_medkit_artifact_url") || !metadata["x_medkit_artifact_url"].is_string()) { + return tl::make_unexpected( + UpdateBackendErrorInfo{UpdateBackendError::InvalidInput, "missing x_medkit_artifact_url"}); + } + + std::error_code ec; + fs::create_directories(staging_dir_, ec); + const std::string url = metadata["x_medkit_artifact_url"].get(); + const std::string staged_path = staging_dir_ + "/" + id + ".tar.gz"; + + reporter.set_progress(10); + auto dl = catalog_client_->download_artifact(url, staged_path); + if (!dl) { + return tl::make_unexpected(UpdateBackendErrorInfo{UpdateBackendError::Internal, "download failed: " + dl.error()}); + } + reporter.set_progress(80); + + { + std::lock_guard lk(mu_); + staged_artifacts_[id] = *dl; + } + reporter.set_progress(100); + return {}; +} + +tl::expected OtaUpdatePlugin::execute( + const std::string & id, ros2_medkit_gateway::UpdateProgressReporter & reporter) { + nlohmann::json metadata; + std::string staged; + { + std::lock_guard lk(mu_); + auto it = registry_.find(id); + if (it == registry_.end()) { + return tl::make_unexpected(UpdateBackendErrorInfo{UpdateBackendError::NotFound, "no such update"}); + } + metadata = it->second; + auto sit = staged_artifacts_.find(id); + staged = (sit != staged_artifacts_.end()) ? sit->second : ""; + } + + const auto kind = OperationDispatcher::classify(metadata); + const std::string target_package = metadata.value("x_medkit_target_package", ""); + const std::string executable = metadata.value("x_medkit_executable", ""); + + if (kind == OperationKind::Update) { + if (staged.empty()) { + return tl::make_unexpected(UpdateBackendErrorInfo{UpdateBackendError::InvalidInput, "call prepare() first"}); + } + if (executable.empty()) { + return tl::make_unexpected( + UpdateBackendErrorInfo{UpdateBackendError::InvalidInput, "missing x_medkit_executable"}); + } + // For an update across packages (e.g. broken_lidar -> fixed_lidar) the + // OLD process binary lives in a different package than the NEW one we + // are about to spawn, so its basename differs from `executable`. Honor + // x_medkit_replaces_executable when present, fall back to executable. + const std::string kill_target = metadata.value("x_medkit_replaces_executable", executable); + reporter.set_progress(20); + auto kr = process_runner_->kill_by_executable(kill_target); + if (!kr) { + return tl::make_unexpected(UpdateBackendErrorInfo{UpdateBackendError::Internal, "kill failed: " + kr.error()}); + } + reporter.set_progress(40); + if (auto sw = extract_and_swap(staged, install_dir_, target_package); !sw) { + return tl::make_unexpected(UpdateBackendErrorInfo{UpdateBackendError::Internal, "swap failed: " + sw.error()}); + } + reporter.set_progress(70); + const std::string bin = install_dir_ + "/" + target_package + "/lib/" + target_package + "/" + executable; + auto sp = process_runner_->spawn(bin); + if (!sp) { + return tl::make_unexpected(UpdateBackendErrorInfo{UpdateBackendError::Internal, "spawn failed: " + sp.error()}); + } + reporter.set_progress(100); + return {}; + } + + if (kind == OperationKind::Install) { + if (staged.empty()) { + return tl::make_unexpected(UpdateBackendErrorInfo{UpdateBackendError::InvalidInput, "call prepare() first"}); + } + if (executable.empty()) { + return tl::make_unexpected( + UpdateBackendErrorInfo{UpdateBackendError::InvalidInput, "missing x_medkit_executable"}); + } + reporter.set_progress(30); + if (auto sw = extract_and_swap(staged, install_dir_, target_package); !sw) { + return tl::make_unexpected(UpdateBackendErrorInfo{UpdateBackendError::Internal, "swap failed: " + sw.error()}); + } + reporter.set_progress(70); + const std::string bin = install_dir_ + "/" + target_package + "/lib/" + target_package + "/" + executable; + auto sp = process_runner_->spawn(bin); + if (!sp) { + return tl::make_unexpected(UpdateBackendErrorInfo{UpdateBackendError::Internal, "spawn failed: " + sp.error()}); + } + reporter.set_progress(100); + return {}; + } + + if (kind == OperationKind::Uninstall) { + reporter.set_progress(30); + if (!target_package.empty()) { + // Best-effort kill: legacy nodes may use the package name as their executable basename. + // Failures are tolerated since the process may already be gone. + auto kr = process_runner_->kill_by_executable(target_package); + (void)kr; + reporter.set_progress(70); + std::error_code ec; + fs::remove_all(install_dir_ + "/" + target_package, ec); + } + reporter.set_progress(100); + return {}; + } + + return tl::make_unexpected(UpdateBackendErrorInfo{UpdateBackendError::Internal, "unknown operation kind"}); +} + +tl::expected OtaUpdatePlugin::supports_automated(const std::string & /*id*/) { + return false; +} + +} // namespace 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..3a8c104 --- /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/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..8ec65e9 --- /dev/null +++ b/demos/ota_nav2_sensor_fix/ota_update_plugin/src/process_runner.cpp @@ -0,0 +1,153 @@ +// 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 + +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'; +} + +} // namespace + +std::vector ProcessRunner::pgrep(const std::string & executable_basename) { + std::vector out; + DIR * d = opendir("/proc"); + if (d == nullptr) { + return out; + } + while (auto * ent = readdir(d)) { + if (!is_pid_dir(ent->d_name)) { + continue; + } + const int pid = std::atoi(ent->d_name); + if (pid <= 0) { + continue; + } + if (proc_cmdline_arg0(pid) == executable_basename) { + out.push_back(pid); + } + } + closedir(d); + return out; +} + +tl::expected ProcessRunner::kill_by_executable(const std::string & executable_basename, + int timeout_ms) { + const auto pids = pgrep(executable_basename); + int signalled = 0; + for (int pid : pids) { + if (::kill(pid, SIGTERM) == 0) { + ++signalled; + } + } + if (signalled == 0) { + return 0; + } + + // Poll for exit. + const auto deadline = std::chrono::steady_clock::now() + std::chrono::milliseconds(timeout_ms); + while (std::chrono::steady_clock::now() < deadline) { + bool any_alive = false; + for (int pid : pids) { + if (::kill(pid, 0) == 0) { + any_alive = true; + break; + } + } + if (!any_alive) { + return signalled; + } + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + } + // Force-kill stragglers. + for (int pid : pids) { + if (::kill(pid, 0) == 0) { + ::kill(pid, SIGKILL); + } + } + return signalled; +} + +tl::expected ProcessRunner::spawn(const std::string & executable_path) { + // Double-fork so the grandchild is reparented to init and never becomes a + // zombie in the gateway process. The intermediate child exits immediately + // and is reaped here. + pid_t pid = fork(); + if (pid < 0) { + return tl::make_unexpected(std::string("fork failed: ") + std::strerror(errno)); + } + if (pid == 0) { + pid_t grandchild = fork(); + if (grandchild < 0) { + _exit(126); + } + if (grandchild == 0) { + setsid(); + execl(executable_path.c_str(), executable_path.c_str(), nullptr); + std::fprintf(stderr, "execl %s failed: %s\n", executable_path.c_str(), + std::strerror(errno)); + _exit(127); + } + _exit(0); + } + int status = 0; + ::waitpid(pid, &status, 0); + return static_cast(pid); +} + +} // namespace ota_update_plugin diff --git a/demos/ota_nav2_sensor_fix/ota_update_plugin/src/process_runner.hpp b/demos/ota_nav2_sensor_fix/ota_update_plugin/src/process_runner.hpp new file mode 100644 index 0000000..70e88cb --- /dev/null +++ b/demos/ota_nav2_sensor_fix/ota_update_plugin/src/process_runner.hpp @@ -0,0 +1,48 @@ +// Copyright 2026 bburda +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include +#include + +#include + +namespace ota_update_plugin { + +/// Process management helper for OTA operations: locate, terminate, and spawn +/// demo nodes by executable basename. Pure-virtual for test substitution. +class ProcessRunner { + public: + ProcessRunner() = default; + virtual ~ProcessRunner() = default; + + ProcessRunner(const ProcessRunner &) = delete; + ProcessRunner & operator=(const ProcessRunner &) = delete; + ProcessRunner(ProcessRunner &&) = delete; + ProcessRunner & operator=(ProcessRunner &&) = delete; + + /// Find PIDs of processes whose /proc//comm matches the given basename. + virtual std::vector pgrep(const std::string & executable_basename); + + /// Send SIGTERM to all matching PIDs, wait up to `timeout_ms` for exit, then + /// SIGKILL any stragglers. Returns the number of processes that were signalled. + virtual tl::expected kill_by_executable(const std::string & executable_basename, + int timeout_ms = 2000); + + /// fork+exec the executable at `executable_path`. Returns child PID or error. + virtual tl::expected spawn(const std::string & executable_path); +}; + +} // namespace ota_update_plugin diff --git a/demos/ota_nav2_sensor_fix/ota_update_plugin/test/test_catalog_client.cpp b/demos/ota_nav2_sensor_fix/ota_update_plugin/test/test_catalog_client.cpp new file mode 100644 index 0000000..2a671e3 --- /dev/null +++ b/demos/ota_nav2_sensor_fix/ota_update_plugin/test/test_catalog_client.cpp @@ -0,0 +1,59 @@ +// Copyright 2026 bburda +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include + +#include "catalog_client.hpp" + +using ota_update_plugin::parse_url; + +TEST(ParseUrl, HostAndPort) { + auto p = parse_url("http://server:9000"); + EXPECT_EQ(p.host, "server"); + EXPECT_EQ(p.port, 9000); + EXPECT_FALSE(p.tls); + EXPECT_EQ(p.path, "/"); +} + +TEST(ParseUrl, PathSplit) { + auto p = parse_url("http://server:9000/catalog"); + EXPECT_EQ(p.host, "server"); + EXPECT_EQ(p.port, 9000); + EXPECT_EQ(p.path, "/catalog"); +} + +TEST(ParseUrl, DefaultsHttpPort) { + auto p = parse_url("http://server/catalog"); + EXPECT_EQ(p.host, "server"); + EXPECT_EQ(p.port, 80); + EXPECT_FALSE(p.tls); + EXPECT_EQ(p.path, "/catalog"); +} + +TEST(ParseUrl, HttpsTls) { + auto p = parse_url("https://server/catalog"); + EXPECT_TRUE(p.tls); + EXPECT_EQ(p.port, 443); + EXPECT_EQ(p.path, "/catalog"); +} + +TEST(ParseUrl, RejectsInvalidScheme) { + EXPECT_THROW(parse_url("ftp://server/foo"), std::invalid_argument); +} + +TEST(ParseUrl, RejectsMissingHost) { + EXPECT_THROW(parse_url("http://:9000/foo"), std::invalid_argument); +} diff --git a/demos/ota_nav2_sensor_fix/ota_update_plugin/test/test_operation_dispatcher.cpp b/demos/ota_nav2_sensor_fix/ota_update_plugin/test/test_operation_dispatcher.cpp new file mode 100644 index 0000000..453346a --- /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", {"obstacle_classifier"}}}; + EXPECT_EQ(OperationDispatcher::classify(m), OperationKind::Install); +} + +TEST(OperationDispatcher, UninstallFromRemovedComponents) { + nlohmann::json m = {{"id", "x"}, {"removed_components", {"broken_lidar_legacy"}}}; + 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..438bba7 --- /dev/null +++ b/demos/ota_nav2_sensor_fix/ota_update_plugin/test/test_plugin_smoke.cpp @@ -0,0 +1,224 @@ +// Copyright 2026 bburda +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include +#include + +#include + +#include + +#include "catalog_client.hpp" +#include "ota_update_plugin/ota_update_plugin.hpp" +#include "process_runner.hpp" + +namespace { + +class FakeCatalogClient : public ota_update_plugin::CatalogClient { + public: + using CatalogClient::CatalogClient; + + nlohmann::json catalog_payload = nlohmann::json::array(); + std::string artifact_to_return = "TARDATA"; + std::string requested_url; + + tl::expected fetch_catalog() override { + return catalog_payload; + } + + tl::expected download_artifact(const std::string & url, const std::string & out) override { + requested_url = url; + std::ofstream o(out, std::ios::binary); + o << artifact_to_return; + return out; + } +}; + +/// ProcessRunner stub: records the basename passed to kill_by_executable so a +/// test can verify the plugin honors x_medkit_replaces_executable. Returns 0 +/// signalled processes (no-op kill) and an error from spawn so execute() halts +/// before touching the (nonexistent) install dir. +class RecordingProcessRunner : public ota_update_plugin::ProcessRunner { + public: + std::string last_kill_target; + + std::vector pgrep(const std::string & /*executable_basename*/) override { + return {}; + } + + tl::expected kill_by_executable(const std::string & executable_basename, + int /*timeout_ms*/ = 2000) override { + last_kill_target = executable_basename; + return 0; + } + + tl::expected spawn(const std::string & /*executable_path*/) override { + return tl::make_unexpected(std::string("stub: spawn intentionally not implemented")); + } +}; + +ros2_medkit_gateway::UpdateProgressReporter make_reporter(ros2_medkit_gateway::UpdateStatusInfo & info, + std::mutex & mu) { + return ros2_medkit_gateway::UpdateProgressReporter(info, mu); +} + +} // namespace + +TEST(OtaUpdatePluginSmoke, NameAndConstructible) { + ota_update_plugin::OtaUpdatePlugin plugin; + EXPECT_EQ(plugin.name(), "ota_update_plugin"); +} + +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)["id"], "u1"); +} + +TEST(OtaUpdatePluginSmoke, RegisterRequiresId) { + ota_update_plugin::OtaUpdatePlugin plugin; + auto rc = plugin.register_update(nlohmann::json::object()); + EXPECT_FALSE(rc); + EXPECT_EQ(rc.error().code, ros2_medkit_gateway::UpdateBackendError::InvalidInput); +} + +TEST(OtaUpdatePluginSmoke, GetUpdateReturnsNotFoundForUnknownId) { + ota_update_plugin::OtaUpdatePlugin plugin; + auto got = plugin.get_update("does-not-exist"); + ASSERT_FALSE(got); + EXPECT_EQ(got.error().code, ros2_medkit_gateway::UpdateBackendError::NotFound); +} + +TEST(OtaUpdatePluginSmoke, DeleteRemovesEntry) { + ota_update_plugin::OtaUpdatePlugin plugin; + ASSERT_TRUE(plugin.register_update({{"id", "to-delete"}, {"updated_components", {"x"}}})); + ASSERT_TRUE(plugin.delete_update("to-delete")); + auto got = plugin.get_update("to-delete"); + EXPECT_FALSE(got); +} + +TEST(OtaUpdatePluginSmoke, BootPollPopulates) { + ota_update_plugin::OtaUpdatePlugin plugin; + plugin.configure(nlohmann::json::object()); + auto fake = std::make_unique("http://x"); + fake->catalog_payload = nlohmann::json::array({ + {{"id", "a"}, + {"updated_components", {"scan"}}, + {"x_medkit_artifact_url", "/artifacts/a.tgz"}, + {"x_medkit_target_package", "a"}}, + }); + plugin.set_catalog_client_for_test(std::move(fake)); + plugin.poll_and_register_catalog(); + + auto ids = plugin.list_updates({}); + ASSERT_TRUE(ids); + ASSERT_EQ(ids->size(), 1u); + EXPECT_EQ((*ids)[0], "a"); +} + +TEST(OtaUpdatePluginSmoke, PrepareRejectsUnknownOperationKind) { + ota_update_plugin::OtaUpdatePlugin plugin; + ASSERT_TRUE(plugin.register_update({{"id", "bad"}})); + ros2_medkit_gateway::UpdateStatusInfo info; + std::mutex mu; + auto reporter = make_reporter(info, mu); + auto rc = plugin.prepare("bad", reporter); + ASSERT_FALSE(rc); + EXPECT_EQ(rc.error().code, ros2_medkit_gateway::UpdateBackendError::InvalidInput); +} + +TEST(OtaUpdatePluginSmoke, PrepareUninstallSkipsDownload) { + ota_update_plugin::OtaUpdatePlugin plugin; + plugin.configure(nlohmann::json::object()); + // No download should happen for uninstall, but provide a fake just in case. + auto fake = std::make_unique("http://x"); + plugin.set_catalog_client_for_test(std::move(fake)); + ASSERT_TRUE(plugin.register_update({{"id", "rm"}, {"removed_components", {"legacy"}}})); + + ros2_medkit_gateway::UpdateStatusInfo info; + std::mutex mu; + auto reporter = make_reporter(info, mu); + auto rc = plugin.prepare("rm", reporter); + EXPECT_TRUE(rc); + EXPECT_EQ(info.progress.value_or(-1), 100); +} + +TEST(OtaUpdatePluginSmoke, ExecuteUpdateUsesReplacesExecutableForKill) { + ota_update_plugin::OtaUpdatePlugin plugin; + plugin.configure({{"staging_dir", ::testing::TempDir() + "/replaces_test"}}); + plugin.set_catalog_client_for_test(std::make_unique("http://x")); + auto runner = std::make_unique(); + RecordingProcessRunner * runner_raw = runner.get(); + plugin.set_process_runner_for_test(std::move(runner)); + + // Update entry with separate old + new executable basenames. + ASSERT_TRUE(plugin.register_update({ + {"id", "u_replaces"}, + {"updated_components", {"scan_sensor_node"}}, + {"x_medkit_artifact_url", "/artifacts/fixed.tgz"}, + {"x_medkit_target_package", "fixed_lidar"}, + {"x_medkit_executable", "fixed_lidar_node"}, + {"x_medkit_replaces_executable", "broken_lidar_node"}, + })); + + ros2_medkit_gateway::UpdateStatusInfo info; + std::mutex mu; + auto reporter = make_reporter(info, mu); + ASSERT_TRUE(plugin.prepare("u_replaces", reporter)); + + // execute() will fail at extract_and_swap (the staged tarball is not a real + // gzipped archive) but the kill step runs first - that is what we are + // checking here. + auto rc = plugin.execute("u_replaces", reporter); + (void)rc; + EXPECT_EQ(runner_raw->last_kill_target, "broken_lidar_node"); +} + +TEST(OtaUpdatePluginSmoke, ExecuteUpdateFallsBackToExecutableWhenReplacesMissing) { + ota_update_plugin::OtaUpdatePlugin plugin; + plugin.configure({{"staging_dir", ::testing::TempDir() + "/replaces_fallback"}}); + plugin.set_catalog_client_for_test(std::make_unique("http://x")); + auto runner = std::make_unique(); + RecordingProcessRunner * runner_raw = runner.get(); + plugin.set_process_runner_for_test(std::move(runner)); + + // Update entry without x_medkit_replaces_executable - kill should target + // the same name as x_medkit_executable. + ASSERT_TRUE(plugin.register_update({ + {"id", "u_no_replaces"}, + {"updated_components", {"scan_sensor_node"}}, + {"x_medkit_artifact_url", "/artifacts/scan.tgz"}, + {"x_medkit_target_package", "scan_pkg"}, + {"x_medkit_executable", "scan_node"}, + })); + + ros2_medkit_gateway::UpdateStatusInfo info; + std::mutex mu; + auto reporter = make_reporter(info, mu); + ASSERT_TRUE(plugin.prepare("u_no_replaces", reporter)); + + auto rc = plugin.execute("u_no_replaces", reporter); + (void)rc; + EXPECT_EQ(runner_raw->last_kill_target, "scan_node"); +} diff --git a/demos/ota_nav2_sensor_fix/ota_update_server/.gitignore b/demos/ota_nav2_sensor_fix/ota_update_server/.gitignore new file mode 100644 index 0000000..d0ad410 --- /dev/null +++ b/demos/ota_nav2_sensor_fix/ota_update_server/.gitignore @@ -0,0 +1,3 @@ +.venv/ +*.egg-info/ +__pycache__/ diff --git a/demos/ota_nav2_sensor_fix/ota_update_server/Dockerfile b/demos/ota_nav2_sensor_fix/ota_update_server/Dockerfile new file mode 100644 index 0000000..7bc0599 --- /dev/null +++ b/demos/ota_nav2_sensor_fix/ota_update_server/Dockerfile @@ -0,0 +1,21 @@ +FROM python:3.11-slim + +# Build context expected to be the demo root (so we can pull in artifacts/) +# rather than ota_update_server/. Compose wires this up. + +WORKDIR /app +COPY ota_update_server/pyproject.toml ./ +COPY ota_update_server/ota_update_server ./ota_update_server +RUN pip install --no-cache-dir . + +# Bake the demo catalog + tarballs into the image so the container is +# self-contained. Bind-mounting artifacts/ at runtime is unreliable on +# WSL2 + Docker Desktop, so we ship them in the image instead. +COPY artifacts /artifacts + +ENV OTA_ARTIFACTS_DIR=/artifacts +ENV OTA_HOST=0.0.0.0 +ENV 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..5414d0e --- /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_2_1_0", "updated_components": ["scan_sensor_node"]}, + {"id": "obstacle_classifier_v2_install", "added_components": ["obstacle_classifier"]}, + ] + (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/ros2_packages/broken_lidar/CMakeLists.txt b/demos/ota_nav2_sensor_fix/ros2_packages/broken_lidar/CMakeLists.txt new file mode 100644 index 0000000..9ecd54e --- /dev/null +++ b/demos/ota_nav2_sensor_fix/ros2_packages/broken_lidar/CMakeLists.txt @@ -0,0 +1,21 @@ +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) + +add_executable(broken_lidar_node src/broken_lidar_node.cpp) +ament_target_dependencies(broken_lidar_node rclcpp sensor_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..1b4ab49 --- /dev/null +++ b/demos/ota_nav2_sensor_fix/ros2_packages/broken_lidar/package.xml @@ -0,0 +1,17 @@ + + + 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 + + + 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..856f760 --- /dev/null +++ b/demos/ota_nav2_sensor_fix/ros2_packages/broken_lidar/src/broken_lidar_node.cpp @@ -0,0 +1,44 @@ +// Copyright 2026 bburda. Apache-2.0. +#include +#include +#include + +#include +#include + +using std::chrono_literals::operator""ms; + +class BrokenLidarNode : public rclcpp::Node { + public: + BrokenLidarNode() : Node("scan_sensor_node") { + pub_ = create_publisher("scan", 10); + timer_ = create_wall_timer(100ms, [this]() { publish_scan(); }); + } + + private: + void publish_scan() { + sensor_msgs::msg::LaserScan msg; + msg.header.stamp = now(); + msg.header.frame_id = "base_scan"; + msg.angle_min = -static_cast(M_PI); + msg.angle_max = static_cast(M_PI); + msg.angle_increment = static_cast(M_PI / 180.0); + msg.range_min = 0.05f; + msg.range_max = 10.0f; + constexpr int kRays = 360; + msg.ranges.assign(kRays, msg.range_max); + // Inject a 1 m phantom return at angle 0 (straight ahead, ray index 180) + msg.ranges[180] = 1.0f; + pub_->publish(msg); + } + + rclcpp::Publisher::SharedPtr pub_; + rclcpp::TimerBase::SharedPtr timer_; +}; + +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/broken_lidar_legacy/CMakeLists.txt b/demos/ota_nav2_sensor_fix/ros2_packages/broken_lidar_legacy/CMakeLists.txt new file mode 100644 index 0000000..eda65f7 --- /dev/null +++ b/demos/ota_nav2_sensor_fix/ros2_packages/broken_lidar_legacy/CMakeLists.txt @@ -0,0 +1,20 @@ +cmake_minimum_required(VERSION 3.16) +project(broken_lidar_legacy) + +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) + +add_executable(broken_lidar_legacy src/legacy_node.cpp) +ament_target_dependencies(broken_lidar_legacy rclcpp) + +install(TARGETS broken_lidar_legacy DESTINATION lib/${PROJECT_NAME}) + +ament_package() diff --git a/demos/ota_nav2_sensor_fix/ros2_packages/broken_lidar_legacy/package.xml b/demos/ota_nav2_sensor_fix/ros2_packages/broken_lidar_legacy/package.xml new file mode 100644 index 0000000..2cd0f61 --- /dev/null +++ b/demos/ota_nav2_sensor_fix/ros2_packages/broken_lidar_legacy/package.xml @@ -0,0 +1,16 @@ + + + broken_lidar_legacy + 1.0.0 + Do-nothing legacy package, target of uninstall demo scene. + bburda + Apache-2.0 + + ament_cmake + + rclcpp + + + ament_cmake + + diff --git a/demos/ota_nav2_sensor_fix/ros2_packages/broken_lidar_legacy/src/legacy_node.cpp b/demos/ota_nav2_sensor_fix/ros2_packages/broken_lidar_legacy/src/legacy_node.cpp new file mode 100644 index 0000000..144b7b4 --- /dev/null +++ b/demos/ota_nav2_sensor_fix/ros2_packages/broken_lidar_legacy/src/legacy_node.cpp @@ -0,0 +1,24 @@ +// Copyright 2026 bburda. Apache-2.0. +#include +#include + +#include + +using std::chrono_literals::operator""s; + +class LegacyNode : public rclcpp::Node { + public: + LegacyNode() : Node("broken_lidar_legacy") { + timer_ = create_wall_timer(5s, []() {}); + } + + private: + rclcpp::TimerBase::SharedPtr timer_; +}; + +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..587b97c --- /dev/null +++ b/demos/ota_nav2_sensor_fix/ros2_packages/fixed_lidar/src/fixed_lidar_node.cpp @@ -0,0 +1,42 @@ +// Copyright 2026 bburda. Apache-2.0. +#include +#include +#include + +#include +#include + +using std::chrono_literals::operator""ms; + +class FixedLidarNode : public rclcpp::Node { + public: + FixedLidarNode() : Node("scan_sensor_node") { + pub_ = create_publisher("scan", 10); + timer_ = create_wall_timer(100ms, [this]() { publish_scan(); }); + } + + private: + void publish_scan() { + sensor_msgs::msg::LaserScan msg; + msg.header.stamp = now(); + msg.header.frame_id = "base_scan"; + msg.angle_min = -static_cast(M_PI); + msg.angle_max = static_cast(M_PI); + msg.angle_increment = static_cast(M_PI / 180.0); + msg.range_min = 0.05f; + msg.range_max = 10.0f; + constexpr int kRays = 360; + msg.ranges.assign(kRays, msg.range_max); + pub_->publish(msg); + } + + rclcpp::Publisher::SharedPtr pub_; + rclcpp::TimerBase::SharedPtr timer_; +}; + +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/obstacle_classifier_v2/CMakeLists.txt b/demos/ota_nav2_sensor_fix/ros2_packages/obstacle_classifier_v2/CMakeLists.txt new file mode 100644 index 0000000..cda1852 --- /dev/null +++ b/demos/ota_nav2_sensor_fix/ros2_packages/obstacle_classifier_v2/CMakeLists.txt @@ -0,0 +1,22 @@ +cmake_minimum_required(VERSION 3.16) +project(obstacle_classifier_v2) + +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(visualization_msgs REQUIRED) + +add_executable(obstacle_classifier_node src/obstacle_classifier_node.cpp) +ament_target_dependencies(obstacle_classifier_node rclcpp sensor_msgs visualization_msgs) + +install(TARGETS obstacle_classifier_node DESTINATION lib/${PROJECT_NAME}) + +ament_package() diff --git a/demos/ota_nav2_sensor_fix/ros2_packages/obstacle_classifier_v2/package.xml b/demos/ota_nav2_sensor_fix/ros2_packages/obstacle_classifier_v2/package.xml new file mode 100644 index 0000000..6d214bd --- /dev/null +++ b/demos/ota_nav2_sensor_fix/ros2_packages/obstacle_classifier_v2/package.xml @@ -0,0 +1,18 @@ + + + obstacle_classifier_v2 + 1.0.0 + Extra safety layer for nav2 (target of install demo scene). + bburda + Apache-2.0 + + ament_cmake + + rclcpp + sensor_msgs + visualization_msgs + + + ament_cmake + + diff --git a/demos/ota_nav2_sensor_fix/ros2_packages/obstacle_classifier_v2/src/obstacle_classifier_node.cpp b/demos/ota_nav2_sensor_fix/ros2_packages/obstacle_classifier_v2/src/obstacle_classifier_node.cpp new file mode 100644 index 0000000..914c689 --- /dev/null +++ b/demos/ota_nav2_sensor_fix/ros2_packages/obstacle_classifier_v2/src/obstacle_classifier_node.cpp @@ -0,0 +1,50 @@ +// Copyright 2026 bburda. Apache-2.0. +#include + +#include +#include +#include + +class ObstacleClassifierNode : public rclcpp::Node { + public: + ObstacleClassifierNode() : Node("obstacle_classifier") { + pub_ = create_publisher("safety_overlay", 10); + sub_ = create_subscription( + "scan", 10, + [this](sensor_msgs::msg::LaserScan::SharedPtr msg) { on_scan(*msg); }); + } + + private: + void on_scan(const sensor_msgs::msg::LaserScan & scan) { + visualization_msgs::msg::MarkerArray markers; + visualization_msgs::msg::Marker m; + m.header = scan.header; + m.ns = "safety_overlay"; + m.id = 0; + m.type = visualization_msgs::msg::Marker::CYLINDER; + m.action = visualization_msgs::msg::Marker::ADD; + m.scale.x = 0.4; + m.scale.y = 0.4; + m.scale.z = 0.4; + m.color.r = 0.0f; + m.color.g = 1.0f; + m.color.b = 0.4f; + m.color.a = 0.6f; + m.pose.position.x = 0.0; + m.pose.position.y = 0.0; + m.pose.position.z = 0.2; + m.pose.orientation.w = 1.0; + markers.markers.push_back(m); + pub_->publish(markers); + } + + 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/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..a431a82 --- /dev/null +++ b/demos/ota_nav2_sensor_fix/ros2_packages/ota_nav2_sensor_fix_demo/CMakeLists.txt @@ -0,0 +1,14 @@ +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 +) + +ament_package() 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..aa8753d --- /dev/null +++ b/demos/ota_nav2_sensor_fix/ros2_packages/ota_nav2_sensor_fix_demo/config/nav2_params.yaml @@ -0,0 +1,353 @@ +# Nav2 parameters for TurtleBot3 + ros2_medkit demo +# Based on default nav2_bringup parameters for TurtleBot3 burger + +# 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 + transform_tolerance: 1.0 + update_min_a: 0.2 + update_min_d: 0.25 + z_hit: 0.5 + z_max: 0.05 + z_rand: 0.5 + z_short: 0.05 + scan_topic: scan + set_initial_pose: true + initial_pose: + x: 0.0 + y: 0.0 + z: 0.0 + yaw: 0.0 + +bt_navigator: + ros__parameters: + use_sim_time: True + global_frame: map + robot_base_frame: base_link + odom_topic: /odom + bt_loop_duration: 10 + default_server_timeout: 20 + wait_for_service_timeout: 1000 + action_server_result_timeout: 900.0 + navigators: ["navigate_to_pose", "navigate_through_poses"] + navigate_to_pose: + plugin: "nav2_bt_navigator::NavigateToPoseNavigator" + navigate_through_poses: + plugin: "nav2_bt_navigator::NavigateThroughPosesNavigator" + # Note: plugin_lib_names is no longer needed in Jazzy - plugins are auto-loaded + +controller_server: + ros__parameters: + use_sim_time: True + enable_stamped_cmd_vel: True + controller_frequency: 20.0 + min_x_velocity_threshold: 0.001 + min_y_velocity_threshold: 0.5 + min_theta_velocity_threshold: 0.001 + failure_tolerance: 0.3 + progress_checker_plugins: ["progress_checker"] + goal_checker_plugins: ["general_goal_checker"] + controller_plugins: ["FollowPath"] + odom_topic: "odom" + + progress_checker: + plugin: "nav2_controller::SimpleProgressChecker" + required_movement_radius: 0.5 + movement_time_allowance: 10.0 + + general_goal_checker: + stateful: True + plugin: "nav2_controller::SimpleGoalChecker" + xy_goal_tolerance: 0.25 + yaw_goal_tolerance: 0.25 + + FollowPath: + plugin: "dwb_core::DWBLocalPlanner" + debug_trajectory_details: True + min_vel_x: 0.0 + min_vel_y: 0.0 + max_vel_x: 0.26 + max_vel_y: 0.0 + max_vel_theta: 1.0 + min_speed_xy: 0.0 + max_speed_xy: 0.26 + min_speed_theta: 0.0 + acc_lim_x: 2.5 + acc_lim_y: 0.0 + acc_lim_theta: 3.2 + decel_lim_x: -2.5 + 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_link + use_sim_time: True + rolling_window: true + width: 3 + height: 3 + resolution: 0.05 + robot_radius: 0.22 + plugins: ["voxel_layer", "inflation_layer"] + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + 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_link + use_sim_time: True + robot_radius: 0.22 + 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.55 + 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_link + 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" + max_velocity: [0.26, 0.0, 1.0] + min_velocity: [-0.26, 0.0, -1.0] + max_accel: [2.5, 0.0, 3.2] + max_decel: [-2.5, 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_link" + 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: ["FootprintApproach"] + FootprintApproach: + type: "polygon" + action_type: "approach" + footprint_topic: "/local_costmap/published_footprint" + time_before_collision: 1.2 + simulation_time_step: 0.1 + min_points: 6 + 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/turtlebot3_world.yaml b/demos/ota_nav2_sensor_fix/ros2_packages/ota_nav2_sensor_fix_demo/config/turtlebot3_world.yaml new file mode 100644 index 0000000..ad2e683 --- /dev/null +++ b/demos/ota_nav2_sensor_fix/ros2_packages/ota_nav2_sensor_fix_demo/config/turtlebot3_world.yaml @@ -0,0 +1,7 @@ +image: /opt/ros/jazzy/share/turtlebot3_navigation2/map/map.pgm +mode: trinary +resolution: 0.05 +origin: [-1.76, -2.42, 0.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..4170833 --- /dev/null +++ b/demos/ota_nav2_sensor_fix/ros2_packages/ota_nav2_sensor_fix_demo/launch/demo.launch.py @@ -0,0 +1,241 @@ +# 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 TurtleBot3 world +# - TurtleBot3 (burger) spawned in that world + robot_state_publisher +# - the full Nav2 stack (bringup_launch.py) with the TB3 default 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) +# - broken_lidar + broken_lidar_legacy (the OTA plugin swaps/uninstalls these) +# - the gateway with our ota_update_plugin loaded via gateway_config.yaml +# +# /scan-override strategy +# ----------------------- +# The TB3 gz-sim simulation publishes its own LaserScan on /scan via the +# ros_gz_bridge. broken_lidar also publishes on /scan with a phantom obstacle +# at ray index 180. Two publishers on /scan would interleave - nav2 would see +# garbage. We solve this by wrapping spawn_turtlebot3 in a GroupAction with a +# SetRemap that pushes the simulator's /scan to /scan_sim. That way the gz +# bridge inside the spawn launch ends up emitting /scan_sim, not /scan, and +# broken_lidar (and later fixed_lidar) is the sole publisher on /scan that +# nav2 + foxglove see. + +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, + GroupAction, + IncludeLaunchDescription, + SetEnvironmentVariable, +) +from launch.conditions import IfCondition, UnlessCondition +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node, SetRemap + + +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(): + turtlebot3_gazebo_dir = get_package_share_directory('turtlebot3_gazebo') + 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') + + nav2_params_file = os.path.join(demo_pkg_dir, 'config', 'nav2_params.yaml') + map_file = os.path.join(demo_pkg_dir, 'config', 'turtlebot3_world.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', + ) + + world_file = os.path.join(turtlebot3_gazebo_dir, 'worlds', 'turtlebot3_world.world') + + use_sim_time = LaunchConfiguration('use_sim_time', default='True') + headless = LaunchConfiguration('headless', default='True') + x_pose = LaunchConfiguration('x_pose', default='-2.0') + y_pose = LaunchConfiguration('y_pose', default='-0.5') + + set_gz_model_path = AppendEnvironmentVariable( + 'GZ_SIM_RESOURCE_PATH', + os.path.join(turtlebot3_gazebo_dir, 'models'), + ) + + # Gazebo headless server: -r runs the world, -s is server-only (no GUI), -v2 + # is INFO logging. on_exit_shutdown ensures gz dying tears the launch down. + gz_headless = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(ros_gz_sim_dir, 'launch', 'gz_sim.launch.py'), + ), + launch_arguments={ + 'gz_args': ['-r', '-s', '-v2', world_file], + 'on_exit_shutdown': 'true', + }.items(), + condition=IfCondition(headless), + ) + + # GUI mode is left in for parity with the sibling TB3 demo (someone running + # this on a workstation can flip headless:=False); in containers we never + # take this branch. + gz_gui = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(turtlebot3_gazebo_dir, 'launch', 'turtlebot3_world.launch.py'), + ), + launch_arguments={'use_sim_time': use_sim_time}.items(), + condition=UnlessCondition(headless), + ) + + # Spawn the robot - SetRemap inside the GroupAction pushes the gazebo + # bridge's /scan onto /scan_sim so broken_lidar owns /scan exclusively. + spawn_robot = GroupAction( + actions=[ + SetRemap(src='/scan', dst='/scan_sim'), + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(turtlebot3_gazebo_dir, 'launch', 'spawn_turtlebot3.launch.py'), + ), + launch_arguments={'x_pose': x_pose, 'y_pose': y_pose}.items(), + ), + ], + condition=IfCondition(headless), + ) + + robot_state_publisher = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(turtlebot3_gazebo_dir, 'launch', 'robot_state_publisher.launch.py'), + ), + launch_arguments={'use_sim_time': use_sim_time}.items(), + condition=IfCondition(headless), + ) + + # 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}], + ) + + 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}, + ], + ) + + broken_lidar_node = Node( + package='broken_lidar', + executable='broken_lidar_node', + name='scan_sensor_node', + output='screen', + parameters=[{'use_sim_time': use_sim_time}], + ) + + broken_lidar_legacy = Node( + package='broken_lidar_legacy', + executable='broken_lidar_legacy', + name='broken_lidar_legacy', + 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 TB3 demo does. + _ = _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'], + ) + + 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', + ), + DeclareLaunchArgument( + 'x_pose', default_value='-2.0', + description='Robot initial X position', + ), + DeclareLaunchArgument( + 'y_pose', default_value='-0.5', + description='Robot initial Y position', + ), + SetEnvironmentVariable( + name='TURTLEBOT3_MODEL', + value=os.environ.get('TURTLEBOT3_MODEL', 'burger'), + ), + set_gz_model_path, + gz_headless, + gz_gui, + spawn_robot, + robot_state_publisher, + nav2, + fault_manager, + foxglove, + broken_lidar_node, + broken_lidar_legacy, + gateway, + ]) 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..04ed2e7 --- /dev/null +++ b/demos/ota_nav2_sensor_fix/ros2_packages/ota_nav2_sensor_fix_demo/package.xml @@ -0,0 +1,36 @@ + + + + ota_nav2_sensor_fix_demo + 0.1.0 + Launch + config glue for the OTA over SOVD nav2 sensor-fix demo (TurtleBot3 + Nav2 + headless Gazebo + broken_lidar/fixed_lidar swap). + bburda + Apache-2.0 + + ament_cmake + + ros2launch + turtlebot3_gazebo + turtlebot3_description + turtlebot3_navigation2 + ros_gz_sim + ros_gz_bridge + 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 + broken_lidar + broken_lidar_legacy + + + ament_cmake + + 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..042d3bd --- /dev/null +++ b/demos/ota_nav2_sensor_fix/run-demo.sh @@ -0,0 +1,152 @@ +#!/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="" +SKIP_ARTIFACTS="false" + +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 " --skip-artifacts Skip rebuilding artifacts/catalog.json" + 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" ;; + --skip-artifacts) SKIP_ARTIFACTS="true" ;; + -h|--help) usage; exit 0 ;; + *) echo "Unknown option: $1"; usage; exit 1 ;; + esac + shift +done + +GATEWAY_PORT="${OTA_GATEWAY_PORT:-8080}" +GATEWAY_URL="http://localhost:${GATEWAY_PORT}" + +echo "OTA over SOVD - nav2 sensor-fix demo" +echo "====================================" +echo "" + +if ! command -v docker &> /dev/null; then + echo "Error: Docker is not installed" + exit 1 +fi + +if [[ "$SKIP_ARTIFACTS" != "true" ]]; then + if [[ ! -x "$SCRIPT_DIR/scripts/build_artifacts.sh" ]]; then + chmod +x "$SCRIPT_DIR/scripts/build_artifacts.sh" + fi + echo "[1/3] Building OTA artifacts (catalog.json + tarballs)..." + "$SCRIPT_DIR/scripts/build_artifacts.sh" + echo "" +fi + +if docker compose version &> /dev/null; then + COMPOSE_CMD="docker compose" +else + COMPOSE_CMD="docker-compose" +fi + +if [[ "$UPDATE_IMAGES" == "true" ]]; then + echo "Pulling latest images..." + ${COMPOSE_CMD} pull +fi + +echo "[2/3] Building and starting demo..." +echo " (First run pulls ros:jazzy and builds the gateway, ~10 minutes)" +echo "" + +DETACH_FLAG="" +if [[ "$DETACH_MODE" == "true" ]]; then + DETACH_FLAG="-d" +fi + +# shellcheck disable=SC2086 +if ! ${COMPOSE_CMD} build ${BUILD_ARGS}; then + echo "Docker build failed. Stopping any partially created containers..." + ${COMPOSE_CMD} down 2>/dev/null || true + exit 1 +fi + +# shellcheck disable=SC2086 +${COMPOSE_CMD} up ${DETACH_FLAG} + +if [[ "$DETACH_MODE" != "true" ]]; then + exit 0 +fi + +echo "" +echo "[3/3] Waiting for gateway to come up..." +for _ in 1 2 3 4 5 6 7 8 9 10 11 12; do + if curl -fsS "${GATEWAY_URL}/api/v1/health" >/dev/null 2>&1; then + break + fi + sleep 2 +done + +if ! curl -fsS "${GATEWAY_URL}/api/v1/health" >/dev/null 2>&1; then + echo "Gateway did not respond on ${GATEWAY_URL} - check logs with:" + echo " ${COMPOSE_CMD} logs gateway" + exit 1 +fi + +echo "" +echo "Demo is up." +echo "" +echo " Gateway HTTP API: ${GATEWAY_URL}/api/v1/" +echo " Foxglove WebSocket: ws://localhost:${OTA_FOXGLOVE_BRIDGE_PORT:-8765}" +echo " Update server: http://localhost:9000/catalog" +echo "" +echo "Registered updates:" +if command -v jq >/dev/null 2>&1; then + curl -fsS "${GATEWAY_URL}/api/v1/updates" | jq -r '.items[]' | sed 's/^/ /' +else + curl -fsS "${GATEWAY_URL}/api/v1/updates" +fi +echo "" +echo "Drive the demo:" +echo " ./check-demo.sh # show current state" +echo " ./trigger-update.sh # update broken_lidar -> fixed_lidar" +echo " ./trigger-install.sh # install obstacle_classifier_v2" +echo " ./trigger-uninstall.sh # uninstall broken_lidar_legacy" +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..c34ce0e --- /dev/null +++ b/demos/ota_nav2_sensor_fix/scripts/build_artifacts.sh @@ -0,0 +1,52 @@ +#!/usr/bin/env bash +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 +set -u + +mkdir -p "$WS/src" +for pkg in broken_lidar fixed_lidar broken_lidar_legacy obstacle_classifier_v2; do + ln -sfn "$DEMO_DIR/ros2_packages/$pkg" "$WS/src/$pkg" +done + +(cd "$WS" && colcon build --packages-select fixed_lidar obstacle_classifier_v2) + +mkdir -p "$ARTIFACTS" +rm -f "$ARTIFACTS/catalog.json" + +PACK=("$SCRIPT_DIR/.venv/bin/python" "$SCRIPT_DIR/pack_artifact.py") + +env -i PATH=/usr/bin:/bin HOME="$HOME" "${PACK[@]}" \ + --package fixed_lidar --version 2.1.0 \ + --kind update --target-component scan_sensor_node \ + --executable fixed_lidar_node \ + --replaces-executable broken_lidar_node \ + --notes "Fix /scan noise filter" \ + --skip-build --workspace "$WS" \ + --out-dir "$ARTIFACTS" --catalog "$ARTIFACTS/catalog.json" + +env -i PATH=/usr/bin:/bin HOME="$HOME" "${PACK[@]}" \ + --package obstacle_classifier_v2 --version 1.0.0 \ + --kind install --target-component obstacle_classifier \ + --executable obstacle_classifier_node \ + --notes "Extra safety layer for nav2" \ + --skip-build --workspace "$WS" \ + --out-dir "$ARTIFACTS" --catalog "$ARTIFACTS/catalog.json" + +env -i PATH=/usr/bin:/bin HOME="$HOME" "${PACK[@]}" \ + --package broken_lidar_legacy --version "" \ + --kind uninstall --target-component broken_lidar_legacy \ + --notes "Clean up deprecated package" \ + --skip-build --workspace "$WS" \ + --out-dir "$ARTIFACTS" --catalog "$ARTIFACTS/catalog.json" + +if command -v jq >/dev/null 2>&1; then + echo "Built catalog with $(jq length "$ARTIFACTS/catalog.json") entries" +else + echo "Built catalog: $(wc -l < "$ARTIFACTS/catalog.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..1937ffc --- /dev/null +++ b/demos/ota_nav2_sensor_fix/scripts/e2e_webui_smoke.mjs @@ -0,0 +1,97 @@ +// 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 all 3 catalog entries register and that the SOVD wire format +// matches what we ship from pack_artifact.py. +// +// 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 \ +// 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 EXPECTED_IDS = [ + "fixed_lidar_2_1_0", + "obstacle_classifier_v2_1_0_0", + "broken_lidar_legacy_remove", +]; + +const EXPECTED_API_PATHS = [ + "/api/v1/updates", + "/api/v1/updates/fixed_lidar_2_1_0/status", + "/api/v1/updates/obstacle_classifier_v2_1_0_0/status", + "/api/v1/updates/broken_lidar_legacy_remove/status", +]; + +(async () => { + const browser = await chromium.launch({ + channel: "chromium-headless-shell", + headless: true, + }); + const ctx = await browser.newContext(); + const page = await ctx.newPage(); + + page.on("pageerror", (err) => console.log(`[pageerror] ${err.message}`)); + + const apiCalls = new Set(); + page.on("request", (req) => { + const u = req.url(); + if (u.includes("/api/v1/")) { + apiCalls.add(`${req.method()} ${u}`); + } + }); + + await page.goto(WEB_UI_URL, { waitUntil: "domcontentloaded" }); + + await page.getByRole("button", { name: /connect to server/i }).click(); + await page.waitForTimeout(300); + await page.locator('input[type="text"], input:not([type])').first().fill(GATEWAY_URL); + await page.getByRole("button", { name: /^connect$/i }).last().click(); + await page.waitForTimeout(2000); + + const updatesButton = page.getByRole("button", { name: /updates/i }).first(); + if (await updatesButton.count()) { + await updatesButton.click(); + await page.waitForTimeout(2000); + } + + const bodyText = await page.locator("body").textContent(); + + let failed = 0; + for (const id of EXPECTED_IDS) { + const visible = bodyText?.includes(id) ?? false; + console.log(` id ${id}: ${visible ? "PASS" : "FAIL"}`); + if (!visible) failed++; + } + + for (const path of EXPECTED_API_PATHS) { + const hit = [...apiCalls].some((c) => c.endsWith(path)); + console.log(` api ${path}: ${hit ? "PASS" : "FAIL"}`); + if (!hit) failed++; + } + + await browser.close(); + + if (failed > 0) { + console.error(`\n${failed} assertion(s) failed`); + process.exit(1); + } + console.log("\nDONE: all SOVD flows verified"); +})().catch((err) => { + console.error("FAIL:", err); + process.exit(1); +}); diff --git a/demos/ota_nav2_sensor_fix/scripts/pack_artifact.py b/demos/ota_nav2_sensor_fix/scripts/pack_artifact.py new file mode 100644 index 0000000..fabe8e4 --- /dev/null +++ b/demos/ota_nav2_sensor_fix/scripts/pack_artifact.py @@ -0,0 +1,238 @@ +#!/usr/bin/env python3 +"""Pack a ROS 2 package into an OTA artifact + SOVD-shaped catalog entry.""" +from __future__ import annotations + +import argparse +import json +import subprocess +import sys +import tarfile +from pathlib import Path +from typing import Literal + +Kind = Literal["update", "install", "uninstall"] + + +def build_parser() -> argparse.ArgumentParser: + parser = argparse.ArgumentParser( + description="Pack a ROS 2 package into an OTA artifact + SOVD catalog entry.", + ) + parser.add_argument("--package", required=True, help="ROS 2 package name to pack.") + parser.add_argument( + "--version", + default="0.0.0", + help="Semantic version of the artifact (pass '' for uninstall).", + ) + parser.add_argument( + "--kind", + required=True, + choices=["update", "install", "uninstall"], + help="Catalog entry kind.", + ) + parser.add_argument( + "--target-component", + required=True, + help="SOVD component the entry targets.", + ) + parser.add_argument( + "--executable", + default="", + help="Executable name inside install//lib (required for install).", + ) + parser.add_argument( + "--replaces-executable", + default="", + help=( + "For kind=update: name of the OLD executable to kill before " + "spawning --executable. Defaults to --executable when omitted." + ), + ) + parser.add_argument("--notes", default="", help="Free-text notes for the catalog entry.") + parser.add_argument( + "--duration", + type=int, + default=10, + help="Estimated install duration in seconds.", + ) + parser.add_argument( + "--out-dir", + default="artifacts", + help="Output directory for tarballs.", + ) + parser.add_argument( + "--catalog", + default="artifacts/catalog.json", + help="Path to the SOVD catalog JSON file.", + ) + parser.add_argument( + "--skip-build", + action="store_true", + help="Skip running colcon build; reuse existing install/ tree.", + ) + parser.add_argument( + "--workspace", + default=".", + help="Path to the colcon workspace root.", + ) + return parser + + +def slug(package: str, version: str) -> str: + return f"{package}_{version.replace('.', '_')}" if version else package + + +def build_entry( + *, + package: str, + version: str, + kind: Kind, + target_component: str, + executable: str, + notes: str, + duration: int, + size_bytes: int, + replaces_executable: str = "", +) -> dict: + entry: dict = { + "id": slug(package, version) if kind != "uninstall" else f"{package}_remove", + # SOVD ISO 17978-3 mandates "update_name". Earlier drafts of this + # script wrote "name" - the gateway passes that through to clients + # but spec-compliant consumers (web UI, Foxglove panel) expect + # update_name. + "update_name": f"{package} {version}".strip(), + "automated": False, + "origins": ["remote"], + "notes": notes, + "duration": duration, + } + if version: + # SOVD spec does not define a top-level version field on update + # detail, so we expose it as a vendor extension. + entry["x_medkit_version"] = version + if size_bytes > 0: + entry["size"] = max(1, size_bytes // 1024) + + if kind == "update": + entry["updated_components"] = [target_component] + elif kind == "install": + entry["added_components"] = [target_component] + else: # uninstall + entry["removed_components"] = [target_component] + + if kind != "uninstall": + entry["x_medkit_artifact_url"] = f"/artifacts/{package}-{version}.tar.gz" + entry["x_medkit_target_package"] = package + if executable: + entry["x_medkit_executable"] = executable + else: + entry["x_medkit_target_package"] = package + + if kind == "update" and replaces_executable: + entry["x_medkit_replaces_executable"] = replaces_executable + + return entry + + +def merge_catalog(catalog_path: Path, entry: dict) -> None: + catalog_path = Path(catalog_path) + catalog_path.parent.mkdir(parents=True, exist_ok=True) + if catalog_path.exists(): + data = json.loads(catalog_path.read_text()) + else: + data = [] + data = [e for e in data if e.get("id") != entry["id"]] + data.append(entry) + catalog_path.write_text(json.dumps(data, indent=2) + "\n") + + +def create_tarball( + *, + package: str, + version: str, + install_dir: Path, + out_dir: Path, +) -> Path: + install_dir = Path(install_dir) + if not install_dir.exists(): + raise FileNotFoundError(f"install dir does not exist: {install_dir}") + out_dir = Path(out_dir) + out_dir.mkdir(parents=True, exist_ok=True) + out_path = out_dir / f"{package}-{version}.tar.gz" + with tarfile.open(out_path, "w:gz") as tf: + tf.add(install_dir, arcname=package) + return out_path + + +def colcon_build(workspace: Path, package: str) -> None: + cmd = ["colcon", "build", "--packages-select", package, "--symlink-install"] + completed = subprocess.run(cmd, cwd=workspace, check=False) + if completed.returncode != 0: + raise SystemExit(f"colcon build failed for {package}") + + +def run( + *, + package: str, + version: str, + kind: Kind, + target_component: str, + executable: str, + notes: str, + duration: int, + out_dir: str, + catalog: str, + skip_build: bool, + workspace: str, + replaces_executable: str = "", +) -> int: + if kind == "install" and not executable: + sys.stderr.write("--executable is required for install\n") + raise SystemExit(2) + if kind != "uninstall" and not version: + sys.stderr.write(f"--version is required for kind={kind}\n") + raise SystemExit(2) + + out_dir_p = Path(out_dir) + catalog_p = Path(catalog) + workspace_p = Path(workspace) + + size_bytes = 0 + if kind != "uninstall": + if not skip_build: + colcon_build(workspace_p, package) + install_dir = workspace_p / "install" / package + if not install_dir.exists(): + sys.stderr.write(f"install dir missing: {install_dir}\n") + raise SystemExit(3) + tarball = create_tarball( + package=package, + version=version, + install_dir=install_dir, + out_dir=out_dir_p, + ) + size_bytes = tarball.stat().st_size + + entry = build_entry( + package=package, + version=version, + kind=kind, + target_component=target_component, + executable=executable, + notes=notes, + duration=duration, + size_bytes=size_bytes, + replaces_executable=replaces_executable, + ) + merge_catalog(catalog_p, entry) + print(f"packed {entry['id']}") + return 0 + + +def main(argv: list[str] | None = None) -> int: + parser = build_parser() + args = parser.parse_args(argv) + return run(**vars(args)) + + +if __name__ == "__main__": + sys.exit(main()) diff --git a/demos/ota_nav2_sensor_fix/scripts/pyrightconfig.json b/demos/ota_nav2_sensor_fix/scripts/pyrightconfig.json new file mode 100644 index 0000000..5b3e8d0 --- /dev/null +++ b/demos/ota_nav2_sensor_fix/scripts/pyrightconfig.json @@ -0,0 +1,5 @@ +{ + "extraPaths": ["."], + "venvPath": ".", + "venv": ".venv" +} diff --git a/demos/ota_nav2_sensor_fix/scripts/test_pack_artifact.py b/demos/ota_nav2_sensor_fix/scripts/test_pack_artifact.py new file mode 100644 index 0000000..36d840c --- /dev/null +++ b/demos/ota_nav2_sensor_fix/scripts/test_pack_artifact.py @@ -0,0 +1,320 @@ +"""Tests for pack_artifact.py.""" +from __future__ import annotations + +import json +import tarfile + +import pytest + +import pack_artifact + + +def test_main_requires_package(): + with pytest.raises(SystemExit): + pack_artifact.main([]) + + +def test_main_parses_basic_args(monkeypatch, tmp_path): + captured = {} + + def fake_run(**kwargs): + captured.update(kwargs) + return 0 + + monkeypatch.setattr(pack_artifact, "run", fake_run) + rc = pack_artifact.main( + [ + "--package", "fixed_lidar", + "--version", "2.1.0", + "--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"] == "2.1.0" + assert captured["kind"] == "update" + assert captured["target_component"] == "scan_sensor_node" + assert captured["executable"] == "fixed_lidar_node" + assert captured["notes"] == "noise filter fix" + assert captured["skip_build"] is True + + +def test_build_entry_update_kind(): + entry = pack_artifact.build_entry( + package="fixed_lidar", + version="2.1.0", + kind="update", + target_component="scan_sensor_node", + executable="fixed_lidar_node", + notes="fix noise", + duration=10, + size_bytes=2048, + ) + assert entry["id"] == "fixed_lidar_2_1_0" + assert entry["update_name"] == "fixed_lidar 2.1.0" + assert "name" not in entry, "use update_name (SOVD spec) not name" + assert entry["x_medkit_version"] == "2.1.0" + 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-2.1.0.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="2.1.0", + 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="obstacle_classifier_v2", + version="1.0.0", + kind="install", + target_component="obstacle_classifier", + executable="obstacle_classifier_node", + notes="extra safety", + duration=15, + size_bytes=4096, + ) + assert entry["added_components"] == ["obstacle_classifier"] + assert "updated_components" not in entry + assert "removed_components" not in entry + + +def test_build_entry_uninstall_kind(): + entry = pack_artifact.build_entry( + package="broken_lidar_legacy", + version="", + kind="uninstall", + target_component="broken_lidar_legacy", + executable="", + notes="cleanup", + duration=5, + size_bytes=0, + ) + assert entry["removed_components"] == ["broken_lidar_legacy"] + 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="2.1.0", + install_dir=install, + out_dir=out_dir, + ) + assert out_path == out_dir / "fixed_lidar-2.1.0.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="2.1.0", + 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-2.1.0.tar.gz").exists() + data = json.loads(catalog.read_text()) + assert data[0]["id"] == "fixed_lidar_2_1_0" + 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="broken_lidar_legacy", + version="", + kind="uninstall", + target_component="broken_lidar_legacy", + 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"] == ["broken_lidar_legacy"] + + +def test_run_install_requires_executable(tmp_path): + with pytest.raises(SystemExit): + pack_artifact.run( + package="obstacle_classifier_v2", + version="1.0.0", + kind="install", + target_component="obstacle_classifier", + 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" / "obstacle_classifier_v2" / "lib" + install.mkdir(parents=True) + (install / "obstacle_classifier_node").write_text("bin") + out_dir = tmp_path / "artifacts" + catalog = out_dir / "catalog.json" + + rc = pack_artifact.run( + package="obstacle_classifier_v2", + version="1.0.0", + kind="install", + target_component="obstacle_classifier", + executable="obstacle_classifier_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 / "obstacle_classifier_v2-1.0.0.tar.gz").exists() + data = json.loads(catalog.read_text()) + assert data[0]["id"] == "obstacle_classifier_v2_1_0_0" + assert data[0]["added_components"] == ["obstacle_classifier"] + assert data[0]["x_medkit_executable"] == "obstacle_classifier_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/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-install.sh b/demos/ota_nav2_sensor_fix/trigger-install.sh new file mode 100755 index 0000000..d4409b0 --- /dev/null +++ b/demos/ota_nav2_sensor_fix/trigger-install.sh @@ -0,0 +1,35 @@ +#!/bin/bash +# Trigger the SOVD install flow: deploy obstacle_classifier_v2 from scratch. + +set -eu + +GATEWAY_URL="${OTA_GATEWAY_URL:-http://localhost:${OTA_GATEWAY_PORT:-8080}}" +API="${GATEWAY_URL}/api/v1" +ID="obstacle_classifier_v2_1_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 "Install: ${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 'obstacle_classifier' \ + 2>/dev/null | grep -v 'pgrep' | sed 's/^/ /' || echo " (none)" +fi diff --git a/demos/ota_nav2_sensor_fix/trigger-uninstall.sh b/demos/ota_nav2_sensor_fix/trigger-uninstall.sh new file mode 100755 index 0000000..0f7c8fa --- /dev/null +++ b/demos/ota_nav2_sensor_fix/trigger-uninstall.sh @@ -0,0 +1,37 @@ +#!/bin/bash +# Trigger the SOVD uninstall flow: remove broken_lidar_legacy. + +set -eu + +GATEWAY_URL="${OTA_GATEWAY_URL:-http://localhost:${OTA_GATEWAY_PORT:-8080}}" +API="${GATEWAY_URL}/api/v1" +ID="broken_lidar_legacy_remove" + +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 "Uninstall: ${ID}" +# Uninstall has no artifact to fetch but the gateway state machine still +# needs prepare->execute to advance. +echo " PUT /updates/${ID}/prepare" +curl -fsS -X PUT -H 'Content-Type: application/json' -d '{}' \ + "${API}/updates/${ID}/prepare" >/dev/null +sleep 2 + +echo " PUT /updates/${ID}/execute" +curl -fsS -X PUT -H 'Content-Type: application/json' -d '{}' \ + "${API}/updates/${ID}/execute" >/dev/null +sleep 4 + +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 (broken_lidar_legacy should be gone):" + docker exec ota_demo_gateway pgrep -af 'broken_lidar_legacy' \ + 2>/dev/null | grep -v 'pgrep' | sed 's/^/ /' || echo " (none - uninstall succeeded)" +fi diff --git a/demos/ota_nav2_sensor_fix/trigger-update.sh b/demos/ota_nav2_sensor_fix/trigger-update.sh new file mode 100755 index 0000000..5f9668a --- /dev/null +++ b/demos/ota_nav2_sensor_fix/trigger-update.sh @@ -0,0 +1,36 @@ +#!/bin/bash +# Trigger the SOVD update flow: replace broken_lidar with fixed_lidar. +# 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_2_1_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/tests/smoke_test_ota.sh b/tests/smoke_test_ota.sh new file mode 100755 index 0000000..f93ffde --- /dev/null +++ b/tests/smoke_test_ota.sh @@ -0,0 +1,239 @@ +#!/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 SOVD catalog is registered with the 3 expected entries +# - the update detail uses spec field names (update_name, no `name`/`version`) +# - the update 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 + +GATEWAY_CONTAINER="${OTA_DEMO_GATEWAY_CONTAINER:-ota_demo_gateway}" + +EXPECTED_IDS=( + "fixed_lidar_2_1_0" + "obstacle_classifier_v2_1_0_0" + "broken_lidar_legacy_remove" +) + +# Confirm a process is or is not running inside the gateway container. +# Usage: assert_process_running +# assert_process_gone +assert_process_running() { + local pattern="$1" + local desc="$2" + if docker exec "$GATEWAY_CONTAINER" pgrep -f "$pattern" >/dev/null 2>&1; then + pass "$desc" + else + fail "$desc" "no process matching '$pattern' in $GATEWAY_CONTAINER" + fi +} + +assert_process_gone() { + local pattern="$1" + local desc="$2" + if ! docker exec "$GATEWAY_CONTAINER" pgrep -f "$pattern" >/dev/null 2>&1; then + pass "$desc" + else + fail "$desc" "process matching '$pattern' still alive in $GATEWAY_CONTAINER" + fi +} + +# --- Wait for gateway startup --- + +wait_for_gateway 90 + +# Plugin's boot poll fetches /catalog and registers entries; wait for it. +echo " Waiting for plugin's boot poll to register catalog (max 30s)..." +if poll_until "/updates" '.items[] | select(. == "fixed_lidar_2_1_0")' 30; then + echo " Catalog registered" +else + echo " Catalog NOT registered within 30s" + exit 1 +fi + +# --- Tests --- + +section "Health" + +if api_get "/health"; then + pass "GET /health returns 200" +else + fail "GET /health returns 200" "unexpected status code" +fi + +section "UpdateProvider plugin loaded" + +# Capture logs into a variable rather than piping `docker logs | grep -q`. +# With `set -o pipefail` and a large log (nav2 lifecycle prints ~500 lines), +# grep -q exits early on first match, SIGPIPEs docker logs, and the pipeline +# returns 141 - which `if` reads as "no match" even when the line was found. +GATEWAY_LOGS=$(docker logs "$GATEWAY_CONTAINER" 2>&1 || true) + +if printf '%s\n' "$GATEWAY_LOGS" | grep -q "Update backend provided by plugin"; 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 printf '%s\n' "$GATEWAY_LOGS" | grep -q "Updates enabled but no UpdateProvider plugin loaded"; then + fail "no 'no UpdateProvider' warning" "warning was logged" +else + pass "no 'no UpdateProvider' warning" +fi + +section "Catalog (GET /updates returns SOVD {items})" + +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 + +for id in "${EXPECTED_IDS[@]}"; do + if echo "$RESPONSE" | jq -e --arg id "$id" '.items[] | select(. == $id)' >/dev/null 2>&1; then + pass "/updates contains '$id'" + else + fail "/updates contains '$id'" "id missing" + fi +done + +section "Detail field shape (SOVD ISO 17978-3 compliance)" + +# fixed_lidar update detail: must use spec field names +if api_get "/updates/fixed_lidar_2_1_0"; then + pass "GET /updates/fixed_lidar_2_1_0 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 == "2.1.0"' >/dev/null 2>&1; then + pass "detail has x_medkit_version = 2.1.0" + else + fail "detail has x_medkit_version = 2.1.0" "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" +assert_process_running "broken_lidar_legacy" "broken_lidar_legacy running before uninstall" + +section "/scan SetRemap regression (only broken_lidar publishes, not gz-bridge)" + +# The launch wraps spawn_turtlebot3 in a SetRemap('/scan' -> '/scan_sim') so +# the gz-bridge ends up on /scan_sim, leaving broken_lidar as the sole +# publisher on /scan. 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. +SCAN_INFO=$(docker exec "$GATEWAY_CONTAINER" bash -lc \ + 'source /opt/ros/jazzy/setup.bash && ros2 topic info /scan -v' 2>/dev/null || true) + +PUB_COUNT=$(printf '%s\n' "$SCAN_INFO" | grep -c "Endpoint type: PUBLISHER" || true) +if [ "$PUB_COUNT" = "1" ]; then + pass "/scan has exactly 1 publisher" +else + fail "/scan has exactly 1 publisher" "got ${PUB_COUNT} publishers - SetRemap regressed?" +fi + +if printf '%s\n' "$SCAN_INFO" | grep -q "ros_gz_bridge"; then + fail "/scan publisher is not ros_gz_bridge" "gz-bridge is publishing on /scan (SetRemap broken)" +else + pass "/scan publisher is not ros_gz_bridge" +fi + +section "Update flow: PUT /updates/fixed_lidar_2_1_0/prepare + /execute" + +curl -fsS -X PUT -H 'Content-Type: application/json' -d '{}' \ + "${API_BASE}/updates/fixed_lidar_2_1_0/prepare" >/dev/null +sleep 4 +curl -fsS -X PUT -H 'Content-Type: application/json' -d '{}' \ + "${API_BASE}/updates/fixed_lidar_2_1_0/execute" >/dev/null +sleep 6 + +if api_get "/updates/fixed_lidar_2_1_0/status"; then + if echo "$RESPONSE" | jq -e '.status == "completed"' >/dev/null 2>&1; then + pass "fixed_lidar_2_1_0 status is completed" + else + fail "fixed_lidar_2_1_0 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" + +section "Install flow: PUT /updates/obstacle_classifier_v2_1_0_0/prepare + /execute" + +curl -fsS -X PUT -H 'Content-Type: application/json' -d '{}' \ + "${API_BASE}/updates/obstacle_classifier_v2_1_0_0/prepare" >/dev/null +sleep 4 +curl -fsS -X PUT -H 'Content-Type: application/json' -d '{}' \ + "${API_BASE}/updates/obstacle_classifier_v2_1_0_0/execute" >/dev/null +sleep 5 + +assert_process_running "obstacle_classifier_node" "obstacle_classifier_node spawned after install" + +section "Uninstall flow: PUT /updates/broken_lidar_legacy_remove/prepare + /execute" + +curl -fsS -X PUT -H 'Content-Type: application/json' -d '{}' \ + "${API_BASE}/updates/broken_lidar_legacy_remove/prepare" >/dev/null +sleep 4 +curl -fsS -X PUT -H 'Content-Type: application/json' -d '{}' \ + "${API_BASE}/updates/broken_lidar_legacy_remove/execute" >/dev/null +sleep 5 + +if api_get "/updates/broken_lidar_legacy_remove/status"; then + if echo "$RESPONSE" | jq -e '.status == "completed"' >/dev/null 2>&1; then + pass "broken_lidar_legacy_remove status is completed" + else + fail "broken_lidar_legacy_remove status is completed" "got $(echo "$RESPONSE" | jq -c .)" + fi +fi + +assert_process_gone "/lib/broken_lidar_legacy/broken_lidar_legacy" "broken_lidar_legacy killed after uninstall"