diff --git a/.buildkite/pipeline.yml b/.buildkite/pipeline.yml
new file mode 100644
index 0000000..0ccc0f3
--- /dev/null
+++ b/.buildkite/pipeline.yml
@@ -0,0 +1,67 @@
+agents:
+  queue: "openmowernext-ci"
+
+env:
+  ROS_DISTRO: "jazzy"
+
+steps:
+  - label: "Unit"
+    key: "unit"
+    command: ".buildkite/scripts/ci-unit.sh"
+    timeout_in_minutes: 120
+    artifact_paths:
+      - "build/open_mower_next/test_results/**/*"
+      - "log/**/*"
+
+  - label: "Docs"
+    key: "docs"
+    command: ".buildkite/scripts/docs-build.sh"
+    timeout_in_minutes: 30
+    if_changed:
+      - "docs/**"
+      - ".buildkite/**"
+    artifact_paths:
+      - "docs/.vitepress/dist/**/*"
+
+  - label: "Runtime image"
+    key: "runtime-image"
+    command: ".buildkite/scripts/docker-build.sh runtime"
+    timeout_in_minutes: 180
+    if_changed:
+      - "Dockerfile"
+      - "Makefile"
+      - "package.xml"
+      - "custom_deps.yaml"
+      - "cmake/**"
+      - "config/**"
+      - "description/**"
+      - "launch/**"
+      - "maps/**"
+      - "protos/**"
+      - "src/**"
+      - "utils/**"
+      - "worlds/**"
+
+  - label: "Devcontainer image"
+    key: "devcontainer-image"
+    command: ".buildkite/scripts/docker-build.sh devcontainer"
+    timeout_in_minutes: 60
+    if_changed:
+      - ".devcontainer/**"
+
+  - label: "Integration"
+    key: "integration"
+    command: ".buildkite/scripts/ci-integration.sh"
+    agents:
+      queue: "openmowernext-webots"
+    timeout_in_minutes: 180
+    concurrency: 1
+    concurrency_group: "openmowernext/webots"
+    if: |
+      build.source == "ui" ||
+      build.source == "api" ||
+      build.source == "schedule" ||
+      build.pull_request.labels includes "integration"
+    artifact_paths:
+      - "build/open_mower_next/test_results/**/*"
+      - "log/**/*"
diff --git a/.buildkite/scripts/ci-integration.sh b/.buildkite/scripts/ci-integration.sh
new file mode 100755
index 0000000..edbc627
--- /dev/null
+++ b/.buildkite/scripts/ci-integration.sh
@@ -0,0 +1,64 @@
+#!/usr/bin/env bash
+
+set -euo pipefail
+
+docker run --rm \
+  --volume "$PWD:/workspace" \
+  --workdir /workspace \
+  --env ROS_DISTRO=jazzy \
+  --env DEBIAN_FRONTEND=noninteractive \
+  --env WEBOTS_OFFSCREEN=1 \
+  ros:jazzy \
+  bash -lc '
+    set -eo pipefail
+
+    apt-get update
+    apt-get install -y --no-install-recommends \
+      build-essential \
+      bzip2 \
+      ca-certificates \
+      cmake \
+      curl \
+      git \
+      libxcb-cursor0 \
+      python3-colcon-common-extensions \
+      python3-rosdep \
+      python3-vcstool \
+      xvfb
+
+    rosdep init >/dev/null 2>&1 || true
+    rosdep update
+
+    webots_root="${HOME}/.ros/webotsR2025a"
+    webots_home="${webots_root}/webots"
+    if [ ! -x "${webots_home}/webots" ]; then
+      rm -rf "${webots_root}"
+      mkdir -p "${webots_root}"
+      curl -L -o /tmp/webots-R2025a-x86-64.tar.bz2 \
+        https://github.com/cyberbotics/webots/releases/download/R2025a/webots-R2025a-x86-64.tar.bz2
+      tar -xjf /tmp/webots-R2025a-x86-64.tar.bz2 -C "${webots_root}"
+    fi
+    test -x "${webots_home}/webots"
+
+    export WEBOTS_HOME="${webots_home}"
+    export WEBOTS_OFFSCREEN=1
+
+    source /opt/ros/${ROS_DISTRO}/setup.bash
+    make custom-deps deps
+
+    make build-libs
+    source install/setup.bash
+    colcon build \
+      --symlink-install \
+      --packages-select open_mower_next \
+      --cmake-args -DOPEN_MOWER_NEXT_ENABLE_INTEGRATION_TESTS=ON
+
+    source install/setup.bash
+    colcon test \
+      --packages-select open_mower_next \
+      --event-handlers console_direct+ \
+      --ctest-args -L integration --output-on-failure
+    colcon test-result \
+      --test-result-base build/open_mower_next/test_results \
+      --verbose
+  '
diff --git a/.buildkite/scripts/ci-unit.sh b/.buildkite/scripts/ci-unit.sh
new file mode 100755
index 0000000..e858ac7
--- /dev/null
+++ b/.buildkite/scripts/ci-unit.sh
@@ -0,0 +1,44 @@
+#!/usr/bin/env bash
+
+set -euo pipefail
+
+docker run --rm \
+  --volume "$PWD:/workspace" \
+  --workdir /workspace \
+  --env ROS_DISTRO=jazzy \
+  --env DEBIAN_FRONTEND=noninteractive \
+  ros:jazzy \
+  bash -lc '
+    set -eo pipefail
+
+    apt-get update
+    apt-get install -y --no-install-recommends \
+      build-essential \
+      ca-certificates \
+      cmake \
+      curl \
+      git \
+      python3-colcon-common-extensions \
+      python3-rosdep \
+      python3-vcstool
+
+    rosdep init >/dev/null 2>&1 || true
+    rosdep update
+
+    source /opt/ros/${ROS_DISTRO}/setup.bash
+    make custom-deps deps
+
+    make build-libs
+
+    source install/setup.bash
+    colcon build --symlink-install --packages-select open_mower_next
+
+    source install/setup.bash
+    colcon test \
+      --packages-select open_mower_next \
+      --event-handlers console_direct+ \
+      --ctest-args -LE integration --output-on-failure
+    colcon test-result \
+      --test-result-base build/open_mower_next/test_results \
+      --verbose
+  '
diff --git a/.buildkite/scripts/docker-build.sh b/.buildkite/scripts/docker-build.sh
new file mode 100755
index 0000000..3052a8f
--- /dev/null
+++ b/.buildkite/scripts/docker-build.sh
@@ -0,0 +1,30 @@
+#!/usr/bin/env bash
+
+set -euo pipefail
+
+target="${1:-runtime}"
+commit="${BUILDKITE_COMMIT:-local}"
+tag="${commit:0:12}"
+
+case "$target" in
+  runtime)
+    image="openmowernext"
+    dockerfile="Dockerfile"
+    context="."
+    ;;
+  devcontainer)
+    image="openmowernext-devcontainer"
+    dockerfile=".devcontainer/Dockerfile"
+    context=".devcontainer"
+    ;;
+  *)
+    echo "unknown docker build target: $target" >&2
+    exit 2
+    ;;
+esac
+
+docker build \
+  --pull \
+  --file "$dockerfile" \
+  --tag "${image}:buildkite-${tag}" \
+  "$context"
diff --git a/.buildkite/scripts/docs-build.sh b/.buildkite/scripts/docs-build.sh
new file mode 100755
index 0000000..e76de45
--- /dev/null
+++ b/.buildkite/scripts/docs-build.sh
@@ -0,0 +1,14 @@
+#!/usr/bin/env bash
+
+set -euo pipefail
+
+docker run --rm \
+  --volume "$PWD:/workspace" \
+  --workdir /workspace/docs \
+  node:18-bookworm \
+  bash -lc '
+    set -euo pipefail
+    npm ci
+    npm run docs:build
+    touch .vitepress/dist/.nojekyll
+  '
diff --git a/.buildkite/scripts/terraform-validate.sh b/.buildkite/scripts/terraform-validate.sh
new file mode 100755
index 0000000..81bf3cc
--- /dev/null
+++ b/.buildkite/scripts/terraform-validate.sh
@@ -0,0 +1,15 @@
+#!/usr/bin/env bash
+
+set -euo pipefail
+
+docker run --rm \
+  --volume "$PWD:/workspace" \
+  --workdir /workspace \
+  --entrypoint sh \
+  hashicorp/terraform:1.9.8 \
+  -lc '
+    set -eu
+    terraform -chdir=infra/buildkite fmt -check
+    terraform -chdir=infra/buildkite init -backend=false -input=false
+    terraform -chdir=infra/buildkite validate
+  '
diff --git a/.devcontainer/default.env b/.devcontainer/default.env
index efb69c6..95dddbf 100644
--- a/.devcontainer/default.env
+++ b/.devcontainer/default.env
@@ -1,3 +1,4 @@
+export ROS_AUTOMATIC_DISCOVERY_RANGE=${ROS_AUTOMATIC_DISCOVERY_RANGE:-LOCALHOST}
 export OM_DATUM_LAT=-22.9
 export OM_DATUM_LONG=-43.2
-export OM_MAP_PATH=.devcontainer/home/map.geojson
\ No newline at end of file
+export OM_MAP_PATH=maps/realistic_garden.geojson
diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml
deleted file mode 100644
index d01b40f..0000000
--- a/.github/workflows/build.yml
+++ /dev/null
@@ -1,55 +0,0 @@
-name: Build
-
-on:
-  workflow_dispatch:
-  push:
-    branches:
-      - 'main'
-  pull_request:
-    types: [opened, synchronize, reopened, labeled]
-    branches:
-      - 'main'
-
-permissions:
-  contents: read
-  packages: write
-  id-token: write
-
-jobs:
-  build:
-    name: Build
-    runs-on: ubuntu-latest
-    steps:
-      - name: Checkout
-        uses: actions/checkout@v4
-      - name: set lower case repository name
-        run: |
-          echo "REPOSITORY_LC=${REPOSITORY,,}" >>${GITHUB_ENV}
-        env:
-          REPOSITORY: '${{ github.repository }}'
-      - name: Docker meta
-        id: meta
-        uses: docker/metadata-action@v4
-        with:
-          images: |
-            ghcr.io/${{env.REPOSITORY_LC}}
-
-      - name: Login to GHCR
-        if: github.event_name != 'pull_request' || contains(github.event.pull_request.labels.*.name, 'push-image')
-        uses: docker/login-action@v2
-        with:
-          registry: ghcr.io
-          username: ${{ github.actor }}
-          password: ${{ secrets.GITHUB_TOKEN }}
-      - uses: depot/setup-action@v1
-      - name: Build and push image
-        uses: depot/build-push-action@v1
-        with:
-          project: l6n2tkl9g1
-          context: .
-          platforms: linux/amd64,linux/arm64
-          outputs: type=image,name=ghcr.io/${{env.REPOSITORY_LC}},push-by-digest=true,name-canonical=true,push=${{ github.event_name != 'pull_request' || contains(github.event.pull_request.labels.*.name, 'push-image') }}
-          push: ${{ github.event_name != 'pull_request' || contains(github.event.pull_request.labels.*.name, 'push-image') }}
-          labels: ${{ steps.meta.outputs.labels }}
-          tags: ${{ steps.meta.outputs.tags }}
-
diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml
deleted file mode 100644
index 25b8d7a..0000000
--- a/.github/workflows/ci.yml
+++ /dev/null
@@ -1,134 +0,0 @@
-name: CI
-
-on:
-  workflow_dispatch:
-  push:
-    branches:
-      - main
-  pull_request:
-    types: [opened, synchronize, reopened, labeled]
-    branches:
-      - main
-
-permissions:
-  contents: read
-
-env:
-  ROS_DISTRO: jazzy
-
-jobs:
-  unit:
-    name: Unit
-    runs-on: ubuntu-24.04
-    steps:
-      - name: Checkout
-        uses: actions/checkout@v4
-
-      - name: Setup ROS
-        uses: ros-tooling/setup-ros@v0.7
-        with:
-          required-ros-distributions: jazzy
-
-      - name: Install tooling
-        run: |
-          sudo apt-get update
-          sudo apt-get install -y python3-colcon-common-extensions python3-vcstool
-          sudo rosdep init || true
-          rosdep update
-
-      - name: Install dependencies
-        shell: bash
-        run: |
-          source /opt/ros/${ROS_DISTRO}/setup.bash
-          make custom-deps deps
-
-      - name: Build external libraries
-        shell: bash
-        run: |
-          source /opt/ros/${ROS_DISTRO}/setup.bash
-          make build-libs
-
-      - name: Build package
-        shell: bash
-        run: |
-          source /opt/ros/${ROS_DISTRO}/setup.bash
-          source install/setup.bash
-          colcon build --symlink-install --packages-select open_mower_next
-
-      - name: Run unit tests
-        shell: bash
-        run: |
-          source /opt/ros/${ROS_DISTRO}/setup.bash
-          source install/setup.bash
-          colcon test --packages-select open_mower_next --event-handlers console_direct+ --ctest-args -LE integration --output-on-failure
-          colcon test-result --test-result-base build/open_mower_next/test_results --verbose
-
-  integration:
-    name: Integration
-    runs-on: ubuntu-24.04
-    if: github.event_name == 'workflow_dispatch' || (github.event_name == 'pull_request' && contains(github.event.pull_request.labels.*.name, 'integration'))
-    steps:
-      - name: Checkout
-        uses: actions/checkout@v4
-
-      - name: Setup ROS
-        uses: ros-tooling/setup-ros@v0.7
-        with:
-          required-ros-distributions: jazzy
-
-      - name: Install tooling and Webots runtime dependencies
-        run: |
-          sudo apt-get update
-          sudo apt-get install -y python3-colcon-common-extensions python3-vcstool xvfb libxcb-cursor0
-          sudo rosdep init || true
-          rosdep update
-
-      - name: Cache Webots
-        uses: actions/cache@v4
-        with:
-          path: ~/.ros/webotsR2025a
-          key: webots-R2025a-linux-x86-64
-
-      - name: Install Webots
-        shell: bash
-        run: |
-          webots_root="${HOME}/.ros/webotsR2025a"
-          webots_home="${webots_root}/webots"
-          if [ ! -x "${webots_home}/webots" ]; then
-            rm -rf "${webots_root}"
-            mkdir -p "${webots_root}"
-            curl -L -o /tmp/webots-R2025a-x86-64.tar.bz2 \
-              https://github.com/cyberbotics/webots/releases/download/R2025a/webots-R2025a-x86-64.tar.bz2
-            tar -xjf /tmp/webots-R2025a-x86-64.tar.bz2 -C "${webots_root}"
-          fi
-          test -x "${webots_home}/webots"
-
-      - name: Install dependencies
-        shell: bash
-        run: |
-          source /opt/ros/${ROS_DISTRO}/setup.bash
-          make custom-deps deps
-
-      - name: Build external libraries
-        shell: bash
-        run: |
-          source /opt/ros/${ROS_DISTRO}/setup.bash
-          make build-libs
-
-      - name: Build package with integration tests
-        shell: bash
-        run: |
-          source /opt/ros/${ROS_DISTRO}/setup.bash
-          source install/setup.bash
-          colcon build --symlink-install --packages-select open_mower_next --cmake-args -DOPEN_MOWER_NEXT_ENABLE_INTEGRATION_TESTS=ON
-
-      - name: Run integration tests
-        shell: bash
-        env:
-          WEBOTS_HOME: /home/runner/.ros/webotsR2025a/webots
-          WEBOTS_OFFSCREEN: "1"
-        run: |
-          source /opt/ros/${ROS_DISTRO}/setup.bash
-          source install/setup.bash
-          colcon test --packages-select open_mower_next --event-handlers console_direct+ --ctest-args -L integration --output-on-failure
-          colcon test-result --test-result-base build/open_mower_next/test_results --verbose
diff --git a/.github/workflows/devcontainer.yml b/.github/workflows/devcontainer.yml
deleted file mode 100644
index 426d19a..0000000
--- a/.github/workflows/devcontainer.yml
+++ /dev/null
@@ -1,67 +0,0 @@
-name: Build devcontainer
-
-on:
-  workflow_dispatch:
-  push:
-    branches:
-      - 'main'
-    paths:
-      - '.devcontainer/Dockerfile'
-      - '.devcontainer/**'
-  pull_request:
-    types:
-      - opened
-      - synchronize
-      - reopened
-      - labeled
-    paths:
-      - '.devcontainer/Dockerfile'
-      - '.devcontainer/**'
-
-concurrency:
-  group: ci-buildtrain-${{ github.ref }}-devcontainer
-  cancel-in-progress: true
-
-permissions:
-  contents: read
-  packages: write
-  id-token: write
-
-jobs:
-  build:
-    name: Build
-    runs-on: ubuntu-latest
-    steps:
-      - name: Checkout
-        uses: actions/checkout@v4
-      - name: set lower case repository name
-        run: |
-          echo "REPOSITORY_LC=${REPOSITORY,,}-devcontainer" >>${GITHUB_ENV}
-        env:
-          REPOSITORY: '${{ github.repository }}'
-      - name: Docker meta
-        id: meta
-        uses: docker/metadata-action@v4
-        with:
-          images: |
-            ghcr.io/${{env.REPOSITORY_LC}}
-
-      - name: Login to GHCR
-        if: github.event_name == 'push' || (github.event_name == 'pull_request' && contains(github.event.pull_request.labels.*.name, 'devcontainer:push-image'))
-        uses: docker/login-action@v2
-        with:
-          registry: ghcr.io
-          username: ${{ github.repository_owner }}
-          password: ${{ secrets.GITHUB_TOKEN }}
-      - uses: depot/setup-action@v1
-      - name: Build and push image
-        uses: depot/build-push-action@v1
-        with:
-          project: l6n2tkl9g1
-          context: .devcontainer/
-          file: .devcontainer/Dockerfile
-          platforms: linux/amd64,linux/arm64
-          outputs: type=image,name=ghcr.io/${{env.REPOSITORY_LC}},push-by-digest=true,name-canonical=true,push=${{ github.event_name == 'push' || (github.event_name == 'pull_request' && contains(github.event.pull_request.labels.*.name, 'devcontainer:push-image')) }}
-          push: ${{ github.event_name == 'push' || (github.event_name == 'pull_request' && contains(github.event.pull_request.labels.*.name, 'devcontainer:push-image')) }}
-          labels: ${{ steps.meta.outputs.labels }}
-          tags: ${{ steps.meta.outputs.tags }}
diff --git a/.github/workflows/docs.yml b/.github/workflows/docs.yml
deleted file mode 100644
index 4726b10..0000000
--- a/.github/workflows/docs.yml
+++ /dev/null
@@ -1,69 +0,0 @@
-name: Docs
-
-on:
-  push:
-    branches: [main]
-    paths:
-      - docs/**
-      - .github/workflows/docs.yml
-  pull_request:
-    types:
-      - opened
-      - synchronize
-      - reopened
-      - labeled
-    paths:
-      - docs/**
-      - .github/workflows/docs.yml
-  workflow_dispatch:
-
-permissions:
-  contents: read
-  pages: write
-  id-token: write
-
-concurrency:
-  group: pages
-  cancel-in-progress: false
-
-jobs:
-  build:
-    runs-on: ubuntu-latest
-    steps:
-      - name: Checkout
-        uses: actions/checkout@v4
-        with:
-          fetch-depth: 0
-      - name: Setup Node
-        uses: actions/setup-node@v4
-        with:
-          node-version: 18
-          cache: npm
-          cache-dependency-path: docs/package-lock.json
-      - name: Setup Pages
-        uses: actions/configure-pages@v5
-      - name: Install dependencies
-        run: npm ci
-        working-directory: docs
-      - name: Build with VitePress
-        run: |
-          npm run docs:build
-          touch .vitepress/dist/.nojekyll
-        working-directory: docs
-      - name: Upload artifact
-        uses: actions/upload-pages-artifact@v3
-        with:
-          path: docs/.vitepress/dist
-
-  deploy:
-    environment:
-      name: github-pages
-      url: ${{ steps.deployment.outputs.page_url }}
-    needs: build
-    runs-on: ubuntu-latest
-    name: Deploy
-    if: github.event_name == 'push' && github.ref == 'refs/heads/main' || (github.event_name == 'pull_request' && contains(github.event.pull_request.labels.*.name, 'docs:deploy'))
-    steps:
-      - name: Deploy to GitHub Pages
-        id: deployment
-        uses: actions/deploy-pages@v4
\ No newline at end of file
diff --git a/.gitignore b/.gitignore
index cfce021..e25fe49 100644
--- a/.gitignore
+++ b/.gitignore
@@ -13,3 +13,16 @@ cmake-build-debug*/
 /.name
 /.idea
 .DS_Store
+
+# Terraform
+**/.terraform/
+*.tfstate
+*.tfstate.*
+terraform.tfvars
+terraform.tfvars.json
+*.tfplan
+crash.log
+override.tf
+override.tf.json
+*_override.tf
+*_override.tf.json
diff --git a/AGENTS.md b/AGENTS.md
index bcb8c40..bdc002a 100644
--- a/AGENTS.md
+++ b/AGENTS.md
@@ -24,6 +24,8 @@
 - Sim launch from repo root: `make sim`; it sources ROS, the installed workspace, `.devcontainer/default.env`, then launches `open_mower_next sim.launch.py` with Webots.
 - Headless Webots smoke launch: `WEBOTS_OFFSCREEN=1 ros2 launch open_mower_next sim.launch.py gui:=false mode:=fast` after sourcing ROS and `install/setup.bash`.
 - ROS MCP rosbridge foreground launch: `make rosbridge`; user service setup: `make rosbridge-service-enable`.
+- `omdev.local` hardware iteration uses a long-lived `docker.io/library/ros:jazzy` dev container, not the GHCR production image: `make omdev-sync`, `make omdev-dev-start`, `make omdev-deps`, `make omdev-build`, `make omdev-run-workspace`, `make omdev-logs`.
+- If `omdev-build` reports stale generated CMake paths after changing the dev container image, run `make omdev-clean` before rebuilding.
 - Direct ROS package launches use `open_mower_next`, e.g. `ros2 launch open_mower_next openmower.launch.py` or `ros2 launch open_mower_next sim.launch.py`.
 - Docs are isolated under `docs/`: `cd docs && npm ci && npm run docs:build`; dev server is `npm run docs:dev`.
 
@@ -41,8 +43,11 @@
 - `launch/rsp.launch.py` references package `open_mower_ros`, unlike the rest of the repo; verify before relying on `make rsp`.
 - If moving the docking plugin XML, update `src/docking_helper/plugins.xml`, `cmake/docking_helper.cmake`, `package.xml`, and the manual copy in `Dockerfile`.
 - The runtime Docker entrypoint creates an empty GeoJSON map when `OM_MAP_PATH` is missing; normal dev shells do not.
+- Do not use `ghcr.io/jkaflik/openmowernext:*` as the default `omdev.local` iteration image; it can carry stale dependency headers. Use the mutable `docker.io/library/ros:jazzy` dev container and refresh deps from the synced workspace with `make omdev-deps`.
+- Do not run `make custom-deps` inside the `omdev.local` dev container; sync `src/lib` from the dev machine and use `rosdep`/`colcon` there so local vendored branches are not overwritten by `vcs import --force`.
 - Webots is the only supported simulation backend; Gazebo assets and `ros_gz` bridge logic are legacy and should not be reintroduced.
 - Webots launch depends on `webots_ros2_driver`, `webots_ros2_control`, and a Webots binary available on the host.
 - If `webots_ros2_driver` auto-installs Webots, it usually lands in `~/.ros/webotsR2025a/webots`; export `WEBOTS_HOME` to that path or use `make sim`.
 - Project OpenCode config starts `ros-mcp` with `uvx`; restart OpenCode after config changes or after installing `uv`.
 - Rosbridge for MCP binds to `127.0.0.1:9090` by default; prefer SSH tunneling over binding it to the LAN.
+- Do not glob/grep against `/` like: `Glob "/opt/ros/jazzy/**/webots_ros2_driver/**/*.py" in / `. Use highest-level path possible and avoid scanning the whole filesystem; the above example run in `/opt/ros/jazzy` would be `Glob "**/webots_ros2_driver/**/*.py" in /opt/ros/jazzy`.
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 89e7dd8..dfaeede 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -17,9 +17,11 @@ find_package(rosidl_default_generators REQUIRED)
 find_package(geometry_msgs REQUIRED)
 find_package(nav_msgs REQUIRED)
 find_package(nav2_msgs REQUIRED)
+find_package(geographic_msgs REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(omros2_firmware_msgs REQUIRED)
 find_package(std_msgs REQUIRED)
 find_package(std_srvs REQUIRED)
-find_package(robot_localization REQUIRED)
 find_package(tf2 REQUIRED)
 find_package(tf2_ros REQUIRED)
 find_package(tf2_geometry_msgs REQUIRED)
@@ -73,8 +75,21 @@ include(cmake/docking_helper.cmake)
 include(cmake/map_recorder.cmake)
 include(cmake/coverage_server.cmake)
 
+install(
+        PROGRAMS scripts/calibrate_robot.py
+        DESTINATION lib/${PROJECT_NAME}
+        RENAME calibrate_robot
+)
+
+install(
+        PROGRAMS scripts/webots_world_to_geojson.py
+        DESTINATION lib/${PROJECT_NAME}
+        RENAME webots_world_to_geojson
+)
+
 if (BUILD_TESTING)
     find_package(ament_lint_auto REQUIRED)
+    find_package(ament_cmake_pytest REQUIRED)
     # the following line skips the linter which checks for copyrights
     # comment the line when a copyright and license is added to all source files
     set(ament_cmake_copyright_FOUND TRUE)
@@ -90,10 +105,16 @@ if (BUILD_TESTING)
     set(ament_cmake_xmllint_FOUND TRUE)
     ament_lint_auto_find_test_dependencies()
 
+    ament_add_pytest_test(calibrate_robot test/test_calibrate_robot.py TIMEOUT 30)
+    ament_add_pytest_test(webots_world_to_geojson test/test_webots_world_to_geojson.py TIMEOUT 30)
+
     if (OPEN_MOWER_NEXT_ENABLE_INTEGRATION_TESTS)
-        find_package(ament_cmake_pytest REQUIRED)
         ament_add_pytest_test(webots_smoke test/integration/test_webots_smoke.py TIMEOUT 180)
         set_tests_properties(webots_smoke PROPERTIES LABELS integration RESOURCE_LOCK webots)
+        ament_add_pytest_test(gps_rotation_heading test/integration/test_gps_rotation_heading.py TIMEOUT 360)
+        set_tests_properties(gps_rotation_heading PROPERTIES LABELS integration RESOURCE_LOCK webots)
+        ament_add_pytest_test(gps_degraded test/integration/test_gps_degraded.py TIMEOUT 480)
+        set_tests_properties(gps_degraded PROPERTIES LABELS integration RESOURCE_LOCK webots)
         ament_add_pytest_test(navigation_docking test/integration/test_navigation_docking.py TIMEOUT 420)
         set_tests_properties(navigation_docking PROPERTIES LABELS integration RESOURCE_LOCK webots)
     endif ()
diff --git a/Makefile b/Makefile
index 2cc624a..11284e8 100644
--- a/Makefile
+++ b/Makefile
@@ -1,7 +1,49 @@
-REMOTE_HOST ?= omdev.local
+REMOTE_HOST ?= 10.0.250.5
 REMOTE_USER ?= openmower
 ROS_DISTRO ?= jazzy
+ROS_AUTOMATIC_DISCOVERY_RANGE ?= LOCALHOST
 ROS_LOG_DIR = log/
+OMDEV_HOST ?= $(REMOTE_HOST)
+OMDEV_USER ?= $(USER)
+OMDEV_SSH ?= $(OMDEV_USER)@$(OMDEV_HOST)
+OMDEV_SSH_OPTS ?=
+OMDEV_RSYNC_SSH ?= ssh $(OMDEV_SSH_OPTS)
+OMDEV_WORKSPACE ?= /home/$(OMDEV_USER)/OpenMowerNext
+OMDEV_MAP_DIR ?= /home/$(OMDEV_USER)/next
+OMDEV_ENV_FILE ?= /home/$(OMDEV_USER)/omnext/openmower.env
+OMDEV_IMAGE ?= docker.io/library/ros:jazzy
+OMDEV_BUILD_IMAGE ?= $(OMDEV_IMAGE)
+OMDEV_CONTAINER_WORKSPACE ?= /target_ws
+OMROS2_FIRMWARE_MSGS_BASE_PATH ?= src/lib/omros2_firmware/extra_packages/omros2_firmware_msgs
+OMDEV_FIELDS2COVER_BASE_PATHS ?= src/lib/fields2cover
+OMDEV_FIELDS2COVER_PACKAGES ?= --packages-select fields2cover
+OMDEV_LIB_BASE_PATHS ?= src/lib/vesc src/lib/micro_ros_agent src/lib/ntrip_client src/lib/fusioncore/compass_msgs src/lib/fusioncore/fusioncore_core src/lib/fusioncore/fusioncore_ros src/lib/ublox_f9p $(OMROS2_FIRMWARE_MSGS_BASE_PATH)
+OMDEV_LIB_PACKAGES ?= --packages-select vesc_msgs vesc_driver vesc_hw_interface vesc micro_ros_agent ntrip_client compass_msgs fusioncore_core fusioncore_ros ublox_f9p omros2_firmware_msgs
+OMDEV_APP_PACKAGES ?= --packages-select open_mower_next
+OMDEV_CMAKE_ARGS ?= --cmake-args -DBUILD_TESTING=OFF
+OMDEV_FIELDS2COVER_CMAKE_ARGS ?= --cmake-args -DBUILD_TESTING=OFF -DBUILD_TUTORIALS=OFF -DBUILD_PYTHON=OFF -DBUILD_DOC=OFF
+OMDEV_LIB_CMAKE_ARGS ?= $(OMDEV_CMAKE_ARGS)
+OMDEV_APP_CMAKE_ARGS ?= $(OMDEV_CMAKE_ARGS)
+OMDEV_LAUNCH_ARGS ?=
+OMDEV_CONTAINER ?= next-dev
+OMDEV_PODMAN ?= sudo podman
+OMDEV_TOOL_DEPS ?= python3-colcon-common-extensions python3-vcstool python3-rosdep
+OMDEV_ROSDEP_PATHS ?= . $(OMDEV_FIELDS2COVER_BASE_PATHS) $(OMDEV_LIB_BASE_PATHS)
+OMDEV_ROSDEP_SKIP_KEYS ?= webots_ros2_control webots_ros2_driver
+OMDEV_LAUNCH_LOG ?= log/omdev-launch.log
+OMDEV_LAUNCH_PID ?= /tmp/openmowernext-launch.pid
+OMDEV_CLEAN_PATHS ?= build install log
+OMDEV_RSYNC_DELETE ?=
+OMDEV_RSYNC_EXCLUDES = \
+	--exclude .git/ \
+	--exclude '**/.git/' \
+	--exclude build/ \
+	--exclude install/ \
+	--exclude log/ \
+	--exclude .cache/ \
+	--exclude .pytest_cache/ \
+	--exclude docs/node_modules/ \
+	--exclude docs/.vitepress/dist/
 ROSBRIDGE_ADDRESS ?= 127.0.0.1
 ROSBRIDGE_PORT ?= 9090
 ROSBRIDGE_SERVICE ?= openmower-rosbridge.service
@@ -11,12 +53,13 @@ FOXGLOVE_SERVICE ?= openmower-foxglove.service
 FOXGLOVE_USE_SIM_TIME ?= false
 WEBOTS_STREAM ?= true
 WEBOTS_PORT ?= 1234
+CALIBRATE_ARGS ?=
 SYSTEMD_USER_DIR ?= $(HOME)/.config/systemd/user
 SHELL := /bin/bash
 
 all: custom-deps deps build
 
-.PHONY: deps custom-deps build-libs build build-release sim run dev run-foxglove foxglove foxglove-deps foxglove-service-install foxglove-service-enable foxglove-service-disable foxglove-service-restart foxglove-service-status foxglove-service-logs rsp remote-devices rosbridge rosbridge-deps rosbridge-service-install rosbridge-service-enable rosbridge-service-disable rosbridge-service-restart rosbridge-service-status rosbridge-service-logs
+.PHONY: deps custom-deps build-libs build build-release sim run calibrate dev run-foxglove foxglove foxglove-deps foxglove-service-install foxglove-service-enable foxglove-service-disable foxglove-service-restart foxglove-service-status foxglove-service-logs rsp remote-devices omdev omdev-sync omdev-pull omdev-dev-create omdev-dev-recreate omdev-dev-start omdev-dev-stop omdev-deps omdev-clean omdev-run omdev-run-workspace omdev-restart omdev-stop omdev-logs omdev-status omdev-shell omdev-calibrate omdev-build rosbridge rosbridge-deps rosbridge-service-install rosbridge-service-enable rosbridge-service-disable rosbridge-service-restart rosbridge-service-status rosbridge-service-logs
 
 deps:
 	rosdep install --from-paths . src/lib --ignore-src -i -y -r
@@ -28,17 +71,17 @@ build-libs:
 	colcon build --base-paths "src/lib/*" --cmake-args -DBUILD_TESTING=OFF
 
 build:
-	colcon build --symlink-install
+	colcon build --symlink-install --base-paths . $(OMROS2_FIRMWARE_MSGS_BASE_PATH)
 
 build-release:
 	colcon build --base-paths "src/lib/*" --cmake-args -DCMAKE_BUILD_TYPE=Release
-	colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release
+	source install/setup.bash && colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release
 
 sim:
-	bash -lc 'if [ -z "$${WEBOTS_HOME}" ] && [ -d "$${HOME}/.ros/webotsR2025a/webots" ]; then export WEBOTS_HOME="$${HOME}/.ros/webotsR2025a/webots"; fi && if [ -z "$${DISPLAY}" ] && [ -z "$${WEBOTS_OFFSCREEN}" ]; then export WEBOTS_OFFSCREEN=1; fi && source /opt/ros/$${ROS_DISTRO:-jazzy}/setup.bash && source install/setup.bash && set -a && source .devcontainer/default.env && set +a && ros2 launch open_mower_next sim.launch.py webots_stream:="$(WEBOTS_STREAM)" webots_port:="$(WEBOTS_PORT)"'
+	ROS_AUTOMATIC_DISCOVERY_RANGE="$(ROS_AUTOMATIC_DISCOVERY_RANGE)" bash -lc 'if [ -z "$${WEBOTS_HOME}" ] && [ -d "$${HOME}/.ros/webotsR2025a/webots" ]; then export WEBOTS_HOME="$${HOME}/.ros/webotsR2025a/webots"; fi && if [ -z "$${DISPLAY}" ] && [ -z "$${WEBOTS_OFFSCREEN}" ]; then export WEBOTS_OFFSCREEN=1; fi && source /opt/ros/$${ROS_DISTRO:-jazzy}/setup.bash && source install/setup.bash && set -a && source .devcontainer/default.env && set +a && ros2 launch open_mower_next sim.launch.py webots_stream:="$(WEBOTS_STREAM)" webots_port:="$(WEBOTS_PORT)"'
 
 rosbridge:
-	ROS_DISTRO="$(ROS_DISTRO)" ROSBRIDGE_ADDRESS="$(ROSBRIDGE_ADDRESS)" ROSBRIDGE_PORT="$(ROSBRIDGE_PORT)" bash utils/run-rosbridge.sh
+	ROS_DISTRO="$(ROS_DISTRO)" ROS_AUTOMATIC_DISCOVERY_RANGE="$(ROS_AUTOMATIC_DISCOVERY_RANGE)" ROSBRIDGE_ADDRESS="$(ROSBRIDGE_ADDRESS)" ROSBRIDGE_PORT="$(ROSBRIDGE_PORT)" bash utils/run-rosbridge.sh
 
 rosbridge-deps:
 	sudo apt update
@@ -46,7 +89,7 @@ rosbridge-deps:
 
 rosbridge-service-install:
 	install -d "$(SYSTEMD_USER_DIR)"
-	sed -e 's|@WORKSPACE@|$(CURDIR)|g' -e 's|@ROS_DISTRO@|$(ROS_DISTRO)|g' -e 's|@ROSBRIDGE_ADDRESS@|$(ROSBRIDGE_ADDRESS)|g' -e 's|@ROSBRIDGE_PORT@|$(ROSBRIDGE_PORT)|g' systemd/openmower-rosbridge.service.in > "$(SYSTEMD_USER_DIR)/$(ROSBRIDGE_SERVICE)"
+	sed -e 's|@WORKSPACE@|$(CURDIR)|g' -e 's|@ROS_DISTRO@|$(ROS_DISTRO)|g' -e 's|@ROS_AUTOMATIC_DISCOVERY_RANGE@|$(ROS_AUTOMATIC_DISCOVERY_RANGE)|g' -e 's|@ROSBRIDGE_ADDRESS@|$(ROSBRIDGE_ADDRESS)|g' -e 's|@ROSBRIDGE_PORT@|$(ROSBRIDGE_PORT)|g' systemd/openmower-rosbridge.service.in > "$(SYSTEMD_USER_DIR)/$(ROSBRIDGE_SERVICE)"
 	systemctl --user daemon-reload
 	@printf 'Installed %s in %s\n' "$(ROSBRIDGE_SERVICE)" "$(SYSTEMD_USER_DIR)"
 
@@ -66,7 +109,7 @@ rosbridge-service-logs:
 	journalctl --user -u "$(ROSBRIDGE_SERVICE)" -f
 
 foxglove:
-	ROS_DISTRO="$(ROS_DISTRO)" FOXGLOVE_ADDRESS="$(FOXGLOVE_ADDRESS)" FOXGLOVE_PORT="$(FOXGLOVE_PORT)" FOXGLOVE_USE_SIM_TIME="$(FOXGLOVE_USE_SIM_TIME)" bash utils/run-foxglove.sh
+	ROS_DISTRO="$(ROS_DISTRO)" ROS_AUTOMATIC_DISCOVERY_RANGE="$(ROS_AUTOMATIC_DISCOVERY_RANGE)" FOXGLOVE_ADDRESS="$(FOXGLOVE_ADDRESS)" FOXGLOVE_PORT="$(FOXGLOVE_PORT)" FOXGLOVE_USE_SIM_TIME="$(FOXGLOVE_USE_SIM_TIME)" bash utils/run-foxglove.sh
 
 foxglove-deps:
 	sudo apt update
@@ -74,7 +117,7 @@ foxglove-deps:
 
 foxglove-service-install:
 	install -d "$(SYSTEMD_USER_DIR)"
-	sed -e 's|@WORKSPACE@|$(CURDIR)|g' -e 's|@ROS_DISTRO@|$(ROS_DISTRO)|g' -e 's|@FOXGLOVE_ADDRESS@|$(FOXGLOVE_ADDRESS)|g' -e 's|@FOXGLOVE_PORT@|$(FOXGLOVE_PORT)|g' -e 's|@FOXGLOVE_USE_SIM_TIME@|$(FOXGLOVE_USE_SIM_TIME)|g' systemd/openmower-foxglove.service.in > "$(SYSTEMD_USER_DIR)/$(FOXGLOVE_SERVICE)"
+	sed -e 's|@WORKSPACE@|$(CURDIR)|g' -e 's|@ROS_DISTRO@|$(ROS_DISTRO)|g' -e 's|@ROS_AUTOMATIC_DISCOVERY_RANGE@|$(ROS_AUTOMATIC_DISCOVERY_RANGE)|g' -e 's|@FOXGLOVE_ADDRESS@|$(FOXGLOVE_ADDRESS)|g' -e 's|@FOXGLOVE_PORT@|$(FOXGLOVE_PORT)|g' -e 's|@FOXGLOVE_USE_SIM_TIME@|$(FOXGLOVE_USE_SIM_TIME)|g' systemd/openmower-foxglove.service.in > "$(SYSTEMD_USER_DIR)/$(FOXGLOVE_SERVICE)"
 	systemctl --user daemon-reload
 	@printf 'Installed %s in %s\n' "$(FOXGLOVE_SERVICE)" "$(SYSTEMD_USER_DIR)"
 
@@ -96,6 +139,8 @@ foxglove-service-logs:
 run:
 	ros2 launch launch/openmower.launch.py
 
+calibrate: omdev-calibrate
+
 dev:
 	cd .devcontainer && docker-compose up -d
 
@@ -107,3 +152,56 @@ rsp:
 
 remote-devices:
 	bash .devcontainer/scripts/remote_devices.sh $(REMOTE_HOST) $(REMOTE_USER)
+
+omdev: omdev-run-workspace
+
+omdev-sync:
+	ssh $(OMDEV_SSH_OPTS) $(OMDEV_SSH) 'mkdir -p "$(OMDEV_WORKSPACE)" "$(OMDEV_MAP_DIR)" "$$(dirname "$(OMDEV_ENV_FILE)")"'
+	rsync -az --human-readable --info=progress2 $(OMDEV_RSYNC_DELETE) $(OMDEV_RSYNC_EXCLUDES) -e '$(OMDEV_RSYNC_SSH)' ./ "$(OMDEV_SSH):$(OMDEV_WORKSPACE)/"
+
+omdev-pull:
+	ssh $(OMDEV_SSH_OPTS) $(OMDEV_SSH) '$(OMDEV_PODMAN) pull "$(OMDEV_IMAGE)"'
+
+omdev-dev-create:
+	ssh $(OMDEV_SSH_OPTS) $(OMDEV_SSH) 'set -e; mkdir -p "$(OMDEV_WORKSPACE)" "$(OMDEV_MAP_DIR)" "$$(dirname "$(OMDEV_ENV_FILE)")"; if ! $(OMDEV_PODMAN) container exists "$(OMDEV_CONTAINER)"; then env_args=""; if [ -f "$(OMDEV_ENV_FILE)" ]; then env_args="--env-file $(OMDEV_ENV_FILE)"; else echo "warning: $(OMDEV_ENV_FILE) not found; using image defaults for datum"; fi; $(OMDEV_PODMAN) pull "$(OMDEV_IMAGE)"; $(OMDEV_PODMAN) create --name "$(OMDEV_CONTAINER)" --privileged --network host --entrypoint /bin/bash -v /dev:/dev -v "$(OMDEV_WORKSPACE):$(OMDEV_CONTAINER_WORKSPACE)" -v "$(OMDEV_MAP_DIR):/next" -w "$(OMDEV_CONTAINER_WORKSPACE)" $$env_args -e OM_MAP_PATH=/next/map.json "$(OMDEV_IMAGE)" -lc "trap : TERM INT; sleep infinity & wait" >/dev/null; fi'
+
+omdev-dev-recreate:
+	ssh $(OMDEV_SSH_OPTS) $(OMDEV_SSH) '$(OMDEV_PODMAN) rm -f -t 0 "$(OMDEV_CONTAINER)" >/dev/null 2>&1 || true'
+	$(MAKE) omdev-dev-create
+
+omdev-dev-start: omdev-dev-create
+	ssh $(OMDEV_SSH_OPTS) $(OMDEV_SSH) 'set -e; if [ "$$($(OMDEV_PODMAN) inspect -f "{{.State.Running}}" "$(OMDEV_CONTAINER)")" != "true" ]; then $(OMDEV_PODMAN) start "$(OMDEV_CONTAINER)" >/dev/null; fi'
+
+omdev-dev-stop:
+	ssh $(OMDEV_SSH_OPTS) $(OMDEV_SSH) '$(OMDEV_PODMAN) stop "$(OMDEV_CONTAINER)" >/dev/null 2>&1 || true'
+
+omdev-deps: omdev-sync omdev-dev-start
+	ssh $(OMDEV_SSH_OPTS) $(OMDEV_SSH) 'set -e; $(OMDEV_PODMAN) exec -u 0 "$(OMDEV_CONTAINER)" bash -lc "set -e; apt-get update; DEBIAN_FRONTEND=noninteractive apt-get install -y $(OMDEV_TOOL_DEPS); rosdep init >/dev/null 2>&1 || true; rosdep update; source /opt/ros/$(ROS_DISTRO)/setup.bash; cd $(OMDEV_CONTAINER_WORKSPACE); DEBIAN_FRONTEND=noninteractive rosdep install --from-paths $(OMDEV_ROSDEP_PATHS) --ignore-src -i -y -r --skip-keys=\"$(OMDEV_ROSDEP_SKIP_KEYS)\""'
+
+omdev-clean: omdev-dev-start
+	ssh $(OMDEV_SSH_OPTS) $(OMDEV_SSH) '$(OMDEV_PODMAN) exec -u 0 "$(OMDEV_CONTAINER)" bash -lc "cd $(OMDEV_CONTAINER_WORKSPACE) && rm -rf $(OMDEV_CLEAN_PATHS)"'
+
+omdev-run: omdev-run-workspace
+
+omdev-run-workspace: omdev-sync omdev-dev-start
+	ssh $(OMDEV_SSH_OPTS) $(OMDEV_SSH) 'env_args=""; if [ -f "$(OMDEV_ENV_FILE)" ]; then env_args="--env-file $(OMDEV_ENV_FILE)"; fi; $(OMDEV_PODMAN) exec $$env_args "$(OMDEV_CONTAINER)" bash -lc "cd $(OMDEV_CONTAINER_WORKSPACE) && bash utils/omdev-run-launch.sh $(ROS_DISTRO) $(OMDEV_LAUNCH_LOG) $(OMDEV_LAUNCH_PID) $(OMDEV_LAUNCH_ARGS)"'
+
+omdev-restart: omdev-run-workspace
+
+omdev-stop:
+	ssh $(OMDEV_SSH_OPTS) $(OMDEV_SSH) '$(OMDEV_PODMAN) exec "$(OMDEV_CONTAINER)" bash -lc "cd $(OMDEV_CONTAINER_WORKSPACE) && bash utils/omdev-stop-launch.sh $(OMDEV_LAUNCH_PID)"'
+
+omdev-logs: omdev-dev-start
+	ssh -t $(OMDEV_SSH_OPTS) $(OMDEV_SSH) '$(OMDEV_PODMAN) exec -it "$(OMDEV_CONTAINER)" bash -lc "cd $(OMDEV_CONTAINER_WORKSPACE) && touch $(OMDEV_LAUNCH_LOG) && tail -n 200 -f $(OMDEV_LAUNCH_LOG)"'
+
+omdev-status:
+	ssh $(OMDEV_SSH_OPTS) $(OMDEV_SSH) 'hostname; id; test -f "$(OMDEV_ENV_FILE)" && echo "env: $(OMDEV_ENV_FILE)" || echo "env missing: $(OMDEV_ENV_FILE)"; test -f "$(OMDEV_MAP_DIR)/map.json" && echo "map: $(OMDEV_MAP_DIR)/map.json" || echo "map missing: $(OMDEV_MAP_DIR)/map.json"; $(OMDEV_PODMAN) ps -a --filter name="^$(OMDEV_CONTAINER)$$"; $(OMDEV_PODMAN) images "$(OMDEV_IMAGE)"'
+
+omdev-shell:
+	ssh -t $(OMDEV_SSH_OPTS) $(OMDEV_SSH) '$(OMDEV_PODMAN) exec -it "$(OMDEV_CONTAINER)" bash'
+
+omdev-calibrate: omdev-sync omdev-dev-start
+	ssh -t $(OMDEV_SSH_OPTS) $(OMDEV_SSH) '$(OMDEV_PODMAN) exec -it "$(OMDEV_CONTAINER)" bash -lc "source /opt/ros/$(ROS_DISTRO)/setup.bash; cd $(OMDEV_CONTAINER_WORKSPACE); if [ -f install/setup.bash ]; then source install/setup.bash; fi; python3 scripts/calibrate_robot.py $(CALIBRATE_ARGS)"'
+
+omdev-build: omdev-sync omdev-dev-start
+	ssh $(OMDEV_SSH_OPTS) $(OMDEV_SSH) 'set -e; uid=$$(id -u); gid=$$(id -g); $(OMDEV_PODMAN) exec --user "$$uid:$$gid" -e HOME=/tmp "$(OMDEV_CONTAINER)" bash -lc "source /opt/ros/$(ROS_DISTRO)/setup.bash && cd $(OMDEV_CONTAINER_WORKSPACE) && colcon build --symlink-install --base-paths $(OMDEV_FIELDS2COVER_BASE_PATHS) $(OMDEV_FIELDS2COVER_PACKAGES) $(OMDEV_FIELDS2COVER_CMAKE_ARGS) && source install/setup.bash && colcon build --symlink-install --base-paths $(OMDEV_LIB_BASE_PATHS) $(OMDEV_LIB_PACKAGES) $(OMDEV_LIB_CMAKE_ARGS) && source install/setup.bash && colcon build --symlink-install $(OMDEV_APP_PACKAGES) $(OMDEV_APP_CMAKE_ARGS)"'
diff --git a/cmake/docking_helper.cmake b/cmake/docking_helper.cmake
index 5450949..21152d6 100644
--- a/cmake/docking_helper.cmake
+++ b/cmake/docking_helper.cmake
@@ -17,9 +17,10 @@ ament_target_dependencies(docking_helper
         tf2
         tf2_ros
         tf2_geometry_msgs
-        nav2_msgs
-        geometry_msgs
-        std_msgs
+  nav2_msgs
+  geometry_msgs
+  omros2_firmware_msgs
+  std_msgs
 )
 
 INSTALL(TARGETS docking_helper
@@ -36,6 +37,7 @@ target_include_directories(charger_presence_charging_dock PUBLIC
 ament_target_dependencies(charger_presence_charging_dock
   pluginlib
   opennav_docking_core
+  omros2_firmware_msgs
   std_msgs
   rclcpp_lifecycle
   tf2_ros
@@ -50,4 +52,4 @@ INSTALL(FILES src/docking_helper/charger_presence_charging_dock.hpp
 set_target_properties(charger_presence_charging_dock PROPERTIES LINKER_LANGUAGE CXX)
 pluginlib_export_plugin_description_file(opennav_docking_core src/docking_helper/plugins.xml)
 
-add_dependencies(docking_helper ${PROJECT_NAME})
\ No newline at end of file
+add_dependencies(docking_helper ${PROJECT_NAME})
diff --git a/cmake/map_recorder.cmake b/cmake/map_recorder.cmake
index a4f1af6..878d9d0 100644
--- a/cmake/map_recorder.cmake
+++ b/cmake/map_recorder.cmake
@@ -19,6 +19,7 @@ ament_target_dependencies(map_recorder
   tf2_geometry_msgs
   geometry_msgs
   nav2_msgs
+  omros2_firmware_msgs
   std_msgs
   std_srvs
 )
@@ -26,4 +27,4 @@ ament_target_dependencies(map_recorder
 INSTALL(TARGETS map_recorder
         DESTINATION lib/${PROJECT_NAME})
 
-add_dependencies(map_recorder ${PROJECT_NAME})
\ No newline at end of file
+add_dependencies(map_recorder ${PROJECT_NAME})
diff --git a/cmake/map_server.cmake b/cmake/map_server.cmake
index ae0f023..ee29a12 100644
--- a/cmake/map_server.cmake
+++ b/cmake/map_server.cmake
@@ -20,12 +20,12 @@ target_link_libraries(map_server_node "${cpp_typesupport_target}")
 
 ament_target_dependencies(map_server_node
         std_msgs
+        geographic_msgs
         geometry_msgs
         nav_msgs
         foxglove_msgs
         visualization_msgs
         rclcpp
-        robot_localization
         tf2
         tf2_geometry_msgs
         unique_identifier_msgs
@@ -39,4 +39,4 @@ target_link_libraries(map_server_node
 INSTALL(TARGETS map_server_node
         DESTINATION lib/${PROJECT_NAME})
 
-add_dependencies(map_server_node ${PROJECT_NAME})
\ No newline at end of file
+add_dependencies(map_server_node ${PROJECT_NAME})
diff --git a/cmake/sim.cmake b/cmake/sim.cmake
index 7d2a71d..90e908f 100644
--- a/cmake/sim.cmake
+++ b/cmake/sim.cmake
@@ -12,12 +12,43 @@ target_include_directories(sim_node PUBLIC
 target_link_libraries(sim_node "${cpp_typesupport_target}")
 
 ament_target_dependencies(sim_node
+        geometry_msgs
+        nav_msgs
+        omros2_firmware_msgs
         rclcpp
+        sensor_msgs
+        std_msgs
         tf2
         tf2_geometry_msgs
+        tf2_ros
 )
 
 INSTALL(TARGETS sim_node
         DESTINATION lib/${PROJECT_NAME})
 
-add_dependencies(sim_node ${PROJECT_NAME})
\ No newline at end of file
+add_dependencies(sim_node ${PROJECT_NAME})
+
+###
+# navigation_readiness_node
+###
+add_executable(navigation_readiness_node
+        src/sim/navigation_readiness_node.cpp)
+target_compile_features(navigation_readiness_node PUBLIC c_std_99 cxx_std_17)
+target_include_directories(navigation_readiness_node PUBLIC
+        $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/src>
+        $<INSTALL_INTERFACE:include>
+)
+target_link_libraries(navigation_readiness_node "${cpp_typesupport_target}")
+
+ament_target_dependencies(navigation_readiness_node
+        nav_msgs
+        rclcpp
+        sensor_msgs
+        tf2
+        tf2_ros
+)
+
+INSTALL(TARGETS navigation_readiness_node
+        DESTINATION lib/${PROJECT_NAME})
+
+add_dependencies(navigation_readiness_node ${PROJECT_NAME})
diff --git a/config/controllers.yaml b/config/controllers.yaml
index 818351e..f7b3e04 100644
--- a/config/controllers.yaml
+++ b/config/controllers.yaml
@@ -19,7 +19,6 @@ diff_drive_base_controller:
 
     left_wheel_names: ['left_wheel_joint']
     right_wheel_names: ['right_wheel_joint']
-    wheel_separation: 0.32
     wheel_radius: 0.0925
     wheels_per_side: 1
 
diff --git a/config/fusioncore.yaml b/config/fusioncore.yaml
new file mode 100644
index 0000000..48a3cad
--- /dev/null
+++ b/config/fusioncore.yaml
@@ -0,0 +1,86 @@
+fusioncore:
+  ros__parameters:
+    base_frame: base_link
+    odom_frame: odom
+    publish_rate: 50.0
+    publish.force_2d: true
+    publish.tf: true
+    motion_model: DifferentialDrive
+
+    imu.topic: /imu/data
+    imu.frame_id: imu
+    imu.has_magnetometer: false
+    imu.gyro_noise: 0.005
+    imu.accel_noise: 0.1
+    imu.remove_gravitational_acceleration: false
+
+    encoder.vel_noise: 0.05
+    encoder.yaw_noise: 0.02
+
+    gnss.base_noise_xy: 0.5
+    gnss.base_noise_z: 1.0
+    gnss.max_hdop: 4.0
+    gnss.min_satellites: 4
+    # ublox_f9p maps RTK fixed to GBAS and RTK float/differential to SBAS,
+    # but allow standalone GNSS so localization can start before corrections converge.
+    gnss.min_fix_type: 1
+    gnss.use_gps_fix: false
+    gnss.velocity_topic: /gps/odom
+    gnss.lever_arm_x: 0.32
+    gnss.lever_arm_y: 0.0
+    gnss.lever_arm_z: 0.1
+    gnss.track_heading_min_speed: 0.2
+    gnss.track_heading_max_yaw_rate: 0.3
+    gnss.track_heading_max_yaw_delta: 0.15
+    gnss.track_heading_min_dist: 5.0
+    gnss.heading_observable_distance: 5.0
+    gnss.rotation_heading_enabled: true
+    gnss.rotation_heading_min_yaw_delta: 1.0
+    gnss.rotation_heading_min_arc_baseline: 0.25
+    gnss.rotation_heading_max_base_translation: 0.20
+    gnss.rotation_heading_max_sigma: 0.4
+    gnss.rotation_heading_sigma_floor: 0.05
+    gnss.rotation_heading_delta_yaw_sigma: 0.03
+    gnss.rotation_heading_max_window_s: 10.0
+    gnss.coast_n: 5
+    gnss.coast_q_factor: 10.0
+    gnss.coast_timeout_s: 30.0
+    gnss.coast_q_bias_factor: 100.0
+    gnss.coast_imu_wz_scale: 500.0
+
+    outlier_rejection: true
+    outlier_threshold_gnss: 16.27
+    outlier_threshold_imu: 15.09
+    outlier_threshold_enc: 11.34
+    outlier_threshold_hdg: 10.83
+
+    init.stationary_window: 2.0
+    init.wait_for_all_sensors: true
+    init.sensor_wait_timeout: 10.0
+
+    zupt.enabled: true
+    zupt.velocity_threshold: 0.05
+    zupt.angular_threshold: 0.05
+    zupt.noise_sigma: 0.01
+
+    adaptive.imu: true
+    adaptive.encoder: true
+    adaptive.gnss: true
+    adaptive.window: 50
+    adaptive.alpha: 0.01
+
+    ukf.q_position: 0.01
+    ukf.q_orientation: 1.0e-9
+    ukf.q_velocity: 0.1
+    ukf.q_angular_vel: 0.1
+    ukf.q_acceleration: 1.0
+    ukf.q_gyro_bias: 1.0e-5
+    ukf.q_accel_bias: 1.0e-5
+
+    input.gnss_crs: EPSG:4326
+    output.crs: EPSG:4978
+    output.convert_to_enu_at_reference: true
+    reference.use_first_fix: false
+    reference.x: 0.0
+    reference.y: 0.0
+    reference.z: 0.0
diff --git a/config/gps.yaml b/config/gps.yaml
index 90ebfd0..586eea5 100644
--- a/config/gps.yaml
+++ b/config/gps.yaml
@@ -1,12 +1,13 @@
 ublox_f9p:
   ros__parameters:
-    frame_id: gps
+    frame_id: gnss_link
     child_frame_id: base_link
     port: /dev/ttyAMA1
     baudrate: 921600
     debug: false
     config:
       enabled: true
-      measurement_frequency: 5
+      measurement_frequency: 10
+      uart_output_rate: 10
     publish:
       motion_odometry: true
diff --git a/config/hardware/yardforce500.yaml b/config/hardware/yardforce500.yaml
index 50331a1..01dd7ef 100644
--- a/config/hardware/yardforce500.yaml
+++ b/config/hardware/yardforce500.yaml
@@ -24,7 +24,7 @@ mower:
 wheel:
   # x, y, z offset in m of wheel from center of robot
   # for the left and right wheels, the y offset is positive and negative respectively
-  offset: [0, 0.16, 0]
+  offset: [0, 0.16535, 0]
   # mass in kg
   mass: 0.2
 
@@ -39,7 +39,7 @@ wheel:
     min: -1.0
   # gear ratio between motor and wheel
 #  gear_ratio: 0.003378378378378 # 1:296 (motor:wheel) - calculated based on value 1600 ticks per m
-  gear_ratio: 0.027027027027027
+  gear_ratio: 0.026345892
 caster_wheel:
   # x, y offset in m of caster wheel from center of base link
   # z is calculated based on the radius of the wheel and it's offset
diff --git a/config/joystick.yaml b/config/joystick.yaml
new file mode 100644
index 0000000..101d98a
--- /dev/null
+++ b/config/joystick.yaml
@@ -0,0 +1,24 @@
+joy_node:
+  ros__parameters:
+    deadzone: 0.05
+    autorepeat_rate: 20.0
+
+teleop_twist_joy_node:
+  ros__parameters:
+    publish_stamped_twist: true
+    frame: base_link
+    require_enable_button: false
+
+    axis_linear:
+      x: 1
+    scale_linear:
+      x: 0.26
+    scale_linear_turbo:
+      x: 0.26
+
+    axis_angular:
+      yaw: 0
+    scale_angular:
+      yaw: 1.0
+    scale_angular_turbo:
+      yaw: 1.0
diff --git a/config/nav2_params.yaml b/config/nav2_params.yaml
index 836c803..0e6df1e 100644
--- a/config/nav2_params.yaml
+++ b/config/nav2_params.yaml
@@ -4,7 +4,7 @@ bt_navigator:
     enable_stamped_cmd_vel: true
     global_frame: map
     robot_base_frame: base_link
-    odom_topic: /odometry/filtered/map
+    odom_topic: /fusion/odom
     bt_loop_duration: 10
     default_server_timeout: 20
     navigators: ["navigate_to_pose", "navigate_through_poses"]
@@ -29,8 +29,8 @@ controller_server:
     # Progress checker parameters
     progress_checker:
       plugin: "nav2_controller::SimpleProgressChecker"
-      required_movement_radius: 0.5
-      movement_time_allowance: 10.0
+      required_movement_radius: 0.1
+      movement_time_allowance: 15.0
     # Goal checker parameters
     general_goal_checker:
       stateful: True
@@ -57,7 +57,7 @@ controller_server:
       use_cost_regulated_linear_velocity_scaling: false
       regulated_linear_scaling_min_radius: 0.9
       regulated_linear_scaling_min_speed: 0.25
-      use_rotate_to_heading: false  # allows to follow coverage path better in sharper turns
+      use_rotate_to_heading: true
       allow_reversing: false
       rotate_to_heading_min_angle: 0.785
       max_angular_accel: 3.2
@@ -118,7 +118,7 @@ velocity_smoother:
     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: "/odometry/filtered/map"
+    odom_topic: "/fusion/odom"
     odom_duration: 0.1
     deadband_velocity: [0.0, 0.0, 0.0]
     velocity_timeout: 1.0
diff --git a/config/robot_localization.yaml b/config/robot_localization.yaml
deleted file mode 100644
index e199d85..0000000
--- a/config/robot_localization.yaml
+++ /dev/null
@@ -1,177 +0,0 @@
-ekf_se_odom:
-  ros__parameters:
-    frequency: 50.0
-    sensor_timeout: 0.025
-    two_d_mode: true
-    transform_time_offset: 0.0
-    transform_timeout: 0.0
-    print_diagnostics: true
-    debug: false
-
-    map_frame: map
-    odom_frame: odom
-    base_link_frame: base_link
-    world_frame: odom
-
-    #x     , y     , z,
-    #roll  , pitch , yaw,
-    #vx    , vy    , vz,
-    #vroll , vpitch, vyaw,
-    #ax    , ay    , az
-
-    odom0: diff_drive_base_controller/odom
-    odom0_config: [false, false, false,
-                   false, false, false,
-                   true,  true,  true,
-                   false, false, true,
-                   false, false, false]
-    odom0_queue_size: 50
-    odom0_nodelay: true
-    odom0_differential: false
-    odom0_relative: false
-
-    imu0: imu/data
-    imu0_config: [false, false, false,
-                  true,  true,  false,
-                  false, false, false,
-                  true,  true,  true,
-                  true,  true,  true]
-    imu0_nodelay: false
-    imu0_differential: false
-    imu0_relative: false
-    imu0_queue_size: 100
-    imu0_remove_gravitational_acceleration: true
-
-    use_control: false
-
-    process_noise_covariance: [1.0e-3, 0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
-                               0.0,    1e-3, 0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
-                               0.0,    0.0,    1e-3, 0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
-                               0.0,    0.0,    0.0,    0.3,  0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
-                               0.0,    0.0,    0.0,    0.0,    0.3,  0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
-                               0.0,    0.0,    0.0,    0.0,    0.0,    0.01, 0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
-                               0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.5,   0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
-                               0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.5,   0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
-                               0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.1,  0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
-                               0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.3,  0.0,    0.0,    0.0,    0.0,    0.0,
-                               0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.3,  0.0,    0.0,    0.0,    0.0,
-                               0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.3,  0.0,    0.0,    0.0,
-                               0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.3,  0.0,    0.0,
-                               0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.3,  0.0,
-                               0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.3]
-
-    initial_estimate_covariance: [1e-9, 0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,     0.0,    0.0,    0.0,
-                                  0.0,    1e-9, 0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,     0.0,    0.0,    0.0,
-                                  0.0,    0.0,    1e-9, 0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,     0.0,    0.0,    0.0,
-                                  0.0,    0.0,    0.0,    1.0,  0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,     0.0,    0.0,    0.0,
-                                  0.0,    0.0,    0.0,    0.0,    1.0,  0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,     0.0,    0.0,    0.0,
-                                  0.0,    0.0,    0.0,    0.0,    0.0,    1e-9, 0.0,    0.0,    0.0,    0.0,     0.0,     0.0,     0.0,    0.0,    0.0,
-                                  0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    1.0,  0.0,    0.0,    0.0,     0.0,     0.0,     0.0,    0.0,    0.0,
-                                  0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    1.0,  0.0,    0.0,     0.0,     0.0,     0.0,    0.0,    0.0,
-                                  0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    1.0,  0.0,     0.0,     0.0,     0.0,    0.0,    0.0,
-                                  0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    1.0,   0.0,     0.0,     0.0,    0.0,    0.0,
-                                  0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     1.0,   0.0,     0.0,    0.0,    0.0,
-                                  0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     1.0,   0.0,    0.0,    0.0,
-                                  0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,     1.0,  0.0,    0.0,
-                                  0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,     0.0,    1.0,  0.0,
-                                  0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,     0.0,    0.0,    1.0]
-
-ekf_se_map:
-  ros__parameters:
-    frequency: 50.0
-    sensor_timeout: 0.1
-    two_d_mode: true
-    transform_time_offset: 0.0
-    transform_timeout: 0.0
-    print_diagnostics: true
-    debug: false
-
-    map_frame: map
-    odom_frame: odom
-    base_link_frame: base_link
-    world_frame: map
-
-
-    #x     , y     , z,§
-    #roll  , pitch , yaw,
-    #vx    , vy    , vz,
-    #vroll , vpitch, vyaw,
-    #ax    , ay    , az
-
-    odom0: diff_drive_base_controller/odom
-    odom0_config: [false, false, false,
-                   false, false, false,
-                   true,  true,  true,
-                   false, false, true,
-                   false, false, false]
-    odom0_queue_size: 10
-    odom0_nodelay: true
-    odom0_differential: false
-    odom0_relative: false
-
-    odom1: odometry/gps
-    odom1_config: [true,  true,  false,
-                   false, false, false,
-                   false, false, false,
-                   false, false, false,
-                   false, false, false]
-    odom1_queue_size: 10
-    odom1_nodelay: true
-    odom1_differential: false
-    odom1_relative: false
-
-    imu0: imu/data
-    imu0_config: [false, false, false,
-                  true,  true,  false,
-                  false, false, false,
-                  true,  true,  true,
-                  true,  true,  true]
-    imu0_nodelay: true
-    imu0_differential: false
-    imu0_relative: false
-    imu0_queue_size: 10
-    imu0_remove_gravitational_acceleration: true
-
-    use_control: false
-
-    process_noise_covariance: [1.0,  0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
-                               0.0,    1.0,  0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
-                               0.0,    0.0,    1e-3, 0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
-                               0.0,    0.0,    0.0,    0.3,  0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
-                               0.0,    0.0,    0.0,    0.0,    0.3,  0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
-                               0.0,    0.0,    0.0,    0.0,    0.0,    0.3, 0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
-                               0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.5,   0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
-                               0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.5,   0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
-                               0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.1,  0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
-                               0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.3,  0.0,    0.0,    0.0,    0.0,    0.0,
-                               0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.3,  0.0,    0.0,    0.0,    0.0,
-                               0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.3,  0.0,    0.0,    0.0,
-                               0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.3,  0.0,    0.0,
-                               0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.3,  0.0,
-                               0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.3]
-
-    initial_estimate_covariance: [1.0,  0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,     0.0,    0.0,    0.0,
-                                  0.0,    1.0,  0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,     0.0,    0.0,    0.0,
-                                  0.0,    0.0,    1e-9, 0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,     0.0,    0.0,    0.0,
-                                  0.0,    0.0,    0.0,    1.0,  0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,     0.0,    0.0,    0.0,
-                                  0.0,    0.0,    0.0,    0.0,    1.0,  0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,     0.0,    0.0,    0.0,
-                                  0.0,    0.0,    0.0,    0.0,    0.0,    1e-9, 0.0,    0.0,    0.0,    0.0,     0.0,     0.0,     0.0,    0.0,    0.0,
-                                  0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    1.0,  0.0,    0.0,    0.0,     0.0,     0.0,     0.0,    0.0,    0.0,
-                                  0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    1.0,  0.0,    0.0,     0.0,     0.0,     0.0,    0.0,    0.0,
-                                  0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    1.0,  0.0,     0.0,     0.0,     0.0,    0.0,    0.0,
-                                  0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    1.0,   0.0,     0.0,     0.0,    0.0,    0.0,
-                                  0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     1.0,   0.0,     0.0,    0.0,    0.0,
-                                  0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     1.0,   0.0,    0.0,    0.0,
-                                  0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,     1.0,  0.0,    0.0,
-                                  0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,     0.0,    1.0,  0.0,
-                                  0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,     0.0,    0.0,    1.0]
-
-navsat_transform_node:
-  ros__parameters:
-    frequency: 30.0
-    delay: 3.0
-    zero_altitude: true
-    publish_filtered_gps: true
-    wait_for_datum: true
-    use_odometry_yaw: true
-    use_local_cartesian: true
diff --git a/config/twist_mux.yaml b/config/twist_mux.yaml
index d97092f..21028de 100644
--- a/config/twist_mux.yaml
+++ b/config/twist_mux.yaml
@@ -6,7 +6,11 @@ twist_mux:
         topic   : cmd_vel_nav
         timeout : 0.5
         priority: 10
+      calibration:
+        topic   : cmd_vel_calibration
+        timeout : 0.5
+        priority: 50
       joystick:
         topic   : cmd_vel_joy
         timeout : 0.5
-        priority: 100
\ No newline at end of file
+        priority: 100
diff --git a/custom_deps.yaml b/custom_deps.yaml
index c4544f5..89c03b7 100644
--- a/custom_deps.yaml
+++ b/custom_deps.yaml
@@ -10,7 +10,7 @@ repositories:
   ublox_f9p:
     type: git
     url: https://github.com/jkaflik/ublox_f9p
-    version: main
+    version: fix/parser-rtk-semantics
   ntrip_client:
     type: git
     url: https://github.com/LORD-MicroStrain/ntrip_client
@@ -19,3 +19,15 @@ repositories:
     type: git
     url: https://github.com/Fields2Cover/Fields2Cover.git
     version: 0ce7fcce82f3f79f773699448769bcd679c739bc
+  fusioncore:
+    type: git
+    url: https://github.com/jkaflik/fusioncore.git
+    version: openmower/gnss-rotation-heading-bootstrap
+  webots_ros2:
+    type: git
+    url: https://github.com/jkaflik/webots_ros2.git
+    version: openmower/fix-gps-navsat-status
+  omros2_firmware:
+    type: git
+    url: https://github.com/jkaflik/omros2-firmware.git
+    version: ba454a21bde8562baa7fe328613e7e3644b9c284
diff --git a/description/gps.xacro b/description/gps.xacro
index c454b69..9fc3ca2 100644
--- a/description/gps.xacro
+++ b/description/gps.xacro
@@ -2,11 +2,11 @@
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="gps">
     <joint name="gps_joint" type="fixed">
         <parent link="base_link"/>
-        <child link="gps"/>
+        <child link="gnss_link"/>
         <origin xyz="${robot['gps']['antenna']['offset'][0]} ${robot['gps']['antenna']['offset'][1]} ${robot['gps']['antenna']['offset'][2]}" rpy="0 0 0"/>
     </joint>
 
-    <link name="gps">
+    <link name="gnss_link">
     </link>
 
 </robot>
diff --git a/docs/.vitepress/config.mts b/docs/.vitepress/config.mts
index 9ccf466..7fe7760 100644
--- a/docs/.vitepress/config.mts
+++ b/docs/.vitepress/config.mts
@@ -56,6 +56,7 @@ export default withMermaid({
                 { text: 'CLion alternative', link: '/clion-env' },
               ],
             },
+            { text: 'Hardware target', link: '/hardware-target' },
             { text: 'Visualisation', link: '/visualisation' },
             { text: 'Simulator', link: '/simulator' },
             { text: 'ROS MCP', link: '/ros-mcp' },
diff --git a/docs/architecture/localization.md b/docs/architecture/localization.md
index 800f10a..99abfa6 100644
--- a/docs/architecture/localization.md
+++ b/docs/architecture/localization.md
@@ -1,26 +1,27 @@
 ---
-title: Robot localization
+title: FusionCore localization
 ---
 # {{ $frontmatter.title }}
 
 ## Overview
 
 Robot pose is based on an absolute position from GPS and relative readings from wheel odometry and IMU.
-Orientation is not known on startup and defaults to 0. 
+Orientation is not known on startup and defaults to 0.
 
-As soon as robot starts moving, orientation is assumed based on robot motion and sensors reading.
-It might take a while to get an accurate orientation. Best to start moving in a straight line and do a few circles.
+FusionCore validates heading from GNSS track motion and, during startup, can also bootstrap heading from a controlled rotation around the GNSS lever arm. Until heading is validated, the GNSS lever-arm correction remains inactive.
 
-Currently, there is no fallback scenario if had its position changed externally. For example, if you move the robot manually, it will not be able to recover position itself. It will require same procedure as on startup. 
+If the robot is moved manually while powered, localization should be reset or restarted before autonomous motion. It will need the same heading validation procedure as on startup.
 
 Later on, when undocking behavior is implemented, it will be possible to recover orientation knowing base station position.
 
 ## Sensors
 
-[robot_localization documentation](http://docs.ros.org/en/melodic/api/robot_localization/html/integrating_gps.html) nicely describes
-how to get wheel odometry, GPS and IMU sensors data fusion to get an accurate localization.
+OpenMowerNext uses [FusionCore](https://github.com/manankharwar/fusioncore) for wheel odometry,
+GPS and IMU sensor fusion. FusionCore publishes `/fusion/odom` and the `odom -> base_link` TF.
+The map datum from `OM_DATUM_LAT` and `OM_DATUM_LONG` is used as the fixed FusionCore reference
+origin so the fused odometry aligns with GeoJSON maps.
 
-![Senor data flow](http://docs.ros.org/en/melodic/api/robot_localization/html/_images/navsat_transform_workflow.png)
+On hardware, the u-blox F9P driver publishes `gps_msgs/GPSFix` on `/gps/fix_extended` and the hardware launch remaps that into FusionCore. This preserves RTK_FLOAT versus RTK_FIX status, satellite count, and receiver accuracy fields that are not available in `sensor_msgs/NavSatFix`. Simulation still uses `/gps/fix` as `NavSatFix`.
 
 ### Wheel odometry
 
@@ -30,11 +31,11 @@ Default motor controller VESC reports wheel odometry.
 
 Accelerometer and gyroscope is required. Magnetometer is not fused.
 
-### GPU
+### GPS
 
 It's expected GPS is RTK capable. Otherwise, localization will be inaccurate.
 More on this in [GPS](../gps.md) section.
 
 ## Configuration
 
-<<< ../../config/robot_localization.yaml{yaml}
+<<< ../../config/fusioncore.yaml{yaml}
diff --git a/docs/architecture/omros2-firmware.md b/docs/architecture/omros2-firmware.md
index 4d137c7..ef4049b 100644
--- a/docs/architecture/omros2-firmware.md
+++ b/docs/architecture/omros2-firmware.md
@@ -14,8 +14,7 @@ This firmware supports only the recent OpenMower mainboard v0.13.x.
 * :white_check_mark: DDS communication
 * :white_check_mark: `sensor_msgs/Imu` message published on `/imu/data_raw` topic
 * :white_check_mark: `sensor_msgs/BatteryState` message published on `/power` topic
-  * :white_check_mark: `std_msgs/Float32` message published on `/power/charge_voltage` topic
-  * :white_check_mark: `std_msgs/Bool` message published on `/power/charger_present` topic
+* :white_check_mark: `omros2_firmware_msgs/PowerStatus` message published on `/power/status` topic
 * :white_check_mark: OpenMower charging logic
 * :white_check_mark: ping micro-ROS agent, make sure it is alive
 * :red_circle: safety features
diff --git a/docs/architecture/ros-workspace.md b/docs/architecture/ros-workspace.md
index 0fcebc1..0cf203e 100644
--- a/docs/architecture/ros-workspace.md
+++ b/docs/architecture/ros-workspace.md
@@ -16,7 +16,7 @@ There are few biggest chunks to mention:
 
 - [ros2_control](https://control.ros.org/master/index.html) - differential drive controller, but also a hardware layer provider
 - [nav2](https://navigation.ros.org/) - navigation stack
-- [robot_localization](http://docs.ros.org/en/noetic/api/robot_localization/html/index.html) - fuse sensors data to get accurate pose estimation
+- [FusionCore](https://github.com/manankharwar/fusioncore) - fuses IMU, wheel odometry and GPS into `/fusion/odom`
 - [NTRIP client](https://github.com/LORD-MicroStrain/ntrip_client) - a ROS node to connect to NTRIP caster and get RTK corrections
 - [Foxglove bridge](https://foxglove.dev/docs/studio/connection/using-foxglove-bridge) - a ROS node exposing websocket connection to Foxglove Studio. See instructions [here](../visualisation). It can be used for custom Web UIs as well.
 
diff --git a/docs/architecture/sim-node.md b/docs/architecture/sim-node.md
index e835d6d..dc9a52d 100644
--- a/docs/architecture/sim-node.md
+++ b/docs/architecture/sim-node.md
@@ -21,9 +21,9 @@ The node is designed to work with the Webots simulator, using a configured docki
 
 The Sim Node detects when the robot's `charging_port` frame is in close proximity to the docking station's charging contacts. When the robot is properly docked:
 
-- The `/power/charger_present` topic will be `true`
+- The `/power/status` `charger_present` field will be `true`
 - The battery will start charging
-- A charging voltage will be published on the `/power/charge_voltage` topic
+- A charging voltage will be published in the `/power/status` `charge_voltage` field
 
 The docking station contact pose is configured in the simulation launch file to match the Webots docking station model.
 
@@ -39,9 +39,8 @@ The battery simulation provides a simplified model of a real battery system:
 
 | Topic | Type | Description |
 |-------|------|-------------|
-| `/power/charger_present` | `std_msgs/Bool` | Indicates if the robot is properly docked |
 | `/power` | `sensor_msgs/BatteryState` | Battery state information including voltage, percentage, and status |
-| `/power/charge_voltage` | `std_msgs/Float32` | Voltage provided by the charger when docked |
+| `/power/status` | `omros2_firmware_msgs/PowerStatus` | Firmware-compatible power and charging snapshot, including dock contact and charge voltage |
 
 ## Parameters
 
@@ -65,5 +64,5 @@ Docking is considered successful when the charging port is within 5cm of the doc
 The battery simulation logic is simplified:
 - If charger is present: Voltage increases at a constant rate until maximum
 - If charger is not present: Voltage decreases at a constant rate
-- Battery percentage is calculated based on min/max voltage range
+- Battery percentage is calculated based on min/max voltage range. `PowerStatus.battery_percentage` is `0..100`; `BatteryState.percentage` follows the ROS convention of `0.0..1.0`.
 - Battery health status (GOOD, DEAD, OVERVOLTAGE) is determined by voltage levels
diff --git a/docs/configuration.md b/docs/configuration.md
index 5157a00..050128a 100644
--- a/docs/configuration.md
+++ b/docs/configuration.md
@@ -17,4 +17,4 @@ The goal is to:
 
 - `OM_MAP_PATH` - path to the map file (see: [map server](architecture/map-server.md))
 - `OM_DATUM_LAT` - latitude of the datum point
-- `OM_DATUM_LON` - longitude of the datum point
+- `OM_DATUM_LONG` - longitude of the datum point
diff --git a/docs/getting-started.md b/docs/getting-started.md
index de45563..c3819e7 100644
--- a/docs/getting-started.md
+++ b/docs/getting-started.md
@@ -48,6 +48,7 @@ Project is tested on YardForce Classic 500B model. It should work on other model
 
 - [Setup as for OpenMower](https://openmower.de/docs/robot-assembly/prepare-the-parts/)
   - [OpenMower v0.13.x mainboard](https://openmower.de/docs/robot-assembly/prepare-the-parts/prepare-mainboard/) with [omros2-firmware](https://github.com/jkaflik/omros2-firmware) flashed. Learn more about the custom firmware in [omros2-firmware](architecture/omros2-firmware).
+- For physical robot testing, see the [hardware target](hardware-target.md) guide. The examples use `omdev.local` as the conventional robot hostname.
 
 ### Software
 
diff --git a/docs/hardware-target.md b/docs/hardware-target.md
new file mode 100644
index 0000000..0de74c5
--- /dev/null
+++ b/docs/hardware-target.md
@@ -0,0 +1,186 @@
+# Hardware Target
+
+`omdev.local` is the conventional example hostname for a physical OpenMowerNext test target on the local network. It is not a required name: replace it with your robot hostname or pass `REMOTE_HOST=...` to the Make targets.
+
+Use this flow when you want to validate OpenMowerNext against real hardware instead of Webots.
+
+## Target Host
+
+A hardware target is usually a small Linux computer mounted on the mower, for example a Raspberry Pi. The host should provide:
+
+- SSH access from the development machine.
+- Access to the robot serial devices under `/dev`.
+- Podman or Docker if using the container runtime.
+- A persistent map directory outside the repository.
+
+The repository defaults to `omdev.local` for remote-device helper commands:
+
+```bash
+make remote-devices
+```
+
+Override it for another robot:
+
+```bash
+make REMOTE_HOST=my-mower.local REMOTE_USER=my-user remote-devices
+```
+
+The `omdev-*` targets use `omdev.local` through `REMOTE_HOST` by default and the current local username as `OMDEV_USER`:
+
+```bash
+make omdev-status
+make omdev-sync
+make omdev-build
+make omdev-run
+make omdev-logs
+make omdev-stop
+```
+
+Override the target host or user when needed:
+
+```bash
+make OMDEV_HOST=my-mower.local OMDEV_USER=pi omdev-status
+```
+
+## Configuration Files
+
+Keep site-specific state outside the repository. A typical target layout is:
+
+```text
+~/next/map.json
+~/omnext/openmower.env
+```
+
+The environment file should provide at least:
+
+```bash
+OM_DATUM_LAT=<latitude>
+OM_DATUM_LONG=<longitude>
+OM_MAP_PATH=/next/map.json
+```
+
+Use raw `KEY=value` entries in this file. `podman --env-file` passes shell quotes
+literally, so `OM_NTRIP_PASSWORD="secret"` includes the quote characters in the
+password and can cause NTRIP caster `401 Unauthorized` responses.
+
+Do not commit private coordinates, credentials, NTRIP settings, or real maps to the repository.
+
+## Container Runtime
+
+`omdev.local` is a mutable hardware-development target. Do not run the production `ghcr.io/jkaflik/openmowernext:*` image there during iteration. The default Makefile flow uses a long-lived `docker.io/library/ros:jazzy` container named `next-dev`, mounts the synced workspace, installs dependencies from that workspace, and launches from the local build output.
+
+Sync the workspace and create or start the dev container:
+
+```bash
+make omdev-sync
+make omdev-dev-start
+```
+
+Recreate it after changing the map mount layout, base image, or container options:
+
+```bash
+make omdev-dev-recreate
+```
+
+`omdev-run-workspace` reloads `~/omnext/openmower.env` for the launch process, so changing datum or NTRIP values does not require recreating the dev container.
+
+Install or refresh dependencies inside the dev container after syncing:
+
+```bash
+make omdev-deps
+```
+
+`omdev-deps` installs ROS tooling, runs `rosdep update`, and installs dependencies for the root package plus the hardware-relevant vendored packages. It intentionally does not run `make custom-deps` on the target because `vcs import --force` can overwrite the synced `src/lib` checkouts.
+
+If the ROS base image or generated CMake metadata changes, clean target-side build artifacts before rebuilding:
+
+```bash
+make omdev-clean
+make omdev-build
+```
+
+After `make omdev-sync` and `make omdev-build`, run the synced workspace:
+
+```bash
+make omdev-run-workspace
+make omdev-logs
+```
+
+Pass extra launch arguments during bring-up when needed:
+
+```bash
+make OMDEV_LAUNCH_ARGS='enable_navigation_readiness:=false' omdev-run-workspace
+```
+
+If a non-IMU sensor is intentionally offline during diagnostics, bypass FusionCore's all-sensor startup gate:
+
+```bash
+make OMDEV_LAUNCH_ARGS='enable_navigation_readiness:=false init_wait_for_all_sensors:=false init_stationary_window:=0.0' omdev-run-workspace
+```
+
+FusionCore still initializes from the first IMU sample, so this does not bypass a missing `/imu/data_raw` publisher.
+
+The image can be changed without editing the Makefile:
+
+```bash
+make OMDEV_IMAGE=docker.io/library/ros:jazzy omdev-dev-recreate
+```
+
+## Fast Iteration
+
+For rapid development, avoid rebuilding and pushing a runtime image for hardware validation. Prefer this loop:
+
+- Edit and validate on the development machine.
+- Sync the working tree to the hardware target with `make omdev-sync`; it excludes `build/`, `install/`, `log/`, docs dependencies, and VCS metadata.
+- Run `make omdev-deps` after dependency changes or after recreating the dev container.
+- Run `make omdev-clean` before rebuilding if the base image changed or stale generated CMake paths appear.
+- Run `make omdev-build` to execute an incremental build of the current branch's hardware-critical packages inside the long-lived dev container.
+- Run `make omdev-run-workspace` to launch the synced and built workspace.
+- Watch logs before enabling any service or autonomous behavior.
+
+By default `omdev-build` builds `fields2cover`, `vesc_*`, `micro_ros_agent`, `ntrip_client`, `compass_msgs`, `fusioncore_core`, `fusioncore_ros`, and `ublox_f9p` from `src/lib`, then builds `open_mower_next` from the workspace root. Override `OMDEV_FIELDS2COVER_BASE_PATHS`, `OMDEV_LIB_BASE_PATHS`, `OMDEV_LIB_PACKAGES`, or `OMDEV_APP_PACKAGES` if your change needs a wider build.
+
+Use a tagged GHCR image only for explicit release or production-image validation. `omdev.local` bring-up does not require a system service; run the workspace from the ROS Jazzy dev container while iterating.
+
+## Calibration
+
+OpenMowerNext includes an interactive hardware calibration tool that can measure wheel geometry, drive response, maximum observed speed, and stable navigation speed recommendations from real odometry, GPS, and IMU data.
+
+Launch the robot first, then run the calibration tool from the same built workspace or from inside the target container:
+
+```bash
+ros2 run open_mower_next calibrate_robot --mode preflight
+```
+
+Run the full automatic flow only with the robot in a clear outdoor test area and with an operator ready to stop it:
+
+```bash
+ros2 run open_mower_next calibrate_robot --mode full
+```
+
+The script publishes stamped commands on `/cmd_vel_calibration`. `twist_mux` gives this input higher priority than Nav2 and lower priority than joystick, so joystick teleop remains the preferred manual override.
+
+If you want to drive manually and only record data, use:
+
+```bash
+ros2 run open_mower_next calibrate_robot --mode full --drive-mode manual
+```
+
+The generated report is written under `log/calibration/` as JSON and YAML. It includes recommended changes for `config/controllers.yaml`, `config/hardware/yardforce500.yaml`, `config/nav2_params.yaml`, and related limits when enough data was collected.
+
+Apply recommendations only after reviewing the report:
+
+```bash
+ros2 run open_mower_next calibrate_robot --mode full --apply
+```
+
+The `--apply` mode creates timestamped `.bak.<timestamp>` backups before writing YAML files.
+
+## Safety
+
+Before launching on real hardware:
+
+- Keep the mower blade physically disabled unless the test explicitly requires it.
+- Put the robot on stands for wheel and controller tests.
+- Start with observation-only launches where possible.
+- Confirm GPS, micro-ROS, VESC, TF, and localization topics before sending motion commands.
diff --git a/docs/simulator.md b/docs/simulator.md
index 76a8a12..0accf34 100644
--- a/docs/simulator.md
+++ b/docs/simulator.md
@@ -49,7 +49,7 @@ The simulator publishes or serves the same ROS-facing contract used by the rest
 - `/gps/fix` from the Webots GPS device
 - `/imu/data_raw` from the Webots IMU plugin
 - `/diff_drive_base_controller/odom` from `diff_drive_controller`
-- `/power/charger_present`, `/power/charge_voltage`, and `/power` from `sim_node`
+- `/power/status` and `/power` from `sim_node`
 
 Velocity commands still flow through `twist_mux` to `/diff_drive_base_controller/cmd_vel`.
 
diff --git a/docs/visualisation.md b/docs/visualisation.md
index 0ab28da..5c7d2ee 100644
--- a/docs/visualisation.md
+++ b/docs/visualisation.md
@@ -30,3 +30,17 @@ make foxglove-service-enable
 ```
 
 Useful service commands are `make foxglove-service-status`, `make foxglove-service-restart`, `make foxglove-service-disable`, and `make foxglove-service-logs`.
+
+## Joystick Control
+
+The Foxglove dashboard uses Josh Newans' `joy-panel.Joystick` panel as the external joystick source. Configure the panel to publish `sensor_msgs/msg/Joy` on `/joy`.
+
+OpenMowerNext runs `teleop_twist_joy` in both the hardware and Webots simulation launches. It subscribes to `/joy` and publishes joystick velocity commands on `/cmd_vel_joy` as `geometry_msgs/msg/TwistStamped` with `base_link` as the frame.
+
+The command flow is:
+
+```text
+Foxglove Joystick -> /joy -> teleop_twist_joy -> /cmd_vel_joy
+```
+
+The stack intentionally stays on stamped velocity commands: `twist_mux` uses stamped input, `diff_drive_base_controller` accepts stamped velocity, and Nav2 has stamped command velocity enabled.
diff --git a/foxglove/default.json b/foxglove/default.json
index 636e126..e936499 100644
--- a/foxglove/default.json
+++ b/foxglove/default.json
@@ -1,41 +1,20 @@
 {
   "configById": {
-    "Parameters!2tk0ho3": {
-      "title": "Parameters"
-    },
-    "SourceInfo!f2w7h8": {},
-    "RosOut!1anfqcv": {
-      "searchTerms": [],
-      "minLogLevel": 1
-    },
-    "3D!2gz3kf4": {
+    "3D!robot-overview": {
       "cameraState": {
         "perspective": true,
-        "distance": 14.70183781243095,
-        "phi": 51.15722910343571,
-        "thetaOffset": 32.968204010013444,
-        "targetOffset": [
-          2.935055849217841,
-          -7.049400716494065,
-          -4.8151705682163236e-15
-        ],
-        "target": [
-          0,
-          0,
-          0
-        ],
-        "targetOrientation": [
-          0,
-          0,
-          0,
-          1
-        ],
+        "distance": 9,
+        "phi": 52,
+        "thetaOffset": 35,
+        "targetOffset": [0, 0, 0],
+        "target": [0, 0, 0],
+        "targetOrientation": [0, 0, 0, 1],
         "fovy": 45,
-        "near": 0.5,
+        "near": 0.05,
         "far": 5000
       },
       "followMode": "follow-pose",
-      "followTf": "map",
+      "followTf": "base_link",
       "scene": {
         "transforms": {
           "labelSize": 0.05,
@@ -45,176 +24,101 @@
         "enableStats": false
       },
       "transforms": {
-        "frame:camera_link": {
-          "visible": false
-        },
-        "frame:camera_accel_frame": {
-          "visible": false
-        },
-        "frame:camera_accel_optical_frame": {
-          "visible": false
-        },
-        "frame:camera_color_frame": {
-          "visible": false
-        },
-        "frame:camera_depth_frame": {
-          "visible": false
-        },
-        "frame:camera_color_optical_frame": {
-          "visible": false
-        },
-        "frame:camera_gyro_frame": {
-          "visible": false
-        },
-        "frame:camera_depth_optical_frame": {
-          "visible": false
-        },
-        "frame:camera_imu_optical_frame": {
-          "visible": false
-        },
-        "frame:camera_gyro_optical_frame": {
-          "visible": false
-        },
-        "frame:camera_infra1_optical_frame": {
-          "visible": false
-        },
-        "frame:camera_infra1_frame": {
-          "visible": false
-        },
-        "frame:camera_infra2_frame": {
-          "visible": false
-        },
-        "frame:camera_infra2_optical_frame": {
-          "visible": false
-        },
-        "frame:camera_bottom_screw_frame": {
-          "visible": false
-        },
-        "frame:chassis": {
-          "visible": true
-        },
-        "frame:map": {
-          "visible": true
-        }
+        "frame:map": { "visible": true },
+        "frame:odom": { "visible": true },
+        "frame:base_link": { "visible": true },
+        "frame:charging_port": { "visible": true },
+        "frame:gnss_link": { "visible": true },
+        "frame:imu": { "visible": false }
       },
       "topics": {
-        "/robot_description": {
-          "visible": true
-        },
-        "/camera/depth/color/points": {
+        "/robot_description": { "visible": true },
+        "/map_grid": { "visible": true },
+        "/map_visualization": { "visible": true },
+        "/docking_stations_poses": { "visible": true },
+        "/map_recorder/record_boundaries_polygon": { "visible": true },
+        "/dock_pose": { "visible": true },
+        "/staging_pose": { "visible": true },
+        "/docking_trajectory": { "visible": true }
+      },
+      "layers": {
+        "grid": {
           "visible": true,
-          "colorMode": "rgb",
-          "stixelsEnabled": false,
-          "colorField": "rgb",
-          "colorMap": "turbo"
-        },
-        "/camera/depth/camera_info": {
-          "visible": false
-        },
-        "/camera/color/camera_info": {
-          "visible": false
-        },
-        "/camera/color/image_raw": {
-          "visible": false,
-          "frameLocked": true,
-          "cameraInfoTopic": "/camera/color/camera_info",
-          "distance": 1,
-          "planarProjectionFactor": 0,
-          "color": "#ffffff"
-        },
-        "/camera/depth/image_rect_raw": {
-          "visible": false,
-          "frameLocked": true,
-          "cameraInfoTopic": "/camera/depth/camera_info",
-          "distance": 1,
-          "planarProjectionFactor": 0,
-          "color": "#ffffff"
-        },
-        "/move_base_simple/goal": {
-          "visible": false,
-          "axisScale": 2.7
-        },
-        "/initialpose": {
-          "visible": false
-        },
-        "/map": {
-          "visible": false
-        },
-        "/scan": {
-          "visible": false
-        },
-        "/amcl_pose": {
-          "visible": false
-        },
-        "/camera/infra1/image_rect_raw": {
-          "visible": false,
           "frameLocked": true,
-          "cameraInfoTopic": "/camera/infra1/camera_info",
-          "distance": 1,
-          "planarProjectionFactor": 0,
-          "color": "#ffffff"
-        },
-        "/camera/infra1/camera_info": {
-          "visible": false
-        },
-        "/local_costmap/costmap": {
-          "visible": false
-        },
-        "/local_costmap/voxel_layer": {
-          "visible": false
-        },
-        "/local_plan": {
-          "visible": false
-        },
-        "/transformed_global_plan": {
-          "visible": false
-        },
-        "/marker": {
-          "visible": false
-        },
-        "/cost_cloud": {
-          "visible": false
-        },
-        "/local_costmap/static_layer": {
-          "visible": false
-        },
-        "/global_costmap/costmap": {
-          "visible": false
-        },
-        "/global_costmap/obstacle_layer": {
-          "visible": false
-        },
-        "/global_costmap/static_layer": {
-          "visible": false
+          "label": "Grid",
+          "instanceId": "grid",
+          "layerId": "foxglove.Grid",
+          "size": 20,
+          "divisions": 20,
+          "lineWidth": 1,
+          "color": "#2f81f7",
+          "position": [0, 0, 0],
+          "rotation": [0, 0, 0],
+          "order": 1
         }
       },
+      "publish": {
+        "type": "pose",
+        "poseTopic": "/goal_pose",
+        "pointTopic": "/clicked_point",
+        "poseEstimateTopic": "/initialpose",
+        "poseEstimateXDeviation": 0.5,
+        "poseEstimateYDeviation": 0.5,
+        "poseEstimateThetaDeviation": 0.26179939
+      },
+      "imageMode": {}
+    },
+    "3D!simulation-ros": {
+      "cameraState": {
+        "perspective": true,
+        "distance": 7,
+        "phi": 48,
+        "thetaOffset": 25,
+        "targetOffset": [0, 0, 0],
+        "target": [0, 0, 0],
+        "targetOrientation": [0, 0, 0, 1],
+        "fovy": 45,
+        "near": 0.05,
+        "far": 5000
+      },
+      "followMode": "follow-pose",
+      "followTf": "base_link",
+      "scene": {
+        "transforms": {
+          "labelSize": 0.05,
+          "showLabel": false,
+          "editable": false
+        },
+        "enableStats": false
+      },
+      "topics": {
+        "/robot_description": { "visible": true },
+        "/map_grid": { "visible": true },
+        "/map_visualization": { "visible": true },
+        "/docking_stations_poses": { "visible": true },
+        "/dock_pose": { "visible": true },
+        "/staging_pose": { "visible": true },
+        "/docking_trajectory": { "visible": true }
+      },
       "layers": {
-        "00763ac2-076e-494b-bf0f-fa441cb46f1c": {
+        "grid": {
           "visible": true,
           "frameLocked": true,
           "label": "Grid",
-          "instanceId": "00763ac2-076e-494b-bf0f-fa441cb46f1c",
+          "instanceId": "grid",
           "layerId": "foxglove.Grid",
-          "size": 10,
-          "divisions": 10,
+          "size": 12,
+          "divisions": 12,
           "lineWidth": 1,
-          "color": "#248eff",
-          "position": [
-            0,
-            0,
-            0
-          ],
-          "rotation": [
-            0,
-            0,
-            0
-          ],
+          "color": "#2f81f7",
+          "position": [0, 0, 0],
+          "rotation": [0, 0, 0],
           "order": 1
         }
       },
       "publish": {
         "type": "pose",
-        "poseTopic": "/move_base_simple/goal",
+        "poseTopic": "/goal_pose",
         "pointTopic": "/clicked_point",
         "poseEstimateTopic": "/initialpose",
         "poseEstimateXDeviation": 0.5,
@@ -223,65 +127,370 @@
       },
       "imageMode": {}
     },
-    "map!3i1j9xz": {
+    "3D!debug-geometry": {
+      "cameraState": {
+        "perspective": true,
+        "distance": 14,
+        "phi": 55,
+        "thetaOffset": 40,
+        "targetOffset": [0, 0, 0],
+        "target": [0, 0, 0],
+        "targetOrientation": [0, 0, 0, 1],
+        "fovy": 45,
+        "near": 0.05,
+        "far": 5000
+      },
+      "followMode": "follow-pose",
+      "followTf": "map",
+      "scene": {
+        "transforms": {
+          "labelSize": 0.05,
+          "showLabel": true,
+          "editable": false
+        },
+        "enableStats": false
+      },
+      "topics": {
+        "/robot_description": { "visible": true },
+        "/map_grid": { "visible": true },
+        "/map_visualization": { "visible": true },
+        "/docking_stations_poses": { "visible": true },
+        "/map_recorder/record_boundaries_polygon": { "visible": true },
+        "/dock_pose": { "visible": true },
+        "/staging_pose": { "visible": true },
+        "/docking_trajectory": { "visible": true }
+      },
+      "layers": {
+        "grid": {
+          "visible": true,
+          "frameLocked": true,
+          "label": "Grid",
+          "instanceId": "grid",
+          "layerId": "foxglove.Grid",
+          "size": 30,
+          "divisions": 30,
+          "lineWidth": 1,
+          "color": "#64748b",
+          "position": [0, 0, 0],
+          "rotation": [0, 0, 0],
+          "order": 1
+        }
+      },
+      "publish": {
+        "type": "point",
+        "poseTopic": "/goal_pose",
+        "pointTopic": "/clicked_point",
+        "poseEstimateTopic": "/initialpose"
+      },
+      "imageMode": {}
+    },
+    "map!robot-gps": {
       "center": {
-        "lat": 52.4954655142106,
-        "lon": 21.385255157947544
+        "lat": -22.9,
+        "lon": -43.2
       },
       "customTileUrl": "",
-      "disabledTopics": [
-        "/gps/filtered"
-      ],
+      "disabledTopics": [],
       "followTopic": "/gps/fix",
       "layer": "map",
       "topicColors": {
-        "/gps/fix": "#4e98e2",
-        "/gps/filtered": "#4e98e2"
+        "/gps/fix": "#2f81f7",
+        "/map/foxglove_geojson": "#22c55e"
       },
-      "zoomLevel": 19,
+      "zoomLevel": 20,
       "maxNativeZoom": 18
     },
-    "virtual-joystick.Virtual Joystick!1bz242v": {
-      "topic": "/cmd_vel",
-      "publishRate": 30,
-      "maxLinearSpeed": 3,
-      "maxAngularSpeed": 3
+    "map!simulation-gps": {
+      "center": {
+        "lat": -22.9,
+        "lon": -43.2
+      },
+      "customTileUrl": "",
+      "disabledTopics": [],
+      "followTopic": "/gps/fix",
+      "layer": "map",
+      "topicColors": {
+        "/gps/fix": "#2f81f7",
+        "/map/foxglove_geojson": "#22c55e"
+      },
+      "zoomLevel": 20,
+      "maxNativeZoom": 18
+    },
+    "foxglove-webots-extension.Webots!simulation-webots": {
+      "serverUrlMode": "auto",
+      "manualServerUrl": "ws://localhost:1234",
+      "webotsPort": 1234,
+      "mode": "w3d",
+      "broadcast": false,
+      "autoConnect": true
+    },
+    "joy-panel.Joystick!robot-joy": {
+      "dataSource": "interactive",
+      "subJoyTopic": "/joy",
+      "gamepadId": 0,
+      "publishMode": true,
+      "pubJoyTopic": "/joy",
+      "publishFrameId": "base_link",
+      "displayMode": "custom",
+      "debugGamepad": false,
+      "layoutName": "xbox",
+      "mapping_name": "TODO"
     },
-    "Gauge!1nkddo2": {
-      "path": "/power.percentage",
+    "joy-panel.Joystick!simulation-joy": {
+      "dataSource": "interactive",
+      "subJoyTopic": "/joy",
+      "gamepadId": 0,
+      "publishMode": true,
+      "pubJoyTopic": "/joy",
+      "publishFrameId": "base_link",
+      "displayMode": "custom",
+      "debugGamepad": false,
+      "layoutName": "xbox",
+      "mapping_name": "TODO"
+    },
+    "Gauge!robot-battery": {
+      "path": "/power/status.battery_percentage",
       "minValue": 0,
       "maxValue": 100,
       "colorMap": "red-yellow-green",
       "colorMode": "colormap",
-      "gradient": [
-        "#0000ff",
-        "#ff00ff"
-      ],
       "reverse": false,
-      "foxglovePanelTitle": "battery percentage"
+      "foxglovePanelTitle": "Battery"
     },
-    "Indicator!1otv6hv": {
-      "path": "/power/charger_present.data",
+    "Gauge!robot-voltage": {
+      "path": "/power.voltage",
+      "minValue": 21.7,
+      "maxValue": 28.7,
+      "colorMap": "red-yellow-green",
+      "colorMode": "colormap",
+      "reverse": false,
+      "foxglovePanelTitle": "Battery V"
+    },
+    "Gauge!simulation-battery": {
+      "path": "/power/status.battery_percentage",
+      "minValue": 0,
+      "maxValue": 100,
+      "colorMap": "red-yellow-green",
+      "colorMode": "colormap",
+      "reverse": false,
+      "foxglovePanelTitle": "Sim battery %"
+    },
+    "Indicator!robot-charger": {
+      "path": "/power/status.charger_present",
+      "style": "background",
+      "fallbackColor": "#7f1d1d",
+      "fallbackLabel": "Not charging",
+      "rules": [
+        {
+          "operator": "=",
+          "rawValue": "true",
+          "color": "#166534",
+          "label": "Charging"
+        }
+      ],
+      "foxglovePanelTitle": "Charger"
+    },
+    "Indicator!simulation-charger": {
+      "path": "/power/status.charger_present",
       "style": "background",
-      "fallbackColor": "#fffb17",
-      "fallbackLabel": "False",
+      "fallbackColor": "#7f1d1d",
+      "fallbackLabel": "Away from dock",
       "rules": [
         {
           "operator": "=",
           "rawValue": "true",
-          "color": "#25bb00",
-          "label": "True"
+          "color": "#166534",
+          "label": "Dock contact"
+        }
+      ],
+      "foxglovePanelTitle": "Dock contact"
+    },
+    "Plot!robot-motion": {
+      "paths": [
+        {
+          "value": "/fusion/odom.twist.twist.linear.x",
+          "enabled": true,
+          "timestampMethod": "receiveTime",
+          "label": "linear x m/s"
+        },
+        {
+          "value": "/fusion/odom.twist.twist.angular.z",
+          "enabled": true,
+          "timestampMethod": "receiveTime",
+          "label": "angular z rad/s"
+        },
+        {
+          "value": "/diff_drive_base_controller/odom.twist.twist.linear.x",
+          "enabled": true,
+          "timestampMethod": "receiveTime",
+          "label": "wheel odom x m/s"
+        }
+      ],
+      "showXAxisLabels": true,
+      "showYAxisLabels": true,
+      "showLegend": true,
+      "legendDisplay": "floating",
+      "showPlotValuesInLegend": false,
+      "isSynced": true,
+      "xAxisVal": "timestamp",
+      "sidebarDimension": 240,
+      "foxglovePanelTitle": "Motion"
+    },
+    "Plot!robot-power": {
+      "paths": [
+        {
+          "value": "/power.voltage",
+          "enabled": true,
+          "timestampMethod": "receiveTime",
+          "label": "battery V"
+        },
+        {
+          "value": "/power.current",
+          "enabled": true,
+          "timestampMethod": "receiveTime",
+          "label": "current A"
+        },
+        {
+          "value": "/power/status.battery_percentage",
+          "enabled": true,
+          "timestampMethod": "receiveTime",
+          "label": "battery"
+        }
+      ],
+      "showXAxisLabels": true,
+      "showYAxisLabels": true,
+      "showLegend": true,
+      "legendDisplay": "floating",
+      "showPlotValuesInLegend": false,
+      "isSynced": true,
+      "xAxisVal": "timestamp",
+      "sidebarDimension": 240,
+      "foxglovePanelTitle": "Power"
+    },
+    "Plot!simulation-commands": {
+      "paths": [
+        {
+          "value": "/cmd_vel_joy.twist.linear.x",
+          "enabled": true,
+          "timestampMethod": "receiveTime",
+          "label": "joy linear x"
+        },
+        {
+          "value": "/cmd_vel_nav.twist.linear.x",
+          "enabled": true,
+          "timestampMethod": "receiveTime",
+          "label": "nav linear x"
+        },
+        {
+          "value": "/cmd_vel_raw.twist.linear.x",
+          "enabled": true,
+          "timestampMethod": "receiveTime",
+          "label": "raw linear x"
+        },
+        {
+          "value": "/diff_drive_base_controller/cmd_vel.twist.linear.x",
+          "enabled": true,
+          "timestampMethod": "receiveTime",
+          "label": "controller linear x"
         }
       ],
-      "foxglovePanelTitle": "in charger"
+      "showXAxisLabels": true,
+      "showYAxisLabels": true,
+      "showLegend": true,
+      "legendDisplay": "floating",
+      "showPlotValuesInLegend": false,
+      "isSynced": true,
+      "xAxisVal": "timestamp",
+      "sidebarDimension": 240,
+      "foxglovePanelTitle": "Command chain"
     },
-    "Plot!1rdecvu": {
+    "Plot!simulation-power": {
       "paths": [
         {
-          "value": "/power/charge_voltage.data",
+          "value": "/power.voltage",
+          "enabled": true,
+          "timestampMethod": "receiveTime",
+          "label": "battery V"
+        },
+        {
+          "value": "/power/status.charge_voltage",
           "enabled": true,
           "timestampMethod": "receiveTime",
           "label": "charge V"
+        },
+        {
+          "value": "/power/status.battery_percentage",
+          "enabled": true,
+          "timestampMethod": "receiveTime",
+          "label": "battery %"
+        }
+      ],
+      "showXAxisLabels": true,
+      "showYAxisLabels": true,
+      "showLegend": true,
+      "legendDisplay": "floating",
+      "showPlotValuesInLegend": false,
+      "isSynced": true,
+      "xAxisVal": "timestamp",
+      "sidebarDimension": 240,
+      "foxglovePanelTitle": "Sim power"
+    },
+    "Plot!debug-fusion": {
+      "paths": [
+        {
+          "value": "/fusion/odom.pose.pose.position.x",
+          "enabled": true,
+          "timestampMethod": "receiveTime",
+          "label": "fusion x"
+        },
+        {
+          "value": "/fusion/odom.pose.pose.position.y",
+          "enabled": true,
+          "timestampMethod": "receiveTime",
+          "label": "fusion y"
+        },
+        {
+          "value": "/gps/odom.twist.twist.linear.x",
+          "enabled": true,
+          "timestampMethod": "receiveTime",
+          "label": "gps speed x"
+        }
+      ],
+      "showXAxisLabels": true,
+      "showYAxisLabels": true,
+      "showLegend": true,
+      "legendDisplay": "floating",
+      "showPlotValuesInLegend": false,
+      "isSynced": true,
+      "xAxisVal": "timestamp",
+      "sidebarDimension": 240,
+      "foxglovePanelTitle": "FusionCore"
+    },
+    "Plot!debug-gps": {
+      "paths": [
+        {
+          "value": "/gps/fix.position_covariance[0]",
+          "enabled": true,
+          "timestampMethod": "receiveTime",
+          "label": "gps cov xx"
+        },
+        {
+          "value": "/gps/fix.position_covariance[4]",
+          "enabled": true,
+          "timestampMethod": "receiveTime",
+          "label": "gps cov yy"
+        },
+        {
+          "value": "/gps/fix_extended.speed",
+          "enabled": true,
+          "timestampMethod": "receiveTime",
+          "label": "gps speed m/s"
+        },
+        {
+          "value": "/gps/fix_extended.err_horz",
+          "enabled": true,
+          "timestampMethod": "receiveTime",
+          "label": "gps horiz err m"
         }
       ],
       "showXAxisLabels": true,
@@ -291,101 +500,407 @@
       "showPlotValuesInLegend": false,
       "isSynced": true,
       "xAxisVal": "timestamp",
-      "sidebarDimension": 240
+      "sidebarDimension": 240,
+      "foxglovePanelTitle": "GNSS quality"
     },
-    "DiagnosticSummary!dqya88": {
+    "DiagnosticSummary!robot-diagnostics": {
       "minLevel": 0,
       "pinnedIds": [],
       "hardwareIdFilter": "",
       "topicToRender": "/diagnostics",
       "sortByLevel": true
     },
-    "DiagnosticStatusPanel!42w5qw9": {
+    "DiagnosticSummary!debug-diagnostics-summary": {
+      "minLevel": 0,
+      "pinnedIds": [],
+      "hardwareIdFilter": "",
       "topicToRender": "/diagnostics",
-      "selectedHardwareId": "none",
-      "selectedName": "ekf_se_odom: Filter diagnostic updater",
+      "sortByLevel": true
+    },
+    "DiagnosticStatusPanel!debug-diagnostics-status": {
+      "topicToRender": "/diagnostics",
+      "selectedHardwareId": "",
+      "selectedName": "",
       "collapsedSections": []
     },
-    "RawMessages!28hnlsl": {
+    "RosOut!simulation-logs": {
+      "searchTerms": [],
+      "minLogLevel": 1
+    },
+    "RosOut!debug-logs": {
+      "searchTerms": [],
+      "minLogLevel": 0
+    },
+    "SourceInfo!debug-source-info": {},
+    "RawMessages!debug-raw-gps": {
       "diffEnabled": false,
       "diffMethod": "custom",
       "diffTopicPath": "",
       "showFullMessageForDiff": false,
       "topicPath": "/gps/fix"
     },
-    "Tab!3h75d6h": {
-      "activeTabIdx": 1,
+    "RawMessages!debug-raw-fusion": {
+      "diffEnabled": false,
+      "diffMethod": "custom",
+      "diffTopicPath": "",
+      "showFullMessageForDiff": false,
+      "topicPath": "/fusion/odom"
+    },
+    "RawMessages!debug-raw-power": {
+      "diffEnabled": false,
+      "diffMethod": "custom",
+      "diffTopicPath": "",
+      "showFullMessageForDiff": false,
+      "topicPath": "/power"
+    },
+    "RawMessages!debug-raw-map": {
+      "diffEnabled": false,
+      "diffMethod": "custom",
+      "diffTopicPath": "",
+      "showFullMessageForDiff": false,
+      "topicPath": "/mowing_map"
+    },
+    "RawMessages!debug-raw-commands": {
+      "diffEnabled": false,
+      "diffMethod": "custom",
+      "diffTopicPath": "",
+      "showFullMessageForDiff": false,
+      "topicPath": "/cmd_vel_nav"
+    },
+    "Tab!debug-raw-tabs": {
+      "activeTabIdx": 0,
       "tabs": [
         {
-          "title": "settings",
+          "title": "GPS",
+          "layout": "RawMessages!debug-raw-gps"
+        },
+        {
+          "title": "Fusion",
+          "layout": "RawMessages!debug-raw-fusion"
+        },
+        {
+          "title": "Power",
+          "layout": "RawMessages!debug-raw-power"
+        },
+        {
+          "title": "Map",
+          "layout": "RawMessages!debug-raw-map"
+        },
+        {
+          "title": "Commands",
+          "layout": "RawMessages!debug-raw-commands"
+        }
+      ]
+    },
+    "Markdown!manual-actions": {
+      "markdown": "# Manual controls\n\nUse this tab for explicit operator actions only. The Debug tab is observation-only.\n\nCommon action names in this stack:\n\n- `/record_area_boundary`\n- `/record_docking_station`\n- `/dock_robot_nearest`\n- `/dock_robot_to`\n- `/dock_robot`\n\nFoxglove provides built-in Publish and Service Call panels. If your Foxglove build does not include a ROS action client panel, call actions from the ROS CLI or add a dedicated action-client extension."
+    },
+    "Publish!manual-stop": {
+      "topicName": "/cmd_vel_joy",
+      "topic": "/cmd_vel_joy",
+      "schemaName": "geometry_msgs/msg/TwistStamped",
+      "message": "{\n  \"header\": {\n    \"frame_id\": \"base_link\"\n  },\n  \"twist\": {\n    \"linear\": { \"x\": 0, \"y\": 0, \"z\": 0 },\n    \"angular\": { \"x\": 0, \"y\": 0, \"z\": 0 }\n  }\n}",
+      "buttonTitle": "Publish stop",
+      "buttonText": "Publish stop",
+      "buttonTooltip": "Publish zero TwistStamped to /cmd_vel_joy",
+      "buttonColor": "#dc2626",
+      "editingMode": false,
+      "foxglovePanelTitle": "Stop command"
+    },
+    "Publish!manual-command-template": {
+      "topicName": "/cmd_vel_joy",
+      "topic": "/cmd_vel_joy",
+      "schemaName": "geometry_msgs/msg/TwistStamped",
+      "message": "{\n  \"header\": {\n    \"frame_id\": \"base_link\"\n  },\n  \"twist\": {\n    \"linear\": { \"x\": 0, \"y\": 0, \"z\": 0 },\n    \"angular\": { \"x\": 0, \"y\": 0, \"z\": 0 }\n  }\n}",
+      "buttonTitle": "Publish command",
+      "buttonText": "Publish command",
+      "buttonTooltip": "Edit and publish a TwistStamped command to /cmd_vel_joy",
+      "buttonColor": "#2563eb",
+      "editingMode": true,
+      "foxglovePanelTitle": "Command template"
+    },
+    "Service Call!manual-find-dock": {
+      "serviceName": "/find_nearest_docking_station",
+      "request": "{}",
+      "requestText": "{}",
+      "layout": "horizontal",
+      "editingMode": true,
+      "timeout": 5,
+      "buttonTitle": "Find dock",
+      "buttonText": "Find dock",
+      "buttonTooltip": "Call /find_nearest_docking_station",
+      "buttonColor": "#2563eb",
+      "foxglovePanelTitle": "Find nearest dock"
+    },
+    "Service Call!manual-area-coverage": {
+      "serviceName": "/area_coverage",
+      "request": "{\n  \"area_id\": \"\",\n  \"with_exclusions\": true,\n  \"headland_loops\": 1,\n  \"swath_angle\": 0\n}",
+      "requestText": "{\n  \"area_id\": \"\",\n  \"with_exclusions\": true,\n  \"headland_loops\": 1,\n  \"swath_angle\": 0\n}",
+      "layout": "vertical",
+      "editingMode": true,
+      "timeout": 10,
+      "buttonTitle": "Generate coverage",
+      "buttonText": "Generate coverage",
+      "buttonTooltip": "Requires coverage_server to be running",
+      "buttonColor": "#16a34a",
+      "foxglovePanelTitle": "Coverage service"
+    },
+    "Service Call!manual-set-recording": {
+      "serviceName": "/set_recording_mode",
+      "request": "{\n  \"data\": true\n}",
+      "requestText": "{\n  \"data\": true\n}",
+      "layout": "vertical",
+      "editingMode": true,
+      "timeout": 5,
+      "buttonTitle": "Set recording mode",
+      "buttonText": "Set recording mode",
+      "buttonTooltip": "Available during active /record_area_boundary action",
+      "buttonColor": "#ca8a04",
+      "foxglovePanelTitle": "Recording mode"
+    },
+    "Service Call!manual-add-boundary-point": {
+      "serviceName": "/add_boundary_point",
+      "request": "{}",
+      "requestText": "{}",
+      "layout": "horizontal",
+      "editingMode": false,
+      "timeout": 5,
+      "buttonTitle": "Add boundary point",
+      "buttonText": "Add boundary point",
+      "buttonTooltip": "Available during active /record_area_boundary action",
+      "buttonColor": "#ca8a04",
+      "foxglovePanelTitle": "Add boundary point"
+    },
+    "Service Call!manual-finish-area-recording": {
+      "serviceName": "/finish_area_recording",
+      "request": "{}",
+      "requestText": "{}",
+      "layout": "horizontal",
+      "editingMode": false,
+      "timeout": 10,
+      "buttonTitle": "Finish area",
+      "buttonText": "Finish area",
+      "buttonTooltip": "Available during active /record_area_boundary action",
+      "buttonColor": "#16a34a",
+      "foxglovePanelTitle": "Finish area recording"
+    },
+    "Service Call!manual-remove-area": {
+      "serviceName": "/remove_area",
+      "request": "{\n  \"id\": \"\"\n}",
+      "requestText": "{\n  \"id\": \"\"\n}",
+      "layout": "vertical",
+      "editingMode": true,
+      "timeout": 10,
+      "buttonTitle": "Remove area",
+      "buttonText": "Remove area",
+      "buttonTooltip": "Call /remove_area with an area id",
+      "buttonColor": "#dc2626",
+      "foxglovePanelTitle": "Remove area"
+    },
+    "Service Call!manual-remove-dock": {
+      "serviceName": "/remove_docking_station",
+      "request": "{\n  \"id\": \"\"\n}",
+      "requestText": "{\n  \"id\": \"\"\n}",
+      "layout": "vertical",
+      "editingMode": true,
+      "timeout": 10,
+      "buttonTitle": "Remove dock",
+      "buttonText": "Remove dock",
+      "buttonTooltip": "Call /remove_docking_station with a docking station id",
+      "buttonColor": "#dc2626",
+      "foxglovePanelTitle": "Remove docking station"
+    },
+    "Parameters!manual-parameters": {
+      "title": "Parameters"
+    },
+    "Tab!openmower-dashboard": {
+      "activeTabIdx": 0,
+      "tabs": [
+        {
+          "title": "Robot",
           "layout": {
-            "first": "Parameters!2tk0ho3",
+            "first": {
+              "first": {
+                "first": "3D!robot-overview",
+                "second": "map!robot-gps",
+                "direction": "row",
+                "splitPercentage": 64
+              },
+              "second": {
+                "first": "Plot!robot-motion",
+                "second": "Plot!robot-power",
+                "direction": "row",
+                "splitPercentage": 50
+              },
+              "direction": "column",
+              "splitPercentage": 70
+            },
             "second": {
-              "first": "SourceInfo!f2w7h8",
-              "second": "RosOut!1anfqcv",
+              "first": "joy-panel.Joystick!robot-joy",
+              "second": {
+                "first": {
+                  "first": "Gauge!robot-battery",
+                  "second": "Gauge!robot-voltage",
+                  "direction": "row",
+                  "splitPercentage": 50
+                },
+                "second": {
+                  "first": "Indicator!robot-charger",
+                  "second": "DiagnosticSummary!robot-diagnostics",
+                  "direction": "column",
+                  "splitPercentage": 25
+                },
+                "direction": "column",
+                "splitPercentage": 28
+              },
               "direction": "column",
-              "splitPercentage": 49.780380673499266
+              "splitPercentage": 18
             },
             "direction": "row",
-            "splitPercentage": 59.50834879406308
+            "splitPercentage": 80
           }
         },
         {
-          "title": "3D",
+          "title": "Simulation",
+          "layout": {
+            "first": "foxglove-webots-extension.Webots!simulation-webots",
+            "second": {
+              "first": {
+                "first": "3D!simulation-ros",
+                "second": "map!simulation-gps",
+                "direction": "row",
+                "splitPercentage": 62
+              },
+              "second": {
+                "first": {
+                  "first": "joy-panel.Joystick!simulation-joy",
+                  "second": {
+                    "first": "Gauge!simulation-battery",
+                    "second": "Indicator!simulation-charger",
+                    "direction": "column",
+                    "splitPercentage": 55
+                  },
+                  "direction": "row",
+                  "splitPercentage": 45
+                },
+                "second": {
+                  "first": "Plot!simulation-commands",
+                  "second": "Plot!simulation-power",
+                  "direction": "row",
+                  "splitPercentage": 55
+                },
+                "direction": "column",
+                "splitPercentage": 42
+              },
+              "direction": "column",
+              "splitPercentage": 58
+            },
+            "direction": "row",
+            "splitPercentage": 56
+          }
+        },
+        {
+          "title": "Debug",
+          "layout": {
+            "first": {
+              "first": "3D!debug-geometry",
+              "second": {
+                "first": "Plot!debug-fusion",
+                "second": "Plot!debug-gps",
+                "direction": "row",
+                "splitPercentage": 50
+              },
+              "direction": "column",
+              "splitPercentage": 64
+            },
+            "second": {
+              "first": {
+                "first": "DiagnosticSummary!debug-diagnostics-summary",
+                "second": "DiagnosticStatusPanel!debug-diagnostics-status",
+                "direction": "row",
+                "splitPercentage": 38
+              },
+              "second": {
+                "first": "Tab!debug-raw-tabs",
+                "second": {
+                  "first": "RosOut!debug-logs",
+                  "second": "SourceInfo!debug-source-info",
+                  "direction": "row",
+                  "splitPercentage": 65
+                },
+                "direction": "column",
+                "splitPercentage": 58
+              },
+              "direction": "column",
+              "splitPercentage": 38
+            },
+            "direction": "row",
+            "splitPercentage": 58
+          }
+        },
+        {
+          "title": "Manual",
           "layout": {
             "first": {
-              "first": "3D!2gz3kf4",
-              "second": "map!3i1j9xz",
-              "direction": "row",
-              "splitPercentage": 83.8126159554731
+              "first": "Markdown!manual-actions",
+              "second": {
+                "first": "Publish!manual-stop",
+                "second": "Publish!manual-command-template",
+                "direction": "column",
+                "splitPercentage": 38
+              },
+              "direction": "column",
+              "splitPercentage": 44
             },
             "second": {
-              "first": "virtual-joystick.Virtual Joystick!1bz242v",
+              "first": {
+                "first": "Service Call!manual-find-dock",
+                "second": "Service Call!manual-area-coverage",
+                "direction": "row",
+                "splitPercentage": 42
+              },
               "second": {
                 "first": {
-                  "first": "Gauge!1nkddo2",
+                  "first": "Service Call!manual-set-recording",
                   "second": {
-                    "first": "Indicator!1otv6hv",
-                    "second": "Plot!1rdecvu",
-                    "direction": "row"
+                    "first": "Service Call!manual-add-boundary-point",
+                    "second": "Service Call!manual-finish-area-recording",
+                    "direction": "column",
+                    "splitPercentage": 50
                   },
-                  "direction": "column"
+                  "direction": "row",
+                  "splitPercentage": 55
                 },
                 "second": {
-                  "direction": "row",
-                  "second": "RawMessages!28hnlsl",
                   "first": {
+                    "first": "Service Call!manual-remove-area",
+                    "second": "Service Call!manual-remove-dock",
                     "direction": "row",
-                    "second": "DiagnosticStatusPanel!42w5qw9",
-                    "first": "DiagnosticSummary!dqya88",
-                    "splitPercentage": 57.19063545150501
+                    "splitPercentage": 50
                   },
-                  "splitPercentage": 69.26640926640925
+                  "second": "Parameters!manual-parameters",
+                  "direction": "column",
+                  "splitPercentage": 45
                 },
-                "direction": "row",
-                "splitPercentage": 25.36023054755044
+                "direction": "column",
+                "splitPercentage": 50
               },
-              "direction": "row",
-              "splitPercentage": 19.52690166975881
+              "direction": "column",
+              "splitPercentage": 34
             },
-            "direction": "column",
-            "splitPercentage": 62.29868228404099
+            "direction": "row",
+            "splitPercentage": 34
           }
         }
       ]
     }
   },
   "globalVariables": {
-    "globalVariable": 1
-  },
-  "userNodes": {
-    "0a710a06-464e-4f15-a8cf-ab93711d06b4": {
-      "sourceCode": "// The ./types module provides helper types for your Input events and messages.\nimport { Input, Message } from \"./types\";\n\n// Your script can output well-known message types, any of your custom message types, or\n// complete custom message types.\n//\n// Use `Message` to access types from the schemas defined in your data source:\n// type Twist = Message<\"geometry_msgs/Twist\">;\n//\n// Import from the @foxglove/schemas package to use foxglove schema types:\n// import { Pose, LocationFix } from \"@foxglove/schemas\";\n//\n// Conventionally, it's common to make a _type alias_ for your script's output type\n// and use that type name as the return type for your script function.\n// Here we've called the type `Output` but you can pick any type name.\ntype Output = {\n  hello: string;\n};\n\n// These are the topics your script \"subscribes\" to. Studio will invoke your script function\n// when any message is received on one of these topics.\nexport const inputs = [\"/input/topic\"];\n\n// Any output your script produces is \"published\" to this topic. Published messages are only visible within Studio, not to your original data source.\nexport const output = \"/studio_script/output_topic\";\n\n// This function is called with messages from your input topics.\n// The first argument is an event with the topic, receive time, and message.\n// Use the `Input<...>` helper to get the correct event type for your input topic messages.\nexport default function script(event: Input<\"/input/topic\">): Output {\n  return {\n    hello: \"world!\",\n  };\n};",
-      "name": "0a710a06"
-    }
+    "webotsPort": 1234
   },
+  "userNodes": {},
   "playbackConfig": {
     "speed": 1
   },
-  "layout": "Tab!3h75d6h"
-}
\ No newline at end of file
+  "layout": "Tab!openmower-dashboard"
+}
diff --git a/infra/buildkite/.terraform.lock.hcl b/infra/buildkite/.terraform.lock.hcl
new file mode 100644
index 0000000..28242c8
--- /dev/null
+++ b/infra/buildkite/.terraform.lock.hcl
@@ -0,0 +1,17 @@
+# This file is maintained automatically by "terraform init".
+# Manual edits may be lost in future updates.
+
+provider "registry.terraform.io/buildkite/buildkite" {
+  version     = "1.33.0"
+  constraints = "~> 1.16"
+  hashes = [
+    "h1:frKo2xdyo3okglzw7QDQetKfIb5bi3jXJL5G8TndYl8=",
+    "zh:3660fa93495158f5d11377da64cb094179fe636be3ecf80150f28678edc37031",
+    "zh:541d4532c875b2ee7ecb98da9a1461e76788893b623b0adf7c634d9fff7770e3",
+    "zh:a2eb8dcb337d5f228aa9ac541735a9e2aaf700e7fdc8daa7cb048f18ba1aa576",
+    "zh:c29faff7d1e83ef0fadec732e2561cc74679497acfb1254742c760dc8bfd82b8",
+    "zh:c843834933c8821c2f0134021886ed838b0c36e0fe5cb45fc8a6230c47ecbde3",
+    "zh:dab83c4263ecbbb96ec8feffc557a550c96603ebf4306edab00768df144812a3",
+    "zh:dfdbe6527122422f8cad29d540c7104344f857e96a11a69f2c36d988aac0a1b2",
+  ]
+}
diff --git a/infra/buildkite/README.md b/infra/buildkite/README.md
new file mode 100644
index 0000000..4539141
--- /dev/null
+++ b/infra/buildkite/README.md
@@ -0,0 +1,40 @@
+# Buildkite Terraform
+
+This directory provisions Buildkite CI for OpenMowerNext without using `bk auth`.
+
+## Prerequisites
+
+- Terraform `>= 1.5` or OpenTofu.
+- A Buildkite API token with GraphQL enabled and these scopes: `read_pipelines`, `write_pipelines`, `write_suites`, `read_clusters`, `write_clusters`, `read_teams`, `read_organizations`.
+- The Buildkite GitHub App connected to `jkaflik/OpenMowerNext` if `create_webhook = true`.
+
+## Apply
+
+```bash
+cd infra/buildkite
+cp terraform.tfvars.example terraform.tfvars
+export BUILDKITE_API_TOKEN="..."
+terraform init
+terraform validate
+terraform plan
+terraform apply
+```
+
+If hosted queue capacity is limited on the Buildkite plan, override the shapes in `terraform.tfvars`:
+
+```hcl
+ci_instance_shape     = "LINUX_AMD64_2X4"
+webots_instance_shape = "LINUX_AMD64_2X4"
+```
+
+If the GitHub App is not connected yet, set:
+
+```hcl
+create_webhook = false
+```
+
+## State
+
+Terraform state can contain sensitive values if secrets are added later. Keep state out of git and move it to an encrypted remote backend before managing Buildkite secrets such as GHCR tokens.
+
+The current configuration does not create Buildkite secrets and does not push Docker images.
diff --git a/infra/buildkite/main.tf b/infra/buildkite/main.tf
new file mode 100644
index 0000000..b12c422
--- /dev/null
+++ b/infra/buildkite/main.tf
@@ -0,0 +1,86 @@
+data "buildkite_teams" "current" {}
+
+locals {
+  default_team_ids = [
+    for team in data.buildkite_teams.current.teams : team.id
+    if team.is_default_team
+  ]
+
+  default_team_id = var.default_team_id != "" ? var.default_team_id : try(local.default_team_ids[0], null)
+
+  upload_steps = <<-YAML
+    steps:
+      - label: "Pipeline upload"
+        command: "buildkite-agent pipeline upload"
+        agents:
+          queue: "${buildkite_cluster_queue.ci.key}"
+  YAML
+}
+
+resource "buildkite_cluster" "openmowernext" {
+  name        = var.cluster_name
+  description = "Hosted CI for OpenMowerNext."
+  emoji       = ":seedling:"
+  color       = "#5f8f3a"
+}
+
+resource "buildkite_cluster_queue" "ci" {
+  cluster_id  = buildkite_cluster.openmowernext.id
+  key         = var.ci_queue_key
+  description = "General OpenMowerNext CI jobs."
+
+  hosted_agents = {
+    instance_shape = var.ci_instance_shape
+  }
+}
+
+resource "buildkite_cluster_queue" "webots" {
+  cluster_id  = buildkite_cluster.openmowernext.id
+  key         = var.webots_queue_key
+  description = "OpenMowerNext Webots integration jobs."
+
+  hosted_agents = {
+    instance_shape = var.webots_instance_shape
+  }
+}
+
+resource "buildkite_pipeline" "openmowernext" {
+  name            = var.pipeline_name
+  slug            = var.pipeline_slug
+  description     = "Build, test, and validate OpenMowerNext."
+  repository      = var.repository
+  cluster_id      = buildkite_cluster.openmowernext.id
+  default_team_id = local.default_team_id
+  default_branch  = "main"
+  steps           = local.upload_steps
+  tags            = ["ros2", "webots", "openmowernext"]
+
+  cancel_intermediate_builds               = true
+  cancel_intermediate_builds_branch_filter = "*"
+
+  provider_settings = {
+    trigger_mode = "code"
+
+    build_branches                         = true
+    build_pull_requests                    = true
+    build_pull_request_base_branch_changed = true
+    build_pull_request_labels_changed      = true
+    build_pull_request_ready_for_review    = true
+    build_pull_request_merge_commits       = true
+    build_tags                             = false
+
+    publish_commit_status          = true
+    publish_commit_status_per_step = true
+    use_step_key_as_commit_status  = true
+
+    filter_enabled   = true
+    filter_condition = "build.source == \"ui\" || build.source == \"api\" || build.source == \"schedule\" || build.source_event == \"push\" || build.source_event == \"pull_request\" || build.source_event == \"merge_group\""
+  }
+}
+
+resource "buildkite_pipeline_webhook" "github" {
+  count = var.create_webhook ? 1 : 0
+
+  pipeline_id = buildkite_pipeline.openmowernext.id
+  repository  = buildkite_pipeline.openmowernext.repository
+}
diff --git a/infra/buildkite/outputs.tf b/infra/buildkite/outputs.tf
new file mode 100644
index 0000000..612302c
--- /dev/null
+++ b/infra/buildkite/outputs.tf
@@ -0,0 +1,24 @@
+output "pipeline_url" {
+  description = "Buildkite pipeline URL."
+  value       = "https://buildkite.com/${var.organization_slug}/${buildkite_pipeline.openmowernext.slug}"
+}
+
+output "cluster_uuid" {
+  description = "Buildkite cluster UUID."
+  value       = buildkite_cluster.openmowernext.uuid
+}
+
+output "ci_queue_key" {
+  description = "General CI hosted queue key."
+  value       = buildkite_cluster_queue.ci.key
+}
+
+output "webots_queue_key" {
+  description = "Webots integration hosted queue key."
+  value       = buildkite_cluster_queue.webots.key
+}
+
+output "webhook_url" {
+  description = "Buildkite webhook URL if the webhook resource was created."
+  value       = try(buildkite_pipeline_webhook.github[0].webhook_url, null)
+}
diff --git a/infra/buildkite/terraform.tfvars.example b/infra/buildkite/terraform.tfvars.example
new file mode 100644
index 0000000..cced4b5
--- /dev/null
+++ b/infra/buildkite/terraform.tfvars.example
@@ -0,0 +1,9 @@
+organization_slug = "kuba-kaflik"
+repository        = "https://github.com/jkaflik/OpenMowerNext.git"
+
+# Set to false if the Buildkite GitHub App is not connected to this repository yet.
+create_webhook = true
+
+# Personal plans may only allow LINUX_AMD64_2X4.
+ci_instance_shape     = "LINUX_AMD64_4X16"
+webots_instance_shape = "LINUX_AMD64_8X32"
diff --git a/infra/buildkite/variables.tf b/infra/buildkite/variables.tf
new file mode 100644
index 0000000..ec35df0
--- /dev/null
+++ b/infra/buildkite/variables.tf
@@ -0,0 +1,65 @@
+variable "organization_slug" {
+  description = "Buildkite organization slug."
+  type        = string
+  default     = "kuba-kaflik"
+}
+
+variable "repository" {
+  description = "Git repository URL used by the Buildkite pipeline."
+  type        = string
+  default     = "https://github.com/jkaflik/OpenMowerNext.git"
+}
+
+variable "cluster_name" {
+  description = "Dedicated Buildkite cluster name. Buildkite cluster names cannot contain spaces."
+  type        = string
+  default     = "OpenMowerNext"
+}
+
+variable "pipeline_name" {
+  description = "Buildkite pipeline display name."
+  type        = string
+  default     = "OpenMowerNext"
+}
+
+variable "pipeline_slug" {
+  description = "Buildkite pipeline URL slug."
+  type        = string
+  default     = "openmowernext"
+}
+
+variable "default_team_id" {
+  description = "Optional Buildkite team GraphQL ID for initial pipeline access. Leave empty to use the organization default team."
+  type        = string
+  default     = ""
+}
+
+variable "create_webhook" {
+  description = "Create a GitHub App webhook for the pipeline. Requires the Buildkite GitHub App to be connected to the repository."
+  type        = bool
+  default     = true
+}
+
+variable "ci_queue_key" {
+  description = "Buildkite hosted queue key for normal CI jobs."
+  type        = string
+  default     = "openmowernext-ci"
+}
+
+variable "webots_queue_key" {
+  description = "Buildkite hosted queue key for Webots integration jobs."
+  type        = string
+  default     = "openmowernext-webots"
+}
+
+variable "ci_instance_shape" {
+  description = "Buildkite hosted Linux instance shape for normal CI jobs. Override to LINUX_AMD64_2X4 on Personal plans if needed."
+  type        = string
+  default     = "LINUX_AMD64_4X16"
+}
+
+variable "webots_instance_shape" {
+  description = "Buildkite hosted Linux instance shape for Webots integration jobs. Override to LINUX_AMD64_2X4 on Personal plans if needed."
+  type        = string
+  default     = "LINUX_AMD64_8X32"
+}
diff --git a/infra/buildkite/versions.tf b/infra/buildkite/versions.tf
new file mode 100644
index 0000000..a77944d
--- /dev/null
+++ b/infra/buildkite/versions.tf
@@ -0,0 +1,14 @@
+terraform {
+  required_version = ">= 1.5.0"
+
+  required_providers {
+    buildkite = {
+      source  = "buildkite/buildkite"
+      version = "~> 1.16"
+    }
+  }
+}
+
+provider "buildkite" {
+  organization = var.organization_slug
+}
diff --git a/launch/gps.launch.py b/launch/gps.launch.py
index b29c3b3..2f25609 100644
--- a/launch/gps.launch.py
+++ b/launch/gps.launch.py
@@ -1,14 +1,10 @@
 import os
-import yaml
 from launch import LaunchDescription
-from launch.actions import IncludeLaunchDescription
-from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch_ros.actions import Node
-from launch_ros.substitutions import FindPackageShare
-from launch.substitutions import LaunchConfiguration
-from launch.actions import DeclareLaunchArgument
 
 from ament_index_python.packages import get_package_share_directory
+
+
 def generate_launch_description():
     nodes = [
         Node(
@@ -35,7 +31,8 @@ def generate_launch_description():
                     'password': os.getenv('OM_NTRIP_PASSWORD', ''),
                     'rtcm_message_package': 'rtcm_msgs',
                 }],
+                remappings=[('fix', '/gps/fix')],
             ),
         )
 
-    return LaunchDescription(nodes)
\ No newline at end of file
+    return LaunchDescription(nodes)
diff --git a/launch/joystick.launch.py b/launch/joystick.launch.py
index d0b0e10..c7b00d6 100644
--- a/launch/joystick.launch.py
+++ b/launch/joystick.launch.py
@@ -2,44 +2,44 @@
 from launch_ros.actions import Node
 from launch.substitutions import LaunchConfiguration
 from launch.actions import DeclareLaunchArgument
+from launch.conditions import IfCondition
 
 import os
 from ament_index_python.packages import get_package_share_directory
 
+
 def generate_launch_description():
     use_sim_time = LaunchConfiguration('use_sim_time')
+    enable_joy_node = LaunchConfiguration('enable_joy_node')
 
-    joy_params = os.path.join(get_package_share_directory('open_mower_next'),'config','joystick.yaml')
+    joy_params = os.path.join(get_package_share_directory('open_mower_next'), 'config', 'joystick.yaml')
 
     joy_node = Node(
-            package='joy',
-            executable='joy_node',
-            parameters=[joy_params, {'use_sim_time': use_sim_time}],
-         )
+        package='joy',
+        executable='joy_node',
+        name='joy_node',
+        parameters=[joy_params, {'use_sim_time': use_sim_time}],
+        remappings=[('joy', '/joy')],
+        condition=IfCondition(enable_joy_node),
+    )
 
     teleop_node = Node(
-            package='teleop_twist_joy',
-            executable='teleop_node',
-            name='teleop_node',
-            parameters=[joy_params, {'use_sim_time': use_sim_time}],
-            remappings=[('/cmd_vel','/cmd_vel_joy')]
-         )
-
-    # twist_stamper = Node(
-    #         package='twist_stamper',
-    #         executable='twist_stamper',
-    #         parameters=[{'use_sim_time': use_sim_time}],
-    #         remappings=[('/cmd_vel_in','/diff_cont/cmd_vel_unstamped'),
-    #                     ('/cmd_vel_out','/diff_cont/cmd_vel')]
-    #      )
-
+        package='teleop_twist_joy',
+        executable='teleop_node',
+        name='teleop_twist_joy_node',
+        parameters=[joy_params, {'use_sim_time': use_sim_time}],
+        remappings=[('joy', '/joy'), ('cmd_vel', '/cmd_vel_joy')],
+    )
 
     return LaunchDescription([
         DeclareLaunchArgument(
             'use_sim_time',
             default_value='false',
             description='Use sim time if true'),
+        DeclareLaunchArgument(
+            'enable_joy_node',
+            default_value='false',
+            description='Start a local joy_node instead of relying on an external /joy publisher'),
         joy_node,
         teleop_node,
-        # twist_stamper       
-    ])
\ No newline at end of file
+    ])
diff --git a/launch/localization.launch.py b/launch/localization.launch.py
index fc542c3..e73c5b4 100644
--- a/launch/localization.launch.py
+++ b/launch/localization.launch.py
@@ -1,44 +1,210 @@
 import os
+import math
 
 from ament_index_python.packages import get_package_share_directory
 
 from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable, TimerAction
+from launch.actions import (
+    DeclareLaunchArgument,
+    EmitEvent,
+    RegisterEventHandler,
+    SetEnvironmentVariable,
+    TimerAction,
+)
 from launch.substitutions import LaunchConfiguration
-from launch_ros.actions import Node
+from launch_ros.actions import LifecycleNode, Node
+from launch_ros.event_handlers import OnStateTransition
+from launch_ros.events.lifecycle import ChangeState
+from launch_ros.parameter_descriptions import ParameterValue
+from lifecycle_msgs.msg import Transition
+
+
+def require_env_float(name):
+    value = os.getenv(name)
+    if value is None:
+        raise RuntimeError(f"{name} must be set")
+    return float(value)
+
+
+def wgs84_to_ecef(lat_deg, lon_deg, alt_m=0.0):
+    lat = math.radians(lat_deg)
+    lon = math.radians(lon_deg)
+    semi_major_axis = 6378137.0
+    flattening = 1.0 / 298.257223563
+    eccentricity_sq = flattening * (2.0 - flattening)
+    sin_lat = math.sin(lat)
+    cos_lat = math.cos(lat)
+    radius = semi_major_axis / math.sqrt(1.0 - eccentricity_sq * sin_lat * sin_lat)
+
+    x = (radius + alt_m) * cos_lat * math.cos(lon)
+    y = (radius + alt_m) * cos_lat * math.sin(lon)
+    z = (radius * (1.0 - eccentricity_sq) + alt_m) * sin_lat
+    return x, y, z
 
 
 def generate_launch_description():
     package_path = get_package_share_directory("open_mower_next")
 
     use_sim_time = LaunchConfiguration("use_sim_time")
+    gnss_base_noise_xy = LaunchConfiguration("gnss_base_noise_xy")
+    gnss_track_heading_min_speed = LaunchConfiguration("gnss_track_heading_min_speed")
+    gnss_track_heading_min_dist = LaunchConfiguration("gnss_track_heading_min_dist")
+    gnss_heading_observable_distance = LaunchConfiguration("gnss_heading_observable_distance")
+    gnss_use_gps_fix = LaunchConfiguration("gnss_use_gps_fix")
+    gnss_fix_topic = LaunchConfiguration("gnss_fix_topic")
+    init_stationary_window = LaunchConfiguration("init_stationary_window")
+    init_wait_for_all_sensors = LaunchConfiguration("init_wait_for_all_sensors")
+    datum_lat = require_env_float("OM_DATUM_LAT")
+    datum_lon = require_env_float("OM_DATUM_LONG")
+    datum_x, datum_y, datum_z = wgs84_to_ecef(datum_lat, datum_lon)
+
+    fusioncore_params_path = os.path.join(
+        package_path, "config", "fusioncore.yaml"
+    )
 
-    localization_params_path = os.path.join(
-        package_path, "config", "robot_localization.yaml"
+    fusioncore_node = LifecycleNode(
+        package="fusioncore_ros",
+        executable="fusioncore_node",
+        name="fusioncore",
+        namespace="",
+        output="screen",
+        parameters=[
+            fusioncore_params_path,
+            {
+                "use_sim_time": use_sim_time,
+                "reference.use_first_fix": False,
+                "reference.x": datum_x,
+                "reference.y": datum_y,
+                "reference.z": datum_z,
+                "gnss.base_noise_xy": ParameterValue(gnss_base_noise_xy, value_type=float),
+                "gnss.track_heading_min_speed": ParameterValue(
+                    gnss_track_heading_min_speed, value_type=float
+                ),
+                "gnss.track_heading_min_dist": ParameterValue(
+                    gnss_track_heading_min_dist, value_type=float
+                ),
+                "gnss.heading_observable_distance": ParameterValue(
+                    gnss_heading_observable_distance, value_type=float
+                ),
+                "gnss.use_gps_fix": ParameterValue(gnss_use_gps_fix, value_type=bool),
+                "init.stationary_window": ParameterValue(init_stationary_window, value_type=float),
+                "init.wait_for_all_sensors": ParameterValue(
+                    init_wait_for_all_sensors, value_type=bool
+                ),
+            },
+        ],
+        remappings=[
+            ("/imu/data", "/imu/data_raw"),
+            ("/odom/wheels", "/diff_drive_base_controller/odom"),
+            ("/gnss/fix", gnss_fix_topic),
+        ],
+    )
+
+    configure_fusioncore = TimerAction(
+        period=2.0,
+        actions=[
+            EmitEvent(
+                event=ChangeState(
+                    lifecycle_node_matcher=lambda action: action is fusioncore_node,
+                    transition_id=Transition.TRANSITION_CONFIGURE,
+                )
+            )
+        ],
+    )
+
+    activate_fusioncore = RegisterEventHandler(
+        OnStateTransition(
+            target_lifecycle_node=fusioncore_node,
+            start_state="configuring",
+            goal_state="inactive",
+            entities=[
+                EmitEvent(
+                    event=ChangeState(
+                        lifecycle_node_matcher=lambda action: action is fusioncore_node,
+                        transition_id=Transition.TRANSITION_ACTIVATE,
+                    )
+                )
+            ],
+        )
     )
 
     return LaunchDescription(
         [
             # Set env var to print messages to stdout immediately
             SetEnvironmentVariable("RCUTILS_LOGGING_BUFFERED_STREAM", "1"),
-            DeclareLaunchArgument(
-                "namespace", default_value="", description="Top-level namespace"
-            ),
             DeclareLaunchArgument(
                 "use_sim_time",
                 default_value="false",
                 description="Use simulation clock if true",
             ),
             DeclareLaunchArgument(
-                "autostart",
-                default_value="true",
-                description="Automatically startup the nav2 stack",
+                "gnss_base_noise_xy",
+                default_value="0.5",
+                description="GNSS horizontal base noise for FusionCore",
+            ),
+            DeclareLaunchArgument(
+                "gnss_track_heading_min_speed",
+                default_value="0.2",
+                description="Minimum speed to count GNSS track motion for heading observability",
+            ),
+            DeclareLaunchArgument(
+                "gnss_track_heading_min_dist",
+                default_value="5.0",
+                description="GNSS track baseline before fusing track heading",
+            ),
+            DeclareLaunchArgument(
+                "gnss_heading_observable_distance",
+                default_value="5.0",
+                description="Valid GNSS track distance before enabling GNSS lever-arm correction",
+            ),
+            DeclareLaunchArgument(
+                "gnss_use_gps_fix",
+                default_value="false",
+                description="Subscribe to gps_msgs/GPSFix instead of sensor_msgs/NavSatFix",
+            ),
+            DeclareLaunchArgument(
+                "gnss_fix_topic",
+                default_value="/gps/fix",
+                description="GNSS fix topic remapped into FusionCore /gnss/fix",
+            ),
+            DeclareLaunchArgument(
+                "init_stationary_window",
+                default_value="2.0",
+                description="Seconds of stationary IMU data to collect before FusionCore init",
             ),
             DeclareLaunchArgument(
-                "params_file",
-                default_value=os.path.join(package_path, "config", "nav2_params.yaml"),
-                description="Full path to the ROS2 parameters file to use",
+                "init_wait_for_all_sensors",
+                default_value="true",
+                description="Wait for every configured FusionCore sensor before initialization",
             ),
+            Node(
+                package="tf2_ros",
+                executable="static_transform_publisher",
+                name="map_to_odom_static_transform",
+                output="screen",
+                arguments=[
+                    "--x",
+                    "0",
+                    "--y",
+                    "0",
+                    "--z",
+                    "0",
+                    "--roll",
+                    "0",
+                    "--pitch",
+                    "0",
+                    "--yaw",
+                    "0",
+                    "--frame-id",
+                    "map",
+                    "--child-frame-id",
+                    "odom",
+                ],
+                parameters=[{"use_sim_time": use_sim_time}],
+            ),
+            fusioncore_node,
+            configure_fusioncore,
+            activate_fusioncore,
             TimerAction(
                 period=3.0,
                 actions=[
@@ -87,47 +253,5 @@ def generate_launch_description():
                     }
                 ],
             ),
-            Node(
-                package="robot_localization",
-                executable="ekf_node",
-                name="ekf_se_odom",
-                output="screen",
-                parameters=[localization_params_path, {"use_sim_time": use_sim_time}],
-                remappings=[
-                    ("imu/data", "imu/data_raw"),
-                ],
-            ),
-            Node(
-                package="robot_localization",
-                executable="ekf_node",
-                name="ekf_se_map",
-                output="screen",
-                parameters=[localization_params_path, {"use_sim_time": use_sim_time}],
-                remappings=[
-                    ("imu/data", "imu/data_raw"),
-                    ("odometry/filtered", "odometry/filtered/map"),
-                ],
-            ),
-            Node(
-                package="robot_localization",
-                executable="navsat_transform_node",
-                name="navsat_transform_node",
-                output="screen",
-                parameters=[
-                    localization_params_path,
-                    {
-                        "use_sim_time": use_sim_time,
-                        "datum": [
-                            float(os.getenv("OM_DATUM_LAT")),
-                            float(os.getenv("OM_DATUM_LONG")),
-                            0.0,
-                        ],
-                    },
-                ],
-                remappings=[
-                    ("odometry/filtered", "odometry/filtered/map"),
-                    ("imu", "gps/orientation"),
-                ],
-            ),
         ]
     )
diff --git a/launch/nav2.launch.py b/launch/nav2.launch.py
index 16bdd09..5531dbd 100644
--- a/launch/nav2.launch.py
+++ b/launch/nav2.launch.py
@@ -1,15 +1,15 @@
 from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable
+from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable, TimerAction
 from launch.substitutions import LaunchConfiguration
-from launch_ros.actions import LoadComposableNodes
 from launch_ros.actions import Node
-from launch_ros.descriptions import ComposableNode, ParameterFile
+from launch_ros.descriptions import ParameterFile
 from nav2_common.launch import RewrittenYaml
 from ament_index_python.packages import get_package_share_directory
 import os
 
 def generate_launch_description():
-    params_file = os.path.join(get_package_share_directory("open_mower_next"), 'config', 'nav2_params.yaml')
+    default_params_file = os.path.join(get_package_share_directory("open_mower_next"), 'config', 'nav2_params.yaml')
+    params_file = LaunchConfiguration('params_file')
 
     lifecycle_nodes = ['controller_server',
                        'planner_server',
@@ -43,78 +43,79 @@ def generate_launch_description():
     stdout_linebuf_envvar = SetEnvironmentVariable(
         'RCUTILS_LOGGING_BUFFERED_STREAM', '1')
 
-    create_container = Node(
-        name='nav2_container',
-        package='rclcpp_components',
-        executable='component_container_isolated',
-        parameters=[configured_params, {'autostart': autostart, 'use_sim_time': use_sim_time}],
-        remappings=remappings,
-        output='screen')
-
     common_node_params = [configured_params, {'use_sim_time': use_sim_time}]
 
-    load_composable_nodes = LoadComposableNodes(
-        target_container='nav2_container',
-        composable_node_descriptions=[
-            ComposableNode(
-                package='nav2_controller',
-                plugin='nav2_controller::ControllerServer',
-                name='controller_server',
-                parameters=common_node_params,
-                remappings=remappings + [('cmd_vel', 'cmd_vel_raw')]),
-            ComposableNode(
-                package='nav2_bt_navigator',
-                plugin='nav2_bt_navigator::BtNavigator',
-                name='bt_navigator',
-                parameters=common_node_params,
-                remappings=remappings),
-            ComposableNode(
-                package='nav2_behaviors',
-                plugin='behavior_server::BehaviorServer',
-                name='behavior_server',
-                parameters=common_node_params,
-                remappings=remappings + [('cmd_vel', 'cmd_vel_raw')],),
-            ComposableNode(
-                package='nav2_planner',
-                plugin='nav2_planner::PlannerServer',
-                name='planner_server',
-                parameters=common_node_params,
-                remappings=remappings),
-            ComposableNode(
-                package='nav2_smoother',
-                plugin='nav2_smoother::SmootherServer',
-                name='smoother_server',
-                parameters=common_node_params,
-                remappings=remappings),
-            ComposableNode(
-                package='nav2_velocity_smoother',
-                plugin='nav2_velocity_smoother::VelocitySmoother',
-                name='velocity_smoother',
-                parameters=common_node_params,
-                remappings=remappings +
-                           [('cmd_vel', 'cmd_vel_raw'), ('cmd_vel_smoothed', 'cmd_vel_nav')]),
-            ComposableNode(
-                package='opennav_docking',
-                plugin='opennav_docking::DockingServer',
-                name='docking_server',
-                parameters=common_node_params,
-                remappings=remappings + [('cmd_vel', 'cmd_vel_raw')]),
-            ComposableNode(
-                package='nav2_lifecycle_manager',
-                plugin='nav2_lifecycle_manager::LifecycleManager',
-                name='lifecycle_manager_navigation',
-                parameters=[{'use_sim_time': use_sim_time,
-                             'autostart': autostart,
-                             'node_names': lifecycle_nodes}]),
-        ],
-    )
+    controller_server = Node(
+        package='nav2_controller',
+        executable='controller_server',
+        name='controller_server',
+        output='screen',
+        parameters=common_node_params,
+        remappings=remappings + [('cmd_vel', 'cmd_vel_raw')])
+    bt_navigator = Node(
+        package='nav2_bt_navigator',
+        executable='bt_navigator',
+        name='bt_navigator',
+        output='screen',
+        parameters=common_node_params,
+        remappings=remappings)
+    behavior_server = Node(
+        package='nav2_behaviors',
+        executable='behavior_server',
+        name='behavior_server',
+        output='screen',
+        parameters=common_node_params,
+        remappings=remappings + [('cmd_vel', 'cmd_vel_raw')])
+    planner_server = Node(
+        package='nav2_planner',
+        executable='planner_server',
+        name='planner_server',
+        output='screen',
+        parameters=common_node_params,
+        remappings=remappings)
+    smoother_server = Node(
+        package='nav2_smoother',
+        executable='smoother_server',
+        name='smoother_server',
+        output='screen',
+        parameters=common_node_params,
+        remappings=remappings)
+    velocity_smoother = Node(
+        package='nav2_velocity_smoother',
+        executable='velocity_smoother',
+        name='velocity_smoother',
+        output='screen',
+        parameters=common_node_params,
+        remappings=remappings + [('cmd_vel', 'cmd_vel_raw'), ('cmd_vel_smoothed', 'cmd_vel_nav')])
+    docking_server = Node(
+        package='opennav_docking',
+        executable='opennav_docking',
+        name='docking_server',
+        output='screen',
+        parameters=common_node_params,
+        remappings=remappings + [('cmd_vel', 'cmd_vel_raw')])
+    lifecycle_manager = Node(
+        package='nav2_lifecycle_manager',
+        executable='lifecycle_manager',
+        name='lifecycle_manager_navigation',
+        output='screen',
+        parameters=[{'use_sim_time': use_sim_time,
+                     'autostart': autostart,
+                     'node_names': lifecycle_nodes}])
 
     # # Create the launch description and populate
     ld = LaunchDescription([
-        DeclareLaunchArgument('params_file', default_value=os.path.join(get_package_share_directory("open_mower_next"), 'config', 'nav2_params.yaml')),
+        stdout_linebuf_envvar,
+        DeclareLaunchArgument('params_file', default_value=default_params_file),
         DeclareLaunchArgument('use_sim_time', default_value='True'),
         DeclareLaunchArgument('autostart', default_value='True'),
-        create_container,
-        load_composable_nodes
+        controller_server,
+        bt_navigator,
+        behavior_server,
+        planner_server,
+        smoother_server,
+        velocity_smoother,
+        docking_server,
+        TimerAction(period=2.0, actions=[lifecycle_manager])
     ])
     return ld
diff --git a/launch/openmower.launch.py b/launch/openmower.launch.py
index c950c79..2a05891 100644
--- a/launch/openmower.launch.py
+++ b/launch/openmower.launch.py
@@ -1,21 +1,59 @@
 import os
+import tempfile
 
 from ament_index_python.packages import get_package_share_directory
+import yaml
 
 from launch import LaunchDescription
-from launch.actions import IncludeLaunchDescription, RegisterEventHandler, ExecuteProcess
+from launch.actions import (
+    DeclareLaunchArgument,
+    EmitEvent,
+    ExecuteProcess,
+    IncludeLaunchDescription,
+    RegisterEventHandler,
+)
+from launch.conditions import IfCondition, UnlessCondition
 from launch.event_handlers import OnProcessStart, OnProcessExit
+from launch.events import Shutdown
 from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration
+from launch_xml.launch_description_sources import XMLLaunchDescriptionSource
 
 from launch_ros.actions import Node
 
 import xacro
 
 
+def controller_parameters_file(share_directory):
+    controller_path = os.path.join(share_directory, 'config', 'controllers.yaml')
+    hardware_path = os.path.join(share_directory, 'config', 'hardware', 'yardforce500.yaml')
+    with open(controller_path, 'r', encoding='utf-8') as stream:
+        controllers = yaml.safe_load(stream)
+    with open(hardware_path, 'r', encoding='utf-8') as stream:
+        hardware = yaml.safe_load(stream)
+    wheel_offset_y = float(hardware['wheel']['offset'][1])
+    controllers.setdefault('diff_drive_base_controller', {}).setdefault('ros__parameters', {})[
+        'wheel_separation'
+    ] = 2.0 * abs(wheel_offset_y)
+
+    params_file = tempfile.NamedTemporaryFile(
+        mode='w', prefix='openmower_controllers_', suffix='.yaml', delete=False
+    )
+    with params_file:
+        yaml.safe_dump(controllers, params_file, sort_keys=False)
+    return params_file.name
+
+
 def generate_launch_description():
     package_name = 'open_mower_next'
 
     share_directory = get_package_share_directory(package_name)
+    enable_foxglove = LaunchConfiguration('enable_foxglove')
+    foxglove_address = LaunchConfiguration('foxglove_address')
+    foxglove_port = LaunchConfiguration('foxglove_port')
+    enable_joy_node = LaunchConfiguration('enable_joy_node')
+    enable_navigation_readiness = LaunchConfiguration('enable_navigation_readiness')
+
     xacro_file = os.path.join(share_directory, 'description/robot.urdf.xacro')
     robot_description_config = xacro.process_file(xacro_file, mappings={
         'use_ros2_control': '1',
@@ -34,7 +72,10 @@ def generate_launch_description():
     joystick = IncludeLaunchDescription(
         PythonLaunchDescriptionSource([os.path.join(
             share_directory, 'launch', 'joystick.launch.py'
-        )]), launch_arguments={'use_sim_time': 'false'}.items()
+        )]), launch_arguments={
+            'use_sim_time': 'false',
+            'enable_joy_node': enable_joy_node,
+        }.items()
     )
 
     twist_mux_params = os.path.join(share_directory, 'config', 'twist_mux.yaml')
@@ -45,7 +86,7 @@ def generate_launch_description():
         remappings=[('/cmd_vel_out', '/diff_drive_base_controller/cmd_vel')]
     )
 
-    controller_params_file = os.path.join(share_directory, 'config', 'controllers.yaml')
+    controller_params_file = controller_parameters_file(share_directory)
 
     controller_manager = Node(
         package="controller_manager",
@@ -72,9 +113,61 @@ def generate_launch_description():
         output='screen'
     )
 
+    navigation_readiness = Node(
+        package='open_mower_next',
+        executable='navigation_readiness_node',
+        output='screen',
+        parameters=[{'use_sim_time': False, 'timeout_seconds': 120.0}],
+        condition=IfCondition(enable_navigation_readiness),
+    )
+
+    def make_nav2(condition=None):
+        return IncludeLaunchDescription(
+            PythonLaunchDescriptionSource([share_directory, '/launch/nav2.launch.py']),
+            launch_arguments={
+                'use_sim_time': 'false',
+                'autostart': 'true',
+            }.items(),
+            condition=condition,
+        )
+
+    nav2 = make_nav2()
+    nav2_without_readiness = make_nav2(UnlessCondition(enable_navigation_readiness))
+
+    foxglove_bridge = IncludeLaunchDescription(
+        XMLLaunchDescriptionSource(
+            os.path.join(
+                get_package_share_directory('foxglove_bridge'),
+                'launch',
+                'foxglove_bridge_launch.xml',
+            )
+        ),
+        launch_arguments={
+            'address': foxglove_address,
+            'port': foxglove_port,
+            'include_hidden': 'true',
+            'use_sim_time': 'false',
+        }.items(),
+        condition=IfCondition(enable_foxglove),
+    )
+
+    def start_nav2_or_shutdown(event, context):
+        if context.is_shutdown:
+            return []
+        if event.returncode == 0:
+            return [nav2]
+        return [EmitEvent(event=Shutdown(reason='Navigation prerequisites timed out'))]
+
     # Launch them all!
     return LaunchDescription([
+        DeclareLaunchArgument('enable_foxglove', default_value='true'),
+        DeclareLaunchArgument('foxglove_address', default_value='0.0.0.0'),
+        DeclareLaunchArgument('foxglove_port', default_value='8765'),
+        DeclareLaunchArgument('enable_joy_node', default_value='false'),
+        DeclareLaunchArgument('enable_navigation_readiness', default_value='true'),
+
         node_robot_state_publisher,
+        joystick,
         twist_mux,
         controller_manager,
 
@@ -94,7 +187,7 @@ def generate_launch_description():
 
         RegisterEventHandler(
             event_handler=OnProcessExit(
-                target_action=load_joint_state_controller,
+                target_action=load_diff_controller,
                 on_exit=[load_mower_controller],
             )
         ),
@@ -107,20 +200,26 @@ def generate_launch_description():
             PythonLaunchDescriptionSource([share_directory, '/launch/localization.launch.py']),
             launch_arguments={
                 'use_sim_time': 'false',
-                'autostart': 'true',
+                'gnss_use_gps_fix': 'true',
+                'gnss_fix_topic': '/gps/fix_extended',
             }.items(),
         ),
 
-        IncludeLaunchDescription(
-            PythonLaunchDescriptionSource([share_directory, '/launch/nav2.launch.py']),
-            launch_arguments={
-                'use_sim_time': 'false',
-                'autostart': 'true',
-            }.items(),
+        navigation_readiness,
+        nav2_without_readiness,
+
+        RegisterEventHandler(
+            event_handler=OnProcessExit(
+                target_action=navigation_readiness,
+                on_exit=start_nav2_or_shutdown,
+            ),
+            condition=IfCondition(enable_navigation_readiness),
         ),
 
         IncludeLaunchDescription(
             PythonLaunchDescriptionSource(
                 [share_directory, '/launch/micro_ros_agent.launch.py']),
         ),
+
+        foxglove_bridge,
     ])
diff --git a/launch/sim.launch.py b/launch/sim.launch.py
index c67f88d..883830c 100644
--- a/launch/sim.launch.py
+++ b/launch/sim.launch.py
@@ -1,6 +1,8 @@
 import os
+import tempfile
 
 import xacro
+import yaml
 from ament_index_python.packages import get_package_share_directory
 from launch import LaunchDescription
 from launch.actions import (
@@ -9,8 +11,9 @@
     IncludeLaunchDescription,
     OpaqueFunction,
     RegisterEventHandler,
+    SetEnvironmentVariable,
 )
-from launch.conditions import IfCondition
+from launch.conditions import IfCondition, UnlessCondition
 from launch.event_handlers import OnProcessExit
 from launch.events import Shutdown
 from launch.launch_description_sources import PythonLaunchDescriptionSource
@@ -22,6 +25,26 @@
 from webots_ros2_driver.webots_launcher import Ros2SupervisorLauncher, WebotsLauncher
 
 
+def controller_parameters_file(share_directory):
+    controller_path = os.path.join(share_directory, "config", "controllers.yaml")
+    hardware_path = os.path.join(share_directory, "config", "hardware", "yardforce500.yaml")
+    with open(controller_path, "r", encoding="utf-8") as stream:
+        controllers = yaml.safe_load(stream)
+    with open(hardware_path, "r", encoding="utf-8") as stream:
+        hardware = yaml.safe_load(stream)
+    wheel_offset_y = float(hardware["wheel"]["offset"][1])
+    controllers.setdefault("diff_drive_base_controller", {}).setdefault("ros__parameters", {})[
+        "wheel_separation"
+    ] = 2.0 * abs(wheel_offset_y)
+
+    params_file = tempfile.NamedTemporaryFile(
+        mode="w", prefix="openmower_controllers_", suffix=".yaml", delete=False
+    )
+    with params_file:
+        yaml.safe_dump(controllers, params_file, sort_keys=False)
+    return params_file.name
+
+
 def shutdown_on_driver_failure(event, context):
     if context.is_shutdown or event.returncode == 0:
         return []
@@ -33,6 +56,13 @@ def launch_setup(context, *args, **kwargs):
 
     package_name = "open_mower_next"
     share_directory = get_package_share_directory(package_name)
+    webots_plugin_path = os.path.join(share_directory, "resource")
+    existing_pythonpath = os.environ.get("PYTHONPATH", "")
+    webots_pythonpath = (
+        webots_plugin_path
+        if not existing_pythonpath
+        else webots_plugin_path + os.pathsep + existing_pythonpath
+    )
 
     world = LaunchConfiguration("world").perform(context)
     mode = LaunchConfiguration("mode").perform(context)
@@ -47,6 +77,8 @@ def launch_setup(context, *args, **kwargs):
     enable_foxglove = LaunchConfiguration("enable_foxglove")
     foxglove_address = LaunchConfiguration("foxglove_address")
     foxglove_port = LaunchConfiguration("foxglove_port")
+    enable_joy_node = LaunchConfiguration("enable_joy_node")
+    enable_navigation_readiness = LaunchConfiguration("enable_navigation_readiness")
 
     xacro_file = os.path.join(share_directory, "description", "robot.urdf.xacro")
     robot_description_config = xacro.process_file(
@@ -72,6 +104,14 @@ def launch_setup(context, *args, **kwargs):
         remappings=[("/cmd_vel_out", "/diff_drive_base_controller/cmd_vel")],
     )
 
+    joystick = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(os.path.join(share_directory, "launch", "joystick.launch.py")),
+        launch_arguments={
+            "use_sim_time": use_sim_time,
+            "enable_joy_node": enable_joy_node,
+        }.items(),
+    )
+
     webots = WebotsLauncher(
         world=os.path.join(share_directory, "worlds", world),
         mode=mode,
@@ -79,9 +119,9 @@ def launch_setup(context, *args, **kwargs):
         stream=webots_stream,
         port=webots_port,
     )
-    webots_supervisor = Ros2SupervisorLauncher(respawn=False)
+    webots_supervisor = Ros2SupervisorLauncher(respawn=False, port=webots_port)
 
-    controller_params_file = os.path.join(share_directory, "config", "controllers.yaml")
+    controller_params_file = controller_parameters_file(share_directory)
     webots_robot_description = os.path.join(share_directory, "resource", "openmower_webots.urdf")
     webots_driver = WebotsController(
         robot_name="openmower",
@@ -94,6 +134,7 @@ def launch_setup(context, *args, **kwargs):
             controller_params_file,
         ],
         respawn=False,
+        port=webots_port,
     )
 
     controller_manager_timeout = ["--controller-manager-timeout", "50"]
@@ -118,14 +159,12 @@ def launch_setup(context, *args, **kwargs):
         arguments=["mower_controller"] + controller_manager_timeout,
         parameters=[{"use_sim_time": use_sim_time}],
     )
-    controller_spawners = [load_joint_state_controller, load_diff_controller, load_mower_controller]
-
     wait_for_webots_driver = WaitForControllerConnection(
         target_driver=webots_driver,
-        nodes_to_start=controller_spawners,
+        nodes_to_start=[load_joint_state_controller],
     )
 
-    # Simulation helper node publishes the hardware-facing power topics.
+    # Simulation helper node publishes the hardware-facing power status topics.
     sim_node = Node(
         package="open_mower_next",
         executable="sim_node",
@@ -139,6 +178,7 @@ def launch_setup(context, *args, **kwargs):
                 "docking_station_contact_y": 1.5,
                 "docking_station_contact_z": 0.06,
                 "docking_station_contact_yaw": 0.0,
+                "docking_detection_tolerance_x": 0.30,
             }
         ],
     )
@@ -147,19 +187,46 @@ def launch_setup(context, *args, **kwargs):
         PythonLaunchDescriptionSource(os.path.join(share_directory, "launch", "localization.launch.py")),
         launch_arguments={
             "use_sim_time": use_sim_time,
-            "autostart": "true",
+            "gnss_base_noise_xy": "0.05",
+            "gnss_track_heading_min_speed": "0.10",
+            "gnss_track_heading_min_dist": "1.0",
+            "gnss_heading_observable_distance": "1.0",
+            "gnss_use_gps_fix": "false",
+            "gnss_fix_topic": "/gps/fix",
+            "init_stationary_window": "0.0",
+            "init_wait_for_all_sensors": "true",
         }.items(),
     )
 
-    nav2 = IncludeLaunchDescription(
-        PythonLaunchDescriptionSource(os.path.join(share_directory, "launch", "nav2.launch.py")),
-        launch_arguments={
-            "use_sim_time": use_sim_time,
-            "autostart": "true",
-            "params_file": os.path.join(share_directory, "config", "nav2_params.yaml"),
-        }.items(),
+    def make_nav2(condition=None):
+        return IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(os.path.join(share_directory, "launch", "nav2.launch.py")),
+            launch_arguments={
+                "use_sim_time": use_sim_time,
+                "autostart": "true",
+                "params_file": os.path.join(share_directory, "config", "nav2_params.yaml"),
+            }.items(),
+            condition=condition,
+        )
+
+    nav2 = make_nav2()
+    nav2_without_readiness = make_nav2(UnlessCondition(enable_navigation_readiness))
+
+    navigation_readiness = Node(
+        package="open_mower_next",
+        executable="navigation_readiness_node",
+        output="screen",
+        parameters=[{"use_sim_time": use_sim_time, "timeout_seconds": 120.0}],
+        condition=IfCondition(enable_navigation_readiness),
     )
 
+    def start_nav2_or_shutdown(event, context):
+        if context.is_shutdown:
+            return []
+        if event.returncode == 0:
+            return [nav2]
+        return [EmitEvent(event=Shutdown(reason="Navigation prerequisites timed out"))]
+
     foxglove_bridge = IncludeLaunchDescription(
         XMLLaunchDescriptionSource(
             os.path.join(get_package_share_directory("foxglove_bridge"), "launch", "foxglove_bridge_launch.xml")
@@ -174,15 +241,37 @@ def launch_setup(context, *args, **kwargs):
     )
 
     return [
+        SetEnvironmentVariable("PYTHONPATH", webots_pythonpath),
         webots,
         webots_supervisor,
         node_robot_state_publisher,
+        joystick,
         twist_mux,
         webots_driver,
         wait_for_webots_driver,
+        RegisterEventHandler(
+            event_handler=OnProcessExit(
+                target_action=load_joint_state_controller,
+                on_exit=[load_diff_controller],
+            )
+        ),
+        RegisterEventHandler(
+            event_handler=OnProcessExit(
+                target_action=load_diff_controller,
+                on_exit=[load_mower_controller],
+            )
+        ),
         sim_node,
         localization,
-        nav2,
+        navigation_readiness,
+        nav2_without_readiness,
+        RegisterEventHandler(
+            event_handler=OnProcessExit(
+                target_action=navigation_readiness,
+                on_exit=start_nav2_or_shutdown,
+            ),
+            condition=IfCondition(enable_navigation_readiness),
+        ),
         foxglove_bridge,
         RegisterEventHandler(
             event_handler=OnProcessExit(
@@ -202,7 +291,7 @@ def launch_setup(context, *args, **kwargs):
 def generate_launch_description():
     return LaunchDescription(
         [
-            DeclareLaunchArgument("world", default_value="openmower.wbt"),
+            DeclareLaunchArgument("world", default_value="realistic_garden.wbt"),
             DeclareLaunchArgument("mode", default_value="realtime"),
             DeclareLaunchArgument("gui", default_value="true"),
             DeclareLaunchArgument("webots_stream", default_value="true"),
@@ -211,6 +300,8 @@ def generate_launch_description():
             DeclareLaunchArgument("enable_foxglove", default_value="false"),
             DeclareLaunchArgument("foxglove_address", default_value="0.0.0.0"),
             DeclareLaunchArgument("foxglove_port", default_value="8765"),
+            DeclareLaunchArgument("enable_joy_node", default_value="false"),
+            DeclareLaunchArgument("enable_navigation_readiness", default_value="true"),
             OpaqueFunction(function=launch_setup),
         ]
     )
diff --git a/maps/realistic_garden.geojson b/maps/realistic_garden.geojson
new file mode 100644
index 0000000..69359a5
--- /dev/null
+++ b/maps/realistic_garden.geojson
@@ -0,0 +1,278 @@
+{
+  "type": "FeatureCollection",
+  "features": [
+    {
+      "type": "Feature",
+      "properties": {
+        "id": "75a7615c-2222-5014-909a-eb54ed07490c",
+        "name": "realistic_garden_operation_area",
+        "type": "operation",
+        "fill": "#00ff00",
+        "style": {
+          "color": "#00ff00"
+        }
+      },
+      "geometry": {
+        "type": "Polygon",
+        "coordinates": [
+          [
+            [
+              -43.20004015677601,
+              -22.90003268840517
+            ],
+            [
+              -43.199959843223986,
+              -22.90003268840517
+            ],
+            [
+              -43.19995984324323,
+              -22.899967311584547
+            ],
+            [
+              -43.20004015675677,
+              -22.899967311584547
+            ],
+            [
+              -43.20004015677601,
+              -22.90003268840517
+            ]
+          ]
+        ]
+      }
+    },
+    {
+      "type": "Feature",
+      "properties": {
+        "id": "30baf514-e0bd-5c3c-9707-86bd71614834",
+        "name": "house_wall",
+        "type": "exclusion",
+        "fill": "#ff0000",
+        "style": {
+          "color": "#ff0000"
+        }
+      },
+      "geometry": {
+        "type": "Polygon",
+        "coordinates": [
+          [
+            [
+              -43.199978069727315,
+              -22.899998645506344
+            ],
+            [
+              -43.1999507787213,
+              -22.899998645500236
+            ],
+            [
+              -43.199950778731726,
+              -22.899969749667864
+            ],
+            [
+              -43.19997806973196,
+              -22.89996974967397
+            ],
+            [
+              -43.199978069727315,
+              -22.899998645506344
+            ]
+          ]
+        ]
+      }
+    },
+    {
+      "type": "Feature",
+      "properties": {
+        "id": "e065db11-639a-590b-8057-31cf7f091f70",
+        "name": "oak_tree",
+        "type": "exclusion",
+        "fill": "#ff0000",
+        "style": {
+          "color": "#ff0000"
+        }
+      },
+      "geometry": {
+        "type": "Polygon",
+        "coordinates": [
+          [
+            [
+              -43.200024172031256,
+              -22.899983746092467
+            ],
+            [
+              -43.20002426106278,
+              -22.899983331419094
+            ],
+            [
+              -43.20002451460325,
+              -22.89998297987595
+            ],
+            [
+              -43.200024894053435,
+              -22.899982744982303
+            ],
+            [
+              -43.20002534164547,
+              -22.89998266249857
+            ],
+            [
+              -43.20002578923754,
+              -22.899982744982157
+            ],
+            [
+              -43.20002616868781,
+              -22.899982979875684
+            ],
+            [
+              -43.20002642222842,
+              -22.899983331418746
+            ],
+            [
+              -43.20002651126009,
+              -22.899983746092094
+            ],
+            [
+              -43.200026422228575,
+              -22.89998416076547
+            ],
+            [
+              -43.200026168688105,
+              -22.899984512308617
+            ],
+            [
+              -43.20002578923792,
+              -22.899984747202264
+            ],
+            [
+              -43.20002534164587,
+              -22.899984829685994
+            ],
+            [
+              -43.2000248940538,
+              -22.899984747202403
+            ],
+            [
+              -43.20002451460352,
+              -22.899984512308876
+            ],
+            [
+              -43.20002426106293,
+              -22.89998416076581
+            ],
+            [
+              -43.200024172031256,
+              -22.899983746092467
+            ]
+          ]
+        ]
+      }
+    },
+    {
+      "type": "Feature",
+      "properties": {
+        "id": "a3f1b67e-0f22-55d6-9369-1703aebb1846",
+        "name": "young_tree",
+        "type": "exclusion",
+        "fill": "#ff0000",
+        "style": {
+          "color": "#ff0000"
+        }
+      },
+      "geometry": {
+        "type": "Polygon",
+        "coordinates": [
+          [
+            [
+              -43.19998713423828,
+              -22.9000171568999
+            ],
+            [
+              -43.199987223269936,
+              -22.900016742226548
+            ],
+            [
+              -43.19998747681056,
+              -22.90001639068346
+            ],
+            [
+              -43.1999878562609,
+              -22.900016155789903
+            ],
+            [
+              -43.19998830385307,
+              -22.900016073306276
+            ],
+            [
+              -43.199988751445225,
+              -22.900016155789967
+            ],
+            [
+              -43.199989130895524,
+              -22.900016390683582
+            ],
+            [
+              -43.1999893844361,
+              -22.9000167422267
+            ],
+            [
+              -43.19998947346768,
+              -22.900017156900066
+            ],
+            [
+              -43.19998938443604,
+              -22.900017571573425
+            ],
+            [
+              -43.19998913089541,
+              -22.900017923116508
+            ],
+            [
+              -43.19998875144507,
+              -22.90001815801007
+            ],
+            [
+              -43.199988303852884,
+              -22.900018240493694
+            ],
+            [
+              -43.19998785626072,
+              -22.900018158010006
+            ],
+            [
+              -43.19998747681042,
+              -22.900017923116387
+            ],
+            [
+              -43.19998722326985,
+              -22.90001757157326
+            ],
+            [
+              -43.19998713423828,
+              -22.9000171568999
+            ]
+          ]
+        ]
+      }
+    },
+    {
+      "type": "Feature",
+      "properties": {
+        "id": "2bec3f7f-ce2f-5985-b39c-bcc845de35da",
+        "name": "docking_station",
+        "type": "docking_station"
+      },
+      "geometry": {
+        "type": "LineString",
+        "coordinates": [
+          [
+            -43.199982260847676,
+            -22.8999864550776
+          ],
+          [
+            -43.19998713424118,
+            -22.899986455078068
+          ]
+        ]
+      }
+    }
+  ]
+}
diff --git a/package.xml b/package.xml
index c64da89..d051b30 100644
--- a/package.xml
+++ b/package.xml
@@ -16,6 +16,7 @@
 
   <depend>rclcpp</depend>
   <depend>rclcpp_action</depend>
+  <depend>rclpy</depend>
   <depend>std_msgs</depend>
   <depend>std_srvs</depend>
   <depend>geometry_msgs</depend>
@@ -28,8 +29,10 @@
   <depend>ament_index_cpp</depend>
   <depend>visualization_msgs</depend>
   <depend>geographiclib</depend>
+  <depend>geographic_msgs</depend>
   <depend>opennav_docking</depend>
   <depend>sensor_msgs</depend>
+  <depend>omros2_firmware_msgs</depend>
   <depend>fields2cover</depend>
 
   <build_depend>nlohmann_json_schema_validator_vendor</build_depend>
@@ -60,11 +63,14 @@
   <exec_depend>unique_identifier_msgs</exec_depend>
   <exec_depend>micro_ros_agent</exec_depend>
 
-  <exec_depend>ublox_gps</exec_depend>
+  <exec_depend>ublox_f9p</exec_depend>
+  <exec_depend>gps_msgs</exec_depend>
   <exec_depend>ntrip_client</exec_depend>
+  <exec_depend>python3-yaml</exec_depend>
 
   <exec_depend>navigation2</exec_depend>
-  <exec_depend>robot_localization</exec_depend>
+  <exec_depend>fusioncore_ros</exec_depend>
+  <exec_depend>lifecycle_msgs</exec_depend>
 
   <exec_depend>foxglove_msgs</exec_depend>
   <exec_depend>foxglove_bridge</exec_depend>
@@ -75,8 +81,6 @@
   <test_depend>ament_cmake_pytest</test_depend>
   <test_depend>ament_lint_common</test_depend>
   <test_depend>action_msgs</test_depend>
-  <test_depend>lifecycle_msgs</test_depend>
-  <test_depend>rclpy</test_depend>
   <test_depend>rosgraph_msgs</test_depend>
 
   <build_depend>rosidl_default_generators</build_depend>
diff --git a/protos/Curb.proto b/protos/Curb.proto
new file mode 100644
index 0000000..3e666e8
--- /dev/null
+++ b/protos/Curb.proto
@@ -0,0 +1,28 @@
+#VRML_SIM R2025a utf8
+
+PROTO Curb [
+  field SFVec3f    translation 0 0 0.035
+  field SFRotation rotation    0 0 1 0
+  field SFString   name        "curb"
+  field SFVec3f    size        1 0.12 0.07
+]
+{
+  Solid {
+    translation IS translation
+    rotation IS rotation
+    name IS name
+    children [
+      Shape {
+        appearance PBRAppearance {
+          baseColor 0.58 0.58 0.54
+          roughness 0.75
+          metalness 0
+        }
+        geometry DEF CURB_BOX Box {
+          size IS size
+        }
+      }
+    ]
+    boundingObject USE CURB_BOX
+  }
+}
diff --git a/protos/FrictionPatch.proto b/protos/FrictionPatch.proto
new file mode 100644
index 0000000..067d5e3
--- /dev/null
+++ b/protos/FrictionPatch.proto
@@ -0,0 +1,20 @@
+#VRML_SIM R2025a utf8
+
+PROTO FrictionPatch [
+  field SFVec3f    translation     0 0 0.001
+  field SFRotation rotation        0 0 1 0
+  field SFString   name            "friction_patch"
+  field SFVec3f    size            1 1 0.002
+  field SFString   contactMaterial "short_grass"
+]
+{
+  Solid {
+    translation IS translation
+    rotation IS rotation
+    name IS name
+    contactMaterial IS contactMaterial
+    boundingObject Box {
+      size IS size
+    }
+  }
+}
diff --git a/protos/GardenBuilding.proto b/protos/GardenBuilding.proto
new file mode 100644
index 0000000..822b28e
--- /dev/null
+++ b/protos/GardenBuilding.proto
@@ -0,0 +1,28 @@
+#VRML_SIM R2025a utf8
+
+PROTO GardenBuilding [
+  field SFVec3f    translation 0 0 1
+  field SFRotation rotation    0 0 1 0
+  field SFString   name        "building"
+  field SFVec3f    size        4 3 2
+]
+{
+  Solid {
+    translation IS translation
+    rotation IS rotation
+    name IS name
+    children [
+      Shape {
+        appearance PBRAppearance {
+          baseColor 0.62 0.58 0.52
+          roughness 0.9
+          metalness 0
+        }
+        geometry DEF BUILDING_BOX Box {
+          size IS size
+        }
+      }
+    ]
+    boundingObject USE BUILDING_BOX
+  }
+}
diff --git a/protos/GardenTree.proto b/protos/GardenTree.proto
new file mode 100644
index 0000000..c5443d7
--- /dev/null
+++ b/protos/GardenTree.proto
@@ -0,0 +1,53 @@
+#VRML_SIM R2025a utf8
+
+PROTO GardenTree [
+  field SFVec3f    translation 0 0 0
+  field SFRotation rotation    0 0 1 0
+  field SFString   name        "tree"
+]
+{
+  Solid {
+    translation IS translation
+    rotation IS rotation
+    name IS name
+    children [
+      Pose {
+        translation 0 0 0.38
+        children [
+          Shape {
+            appearance PBRAppearance {
+              baseColor 0.33 0.18 0.08
+              roughness 0.85
+              metalness 0
+            }
+            geometry DEF TREE_TRUNK Cylinder {
+              radius 0.12
+              height 0.76
+            }
+          }
+        ]
+      }
+      Pose {
+        translation 0 0 1.05
+        children [
+          Shape {
+            appearance PBRAppearance {
+              baseColor 0.04 0.28 0.07
+              roughness 1
+              metalness 0
+            }
+            geometry Sphere {
+              radius 0.72
+            }
+          }
+        ]
+      }
+    ]
+    boundingObject Pose {
+      translation 0 0 0.38
+      children [
+        USE TREE_TRUNK
+      ]
+    }
+  }
+}
diff --git a/protos/GnssDegradationZone.proto b/protos/GnssDegradationZone.proto
new file mode 100644
index 0000000..d0fcff6
--- /dev/null
+++ b/protos/GnssDegradationZone.proto
@@ -0,0 +1,32 @@
+#VRML_SIM R2025a utf8
+
+PROTO GnssDegradationZone [
+  field SFVec3f    translation        0 0 0.02
+  field SFRotation rotation           0 0 1 0
+  field SFString   name               "gnss_degradation_zone"
+  field SFVec3f    size               1 1 0.04
+  field SFString   metadata           "shape=box;sigmaXY=0;biasX=0;biasY=0;dropoutProbability=0;covarianceXY=0;edgeWidth=0.25;driftTimeConstant=8"
+  field SFColor    color              0.1 0.35 1
+  field SFFloat    transparency       0.82
+]
+{
+  Solid {
+    translation IS translation
+    rotation IS rotation
+    name IS name
+    model IS metadata
+    children [
+      Shape {
+        appearance PBRAppearance {
+          baseColor IS color
+          transparency IS transparency
+          roughness 1
+          metalness 0
+        }
+        geometry Box {
+          size IS size
+        }
+      }
+    ]
+  }
+}
diff --git a/protos/OpenMower.proto b/protos/OpenMower.proto
index 8cf3611..d8daae8 100644
--- a/protos/OpenMower.proto
+++ b/protos/OpenMower.proto
@@ -53,9 +53,6 @@ PROTO OpenMower [
         translation 0.32 0 0.1
         name "gps"
       }
-      InertialUnit {
-        name "inertial_unit"
-      }
       Gyro {
         name "gyro"
       }
@@ -94,6 +91,7 @@ PROTO OpenMower [
             }
           ]
           name "left_wheel"
+          contactMaterial "mower_wheel"
           boundingObject USE WHEEL_CYLINDER
           physics Physics {
             density -1
@@ -126,6 +124,7 @@ PROTO OpenMower [
             }
           ]
           name "right_wheel"
+          contactMaterial "mower_wheel"
           boundingObject USE WHEEL_CYLINDER
           physics Physics {
             density -1
@@ -190,6 +189,7 @@ PROTO OpenMower [
             }
           ]
           name "left_caster_wheel"
+          contactMaterial "mower_wheel"
           boundingObject USE CASTER_SPHERE
           physics Physics {
             density -1
@@ -210,6 +210,7 @@ PROTO OpenMower [
             }
           ]
           name "right_caster_wheel"
+          contactMaterial "mower_wheel"
           boundingObject USE CASTER_SPHERE
           physics Physics {
             density -1
diff --git a/protos/TerrainPatch.proto b/protos/TerrainPatch.proto
new file mode 100644
index 0000000..c1d6dae
--- /dev/null
+++ b/protos/TerrainPatch.proto
@@ -0,0 +1,29 @@
+#VRML_SIM R2025a utf8
+
+PROTO TerrainPatch [
+  field SFVec3f    translation 0 0 0.006
+  field SFRotation rotation    0 0 1 0
+  field SFString   name        "terrain_patch"
+  field SFVec3f    size        1 1 0.012
+  field SFColor    color       0.12 0.42 0.12
+  field SFFloat    roughness   1
+]
+{
+  Solid {
+    translation IS translation
+    rotation IS rotation
+    name IS name
+    children [
+      Shape {
+        appearance PBRAppearance {
+          baseColor IS color
+          roughness IS roughness
+          metalness 0
+        }
+        geometry DEF PATCH_BOX Box {
+          size IS size
+        }
+      }
+    ]
+  }
+}
diff --git a/resource/open_mower_next_webots_plugins.py b/resource/open_mower_next_webots_plugins.py
new file mode 100644
index 0000000..2a58e66
--- /dev/null
+++ b/resource/open_mower_next_webots_plugins.py
@@ -0,0 +1,345 @@
+import ctypes
+import math
+import random
+from dataclasses import dataclass
+
+import rclpy
+from geometry_msgs.msg import Vector3
+from sensor_msgs.msg import NavSatFix, NavSatStatus
+
+from controller import Field, Node
+from controller.wb import wb
+
+
+wb.wb_supervisor_field_get_mf_node.restype = ctypes.c_void_p
+
+
+@dataclass
+class GnssZone:
+    name: str
+    shape: str
+    x: float
+    y: float
+    yaw: float
+    size_x: float
+    size_y: float
+    radius: float
+    sigma_xy: float
+    bias_x: float
+    bias_y: float
+    dropout_probability: float
+    covariance_xy: float
+    edge_width: float
+    drift_time_constant: float
+
+    def influence_at(self, x: float, y: float) -> float:
+        dx = x - self.x
+        dy = y - self.y
+        if self.shape in ("circle", "cylinder"):
+            radius = self.radius if self.radius > 0.0 else max(self.size_x, self.size_y) * 0.5
+            distance_to_edge = radius - math.hypot(dx, dy)
+            return _edge_influence(distance_to_edge, min(self.edge_width, radius))
+
+        cos_yaw = math.cos(-self.yaw)
+        sin_yaw = math.sin(-self.yaw)
+        local_x = cos_yaw * dx - sin_yaw * dy
+        local_y = sin_yaw * dx + cos_yaw * dy
+        half_x = self.size_x * 0.5
+        half_y = self.size_y * 0.5
+        if half_x <= 0.0 or half_y <= 0.0:
+            return 0.0
+        distance_to_edge = min(half_x - abs(local_x), half_y - abs(local_y))
+        return _edge_influence(distance_to_edge, min(self.edge_width, half_x, half_y))
+
+
+def _smoothstep(value: float) -> float:
+    value = max(0.0, min(1.0, value))
+    return value * value * (3.0 - 2.0 * value)
+
+
+def _edge_influence(distance_to_edge: float, edge_width: float) -> float:
+    if distance_to_edge <= 0.0:
+        return 0.0
+    if edge_width <= 0.0 or distance_to_edge >= edge_width:
+        return 1.0
+    return _smoothstep(distance_to_edge / edge_width)
+
+
+def _get_mf_node(field: Field, index: int):
+    ref = wb.wb_supervisor_field_get_mf_node(field._ref, index)
+    return Node(ref=ref) if ref else None
+
+
+def _field_value(node: Node, name: str, default):
+    field = node.getField(name)
+    if field is None:
+        return default
+    value = field.value
+    return default if value is None else value
+
+
+def _field_float(node: Node, name: str, default: float) -> float:
+    try:
+        return float(_field_value(node, name, default))
+    except (TypeError, ValueError):
+        return default
+
+
+def _field_string(node: Node, name: str, default: str) -> str:
+    value = _field_value(node, name, default)
+    return value if isinstance(value, str) else default
+
+
+def _field_vec3(node: Node, name: str, default):
+    value = _field_value(node, name, default)
+    if isinstance(value, list) and len(value) == 3:
+        return [float(value[0]), float(value[1]), float(value[2])]
+    return default
+
+
+def _field_rotation_yaw(node: Node) -> float:
+    rotation = _field_value(node, "rotation", [0.0, 0.0, 1.0, 0.0])
+    if not isinstance(rotation, list) or len(rotation) != 4:
+        return 0.0
+    axis_z = float(rotation[2])
+    angle = float(rotation[3])
+    return angle if axis_z >= 0.0 else -angle
+
+
+def _clamp_probability(value: float) -> float:
+    return max(0.0, min(1.0, value))
+
+
+def _parse_metadata(text: str) -> dict[str, str]:
+    values = {}
+    for item in text.replace("\n", ";").split(";"):
+        if "=" not in item:
+            continue
+        key, value = item.split("=", 1)
+        key = key.strip()
+        if key:
+            values[key] = value.strip()
+    return values
+
+
+def _metadata_float(metadata: dict[str, str], name: str, default: float) -> float:
+    try:
+        return float(metadata.get(name, default))
+    except (TypeError, ValueError):
+        return default
+
+
+def _stamp_seconds(stamp):
+    try:
+        return float(stamp.sec) + float(stamp.nanosec) * 1e-9
+    except AttributeError:
+        return None
+
+
+class GnssDegradationPlugin:
+    def init(self, webots_node, properties):
+        if not rclpy.ok():
+            rclpy.init(args=None)
+
+        self._robot = webots_node.robot
+        self._node = rclpy.create_node("sim_gnss_degradation_node")
+        self._rng = random.Random(int(properties.get("seed", "7")))
+        self._zones = []
+        self._zone_drift = {}
+        self._logged_supervisor_warning = False
+
+        self._input_fix_topic = properties.get("inputFixTopic", "/sim/gps/fix_raw")
+        self._output_fix_topic = properties.get("outputFixTopic", "/gps/fix")
+        self._input_speed_vector_topic = properties.get(
+            "inputSpeedVectorTopic", f"{self._input_fix_topic}/speed_vector"
+        )
+        self._output_speed_vector_topic = properties.get(
+            "outputSpeedVectorTopic", f"{self._output_fix_topic}/speed_vector"
+        )
+
+        self._fix_publisher = self._node.create_publisher(NavSatFix, self._output_fix_topic, 10)
+        self._speed_vector_publisher = self._node.create_publisher(
+            Vector3, self._output_speed_vector_topic, 10
+        )
+        self._fix_subscription = self._node.create_subscription(
+            NavSatFix, self._input_fix_topic, self._fix_callback, 10
+        )
+        self._speed_vector_subscription = self._node.create_subscription(
+            Vector3, self._input_speed_vector_topic, self._speed_vector_callback, 10
+        )
+
+        self._refresh_zones()
+        self._node.get_logger().info(
+            f"GNSS degradation publishing {self._output_fix_topic} from {self._input_fix_topic}; "
+            f"loaded {len(self._zones)} Webots zone(s)"
+        )
+
+    def step(self):
+        if self._node is not None:
+            rclpy.spin_once(self._node, timeout_sec=0.0)
+
+    def stop(self):
+        if getattr(self, "_node", None) is not None:
+            self._node.destroy_node()
+            self._node = None
+        if rclpy.ok():
+            rclpy.shutdown()
+
+    def _refresh_zones(self):
+        self._zones = []
+        if not self._robot.getSupervisor():
+            if not self._logged_supervisor_warning:
+                self._node.get_logger().warn(
+                    "OpenMower Webots robot is not a supervisor; GNSS degradation zones are ignored"
+                )
+                self._logged_supervisor_warning = True
+            return
+
+        root = self._robot.getRoot()
+        children = root.getField("children")
+        if children is None:
+            return
+
+        for index in range(children.getCount()):
+            child = _get_mf_node(children, index)
+            zone = self._zone_from_node(child) if child is not None else None
+            if zone is not None:
+                self._zones.append(zone)
+
+    def _zone_from_node(self, node):
+        metadata_field = node.getField("metadata")
+        if metadata_field is None:
+            return None
+
+        metadata = _parse_metadata(_field_string(node, "metadata", ""))
+
+        try:
+            position = node.getPosition()
+        except Exception:
+            position = _field_vec3(node, "translation", [0.0, 0.0, 0.0])
+        size = _field_vec3(node, "size", [1.0, 1.0, 0.0])
+        name = _field_string(node, "name", node.getTypeName())
+        shape = metadata.get("shape", "box").lower()
+        sigma_xy = max(0.0, _metadata_float(metadata, "sigmaXY", 0.0))
+        covariance_xy = max(0.0, _metadata_float(metadata, "covarianceXY", sigma_xy * sigma_xy))
+        radius = max(0.0, _metadata_float(metadata, "radius", 0.0))
+        if shape in ("circle", "cylinder"):
+            default_radius = radius if radius > 0.0 else max(size[0], size[1]) * 0.5
+            default_edge_width = max(0.1, 0.35 * default_radius)
+        else:
+            default_edge_width = max(0.1, 0.25 * min(size[0], size[1]))
+
+        return GnssZone(
+            name=name,
+            shape=shape,
+            x=float(position[0]),
+            y=float(position[1]),
+            yaw=_field_rotation_yaw(node),
+            size_x=max(0.0, float(size[0])),
+            size_y=max(0.0, float(size[1])),
+            radius=radius,
+            sigma_xy=sigma_xy,
+            bias_x=_metadata_float(metadata, "biasX", 0.0),
+            bias_y=_metadata_float(metadata, "biasY", 0.0),
+            dropout_probability=_clamp_probability(
+                _metadata_float(metadata, "dropoutProbability", 0.0)
+            ),
+            covariance_xy=covariance_xy,
+            edge_width=max(0.0, _metadata_float(metadata, "edgeWidth", default_edge_width)),
+            drift_time_constant=max(
+                0.1, _metadata_float(metadata, "driftTimeConstant", 8.0)
+            ),
+        )
+
+    def _fix_callback(self, msg: NavSatFix):
+        if not self._zones or not self._robot.getSupervisor():
+            self._fix_publisher.publish(msg)
+            return
+
+        try:
+            robot_position = self._robot.getSelf().getPosition()
+        except Exception:
+            self._fix_publisher.publish(msg)
+            return
+
+        active_zones = [
+            (zone, influence)
+            for zone in self._zones
+            if (influence := zone.influence_at(robot_position[0], robot_position[1])) > 0.0
+        ]
+        if not active_zones:
+            self._fix_publisher.publish(msg)
+            return
+
+        self._fix_publisher.publish(self._degrade_fix(msg, active_zones))
+
+    def _degrade_fix(self, msg: NavSatFix, zones: list[tuple[GnssZone, float]]) -> NavSatFix:
+        out = NavSatFix()
+        out.header = msg.header
+        out.status.status = msg.status.status
+        out.status.service = msg.status.service
+        out.latitude = msg.latitude
+        out.longitude = msg.longitude
+        out.altitude = msg.altitude
+        out.position_covariance = list(msg.position_covariance)
+        out.position_covariance_type = msg.position_covariance_type
+
+        effective_sigma_sq = sum((influence * zone.sigma_xy) ** 2 for zone, influence in zones)
+        covariance_xy = sum(
+            influence
+            * influence
+            * max(zone.covariance_xy, zone.sigma_xy * zone.sigma_xy)
+            for zone, influence in zones
+        )
+        dropout_probability = 1.0
+        for zone, influence in zones:
+            dropout_probability *= 1.0 - zone.dropout_probability * influence * influence
+        dropout_probability = 1.0 - dropout_probability
+
+        stamp_seconds = _stamp_seconds(msg.header.stamp)
+        offset_x = 0.0
+        offset_y = 0.0
+        for zone, influence in zones:
+            drift_x, drift_y = self._drift_for_zone(zone, stamp_seconds)
+            offset_x += influence * (zone.bias_x + drift_x)
+            offset_y += influence * (zone.bias_y + drift_y)
+        jitter_sigma = math.sqrt(effective_sigma_sq) * 0.15
+        if jitter_sigma > 0.0:
+            offset_x += self._rng.gauss(0.0, jitter_sigma)
+            offset_y += self._rng.gauss(0.0, jitter_sigma)
+
+        meters_per_degree_lat = 111320.0
+        meters_per_degree_lon = max(
+            1.0, meters_per_degree_lat * math.cos(math.radians(max(-89.9, min(89.9, msg.latitude))))
+        )
+        out.latitude += offset_y / meters_per_degree_lat
+        out.longitude += offset_x / meters_per_degree_lon
+
+        if self._rng.random() < dropout_probability:
+            out.status.status = NavSatStatus.STATUS_NO_FIX
+
+        out.position_covariance[0] = max(out.position_covariance[0], covariance_xy)
+        out.position_covariance[4] = max(out.position_covariance[4], covariance_xy)
+        out.position_covariance[8] = max(out.position_covariance[8], covariance_xy)
+        out.position_covariance_type = NavSatFix.COVARIANCE_TYPE_KNOWN
+        return out
+
+    def _drift_for_zone(self, zone: GnssZone, stamp_seconds):
+        if zone.sigma_xy <= 0.0:
+            return 0.0, 0.0
+
+        drift_x, drift_y, last_stamp = self._zone_drift.get(zone.name, (0.0, 0.0, None))
+        dt = 0.2
+        if stamp_seconds is not None and last_stamp is not None:
+            dt = max(0.0, min(2.0, stamp_seconds - last_stamp))
+        if dt > 0.0:
+            alpha = math.exp(-dt / zone.drift_time_constant)
+            drift_sigma = zone.sigma_xy * 0.85
+            process_sigma = drift_sigma * math.sqrt(max(0.0, 1.0 - alpha * alpha))
+            drift_x = alpha * drift_x + self._rng.gauss(0.0, process_sigma)
+            drift_y = alpha * drift_y + self._rng.gauss(0.0, process_sigma)
+        self._zone_drift[zone.name] = (drift_x, drift_y, stamp_seconds)
+        return drift_x, drift_y
+
+    def _speed_vector_callback(self, msg: Vector3):
+        self._speed_vector_publisher.publish(msg)
diff --git a/resource/openmower_webots.urdf b/resource/openmower_webots.urdf
index 2e8a07e..16976bc 100644
--- a/resource/openmower_webots.urdf
+++ b/resource/openmower_webots.urdf
@@ -6,18 +6,25 @@
         <enabled>true</enabled>
         <alwaysOn>true</alwaysOn>
         <updateRate>5</updateRate>
-        <topicName>/gps/fix</topicName>
-        <frameName>gps</frameName>
+        <topicName>/sim/gps/fix_raw</topicName>
+        <frameName>gnss_link</frameName>
       </ros>
     </device>
 
+    <plugin type="open_mower_next_webots_plugins.GnssDegradationPlugin">
+      <inputFixTopic>/sim/gps/fix_raw</inputFixTopic>
+      <outputFixTopic>/gps/fix</outputFixTopic>
+      <inputSpeedVectorTopic>/sim/gps/fix_raw/speed_vector</inputSpeedVectorTopic>
+      <outputSpeedVectorTopic>/gps/fix/speed_vector</outputSpeedVectorTopic>
+      <seed>7</seed>
+    </plugin>
+
     <plugin type="webots_ros2_driver::Ros2IMU">
       <enabled>true</enabled>
       <alwaysOn>true</alwaysOn>
       <updateRate>50</updateRate>
       <topicName>/imu/data_raw</topicName>
       <frameName>imu</frameName>
-      <inertialUnitName>inertial_unit</inertialUnitName>
       <gyroName>gyro</gyroName>
       <accelerometerName>accelerometer</accelerometerName>
     </plugin>
diff --git a/scripts/calibrate_robot.py b/scripts/calibrate_robot.py
new file mode 100755
index 0000000..601f085
--- /dev/null
+++ b/scripts/calibrate_robot.py
@@ -0,0 +1,1251 @@
+#!/usr/bin/env python3
+"""Interactive hardware calibration and speed characterization flow."""
+
+import argparse
+import json
+import math
+import os
+import shutil
+import statistics
+import sys
+import time
+from dataclasses import dataclass
+from datetime import datetime, timezone
+from pathlib import Path
+from typing import Any, Callable, Dict, Iterable, List, Optional, Sequence, Tuple, Union
+
+import rclpy
+from geometry_msgs.msg import TwistStamped
+from nav_msgs.msg import Odometry
+from rclpy.node import Node
+from sensor_msgs.msg import Imu, NavSatFix
+
+try:
+    import yaml
+except ImportError:  # pragma: no cover - package.xml declares python3-yaml
+    yaml = None
+
+try:
+    from gps_msgs.msg import GPSFix
+except ImportError:  # pragma: no cover - NavSatFix mode still works
+    GPSFix = None
+
+
+WGS84_A = 6378137.0
+WGS84_F = 1.0 / 298.257223563
+WGS84_E2 = WGS84_F * (2.0 - WGS84_F)
+
+
+@dataclass
+class OdomSample:
+    t: float
+    x: float
+    y: float
+    yaw: float
+    vx: float
+    vy: float
+    wz: float
+
+
+@dataclass
+class ImuSample:
+    t: float
+    wz: float
+
+
+@dataclass
+class GpsFixSample:
+    t: float
+    lat: float
+    lon: float
+    alt: float
+    hacc: Optional[float]
+    status: Optional[int]
+    satellites: Optional[int]
+    source: str
+
+
+@dataclass
+class StageWindow:
+    name: str
+    command_linear: float
+    command_angular: float
+    start: float
+    end: float
+
+
+@dataclass
+class ConfigSnapshot:
+    root: Path
+    controllers: Dict[str, Any]
+    hardware: Dict[str, Any]
+    nav2: Dict[str, Any]
+    fusioncore: Dict[str, Any]
+
+
+def stamp_to_sec(stamp: Any, fallback: Callable[[], float]) -> float:
+    if getattr(stamp, "sec", 0) == 0 and getattr(stamp, "nanosec", 0) == 0:
+        return fallback()
+    return float(stamp.sec) + float(stamp.nanosec) * 1.0e-9
+
+
+def yaw_from_quaternion(q: Any) -> float:
+    siny_cosp = 2.0 * (q.w * q.z + q.x * q.y)
+    cosy_cosp = 1.0 - 2.0 * (q.y * q.y + q.z * q.z)
+    return math.atan2(siny_cosp, cosy_cosp)
+
+
+def angle_diff(current: float, previous: float) -> float:
+    diff = current - previous
+    while diff > math.pi:
+        diff -= 2.0 * math.pi
+    while diff < -math.pi:
+        diff += 2.0 * math.pi
+    return diff
+
+
+def odom_yaw_delta(samples: Sequence[OdomSample]) -> Optional[float]:
+    if len(samples) < 2:
+        return None
+    total = 0.0
+    previous = samples[0].yaw
+    for sample in samples[1:]:
+        total += angle_diff(sample.yaw, previous)
+        previous = sample.yaw
+    return total
+
+
+def integrate_imu_yaw(samples: Sequence[ImuSample]) -> Optional[float]:
+    if len(samples) < 2:
+        return None
+    total = 0.0
+    previous = samples[0]
+    for sample in samples[1:]:
+        dt = sample.t - previous.t
+        if 0.0 < dt <= 1.0:
+            total += 0.5 * (previous.wz + sample.wz) * dt
+        previous = sample
+    return total
+
+
+def wgs84_to_ecef(lat_deg: float, lon_deg: float, alt_m: float) -> Tuple[float, float, float]:
+    lat = math.radians(lat_deg)
+    lon = math.radians(lon_deg)
+    sin_lat = math.sin(lat)
+    cos_lat = math.cos(lat)
+    radius = WGS84_A / math.sqrt(1.0 - WGS84_E2 * sin_lat * sin_lat)
+    x = (radius + alt_m) * cos_lat * math.cos(lon)
+    y = (radius + alt_m) * cos_lat * math.sin(lon)
+    z = (radius * (1.0 - WGS84_E2) + alt_m) * sin_lat
+    return x, y, z
+
+
+def ecef_to_enu(
+    x: float,
+    y: float,
+    z: float,
+    ref_lat_deg: float,
+    ref_lon_deg: float,
+    ref_ecef: Tuple[float, float, float],
+) -> Tuple[float, float, float]:
+    lat = math.radians(ref_lat_deg)
+    lon = math.radians(ref_lon_deg)
+    dx = x - ref_ecef[0]
+    dy = y - ref_ecef[1]
+    dz = z - ref_ecef[2]
+
+    sin_lat = math.sin(lat)
+    cos_lat = math.cos(lat)
+    sin_lon = math.sin(lon)
+    cos_lon = math.cos(lon)
+
+    east = -sin_lon * dx + cos_lon * dy
+    north = -sin_lat * cos_lon * dx - sin_lat * sin_lon * dy + cos_lat * dz
+    up = cos_lat * cos_lon * dx + cos_lat * sin_lon * dy + sin_lat * dz
+    return east, north, up
+
+
+def gps_samples_to_enu(samples: Sequence[GpsFixSample]) -> List[OdomSample]:
+    if not samples:
+        return []
+    ref = samples[0]
+    ref_ecef = wgs84_to_ecef(ref.lat, ref.lon, ref.alt)
+    points: List[OdomSample] = []
+    for sample in samples:
+        ecef = wgs84_to_ecef(sample.lat, sample.lon, sample.alt)
+        east, north, _ = ecef_to_enu(ecef[0], ecef[1], ecef[2], ref.lat, ref.lon, ref_ecef)
+        points.append(OdomSample(sample.t, east, north, 0.0, 0.0, 0.0, 0.0))
+    return points
+
+
+def path_length(samples: Sequence[OdomSample]) -> Optional[float]:
+    if len(samples) < 2:
+        return None
+    total = 0.0
+    previous = samples[0]
+    for sample in samples[1:]:
+        total += math.hypot(sample.x - previous.x, sample.y - previous.y)
+        previous = sample
+    return total
+
+
+def median(values: Iterable[float]) -> Optional[float]:
+    filtered = [float(v) for v in values if math.isfinite(float(v))]
+    if not filtered:
+        return None
+    return float(statistics.median(filtered))
+
+
+def population_stddev(values: Iterable[float]) -> Optional[float]:
+    filtered = [float(v) for v in values if math.isfinite(float(v))]
+    if not filtered:
+        return None
+    if len(filtered) == 1:
+        return 0.0
+    return float(statistics.pstdev(filtered))
+
+
+def percentile(values: Iterable[float], pct: float) -> Optional[float]:
+    filtered = sorted(float(v) for v in values if math.isfinite(float(v)))
+    if not filtered:
+        return None
+    if len(filtered) == 1:
+        return filtered[0]
+    pos = (len(filtered) - 1) * pct
+    lower = int(math.floor(pos))
+    upper = int(math.ceil(pos))
+    if lower == upper:
+        return filtered[lower]
+    weight = pos - lower
+    return filtered[lower] * (1.0 - weight) + filtered[upper] * weight
+
+
+def rounded(value: Optional[float], digits: int = 4) -> Optional[float]:
+    if value is None or not math.isfinite(value):
+        return None
+    return round(float(value), digits)
+
+
+def round_down(value: float, step: float) -> float:
+    if step <= 0.0:
+        return value
+    return math.floor(value / step) * step
+
+
+def clamp(value: float, low: float, high: float) -> float:
+    return min(max(value, low), high)
+
+
+def parse_sweep(value: str) -> List[float]:
+    result: List[float] = []
+    for item in value.split(","):
+        item = item.strip()
+        if item:
+            result.append(float(item))
+    return result
+
+
+Key = Union[int, str]
+
+
+def get_nested(data: Dict[str, Any], keys: Sequence[Key], default: Any = None) -> Any:
+    current: Any = data
+    for key in keys:
+        if isinstance(current, dict) and key in current:
+            current = current[key]
+        elif isinstance(current, list) and isinstance(key, int) and 0 <= key < len(current):
+            current = current[key]
+        else:
+            return default
+    return current
+
+
+def set_nested(data: Dict[str, Any], keys: Sequence[Key], value: Any) -> None:
+    if not keys:
+        raise ValueError("key path must not be empty")
+
+    current: Any = data
+    for index, key in enumerate(keys[:-1]):
+        next_key = keys[index + 1]
+        if isinstance(current, dict):
+            if key not in current or not isinstance(current[key], (dict, list)):
+                current[key] = [] if isinstance(next_key, int) else {}
+            current = current[key]
+        elif isinstance(current, list) and isinstance(key, int):
+            while len(current) <= key:
+                current.append({})
+            if not isinstance(current[key], (dict, list)):
+                current[key] = [] if isinstance(next_key, int) else {}
+            current = current[key]
+        else:
+            raise TypeError(f"cannot descend into {type(current).__name__} with key {key!r}")
+
+    last_key = keys[-1]
+    if isinstance(current, dict):
+        current[last_key] = value
+    elif isinstance(current, list) and isinstance(last_key, int):
+        while len(current) <= last_key:
+            current.append(None)
+        current[last_key] = value
+    else:
+        raise TypeError(f"cannot set key {last_key!r} on {type(current).__name__}")
+
+
+def load_yaml_file(path: Path) -> Dict[str, Any]:
+    if yaml is None or not path.exists():
+        return {}
+    with path.open("r", encoding="utf-8") as stream:
+        loaded = yaml.safe_load(stream)
+    return loaded if isinstance(loaded, dict) else {}
+
+
+def load_config_snapshot(root: Path) -> ConfigSnapshot:
+    return ConfigSnapshot(
+        root=root,
+        controllers=load_yaml_file(root / "controllers.yaml"),
+        hardware=load_yaml_file(root / "hardware" / "yardforce500.yaml"),
+        nav2=load_yaml_file(root / "nav2_params.yaml"),
+        fusioncore=load_yaml_file(root / "fusioncore.yaml"),
+    )
+
+
+def config_value_summary(config: ConfigSnapshot) -> Dict[str, Any]:
+    wheel_offset = get_nested(config.hardware, ["wheel", "offset"])
+    return {
+        "controllers.wheel_radius": get_nested(
+            config.controllers,
+            ["diff_drive_base_controller", "ros__parameters", "wheel_radius"],
+        ),
+        "controllers.wheel_separation": get_nested(
+            config.controllers,
+            ["diff_drive_base_controller", "ros__parameters", "wheel_separation"],
+        ),
+        "hardware.wheel.radius": get_nested(config.hardware, ["wheel", "radius"]),
+        "hardware.wheel.gear_ratio": get_nested(config.hardware, ["wheel", "gear_ratio"]),
+        "hardware.wheel.offset_y": wheel_offset[1]
+        if isinstance(wheel_offset, list) and len(wheel_offset) > 1
+        else None,
+        "hardware.wheel.velocity.max": get_nested(config.hardware, ["wheel", "velocity", "max"]),
+        "nav2.velocity_smoother.max_velocity": get_nested(
+            config.nav2, ["velocity_smoother", "ros__parameters", "max_velocity"]
+        ),
+        "nav2.FollowPath.desired_linear_vel": get_nested(
+            config.nav2,
+            ["controller_server", "ros__parameters", "FollowPath", "desired_linear_vel"],
+        ),
+    }
+
+
+def latest(items: Sequence[Any]) -> Optional[Any]:
+    return items[-1] if items else None
+
+
+def filter_window(samples: Sequence[Any], window: StageWindow, settle: float = 0.0) -> List[Any]:
+    start = window.start + settle
+    return [sample for sample in samples if start <= sample.t <= window.end]
+
+
+class CalibrationNode(Node):
+    def __init__(self, args: argparse.Namespace) -> None:
+        super().__init__("open_mower_next_calibration")
+        self.args = args
+        self.wheel_odom: List[OdomSample] = []
+        self.fusion_odom: List[OdomSample] = []
+        self.gps_odom: List[OdomSample] = []
+        self.imu: List[ImuSample] = []
+        self.gps_fixes: List[GpsFixSample] = []
+
+        self.command_pub = self.create_publisher(TwistStamped, args.cmd_topic, 10)
+        self.create_subscription(Odometry, args.wheel_odom_topic, self._wheel_odom_callback, 50)
+        self.create_subscription(Odometry, args.fusion_odom_topic, self._fusion_odom_callback, 50)
+        self.create_subscription(Odometry, args.gps_odom_topic, self._gps_odom_callback, 20)
+        self.create_subscription(Imu, args.imu_topic, self._imu_callback, 100)
+        self.create_subscription(NavSatFix, args.gps_fix_topic, self._navsat_fix_callback, 20)
+        if GPSFix is not None:
+            self.create_subscription(GPSFix, args.gps_fix_extended_topic, self._gps_fix_callback, 20)
+
+    def now_sec(self) -> float:
+        return self.get_clock().now().nanoseconds * 1.0e-9
+
+    def _message_time(self, msg: Any) -> float:
+        return stamp_to_sec(msg.header.stamp, self.now_sec)
+
+    def _odom_sample(self, msg: Odometry) -> OdomSample:
+        return OdomSample(
+            t=self._message_time(msg),
+            x=msg.pose.pose.position.x,
+            y=msg.pose.pose.position.y,
+            yaw=yaw_from_quaternion(msg.pose.pose.orientation),
+            vx=msg.twist.twist.linear.x,
+            vy=msg.twist.twist.linear.y,
+            wz=msg.twist.twist.angular.z,
+        )
+
+    def _wheel_odom_callback(self, msg: Odometry) -> None:
+        self.wheel_odom.append(self._odom_sample(msg))
+
+    def _fusion_odom_callback(self, msg: Odometry) -> None:
+        self.fusion_odom.append(self._odom_sample(msg))
+
+    def _gps_odom_callback(self, msg: Odometry) -> None:
+        self.gps_odom.append(self._odom_sample(msg))
+
+    def _imu_callback(self, msg: Imu) -> None:
+        self.imu.append(ImuSample(t=self._message_time(msg), wz=msg.angular_velocity.z))
+
+    def _navsat_fix_callback(self, msg: NavSatFix) -> None:
+        hacc = None
+        if msg.position_covariance_type != NavSatFix.COVARIANCE_TYPE_UNKNOWN:
+            cov = msg.position_covariance
+            hacc = math.sqrt(max(cov[0], cov[4], 0.0))
+        self.gps_fixes.append(
+            GpsFixSample(
+                t=self._message_time(msg),
+                lat=msg.latitude,
+                lon=msg.longitude,
+                alt=msg.altitude,
+                hacc=hacc,
+                status=msg.status.status,
+                satellites=None,
+                source="gps/fix",
+            )
+        )
+
+    def _gps_fix_callback(self, msg: Any) -> None:
+        hacc = None
+        if getattr(msg, "err_horz", 0.0) > 0.0:
+            hacc = msg.err_horz / 1.96
+        elif getattr(msg, "position_covariance_type", 0) != 0:
+            cov = msg.position_covariance
+            hacc = math.sqrt(max(cov[0], cov[4], 0.0))
+        self.gps_fixes.append(
+            GpsFixSample(
+                t=self._message_time(msg),
+                lat=msg.latitude,
+                lon=msg.longitude,
+                alt=msg.altitude,
+                hacc=hacc,
+                status=msg.status.status,
+                satellites=getattr(msg.status, "satellites_used", None),
+                source="gps/fix_extended",
+            )
+        )
+
+    def spin_for(self, duration: float) -> None:
+        end_time = time.monotonic() + duration
+        while rclpy.ok() and time.monotonic() < end_time:
+            rclpy.spin_once(self, timeout_sec=0.05)
+
+    def publish_command(self, linear: float, angular: float) -> None:
+        msg = TwistStamped()
+        msg.header.stamp = self.get_clock().now().to_msg()
+        msg.header.frame_id = "base_link"
+        msg.twist.linear.x = float(linear)
+        msg.twist.angular.z = float(angular)
+        self.command_pub.publish(msg)
+
+    def stop_robot(self, duration: float = 0.7) -> None:
+        period = 1.0 / max(self.args.cmd_rate, 1.0)
+        end_time = time.monotonic() + duration
+        while rclpy.ok() and time.monotonic() < end_time:
+            self.publish_command(0.0, 0.0)
+            rclpy.spin_once(self, timeout_sec=min(period, 0.05))
+
+
+def confirm(args: argparse.Namespace, prompt: str) -> bool:
+    if args.yes:
+        return True
+    if not sys.stdin.isatty():
+        print("Refusing to continue without --yes because stdin is not interactive.")
+        return False
+    answer = input(f"{prompt} [y/N] ").strip().lower()
+    return answer in {"y", "yes"}
+
+
+def run_stage(
+    node: CalibrationNode,
+    args: argparse.Namespace,
+    name: str,
+    linear: float,
+    angular: float,
+    duration: float,
+) -> Optional[StageWindow]:
+    print(f"\nStage: {name}")
+    print(f"Command: linear={linear:.3f} m/s angular={angular:.3f} rad/s duration={duration:.1f}s")
+    if args.drive_mode == "auto":
+        if not confirm(args, "Ensure the robot path is clear and start this motion?"):
+            print("Stage skipped.")
+            return None
+    else:
+        if not confirm(args, "Start recording this manually driven stage?"):
+            print("Stage skipped.")
+            return None
+        print("Drive the robot manually now; the script will only record sensor data.")
+
+    node.stop_robot(0.4)
+    node.spin_for(0.1)
+    start = node.now_sec()
+    end_monotonic = time.monotonic() + duration
+    period = 1.0 / max(args.cmd_rate, 1.0)
+    next_publish = time.monotonic()
+
+    try:
+        while rclpy.ok() and time.monotonic() < end_monotonic:
+            now = time.monotonic()
+            if args.drive_mode == "auto" and now >= next_publish:
+                node.publish_command(linear, angular)
+                next_publish = now + period
+            rclpy.spin_once(node, timeout_sec=0.02)
+    finally:
+        if args.drive_mode == "auto":
+            node.stop_robot()
+        node.spin_for(0.2)
+
+    end = node.now_sec()
+    return StageWindow(name=name, command_linear=linear, command_angular=angular, start=start, end=end)
+
+
+def choose_gps_fixes(samples: Sequence[GpsFixSample]) -> List[GpsFixSample]:
+    extended = [sample for sample in samples if sample.source == "gps/fix_extended"]
+    if len(extended) >= 2:
+        return extended
+    return list(samples)
+
+
+def analyze_straight_stage(
+    node: CalibrationNode,
+    args: argparse.Namespace,
+    window: StageWindow,
+) -> Dict[str, Any]:
+    wheel = filter_window(node.wheel_odom, window)
+    fusion = filter_window(node.fusion_odom, window)
+    gps_fixes = choose_gps_fixes(filter_window(node.gps_fixes, window))
+    gps_points = gps_samples_to_enu(gps_fixes)
+
+    wheel_distance = path_length(wheel)
+    gps_distance = path_length(gps_points)
+    fusion_distance = path_length(fusion)
+    scale = None
+    if (
+        wheel_distance is not None
+        and gps_distance is not None
+        and wheel_distance >= args.min_calibration_distance
+        and gps_distance >= args.min_calibration_distance
+    ):
+        scale = gps_distance / wheel_distance
+
+    yaw_delta = odom_yaw_delta(wheel)
+    gps_hacc = median(sample.hacc for sample in gps_fixes if sample.hacc is not None)
+    return {
+        "name": window.name,
+        "command_linear": window.command_linear,
+        "duration": rounded(window.end - window.start, 2),
+        "wheel_distance_m": rounded(wheel_distance),
+        "gps_distance_m": rounded(gps_distance),
+        "fusion_distance_m": rounded(fusion_distance),
+        "effective_wheel_scale": rounded(scale, 6),
+        "wheel_yaw_delta_rad": rounded(yaw_delta),
+        "gps_samples": len(gps_fixes),
+        "wheel_odom_samples": len(wheel),
+        "gps_hacc_m_median": rounded(gps_hacc),
+        "usable": scale is not None,
+    }
+
+
+def analyze_rotation_stage(node: CalibrationNode, args: argparse.Namespace, window: StageWindow) -> Dict[str, Any]:
+    wheel = filter_window(node.wheel_odom, window)
+    fusion = filter_window(node.fusion_odom, window)
+    imu = filter_window(node.imu, window)
+
+    wheel_yaw = odom_yaw_delta(wheel)
+    fusion_yaw = odom_yaw_delta(fusion)
+    imu_yaw = integrate_imu_yaw(imu)
+
+    reference_source = None
+    reference_yaw = None
+    if imu_yaw is not None and abs(imu_yaw) >= args.min_rotation_yaw:
+        reference_source = "imu/angular_velocity.z"
+        reference_yaw = imu_yaw
+    elif fusion_yaw is not None and abs(fusion_yaw) >= args.min_rotation_yaw:
+        reference_source = "fusion/odom"
+        reference_yaw = fusion_yaw
+
+    separation_scale = None
+    if wheel_yaw is not None and reference_yaw is not None and abs(reference_yaw) >= args.min_rotation_yaw:
+        separation_scale = abs(wheel_yaw) / abs(reference_yaw)
+
+    return {
+        "name": window.name,
+        "command_angular": window.command_angular,
+        "duration": rounded(window.end - window.start, 2),
+        "wheel_yaw_delta_rad": rounded(wheel_yaw),
+        "imu_yaw_delta_rad": rounded(imu_yaw),
+        "fusion_yaw_delta_rad": rounded(fusion_yaw),
+        "reference_source": reference_source,
+        "wheel_separation_scale": rounded(separation_scale, 6),
+        "wheel_odom_samples": len(wheel),
+        "imu_samples": len(imu),
+        "usable": separation_scale is not None,
+    }
+
+
+def speed_values_from_odom(samples: Sequence[OdomSample], angular: bool) -> List[float]:
+    if angular:
+        return [abs(sample.wz) for sample in samples]
+    return [abs(sample.vx) for sample in samples]
+
+
+def speed_values_from_gps_odom(samples: Sequence[OdomSample]) -> List[float]:
+    return [math.hypot(sample.vx, sample.vy) for sample in samples]
+
+
+def estimate_path_speed(samples: Sequence[OdomSample], window: StageWindow, settle: float) -> Optional[float]:
+    distance = path_length(samples)
+    duration = max(window.end - window.start - settle, 0.0)
+    if distance is None or duration <= 0.0:
+        return None
+    return distance / duration
+
+
+def estimate_rise_time(values: Sequence[Tuple[float, float]], start: float, target: float) -> Optional[float]:
+    if target is None or target <= 0.0:
+        return None
+    threshold = target * 0.9
+    for t, value in values:
+        if t >= start and abs(value) >= threshold:
+            return max(t - start, 0.0)
+    return None
+
+
+def select_linear_speed_source(
+    node: CalibrationNode,
+    window: StageWindow,
+    settle: float,
+) -> Tuple[str, List[float], Optional[float]]:
+    gps_odom = filter_window(node.gps_odom, window, settle)
+    if len(gps_odom) >= 3:
+        values = speed_values_from_gps_odom(gps_odom)
+        rise = estimate_rise_time([(s.t, math.hypot(s.vx, s.vy)) for s in gps_odom], window.start, median(values) or 0.0)
+        return "gps/odom", values, rise
+
+    gps_fixes = gps_samples_to_enu(choose_gps_fixes(filter_window(node.gps_fixes, window, settle)))
+    gps_path_speed = estimate_path_speed(gps_fixes, window, settle)
+    if gps_path_speed is not None:
+        return "gps/fix_path", [gps_path_speed], None
+
+    fusion = filter_window(node.fusion_odom, window, settle)
+    if len(fusion) >= 3:
+        values = speed_values_from_odom(fusion, angular=False)
+        rise = estimate_rise_time([(s.t, s.vx) for s in fusion], window.start, median(values) or 0.0)
+        return "fusion/odom", values, rise
+
+    wheel = filter_window(node.wheel_odom, window, settle)
+    values = speed_values_from_odom(wheel, angular=False)
+    rise = estimate_rise_time([(s.t, s.vx) for s in wheel], window.start, median(values) or 0.0)
+    return "diff_drive_base_controller/odom", values, rise
+
+
+def select_angular_speed_source(
+    node: CalibrationNode,
+    window: StageWindow,
+    settle: float,
+) -> Tuple[str, List[float], Optional[float]]:
+    imu = filter_window(node.imu, window, settle)
+    if len(imu) >= 3:
+        values = [abs(sample.wz) for sample in imu]
+        rise = estimate_rise_time([(s.t, s.wz) for s in imu], window.start, median(values) or 0.0)
+        return "imu/angular_velocity.z", values, rise
+
+    fusion = filter_window(node.fusion_odom, window, settle)
+    if len(fusion) >= 3:
+        values = speed_values_from_odom(fusion, angular=True)
+        rise = estimate_rise_time([(s.t, s.wz) for s in fusion], window.start, median(values) or 0.0)
+        return "fusion/odom", values, rise
+
+    wheel = filter_window(node.wheel_odom, window, settle)
+    values = speed_values_from_odom(wheel, angular=True)
+    rise = estimate_rise_time([(s.t, s.wz) for s in wheel], window.start, median(values) or 0.0)
+    return "diff_drive_base_controller/odom", values, rise
+
+
+def analyze_speed_stage(
+    node: CalibrationNode,
+    args: argparse.Namespace,
+    window: StageWindow,
+    angular: bool,
+) -> Dict[str, Any]:
+    if angular:
+        source, values, rise_time = select_angular_speed_source(node, window, args.speed_settle_time)
+        command = abs(window.command_angular)
+    else:
+        source, values, rise_time = select_linear_speed_source(node, window, args.speed_settle_time)
+        command = abs(window.command_linear)
+
+    actual = median(values)
+    spread = population_stddev(values)
+    tracking_ratio = actual / command if actual is not None and command > 0.0 else None
+    stable = False
+    if actual is not None and spread is not None and tracking_ratio is not None:
+        max_spread = max(args.max_speed_stddev, args.max_relative_speed_stddev * actual)
+        stable = tracking_ratio >= args.min_tracking_ratio and spread <= max_spread
+
+    accel = actual / rise_time if actual is not None and rise_time not in (None, 0.0) else None
+    return {
+        "name": window.name,
+        "command": rounded(command),
+        "actual_median": rounded(actual),
+        "actual_p10": rounded(percentile(values, 0.10)),
+        "actual_p90": rounded(percentile(values, 0.90)),
+        "stddev": rounded(spread),
+        "tracking_ratio": rounded(tracking_ratio),
+        "rise_time_s": rounded(rise_time),
+        "observed_accel": rounded(accel),
+        "source": source,
+        "samples": len(values),
+        "stable": stable,
+    }
+
+
+def config_limit_warnings(config: ConfigSnapshot) -> List[str]:
+    warnings: List[str] = []
+    summary = config_value_summary(config)
+    radius = summary["controllers.wheel_radius"] or summary["hardware.wheel.radius"]
+    wheel_joint_max = summary["hardware.wheel.velocity.max"]
+    nav_max = summary["nav2.velocity_smoother.max_velocity"]
+    if isinstance(nav_max, list) and len(nav_max) >= 3 and radius and wheel_joint_max:
+        required_wheel_speed = abs(float(nav_max[0])) / float(radius)
+        if required_wheel_speed > abs(float(wheel_joint_max)) * 1.05:
+            warnings.append(
+                "Nav2 max linear speed requires %.2f rad/s wheel speed, but "
+                "hardware wheel.velocity.max is %.2f. Verify whether that YAML value is "
+                "really rad/s; its comment currently says m/s."
+                % (required_wheel_speed, float(wheel_joint_max))
+            )
+        required_turn_wheel_speed = abs(float(nav_max[2])) * float(summary["controllers.wheel_separation"] or 0.0) / (
+            2.0 * float(radius)
+        )
+        if required_turn_wheel_speed > abs(float(wheel_joint_max)) * 1.05:
+            warnings.append(
+                "Nav2 max angular speed requires %.2f rad/s wheel speed, but "
+                "hardware wheel.velocity.max is %.2f."
+                % (required_turn_wheel_speed, float(wheel_joint_max))
+            )
+    return warnings
+
+
+def run_preflight(node: CalibrationNode, args: argparse.Namespace, config: ConfigSnapshot) -> Dict[str, Any]:
+    print("Collecting preflight samples...")
+    node.spin_for(args.preflight_seconds)
+    topics = {name: types for name, types in node.get_topic_names_and_types()}
+    gps = latest(node.gps_fixes)
+    message_counts = {
+        "wheel_odom": len(node.wheel_odom),
+        "fusion_odom": len(node.fusion_odom),
+        "gps_odom": len(node.gps_odom),
+        "gps_fix": len(node.gps_fixes),
+        "imu": len(node.imu),
+    }
+    warnings = config_limit_warnings(config)
+    if len(node.wheel_odom) == 0:
+        warnings.append(f"No wheel odometry received on {args.wheel_odom_topic}.")
+    if len(node.imu) == 0:
+        warnings.append(f"No IMU received on {args.imu_topic}; rotation calibration will be weaker.")
+    if gps is None:
+        warnings.append("No GPS fix received; straight distance calibration will not be usable.")
+    elif gps.hacc is not None and gps.hacc > args.max_gps_hacc:
+        warnings.append(
+            "Latest GPS horizontal accuracy %.2f m is above %.2f m; RTK-quality "
+            "speed and geometry estimates may be poor." % (gps.hacc, args.max_gps_hacc)
+        )
+    if args.cmd_topic not in topics:
+        warnings.append(
+            f"Command topic {args.cmd_topic} is not visible yet. This is normal before "
+            "the script publishes, but twist_mux must have a matching input."
+        )
+
+    report = {
+        "message_counts": message_counts,
+        "latest_gps": {
+            "source": gps.source if gps else None,
+            "status": gps.status if gps else None,
+            "satellites": gps.satellites if gps else None,
+            "hacc_m": rounded(gps.hacc) if gps else None,
+        },
+        "config": config_value_summary(config),
+        "warnings": warnings,
+    }
+
+    print(json.dumps(report, indent=2))
+    if warnings and not args.yes:
+        if not confirm(args, "Preflight has warnings. Continue anyway?"):
+            raise RuntimeError("Calibration aborted by user after preflight warnings.")
+    return report
+
+
+def run_geometry(node: CalibrationNode, args: argparse.Namespace) -> Dict[str, Any]:
+    result: Dict[str, Any] = {"straight": [], "rotation": []}
+    forward = run_stage(
+        node,
+        args,
+        "straight_forward",
+        args.geometry_linear_speed,
+        0.0,
+        args.geometry_linear_duration,
+    )
+    if forward is not None:
+        result["straight"].append(analyze_straight_stage(node, args, forward))
+
+    if args.include_reverse:
+        reverse = run_stage(
+            node,
+            args,
+            "straight_reverse",
+            -args.geometry_linear_speed,
+            0.0,
+            args.geometry_linear_duration,
+        )
+        if reverse is not None:
+            result["straight"].append(analyze_straight_stage(node, args, reverse))
+
+    left = run_stage(
+        node,
+        args,
+        "rotate_ccw",
+        0.0,
+        args.geometry_angular_speed,
+        args.geometry_angular_duration,
+    )
+    if left is not None:
+        result["rotation"].append(analyze_rotation_stage(node, args, left))
+
+    right = run_stage(
+        node,
+        args,
+        "rotate_cw",
+        0.0,
+        -args.geometry_angular_speed,
+        args.geometry_angular_duration,
+    )
+    if right is not None:
+        result["rotation"].append(analyze_rotation_stage(node, args, right))
+    return result
+
+
+def run_speed_sweep(node: CalibrationNode, args: argparse.Namespace) -> Dict[str, Any]:
+    result: Dict[str, Any] = {"linear": [], "angular": []}
+
+    seen_stable = False
+    unstable_after_stable = 0
+    for speed in parse_sweep(args.linear_sweep):
+        window = run_stage(node, args, f"linear_speed_{speed:.2f}", speed, 0.0, args.speed_step_duration)
+        if window is None:
+            continue
+        analysis = analyze_speed_stage(node, args, window, angular=False)
+        result["linear"].append(analysis)
+        print(json.dumps(analysis, indent=2))
+        if analysis["stable"]:
+            seen_stable = True
+            unstable_after_stable = 0
+        elif seen_stable:
+            unstable_after_stable += 1
+        if unstable_after_stable >= args.stop_after_unstable:
+            print("Stopping linear sweep after repeated unstable steps.")
+            break
+
+    seen_stable = False
+    unstable_after_stable = 0
+    for speed in parse_sweep(args.angular_sweep):
+        window = run_stage(node, args, f"angular_speed_{speed:.2f}", 0.0, speed, args.speed_step_duration)
+        if window is None:
+            continue
+        analysis = analyze_speed_stage(node, args, window, angular=True)
+        result["angular"].append(analysis)
+        print(json.dumps(analysis, indent=2))
+        if analysis["stable"]:
+            seen_stable = True
+            unstable_after_stable = 0
+        elif seen_stable:
+            unstable_after_stable += 1
+        if unstable_after_stable >= args.stop_after_unstable:
+            print("Stopping angular sweep after repeated unstable steps.")
+            break
+
+    return result
+
+
+def stable_speed_summary(entries: Sequence[Dict[str, Any]]) -> Dict[str, Any]:
+    stable = [entry for entry in entries if entry.get("stable") and entry.get("actual_median")]
+    observed = [entry for entry in entries if entry.get("actual_median")]
+    return {
+        "max_observed": max((entry["actual_median"] for entry in observed), default=None),
+        "max_stable": max((entry["actual_median"] for entry in stable), default=None),
+        "min_stable": min((entry["actual_median"] for entry in stable), default=None),
+        "observed_accel_median": median(
+            entry["observed_accel"] for entry in stable if entry.get("observed_accel")
+        ),
+    }
+
+
+def make_recommendation(
+    file_path: str,
+    key_path: Sequence[Key],
+    current: Any,
+    recommended: Any,
+    reason: str,
+) -> Dict[str, Any]:
+    return {
+        "file": file_path,
+        "key_path": list(key_path),
+        "current": current,
+        "recommended": recommended,
+        "reason": reason,
+    }
+
+
+def build_recommendations(
+    args: argparse.Namespace,
+    config: ConfigSnapshot,
+    geometry: Optional[Dict[str, Any]],
+    speed: Optional[Dict[str, Any]],
+) -> Dict[str, Any]:
+    recommendations: List[Dict[str, Any]] = []
+    summary = config_value_summary(config)
+    current_radius = summary["controllers.wheel_radius"] or summary["hardware.wheel.radius"]
+    current_separation = None
+    if summary["hardware.wheel.offset_y"] is not None:
+        current_separation = 2.0 * abs(float(summary["hardware.wheel.offset_y"]))
+    elif summary["controllers.wheel_separation"] is not None:
+        current_separation = summary["controllers.wheel_separation"]
+    current_gear_ratio = summary["hardware.wheel.gear_ratio"]
+
+    if geometry:
+        scales = [stage["effective_wheel_scale"] for stage in geometry["straight"] if stage.get("usable")]
+        scale = median(scales)
+        if scale is not None and 0.5 <= scale <= 1.5:
+            if current_radius:
+                radius = round(float(current_radius) * scale, 5)
+                recommendations.append(
+                    make_recommendation(
+                        "controllers.yaml",
+                        ["diff_drive_base_controller", "ros__parameters", "wheel_radius"],
+                        current_radius,
+                        radius,
+                        "GPS distance divided by wheel odometry distance from straight calibration runs.",
+                    )
+                )
+                recommendations.append(
+                    make_recommendation(
+                        "hardware/yardforce500.yaml",
+                        ["wheel", "radius"],
+                        summary["hardware.wheel.radius"],
+                        radius,
+                        "Keep URDF wheel radius aligned with diff_drive_controller.",
+                    )
+                )
+            if current_gear_ratio:
+                gear_ratio = round(float(current_gear_ratio) * scale, 9)
+                recommendations.append(
+                    make_recommendation(
+                        "hardware/yardforce500.yaml",
+                        ["wheel", "gear_ratio"],
+                        current_gear_ratio,
+                        gear_ratio,
+                        "Alternative to changing wheel radius if physical wheel radius is trusted.",
+                    )
+                )
+
+        sep_scales = [stage["wheel_separation_scale"] for stage in geometry["rotation"] if stage.get("usable")]
+        sep_scale = median(sep_scales)
+        if sep_scale is not None and current_separation and 0.5 <= sep_scale <= 1.5:
+            separation = round(float(current_separation) * sep_scale, 5)
+            offset_y = round(separation / 2.0, 5)
+            recommendations.append(
+                make_recommendation(
+                    "hardware/yardforce500.yaml",
+                    ["wheel", "offset", 1],
+                    summary["hardware.wheel.offset_y"],
+                    offset_y,
+                    "Wheel separation is derived as 2 * abs(wheel.offset[1]) in launch files.",
+                )
+            )
+
+    if speed:
+        linear = stable_speed_summary(speed.get("linear", []))
+        angular = stable_speed_summary(speed.get("angular", []))
+        linear_max = linear.get("max_stable")
+        angular_max = angular.get("max_stable")
+        if linear_max and angular_max:
+            safe_linear = round_down(linear_max * args.nav_max_factor, 0.01)
+            safe_angular = round_down(angular_max * args.nav_max_factor, 0.01)
+            desired_linear = round_down(linear_max * args.nav_desired_factor, 0.01)
+            desired_angular = round_down(angular_max * args.nav_desired_factor, 0.01)
+            min_linear = round(max(linear.get("min_stable") or 0.03, 0.03), 2)
+            min_angular = round(max(angular.get("min_stable") or 0.10, 0.10), 2)
+            accel = linear.get("observed_accel_median")
+            angular_accel = angular.get("observed_accel_median")
+            recommended_accel = round(clamp((accel or 0.5) * 0.7, 0.1, 2.5), 2)
+            recommended_angular_accel = round(clamp((angular_accel or 1.0) * 0.7, 0.2, 3.2), 2)
+
+            recommendations.append(
+                make_recommendation(
+                    "nav2_params.yaml",
+                    ["velocity_smoother", "ros__parameters", "max_velocity"],
+                    get_nested(config.nav2, ["velocity_smoother", "ros__parameters", "max_velocity"]),
+                    [round(safe_linear, 2), 0.0, round(safe_angular, 2)],
+                    "85% of highest stable measured linear and angular speed.",
+                )
+            )
+            recommendations.append(
+                make_recommendation(
+                    "nav2_params.yaml",
+                    ["velocity_smoother", "ros__parameters", "min_velocity"],
+                    get_nested(config.nav2, ["velocity_smoother", "ros__parameters", "min_velocity"]),
+                    [round(-safe_linear, 2), 0.0, round(-safe_angular, 2)],
+                    "Symmetric reverse limits matching calibrated safe max speeds.",
+                )
+            )
+            recommendations.append(
+                make_recommendation(
+                    "nav2_params.yaml",
+                    ["velocity_smoother", "ros__parameters", "max_accel"],
+                    get_nested(config.nav2, ["velocity_smoother", "ros__parameters", "max_accel"]),
+                    [recommended_accel, 0.0, recommended_angular_accel],
+                    "70% of observed step-response acceleration, capped conservatively.",
+                )
+            )
+            recommendations.append(
+                make_recommendation(
+                    "nav2_params.yaml",
+                    ["velocity_smoother", "ros__parameters", "max_decel"],
+                    get_nested(config.nav2, ["velocity_smoother", "ros__parameters", "max_decel"]),
+                    [-recommended_accel, 0.0, -recommended_angular_accel],
+                    "Symmetric deceleration limits from measured acceleration.",
+                )
+            )
+            recommendations.append(
+                make_recommendation(
+                    "nav2_params.yaml",
+                    ["controller_server", "ros__parameters", "FollowPath", "desired_linear_vel"],
+                    get_nested(
+                        config.nav2,
+                        ["controller_server", "ros__parameters", "FollowPath", "desired_linear_vel"],
+                    ),
+                    round(desired_linear, 2),
+                    "70% of highest stable measured linear speed for nominal path following.",
+                )
+            )
+            recommendations.append(
+                make_recommendation(
+                    "nav2_params.yaml",
+                    ["controller_server", "ros__parameters", "FollowPath", "regulated_linear_scaling_min_speed"],
+                    get_nested(
+                        config.nav2,
+                        [
+                            "controller_server",
+                            "ros__parameters",
+                            "FollowPath",
+                            "regulated_linear_scaling_min_speed",
+                        ],
+                    ),
+                    round(max(min_linear, min(desired_linear * 0.5, desired_linear)), 2),
+                    "Do not let RPP regulate below the measured repeatable low-speed range.",
+                )
+            )
+            recommendations.append(
+                make_recommendation(
+                    "nav2_params.yaml",
+                    ["controller_server", "ros__parameters", "FollowPath", "rotate_to_heading_angular_vel"],
+                    get_nested(
+                        config.nav2,
+                        ["controller_server", "ros__parameters", "FollowPath", "rotate_to_heading_angular_vel"],
+                    ),
+                    round(desired_angular, 2),
+                    "70% of highest stable measured angular speed for heading alignment.",
+                )
+            )
+            recommendations.append(
+                make_recommendation(
+                    "nav2_params.yaml",
+                    ["behavior_server", "ros__parameters", "max_rotational_vel"],
+                    get_nested(config.nav2, ["behavior_server", "ros__parameters", "max_rotational_vel"]),
+                    round(safe_angular, 2),
+                    "Safe angular speed limit for Nav2 behavior plugins.",
+                )
+            )
+            recommendations.append(
+                make_recommendation(
+                    "nav2_params.yaml",
+                    ["behavior_server", "ros__parameters", "min_rotational_vel"],
+                    get_nested(config.nav2, ["behavior_server", "ros__parameters", "min_rotational_vel"]),
+                    min_angular,
+                    "Lowest repeatable measured angular speed.",
+                )
+            )
+            docking_speed = round(clamp(min(desired_linear * 0.5, 0.15), min_linear, safe_linear), 2)
+            recommendations.append(
+                make_recommendation(
+                    "nav2_params.yaml",
+                    ["docking_server", "ros__parameters", "controller", "v_linear_min"],
+                    get_nested(config.nav2, ["docking_server", "ros__parameters", "controller", "v_linear_min"]),
+                    docking_speed,
+                    "Conservative stable low speed for docking approach.",
+                )
+            )
+            recommendations.append(
+                make_recommendation(
+                    "nav2_params.yaml",
+                    ["docking_server", "ros__parameters", "controller", "v_linear_max"],
+                    get_nested(config.nav2, ["docking_server", "ros__parameters", "controller", "v_linear_max"]),
+                    docking_speed,
+                    "Keep docking speed fixed until docking controller is separately tuned.",
+                )
+            )
+
+    return {"recommendations": recommendations}
+
+
+def apply_recommendations(
+    config_root: Path,
+    recommendations: Sequence[Dict[str, Any]],
+    timestamp: str,
+) -> List[str]:
+    if yaml is None:
+        raise RuntimeError("Cannot apply recommendations because python3-yaml is unavailable.")
+
+    changed: Dict[Path, Dict[str, Any]] = {}
+    for rec in recommendations:
+        path = config_root / rec["file"]
+        if path not in changed:
+            changed[path] = load_yaml_file(path)
+        set_nested(changed[path], rec["key_path"], rec["recommended"])
+
+    written: List[str] = []
+    for path, data in changed.items():
+        if not path.exists():
+            raise RuntimeError(f"Cannot apply recommendation; {path} does not exist.")
+        backup = path.with_suffix(path.suffix + f".bak.{timestamp}")
+        shutil.copy2(path, backup)
+        with path.open("w", encoding="utf-8") as stream:
+            yaml.safe_dump(data, stream, sort_keys=False)
+        written.append(str(path))
+    return written
+
+
+def save_report(report: Dict[str, Any], args: argparse.Namespace, timestamp: str) -> Tuple[Path, Optional[Path]]:
+    output_dir = Path(args.output_dir)
+    output_dir.mkdir(parents=True, exist_ok=True)
+    json_path = output_dir / f"calibration_{timestamp}.json"
+    with json_path.open("w", encoding="utf-8") as stream:
+        json.dump(report, stream, indent=2, sort_keys=True)
+
+    yaml_path = None
+    if yaml is not None:
+        yaml_path = output_dir / f"calibration_{timestamp}.yaml"
+        with yaml_path.open("w", encoding="utf-8") as stream:
+            yaml.safe_dump(report, stream, sort_keys=False)
+    return json_path, yaml_path
+
+
+def parse_args(argv: Optional[Sequence[str]] = None) -> argparse.Namespace:
+    parser = argparse.ArgumentParser(
+        description="Run OpenMowerNext hardware calibration and speed characterization."
+    )
+    parser.add_argument("--mode", choices=["full", "geometry", "speed", "preflight"], default="full")
+    parser.add_argument("--drive-mode", choices=["auto", "manual"], default="auto")
+    parser.add_argument("--yes", action="store_true", help="Do not prompt before each motion stage.")
+    parser.add_argument("--apply", action="store_true", help="Apply recommended YAML values after confirmation.")
+    parser.add_argument("--config-root", default="config", help="Directory containing project config YAML files.")
+    parser.add_argument("--output-dir", default="log/calibration")
+    parser.add_argument("--cmd-topic", default="/cmd_vel_calibration")
+    parser.add_argument("--wheel-odom-topic", default="/diff_drive_base_controller/odom")
+    parser.add_argument("--fusion-odom-topic", default="/fusion/odom")
+    parser.add_argument("--gps-odom-topic", default="/gps/odom")
+    parser.add_argument("--gps-fix-topic", default="/gps/fix")
+    parser.add_argument("--gps-fix-extended-topic", default="/gps/fix_extended")
+    parser.add_argument("--imu-topic", default="/imu/data_raw")
+    parser.add_argument("--cmd-rate", type=float, default=20.0)
+    parser.add_argument("--preflight-seconds", type=float, default=3.0)
+    parser.add_argument("--max-gps-hacc", type=float, default=0.5)
+    parser.add_argument("--geometry-linear-speed", type=float, default=0.15)
+    parser.add_argument("--geometry-linear-duration", type=float, default=10.0)
+    parser.add_argument("--geometry-angular-speed", type=float, default=0.5)
+    parser.add_argument("--geometry-angular-duration", type=float, default=8.0)
+    parser.add_argument("--include-reverse", action="store_true")
+    parser.add_argument("--min-calibration-distance", type=float, default=1.0)
+    parser.add_argument("--min-rotation-yaw", type=float, default=1.0)
+    parser.add_argument("--linear-sweep", default="0.05,0.10,0.15,0.20,0.26,0.32,0.40,0.50")
+    parser.add_argument("--angular-sweep", default="0.20,0.40,0.60,0.80,1.00,1.20")
+    parser.add_argument("--speed-step-duration", type=float, default=4.0)
+    parser.add_argument("--speed-settle-time", type=float, default=1.0)
+    parser.add_argument("--min-tracking-ratio", type=float, default=0.75)
+    parser.add_argument("--max-speed-stddev", type=float, default=0.03)
+    parser.add_argument("--max-relative-speed-stddev", type=float, default=0.25)
+    parser.add_argument("--stop-after-unstable", type=int, default=2)
+    parser.add_argument("--nav-max-factor", type=float, default=0.85)
+    parser.add_argument("--nav-desired-factor", type=float, default=0.70)
+    return parser.parse_args(argv)
+
+
+def main(argv: Optional[Sequence[str]] = None) -> int:
+    args = parse_args(argv)
+    timestamp = datetime.now(timezone.utc).strftime("%Y%m%dT%H%M%SZ")
+    config = load_config_snapshot(Path(args.config_root))
+
+    rclpy.init(args=None)
+    node = CalibrationNode(args)
+    report: Dict[str, Any] = {
+        "timestamp_utc": timestamp,
+        "mode": args.mode,
+        "drive_mode": args.drive_mode,
+        "arguments": vars(args),
+    }
+
+    try:
+        if args.drive_mode == "auto" and args.mode != "preflight":
+            print("This flow sends motion commands to real hardware.")
+            print("Use joystick/emergency stop as the final safety override.")
+            if not confirm(args, "Continue with hardware motion calibration?"):
+                return 1
+
+        report["preflight"] = run_preflight(node, args, config)
+
+        geometry = None
+        speed = None
+        if args.mode in {"full", "geometry"}:
+            geometry = run_geometry(node, args)
+            report["geometry"] = geometry
+        if args.mode in {"full", "speed"}:
+            speed = run_speed_sweep(node, args)
+            report["speed"] = speed
+
+        report["recommendations"] = build_recommendations(args, config, geometry, speed)
+        json_path, yaml_path = save_report(report, args, timestamp)
+        print(f"\nCalibration report written to {json_path}")
+        if yaml_path is not None:
+            print(f"Calibration YAML written to {yaml_path}")
+        print(json.dumps(report["recommendations"], indent=2))
+
+        recs = report["recommendations"].get("recommendations", [])
+        if args.apply and recs:
+            if confirm(args, f"Apply {len(recs)} recommendations to {args.config_root}?"):
+                written = apply_recommendations(Path(args.config_root), recs, timestamp)
+                print("Updated config files:")
+                for path in written:
+                    print(f"  {path}")
+        return 0
+    except KeyboardInterrupt:
+        print("\nCalibration interrupted; stopping robot.")
+        return 130
+    except Exception as exc:
+        print(f"Calibration failed: {exc}", file=sys.stderr)
+        return 1
+    finally:
+        try:
+            node.stop_robot()
+        finally:
+            node.destroy_node()
+            rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    sys.exit(main())
diff --git a/scripts/webots_world_to_geojson.py b/scripts/webots_world_to_geojson.py
new file mode 100755
index 0000000..33bd123
--- /dev/null
+++ b/scripts/webots_world_to_geojson.py
@@ -0,0 +1,570 @@
+#!/usr/bin/env python3
+"""Convert supported OpenMower Webots worlds into OpenMower GeoJSON maps."""
+
+from __future__ import annotations
+
+import argparse
+import ast
+import json
+import math
+import re
+import sys
+import uuid
+from dataclasses import dataclass
+from pathlib import Path
+from typing import Iterable, Sequence
+
+
+WGS84_A = 6378137.0
+WGS84_F = 1.0 / 298.257223563
+WGS84_E2 = WGS84_F * (2.0 - WGS84_F)
+
+TREE_TRUNK_RADIUS = 0.12
+TREE_TRUNK_SEGMENTS = 16
+DOCK_CONTACT_OFFSET_X = 0.32
+DOCK_ORIENTATION_LENGTH = 0.5
+FEATURE_NAMESPACE = uuid.UUID("2e102a30-82dc-4338-8400-edfef8ef00f3")
+
+AREA_COLORS = {
+    "navigation": "#0000ff",
+    "operation": "#00ff00",
+    "exclusion": "#ff0000",
+}
+
+
+@dataclass(frozen=True)
+class WebotsNode:
+    type: str
+    fields: dict[str, object]
+    tokens: list[str]
+
+
+@dataclass(frozen=True)
+class WebotsWorld:
+    name: str
+    datum: tuple[float, float, float]
+    nodes: list[WebotsNode]
+
+
+@dataclass(frozen=True)
+class LocalFeature:
+    feature_type: str
+    name: str
+    geometry_type: str
+    coordinates: list[tuple[float, float]]
+
+
+def strip_comments(text: str) -> str:
+    output: list[str] = []
+    in_string = False
+    escaped = False
+    i = 0
+    while i < len(text):
+        char = text[i]
+        if in_string:
+            output.append(char)
+            if escaped:
+                escaped = False
+            elif char == "\\":
+                escaped = True
+            elif char == '"':
+                in_string = False
+            i += 1
+            continue
+
+        if char == '"':
+            in_string = True
+            output.append(char)
+            i += 1
+            continue
+
+        if char == "#":
+            while i < len(text) and text[i] != "\n":
+                i += 1
+            continue
+
+        output.append(char)
+        i += 1
+    return "".join(output)
+
+
+TOKEN_RE = re.compile(
+    r'"(?:\\.|[^"\\])*"'
+    r"|[{}\[\]]"
+    r"|[+-]?(?:\d+\.\d*|\.\d+|\d+)(?:[eE][+-]?\d+)?"
+    r"|[A-Za-z_][A-Za-z0-9_]*"
+)
+
+
+def tokenize(text: str) -> list[str]:
+    return TOKEN_RE.findall(strip_comments(text))
+
+
+def is_number(token: str) -> bool:
+    try:
+        float(token)
+    except ValueError:
+        return False
+    return True
+
+
+def is_identifier(token: str) -> bool:
+    return bool(re.fullmatch(r"[A-Za-z_][A-Za-z0-9_]*", token))
+
+
+def parse_string(token: str) -> str:
+    return ast.literal_eval(token)
+
+
+def iter_direct_token_positions(tokens: Sequence[str]) -> Iterable[tuple[int, str]]:
+    brace_depth = 0
+    bracket_depth = 0
+    for index, token in enumerate(tokens):
+        if token == "{":
+            brace_depth += 1
+            continue
+        if token == "}":
+            brace_depth -= 1
+            continue
+        if token == "[":
+            bracket_depth += 1
+            continue
+        if token == "]":
+            bracket_depth -= 1
+            continue
+        if brace_depth == 0 and bracket_depth == 0:
+            yield index, token
+
+
+def direct_numbers(tokens: Sequence[str], field: str, count: int) -> tuple[float, ...] | None:
+    direct_positions = dict(iter_direct_token_positions(tokens))
+    for index, token in direct_positions.items():
+        if token != field:
+            continue
+        values: list[float] = []
+        cursor = index + 1
+        while cursor < len(tokens) and len(values) < count:
+            if cursor in direct_positions and is_identifier(tokens[cursor]) and values:
+                break
+            if tokens[cursor] in "{}[]":
+                break
+            if is_number(tokens[cursor]):
+                values.append(float(tokens[cursor]))
+            elif values:
+                break
+            cursor += 1
+        if len(values) == count:
+            return tuple(values)
+    return None
+
+
+def direct_string(tokens: Sequence[str], field: str) -> str | None:
+    direct_positions = dict(iter_direct_token_positions(tokens))
+    for index, token in direct_positions.items():
+        if token != field:
+            continue
+        cursor = index + 1
+        while cursor < len(tokens):
+            if cursor in direct_positions and is_identifier(tokens[cursor]):
+                return None
+            if tokens[cursor].startswith('"'):
+                return parse_string(tokens[cursor])
+            if tokens[cursor] in "{}[]":
+                return None
+            cursor += 1
+    return None
+
+
+def collect_braced_block(tokens: Sequence[str], open_brace_index: int) -> tuple[list[str], int]:
+    depth = 1
+    cursor = open_brace_index + 1
+    content: list[str] = []
+    while cursor < len(tokens):
+        token = tokens[cursor]
+        if token == "{":
+            depth += 1
+        elif token == "}":
+            depth -= 1
+            if depth == 0:
+                return content, cursor + 1
+        content.append(token)
+        cursor += 1
+    raise ValueError("Unterminated braced block")
+
+
+def top_level_nodes(tokens: Sequence[str]) -> Iterable[tuple[str, list[str]]]:
+    cursor = 0
+    while cursor < len(tokens):
+        token = tokens[cursor]
+        if is_identifier(token) and cursor + 1 < len(tokens) and tokens[cursor + 1] == "{":
+            content, cursor = collect_braced_block(tokens, cursor + 1)
+            yield token, content
+            continue
+        cursor += 1
+
+
+def nested_first_box_size(tokens: Sequence[str]) -> tuple[float, float, float] | None:
+    cursor = 0
+    while cursor < len(tokens):
+        if tokens[cursor] == "Box" and cursor + 1 < len(tokens) and tokens[cursor + 1] == "{":
+            content, next_cursor = collect_braced_block(tokens, cursor + 1)
+            size = direct_numbers(content, "size", 3)
+            if size is not None:
+                return size
+            cursor = next_cursor
+            continue
+        cursor += 1
+    return None
+
+
+def parse_node(node_type: str, tokens: list[str]) -> WebotsNode:
+    fields: dict[str, object] = {}
+    translation = direct_numbers(tokens, "translation", 3)
+    rotation = direct_numbers(tokens, "rotation", 4)
+    size = direct_numbers(tokens, "size", 3)
+    gps_reference = direct_numbers(tokens, "gpsReference", 3)
+    name = direct_string(tokens, "name")
+    box_size = nested_first_box_size(tokens)
+
+    if translation is not None:
+        fields["translation"] = translation
+    if rotation is not None:
+        fields["rotation"] = rotation
+    if size is not None:
+        fields["size"] = size
+    if gps_reference is not None:
+        fields["gpsReference"] = gps_reference
+    if name is not None:
+        fields["name"] = name
+    if box_size is not None:
+        fields["box_size"] = box_size
+
+    return WebotsNode(node_type, fields, tokens)
+
+
+def parse_world(path: Path) -> WebotsWorld:
+    tokens = tokenize(path.read_text(encoding="utf-8"))
+    nodes = [parse_node(node_type, content) for node_type, content in top_level_nodes(tokens)]
+    world_info = next((node for node in nodes if node.type == "WorldInfo"), None)
+    if world_info is None or "gpsReference" not in world_info.fields:
+        raise ValueError(f"{path} does not define WorldInfo.gpsReference")
+
+    datum = world_info.fields["gpsReference"]
+    assert isinstance(datum, tuple)
+    return WebotsWorld(path.stem, datum, nodes)
+
+
+def translation(node: WebotsNode) -> tuple[float, float, float]:
+    value = node.fields.get("translation", (0.0, 0.0, 0.0))
+    assert isinstance(value, tuple)
+    return value
+
+
+def size(node: WebotsNode, default: tuple[float, float, float]) -> tuple[float, float, float]:
+    value = node.fields.get("size", default)
+    assert isinstance(value, tuple)
+    return value
+
+
+def node_name(node: WebotsNode) -> str:
+    value = node.fields.get("name", node.type)
+    assert isinstance(value, str)
+    return value
+
+
+def yaw_from_node(node: WebotsNode) -> float:
+    rotation = node.fields.get("rotation", (0.0, 0.0, 1.0, 0.0))
+    assert isinstance(rotation, tuple)
+    axis_x, axis_y, axis_z, angle = rotation
+    if abs(axis_x) < 1.0e-6 and abs(axis_y) < 1.0e-6 and abs(axis_z) > 1.0e-6:
+        return math.copysign(angle, axis_z)
+    return 0.0
+
+
+def rotate_point(x: float, y: float, yaw: float) -> tuple[float, float]:
+    cos_yaw = math.cos(yaw)
+    sin_yaw = math.sin(yaw)
+    return x * cos_yaw - y * sin_yaw, x * sin_yaw + y * cos_yaw
+
+
+def transform_local_point(
+    origin: tuple[float, float, float], yaw: float, point: tuple[float, float]
+) -> tuple[float, float]:
+    rotated_x, rotated_y = rotate_point(point[0], point[1], yaw)
+    return origin[0] + rotated_x, origin[1] + rotated_y
+
+
+def rectangle_polygon(
+    origin: tuple[float, float, float], footprint_size: tuple[float, float, float], yaw: float
+) -> list[tuple[float, float]]:
+    half_x = footprint_size[0] / 2.0
+    half_y = footprint_size[1] / 2.0
+    corners = [
+        (-half_x, -half_y),
+        (half_x, -half_y),
+        (half_x, half_y),
+        (-half_x, half_y),
+    ]
+    points = [transform_local_point(origin, yaw, corner) for corner in corners]
+    points.append(points[0])
+    return points
+
+
+def circle_polygon(
+    center: tuple[float, float, float], radius: float, segments: int
+) -> list[tuple[float, float]]:
+    points = [
+        (
+            center[0] + radius * math.cos(2.0 * math.pi * index / segments),
+            center[1] + radius * math.sin(2.0 * math.pi * index / segments),
+        )
+        for index in range(segments)
+    ]
+    points.append(points[0])
+    return points
+
+
+def operation_from_curbs(world: WebotsWorld) -> LocalFeature | None:
+    min_x_edges: list[float] = []
+    max_x_edges: list[float] = []
+    min_y_edges: list[float] = []
+    max_y_edges: list[float] = []
+
+    for node in world.nodes:
+        if node.type != "Curb":
+            continue
+        polygon = rectangle_polygon(translation(node), size(node, (1.0, 0.12, 0.07)), yaw_from_node(node))
+        xs = [point[0] for point in polygon[:-1]]
+        ys = [point[1] for point in polygon[:-1]]
+        width = max(xs) - min(xs)
+        height = max(ys) - min(ys)
+        center_x, center_y, _ = translation(node)
+        if height > width:
+            if center_x < 0.0:
+                min_x_edges.append(max(xs))
+            else:
+                max_x_edges.append(min(xs))
+        elif width > height:
+            if center_y < 0.0:
+                min_y_edges.append(max(ys))
+            else:
+                max_y_edges.append(min(ys))
+
+    if not (min_x_edges and max_x_edges and min_y_edges and max_y_edges):
+        return None
+
+    min_x = max(min_x_edges)
+    max_x = min(max_x_edges)
+    min_y = max(min_y_edges)
+    max_y = min(max_y_edges)
+    if min_x >= max_x or min_y >= max_y:
+        return None
+
+    return LocalFeature(
+        "operation",
+        f"{world.name}_operation_area",
+        "Polygon",
+        [(min_x, min_y), (max_x, min_y), (max_x, max_y), (min_x, max_y), (min_x, min_y)],
+    )
+
+
+def operation_from_floor(world: WebotsWorld) -> LocalFeature | None:
+    candidates: list[tuple[float, WebotsNode, tuple[float, float, float]]] = []
+    for node in world.nodes:
+        if node.type != "Solid":
+            continue
+        name = node_name(node).lower()
+        if not any(part in name for part in ("grass", "lawn", "floor")):
+            continue
+        box_size = node.fields.get("box_size")
+        if not isinstance(box_size, tuple):
+            continue
+        candidates.append((box_size[0] * box_size[1], node, box_size))
+
+    if not candidates:
+        return None
+
+    _, node, box_size = max(candidates, key=lambda candidate: candidate[0])
+    return LocalFeature(
+        "operation",
+        f"{world.name}_operation_area",
+        "Polygon",
+        rectangle_polygon(translation(node), box_size, yaw_from_node(node)),
+    )
+
+
+def build_local_features(world: WebotsWorld) -> list[LocalFeature]:
+    features: list[LocalFeature] = []
+    operation = operation_from_curbs(world) or operation_from_floor(world)
+    if operation is None:
+        raise ValueError(f"Could not infer an operation area from {world.name}")
+    features.append(operation)
+
+    for node in world.nodes:
+        if node.type == "GardenBuilding":
+            features.append(
+                LocalFeature(
+                    "exclusion",
+                    node_name(node),
+                    "Polygon",
+                    rectangle_polygon(translation(node), size(node, (4.0, 3.0, 2.0)), yaw_from_node(node)),
+                )
+            )
+        elif node.type == "GardenTree":
+            features.append(
+                LocalFeature(
+                    "exclusion",
+                    node_name(node),
+                    "Polygon",
+                    circle_polygon(translation(node), TREE_TRUNK_RADIUS, TREE_TRUNK_SEGMENTS),
+                )
+            )
+        elif node.type == "DockingStation":
+            origin = translation(node)
+            yaw = yaw_from_node(node)
+            contact = transform_local_point(origin, yaw, (DOCK_CONTACT_OFFSET_X, 0.0))
+            orientation = transform_local_point(
+                origin, yaw, (DOCK_CONTACT_OFFSET_X - DOCK_ORIENTATION_LENGTH, 0.0)
+            )
+            features.append(
+                LocalFeature("docking_station", node_name(node), "LineString", [contact, orientation])
+            )
+
+    return features
+
+
+def geodetic_to_ecef(lat_deg: float, lon_deg: float, alt_m: float) -> tuple[float, float, float]:
+    lat = math.radians(lat_deg)
+    lon = math.radians(lon_deg)
+    sin_lat = math.sin(lat)
+    cos_lat = math.cos(lat)
+    radius = WGS84_A / math.sqrt(1.0 - WGS84_E2 * sin_lat * sin_lat)
+    x = (radius + alt_m) * cos_lat * math.cos(lon)
+    y = (radius + alt_m) * cos_lat * math.sin(lon)
+    z = (radius * (1.0 - WGS84_E2) + alt_m) * sin_lat
+    return x, y, z
+
+
+def ecef_to_geodetic(x: float, y: float, z: float) -> tuple[float, float, float]:
+    lon = math.atan2(y, x)
+    p = math.hypot(x, y)
+    lat = math.atan2(z, p * (1.0 - WGS84_E2))
+    alt = 0.0
+    for _ in range(8):
+        sin_lat = math.sin(lat)
+        radius = WGS84_A / math.sqrt(1.0 - WGS84_E2 * sin_lat * sin_lat)
+        alt = p / math.cos(lat) - radius
+        lat = math.atan2(z, p * (1.0 - WGS84_E2 * radius / (radius + alt)))
+    return math.degrees(lat), math.degrees(lon), alt
+
+
+def enu_to_ecef(
+    east: float,
+    north: float,
+    up: float,
+    ref_lat_deg: float,
+    ref_lon_deg: float,
+    ref_ecef: tuple[float, float, float],
+) -> tuple[float, float, float]:
+    lat = math.radians(ref_lat_deg)
+    lon = math.radians(ref_lon_deg)
+    sin_lat = math.sin(lat)
+    cos_lat = math.cos(lat)
+    sin_lon = math.sin(lon)
+    cos_lon = math.cos(lon)
+
+    dx = -sin_lon * east - sin_lat * cos_lon * north + cos_lat * cos_lon * up
+    dy = cos_lon * east - sin_lat * sin_lon * north + cos_lat * sin_lon * up
+    dz = cos_lat * north + sin_lat * up
+    return ref_ecef[0] + dx, ref_ecef[1] + dy, ref_ecef[2] + dz
+
+
+def local_to_lon_lat(point: tuple[float, float], datum: tuple[float, float, float]) -> list[float]:
+    ref_ecef = geodetic_to_ecef(datum[0], datum[1], datum[2])
+    ecef = enu_to_ecef(point[0], point[1], 0.0, datum[0], datum[1], ref_ecef)
+    lat, lon, _ = ecef_to_geodetic(*ecef)
+    return [lon, lat]
+
+
+def feature_id(world_name: str, feature: LocalFeature) -> str:
+    return str(uuid.uuid5(FEATURE_NAMESPACE, f"{world_name}:{feature.feature_type}:{feature.name}"))
+
+
+def area_feature_to_geojson(
+    world: WebotsWorld, feature: LocalFeature, datum: tuple[float, float, float]
+) -> dict[str, object]:
+    color = AREA_COLORS[feature.feature_type]
+    return {
+        "type": "Feature",
+        "properties": {
+            "id": feature_id(world.name, feature),
+            "name": feature.name,
+            "type": feature.feature_type,
+            "fill": color,
+            "style": {"color": color},
+        },
+        "geometry": {
+            "type": "Polygon",
+            "coordinates": [[local_to_lon_lat(point, datum) for point in feature.coordinates]],
+        },
+    }
+
+
+def dock_feature_to_geojson(
+    world: WebotsWorld, feature: LocalFeature, datum: tuple[float, float, float]
+) -> dict[str, object]:
+    return {
+        "type": "Feature",
+        "properties": {
+            "id": feature_id(world.name, feature),
+            "name": feature.name,
+            "type": "docking_station",
+        },
+        "geometry": {
+            "type": "LineString",
+            "coordinates": [local_to_lon_lat(point, datum) for point in feature.coordinates],
+        },
+    }
+
+
+def local_features_to_geojson(world: WebotsWorld, features: Sequence[LocalFeature]) -> dict[str, object]:
+    geojson_features: list[dict[str, object]] = []
+    for feature in features:
+        if feature.geometry_type == "Polygon":
+            geojson_features.append(area_feature_to_geojson(world, feature, world.datum))
+        elif feature.geometry_type == "LineString":
+            geojson_features.append(dock_feature_to_geojson(world, feature, world.datum))
+        else:
+            raise ValueError(f"Unsupported geometry type: {feature.geometry_type}")
+    return {"type": "FeatureCollection", "features": geojson_features}
+
+
+def convert_world(path: Path) -> dict[str, object]:
+    world = parse_world(path)
+    return local_features_to_geojson(world, build_local_features(world))
+
+
+def parse_args(argv: Sequence[str]) -> argparse.Namespace:
+    parser = argparse.ArgumentParser(
+        description="Convert supported OpenMower Webots worlds into OpenMower GeoJSON maps."
+    )
+    parser.add_argument("world", type=Path, help="Input Webots .wbt world")
+    parser.add_argument("--output", "-o", type=Path, help="Output GeoJSON path; stdout if omitted")
+    return parser.parse_args(argv)
+
+
+def main(argv: Sequence[str] | None = None) -> int:
+    args = parse_args(sys.argv[1:] if argv is None else argv)
+    geojson = convert_world(args.world)
+    output = json.dumps(geojson, indent=2) + "\n"
+    if args.output is None:
+        sys.stdout.write(output)
+        return 0
+
+    args.output.parent.mkdir(parents=True, exist_ok=True)
+    args.output.write_text(output, encoding="utf-8")
+    return 0
+
+
+if __name__ == "__main__":
+    raise SystemExit(main())
diff --git a/src/docking_helper/charger_presence_charging_dock.cpp b/src/docking_helper/charger_presence_charging_dock.cpp
index a2e3c95..b7939c3 100644
--- a/src/docking_helper/charger_presence_charging_dock.cpp
+++ b/src/docking_helper/charger_presence_charging_dock.cpp
@@ -27,8 +27,9 @@ void ChargerPresenceChargingDock::configure(const rclcpp_lifecycle::LifecycleNod
   staging_x_offset_ = node_->get_parameter(name + ".staging_x_offset").as_double();
   staging_yaw_offset_ = node_->get_parameter(name + ".staging_yaw_offset").as_double();
 
-  is_charging_sub_ = node_->create_subscription<std_msgs::msg::Bool>(
-      "/power/charger_present", 10, [this](const std_msgs::msg::Bool::SharedPtr msg) { is_charging_ = msg->data; });
+  power_status_sub_ = node_->create_subscription<omros2_firmware_msgs::msg::PowerStatus>(
+      "/power/status", 10,
+      [this](const omros2_firmware_msgs::msg::PowerStatus::SharedPtr msg) { is_charging_ = msg->charger_present; });
 
   dock_pose_pub_ = node_->create_publisher<geometry_msgs::msg::PoseStamped>("dock_pose", 1);
   staging_pose_pub_ = node_->create_publisher<geometry_msgs::msg::PoseStamped>("staging_pose", 1);
@@ -41,7 +42,7 @@ void ChargerPresenceChargingDock::configure(const rclcpp_lifecycle::LifecycleNod
 void ChargerPresenceChargingDock::cleanup()
 {
   // Clean up subscriptions
-  is_charging_sub_.reset();
+  power_status_sub_.reset();
 }
 
 void ChargerPresenceChargingDock::activate()
@@ -85,6 +86,7 @@ geometry_msgs::msg::PoseStamped ChargerPresenceChargingDock::getStagingPose(cons
 
 bool ChargerPresenceChargingDock::getRefinedPose(geometry_msgs::msg::PoseStamped& pose, std::string id)
 {
+  (void)id;
   // just publish to a topic
   dock_pose_pub_->publish(pose);
   return true;
diff --git a/src/docking_helper/charger_presence_charging_dock.hpp b/src/docking_helper/charger_presence_charging_dock.hpp
index 03eadf5..06a5082 100644
--- a/src/docking_helper/charger_presence_charging_dock.hpp
+++ b/src/docking_helper/charger_presence_charging_dock.hpp
@@ -3,7 +3,7 @@
 #include <rclcpp/rclcpp.hpp>
 #include <rclcpp_lifecycle/lifecycle_node.hpp>
 #include <opennav_docking_core/charging_dock.hpp>
-#include <std_msgs/msg/bool.hpp>
+#include <omros2_firmware_msgs/msg/power_status.hpp>
 #include <tf2/utils.h>
 #include <geometry_msgs/msg/pose_stamped.hpp>
 
@@ -32,7 +32,7 @@ class ChargerPresenceChargingDock : public opennav_docking_core::ChargingDock
   bool hasStoppedCharging() override;
 
 private:
-  rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr is_charging_sub_;
+  rclcpp::Subscription<omros2_firmware_msgs::msg::PowerStatus>::SharedPtr power_status_sub_;
   bool is_charging_ = false;  // Initialize to prevent undefined behavior
   rclcpp_lifecycle::LifecycleNode::SharedPtr node_;
   std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
diff --git a/src/docking_helper/docking_helper_node.cpp b/src/docking_helper/docking_helper_node.cpp
index 8e0ea0d..c5ae4db 100644
--- a/src/docking_helper/docking_helper_node.cpp
+++ b/src/docking_helper/docking_helper_node.cpp
@@ -258,7 +258,9 @@ void open_mower_next::docking_helper::DockingHelperNode::executeDockingAction(
       [this, current_status,
        current_retries](typename rclcpp_action::ClientGoalHandle<nav2_msgs::action::DockRobot>::SharedPtr,
                         const std::shared_ptr<const nav2_msgs::action::DockRobot::Feedback> feedback) {
-        RCLCPP_INFO(get_logger(), "Docking state: %d, retries: %d", feedback->state, feedback->num_retries);
+        RCLCPP_INFO_THROTTLE(
+            get_logger(), *get_clock(), 1000, "Docking state: %d, retries: %d", feedback->state,
+            feedback->num_retries);
 
         *current_status = feedback->state;
         *current_retries = feedback->num_retries;
diff --git a/src/lib/fusioncore b/src/lib/fusioncore
new file mode 160000
index 0000000..a40081d
--- /dev/null
+++ b/src/lib/fusioncore
@@ -0,0 +1 @@
+Subproject commit a40081d0a79015e14a7c428ef3c04f5d3ac0ed74
diff --git a/src/lib/omros2_firmware b/src/lib/omros2_firmware
new file mode 160000
index 0000000..ba454a2
--- /dev/null
+++ b/src/lib/omros2_firmware
@@ -0,0 +1 @@
+Subproject commit ba454a21bde8562baa7fe328613e7e3644b9c284
diff --git a/src/lib/ublox_f9p b/src/lib/ublox_f9p
index 7ed7ee3..ca951c5 160000
--- a/src/lib/ublox_f9p
+++ b/src/lib/ublox_f9p
@@ -1 +1 @@
-Subproject commit 7ed7ee3d5eda7f55325d54f3b3c5dc831d6a7285
+Subproject commit ca951c506ddf5f2642c6b1ead977b7b0937a14a5
diff --git a/src/lib/webots_ros2 b/src/lib/webots_ros2
new file mode 160000
index 0000000..74ad17a
--- /dev/null
+++ b/src/lib/webots_ros2
@@ -0,0 +1 @@
+Subproject commit 74ad17a92938ad07ed86336350db0719d1320e35
diff --git a/src/map_recorder/map_recorder_node.cpp b/src/map_recorder/map_recorder_node.cpp
index 6711f4f..5cee27f 100644
--- a/src/map_recorder/map_recorder_node.cpp
+++ b/src/map_recorder/map_recorder_node.cpp
@@ -44,8 +44,8 @@ MapRecorderNode::MapRecorderNode(const rclcpp::NodeOptions& options)
   save_docking_station_client_ = create_client<open_mower_next::srv::SaveDockingStation>("save_docking_station");
   save_area_client_ = create_client<open_mower_next::srv::SaveArea>("save_area");
 
-  charging_status_sub_ = create_subscription<std_msgs::msg::Bool>(
-      "/power/charger_present", 10, std::bind(&MapRecorderNode::chargingStatusCallback, this, _1));
+  charging_status_sub_ = create_subscription<omros2_firmware_msgs::msg::PowerStatus>(
+      "/power/status", 10, std::bind(&MapRecorderNode::chargingStatusCallback, this, _1));
 
   boundary_polygon_pub_ =
       create_publisher<geometry_msgs::msg::PolygonStamped>("/map_recorder/record_boundaries_polygon", 10);
@@ -627,9 +627,9 @@ void MapRecorderNode::handleFinishAreaRecording(const std::shared_ptr<std_srvs::
   return;
 }
 
-void MapRecorderNode::chargingStatusCallback(const std_msgs::msg::Bool::SharedPtr msg)
+void MapRecorderNode::chargingStatusCallback(const omros2_firmware_msgs::msg::PowerStatus::SharedPtr msg)
 {
-  is_charging_detected_ = msg->data;
+  is_charging_detected_ = msg->charger_present;
 }
 
 bool MapRecorderNode::checkPositionCovariance()
diff --git a/src/map_recorder/map_recorder_node.hpp b/src/map_recorder/map_recorder_node.hpp
index 61aa97b..0569d4c 100644
--- a/src/map_recorder/map_recorder_node.hpp
+++ b/src/map_recorder/map_recorder_node.hpp
@@ -7,7 +7,7 @@
 #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
 #include <geometry_msgs/msg/pose_stamped.hpp>
 #include <geometry_msgs/msg/polygon_stamped.hpp>
-#include <std_msgs/msg/bool.hpp>
+#include <omros2_firmware_msgs/msg/power_status.hpp>
 #include <std_srvs/srv/set_bool.hpp>
 #include <std_srvs/srv/trigger.hpp>
 
@@ -57,7 +57,7 @@ class MapRecorderNode : public rclcpp::Node
   rclcpp::Client<open_mower_next::srv::SaveDockingStation>::SharedPtr save_docking_station_client_;
   rclcpp::Client<open_mower_next::srv::SaveArea>::SharedPtr save_area_client_;
 
-  rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr charging_status_sub_;
+  rclcpp::Subscription<omros2_firmware_msgs::msg::PowerStatus>::SharedPtr charging_status_sub_;
 
   rclcpp::Publisher<geometry_msgs::msg::PolygonStamped>::SharedPtr boundary_polygon_pub_;
 
@@ -95,7 +95,7 @@ class MapRecorderNode : public rclcpp::Node
   void handleFinishAreaRecording(const std::shared_ptr<std_srvs::srv::Trigger::Request> request,
                                  std::shared_ptr<std_srvs::srv::Trigger::Response> response);
 
-  void chargingStatusCallback(const std_msgs::msg::Bool::SharedPtr msg);
+  void chargingStatusCallback(const omros2_firmware_msgs::msg::PowerStatus::SharedPtr msg);
 
   bool validateAreaBoundary();
 
diff --git a/src/map_server/geo_json_map.hpp b/src/map_server/geo_json_map.hpp
index a8f831d..f2928c5 100644
--- a/src/map_server/geo_json_map.hpp
+++ b/src/map_server/geo_json_map.hpp
@@ -5,7 +5,6 @@
 #include <geographic_msgs/msg/geo_point.hpp>
 #include <GeographicLib/Geocentric.hpp>
 #include <GeographicLib/LocalCartesian.hpp>
-#include <robot_localization/srv/to_ll.hpp>
 
 #include "map_server_node.hpp"
 
diff --git a/src/map_server/test/load_geojson_map.test.py b/src/map_server/test/load_geojson_map.test.py
index c68297f..b1a21a3 100644
--- a/src/map_server/test/load_geojson_map.test.py
+++ b/src/map_server/test/load_geojson_map.test.py
@@ -37,14 +37,6 @@ def generate_test_description():
                 'path': test_map,
             }],
         ),
-        launch_ros.actions.Node(
-            package='robot_localization',
-            executable='navsat_transform_node',
-            name='navsat_transform_node',
-            parameters=[{
-                'datum': [-22.9, -43.2, 0.0],
-            }],
-        ),
         launch_testing.actions.ReadyToTest()
     ])
 
diff --git a/src/sim/navigation_readiness_node.cpp b/src/sim/navigation_readiness_node.cpp
new file mode 100644
index 0000000..3e69be7
--- /dev/null
+++ b/src/sim/navigation_readiness_node.cpp
@@ -0,0 +1,180 @@
+#include <chrono>
+#include <cmath>
+#include <set>
+#include <sstream>
+#include <string>
+
+#include <nav_msgs/msg/occupancy_grid.hpp>
+#include <nav_msgs/msg/odometry.hpp>
+#include <open_mower_next/msg/map.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <sensor_msgs/msg/imu.hpp>
+#include <sensor_msgs/msg/nav_sat_fix.hpp>
+#include <tf2/time.h>
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/transform_listener.h>
+
+namespace
+{
+
+bool isFiniteOdom(const nav_msgs::msg::Odometry & msg)
+{
+  const auto & pose = msg.pose.pose;
+  const auto & twist = msg.twist.twist;
+  return std::isfinite(pose.position.x) && std::isfinite(pose.position.y) &&
+         std::isfinite(pose.position.z) && std::isfinite(pose.orientation.x) &&
+         std::isfinite(pose.orientation.y) && std::isfinite(pose.orientation.z) &&
+         std::isfinite(pose.orientation.w) && std::isfinite(twist.linear.x) &&
+         std::isfinite(twist.linear.y) && std::isfinite(twist.angular.z);
+}
+
+std::string joinMissing(const std::set<std::string> & missing)
+{
+  std::ostringstream stream;
+  bool first = true;
+  for (const auto & topic : missing) {
+    if (!first) {
+      stream << ", ";
+    }
+    first = false;
+    stream << topic;
+  }
+  return stream.str();
+}
+
+class NavigationReadinessNode final : public rclcpp::Node
+{
+public:
+  NavigationReadinessNode()
+  : Node("navigation_readiness_node"), tf_buffer_(get_clock()), tf_listener_(tf_buffer_)
+  {
+    timeout_seconds_ = declare_parameter<double>("timeout_seconds", 90.0);
+    map_frame_ = declare_parameter<std::string>("map_frame", "map");
+    base_frame_ = declare_parameter<std::string>("base_frame", "base_link");
+
+    required_topics_ = {
+      "/diff_drive_base_controller/odom", "/gps/fix", "/gps/odom", "/imu/data_raw",
+      "/fusion/odom", "/map_grid", "/mowing_map"};
+
+    const auto map_qos = rclcpp::QoS(1).transient_local().reliable();
+
+    diff_odom_subscription_ = create_subscription<nav_msgs::msg::Odometry>(
+      "/diff_drive_base_controller/odom", 10,
+      [this](const nav_msgs::msg::Odometry::SharedPtr) { markReady("/diff_drive_base_controller/odom"); });
+    gps_fix_subscription_ = create_subscription<sensor_msgs::msg::NavSatFix>(
+      "/gps/fix", 10, [this](const sensor_msgs::msg::NavSatFix::SharedPtr) { markReady("/gps/fix"); });
+    gps_odom_subscription_ = create_subscription<nav_msgs::msg::Odometry>(
+      "/gps/odom", 10, [this](const nav_msgs::msg::Odometry::SharedPtr msg) {
+        if (isFiniteOdom(*msg)) {
+          markReady("/gps/odom");
+        }
+      });
+    imu_subscription_ = create_subscription<sensor_msgs::msg::Imu>(
+      "/imu/data_raw", 10,
+      [this](const sensor_msgs::msg::Imu::SharedPtr) { markReady("/imu/data_raw"); });
+    fusion_odom_subscription_ = create_subscription<nav_msgs::msg::Odometry>(
+      "/fusion/odom", 10, [this](const nav_msgs::msg::Odometry::SharedPtr msg) {
+        if (isFiniteOdom(*msg)) {
+          markReady("/fusion/odom");
+        }
+      });
+    map_grid_subscription_ = create_subscription<nav_msgs::msg::OccupancyGrid>(
+      "/map_grid", map_qos,
+      [this](const nav_msgs::msg::OccupancyGrid::SharedPtr) { markReady("/map_grid"); });
+    mowing_map_subscription_ = create_subscription<open_mower_next::msg::Map>(
+      "/mowing_map", map_qos,
+      [this](const open_mower_next::msg::Map::SharedPtr) { markReady("/mowing_map"); });
+
+    timer_ = create_wall_timer(std::chrono::milliseconds(100), [this] { checkReady(); });
+    RCLCPP_INFO(get_logger(), "Waiting for navigation prerequisites before starting Nav2");
+  }
+
+  int exitCode() const { return exit_code_; }
+
+private:
+  void markReady(const std::string & name) { ready_topics_.insert(name); }
+
+  bool hasMapToBaseLinkTf()
+  {
+    try {
+      return tf_buffer_.canTransform(
+        map_frame_, base_frame_, tf2::TimePointZero, tf2::durationFromSec(0.01));
+    } catch (const tf2::TransformException &) {
+      return false;
+    }
+  }
+
+  std::set<std::string> missingTopics() const
+  {
+    std::set<std::string> missing;
+    for (const auto & topic : required_topics_) {
+      if (!ready_topics_.count(topic)) {
+        missing.insert(topic);
+      }
+    }
+    return missing;
+  }
+
+  void checkReady()
+  {
+    const auto missing = missingTopics();
+    const bool tf_ready = hasMapToBaseLinkTf();
+    if (missing.empty() && tf_ready) {
+      exit_code_ = 0;
+      RCLCPP_INFO(get_logger(), "Navigation prerequisites ready; starting Nav2");
+      rclcpp::shutdown();
+      return;
+    }
+
+    const auto elapsed = std::chrono::duration<double>(std::chrono::steady_clock::now() - start_time_).count();
+    if (elapsed >= timeout_seconds_) {
+      exit_code_ = 1;
+      RCLCPP_ERROR(
+        get_logger(), "Timed out waiting for navigation prerequisites. Missing topics: [%s], tf %s -> %s: %s",
+        joinMissing(missing).c_str(), map_frame_.c_str(), base_frame_.c_str(), tf_ready ? "ready" : "missing");
+      rclcpp::shutdown();
+      return;
+    }
+
+    RCLCPP_INFO_THROTTLE(
+      get_logger(), *get_clock(), 5000,
+      "Navigation prerequisites pending. Missing topics: [%s], tf %s -> %s: %s",
+      joinMissing(missing).c_str(), map_frame_.c_str(), base_frame_.c_str(), tf_ready ? "ready" : "missing");
+  }
+
+  std::chrono::steady_clock::time_point start_time_ = std::chrono::steady_clock::now();
+  double timeout_seconds_ = 90.0;
+  std::string map_frame_;
+  std::string base_frame_;
+  int exit_code_ = 1;
+
+  std::set<std::string> required_topics_;
+  std::set<std::string> ready_topics_;
+
+  tf2_ros::Buffer tf_buffer_;
+  tf2_ros::TransformListener tf_listener_;
+  rclcpp::TimerBase::SharedPtr timer_;
+
+  rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr diff_odom_subscription_;
+  rclcpp::Subscription<sensor_msgs::msg::NavSatFix>::SharedPtr gps_fix_subscription_;
+  rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr gps_odom_subscription_;
+  rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr imu_subscription_;
+  rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr fusion_odom_subscription_;
+  rclcpp::Subscription<nav_msgs::msg::OccupancyGrid>::SharedPtr map_grid_subscription_;
+  rclcpp::Subscription<open_mower_next::msg::Map>::SharedPtr mowing_map_subscription_;
+};
+
+}  // namespace
+
+int main(int argc, char ** argv)
+{
+  setvbuf(stdout, nullptr, _IONBF, BUFSIZ);
+  rclcpp::init(argc, argv);
+  auto node = std::make_shared<NavigationReadinessNode>();
+  rclcpp::spin(node);
+  const int exit_code = node->exitCode();
+  if (rclcpp::ok()) {
+    rclcpp::shutdown();
+  }
+  return exit_code;
+}
diff --git a/src/sim/sim_node.cpp b/src/sim/sim_node.cpp
index a8920cb..3aa7b1a 100644
--- a/src/sim/sim_node.cpp
+++ b/src/sim/sim_node.cpp
@@ -5,6 +5,7 @@
 #include <tf2/LinearMath/Quaternion.h>
 #include <tf2/LinearMath/Transform.h>
 
+#include <algorithm>
 #include <cmath>
 
 open_mower_next::sim::SimNode::SimNode(const rclcpp::NodeOptions & options)
@@ -14,11 +15,23 @@ open_mower_next::sim::SimNode::SimNode(const rclcpp::NodeOptions & options)
   tf_buffer_ = std::make_unique<tf2_ros::Buffer>(this->get_clock());
   tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
 
-  charger_present_publisher_ =
-    this->create_publisher<std_msgs::msg::Bool>("/power/charger_present", 10);
   battery_state_publisher_ = this->create_publisher<sensor_msgs::msg::BatteryState>("/power", 10);
-  charge_voltage_publisher_ =
-    this->create_publisher<std_msgs::msg::Float32>("/power/charge_voltage", 10);
+  power_status_publisher_ =
+    this->create_publisher<omros2_firmware_msgs::msg::PowerStatus>("/power/status", 10);
+  gps_odom_publisher_ = this->create_publisher<nav_msgs::msg::Odometry>("/gps/odom", 10);
+
+  gps_odom_frame_ = this->declare_parameter<std::string>("gps_odom_frame", "map");
+  gps_child_frame_ = this->declare_parameter<std::string>("gps_child_frame", "gnss_link");
+  gps_speed_stddev_ = this->declare_parameter<double>("gps_speed_stddev", 0.05);
+
+  gps_fix_subscription_ = this->create_subscription<sensor_msgs::msg::NavSatFix>(
+    "/gps/fix", 10, [this](const sensor_msgs::msg::NavSatFix::SharedPtr msg) {
+      gpsFixCallback(msg);
+    });
+  gps_speed_vector_subscription_ = this->create_subscription<geometry_msgs::msg::Vector3>(
+    "/gps/fix/speed_vector", 10, [this](const geometry_msgs::msg::Vector3::SharedPtr msg) {
+      gpsSpeedVectorCallback(msg);
+    });
 
   auto freq_ = this->declare_parameter<int32_t>("charger_simulation_freq", 15);
 
@@ -44,6 +57,10 @@ open_mower_next::sim::SimNode::SimNode(const rclcpp::NodeOptions & options)
   battery_state_msg_.power_supply_technology =
     sensor_msgs::msg::BatteryState::POWER_SUPPLY_TECHNOLOGY_LION;
   battery_state_msg_.present = true;
+  battery_state_msg_.percentage = std::clamp(
+    (battery_state_msg_.voltage - battery_state_min_voltage_) /
+      (battery_state_max_voltage_ - battery_state_min_voltage_),
+    0.0, 1.0);
 
   charger_timer_ = this->create_timer(
     std::chrono::milliseconds(1000 / freq_), [this] { chargerPresentSimulationCallback(); });
@@ -83,6 +100,37 @@ open_mower_next::sim::SimNode::SimNode(const rclcpp::NodeOptions & options)
   RCLCPP_INFO(get_logger(), "SimNode created with timer frequency: %d Hz", static_cast<int>(freq_));
 }
 
+void open_mower_next::sim::SimNode::gpsFixCallback(
+  const sensor_msgs::msg::NavSatFix::SharedPtr msg)
+{
+  has_gps_fix_ = true;
+  last_gps_fix_stamp_ = msg->header.stamp;
+}
+
+void open_mower_next::sim::SimNode::gpsSpeedVectorCallback(
+  const geometry_msgs::msg::Vector3::SharedPtr msg)
+{
+  nav_msgs::msg::Odometry odom;
+  if (has_gps_fix_) {
+    odom.header.stamp = last_gps_fix_stamp_;
+  } else {
+    odom.header.stamp = now();
+  }
+  odom.header.frame_id = gps_odom_frame_;
+  odom.child_frame_id = gps_child_frame_;
+  odom.twist.twist.linear.x = msg->x;
+  odom.twist.twist.linear.y = msg->y;
+  odom.twist.twist.linear.z = msg->z;
+
+  const double speed_var = gps_speed_stddev_ * gps_speed_stddev_;
+  odom.twist.covariance[0] = speed_var;
+  odom.twist.covariance[7] = speed_var;
+  odom.twist.covariance[14] = speed_var;
+  odom.twist.covariance[35] = -1.0;
+
+  gps_odom_publisher_->publish(odom);
+}
+
 bool open_mower_next::sim::SimNode::isInDockingStation()
 {
   // Try to get the current charging port position
@@ -119,7 +167,8 @@ bool open_mower_next::sim::SimNode::isInDockingStation()
   if (!inDockingStation && distance < 1.0) {
     RCLCPP_INFO_THROTTLE(
       get_logger(), *get_clock(), 5000,
-      "Distance from charging port to docking station: %f m, angle: %f rad", distance, angle);
+      "Charging port offset from docking station: x=%f m, y=%f m, distance=%f m, angle=%f rad",
+      translation.x(), translation.y(), distance, angle);
   }
 
   if (inDockingStation) {
@@ -135,16 +184,15 @@ void open_mower_next::sim::SimNode::chargerPresentSimulationCallback()
 {
   auto inDock = isInDockingStation();
 
-  charger_present_msg_.data = inDock;
-  charger_present_publisher_->publish(charger_present_msg_);
+  charger_present_ = inDock;
 
   if (inDock) {
-    charge_voltage_msg_.data = battery_state_max_voltage_;
+    charge_voltage_ = static_cast<float>(battery_state_max_voltage_);
   } else {
-    charge_voltage_msg_.data = 0.0;
+    charge_voltage_ = 0.0f;
   }
 
-  charge_voltage_publisher_->publish(charge_voltage_msg_);
+  publishPowerStatus(get_clock()->now());
 }
 
 // This callback is called in a given interval to simulate the battery state
@@ -165,7 +213,7 @@ void open_mower_next::sim::SimNode::batteryStateSimulationCallback()
     last_battery_voltage_update_ = now;
   }
 
-  if (!charger_present_msg_.data) {
+  if (!charger_present_) {
     auto sinceLastVoltageUpdate = (now - last_battery_voltage_update_).seconds();
     if (sinceLastVoltageUpdate >= 1) {
       battery_state_msg_.voltage -= sinceLastVoltageUpdate * battery_state_voltage_drop_per_second_;
@@ -177,17 +225,22 @@ void open_mower_next::sim::SimNode::batteryStateSimulationCallback()
   }
 
   last_battery_voltage_update_ = now;
-  battery_state_msg_.percentage = (battery_state_msg_.voltage - battery_state_min_voltage_) /
-                                  (battery_state_max_voltage_ - battery_state_min_voltage_) * 100.0;
+  battery_state_msg_.percentage = std::clamp(
+    (battery_state_msg_.voltage - battery_state_min_voltage_) /
+      (battery_state_max_voltage_ - battery_state_min_voltage_),
+    0.0, 1.0);
 
-  if (charger_present_msg_.data) {
-    if (battery_state_msg_.percentage < 100.0) {
+  if (charger_present_) {
+    if (battery_state_msg_.percentage < 1.0) {
       battery_state_msg_.power_supply_status =
         sensor_msgs::msg::BatteryState::POWER_SUPPLY_STATUS_CHARGING;
     } else {
       battery_state_msg_.power_supply_status =
         sensor_msgs::msg::BatteryState::POWER_SUPPLY_STATUS_FULL;
     }
+  } else {
+    battery_state_msg_.power_supply_status =
+      sensor_msgs::msg::BatteryState::POWER_SUPPLY_STATUS_DISCHARGING;
   }
 
   battery_state_msg_.power_supply_health =
@@ -201,4 +254,31 @@ void open_mower_next::sim::SimNode::batteryStateSimulationCallback()
 
   battery_state_msg_.header.stamp = now;
   battery_state_publisher_->publish(battery_state_msg_);
+  publishPowerStatus(now);
+}
+
+void open_mower_next::sim::SimNode::publishPowerStatus(const rclcpp::Time & stamp)
+{
+  const float battery_percentage = static_cast<float>(battery_state_msg_.percentage * 100.0);
+  const bool charging_allowed = charger_present_ && battery_percentage < 100.0f;
+
+  power_status_msg_.stamp = stamp;
+  power_status_msg_.battery_voltage = static_cast<float>(battery_state_msg_.voltage);
+  power_status_msg_.battery_percentage = battery_percentage;
+  power_status_msg_.battery_present = battery_state_msg_.present;
+  power_status_msg_.charge_voltage = charge_voltage_;
+  power_status_msg_.charge_current = charging_allowed ? 0.5f : 0.0f;
+  power_status_msg_.charger_present = charger_present_;
+  power_status_msg_.charge_path_enabled = !charger_present_ || charging_allowed;
+  power_status_msg_.charging_allowed = charging_allowed;
+  power_status_msg_.charging_state = charger_present_
+                                      ? (charging_allowed
+                                           ? omros2_firmware_msgs::msg::PowerStatus::CHARGING_STATE_CHARGING
+                                           : omros2_firmware_msgs::msg::PowerStatus::CHARGING_STATE_NOT_CHARGING)
+                                      : omros2_firmware_msgs::msg::PowerStatus::CHARGING_STATE_REGEN_PATH;
+  power_status_msg_.inhibit_reason = charger_present_ && !charging_allowed
+                                      ? omros2_firmware_msgs::msg::PowerStatus::INHIBIT_BATTERY_FULL
+                                      : omros2_firmware_msgs::msg::PowerStatus::INHIBIT_NONE;
+  power_status_msg_.retry_remaining_ms = 0;
+  power_status_publisher_->publish(power_status_msg_);
 }
diff --git a/src/sim/sim_node.hpp b/src/sim/sim_node.hpp
index b2fb46e..15af41f 100644
--- a/src/sim/sim_node.hpp
+++ b/src/sim/sim_node.hpp
@@ -1,11 +1,14 @@
 #pragma once
 
+#include <builtin_interfaces/msg/time.hpp>
 #include <rclcpp/node.hpp>
 
 #include <geometry_msgs/msg/pose_stamped.hpp>
+#include <geometry_msgs/msg/vector3.hpp>
+#include <nav_msgs/msg/odometry.hpp>
+#include <omros2_firmware_msgs/msg/power_status.hpp>
 #include <sensor_msgs/msg/battery_state.hpp>
-#include <std_msgs/msg/bool.hpp>
-#include <std_msgs/msg/float32.hpp>
+#include <sensor_msgs/msg/nav_sat_fix.hpp>
 
 #include <tf2_ros/buffer.h>
 #include <tf2_ros/transform_listener.h>
@@ -30,13 +33,21 @@ class SimNode final : public rclcpp::Node
   double docking_detection_tolerance_x_;
   double docking_detection_tolerance_y_;
 
-  std_msgs::msg::Bool charger_present_msg_;
   sensor_msgs::msg::BatteryState battery_state_msg_;
-  std_msgs::msg::Float32 charge_voltage_msg_;
+  omros2_firmware_msgs::msg::PowerStatus power_status_msg_;
+
+  std::string gps_odom_frame_;
+  std::string gps_child_frame_;
+  double gps_speed_stddev_;
+  bool has_gps_fix_ = false;
+  builtin_interfaces::msg::Time last_gps_fix_stamp_;
 
-  rclcpp::Publisher<std_msgs::msg::Bool>::SharedPtr charger_present_publisher_;
   rclcpp::Publisher<sensor_msgs::msg::BatteryState>::SharedPtr battery_state_publisher_;
-  rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr charge_voltage_publisher_;
+  rclcpp::Publisher<omros2_firmware_msgs::msg::PowerStatus>::SharedPtr power_status_publisher_;
+  rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr gps_odom_publisher_;
+
+  rclcpp::Subscription<sensor_msgs::msg::NavSatFix>::SharedPtr gps_fix_subscription_;
+  rclcpp::Subscription<geometry_msgs::msg::Vector3>::SharedPtr gps_speed_vector_subscription_;
 
   rclcpp::TimerBase::SharedPtr charger_timer_;
   rclcpp::TimerBase::SharedPtr battery_timer_;
@@ -45,6 +56,8 @@ class SimNode final : public rclcpp::Node
   double battery_state_min_voltage_;
   double battery_state_voltage_drop_per_second_;
   double battery_state_voltage_charge_per_second_;
+  bool charger_present_ = false;
+  float charge_voltage_ = 0.0f;
   rclcpp::Time last_battery_voltage_update_;
 
   // TF2 buffer and listener
@@ -53,6 +66,9 @@ class SimNode final : public rclcpp::Node
 
   void chargerPresentSimulationCallback();
   void batteryStateSimulationCallback();
+  void publishPowerStatus(const rclcpp::Time & stamp);
+  void gpsFixCallback(const sensor_msgs::msg::NavSatFix::SharedPtr msg);
+  void gpsSpeedVectorCallback(const geometry_msgs::msg::Vector3::SharedPtr msg);
 
   bool isInDockingStation();
 };
diff --git a/systemd/openmower-foxglove.service.in b/systemd/openmower-foxglove.service.in
index f98f7a9..b40cec2 100644
--- a/systemd/openmower-foxglove.service.in
+++ b/systemd/openmower-foxglove.service.in
@@ -6,6 +6,7 @@ After=default.target
 Type=simple
 WorkingDirectory=@WORKSPACE@
 Environment=ROS_DISTRO=@ROS_DISTRO@
+Environment=ROS_AUTOMATIC_DISCOVERY_RANGE=@ROS_AUTOMATIC_DISCOVERY_RANGE@
 Environment=FOXGLOVE_ADDRESS=@FOXGLOVE_ADDRESS@
 Environment=FOXGLOVE_PORT=@FOXGLOVE_PORT@
 Environment=FOXGLOVE_USE_SIM_TIME=@FOXGLOVE_USE_SIM_TIME@
diff --git a/systemd/openmower-rosbridge.service.in b/systemd/openmower-rosbridge.service.in
index 3090f7d..7e63c17 100644
--- a/systemd/openmower-rosbridge.service.in
+++ b/systemd/openmower-rosbridge.service.in
@@ -6,6 +6,7 @@ After=default.target
 Type=simple
 WorkingDirectory=@WORKSPACE@
 Environment=ROS_DISTRO=@ROS_DISTRO@
+Environment=ROS_AUTOMATIC_DISCOVERY_RANGE=@ROS_AUTOMATIC_DISCOVERY_RANGE@
 Environment=ROSBRIDGE_ADDRESS=@ROSBRIDGE_ADDRESS@
 Environment=ROSBRIDGE_PORT=@ROSBRIDGE_PORT@
 ExecStart=/bin/bash @WORKSPACE@/utils/run-rosbridge.sh
diff --git a/test/integration/test_gps_degraded.py b/test/integration/test_gps_degraded.py
new file mode 100644
index 0000000..3300940
--- /dev/null
+++ b/test/integration/test_gps_degraded.py
@@ -0,0 +1,642 @@
+import math
+import os
+import signal
+import subprocess
+import time
+from dataclasses import dataclass
+from pathlib import Path
+
+import pytest
+import rclpy
+import tf2_ros
+from action_msgs.msg import GoalStatus
+from diagnostic_msgs.msg import DiagnosticArray
+from lifecycle_msgs.msg import State
+from lifecycle_msgs.srv import GetState
+from nav2_msgs.action import NavigateToPose, Spin
+from nav_msgs.msg import OccupancyGrid, Odometry
+from open_mower_next.msg import Map
+from rclpy.action import ActionClient
+from rclpy.duration import Duration
+from rclpy.node import Node
+from rclpy.parameter import Parameter
+from rclpy.qos import DurabilityPolicy, QoSProfile, ReliabilityPolicy
+from rclpy.time import Time
+from rosgraph_msgs.msg import Clock
+from sensor_msgs.msg import NavSatFix
+
+
+START_X = 1.50
+START_Y = -0.40
+START_YAW = 0.0
+ZONE_X = 0.40
+ZONE_Y = 0.65
+ZONE_RADIUS = 1.40
+GOAL_1_X = -0.20
+GOAL_1_Y = 0.80
+GOAL_1_YAW = math.pi
+GOAL_2_X = 0.95
+GOAL_2_Y = 0.80
+GOAL_2_YAW = 0.0
+FINAL_GOAL_TOLERANCE = 0.55
+MAX_SEGMENT_YAW_DELTA_ERROR = 0.65
+LOCALIZATION_MIN_X = -2.0
+LOCALIZATION_MAX_X = 4.5
+LOCALIZATION_MIN_Y = -2.0
+LOCALIZATION_MAX_Y = 3.5
+DEGRADED_COVARIANCE_THRESHOLD = 0.05
+NAV2_LIFECYCLE_NODES = (
+    "controller_server",
+    "planner_server",
+    "behavior_server",
+    "bt_navigator",
+    "velocity_smoother",
+    "docking_server",
+    "smoother_server",
+)
+
+LOG_FAILURE_PATTERNS = (
+    "[FATAL]",
+    "Could not load library",
+    "Could not listen to extern controllers",
+    "Cannot shutdown a ROS adapter",
+    "Webots driver exited unexpectedly",
+    "process has died",
+    "Traceback (most recent call last)",
+)
+
+
+@dataclass
+class OdomSnapshot:
+    fusion_x: float
+    fusion_y: float
+    fusion_yaw: float
+    wheel_yaw: float
+
+
+def repo_root() -> Path:
+    return Path(__file__).resolve().parents[2]
+
+
+def prepare_env() -> dict[str, str]:
+    root = repo_root()
+    env = os.environ.copy()
+    env["ROS_DOMAIN_ID"] = str(170 + (os.getpid() % 30))
+    env["WEBOTS_PORT"] = str(15000 + (os.getpid() % 1000))
+    env["WEBOTS_OFFSCREEN"] = "1"
+    env["OM_DATUM_LAT"] = "-22.9"
+    env["OM_DATUM_LONG"] = "-43.2"
+    env["OM_MAP_PATH"] = str(root / "test" / "integration" / "navigation_docking_map.geojson")
+
+    if not env.get("WEBOTS_HOME"):
+        default_webots_home = Path.home() / ".ros" / "webotsR2025a" / "webots"
+        if default_webots_home.is_dir():
+            env["WEBOTS_HOME"] = str(default_webots_home)
+
+    if not env.get("WEBOTS_HOME") or not Path(env["WEBOTS_HOME"]).is_dir():
+        pytest.fail("WEBOTS_HOME must point to a Webots R2025a installation for integration tests")
+
+    return env
+
+
+def create_degraded_world(tmp_path: Path) -> Path:
+    root = repo_root()
+    world_path = tmp_path / "gps_degraded.wbt"
+    content = (root / "worlds" / "simple_lawn.wbt").read_text(encoding="utf-8")
+    content = content.replace(
+        'EXTERNPROTO "../protos/OpenMower.proto"',
+        f'EXTERNPROTO "{(root / "protos" / "OpenMower.proto").as_posix()}"',
+    )
+    content = content.replace(
+        'EXTERNPROTO "../protos/DockingStation.proto"',
+        (
+            f'EXTERNPROTO "{(root / "protos" / "DockingStation.proto").as_posix()}"\n'
+            f'EXTERNPROTO "{(root / "protos" / "GnssDegradationZone.proto").as_posix()}"'
+        ),
+    )
+    content = content.replace(
+        'title "OpenMowerNext simple lawn"',
+        'title "OpenMowerNext degraded GPS integration test"',
+        1,
+    )
+    content = content.replace(
+        """DockingStation {
+  translation 1.5 1.5 0
+  name "docking_station"
+}""",
+        """DockingStation {
+  translation 4.5 4.5 0
+  name "docking_station"
+}""",
+        1,
+    )
+    content = content.replace(
+        """OpenMower {
+  translation 0 0 0.0925
+  rotation 0 0 1 0
+  name "openmower"
+  controller "<extern>"
+  supervisor TRUE
+}""",
+        f"""OpenMower {{
+  translation {START_X:.6f} {START_Y:.6f} 0.0925
+  rotation 0 0 1 {START_YAW:.9f}
+  name "openmower"
+  controller "<extern>"
+  supervisor TRUE
+}}""",
+        1,
+    )
+    zone = f"""GnssDegradationZone {{
+  translation {ZONE_X:.3f} {ZONE_Y:.3f} 0.03
+  name "route_degraded_gps"
+  size {ZONE_RADIUS * 2.0:.3f} {ZONE_RADIUS * 2.0:.3f} 0.06
+  metadata "shape=circle;radius={ZONE_RADIUS:.3f};model=route_gps_shadow;sigmaXY=0.20;biasX=0.0;biasY=0.0;dropoutProbability=0.02;covarianceXY=0.16;edgeWidth=0.35;driftTimeConstant=6"
+  color 1 0.22 0.05
+}}"""
+    content = content.replace('\nRobot {\n  name "Ros2Supervisor"', f"\n{zone}\nRobot {{\n  name \"Ros2Supervisor\"", 1)
+    world_path.write_text(content, encoding="utf-8")
+    return world_path
+
+
+def start_simulation(log_path: Path, env: dict[str, str], world_path: Path):
+    log_file = log_path.open("w", encoding="utf-8")
+    process = subprocess.Popen(
+        [
+            "ros2",
+            "launch",
+            "open_mower_next",
+            "sim.launch.py",
+            "gui:=false",
+            "mode:=realtime",
+            "enable_foxglove:=false",
+            f"webots_port:={env['WEBOTS_PORT']}",
+            f"world:={world_path}",
+        ],
+        stdout=log_file,
+        stderr=subprocess.STDOUT,
+        env=env,
+        start_new_session=True,
+        text=True,
+    )
+    return process, log_file
+
+
+def stop_process(process: subprocess.Popen) -> None:
+    if process.poll() is not None:
+        return
+    try:
+        os.killpg(os.getpgid(process.pid), signal.SIGINT)
+        process.wait(timeout=15)
+        return
+    except (ProcessLookupError, subprocess.TimeoutExpired):
+        pass
+    if process.poll() is not None:
+        return
+    try:
+        os.killpg(os.getpgid(process.pid), signal.SIGTERM)
+        process.wait(timeout=10)
+        return
+    except (ProcessLookupError, subprocess.TimeoutExpired):
+        pass
+    if process.poll() is None:
+        os.killpg(os.getpgid(process.pid), signal.SIGKILL)
+        process.wait(timeout=5)
+
+
+def read_log_tail(log_path: Path, chars: int = 8000) -> str:
+    if not log_path.exists():
+        return ""
+    return log_path.read_text(encoding="utf-8", errors="replace")[-chars:]
+
+
+def assert_no_launch_failures(log_path: Path) -> None:
+    log = log_path.read_text(encoding="utf-8", errors="replace") if log_path.exists() else ""
+    shutdown_marker = "[WARNING] [launch]: user interrupted with ctrl-c (SIGINT)"
+    checked_log = log.split(shutdown_marker, 1)[0]
+    failures = [pattern for pattern in LOG_FAILURE_PATTERNS if pattern in checked_log]
+    if failures:
+        pytest.fail(f"Launch log contains failure patterns: {', '.join(failures)}\n{checked_log[-8000:]}")
+
+
+def check_process(process: subprocess.Popen, log_path: Path) -> None:
+    if process.poll() is not None:
+        pytest.fail(f"Simulation exited early with code {process.returncode}\n{read_log_tail(log_path)}")
+
+
+def normalize_angle(angle: float) -> float:
+    return math.atan2(math.sin(angle), math.cos(angle))
+
+
+def yaw_to_quaternion(yaw: float):
+    return (0.0, 0.0, math.sin(yaw / 2.0), math.cos(yaw / 2.0))
+
+
+def quaternion_to_yaw(q) -> float:
+    return math.atan2(2.0 * (q.w * q.z + q.x * q.y), 1.0 - 2.0 * (q.y * q.y + q.z * q.z))
+
+
+def is_finite_odom(msg: Odometry) -> bool:
+    pose = msg.pose.pose
+    twist = msg.twist.twist
+    values = (
+        pose.position.x,
+        pose.position.y,
+        pose.position.z,
+        pose.orientation.x,
+        pose.orientation.y,
+        pose.orientation.z,
+        pose.orientation.w,
+        twist.linear.x,
+        twist.linear.y,
+        twist.angular.z,
+    )
+    return all(math.isfinite(value) for value in values)
+
+
+class GpsDegradedTestNode(Node):
+    def __init__(self):
+        super().__init__(
+            "gps_degraded_e2e_test",
+            parameter_overrides=[Parameter("use_sim_time", Parameter.Type.BOOL, True)],
+            automatically_declare_parameters_from_overrides=True,
+        )
+        self.clock_received = False
+        self.map_grid_received = False
+        self.mowing_map_received = False
+        self.diff_drive_odom_received = False
+        self.gps_fix_received = False
+        self.gps_odom_received = False
+        self.filtered_odom = None
+        self.wheel_odom = None
+        self.heading_source = ""
+        self.heading_validated = False
+        self.last_heading_sigma = math.inf
+        self.degraded_fix_count = 0
+        self.max_gps_covariance_xy = 0.0
+        self.path_points = []
+        self.record_path = False
+        self.navigate_feedback = None
+        self.spin_feedback = None
+
+        self.tf_buffer = tf2_ros.Buffer()
+        self.tf_listener = tf2_ros.TransformListener(self.tf_buffer, self)
+        self.navigate_client = ActionClient(self, NavigateToPose, "/navigate_to_pose")
+        self.spin_client = ActionClient(self, Spin, "/spin")
+        map_qos = QoSProfile(
+            depth=1,
+            durability=DurabilityPolicy.TRANSIENT_LOCAL,
+            reliability=ReliabilityPolicy.RELIABLE,
+        )
+
+        self.create_subscription(Clock, "/clock", self._clock_callback, 10)
+        self.create_subscription(OccupancyGrid, "/map_grid", self._map_grid_callback, map_qos)
+        self.create_subscription(Map, "/mowing_map", self._mowing_map_callback, map_qos)
+        self.create_subscription(Odometry, "/diff_drive_base_controller/odom", self._diff_odom_callback, 10)
+        self.create_subscription(NavSatFix, "/gps/fix", self._gps_fix_callback, 10)
+        self.create_subscription(Odometry, "/gps/odom", self._gps_odom_callback, 10)
+        self.create_subscription(Odometry, "/fusion/odom", self._odom_callback, 10)
+        self.create_subscription(DiagnosticArray, "/diagnostics", self._diagnostics_callback, 10)
+
+    def _clock_callback(self, _msg: Clock) -> None:
+        self.clock_received = True
+
+    def _map_grid_callback(self, _msg: OccupancyGrid) -> None:
+        self.map_grid_received = True
+
+    def _mowing_map_callback(self, _msg: Map) -> None:
+        self.mowing_map_received = True
+
+    def _diff_odom_callback(self, msg: Odometry) -> None:
+        if is_finite_odom(msg):
+            self.diff_drive_odom_received = True
+            self.wheel_odom = msg
+
+    def _gps_fix_callback(self, msg: NavSatFix) -> None:
+        self.gps_fix_received = True
+        covariance_xy = max(msg.position_covariance[0], msg.position_covariance[4])
+        self.max_gps_covariance_xy = max(self.max_gps_covariance_xy, covariance_xy)
+        if msg.position_covariance_type == NavSatFix.COVARIANCE_TYPE_KNOWN and covariance_xy >= DEGRADED_COVARIANCE_THRESHOLD:
+            self.degraded_fix_count += 1
+
+    def _gps_odom_callback(self, msg: Odometry) -> None:
+        if is_finite_odom(msg):
+            self.gps_odom_received = True
+
+    def _odom_callback(self, msg: Odometry) -> None:
+        self.filtered_odom = msg
+        if self.record_path and is_finite_odom(msg):
+            point = (msg.pose.pose.position.x, msg.pose.pose.position.y)
+            if not self.path_points or math.hypot(point[0] - self.path_points[-1][0], point[1] - self.path_points[-1][1]) >= 0.02:
+                self.path_points.append(point)
+
+    def _diagnostics_callback(self, msg: DiagnosticArray) -> None:
+        for status in msg.status:
+            if status.name != "fusioncore: Filter":
+                continue
+            values = {item.key: item.value for item in status.values}
+            self.heading_source = values.get("heading_source", self.heading_source)
+            self.heading_validated = values.get("heading_validated") == "true"
+            try:
+                self.last_heading_sigma = float(values.get("last_heading_sigma_rad", "inf"))
+            except ValueError:
+                self.last_heading_sigma = math.inf
+
+    def has_ready_state(self) -> bool:
+        if not self.clock_received or not self.map_grid_received or not self.mowing_map_received:
+            return False
+        if not self.diff_drive_odom_received or not self.gps_fix_received or not self.gps_odom_received:
+            return False
+        if self.filtered_odom is None or self.wheel_odom is None:
+            return False
+        if not is_finite_odom(self.filtered_odom) or not is_finite_odom(self.wheel_odom):
+            return False
+        if not self.has_bounded_filtered_pose():
+            return False
+        return self.has_map_to_base_link_tf()
+
+    def has_map_to_base_link_tf(self) -> bool:
+        try:
+            return self.tf_buffer.can_transform("map", "base_link", Time(), timeout=Duration(seconds=0.1))
+        except Exception:
+            return False
+
+    def has_bounded_filtered_pose(self) -> bool:
+        if self.filtered_odom is None:
+            return False
+        position = self.filtered_odom.pose.pose.position
+        return LOCALIZATION_MIN_X <= position.x <= LOCALIZATION_MAX_X and LOCALIZATION_MIN_Y <= position.y <= LOCALIZATION_MAX_Y
+
+    def current_snapshot(self) -> OdomSnapshot:
+        assert self.filtered_odom is not None
+        assert self.wheel_odom is not None
+        fusion_pose = self.filtered_odom.pose.pose
+        wheel_pose = self.wheel_odom.pose.pose
+        return OdomSnapshot(
+            fusion_x=fusion_pose.position.x,
+            fusion_y=fusion_pose.position.y,
+            fusion_yaw=quaternion_to_yaw(fusion_pose.orientation),
+            wheel_yaw=quaternion_to_yaw(wheel_pose.orientation),
+        )
+
+    def reset_degraded_observations(self) -> None:
+        self.degraded_fix_count = 0
+        self.max_gps_covariance_xy = 0.0
+        self.path_points = []
+
+    def describe_state(self) -> str:
+        if self.filtered_odom is None:
+            fusion_pose = "None"
+        else:
+            position = self.filtered_odom.pose.pose.position
+            yaw = quaternion_to_yaw(self.filtered_odom.pose.pose.orientation)
+            fusion_pose = f"({position.x:.3f}, {position.y:.3f}, yaw={yaw:.3f})"
+        if self.wheel_odom is None:
+            wheel_pose = "None"
+        else:
+            position = self.wheel_odom.pose.pose.position
+            yaw = quaternion_to_yaw(self.wheel_odom.pose.pose.orientation)
+            wheel_pose = f"({position.x:.3f}, {position.y:.3f}, yaw={yaw:.3f})"
+        return (
+            "state: "
+            f"test_time={self.get_clock().now().nanoseconds / 1e9:.3f}, "
+            f"clock={self.clock_received}, map_grid={self.map_grid_received}, mowing_map={self.mowing_map_received}, "
+            f"diff_drive_odom={self.diff_drive_odom_received}, gps_fix={self.gps_fix_received}, "
+            f"gps_odom={self.gps_odom_received}, fusion_pose={fusion_pose}, wheel_pose={wheel_pose}, "
+            f"map_to_base_link_tf={self.has_map_to_base_link_tf()}, heading_source={self.heading_source}, "
+            f"heading_validated={self.heading_validated}, last_heading_sigma={self.last_heading_sigma:.3f}, "
+            f"degraded_fix_count={self.degraded_fix_count}, max_gps_covariance_xy={self.max_gps_covariance_xy:.3f}, "
+            f"path_points={len(self.path_points)}"
+        )
+
+
+def spin_until(node, predicate, timeout: float, failure_message: str, process, log_path: Path) -> None:
+    deadline = time.monotonic() + timeout
+    while time.monotonic() < deadline:
+        check_process(process, log_path)
+        if predicate():
+            return
+        rclpy.spin_once(node, timeout_sec=0.1)
+    pytest.fail(f"{failure_message}\n{node.describe_state()}\n{read_log_tail(log_path)}")
+
+
+def spin_for(node, seconds: float, process, log_path: Path) -> None:
+    deadline = time.monotonic() + seconds
+    while time.monotonic() < deadline:
+        check_process(process, log_path)
+        rclpy.spin_once(node, timeout_sec=0.1)
+
+
+def wait_for_future(node, future, timeout: float, label: str, process, log_path: Path):
+    deadline = time.monotonic() + timeout
+    while time.monotonic() < deadline:
+        check_process(process, log_path)
+        if future.done():
+            return future.result()
+        rclpy.spin_once(node, timeout_sec=0.1)
+    pytest.fail(f"Timed out waiting for {label}\n{node.describe_state()}\n{read_log_tail(log_path)}")
+
+
+def wait_for_action_server(node, action_client, name: str, timeout: float, process, log_path: Path) -> None:
+    deadline = time.monotonic() + timeout
+    while time.monotonic() < deadline:
+        check_process(process, log_path)
+        if action_client.wait_for_server(timeout_sec=1.0):
+            return
+        rclpy.spin_once(node, timeout_sec=0.1)
+    pytest.fail(f"Timed out waiting for action server {name}\n{read_log_tail(log_path)}")
+
+
+def wait_for_lifecycle_nodes_active(node, names: tuple[str, ...], timeout: float, process, log_path: Path) -> None:
+    clients = {name: node.create_client(GetState, f"/{name}/get_state") for name in names}
+    states = {name: "unknown" for name in names}
+    deadline = time.monotonic() + timeout
+
+    while time.monotonic() < deadline:
+        check_process(process, log_path)
+        all_active = True
+
+        for name, client in clients.items():
+            if not client.service_is_ready():
+                client.wait_for_service(timeout_sec=0.1)
+            if not client.service_is_ready():
+                states[name] = "service_unavailable"
+                all_active = False
+                continue
+
+            future = client.call_async(GetState.Request())
+            query_deadline = time.monotonic() + 2.0
+            while time.monotonic() < query_deadline:
+                check_process(process, log_path)
+                if future.done():
+                    break
+                rclpy.spin_once(node, timeout_sec=0.1)
+            if not future.done():
+                states[name] = "state_query_timeout"
+                all_active = False
+                continue
+
+            response = future.result()
+            states[name] = response.current_state.label or str(response.current_state.id)
+            if response.current_state.id != State.PRIMARY_STATE_ACTIVE:
+                all_active = False
+
+        if all_active:
+            return
+        spin_for(node, 0.5, process, log_path)
+
+    pytest.fail(
+        f"Timed out waiting for Nav2 lifecycle nodes to become active: {states}\n"
+        f"{node.describe_state()}\n{read_log_tail(log_path)}"
+    )
+
+
+def send_spin_goal(node: GpsDegradedTestNode, process, log_path: Path) -> None:
+    goal = Spin.Goal()
+    goal.target_yaw = math.pi / 2.0
+    goal.time_allowance.sec = 30
+
+    goal_future = node.spin_client.send_goal_async(
+        goal, feedback_callback=lambda feedback: setattr(node, "spin_feedback", feedback.feedback)
+    )
+    goal_handle = wait_for_future(node, goal_future, 15.0, "Spin goal response", process, log_path)
+    assert goal_handle.accepted, f"Spin goal was rejected\n{node.describe_state()}\n{read_log_tail(log_path)}"
+
+    result_response = wait_for_future(node, goal_handle.get_result_async(), 60.0, "Spin result", process, log_path)
+    assert result_response.status == GoalStatus.STATUS_SUCCEEDED, (
+        f"Spin failed with status={result_response.status}, "
+        f"error_code={result_response.result.error_code}, error_msg={result_response.result.error_msg!r}\n"
+        f"{node.describe_state()}\n{read_log_tail(log_path)}"
+    )
+    assert result_response.result.error_code == Spin.Result.NONE, (
+        f"Spin returned error_code={result_response.result.error_code}, "
+        f"error_msg={result_response.result.error_msg!r}\n{read_log_tail(log_path)}"
+    )
+
+
+def send_navigation_goal(
+    node: GpsDegradedTestNode,
+    x: float,
+    y: float,
+    yaw: float,
+    label: str,
+    process,
+    log_path: Path,
+) -> None:
+    goal = NavigateToPose.Goal()
+    goal.pose.header.frame_id = "map"
+    goal.pose.header.stamp = node.get_clock().now().to_msg()
+    goal.pose.pose.position.x = x
+    goal.pose.pose.position.y = y
+    goal.pose.pose.position.z = 0.0
+    qx, qy, qz, qw = yaw_to_quaternion(yaw)
+    goal.pose.pose.orientation.x = qx
+    goal.pose.pose.orientation.y = qy
+    goal.pose.pose.orientation.z = qz
+    goal.pose.pose.orientation.w = qw
+
+    goal_future = node.navigate_client.send_goal_async(
+        goal, feedback_callback=lambda feedback: setattr(node, "navigate_feedback", feedback.feedback)
+    )
+    goal_handle = wait_for_future(node, goal_future, 15.0, f"{label} goal response", process, log_path)
+    assert goal_handle.accepted, f"{label} was rejected\n{node.describe_state()}\n{read_log_tail(log_path)}"
+
+    result_response = wait_for_future(node, goal_handle.get_result_async(), 140.0, f"{label} result", process, log_path)
+    assert result_response.status == GoalStatus.STATUS_SUCCEEDED, (
+        f"{label} failed with status={result_response.status}, "
+        f"error_code={result_response.result.error_code}, error_msg={result_response.result.error_msg!r}\n"
+        f"{node.describe_state()}\n{read_log_tail(log_path)}"
+    )
+    assert result_response.result.error_code == NavigateToPose.Result.NONE, (
+        f"{label} returned error_code={result_response.result.error_code}, "
+        f"error_msg={result_response.result.error_msg!r}\n{read_log_tail(log_path)}"
+    )
+    spin_for(node, 0.8, process, log_path)
+
+
+def assert_segment_yaw_consistent(start: OdomSnapshot, end: OdomSnapshot, label: str) -> None:
+    fusion_delta = normalize_angle(end.fusion_yaw - start.fusion_yaw)
+    wheel_delta = normalize_angle(end.wheel_yaw - start.wheel_yaw)
+    error = abs(normalize_angle(fusion_delta - wheel_delta))
+    assert error < MAX_SEGMENT_YAW_DELTA_ERROR, (
+        f"{label} yaw delta diverged in degraded GPS: "
+        f"fusion_delta={fusion_delta:.3f}, wheel_delta={wheel_delta:.3f}, error={error:.3f}"
+    )
+
+
+def assert_path_entered_degraded_zone(points) -> None:
+    assert len(points) >= 6, f"Too few path points to assess degraded-zone route: {len(points)}"
+    min_distance = min(math.hypot(x - ZONE_X, y - ZONE_Y) for x, y in points)
+    assert min_distance < ZONE_RADIUS * 0.65, (
+        f"Recorded fusion path did not enter degraded GPS zone deeply enough: min_distance={min_distance:.3f}"
+    )
+
+
+def test_gps_degraded_two_paths_with_turns(tmp_path):
+    log_path = tmp_path / "gps_degraded.log"
+    env = prepare_env()
+    world_path = create_degraded_world(tmp_path)
+    previous_ros_domain_id = os.environ.get("ROS_DOMAIN_ID")
+    os.environ["ROS_DOMAIN_ID"] = env["ROS_DOMAIN_ID"]
+
+    process, log_file = start_simulation(log_path, env, world_path)
+    rclpy.init()
+    node = GpsDegradedTestNode()
+
+    try:
+        spin_until(
+            node,
+            node.has_ready_state,
+            120.0,
+            "Simulation did not reach ready state for degraded GPS test",
+            process,
+            log_path,
+        )
+        wait_for_lifecycle_nodes_active(node, NAV2_LIFECYCLE_NODES, 60.0, process, log_path)
+        wait_for_action_server(node, node.spin_client, "/spin", 60.0, process, log_path)
+        wait_for_action_server(node, node.navigate_client, "/navigate_to_pose", 60.0, process, log_path)
+
+        send_spin_goal(node, process, log_path)
+        spin_until(
+            node,
+            lambda: node.heading_validated and node.heading_source == "GPS_ROTATION",
+            20.0,
+            "FusionCore did not validate heading from GPS rotation before degraded route",
+            process,
+            log_path,
+        )
+
+        node.reset_degraded_observations()
+        node.record_path = True
+        route_start = node.current_snapshot()
+        send_navigation_goal(node, GOAL_1_X, GOAL_1_Y, GOAL_1_YAW, "first degraded path", process, log_path)
+        after_first_path = node.current_snapshot()
+        send_navigation_goal(node, GOAL_2_X, GOAL_2_Y, GOAL_2_YAW, "second degraded path", process, log_path)
+        route_end = node.current_snapshot()
+        node.record_path = False
+
+        assert node.degraded_fix_count >= 5, (
+            f"Expected degraded GPS fixes during route, got {node.degraded_fix_count}; "
+            f"max covariance xy={node.max_gps_covariance_xy:.3f}\n{node.describe_state()}"
+        )
+        assert_path_entered_degraded_zone(node.path_points)
+        assert_segment_yaw_consistent(route_start, after_first_path, "first path")
+        assert_segment_yaw_consistent(after_first_path, route_end, "second path and in-zone turn")
+
+        final_distance = math.hypot(route_end.fusion_x - GOAL_2_X, route_end.fusion_y - GOAL_2_Y)
+        assert final_distance < FINAL_GOAL_TOLERANCE, (
+            f"Final fusion pose too far from second goal: distance={final_distance:.3f}\n{node.describe_state()}"
+        )
+        assert node.heading_validated, f"Heading validation was lost\n{node.describe_state()}"
+        assert process.poll() is None, f"Simulation exited early with code {process.returncode}"
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+        stop_process(process)
+        log_file.close()
+        if previous_ros_domain_id is None:
+            os.environ.pop("ROS_DOMAIN_ID", None)
+        else:
+            os.environ["ROS_DOMAIN_ID"] = previous_ros_domain_id
+
+    assert_no_launch_failures(log_path)
diff --git a/test/integration/test_gps_rotation_heading.py b/test/integration/test_gps_rotation_heading.py
new file mode 100644
index 0000000..1a9986f
--- /dev/null
+++ b/test/integration/test_gps_rotation_heading.py
@@ -0,0 +1,575 @@
+import math
+import os
+import random
+import signal
+import subprocess
+import time
+from pathlib import Path
+
+import pytest
+import rclpy
+import tf2_ros
+from action_msgs.msg import GoalStatus
+from diagnostic_msgs.msg import DiagnosticArray
+from lifecycle_msgs.msg import State
+from lifecycle_msgs.srv import GetState
+from nav2_msgs.action import NavigateToPose, Spin
+from nav_msgs.msg import OccupancyGrid, Odometry
+from open_mower_next.msg import Map
+from rclpy.action import ActionClient
+from rclpy.duration import Duration
+from rclpy.node import Node
+from rclpy.parameter import Parameter
+from rclpy.qos import DurabilityPolicy, QoSProfile, ReliabilityPolicy
+from rclpy.time import Time
+from rosgraph_msgs.msg import Clock
+from sensor_msgs.msg import NavSatFix
+
+
+GOAL_X = -0.2
+GOAL_Y = 0.8
+GOAL_YAW = 0.0
+GOAL_TOLERANCE = 0.3
+LOCALIZATION_MIN_X = -2.0
+LOCALIZATION_MAX_X = 4.5
+LOCALIZATION_MIN_Y = -2.0
+LOCALIZATION_MAX_Y = 3.5
+NAV2_LIFECYCLE_NODES = (
+    "controller_server",
+    "planner_server",
+    "behavior_server",
+    "bt_navigator",
+    "velocity_smoother",
+    "docking_server",
+    "smoother_server",
+)
+
+LOG_FAILURE_PATTERNS = (
+    "[FATAL]",
+    "Could not load library",
+    "Could not listen to extern controllers",
+    "Cannot shutdown a ROS adapter",
+    "Webots driver exited unexpectedly",
+    "process has died",
+    "Traceback (most recent call last)",
+)
+
+
+def repo_root() -> Path:
+    return Path(__file__).resolve().parents[2]
+
+
+def prepare_env() -> dict[str, str]:
+    root = repo_root()
+    env = os.environ.copy()
+    env["ROS_DOMAIN_ID"] = str(150 + (os.getpid() % 50))
+    env["WEBOTS_PORT"] = str(14000 + (os.getpid() % 1000))
+    env["WEBOTS_OFFSCREEN"] = "1"
+    env["OM_DATUM_LAT"] = "-22.9"
+    env["OM_DATUM_LONG"] = "-43.2"
+    env["OM_MAP_PATH"] = str(root / "test" / "integration" / "navigation_docking_map.geojson")
+
+    if not env.get("WEBOTS_HOME"):
+        default_webots_home = Path.home() / ".ros" / "webotsR2025a" / "webots"
+        if default_webots_home.is_dir():
+            env["WEBOTS_HOME"] = str(default_webots_home)
+
+    if not env.get("WEBOTS_HOME") or not Path(env["WEBOTS_HOME"]).is_dir():
+        pytest.fail("WEBOTS_HOME must point to a Webots R2025a installation for integration tests")
+
+    return env
+
+
+def create_randomized_world(tmp_path: Path):
+    seed = int(os.environ.get("OPEN_MOWER_NEXT_GPS_ROTATION_TEST_SEED", "7"))
+    rng = random.Random(seed)
+
+    start_x = 1.5
+    start_y = -0.4
+    for _ in range(100):
+        candidate_x = rng.uniform(0.9, 2.2)
+        candidate_y = rng.uniform(-0.8, 0.2)
+        distance = math.hypot(candidate_x - GOAL_X, candidate_y - GOAL_Y)
+        if 1.4 <= distance <= 2.8:
+            start_x = candidate_x
+            start_y = candidate_y
+            break
+    start_yaw = rng.uniform(-math.pi, math.pi)
+
+    root = repo_root()
+    world_path = tmp_path / f"gps_rotation_heading_{seed}.wbt"
+    content = (root / "worlds" / "simple_lawn.wbt").read_text(encoding="utf-8")
+    content = content.replace(
+        'EXTERNPROTO "../protos/OpenMower.proto"',
+        f'EXTERNPROTO "{(root / "protos" / "OpenMower.proto").as_posix()}"',
+    )
+    content = content.replace(
+        'EXTERNPROTO "../protos/DockingStation.proto"',
+        f'EXTERNPROTO "{(root / "protos" / "DockingStation.proto").as_posix()}"',
+    )
+    content = content.replace(
+        'title "OpenMowerNext simple lawn"',
+        f'title "OpenMowerNext GPS rotation heading test seed {seed}"',
+        1,
+    )
+    content = content.replace(
+        """OpenMower {
+  translation 0 0 0.0925
+  rotation 0 0 1 0
+  name "openmower"
+  controller "<extern>"
+  supervisor TRUE
+}""",
+        f"""OpenMower {{
+  translation {start_x:.6f} {start_y:.6f} 0.0925
+  rotation 0 0 1 {start_yaw:.9f}
+  name "openmower"
+  controller "<extern>"
+  supervisor TRUE
+}}""",
+        1,
+    )
+    world_path.write_text(content, encoding="utf-8")
+    return world_path, (start_x, start_y, start_yaw), seed
+
+
+def start_simulation(log_path: Path, env: dict[str, str], world_path: Path):
+    log_file = log_path.open("w", encoding="utf-8")
+    process = subprocess.Popen(
+        [
+            "ros2",
+            "launch",
+            "open_mower_next",
+            "sim.launch.py",
+            "gui:=false",
+            "mode:=realtime",
+            "enable_foxglove:=false",
+            f"webots_port:={env['WEBOTS_PORT']}",
+            f"world:={world_path}",
+        ],
+        stdout=log_file,
+        stderr=subprocess.STDOUT,
+        env=env,
+        start_new_session=True,
+        text=True,
+    )
+    return process, log_file
+
+
+def stop_process(process: subprocess.Popen) -> None:
+    if process.poll() is not None:
+        return
+    try:
+        os.killpg(os.getpgid(process.pid), signal.SIGINT)
+        process.wait(timeout=15)
+        return
+    except (ProcessLookupError, subprocess.TimeoutExpired):
+        pass
+    if process.poll() is not None:
+        return
+    try:
+        os.killpg(os.getpgid(process.pid), signal.SIGTERM)
+        process.wait(timeout=10)
+        return
+    except (ProcessLookupError, subprocess.TimeoutExpired):
+        pass
+    if process.poll() is None:
+        os.killpg(os.getpgid(process.pid), signal.SIGKILL)
+        process.wait(timeout=5)
+
+
+def read_log_tail(log_path: Path, chars: int = 8000) -> str:
+    if not log_path.exists():
+        return ""
+    return log_path.read_text(encoding="utf-8", errors="replace")[-chars:]
+
+
+def assert_no_launch_failures(log_path: Path) -> None:
+    log = log_path.read_text(encoding="utf-8", errors="replace") if log_path.exists() else ""
+    shutdown_marker = "[WARNING] [launch]: user interrupted with ctrl-c (SIGINT)"
+    checked_log = log.split(shutdown_marker, 1)[0]
+    failures = [pattern for pattern in LOG_FAILURE_PATTERNS if pattern in checked_log]
+    if failures:
+        pytest.fail(f"Launch log contains failure patterns: {', '.join(failures)}\n{checked_log[-8000:]}")
+
+
+def check_process(process: subprocess.Popen, log_path: Path) -> None:
+    if process.poll() is not None:
+        pytest.fail(f"Simulation exited early with code {process.returncode}\n{read_log_tail(log_path)}")
+
+
+def yaw_to_quaternion(yaw: float):
+    return (0.0, 0.0, math.sin(yaw / 2.0), math.cos(yaw / 2.0))
+
+
+def quaternion_to_yaw(q) -> float:
+    return math.atan2(2.0 * (q.w * q.z + q.x * q.y), 1.0 - 2.0 * (q.y * q.y + q.z * q.z))
+
+
+def is_finite_odom(msg: Odometry) -> bool:
+    pose = msg.pose.pose
+    twist = msg.twist.twist
+    values = (
+        pose.position.x,
+        pose.position.y,
+        pose.position.z,
+        pose.orientation.x,
+        pose.orientation.y,
+        pose.orientation.z,
+        pose.orientation.w,
+        twist.linear.x,
+        twist.linear.y,
+        twist.angular.z,
+    )
+    return all(math.isfinite(value) for value in values)
+
+
+class GpsRotationHeadingTestNode(Node):
+    def __init__(self):
+        super().__init__(
+            "gps_rotation_heading_e2e_test",
+            parameter_overrides=[Parameter("use_sim_time", Parameter.Type.BOOL, True)],
+            automatically_declare_parameters_from_overrides=True,
+        )
+        self.clock_received = False
+        self.map_grid_received = False
+        self.mowing_map_received = False
+        self.diff_drive_odom_received = False
+        self.gps_fix_received = False
+        self.gps_odom_received = False
+        self.filtered_odom = None
+        self.heading_source = ""
+        self.heading_validated = False
+        self.last_heading_sigma = math.inf
+        self.record_path = False
+        self.path_points = []
+        self.navigate_feedback = None
+        self.spin_feedback = None
+
+        self.tf_buffer = tf2_ros.Buffer()
+        self.tf_listener = tf2_ros.TransformListener(self.tf_buffer, self)
+        self.navigate_client = ActionClient(self, NavigateToPose, "/navigate_to_pose")
+        self.spin_client = ActionClient(self, Spin, "/spin")
+        map_qos = QoSProfile(
+            depth=1,
+            durability=DurabilityPolicy.TRANSIENT_LOCAL,
+            reliability=ReliabilityPolicy.RELIABLE,
+        )
+
+        self.create_subscription(Clock, "/clock", self._clock_callback, 10)
+        self.create_subscription(OccupancyGrid, "/map_grid", self._map_grid_callback, map_qos)
+        self.create_subscription(Map, "/mowing_map", self._mowing_map_callback, map_qos)
+        self.create_subscription(Odometry, "/diff_drive_base_controller/odom", self._diff_odom_callback, 10)
+        self.create_subscription(NavSatFix, "/gps/fix", self._gps_fix_callback, 10)
+        self.create_subscription(Odometry, "/gps/odom", self._gps_odom_callback, 10)
+        self.create_subscription(Odometry, "/fusion/odom", self._odom_callback, 10)
+        self.create_subscription(DiagnosticArray, "/diagnostics", self._diagnostics_callback, 10)
+
+    def _clock_callback(self, _msg: Clock) -> None:
+        self.clock_received = True
+
+    def _map_grid_callback(self, _msg: OccupancyGrid) -> None:
+        self.map_grid_received = True
+
+    def _mowing_map_callback(self, _msg: Map) -> None:
+        self.mowing_map_received = True
+
+    def _diff_odom_callback(self, _msg: Odometry) -> None:
+        self.diff_drive_odom_received = True
+
+    def _gps_fix_callback(self, _msg: NavSatFix) -> None:
+        self.gps_fix_received = True
+
+    def _gps_odom_callback(self, msg: Odometry) -> None:
+        if is_finite_odom(msg):
+            self.gps_odom_received = True
+
+    def _odom_callback(self, msg: Odometry) -> None:
+        self.filtered_odom = msg
+        if self.record_path and is_finite_odom(msg):
+            point = (msg.pose.pose.position.x, msg.pose.pose.position.y)
+            if not self.path_points or math.hypot(
+                point[0] - self.path_points[-1][0], point[1] - self.path_points[-1][1]
+            ) >= 0.02:
+                self.path_points.append(point)
+
+    def _diagnostics_callback(self, msg: DiagnosticArray) -> None:
+        for status in msg.status:
+            if status.name != "fusioncore: Filter":
+                continue
+            values = {item.key: item.value for item in status.values}
+            self.heading_source = values.get("heading_source", self.heading_source)
+            self.heading_validated = values.get("heading_validated") == "true"
+            try:
+                self.last_heading_sigma = float(values.get("last_heading_sigma_rad", "inf"))
+            except ValueError:
+                self.last_heading_sigma = math.inf
+
+    def has_ready_state(self) -> bool:
+        if not self.clock_received or not self.map_grid_received or not self.mowing_map_received:
+            return False
+        if not self.diff_drive_odom_received or not self.gps_fix_received or not self.gps_odom_received:
+            return False
+        if self.filtered_odom is None or not is_finite_odom(self.filtered_odom):
+            return False
+        if not self.has_bounded_filtered_pose():
+            return False
+        return self.has_map_to_base_link_tf()
+
+    def has_map_to_base_link_tf(self) -> bool:
+        try:
+            return self.tf_buffer.can_transform("map", "base_link", Time(), timeout=Duration(seconds=0.1))
+        except Exception:
+            return False
+
+    def has_bounded_filtered_pose(self) -> bool:
+        if self.filtered_odom is None:
+            return False
+        position = self.filtered_odom.pose.pose.position
+        return LOCALIZATION_MIN_X <= position.x <= LOCALIZATION_MAX_X and LOCALIZATION_MIN_Y <= position.y <= LOCALIZATION_MAX_Y
+
+    def current_pose_xy(self):
+        if self.filtered_odom is None:
+            return None
+        position = self.filtered_odom.pose.pose.position
+        return (position.x, position.y)
+
+    def describe_state(self) -> str:
+        if self.filtered_odom is None:
+            odom_pose = "None"
+        else:
+            position = self.filtered_odom.pose.pose.position
+            yaw = quaternion_to_yaw(self.filtered_odom.pose.pose.orientation)
+            odom_pose = f"({position.x:.3f}, {position.y:.3f}, yaw={yaw:.3f})"
+        return (
+            "state: "
+            f"test_time={self.get_clock().now().nanoseconds / 1e9:.3f}, "
+            f"clock={self.clock_received}, map_grid={self.map_grid_received}, "
+            f"mowing_map={self.mowing_map_received}, diff_drive_odom={self.diff_drive_odom_received}, "
+            f"gps_fix={self.gps_fix_received}, gps_odom={self.gps_odom_received}, "
+            f"filtered_pose={odom_pose}, map_to_base_link_tf={self.has_map_to_base_link_tf()}, "
+            f"heading_source={self.heading_source}, heading_validated={self.heading_validated}, "
+            f"last_heading_sigma={self.last_heading_sigma:.3f}, path_points={len(self.path_points)}"
+        )
+
+
+def spin_until(node, predicate, timeout: float, failure_message: str, process, log_path: Path) -> None:
+    deadline = time.monotonic() + timeout
+    while time.monotonic() < deadline:
+        check_process(process, log_path)
+        if predicate():
+            return
+        rclpy.spin_once(node, timeout_sec=0.1)
+    pytest.fail(f"{failure_message}\n{node.describe_state()}\n{read_log_tail(log_path)}")
+
+
+def spin_for(node, seconds: float, process, log_path: Path) -> None:
+    deadline = time.monotonic() + seconds
+    while time.monotonic() < deadline:
+        check_process(process, log_path)
+        rclpy.spin_once(node, timeout_sec=0.1)
+
+
+def wait_for_future(node, future, timeout: float, label: str, process, log_path: Path):
+    deadline = time.monotonic() + timeout
+    while time.monotonic() < deadline:
+        check_process(process, log_path)
+        if future.done():
+            return future.result()
+        rclpy.spin_once(node, timeout_sec=0.1)
+    pytest.fail(f"Timed out waiting for {label}\n{node.describe_state()}\n{read_log_tail(log_path)}")
+
+
+def wait_for_action_server(node, action_client, name: str, timeout: float, process, log_path: Path) -> None:
+    deadline = time.monotonic() + timeout
+    while time.monotonic() < deadline:
+        check_process(process, log_path)
+        if action_client.wait_for_server(timeout_sec=1.0):
+            return
+        rclpy.spin_once(node, timeout_sec=0.1)
+    pytest.fail(f"Timed out waiting for action server {name}\n{read_log_tail(log_path)}")
+
+
+def wait_for_lifecycle_nodes_active(node, names: tuple[str, ...], timeout: float, process, log_path: Path) -> None:
+    clients = {name: node.create_client(GetState, f"/{name}/get_state") for name in names}
+    states = {name: "unknown" for name in names}
+    deadline = time.monotonic() + timeout
+
+    while time.monotonic() < deadline:
+        check_process(process, log_path)
+        all_active = True
+
+        for name, client in clients.items():
+            if not client.service_is_ready():
+                client.wait_for_service(timeout_sec=0.1)
+            if not client.service_is_ready():
+                states[name] = "service_unavailable"
+                all_active = False
+                continue
+
+            future = client.call_async(GetState.Request())
+            query_deadline = time.monotonic() + 2.0
+            while time.monotonic() < query_deadline:
+                check_process(process, log_path)
+                if future.done():
+                    break
+                rclpy.spin_once(node, timeout_sec=0.1)
+            if not future.done():
+                states[name] = "state_query_timeout"
+                all_active = False
+                continue
+
+            response = future.result()
+            states[name] = response.current_state.label or str(response.current_state.id)
+            if response.current_state.id != State.PRIMARY_STATE_ACTIVE:
+                all_active = False
+
+        if all_active:
+            return
+        spin_for(node, 0.5, process, log_path)
+
+    pytest.fail(
+        f"Timed out waiting for Nav2 lifecycle nodes to become active: {states}\n"
+        f"{node.describe_state()}\n{read_log_tail(log_path)}"
+    )
+
+
+def send_spin_goal(node: GpsRotationHeadingTestNode, process, log_path: Path) -> None:
+    goal = Spin.Goal()
+    goal.target_yaw = math.pi / 2.0
+    goal.time_allowance.sec = 30
+
+    goal_future = node.spin_client.send_goal_async(
+        goal, feedback_callback=lambda feedback: setattr(node, "spin_feedback", feedback.feedback)
+    )
+    goal_handle = wait_for_future(node, goal_future, 15.0, "Spin goal response", process, log_path)
+    assert goal_handle.accepted, f"Spin goal was rejected\n{node.describe_state()}\n{read_log_tail(log_path)}"
+
+    result_response = wait_for_future(node, goal_handle.get_result_async(), 60.0, "Spin result", process, log_path)
+    assert result_response.status == GoalStatus.STATUS_SUCCEEDED, (
+        f"Spin failed with status={result_response.status}, "
+        f"error_code={result_response.result.error_code}, error_msg={result_response.result.error_msg!r}\n"
+        f"{node.describe_state()}\n{read_log_tail(log_path)}"
+    )
+    assert result_response.result.error_code == Spin.Result.NONE, (
+        f"Spin returned error_code={result_response.result.error_code}, "
+        f"error_msg={result_response.result.error_msg!r}\n{read_log_tail(log_path)}"
+    )
+
+
+def send_navigation_goal(node: GpsRotationHeadingTestNode, process, log_path: Path):
+    start = node.current_pose_xy()
+    assert start is not None, f"No filtered odom before navigation\n{node.describe_state()}"
+    assert math.hypot(start[0] - GOAL_X, start[1] - GOAL_Y) > GOAL_TOLERANCE, (
+        f"Goal starts too close to robot\n{node.describe_state()}"
+    )
+
+    goal = NavigateToPose.Goal()
+    goal.pose.header.frame_id = "map"
+    goal.pose.header.stamp = node.get_clock().now().to_msg()
+    goal.pose.pose.position.x = GOAL_X
+    goal.pose.pose.position.y = GOAL_Y
+    goal.pose.pose.position.z = 0.0
+    qx, qy, qz, qw = yaw_to_quaternion(GOAL_YAW)
+    goal.pose.pose.orientation.x = qx
+    goal.pose.pose.orientation.y = qy
+    goal.pose.pose.orientation.z = qz
+    goal.pose.pose.orientation.w = qw
+
+    node.path_points = [start]
+    node.record_path = True
+    try:
+        goal_future = node.navigate_client.send_goal_async(
+            goal, feedback_callback=lambda feedback: setattr(node, "navigate_feedback", feedback.feedback)
+        )
+        goal_handle = wait_for_future(node, goal_future, 15.0, "NavigateToPose goal response", process, log_path)
+        assert goal_handle.accepted, f"NavigateToPose goal was rejected\n{node.describe_state()}\n{read_log_tail(log_path)}"
+
+        result_response = wait_for_future(node, goal_handle.get_result_async(), 120.0, "NavigateToPose result", process, log_path)
+    finally:
+        node.record_path = False
+
+    assert result_response.status == GoalStatus.STATUS_SUCCEEDED, (
+        f"NavigateToPose failed with status={result_response.status}, "
+        f"error_code={result_response.result.error_code}, error_msg={result_response.result.error_msg!r}\n"
+        f"{node.describe_state()}\n{read_log_tail(log_path)}"
+    )
+    assert result_response.result.error_code == NavigateToPose.Result.NONE, (
+        f"NavigateToPose returned error_code={result_response.result.error_code}, "
+        f"error_msg={result_response.result.error_msg!r}\n{read_log_tail(log_path)}"
+    )
+    spin_for(node, 1.0, process, log_path)
+    return start
+
+
+def assert_path_is_straight(points, start, goal):
+    assert len(points) >= 4, f"Too few path points to assess straightness: {len(points)}"
+    direct = math.hypot(goal[0] - start[0], goal[1] - start[1])
+    assert direct > 0.5, f"Direct path too short: {direct:.3f}"
+
+    path_length = 0.0
+    for p0, p1 in zip(points, points[1:]):
+        path_length += math.hypot(p1[0] - p0[0], p1[1] - p0[1])
+
+    dx = goal[0] - start[0]
+    dy = goal[1] - start[1]
+    cross_track = [abs(dx * (p[1] - start[1]) - dy * (p[0] - start[0])) / direct for p in points]
+    rms_cross_track = math.sqrt(sum(error * error for error in cross_track) / len(cross_track))
+    max_cross_track = max(cross_track)
+    ratio = path_length / direct
+
+    assert ratio < 1.5, (
+        f"Path is too indirect: path_length={path_length:.3f}, direct={direct:.3f}, ratio={ratio:.3f}"
+    )
+    assert rms_cross_track < 0.35, f"RMS cross-track error too high: {rms_cross_track:.3f}"
+    assert max_cross_track < 0.65, f"Max cross-track error too high: {max_cross_track:.3f}"
+
+
+def test_gps_rotation_heading_spin_then_straight_navigation(tmp_path):
+    log_path = tmp_path / "gps_rotation_heading.log"
+    env = prepare_env()
+    world_path, start_pose, seed = create_randomized_world(tmp_path)
+    previous_ros_domain_id = os.environ.get("ROS_DOMAIN_ID")
+    os.environ["ROS_DOMAIN_ID"] = env["ROS_DOMAIN_ID"]
+
+    process, log_file = start_simulation(log_path, env, world_path)
+    rclpy.init()
+    node = GpsRotationHeadingTestNode()
+
+    try:
+        spin_until(
+            node,
+            node.has_ready_state,
+            120.0,
+            f"Simulation did not reach ready state for seed={seed}, start_pose={start_pose}",
+            process,
+            log_path,
+        )
+        wait_for_lifecycle_nodes_active(node, NAV2_LIFECYCLE_NODES, 60.0, process, log_path)
+        wait_for_action_server(node, node.spin_client, "/spin", 60.0, process, log_path)
+        wait_for_action_server(node, node.navigate_client, "/navigate_to_pose", 60.0, process, log_path)
+
+        send_spin_goal(node, process, log_path)
+        spin_until(
+            node,
+            lambda: node.heading_validated and node.heading_source == "GPS_ROTATION",
+            20.0,
+            "FusionCore did not validate heading from GPS rotation after Nav2 Spin",
+            process,
+            log_path,
+        )
+
+        navigation_start = send_navigation_goal(node, process, log_path)
+        assert_path_is_straight(node.path_points, navigation_start, (GOAL_X, GOAL_Y))
+        assert process.poll() is None, f"Simulation exited early with code {process.returncode}"
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+        stop_process(process)
+        log_file.close()
+        if previous_ros_domain_id is None:
+            os.environ.pop("ROS_DOMAIN_ID", None)
+        else:
+            os.environ["ROS_DOMAIN_ID"] = previous_ros_domain_id
+
+    assert_no_launch_failures(log_path)
diff --git a/test/integration/test_navigation_docking.py b/test/integration/test_navigation_docking.py
index 33c6327..8242b80 100644
--- a/test/integration/test_navigation_docking.py
+++ b/test/integration/test_navigation_docking.py
@@ -13,6 +13,7 @@
 from lifecycle_msgs.srv import GetState
 from nav2_msgs.action import NavigateToPose
 from nav_msgs.msg import OccupancyGrid, Odometry
+from omros2_firmware_msgs.msg import PowerStatus
 from open_mower_next.action import DockRobotNearest
 from open_mower_next.msg import Map
 from rclpy.action import ActionClient
@@ -22,7 +23,7 @@
 from rclpy.qos import DurabilityPolicy, QoSProfile, ReliabilityPolicy
 from rclpy.time import Time
 from rosgraph_msgs.msg import Clock
-from std_msgs.msg import Bool, Float32
+from sensor_msgs.msg import NavSatFix
 
 
 NAVIGATION_GOAL_X = -0.2
@@ -53,6 +54,25 @@
     "Traceback (most recent call last)",
 )
 
+ROSBAG_TOPICS = (
+    "/clock",
+    "/tf",
+    "/tf_static",
+    "/fusion/odom",
+    "/diff_drive_base_controller/odom",
+    "/gps/fix",
+    "/gps/fix/speed_vector",
+    "/gps/odom",
+    "/cmd_vel_raw",
+    "/cmd_vel_nav",
+    "/diff_drive_base_controller/cmd_vel",
+    "/power/status",
+    "/dock_pose",
+    "/staging_pose",
+    "/docking_trajectory",
+    "/joint_states",
+)
+
 
 def repo_root() -> Path:
     return Path(__file__).resolve().parents[2]
@@ -62,6 +82,7 @@ def prepare_env() -> dict[str, str]:
     root = repo_root()
     env = os.environ.copy()
     env["ROS_DOMAIN_ID"] = str(100 + (os.getpid() % 100))
+    env["WEBOTS_PORT"] = str(13000 + (os.getpid() % 1000))
     env["WEBOTS_OFFSCREEN"] = "1"
     env["OM_DATUM_LAT"] = "-22.9"
     env["OM_DATUM_LONG"] = "-43.2"
@@ -89,6 +110,8 @@ def start_simulation(log_path: Path, env: dict[str, str]):
             "gui:=false",
             "mode:=realtime",
             "enable_foxglove:=false",
+            "world:=simple_lawn.wbt",
+            f"webots_port:={env['WEBOTS_PORT']}",
         ],
         stdout=log_file,
         stderr=subprocess.STDOUT,
@@ -99,6 +122,26 @@ def start_simulation(log_path: Path, env: dict[str, str]):
     return process, log_file
 
 
+def start_rosbag(tmp_path: Path, env: dict[str, str]):
+    bag_root = env.get("OPEN_MOWER_NEXT_TEST_ROSBAG_DIR")
+    if not bag_root:
+        return None, None, None
+
+    output_dir = Path(bag_root) / f"navigation_docking_{os.getpid()}"
+    output_dir.parent.mkdir(parents=True, exist_ok=True)
+    log_path = tmp_path / "rosbag.log"
+    log_file = log_path.open("w", encoding="utf-8")
+    process = subprocess.Popen(
+        ["ros2", "bag", "record", "-o", str(output_dir), *ROSBAG_TOPICS],
+        stdout=log_file,
+        stderr=subprocess.STDOUT,
+        env=env,
+        start_new_session=True,
+        text=True,
+    )
+    return process, log_file, output_dir
+
+
 def stop_simulation(process: subprocess.Popen) -> None:
     if process.poll() is not None:
         return
@@ -184,8 +227,8 @@ def __init__(self):
         self.map_grid_received = False
         self.mowing_map = None
         self.diff_drive_odom_received = False
+        self.gps_fix_received = False
         self.gps_odom_received = False
-        self.filtered_odom_after_gps = False
         self.filtered_odom = None
         self.closest_navigation_goal_distance = math.inf
         self.bounded_localization_since = None
@@ -208,10 +251,10 @@ def __init__(self):
         self.create_subscription(OccupancyGrid, "/map_grid", self._map_grid_callback, map_qos)
         self.create_subscription(Map, "/mowing_map", self._mowing_map_callback, map_qos)
         self.create_subscription(Odometry, "/diff_drive_base_controller/odom", self._diff_odom_callback, 10)
-        self.create_subscription(Odometry, "/odometry/gps", self._gps_odom_callback, 10)
-        self.create_subscription(Odometry, "/odometry/filtered/map", self._odom_callback, 10)
-        self.create_subscription(Bool, "/power/charger_present", self._charger_callback, 10)
-        self.create_subscription(Float32, "/power/charge_voltage", self._charge_voltage_callback, 10)
+        self.create_subscription(NavSatFix, "/gps/fix", self._gps_fix_callback, 10)
+        self.create_subscription(Odometry, "/gps/odom", self._gps_odom_callback, 10)
+        self.create_subscription(Odometry, "/fusion/odom", self._odom_callback, 10)
+        self.create_subscription(PowerStatus, "/power/status", self._power_status_callback, 10)
 
     def _clock_callback(self, _msg: Clock) -> None:
         self.clock_received = True
@@ -225,25 +268,22 @@ def _mowing_map_callback(self, msg: Map) -> None:
     def _diff_odom_callback(self, _msg: Odometry) -> None:
         self.diff_drive_odom_received = True
 
-    def _gps_odom_callback(self, _msg: Odometry) -> None:
-        if not self.gps_odom_received:
-            self.filtered_odom_after_gps = False
-            self.bounded_localization_since = None
-        self.gps_odom_received = True
+    def _gps_fix_callback(self, _msg: NavSatFix) -> None:
+        self.gps_fix_received = True
+
+    def _gps_odom_callback(self, msg: Odometry) -> None:
+        if is_finite_odom(msg):
+            self.gps_odom_received = True
 
     def _odom_callback(self, msg: Odometry) -> None:
         self.filtered_odom = msg
-        if self.gps_odom_received:
-            self.filtered_odom_after_gps = True
         position = msg.pose.pose.position
         distance = math.hypot(position.x - NAVIGATION_GOAL_X, position.y - NAVIGATION_GOAL_Y)
         self.closest_navigation_goal_distance = min(self.closest_navigation_goal_distance, distance)
 
-    def _charger_callback(self, msg: Bool) -> None:
-        self.charger_present = msg.data
-
-    def _charge_voltage_callback(self, msg: Float32) -> None:
-        self.charge_voltage = msg.data
+    def _power_status_callback(self, msg: PowerStatus) -> None:
+        self.charger_present = msg.charger_present
+        self.charge_voltage = msg.charge_voltage
 
     def has_ready_state(self) -> bool:
         if not self.clock_received or not self.map_grid_received:
@@ -252,12 +292,12 @@ def has_ready_state(self) -> bool:
             return False
         if not self.diff_drive_odom_received:
             return False
+        if not self.gps_fix_received:
+            return False
         if not self.gps_odom_received:
             return False
         if self.filtered_odom is None or not is_finite_odom(self.filtered_odom):
             return False
-        if not self.filtered_odom_after_gps:
-            return False
         if not self.has_bounded_filtered_pose():
             self.bounded_localization_since = None
             return False
@@ -324,8 +364,8 @@ def describe_state(self) -> str:
             f"dock_count={dock_count}, "
             f"map_dock={map_dock}, "
             f"diff_drive_odom={self.diff_drive_odom_received}, "
+            f"gps_fix={self.gps_fix_received}, "
             f"gps_odom={self.gps_odom_received}, "
-            f"filtered_odom_after_gps={self.filtered_odom_after_gps}, "
             f"filtered_odom={odom_ready}, "
             f"filtered_pose={odom_pose}, "
             f"closest_goal_distance={self.closest_navigation_goal_distance:.3f}, "
@@ -402,14 +442,19 @@ def wait_for_lifecycle_nodes_active(
                 all_active = False
                 continue
 
-            response = wait_for_future(
-                node,
-                client.call_async(GetState.Request()),
-                2.0,
-                f"{name} lifecycle state",
-                process,
-                log_path,
-            )
+            future = client.call_async(GetState.Request())
+            query_deadline = time.monotonic() + 2.0
+            while time.monotonic() < query_deadline:
+                check_process(process, log_path)
+                if future.done():
+                    break
+                rclpy.spin_once(node, timeout_sec=0.1)
+            if not future.done():
+                states[name] = "state_query_timeout"
+                all_active = False
+                continue
+
+            response = future.result()
             states[name] = response.current_state.label or str(response.current_state.id)
             if response.current_state.id != State.PRIMARY_STATE_ACTIVE:
                 all_active = False
@@ -479,52 +524,9 @@ def send_navigation_goal(node: NavigationDockingTestNode, process, log_path: Pat
         f"NavigateToPose goal was rejected\n{node.describe_state()}\n{read_log_tail(log_path)}"
     )
 
-    result_future = goal_handle.get_result_async()
-    result_response = None
-    deadline = time.monotonic() + 120.0
-    while time.monotonic() < deadline:
-        check_process(process, log_path)
-        if node.distance_to_navigation_goal() <= NAVIGATION_GOAL_TOLERANCE:
-            break
-        if result_future.done():
-            result_response = result_future.result()
-            break
-        rclpy.spin_once(node, timeout_sec=0.1)
-
-    if result_response is None and node.distance_to_navigation_goal() > NAVIGATION_GOAL_TOLERANCE:
-        pytest.fail(
-            "Timed out waiting for NavigateToPose to reach the waypoint\n"
-            f"{node.describe_state()}\n{read_log_tail(log_path)}"
-        )
-
-    if result_response is None and not result_future.done():
-        cancel_future = goal_handle.cancel_goal_async()
-        cancel_response = wait_for_future(
-            node, cancel_future, 15.0, "NavigateToPose cancellation", process, log_path
-        )
-        if cancel_response.goals_canceling:
-            cancel_result = wait_for_future(
-                node, result_future, 15.0, "NavigateToPose canceled result", process, log_path
-            )
-            assert cancel_result.status in (
-                GoalStatus.STATUS_CANCELED,
-                GoalStatus.STATUS_SUCCEEDED,
-            ), (
-                f"NavigateToPose reached the waypoint but finished with status={cancel_result.status}\n"
-                f"{node.describe_state()}\n{read_log_tail(log_path)}"
-            )
-            spin_for(node, 1.0, process, log_path)
-            return
-        if result_future.done():
-            result_response = result_future.result()
-        else:
-            pytest.fail(
-                "NavigateToPose reached the waypoint but cancellation was not accepted\n"
-                f"{node.describe_state()}\n{read_log_tail(log_path)}"
-            )
-
-    if result_response is None:
-        result_response = result_future.result()
+    result_response = wait_for_future(
+        node, goal_handle.get_result_async(), 120.0, "NavigateToPose result", process, log_path
+    )
 
     assert result_response.status == GoalStatus.STATUS_SUCCEEDED, (
         f"NavigateToPose failed with status={result_response.status}, "
@@ -569,9 +571,12 @@ def send_docking_goal(node: NavigationDockingTestNode, process, log_path: Path):
 def test_navigate_to_point_then_dock(tmp_path):
     log_path = tmp_path / "navigation_docking.log"
     env = prepare_env()
+    previous_ros_domain_id = os.environ.get("ROS_DOMAIN_ID")
     os.environ["ROS_DOMAIN_ID"] = env["ROS_DOMAIN_ID"]
 
     process, log_file = start_simulation(log_path, env)
+    rosbag_process = None
+    rosbag_log_file = None
     rclpy.init()
     node = NavigationDockingTestNode()
 
@@ -584,11 +589,11 @@ def test_navigate_to_point_then_dock(tmp_path):
             process,
             log_path,
         )
+        wait_for_lifecycle_nodes_active(node, NAV2_LIFECYCLE_NODES, 60.0, process, log_path)
         wait_for_action_server(node, node.navigate_client, "/navigate_to_pose", 60.0, process, log_path)
         wait_for_action_server(node, node.dock_client, "/dock_robot_nearest", 60.0, process, log_path)
-        wait_for_lifecycle_nodes_active(node, NAV2_LIFECYCLE_NODES, 60.0, process, log_path)
-
         send_navigation_goal(node, process, log_path)
+        rosbag_process, rosbag_log_file, _rosbag_output_dir = start_rosbag(tmp_path, env)
         send_docking_goal(node, process, log_path)
         spin_until(
             node,
@@ -603,7 +608,15 @@ def test_navigate_to_point_then_dock(tmp_path):
     finally:
         node.destroy_node()
         rclpy.shutdown()
+        if rosbag_process is not None:
+            stop_simulation(rosbag_process)
+        if rosbag_log_file is not None:
+            rosbag_log_file.close()
         stop_simulation(process)
         log_file.close()
+        if previous_ros_domain_id is None:
+            os.environ.pop("ROS_DOMAIN_ID", None)
+        else:
+            os.environ["ROS_DOMAIN_ID"] = previous_ros_domain_id
 
     assert_no_launch_failures(log_path)
diff --git a/test/integration/test_webots_smoke.py b/test/integration/test_webots_smoke.py
index ebf5fdb..b20ff4b 100644
--- a/test/integration/test_webots_smoke.py
+++ b/test/integration/test_webots_smoke.py
@@ -9,19 +9,20 @@
 import rclpy
 from geometry_msgs.msg import TwistStamped
 from nav_msgs.msg import Odometry
+from omros2_firmware_msgs.msg import PowerStatus
 from rosgraph_msgs.msg import Clock
 from sensor_msgs.msg import BatteryState, Imu, NavSatFix
-from std_msgs.msg import Bool, Float32
 
 
 REQUIRED_TOPICS = {
     "/clock",
     "/gps/fix",
+    "/gps/odom",
     "/imu/data_raw",
     "/diff_drive_base_controller/odom",
+    "/fusion/odom",
     "/power",
-    "/power/charge_voltage",
-    "/power/charger_present",
+    "/power/status",
 }
 
 LOG_FAILURE_PATTERNS = (
@@ -42,6 +43,8 @@ def repo_root() -> Path:
 def prepare_env() -> dict[str, str]:
     env = os.environ.copy()
     env.setdefault("WEBOTS_OFFSCREEN", "1")
+    env.setdefault("WEBOTS_PORT", str(12000 + (os.getpid() % 1000)))
+    env.setdefault("ROS_DOMAIN_ID", str(50 + (os.getpid() % 50)))
 
     if not env.get("WEBOTS_HOME"):
         default_webots_home = Path.home() / ".ros" / "webotsR2025a" / "webots"
@@ -74,6 +77,8 @@ def start_simulation(log_path: Path, env: dict[str, str]):
             "sim.launch.py",
             "gui:=false",
             "mode:=fast",
+            "world:=simple_lawn.wbt",
+            f"webots_port:={env['WEBOTS_PORT']}",
         ],
         stdout=log_file,
         stderr=subprocess.STDOUT,
@@ -122,15 +127,22 @@ def wait_for_topics_and_motion() -> None:
     node = rclpy.create_node("webots_smoke_test")
     received = set()
     poses = []
+    gps_statuses = []
 
     def mark(name):
         return lambda _: received.add(name)
 
     node.create_subscription(Clock, "/clock", mark("/clock"), 10)
-    node.create_subscription(NavSatFix, "/gps/fix", mark("/gps/fix"), 10)
+    node.create_subscription(
+        NavSatFix,
+        "/gps/fix",
+        lambda msg: (received.add("/gps/fix"), gps_statuses.append(msg.status.status)),
+        10,
+    )
+    node.create_subscription(Odometry, "/gps/odom", mark("/gps/odom"), 10)
     node.create_subscription(Imu, "/imu/data_raw", mark("/imu/data_raw"), 10)
-    node.create_subscription(Bool, "/power/charger_present", mark("/power/charger_present"), 10)
-    node.create_subscription(Float32, "/power/charge_voltage", mark("/power/charge_voltage"), 10)
+    node.create_subscription(Odometry, "/fusion/odom", mark("/fusion/odom"), 10)
+    node.create_subscription(PowerStatus, "/power/status", mark("/power/status"), 10)
     node.create_subscription(BatteryState, "/power", mark("/power"), 10)
     node.create_subscription(
         Odometry,
@@ -150,6 +162,7 @@ def mark(name):
 
         missing = sorted(REQUIRED_TOPICS - received)
         assert not missing, f"Missing expected simulation topics: {missing}"
+        assert gps_statuses[-1] >= 0, f"Expected valid GPS status, got {gps_statuses[-1]}"
         assert poses, "No odometry received before motion command"
 
         start = poses[-1]
@@ -181,7 +194,10 @@ def mark(name):
 
 def test_webots_smoke(tmp_path):
     log_path = tmp_path / "webots_smoke.log"
-    process, log_file = start_simulation(log_path, prepare_env())
+    env = prepare_env()
+    previous_ros_domain_id = os.environ.get("ROS_DOMAIN_ID")
+    os.environ["ROS_DOMAIN_ID"] = env["ROS_DOMAIN_ID"]
+    process, log_file = start_simulation(log_path, env)
 
     try:
         wait_for_topics_and_motion()
@@ -190,3 +206,7 @@ def test_webots_smoke(tmp_path):
     finally:
         stop_simulation(process)
         log_file.close()
+        if previous_ros_domain_id is None:
+            os.environ.pop("ROS_DOMAIN_ID", None)
+        else:
+            os.environ["ROS_DOMAIN_ID"] = previous_ros_domain_id
diff --git a/test/test_calibrate_robot.py b/test/test_calibrate_robot.py
new file mode 100644
index 0000000..0ef5d6b
--- /dev/null
+++ b/test/test_calibrate_robot.py
@@ -0,0 +1,76 @@
+import importlib.util
+import math
+import sys
+from pathlib import Path
+
+
+def load_module():
+    script = Path(__file__).resolve().parents[1] / "scripts" / "calibrate_robot.py"
+    spec = importlib.util.spec_from_file_location("calibrate_robot", script)
+    module = importlib.util.module_from_spec(spec)
+    sys.modules[spec.name] = module
+    spec.loader.exec_module(module)
+    return module
+
+
+calibrate_robot = load_module()
+
+
+def test_gps_samples_to_enu_starts_at_origin_and_tracks_east():
+    samples = [
+        calibrate_robot.GpsFixSample(0.0, 0.0, 0.0, 0.0, None, None, None, "test"),
+        calibrate_robot.GpsFixSample(1.0, 0.0, 0.00001, 0.0, None, None, None, "test"),
+    ]
+
+    points = calibrate_robot.gps_samples_to_enu(samples)
+
+    assert math.isclose(points[0].x, 0.0, abs_tol=1.0e-6)
+    assert math.isclose(points[0].y, 0.0, abs_tol=1.0e-6)
+    assert 1.10 < points[1].x < 1.12
+    assert abs(points[1].y) < 1.0e-4
+
+
+def test_odom_yaw_delta_unwraps_across_pi_boundary():
+    samples = [
+        calibrate_robot.OdomSample(0.0, 0.0, 0.0, math.radians(179.0), 0.0, 0.0, 0.0),
+        calibrate_robot.OdomSample(1.0, 0.0, 0.0, math.radians(-179.0), 0.0, 0.0, 0.0),
+    ]
+
+    delta = calibrate_robot.odom_yaw_delta(samples)
+
+    assert math.isclose(delta, math.radians(2.0), rel_tol=1.0e-6)
+
+
+def test_integrate_imu_yaw_uses_trapezoid_rule():
+    samples = [
+        calibrate_robot.ImuSample(0.0, 0.5),
+        calibrate_robot.ImuSample(1.0, 0.5),
+        calibrate_robot.ImuSample(2.0, 0.5),
+    ]
+
+    delta = calibrate_robot.integrate_imu_yaw(samples)
+
+    assert math.isclose(delta, 1.0, rel_tol=1.0e-6)
+
+
+def test_speed_summary_ignores_unstable_entries_for_stable_limits():
+    entries = [
+        {"actual_median": 0.10, "stable": True, "observed_accel": 0.4},
+        {"actual_median": 0.20, "stable": True, "observed_accel": 0.6},
+        {"actual_median": 0.25, "stable": False, "observed_accel": 0.7},
+    ]
+
+    summary = calibrate_robot.stable_speed_summary(entries)
+
+    assert summary["max_observed"] == 0.25
+    assert summary["max_stable"] == 0.20
+    assert summary["min_stable"] == 0.10
+    assert summary["observed_accel_median"] == 0.5
+
+
+def test_set_nested_updates_list_item():
+    data = {"wheel": {"offset": [0.0, 0.16, 0.0]}}
+
+    calibrate_robot.set_nested(data, ["wheel", "offset", 1], 0.18)
+
+    assert data["wheel"]["offset"] == [0.0, 0.18, 0.0]
diff --git a/test/test_webots_world_to_geojson.py b/test/test_webots_world_to_geojson.py
new file mode 100644
index 0000000..526a0a9
--- /dev/null
+++ b/test/test_webots_world_to_geojson.py
@@ -0,0 +1,80 @@
+import importlib.util
+import math
+import sys
+from pathlib import Path
+
+
+def load_module():
+    script = Path(__file__).resolve().parents[1] / "scripts" / "webots_world_to_geojson.py"
+    spec = importlib.util.spec_from_file_location("webots_world_to_geojson", script)
+    module = importlib.util.module_from_spec(spec)
+    sys.modules[spec.name] = module
+    spec.loader.exec_module(module)
+    return module
+
+
+converter = load_module()
+
+
+def realistic_garden_features():
+    root = Path(__file__).resolve().parents[1]
+    world = converter.parse_world(root / "worlds" / "realistic_garden.wbt")
+    return world, converter.build_local_features(world)
+
+
+def find_feature(features, feature_type, name):
+    for feature in features:
+        if feature.feature_type == feature_type and feature.name == name:
+            return feature
+    raise AssertionError(f"Feature not found: {feature_type} {name}")
+
+
+def bounds(points):
+    xs = [point[0] for point in points]
+    ys = [point[1] for point in points]
+    return min(xs), min(ys), max(xs), max(ys)
+
+
+def test_realistic_garden_operation_area_uses_curb_inner_edges():
+    _, features = realistic_garden_features()
+
+    operation = find_feature(features, "operation", "realistic_garden_operation_area")
+    actual_bounds = bounds(operation.coordinates)
+
+    for actual, expected in zip(actual_bounds, (-4.12, -3.62, 4.12, 3.62)):
+        assert math.isclose(actual, expected, abs_tol=1.0e-9)
+
+
+def test_realistic_garden_tree_exclusion_is_trunk_only():
+    _, features = realistic_garden_features()
+
+    oak = find_feature(features, "exclusion", "oak_tree")
+
+    radii = [math.hypot(x + 2.6, y - 1.8) for x, y in oak.coordinates[:-1]]
+
+    assert len(radii) == converter.TREE_TRUNK_SEGMENTS
+    assert all(math.isclose(radius, converter.TREE_TRUNK_RADIUS, abs_tol=1.0e-9) for radius in radii)
+
+
+def test_realistic_garden_docking_station_matches_webots_contact():
+    _, features = realistic_garden_features()
+
+    dock = find_feature(features, "docking_station", "docking_station")
+
+    expected = [(1.82, 1.5), (1.32, 1.5)]
+    for actual_point, expected_point in zip(dock.coordinates, expected):
+        for actual, expected_value in zip(actual_point, expected_point):
+            assert math.isclose(actual, expected_value, abs_tol=1.0e-9)
+
+
+def test_realistic_garden_geojson_is_deterministic_feature_collection():
+    world, features = realistic_garden_features()
+
+    geojson = converter.local_features_to_geojson(world, features)
+    repeated_geojson = converter.local_features_to_geojson(world, features)
+
+    assert geojson == repeated_geojson
+    assert geojson["type"] == "FeatureCollection"
+    assert len(geojson["features"]) == 5
+    assert geojson["features"][0]["properties"]["type"] == "operation"
+    assert geojson["features"][-1]["properties"]["type"] == "docking_station"
diff --git a/utils/docker-entrypoint.sh b/utils/docker-entrypoint.sh
index 48429a5..a140ffc 100755
--- a/utils/docker-entrypoint.sh
+++ b/utils/docker-entrypoint.sh
@@ -1,6 +1,8 @@
 #!/bin/bash
 set -e
 
+export ROS_AUTOMATIC_DISCOVERY_RANGE="${ROS_AUTOMATIC_DISCOVERY_RANGE:-LOCALHOST}"
+
 if [ -z "${OM_DATUM_LAT}" ]; then
   export OM_DATUM_LAT=30.0
 
diff --git a/utils/install-custom-deps.sh b/utils/install-custom-deps.sh
index 5010ef3..99d2e40 100755
--- a/utils/install-custom-deps.sh
+++ b/utils/install-custom-deps.sh
@@ -8,4 +8,25 @@ cd "$SCRIPT_PATH/.."
 mkdir -p src/lib
 vcs import src/lib --force --shallow < custom_deps.yaml
 
+if [ -d src/lib/webots_ros2 ]; then
+  for package in \
+    webots_ros2 \
+    webots_ros2_control \
+    webots_ros2_crazyflie \
+    webots_ros2_epuck \
+    webots_ros2_husarion \
+    webots_ros2_importer \
+    webots_ros2_mavic \
+    webots_ros2_msgs \
+    webots_ros2_tesla \
+    webots_ros2_tests \
+    webots_ros2_tiago \
+    webots_ros2_turtlebot \
+    webots_ros2_universal_robot
+  do
+    touch "src/lib/webots_ros2/${package}/COLCON_IGNORE"
+    touch "src/lib/webots_ros2/${package}/AMENT_IGNORE"
+  done
+fi
+
 rosdep install --from-paths src/lib --ignore-src --rosdistro $ROS_DISTRO -y
diff --git a/utils/omdev-run-launch.sh b/utils/omdev-run-launch.sh
new file mode 100644
index 0000000..6dafd38
--- /dev/null
+++ b/utils/omdev-run-launch.sh
@@ -0,0 +1,30 @@
+#!/usr/bin/env bash
+set -euo pipefail
+
+ros_distro="$1"
+launch_log="$2"
+pid_file="$3"
+shift 3
+
+export ROS_AUTOMATIC_DISCOVERY_RANGE="${ROS_AUTOMATIC_DISCOVERY_RANGE:-LOCALHOST}"
+
+bash utils/omdev-stop-launch.sh "$pid_file" >/dev/null 2>&1 || true
+
+mkdir -p "$(dirname "$launch_log")"
+: > "$launch_log"
+
+setsid bash -c '
+  set -euo pipefail
+  ros_distro="$1"
+  shift
+  export ROS_AUTOMATIC_DISCOVERY_RANGE="${ROS_AUTOMATIC_DISCOVERY_RANGE:-LOCALHOST}"
+  set +u
+  source "/opt/ros/${ros_distro}/setup.bash"
+  source install/setup.bash
+  set -u
+  exec ros2 launch open_mower_next openmower.launch.py "$@"
+' omdev-launch "$ros_distro" "$@" >> "$launch_log" 2>&1 &
+
+pid="$!"
+printf '%s\n' "$pid" > "$pid_file"
+printf 'Started OpenMowerNext launch pid %s; log: %s\n' "$pid" "$launch_log"
diff --git a/utils/omdev-stop-launch.sh b/utils/omdev-stop-launch.sh
new file mode 100644
index 0000000..d5f0cfa
--- /dev/null
+++ b/utils/omdev-stop-launch.sh
@@ -0,0 +1,44 @@
+#!/usr/bin/env bash
+set -euo pipefail
+
+pid_file="${1:-/tmp/openmowernext-launch.pid}"
+
+if [ ! -f "$pid_file" ]; then
+  printf 'No OpenMowerNext launch pid file at %s\n' "$pid_file"
+  exit 0
+fi
+
+pid=""
+read -r pid < "$pid_file" || true
+rm -f "$pid_file"
+
+if [ -z "$pid" ]; then
+  printf 'OpenMowerNext launch pid file was empty\n'
+  exit 0
+fi
+
+if ! kill -0 "$pid" 2>/dev/null; then
+  printf 'OpenMowerNext launch pid %s is not running\n' "$pid"
+  exit 0
+fi
+
+kill_tree() {
+  local parent="$1"
+  local child
+  for child in $(pgrep -P "$parent" 2>/dev/null || true); do
+    kill_tree "$child"
+    kill -TERM "$child" 2>/dev/null || true
+  done
+}
+
+kill -TERM -- "-$pid" 2>/dev/null || true
+kill_tree "$pid"
+kill -TERM "$pid" 2>/dev/null || true
+sleep 2
+if kill -0 "$pid" 2>/dev/null; then
+  kill -KILL -- "-$pid" 2>/dev/null || true
+  kill_tree "$pid"
+  kill -KILL "$pid" 2>/dev/null || true
+fi
+
+printf 'Stopped OpenMowerNext launch pid %s\n' "$pid"
diff --git a/utils/run-foxglove.sh b/utils/run-foxglove.sh
index 65a7789..0f761bc 100755
--- a/utils/run-foxglove.sh
+++ b/utils/run-foxglove.sh
@@ -2,9 +2,11 @@
 set -euo pipefail
 
 ROS_DISTRO="${ROS_DISTRO:-jazzy}"
+ROS_AUTOMATIC_DISCOVERY_RANGE="${ROS_AUTOMATIC_DISCOVERY_RANGE:-LOCALHOST}"
 FOXGLOVE_ADDRESS="${FOXGLOVE_ADDRESS:-0.0.0.0}"
 FOXGLOVE_PORT="${FOXGLOVE_PORT:-8765}"
 FOXGLOVE_USE_SIM_TIME="${FOXGLOVE_USE_SIM_TIME:-false}"
+export ROS_AUTOMATIC_DISCOVERY_RANGE
 
 repo_root="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
 
diff --git a/utils/run-rosbridge.sh b/utils/run-rosbridge.sh
index 0942478..9ea3ec6 100644
--- a/utils/run-rosbridge.sh
+++ b/utils/run-rosbridge.sh
@@ -2,8 +2,10 @@
 set -euo pipefail
 
 ROS_DISTRO="${ROS_DISTRO:-jazzy}"
+ROS_AUTOMATIC_DISCOVERY_RANGE="${ROS_AUTOMATIC_DISCOVERY_RANGE:-LOCALHOST}"
 ROSBRIDGE_ADDRESS="${ROSBRIDGE_ADDRESS:-127.0.0.1}"
 ROSBRIDGE_PORT="${ROSBRIDGE_PORT:-9090}"
+export ROS_AUTOMATIC_DISCOVERY_RANGE
 
 repo_root="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
 
diff --git a/worlds/openmower.wbt b/worlds/openmower.wbt
index 8f2d24c..bcab18a 100644
--- a/worlds/openmower.wbt
+++ b/worlds/openmower.wbt
@@ -43,9 +43,10 @@ Solid {
 }
 OpenMower {
   translation 0 0 0.0925
-  rotation 0 0 1 2.0944
+  rotation 0 0 1 0
   name "openmower"
   controller "<extern>"
+  supervisor TRUE
 }
 DockingStation {
   translation 1.5 1.5 0
diff --git a/worlds/realistic_garden.wbt b/worlds/realistic_garden.wbt
new file mode 100644
index 0000000..9906f3e
--- /dev/null
+++ b/worlds/realistic_garden.wbt
@@ -0,0 +1,274 @@
+#VRML_SIM R2025a utf8
+
+EXTERNPROTO "../protos/OpenMower.proto"
+EXTERNPROTO "../protos/DockingStation.proto"
+EXTERNPROTO "../protos/GnssDegradationZone.proto"
+EXTERNPROTO "../protos/GardenBuilding.proto"
+EXTERNPROTO "../protos/GardenTree.proto"
+EXTERNPROTO "../protos/Curb.proto"
+EXTERNPROTO "../protos/TerrainPatch.proto"
+EXTERNPROTO "../protos/FrictionPatch.proto"
+
+WorldInfo {
+  title "OpenMowerNext realistic garden"
+  basicTimeStep 10
+  coordinateSystem "ENU"
+  gpsCoordinateSystem "WGS84"
+  gpsReference -22.9 -43.2 0
+  contactProperties [
+    ContactProperties {
+      material1 "mower_wheel"
+      material2 "short_grass"
+      coulombFriction [
+        1.0
+      ]
+    }
+    ContactProperties {
+      material1 "mower_wheel"
+      material2 "dry_soil"
+      coulombFriction [
+        0.85
+      ]
+    }
+    ContactProperties {
+      material1 "mower_wheel"
+      material2 "loose_soil"
+      coulombFriction [
+        0.70
+      ]
+    }
+    ContactProperties {
+      material1 "mower_wheel"
+      material2 "wet_grass"
+      coulombFriction [
+        0.65
+      ]
+    }
+    ContactProperties {
+      material1 "mower_wheel"
+      material2 "concrete"
+      coulombFriction [
+        1.1
+      ]
+    }
+  ]
+}
+Viewpoint {
+  orientation -0.371979 0.445814 0.814044 1.32098
+  position -4.5 -7.5 5.2
+  follow "openmower"
+}
+Background {
+  skyColor [
+    0.43 0.61 0.82
+  ]
+}
+DirectionalLight {
+  direction -0.45 -0.25 -1
+  intensity 1
+}
+Solid {
+  translation -12 -9 0
+  children [
+    Shape {
+      appearance PBRAppearance {
+        baseColor 0.10 0.37 0.09
+        roughness 1
+        metalness 0
+      }
+      geometry DEF REALISTIC_GRASS_TERRAIN ElevationGrid {
+        xDimension 13
+        yDimension 10
+        xSpacing 2
+        ySpacing 2
+        thickness 0.35
+        height [
+          0.010 0.018 0.012 -0.010 -0.026 -0.018 0.006 0.018 0.011 -0.006 -0.012 0.004 0.016
+          0.020 0.030 0.021 -0.006 -0.030 -0.024 0.000 0.020 0.016 -0.004 -0.016 0.000 0.014
+          0.008 0.018 0.014 -0.018 -0.045 -0.035 -0.006 0.018 0.018 0.006 -0.008 -0.010 0.004
+          -0.008 0.000 0.018 0.006 -0.015 -0.030 -0.015 0.018 0.030 0.022 0.002 -0.020 -0.012
+          -0.020 -0.012 0.020 0.034 0.018 0.000 0.000 0.002 0.004 0.030 0.010 -0.014 -0.018
+          -0.016 -0.004 0.018 0.040 0.028 0.000 0.000 0.002 0.004 0.024 0.018 0.000 -0.006
+          0.000 0.010 0.018 0.026 0.018 0.000 0.000 0.002 0.004 0.004 0.014 0.018 0.012
+          0.012 0.022 0.016 0.006 -0.004 -0.012 -0.034 -0.055 -0.036 -0.006 0.010 0.018 0.014
+          0.018 0.028 0.020 0.000 -0.012 -0.020 -0.026 -0.035 -0.020 0.000 0.012 0.016 0.010
+          0.014 0.020 0.014 -0.006 -0.018 -0.016 0.000 0.010 0.012 0.004 0.004 0.008 0.006
+        ]
+      }
+    }
+  ]
+  name "short_grass_floor"
+}
+Solid {
+  translation 0 0 -0.005
+  name "flat_lawn_collision"
+  contactMaterial "short_grass"
+  boundingObject Box {
+    size 24 18 0.01
+  }
+}
+TerrainPatch {
+  translation -3.2 -2.4 0.005
+  rotation 0 0 1 0.12
+  name "dry_soil_patch"
+  size 2.4 1.3 0.012
+  color 0.27 0.16 0.08
+}
+FrictionPatch {
+  translation -3.2 -2.4 0.001
+  rotation 0 0 1 0.12
+  name "dry_soil_friction"
+  size 2.4 1.3 0.002
+  contactMaterial "dry_soil"
+}
+TerrainPatch {
+  translation -1.8 2.8 0.008
+  rotation 0 0 1 -0.35
+  name "wet_grass_patch"
+  size 2.0 1.1 0.018
+  color 0.04 0.24 0.08
+  roughness 0.45
+}
+FrictionPatch {
+  translation -1.8 2.8 0.001
+  rotation 0 0 1 -0.35
+  name "wet_grass_friction"
+  size 2.0 1.1 0.002
+  contactMaterial "wet_grass"
+}
+TerrainPatch {
+  translation 0.9 -2.7 0.018
+  rotation 1 0 0 0.08
+  name "gentle_slope"
+  size 3.0 1.2 0.035
+  color 0.12 0.43 0.10
+}
+TerrainPatch {
+  translation -0.9 -1.1 0.012
+  rotation 0 0 1 0.2
+  name "low_mound"
+  size 1.0 0.75 0.03
+  color 0.13 0.45 0.11
+}
+TerrainPatch {
+  translation 2.1 -2.4 0.004
+  rotation 0 0 1 -0.15
+  name "shallow_rut_left"
+  size 1.7 0.18 0.008
+  color 0.18 0.25 0.08
+}
+FrictionPatch {
+  translation 2.1 -2.4 0.001
+  rotation 0 0 1 -0.15
+  name "shallow_rut_left_friction"
+  size 1.7 0.18 0.002
+  contactMaterial "loose_soil"
+}
+TerrainPatch {
+  translation 2.1 -2.75 0.004
+  rotation 0 0 1 -0.15
+  name "shallow_rut_right"
+  size 1.7 0.18 0.008
+  color 0.18 0.25 0.08
+}
+FrictionPatch {
+  translation 2.1 -2.75 0.001
+  rotation 0 0 1 -0.15
+  name "shallow_rut_right_friction"
+  size 1.7 0.18 0.002
+  contactMaterial "loose_soil"
+}
+Curb {
+  translation -4.2 0 0.035
+  rotation 0 0 1 0
+  name "west_curb"
+  size 0.16 7.4 0.07
+}
+Curb {
+  translation 0 -3.7 0.035
+  rotation 0 0 1 0
+  name "south_curb"
+  size 8.6 0.16 0.07
+}
+Curb {
+  translation 0 3.7 0.035
+  rotation 0 0 1 0
+  name "north_curb"
+  size 8.6 0.16 0.07
+}
+Curb {
+  translation 4.2 -1.4 0.035
+  rotation 0 0 1 0
+  name "east_curb_lower"
+  size 0.16 4.6 0.07
+}
+TerrainPatch {
+  translation 0 -4.35 0.002
+  name "concrete_walkway"
+  size 9.2 1.0 0.014
+  color 0.55 0.55 0.52
+  roughness 0.72
+}
+FrictionPatch {
+  translation 0 -4.35 0.001
+  name "concrete_walkway_friction"
+  size 9.2 1.0 0.002
+  contactMaterial "concrete"
+}
+GardenBuilding {
+  translation 3.65 1.75 0.9
+  name "house_wall"
+  size 2.8 3.2 1.8
+}
+GardenTree {
+  translation -2.6 1.8 0
+  name "oak_tree"
+}
+GardenTree {
+  translation 1.2 -1.9 0
+  name "young_tree"
+}
+DockingStation {
+  translation 1.5 1.5 0
+  name "docking_station"
+}
+OpenMower {
+  translation 0 0 0.0925
+  rotation 0 0 1 0
+  name "openmower"
+  controller "<extern>"
+  supervisor TRUE
+}
+GnssDegradationZone {
+  translation 2.35 1.65 0.03
+  name "dock_wall_multipath"
+  size 1.6 3.6 0.06
+  metadata "shape=box;model=dock_wall;sigmaXY=0.08;biasX=-0.08;biasY=0.02;dropoutProbability=0;covarianceXY=0.04;edgeWidth=0.55;driftTimeConstant=10"
+  color 1 0.45 0.10
+}
+GnssDegradationZone {
+  translation 3.2 1.75 0.03
+  name "building_multipath"
+  size 2.1 4.4 0.06
+  metadata "shape=box;model=multipath;sigmaXY=0.14;biasX=-0.20;biasY=0.06;dropoutProbability=0;covarianceXY=0.09;edgeWidth=0.65;driftTimeConstant=12"
+  color 1 0.18 0.08
+}
+GnssDegradationZone {
+  translation -2.6 1.8 0.03
+  name "oak_canopy_gnss_shadow"
+  size 3.2 3.2 0.06
+  metadata "shape=circle;radius=1.6;model=tree_canopy;sigmaXY=0.18;biasX=0.04;biasY=-0.05;dropoutProbability=0.001;covarianceXY=0.12;edgeWidth=0.75;driftTimeConstant=14"
+  color 0.15 0.8 0.2
+}
+GnssDegradationZone {
+  translation 1.2 -1.9 0.03
+  name "young_tree_canopy_gnss_shadow"
+  size 2.2 2.2 0.06
+  metadata "shape=circle;radius=1.1;model=tree_canopy;sigmaXY=0.10;dropoutProbability=0;covarianceXY=0.05;edgeWidth=0.50;driftTimeConstant=12"
+  color 0.15 0.8 0.2
+}
+Robot {
+  name "Ros2Supervisor"
+  controller "<extern>"
+  supervisor TRUE
+}
diff --git a/worlds/simple_lawn.wbt b/worlds/simple_lawn.wbt
new file mode 100644
index 0000000..937dc28
--- /dev/null
+++ b/worlds/simple_lawn.wbt
@@ -0,0 +1,59 @@
+#VRML_SIM R2025a utf8
+
+EXTERNPROTO "../protos/OpenMower.proto"
+EXTERNPROTO "../protos/DockingStation.proto"
+
+WorldInfo {
+  title "OpenMowerNext simple lawn"
+  basicTimeStep 10
+  coordinateSystem "ENU"
+  gpsCoordinateSystem "WGS84"
+  gpsReference -22.9 -43.2 0
+}
+Viewpoint {
+  orientation -0.330491 0.451759 0.828566 1.32217
+  position -2.3 -4.4 3.0
+  follow "openmower"
+}
+Background {
+  skyColor [
+    0.45 0.62 0.85
+  ]
+}
+DirectionalLight {
+  direction -0.4 -0.2 -1
+  intensity 1
+}
+Solid {
+  translation 0 0 -0.005
+  children [
+    Shape {
+      appearance PBRAppearance {
+        baseColor 0.12 0.42 0.12
+        roughness 1
+        metalness 0
+      }
+      geometry DEF FLOOR_BOX Box {
+        size 20 20 0.01
+      }
+    }
+  ]
+  name "grass_floor"
+  boundingObject USE FLOOR_BOX
+}
+OpenMower {
+  translation 0 0 0.0925
+  rotation 0 0 1 0
+  name "openmower"
+  controller "<extern>"
+  supervisor TRUE
+}
+DockingStation {
+  translation 1.5 1.5 0
+  name "docking_station"
+}
+Robot {
+  name "Ros2Supervisor"
+  controller "<extern>"
+  supervisor TRUE
+}
diff --git a/worlds/stress_garden.wbt b/worlds/stress_garden.wbt
new file mode 100644
index 0000000..3f52cc5
--- /dev/null
+++ b/worlds/stress_garden.wbt
@@ -0,0 +1,232 @@
+#VRML_SIM R2025a utf8
+
+EXTERNPROTO "../protos/OpenMower.proto"
+EXTERNPROTO "../protos/DockingStation.proto"
+EXTERNPROTO "../protos/GnssDegradationZone.proto"
+EXTERNPROTO "../protos/GardenBuilding.proto"
+EXTERNPROTO "../protos/GardenTree.proto"
+EXTERNPROTO "../protos/Curb.proto"
+EXTERNPROTO "../protos/TerrainPatch.proto"
+EXTERNPROTO "../protos/FrictionPatch.proto"
+
+WorldInfo {
+  title "OpenMowerNext stress garden"
+  basicTimeStep 10
+  coordinateSystem "ENU"
+  gpsCoordinateSystem "WGS84"
+  gpsReference -22.9 -43.2 0
+  contactProperties [
+    ContactProperties {
+      material1 "mower_wheel"
+      material2 "short_grass"
+      coulombFriction [
+        1.0
+      ]
+    }
+    ContactProperties {
+      material1 "mower_wheel"
+      material2 "loose_soil"
+      coulombFriction [
+        0.65
+      ]
+    }
+    ContactProperties {
+      material1 "mower_wheel"
+      material2 "mud"
+      coulombFriction [
+        0.50
+      ]
+    }
+  ]
+}
+Viewpoint {
+  orientation -0.39151 0.47828 0.786181 1.28795
+  position -5.8 -8.6 5.8
+  follow "openmower"
+}
+Background {
+  skyColor [
+    0.39 0.55 0.76
+  ]
+}
+DirectionalLight {
+  direction -0.35 -0.45 -1
+  intensity 0.95
+}
+Solid {
+  translation -12 -9 0
+  children [
+    Shape {
+      appearance PBRAppearance {
+        baseColor 0.09 0.34 0.08
+        roughness 1
+        metalness 0
+      }
+      geometry DEF STRESS_GRASS_TERRAIN ElevationGrid {
+        xDimension 13
+        yDimension 10
+        xSpacing 2
+        ySpacing 2
+        thickness 0.45
+        height [
+          0.020 0.040 0.024 -0.020 -0.055 -0.030 0.015 0.036 0.020 -0.020 -0.035 -0.006 0.028
+          0.035 0.065 0.040 -0.010 -0.070 -0.052 -0.008 0.030 0.026 -0.014 -0.040 -0.018 0.018
+          0.010 0.030 0.018 -0.050 -0.105 -0.080 -0.022 0.020 0.032 0.006 -0.030 -0.036 -0.004
+          -0.030 -0.015 0.026 0.010 -0.040 -0.080 -0.050 0.020 0.058 0.040 -0.006 -0.055 -0.028
+          -0.052 -0.030 0.030 0.075 0.045 -0.018 -0.032 0.008 0.062 0.060 0.018 -0.050 -0.042
+          -0.040 -0.012 0.034 0.090 0.070 0.014 -0.024 -0.024 0.026 0.052 0.038 -0.012 -0.020
+          -0.004 0.020 0.032 0.052 0.030 -0.005 -0.070 -0.120 -0.075 -0.010 0.024 0.034 0.020
+          0.024 0.048 0.032 0.004 -0.028 -0.050 -0.100 -0.155 -0.105 -0.030 0.016 0.032 0.026
+          0.036 0.060 0.035 -0.012 -0.045 -0.062 -0.076 -0.105 -0.064 -0.010 0.022 0.028 0.016
+          0.026 0.042 0.025 -0.018 -0.050 -0.040 -0.008 0.018 0.020 0.000 0.004 0.014 0.010
+        ]
+      }
+    }
+  ]
+  name "stress_grass_floor"
+  contactMaterial "short_grass"
+  boundingObject USE STRESS_GRASS_TERRAIN
+}
+TerrainPatch {
+  translation -2.8 -2.2 0.025
+  rotation 1 0 0 0.12
+  name "steeper_slope"
+  size 3.2 1.4 0.05
+  color 0.13 0.43 0.10
+}
+TerrainPatch {
+  translation -0.9 -0.9 0.022
+  rotation 0 0 1 0.25
+  name "larger_mound"
+  size 1.3 0.95 0.045
+  color 0.13 0.44 0.11
+}
+TerrainPatch {
+  translation 1.2 -2.55 0.007
+  rotation 0 0 1 -0.22
+  name "deep_rut_left"
+  size 2.5 0.22 0.014
+  color 0.17 0.20 0.06
+}
+FrictionPatch {
+  translation 1.2 -2.55 0.001
+  rotation 0 0 1 -0.22
+  name "deep_rut_left_friction"
+  size 2.5 0.22 0.002
+  contactMaterial "loose_soil"
+}
+TerrainPatch {
+  translation 1.2 -2.95 0.007
+  rotation 0 0 1 -0.22
+  name "deep_rut_right"
+  size 2.5 0.22 0.014
+  color 0.17 0.20 0.06
+}
+FrictionPatch {
+  translation 1.2 -2.95 0.001
+  rotation 0 0 1 -0.22
+  name "deep_rut_right_friction"
+  size 2.5 0.22 0.002
+  contactMaterial "loose_soil"
+}
+TerrainPatch {
+  translation -3.1 2.4 0.01
+  name "muddy_patch"
+  size 2.6 1.6 0.02
+  color 0.07 0.15 0.05
+  roughness 0.35
+}
+FrictionPatch {
+  translation -3.1 2.4 0.001
+  name "muddy_patch_friction"
+  size 2.6 1.6 0.002
+  contactMaterial "mud"
+}
+Curb {
+  translation -4.2 0 0.035
+  name "west_curb"
+  size 0.16 7.4 0.07
+}
+Curb {
+  translation 0 -3.7 0.035
+  name "south_curb"
+  size 8.6 0.16 0.07
+}
+Curb {
+  translation 0 3.7 0.035
+  name "north_curb"
+  size 8.6 0.16 0.07
+}
+Curb {
+  translation 4.2 -1.15 0.035
+  name "east_curb_narrow_passage"
+  size 0.16 5.1 0.07
+}
+GardenBuilding {
+  translation 3.65 1.75 0.9
+  name "house_wall"
+  size 2.8 3.2 1.8
+}
+GardenTree {
+  translation -2.7 1.8 0
+  name "large_tree"
+}
+GardenTree {
+  translation 0.9 -1.9 0
+  name "middle_tree"
+}
+GardenTree {
+  translation 2.7 -0.3 0
+  name "narrow_passage_tree"
+}
+DockingStation {
+  translation 1.5 1.5 0
+  name "docking_station"
+}
+OpenMower {
+  translation 0 0 0.0925
+  rotation 0 0 1 0
+  name "openmower"
+  controller "<extern>"
+  supervisor TRUE
+}
+GnssDegradationZone {
+  translation 2.55 1.65 0.03
+  name "dock_wall_harsh_multipath"
+  size 1.9 3.8 0.06
+  metadata "shape=box;model=dock_wall;sigmaXY=0.22;biasX=-0.28;biasY=0.10;dropoutProbability=0.002;covarianceXY=0.18;edgeWidth=0.65;driftTimeConstant=9"
+  color 1 0.40 0.08
+}
+GnssDegradationZone {
+  translation 3.25 1.75 0.03
+  name "building_harsh_multipath"
+  size 2.3 4.8 0.06
+  metadata "shape=box;model=multipath;sigmaXY=0.32;biasX=-0.45;biasY=0.16;dropoutProbability=0.004;covarianceXY=0.30;edgeWidth=0.75;driftTimeConstant=10"
+  color 1 0.10 0.05
+}
+GnssDegradationZone {
+  translation -2.7 1.8 0.03
+  name "large_tree_gnss_shadow"
+  size 3.6 3.6 0.06
+  metadata "shape=circle;radius=1.8;model=tree_canopy;sigmaXY=0.35;dropoutProbability=0.006;covarianceXY=0.36;edgeWidth=0.85;driftTimeConstant=12"
+  color 0.12 0.75 0.18
+}
+GnssDegradationZone {
+  translation 0.9 -1.9 0.03
+  name "middle_tree_gnss_shadow"
+  size 2.8 2.8 0.06
+  metadata "shape=circle;radius=1.4;model=tree_canopy;sigmaXY=0.26;biasX=0.08;dropoutProbability=0.003;covarianceXY=0.22;edgeWidth=0.65;driftTimeConstant=11"
+  color 0.12 0.75 0.18
+}
+GnssDegradationZone {
+  translation 2.7 -0.3 0.03
+  name "narrow_passage_gnss_shadow"
+  size 2.4 2.4 0.06
+  metadata "shape=circle;radius=1.2;model=tree_canopy;sigmaXY=0.24;biasY=-0.08;dropoutProbability=0.002;covarianceXY=0.18;edgeWidth=0.55;driftTimeConstant=10"
+  color 0.12 0.75 0.18
+}
+Robot {
+  name "Ros2Supervisor"
+  controller "<extern>"
+  supervisor TRUE
+}